dynamicFriction.jl

# Amira Abdel-Rahman
# (c) Massachusetts Institute of Technology 2020


using LinearAlgebra
using Plots
import JSON
# using Quaternions
using StaticArrays
# using Distributed, Rotations
using StaticArrays, BenchmarkTools
using Base.Threads
using CUDAnative
using CuArrays,CUDAdrv 
using Test
import Base: +, * , -, ^

#####################################################
struct Vector3
    x::Float64
    y::Float64
    z::Float64
    function Vector3()
        x=0.0
        y=0.0
        z=0.0
        new(x,y,z)
    end
    function Vector3(x,y,z)
       new(x,y,z)
    end
end
struct Quaternion
    x::Float64
    y::Float64
    z::Float64
    w::Float64
    function Quaternion()
        x=0.0
        y=0.0
        z=0.0
        w=1.0
        new(x,y,z,w)
    end
    function Quaternion(x,y,z,w)
        new(x,y,z,w)
    end
end
struct RotationMatrix
    te1::Float64
    te2::Float64
    te3::Float64
    te4::Float64
    te5::Float64
    te6::Float64
    te7::Float64
    te8::Float64
    te9::Float64
    te10::Float64
    te11::Float64
    te12::Float64
    te13::Float64
    te14::Float64
    te15::Float64
    te16::Float64
    function RotationMatrix()
        te1 =0.0
        te2 =0.0
        te3 =0.0
        te4 =0.0
        te5 =0.0
        te6 =0.0
        te7 =0.0
        te8 =0.0
        te9 =0.0
        te10=0.0
        te11=0.0
        te12=0.0
        te13=0.0
        te14=0.0
        te15=0.0
        te16=0.0
        new(te1,te2,te3,te4,te5,te6,te7,te8,te9,te10,te11,te12,te13,te14,te15,te16)
    end
    function RotationMatrix(te1,te2,te3,te4,te5,te6,te7,te8,te9,te10,te11,te12,te13,te14,te15,te16)
        new(te1,te2,te3,te4,te5,te6,te7,te8,te9,te10,te11,te12,te13,te14,te15,te16)
    end
end

+(f::Vector3, g::Vector3)=Vector3(f.x+g.x , f.y+g.y,f.z+g.z )
-(f::Vector3, g::Vector3)=Vector3(f.x-g.x , f.y-g.y,f.z-g.z )
*(f::Vector3, g::Vector3)=Vector3(f.x*g.x , f.y*g.y,f.z*g.z )

+(f::Vector3, g::Number)=Vector3(f.x+g , f.y+g,f.z+g )
-(f::Vector3, g::Number)=Vector3(f.x-g , f.y-g,f.z-g )
*(f::Vector3, g::Number)=Vector3(f.x*g , f.y*g,f.z*g )

+(g::Vector3, f::Number)=Vector3(f.x+g , f.y+g,f.z+g )
-(g::Vector3, f::Number)=Vector3(g-f.x , g-f.y,g-f.z )
*(g::Vector3, f::Number)=Vector3(f.x*g , f.y*g,f.z*g )

addX(f::Vector3, g::Number)=Vector3(f.x+g , f.y,f.z)
addY(f::Vector3, g::Number)=Vector3(f.x , f.y+g,f.z )
addZ(f::Vector3, g::Number)=Vector3(f.x , f.y,f.z+g )

function normalizeVector3(f::Vector3)
    leng=sqrt((f.x * f.x) + (f.y * f.y) + (f.z * f.z))
    return Vector3(f.x/leng,f.y/leng,f.z/leng)
    
end
function normalizeQuaternion(f::Quaternion)
    l = sqrt((f.x * f.x) + (f.y * f.y) + (f.z * f.z)+ (f.w * f.w))
    if l === 0 
        qx = 0
        qy = 0
        qz = 0
        qw = 1
    else 
        l = 1 / l
        qx = f.x * l
        qy = f.y * l
        qz = f.z * l
        qw = f.w * l
    end
    return Quaternion(qx,qy,qz,qw)
end

function normalizeQuaternion1!(fx::Float64,fy::Float64,fz::Float64,fw::Float64)
    l = sqrt((fx * fx) + (fy * fy) + (fz * fz)+ (fw * fw))
    if l === 0 
        qx = 0.0
        qy = 0.0
        qz = 0.0
        qw = 1.0
    else 
        l = 1.0 / l
        qx = fx * l
        qy = fy * l
        qz = fz * l
        qw = fw * l
    end
    return qx,qy,qz,qw
end


function dotVector3(f::Vector3, g::Vector3)
    return (f.x * g.x) + (f.y * g.y) + (f.z * g.z)
end

function Base.show(io::IO, v::Vector3)
    print(io, "x:$(v.x), y:$(v.y), z:$(v.z)")
end

function Base.show(io::IO, v::Quaternion)
    print(io, "x:$(v.x), y:$(v.y), z:$(v.z), w:$(v.z)")
