Newer
Older
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
# Amira Abdel-Rahman
# (c) Massachusetts Institute of Technology 2020
# BASED ON https://github.com/jonhiller/Voxelyze
function force(i,N_intForce,N_orient,N_force,N_position,currentTimeStep,material,linMom)
# forces from internal bonds
totalForce=Vector3(0,0,0)
# new THREE.Vector3(node.force.x,node.force.y,node.force.z);
# todo
totalForce=totalForce+N_intForce
if(i==10)
# x=translate.x*1e6
# y=translate.y*1e6
# z=translate.z*1e6
# @cuprintln("translate x $x 1e-6, y $y 1e-6, z $z 1e-6")
# x=totalForce.x*1e6
# y=totalForce.y*1e6
# z=totalForce.z*1e6
# @cuprintln("totalForce x $x 1e-6, y $y 1e-6, z $z 1e-6")
end
totalForce = RotateVec3D(N_orient,totalForce); # from local to global coordinates
totalForce=totalForce+externalForce(currentTimeStep,N_position,N_force)
mass=material.mass
massInverse=material.massInverse
globalDampingTranslateC= material.zetaGlobal*material._2xSqMxExS
# x=totalForce.x*1e6
# y=totalForce.y*1e6
# z=totalForce.z*1e6
# @cuprintln("totalForce 2 x $x 1e-6, y $y 1e-6, z $z 1e-6")
vel=linMom*Vector3((massInverse*globalDampingTranslateC),(massInverse*globalDampingTranslateC),(massInverse*globalDampingTranslateC))
# x=vel.x*1e0/globalDampingTranslateC
# y=vel.y*1e0/globalDampingTranslateC
# z=vel.z*1e0/globalDampingTranslateC
# @cuprintln("vel x $x, y $y, z $z ")
totalForce =totalForce - vel; #global damping f-cv
# other forces
# if(!static)
#massInverse=1.0/mass #
#vel = linMom*Vector3((massInverse),(massInverse),(massInverse))
#totalForce =totalForce- vel*globalDampingTranslateC(); #global damping f-cv
gravity=gravityEnabled();
if(gravity)
totalForce =totalForce +Vector3(0,grav,0);
end
# end
# x=totalForce.x*1e6
# y=totalForce.y*1e6
# z=totalForce.z*1e6
# @cuprintln("totalForce 3 x $x 1e-6, y $y 1e-6, z $z 1e-6")
return totalForce
end
function moment(intMoment,orient,moment,material,angMom)
#moments from internal bonds
totalMoment=Vector3(0,0,0)
# for (int i=0; i<6; i++){
# if (links[i]) totalMoment += links[i]->moment(isNegative((linkDirection)i)); //total force in LCS
# }
totalMoment=totalMoment+intMoment
totalMoment = RotateVec3D(orient,totalMoment);
totalMoment=totalMoment+moment
#other moments
# if (externalExists()) totalMoment += external()->moment(); //external moments
# totalMoment -= angularVelocity()*mat->globalDampingRotateC(); //global damping
_2xSqIxExSxSxS = 2.0*sqrt(material.inertia*material.E*material.nomSize*material.nomSize*material.nomSize)
globalDampingRotateC=material.zetaGlobal*_2xSqIxExSxSxS
angularVelocity=angMom*Vector3(material.momentInertiaInverse*globalDampingRotateC,material.momentInertiaInverse*globalDampingRotateC,material.momentInertiaInverse*globalDampingRotateC)
totalMoment= totalMoment-angularVelocity; #global damping
# x=totalMoment.x
# y=totalMoment.y
# z=totalMoment.z
# @cuprintln("totalMoment x $x, y $y, z $z ")
return totalMoment
end
################################################################
p=0.0
d=10.0
if(y<floor)
# if(y<floor&& (x<5.0*d || x>=14.0*d))
p=floor-y
end
# if(y<floor)
# p=floor-y
# end
return p
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,nomSize)
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 globalDampingTranslateC(_2xSqMxExS,zetaGlobal)
return zetaGlobal*_2xSqMxExS;
end #!< Returns the global material damping coefficient (translation)
function collisionDampingTranslateC(_2xSqMxExS,zetaCollision)
# _2xSqIxExSxSxS = (2.0f*sqrt(_momentInertia*E*nomSize*nomSize*nomSize));
return zetaCollision*_2xSqMxExS;
end #!< Returns the global material damping coefficient (translation)
function floorForce!(dt,pTotalForce,pos,linMom,FloorStaticFriction,N_material)
E=convert(Float64,N_material.E)
nomSize=convert(Float64,N_material.nomSize)
mass=convert(Float64,N_material.mass)
massInverse=convert(Float64,N_material.massInverse)
muStatic=convert(Float64,N_material.muStatic)*1.0
muKinetic=convert(Float64,N_material.muKinetic)*100.0
_2xSqMxExS=convert(Float64,N_material._2xSqMxExS)
zetaCollision=convert(Float64,N_material.zetaCollision)
CurPenetration = floorPenetration(convert(Float64,pos.x),convert(Float64,pos.y),nomSize); #for now use the average.
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 = penetrationStiffness(E,nomSize)*CurPenetration;
pTotalForce=Vector3( pTotalForce.x, convert(Float64,pTotalForce.y) + normalForce - collisionDampingTranslateC(_2xSqMxExS,zetaCollision)*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=sqrt((convert(Float64,vel.x) * convert(Float64,vel.x)) + (0.0 * 0.0) + (convert(Float64,vel.z) * convert(Float64,vel.z)))
if(leng>0)
horizontalVel= Vector3(convert(Float64,vel.x)/(leng),0.0,convert(Float64,vel.z)/(leng))
else
horizontalVel= Vector3(convert(Float64,vel.x)*(leng),0.0,convert(Float64,vel.z)*(leng))
end
pTotalForce = pTotalForce- Vector3(muKinetic*normalForce,muKinetic*normalForce,muKinetic*normalForce) * horizontalVel;
end
else
FloorStaticFriction=false;
end
return pTotalForce,FloorStaticFriction
end
################################################################