end

Base.Broadcast.broadcastable(q::Vector3) = Ref(q)
#####################################################
function doTimeStep!(metavoxel,dt,currentTimeStep)
    # update forces: go through edges, get currentposition from nodes, calc pos2 and update stresses and interior forces of nodes
    run_updateEdges!(
        metavoxel["E_sourceGPU"], 
        metavoxel["E_targetGPU"],
        metavoxel["E_areaGPU"],
        metavoxel["E_densityGPU"],
        metavoxel["E_stiffnessGPU"],
        metavoxel["E_stressGPU"],
        metavoxel["E_axisGPU"],
        metavoxel["E_currentRestLengthGPU"],
        metavoxel["E_pos2GPU"],
        metavoxel["E_angle1vGPU"],
        metavoxel["E_angle2vGPU"],
        metavoxel["E_angle1GPU"],
        metavoxel["E_angle2GPU"],
        metavoxel["E_intForce1GPU"],
        metavoxel["E_intMoment1GPU"],
        metavoxel["E_intForce2GPU"],
        metavoxel["E_intMoment2GPU"],
        metavoxel["E_dampGPU"],
        metavoxel["N_currPositionGPU"],
        metavoxel["N_orientGPU"])
    
    # update forces: go through nodes and update interior force (according to int forces from edges), integrate and update currpos
    run_updateNodes!(dt,currentTimeStep,
        metavoxel["N_positionGPU"], 
        metavoxel["N_restrainedGPU"],
        metavoxel["N_displacementGPU"],
        metavoxel["N_angleGPU"],
        metavoxel["N_currPositionGPU"],
        metavoxel["N_linMomGPU"],
        metavoxel["N_angMomGPU"],
        metavoxel["N_intForceGPU"],
        metavoxel["N_intMomentGPU"],
        metavoxel["N_forceGPU"],
        metavoxel["N_momentGPU"],
        metavoxel["N_orientGPU"],
        metavoxel["N_edgeIDGPU"], 
        metavoxel["N_edgeFirstGPU"], 
        metavoxel["E_intForce1GPU"],
        metavoxel["E_intMoment1GPU"],
        metavoxel["E_intForce2GPU"],
        metavoxel["E_intMoment2GPU"])
    
end

#####################################################
function updateEdges!(E_source,E_target,E_area,E_density,E_stiffness,E_stress,E_axis,E_currentRestLength,E_pos2,E_angle1v,E_angle2v,E_angle1,E_angle2,E_intForce1,E_intMoment1,E_intForce2,E_intMoment2,E_damp,N_currPosition,N_orient)

    index = (blockIdx().x - 1) * blockDim().x + threadIdx().x
    stride = blockDim().x * gridDim().x
    ## @cuprintln("thread $index, block $stride")
    N=length(E_source)
    for i = index:stride:N
        
        @inbounds pVNeg=N_currPosition[E_source[i]]
        @inbounds pVPos=N_currPosition[E_target[i]]
        
        @inbounds oVNeg=N_orient[E_source[i]]
        @inbounds oVPos=N_orient[E_target[i]]
        
        @inbounds oldPos2=Vector3(E_pos2[i].x,E_pos2[i].y,E_pos2[i].z) #?copy?
        @inbounds oldAngle1v = Vector3(E_angle1v[i].x,E_angle1v[i].y,E_angle1v[i].z)
        @inbounds oldAngle2v = Vector3(E_angle2v[i].x,E_angle2v[i].y,E_angle2v[i].z)# remember the positions/angles from last timestep to calculate velocity
        
        
        @inbounds E_pos2[i],E_angle1v[i],E_angle2v[i],E_angle1[i],E_angle2[i],totalRot= orientLink!(E_currentRestLength[i],pVNeg,pVPos,oVNeg,oVPos,E_axis[i])
        
        @inbounds dPos2   = Vector3(0.5,0.5,0.5) * (E_pos2[i]-oldPos2)  #deltas for local damping. velocity at center is half the total velocity
        @inbounds dAngle1 = Vector3(0.5,0.5,0.5) *(E_angle1v[i]-oldAngle1v)
        @inbounds dAngle2 = Vector3(0.5,0.5,0.5) *(E_angle2v[i]-oldAngle2v)
        
        
        @inbounds strain=(E_pos2[i].x/E_currentRestLength[i])
        
        positiveEnd=true
        if axialStrain( positiveEnd,strain)>100.0
            diverged=true
            @cuprintln("DIVERGED!!!!!!!!!!")
            return 
        end
        
        
        @inbounds E = E_stiffness[i]
        
        
        
        @inbounds l   = E_currentRestLength[i]
        
        
        
        nu=0
            # L = 5.0 #?? change!!!!!!
        L=l
        a1 = E*L # EA/L : Units of N/m
        a2 = E * L*L*L / (12.0*(1+nu)) # GJ/L : Units of N-m
        b1 = E*L # 12EI/L^3 : Units of N/m
        b2 = E*L*L/2.0 # 6EI/L^2 : Units of N (or N-m/m: torque related to linear distance)
        b3 = E*L*L*L/6.0 # 2EI/L : Units of N-m
        
        nu=0.35
        W = 75
            # L = W/sqrt(2)
        l=L
        n_min = 1
        n_max = 7
        # Cross Section inputs, must be floats
        mass=125000 #before for voxel
        mass=10
        E = 2000  # MPa
        G = E * 1 / 3  # MPa
        h = 2.38  # mm
        b = 2.38 # mm
        rho = 7.85e-9 / 3  # kg/mm^3
        S = h * b
        Sy = (S * (6 + 12 * nu + 6 * nu^2)/ (7 + 12 * nu + 4 * nu^2))
        # For solid rectangular cross section (width=b, depth=d & ( b < d )):
        Q = 1 / 3 - 0.2244 / (min(h / b, b / h) + 0.1607)
        Jxx = Q * min(h * b^3, b * h^3)
        s=b

        MaxFreq2=E*s/mass
        dt= 1/(6.283185*sqrt(MaxFreq2))


        ##if voxels
        #nu=0
        #L=l
        #a1 = E*L # EA/L : Units of N/m
        #a2 = E * L*L*L / (12.0*(1+nu)) # GJ/L : Units of N-m
        #b1 = E*L # 12EI/L^3 : Units of N/m
        #b2 = E*L*L/2.0 # 6EI/L^2 : Units of N (or N-m/m: torque related to linear distance)
        #b3 = E*L*L*L/6.0 # 2EI/L : Units of N-m

        I= b*h^3/12
        J=b*h*(b*b+h*h)/12
        a1=E*b*h/L
        a2=G*J/L
        b1=12*E*I/(L^3)
        b2=6*E*I/(L^2)
        b3=2*E*I/(L)
        
        

        
        #inbounds currentTransverseArea=25.0 #?? change!!!!!! E_area[i]
        @inbounds currentTransverseArea= b*h
        @inbounds _stress=updateStrain(strain,E)
        
        #@inbounds currentTransverseArea= E_area[i]
        #@inbounds _stress=updateStrain(strain,E_stiffness[i])
        
        @inbounds E_stress[i]=_stress
        
        #@cuprintln("_stress $_stress")
        x=(_stress*currentTransverseArea)
        @inbounds y=(b1*E_pos2[i].y-b2*(E_angle1v[i].z + E_angle2v[i].z))
        @inbounds z=(b1*E_pos2[i].z + b2*(E_angle1v[i].y + E_angle2v[i].y))
        
        x=convert(Float64,x)
        y=convert(Float64,y)
        z=convert(Float64,z)
        
        # Use Curstress instead of -a1*Pos2.x to account for non-linear deformation 
        forceNeg = Vector3(x,y,z)
        
        forcePos = Vector3(-x,-y,-z)
        
        @inbounds x= (a2*(E_angle2v[i].x-E_angle1v[i].x))
        @inbounds y= (-b2*E_pos2[i].z-b3*(2.0*E_angle1v[i].y+E_angle2v[i].y))
        @inbounds z=(b2*E_pos2[i].y - b3*(2.0*E_angle1v[i].z + E_angle2v[i].z))  
        x=convert(Float64,x)
        y=convert(Float64,y)
        z=convert(Float64,z)
        momentNeg = Vector3(x,y,z)
        

        @inbounds x= (a2*(E_angle1v[i].x-E_angle2v[i].x))
        @inbounds y= (-b2*E_pos2[i].z- b3*(E_angle1v[i].y+2.0*E_angle2v[i].y))
        @inbounds z=(b2*E_pos2[i].y - b3*(E_angle1v[i].z + 2.0*E_angle2v[i].z))
        x=convert(Float64,x)
        y=convert(Float64,y)
        z=convert(Float64,z)
        momentPos = Vector3(x,y,z)
        
        
        ### damping
        @inbounds if E_damp[i] #first pass no damping
            sqA1=CUDAnative.sqrt(a1) 
            sqA2xIp=CUDAnative.sqrt(a2*L*L/6.0) 
            sqB1=CUDAnative.sqrt(b1) 
            sqB2xFMp=CUDAnative.sqrt(b2*L/2) 
            sqB3xIp=CUDAnative.sqrt(b3*L*L/6.0)
            
            dampingMultiplier=Vector3(28099.3,28099.3,28099.3) # 2*mat->_sqrtMass*mat->zetaInternal/previousDt;?? todo link to material
            
            zeta=1
            dampingM= 2*sqrt(mass)*zeta/dt
            dampingMultiplier=Vector3(dampingM,dampingM,dampingM)
            
            posCalc=Vector3(sqA1*dPos2.x, sqB1*dPos2.y - sqB2xFMp*(dAngle1.z+dAngle2.z),sqB1*dPos2.z + sqB2xFMp*(dAngle1.y+dAngle2.y))
            
            
            forceNeg =forceNeg + (dampingMultiplier*posCalc);
            forcePos =forcePos - (dampingMultiplier*posCalc);

            momentNeg -= Vector3(0.5,0.5,0.5)*dampingMultiplier*Vector3(-sqA2xIp*(dAngle2.x - dAngle1.x),
                                                                    sqB2xFMp*dPos2.z + sqB3xIp*(2*dAngle1.y + dAngle2.y),
                                                                    -sqB2xFMp*dPos2.y + sqB3xIp*(2*dAngle1.z + dAngle2.z));
            momentPos -= Vector3(0.5,0.5,0.5)*dampingMultiplier*Vector3(sqA2xIp*(dAngle2.x - dAngle1.x),
                                                                sqB2xFMp*dPos2.z + sqB3xIp*(dAngle1.y + 2*dAngle2.y),
                                                                -sqB2xFMp*dPos2.y + sqB3xIp*(dAngle1.z + 2*dAngle2.z));

        else
        @inbounds E_damp[i]=true 
        end

        smallAngle=true
        if !smallAngle # ?? check
            @inbounds forceNeg = RotateVec3DInv(E_angle1[i],forceNeg)
            @inbounds momentNeg = RotateVec3DInv(E_angle1[i],momentNeg)
        end
        
        @inbounds forcePos = RotateVec3DInv(E_angle2[i],forcePos)
        @inbounds momentPos = RotateVec3DInv(E_angle2[i],momentPos)

        @inbounds forceNeg =toAxisOriginalVector3(forceNeg,E_axis[i])
        @inbounds forcePos =toAxisOriginalVector3(forcePos,E_axis[i])

        @inbounds momentNeg=toAxisOriginalQuat(momentNeg,E_axis[i])# TODOO CHECKKKKKK
        @inbounds momentPos=toAxisOriginalQuat(momentPos,E_axis[i])


        @inbounds E_intForce1[i] =forceNeg
        @inbounds E_intForce2[i] =forcePos
        


        @inbounds x= momentNeg.x
        @inbounds y= momentNeg.y
        @inbounds z= momentNeg.z  
        x=convert(Float64,x)
        y=convert(Float64,y)
        z=convert(Float64,z)
        
        @inbounds E_intMoment1[i]=Vector3(x,y,z)

        @inbounds x= momentNeg.x
        @inbounds y= momentNeg.y
        @inbounds z= momentNeg.z
        x=convert(Float64,x)
        y=convert(Float64,y)
        z=convert(Float64,z)
        
        @inbounds E_intMoment2[i]=Vector3(x,y,z)
        
        #x=E_pos2[i].x*10000000000
        #y=E_pos2[i].y*10000000000
        #z=E_pos2[i].z*10000000000
        #@cuprintln("pos2 x $x, y $y, z $z ")
        ##x=E_intMoment2[i].x*10000000000
        #y=E_intMoment2[i].y*10000000000
        #z=E_intMoment2[i].z*10000000000
        #@cuprintln("E_intMoment2 x $x, y $y, z $z ")

        
        
    end
    return
end

function run_updateEdges!(E_source,E_target,E_area,E_density,E_stiffness,E_stress,E_axis,E_currentRestLength,E_pos2,E_angle1v,E_angle2v,E_angle1,E_angle2,E_intForce1,E_intMoment1,E_intForce2,E_intMoment2,E_damp,N_currPosition,N_orient)
    
    N=length(E_source)
    numblocks = ceil(Int, N/256)
    CuArrays.@sync begin
        @cuda threads=256 blocks=numblocks updateEdges!(E_source,E_target,E_area,E_density,
            E_stiffness,E_stress,E_axis,E_currentRestLength,E_pos2,E_angle1v,
            E_angle2v,E_angle1,E_angle2,E_intForce1,E_intMoment1,E_intForce2,
            E_intMoment2,E_damp,N_currPosition,N_orient)
    end
end
#####################################################

function floorPenetration(y)
    nomSize=0.001
    floor=-25
    m=0.0
    if(y-floor<0.0)
        m=(-(y-floor))
    end
    return m
end

#Returns the interference (in meters) between the collision envelope of this voxel and the floor at Z=0. Positive numbers correspond to interference. If the voxel is not touching the floor 0 is returned.
function penetrationStiffness()
    E=2000.0
    nomSize=0.001
    mass=10
    massInverse=1.0/mass #
    
    return (2.0*E*nomSize)
end 
#!< returns the stiffness with which this voxel will resist penetration. This is calculated according to E*A/L with L = voxelSize/2.

function collisionDampingTranslateC()
    E=2000.0
    nomSize=0.001
    mass=10
    massInverse=1.0/mass #
    _2xSqMxExS = (2.0*CUDAnative.sqrt(mass*E*nomSize));
    zetaCollision=0.5;
    return zetaCollision*_2xSqMxExS;
end #!< Returns the global material damping coefficient (translation)


function floorForce!(dt,pTotalForce,y ,linMom,FloorStaticFriction)
    E=2000.0
    nomSize=0.001
    mass=10
    massInverse=1.0/mass #
    
    CurPenetration = floorPenetration(convert(Float64,y)); #for now use the average.
    muStatic=0.2 ;
    muKinetic=0.01; #normal force = 1e3*0.001


    if (CurPenetration>=0.0)
        vel = linMom*Vector3((massInverse),(massInverse),(massInverse)) #Returns the 3D velocity of this voxel in m/s (GCS)
        horizontalVel= Vector3(convert(Float64,vel.x), 0.0, convert(Float64,vel.z));
        normalForce = 2.0*E*nomSize*CurPenetration;
        pTotalForce=Vector3( pTotalForce.x, convert(Float64,pTotalForce.y) + normalForce - collisionDampingTranslateC()*convert(Float64,vel.y),pTotalForce.z)
        #in the z direction: k*x-C*v - spring and damping

        if (FloorStaticFriction) #If this voxel is currently in static friction mode (no lateral motion) 
            # assert(horizontalVel.Length2() == 0);
            surfaceForceSq = convert(Float64,(pTotalForce.x*pTotalForce.x + pTotalForce.z*pTotalForce.z)); #use squares to avoid a square root
            frictionForceSq = (muStatic*normalForce)*(muStatic*normalForce);


            if (surfaceForceSq > frictionForceSq) 
                FloorStaticFriction=false; #if we're breaking static friction, leave the forces as they currently have been calculated to initiate motion this time step
            end

        else  #even if we just transitioned don't process here or else with a complete lack of momentum it'll just go back to static friction
            #add a friction force opposing velocity according to the normal force and the kinetic coefficient of friction
            leng=CUDAnative.sqrt((convert(Float64,vel.x) * convert(Float64,vel.x)) + (0.0 * 0.0) + (convert(Float64,vel.z) * convert(Float64,vel.z)))
            horizontalVel= Vector3(convert(Float64,vel.x)/(leng+0.00000000001),0.0,convert(Float64,vel.z)/(leng+0.00000000001))
            pTotalForce = pTotalForce- Vector3(muKinetic*normalForce,muKinetic*normalForce,muKinetic*normalForce) * horizontalVel;
        end

    else 
        FloorStaticFriction=false;
    end
    
    
    
    return pTotalForce,FloorStaticFriction
end

#####################################################
function updateNodes!(dt,currentTimeStep,N_position, N_restrained,N_displacement,N_angle,N_currPosition,N_linMom,N_angMom,N_intForce,N_intMoment,N_force,N_moment,N_orient,N_edgeID,N_edgeFirst,E_intForce1,E_intMoment1,E_intForce2,E_intMoment2)

    index = (blockIdx().x - 1) * blockDim().x + threadIdx().x
    stride = blockDim().x * gridDim().x
    ## @cuprintln("thread $index, block $stride")
    N,M=size(N_edgeID)
    for i = index:stride:N
        if false
        # @inbounds if N_restrained[i]
            return
        else
            for j in 1:M
                temp=N_edgeID[i,j]
                @inbounds if (N_edgeID[i,j]!=-1)
                    #@cuprintln("i $i, j $j, N_edgeID[i,j] $temp")
                    @inbounds N_intForce[i]=ifelse(N_edgeFirst[i,j], N_intForce[i]+E_intForce1[N_edgeID[i,j]], N_intForce[i]+E_intForce2[N_edgeID[i,j]] )
                    @inbounds N_intMoment[i]=ifelse(N_edgeFirst[i,j], N_intMoment[i]+E_intMoment1[N_edgeID[i,j]], N_intMoment[i]+E_intMoment2[N_edgeID[i,j]] )
                end
            end
            @inbounds curForce = force(N_intForce[i],N_orient[i],N_force[i],true,currentTimeStep)
            
            gravity=true;
            isFloorEnabled=true;
            static=false;
            FloorStaticFriction=false;
            
            E=2000.0
            nomSize=0.001
            mass=10
            massInverse=1.0/mass #
            
            
            grav=-mass*9.80665*0.1;
            if(gravity && !static)
                curForce =curForce +Vector3(0,grav,0);
            end
            
            fricForce = Vector3(curForce.x,curForce.y,curForce.z);
            if (isFloorEnabled)
                @inbounds curForce,FloorStaticFriction=floorForce!(dt,curForce,N_currPosition[i].y,Vector3(N_linMom[i].x,N_linMom[i].y ,N_linMom[i].z),FloorStaticFriction)
                
            end
            
            fricForce = curForce-fricForce;
            
            
            #########################################
            
            @inbounds N_intForce[i]=Vector3(0,0,0)
        
            
            @inbounds N_linMom[i]=N_linMom[i]+curForce*Vector3(dt,dt,dt) #todo make sure right
            @inbounds translate=N_linMom[i]*Vector3((dt*massInverse),(dt*massInverse),(dt*massInverse)) # ??massInverse
            
            
            ############################
            
            
            #we need to check for friction conditions here (after calculating the translation) and stop things accordingly
            @inbounds if (isFloorEnabled && floorPenetration( N_currPosition[i].y)>= 0.0 && !static)#we must catch a slowing voxel here since it all boils down to needing access to the dt of this timestep.
                work =convert(Float64,fricForce.x*translate.x + fricForce.z*translate.z); #F dot disp
                hKe = 0.5*massInverse*convert(Float64,(N_linMom[i].x*N_linMom[i].x + N_linMom[i].z*N_linMom[i].z)); #horizontal kinetic energy

                if((hKe + work) <= 0.0) 
                    FloorStaticFriction=true; #this checks for a change of direction according to the work-energy principle
                end

                if(FloorStaticFriction)
                    #if we're in a state of static friction, zero out all horizontal motion
                    N_linMom[i]=Vector3(0.0 ,convert(Float64,N_linMom[i].y) ,0.0)
                    translate=Vector3(0.0 ,convert(Float64,translate.y) ,0.0)
                end
            else
                FloorStaticFriction=false
            end
            
            
            
            

            
            
            
            #x=translate.x*10000000000
            #y=translate.y*10000000000
            #z=translate.z*10000000000
            #@cuprintln("translate x $x, y $y, z $z ")
            
            @inbounds N_currPosition[i]=N_currPosition[i]+translate
            @inbounds N_displacement[i]=N_displacement[i]+translate
            
            
            
            # Rotation
            @inbounds curMoment = moment(N_intMoment[i],N_orient[i],N_moment[i]) 
            
            
            
            @inbounds N_intMoment[i]=Vector3(0,0,0) # do i really need it?
            
            @inbounds N_angMom[i]=N_angMom[i]+curMoment*Vector3(dt,dt,dt)
            
            
            
            
            momentInertiaInverse=1.92e-6 # todo ?? later change 1/Inertia (1/(kg*m^2))
            
            
            @inbounds temp=FromRotationVector(N_angMom[i]*Vector3((dt*momentInertiaInverse),(dt*momentInertiaInverse),(dt*momentInertiaInverse)))
            
            
            #x=temp.x*10000000000
            #y=temp.y*10000000000
            #z=temp.z*10000000000
            #@cuprintln("temp x $x, y $y, z $z ")
            
            @inbounds N_orient[i]=multiplyQuaternions(temp,N_orient[i])
            
            #@inbounds x= N_orient[i].x*temp.x
            #@inbounds y= N_orient[i].y*temp.y
            #@inbounds z= N_orient[i].z*temp.z
            #@inbounds w= N_orient[i].w*temp.w
            #x=convert(Float64,x)
            #y=convert(Float64,y)
            #z=convert(Float64,z)
            #w=convert(Float64,w)
            
            #@inbounds N_orient[i]=Quaternion(x,y,z,w)
            
            #x=N_orient[i].x*10000000000
            #y=N_orient[i].y*10000000000
            #z=N_orient[i].z*10000000000
            #w=N_orient[i].w
            #@cuprintln("N_orient x $x, y $y, z $z, w $w ")
            
            
        end
    end
    return
end


function run_updateNodes!(dt,currentTimeStep,N_position, N_restrained,N_displacement, N_angle,N_currPosition,N_linMom,N_angMom,N_intForce,N_intMoment,N_force,N_moment,N_orient,N_edgeID,N_edgeFirst,E_intForce1,E_intMoment1,E_intForce2,E_intMoment2)
    N=length(N_intForce)
    numblocks = ceil(Int, N/256)
    CuArrays.@sync begin
        @cuda threads=256 blocks=numblocks updateNodes!(dt,currentTimeStep,N_position, N_restrained,N_displacement, N_angle,N_currPosition,N_linMom,N_angMom,N_intForce,N_intMoment,N_force,N_moment,N_orient,N_edgeID,N_edgeFirst,E_intForce1,E_intMoment1,E_intForce2,E_intMoment2)
    end
end
#####################################################
function orientLink!(currentRestLength,pVNeg,pVPos,oVNeg,oVPos,axis)  # updates pos2, angle1, angle2, and smallAngle //Quat3D<double> /*double restLength*/
        
    pos2 = toAxisXVector3(pVPos-pVNeg,axis) # digit truncation happens here...
    angle1 = toAxisXQuat(oVNeg,axis)
    angle2 = toAxisXQuat(oVPos,axis)

    
    
    
    totalRot = conjugate(angle1) #keep track of the total rotation of this bond (after toAxisX()) # Quat3D<double>
    pos2 = RotateVec3D(totalRot,pos2)
    
    #x=pos2.x*10000000000
    #y=pos2.y*10000000000
    #z=pos2.z*10000000000
    #@cuprintln("pos2 2 x $x, y $y, z $z ")
    
    
    #x=totalRot.x*10000000000
    #y=totalRot.y*10000000000
    #z=totalRot.z*10000000000
    #@cuprintln("totalRot x $x, y $y, z $z ")
    
    
    # x=pos2.x*10000000000
    # y=pos2.y*10000000000
    # z=pos2.z*10000000000
    # @cuprintln("pos2 x $x, y $y, z $z ")
    
    angle2 = Quaternion(angle2.x*totalRot.x,angle2.y*totalRot.y,angle2.z*totalRot.z,angle2.w*totalRot.w)
    angle1 = Quaternion(0.0,0.0,0.0,1.0)#new THREE.Quaternion() #zero for now...

    smallAngle=true #todo later remove
    
    
    if (smallAngle)	 #Align so Angle1 is all zeros
        #pos2[1] =pos2[1]- currentRestLength #only valid for small angles
        pos2=Vector3(pos2.x-currentRestLength,pos2.y,pos2.z)
    else  #Large angle. Align so that Pos2.y, Pos2.z are zero.
        # FromAngleToPosX(angle1,pos2) #get the angle to align Pos2 with the X axis
        # totalRot = angle1.clone().multiply(totalRot)  #update our total rotation to reflect this
        # angle2 = angle1.clone().multiply(  angle2) #rotate angle2
        # pos2 = new THREE.Vector3(pos2.length() - currentRestLength, 0, 0);
    end
    
    angle1v = ToRotationVector(angle1)
    angle2v = ToRotationVector(angle2)
    
    
    
    
    # pos2,angle1v,angle2v,angle1,angle2,
    return pos2,angle1v,angle2v,angle1,angle2,totalRot
end
#####################################################
function toAxisXVector3(pV::Vector3,axis::Vector3) #TODO CHANGE
    xaxis=Vector3(1.0,0.0,0.0)

    vector=normalizeVector3(axis)
    q=setFromUnitVectors(vector,xaxis)

    v=applyQuaternion1( pV ,q )
    
    return Vector3(v.x,v.y,v.z)
end

function toAxisOriginalVector3(pV::Vector3,axis::Vector3)
    
    xaxis=Vector3(1.0,0.0,0.0)

    vector=normalizeVector3(axis)

    q=setFromUnitVectors(xaxis,vector)
    
    v=applyQuaternion1( pV ,q )
    
    return Vector3(v.x,v.y,v.z)
end

function  toAxisXQuat(pQ::Quaternion,axis::Vector3)
    
    xaxis=Vector3(1.0,0.0,0.0)

    vector=normalizeVector3(axis)

    q=setFromUnitVectors(vector,xaxis)
        
    
    pV=Vector3(pQ.x,pQ.y,pQ.z)
    
    v=applyQuaternion1( pV ,q )
    

    
    return Quaternion(v.x,v.y,v.z,1.0)
    
end

function toAxisOriginalQuat(pQ::Vector3,axis::Vector3)
    xaxis=Vector3(1.0,0.0,0.0)

    vector=normalizeVector3(axis)
    
    q=setFromUnitVectors(xaxis,vector)
    
    
    pV=Vector3(pQ.x,pQ.y,pQ.z)
    v=applyQuaternion1( pV ,q )
    
    return Quaternion(v.x,v.y,v.z,1.0)
    
end
#####################################################
function setFromUnitVectors(vFrom::Vector3, vTo::Vector3)
    # assumes direction vectors vFrom and vTo are normalized
    EPS = 0.000000001;
    r= dotVector3(vFrom,vTo)+1.0
    # r =  dot(vFrom,vTo)+1

    if r < EPS
        r = 0;
        if abs( vFrom.x ) > abs( vFrom.z ) 
            qx = - vFrom.y
            qy = vFrom.x
            qz = 0.0
            qw = r
        else 
            qx = 0.0
            qy = -(vFrom.z)
            qz = vFrom.y
            qw = r
        end
   else 
        # crossVectors( vFrom, vTo ); // inlined to avoid cyclic dependency on Vector3
        qx = vFrom.y * vTo.z - vFrom.z * vTo.y
        qy = vFrom.z * vTo.x - vFrom.x * vTo.z
        qz = vFrom.x * vTo.y - vFrom.y * vTo.x
        qw = r

    end
    qx= (qx==-0.0) ? 0.0 : qx
    qy= (qy==-0.0) ? 0.0 : qy
    qz= (qz==-0.0) ? 0.0 : qz
    qw= (qw==-0.0) ? 0.0 : qw
        
    
    mx=qx*qx
    my=qy*qy
    mz=qz*qz
    mw=qw*qw
    mm=mx+my
    mm=mm+mz
    mm=mm+mw
    mm=convert(Float64,mm)#??????????????????? todo check later
    
    l=CUDAnative.sqrt(mm)
    
    #l = sqrt((qx * qx) + (qy * qy) + (qz * qz)+ (qw * qw))
    if l === 0 
        qx = 0.0
        qy = 0.0
        qz = 0.0
        qw = 1.0
    else 
        l = 1.0 / l
        qx = qx * l
        qy = qy * l
        qz = qz * l
        qw = qw * l
    end
    
    

    # return qx,qy,qz,qw
    return Quaternion(qx,qy,qz,qw)
    
    # return normalizeQ(Quat(qw,qx,qy,qz))
    # return Quat(nn[1], nn[2], nn[3], nn[4])

end

function quatToMatrix( quaternion::Quaternion)

    #te = RotationMatrix()
    
    x = quaternion.x
    y = quaternion.y
    z = quaternion.z
    w = quaternion.w
    
    x2 = x + x
    y2 = y + y
    z2 = z + z
    xx = x * x2
    xy = x * y2
    xz = x * z2
    yy = y * y2
    yz = y * z2
    zz = z * z2
    wx = w * x2
    wy = w * y2
    wz = w * z2

    sx = 1.0
    sy = 1.0
    sz = 1.0

    te1 = ( 1.0 - ( yy + zz ) ) * sx
    te2 = ( xy + wz ) * sx
    te3 = ( xz - wy ) * sx
    te4 = 0.0

    te5 = ( xy - wz ) * sy
    te6 = ( 1.0 - ( xx + zz ) ) * sy
    te7 = ( yz + wx ) * sy
    te8 = 0.0

    te9 = ( xz + wy ) * sz
    te10 = ( yz - wx ) * sz
    te11 = ( 1.0 - ( xx + yy ) ) * sz
    te12 = 0.0

    te13 = 0.0 #position.x;
    te14 = 0.0 #position.y;
    te15 = 0.0 #position.z;
    te16 = 1.0
    
        
    te= RotationMatrix(te1,te2,te3,te4,te5,te6,te7,te8,te9,te10,te11,te12,te13,te14,te15,te16)

    return te

end

function  setFromRotationMatrix(m::RotationMatrix)

    m11 = convert(Float64,m.te1 )
    m12 = convert(Float64,m.te5 )
    m13 = convert(Float64,m.te9 )
    m21 = convert(Float64,m.te2 )
    m22 = convert(Float64,m.te6 )
    m23 = convert(Float64,m.te10)
    m31 = convert(Float64,m.te3 )
    m32 = convert(Float64,m.te7 )
    m33 = convert(Float64,m.te11)
    

    y = CUDAnative.asin( clamp( m13, -1.0, 1.0 ) ) ##check if has to be changed to cuda

    if ( abs( m13 ) < 0.9999999999 ) 
        
        x = CUDAnative.atan2( - m23, m33 )
        z = CUDAnative.atan2( - m12, m11 )#-m12, m11


    else

        x = CUDAnative.atan2( m32, m22 )
        z = 0.0;

    end
    
    
    return Vector3(x,y,z)
    
end

function setQuaternionFromEuler(euler::Vector3)
    x=euler.x
    y=euler.y
    z=euler.z
    
    
    c1 = CUDAnative.cos( x / 2.0 )
    c2 = CUDAnative.cos( y / 2.0 )
    c3 = CUDAnative.cos( z / 2.0 )

    s1 = CUDAnative.sin( x / 2.0 )
    s2 = CUDAnative.sin( y / 2.0 )
    s3 = CUDAnative.sin( z / 2.0 )
    
   
    x = s1 * c2 * c3 + c1 * s2 * s3
    y = c1 * s2 * c3 - s1 * c2 * s3
    z = c1 * c2 * s3 + s1 * s2 * c3
    w = c1 * c2 * c3 - s1 * s2 * s3
        
    return Quaternion(x,y,z,w)
end

function applyQuaternion1(e::Vector3,q2::Quaternion)
    x = e.x
    y = e.y
    z = e.z

    qx = q2.x
    qy = q2.y
    qz = q2.z
    qw = q2.w

    # calculate quat * vector

    ix = qw * x + qy * z - qz * y
    iy = qw * y + qz * x - qx * z
    iz = qw * z + qx * y - qy * x
    iw = - qx * x - qy * y - qz * z

    # calculate result * inverse quat

    xx = ix * qw + iw * - qx + iy * - qz - iz * - qy
    yy = iy * qw + iw * - qy + iz * - qx - ix * - qz
    zz = iz * qw + iw * - qz + ix * - qy - iy * - qx
    
    d=15

    return Vector3(xx,yy,zz)
end

#####################################################
function conjugate(q::Quaternion)
    x= (-q.x==-0) ? 0.0 : -q.x
    y= (-q.y==-0) ? 0.0 : -q.y
    z= (-q.z==-0) ? 0.0 : -q.z
    w=q.w
    x=convert(Float64,x)
    y=convert(Float64,y)
    z=convert(Float64,z)
    w=convert(Float64,w)
    return Quaternion(x,y,z,w)