# Amira Abdel-Rahman
# (c) Massachusetts Institute of Technology 2020
using LinearAlgebra
using Plots
import JSON
using StaticArrays, Rotations
# BASED ON https://github.com/jonhiller/Voxelyze
function simulateParallel(setup,numTimeSteps,dt,static=true,saveInterval=10)
initialize(setup)
for i in 1:numTimeSteps
# println("Timestep:",i)
doTimeStep(setup,dt,static,i,saveInterval);
end
end
function initialize(setup)
nodes = setup["nodes"]
edges = setup["edges"]
# pre-calculate current position
for node in nodes
# element=parse(Int,node["id"][2:end])
append!(N_position,[[node["position"]["x"] node["position"]["y"] node["position"]["z"]]])
append!(N_degrees_of_freedom,[node["degrees_of_freedom"]])
append!(N_restrained_degrees_of_freedom, [node["restrained_degrees_of_freedom"]])
append!(N_displacement,[[node["displacement"]["x"] node["displacement"]["y"] node["displacement"]["z"]]])
append!(N_angle,[[node["angle"]["x"] node["angle"]["y"] node["angle"]["z"]]])
append!(N_force,[[node["force"]["x"] node["force"]["y"] node["force"]["z"]]])
append!(N_currPosition,[[node["position"]["x"] node["position"]["y"] node["position"]["z"]]])
append!(N_orient,[Quat(1.0,0.0,0.0,0.0)])#quat
append!(N_linMom,[[0 0 0]])
append!(N_angMom,[[0 0 0]])
append!(N_intForce,[[0 0 0]])
append!(N_intMoment,[[0 0 0]])
append!(N_moment,[[0 0 0]])
# for dynamic simulations
append!(N_posTimeSteps,[[]])
append!(N_angTimeSteps,[[]])
end
# pre-calculate the axis
for edge in edges
# element=parse(Int,edge["id"][2:end])
# find the nodes that the lements connects
fromNode = nodes[edge["source"]+1]
toNode = nodes[edge["target"]+1]
node1 = [fromNode["position"]["x"] fromNode["position"]["y"] fromNode["position"]["z"]]
node2 = [toNode["position"]["x"] toNode["position"]["y"] toNode["position"]["z"]]
length=norm(node2-node1)
axis=normalize(collect(Iterators.flatten(node2-node1)))
append!(E_source,[edge["source"]+1])
append!(E_target,[edge["target"]+1])
append!(E_area,[edge["area"]])
append!(E_density,[edge["density"]])
append!(E_stiffness,[edge["stiffness"]])
append!(E_stress,[0])
append!(E_axis,[axis])
append!(E_currentRestLength,[length])
append!(E_pos2,[[0 0 0]])
append!(E_angle1v,[[0 0 0]])
append!(E_angle2v,[[0 0 0]])
append!(E_angle1,[Quat(1.0,0,0,0)]) #quat
append!(E_angle2,[Quat(1.0,0,0,0)]) #quat
append!(E_currentTransverseStrainSum,[0])
# for dynamic simulations
append!(E_stressTimeSteps,[[]])
end
end
function doTimeStep(setup,dt,static,currentTimeStep,saveInterval)
nodes = setup["nodes"]
edges = setup["edges"]
voxCount=size(nodes)[1]
linkCount=size(edges)[1]
if dt==0
return true
elseif (dt<0)
dt = recommendedTimeStep()
end
# if (collisions) updateCollisions();
collisions=false
# Euler integration:
Diverged = false
# for edge in edges
for i in 1:linkCount
# fromNode = nodes[edge["source"]+1]
# toNode = nodes[edge["target"]+1]
# node1 = [fromNode["position"]["x"] fromNode["position"]["y"] fromNode["position"]["z"]]
# node2 = [toNode["position"]["x"] toNode["position"]["y"] toNode["position"]["z"]]
# updateForces(setup,edge,node1,node2,static)# element numbers??
updateForces(setup,i,static)# element numbers??
# todo: update forces and whatever
if axialStrain(true) > 100
Diverged = true; # catch divergent condition! (if any thread sets true we will fail, so don't need mutex...
end
end
if Diverged
println("Divergedd!!!!!")
return false
end
for i in 1:voxCount
timeStep(dt,i,static,currentTimeStep)
# timeStep(dt,node,static,currentTimeStep)
if(!static&& currentTimeStep%saveInterval==0)
append!(N_posTimeSteps[i],[N_displacement[i]])
append!(N_angTimeSteps[i],[N_angle[i]])
end
# todo: update linMom,angMom, orient and whatever
end
currentTimeStep = currentTimeStep+dt
return true
end
function updateForces(setup,edge,static=true)
# pVNeg=new THREE.Vector3(node1.position.x,node1.position.y,node1.position.z);
# pVPos=new THREE.Vector3(node2.position.x,node2.position.y,node2.position.z);
# currentRestLength=pVPos.clone().sub(pVNeg).length();
# edge.currentRestLength=currentRestLength; # todo make sure updated
node1=E_source[edge]
node2=E_target[edge]
currentRestLength=E_currentRestLength[edge]
pVNeg=copy(N_currPosition[node1])# todo change to be linked to edge not node
pVPos=copy(N_currPosition[node2])# todo change to be linked to edge not node
# Vec3D<double> three
oldPos2 = copy(E_pos2[edge])
oldAngle1v = copy(E_angle1v[edge])
oldAngle2v = copy(E_angle2v[edge])# remember the positions/angles from last timestep to calculate velocity
# var oldAngle1v=new THREE.Vector3(node1.angle.x,node1.angle.y,node1.angle.z);//?
# var oldAngle2v=new THREE.Vector3(node2.angle.x,node2.angle.y,node2.angle.z); //??
totalRot= orientLink(edge) # sets pos2, angle1, angle2 /*restLength*/
dPos2=0.5*(copy(E_pos2[edge])-oldPos2)
dAngle1=0.5*(copy(E_angle1v[edge])-oldAngle1v)
dAngle2=0.5*(copy(E_angle2v[edge])-oldAngle2v)
# if volume effects...
# if (!mat->isXyzIndependent() || currentTransverseStrainSum != 0)
# updateTransverseInfo(); //currentTransverseStrainSum != 0 catches when we disable poissons mid-simulation
_stress=updateStrain((E_pos2[edge][1]/E_currentRestLength[edge]),E_stiffness[edge])
# var _stress=updateStrain(1.0);
E_stress[edge] = _stress
if !static
append!(E_stressTimeSteps[edge],[_stress])
end
######### check this
if setup["viz"]["minStress"]>_stress
setup["viz"]["minStress"]=_stress
elseif setup["viz"]["maxStress"]<_stress
setup["viz"]["maxStress"]=_stress
end
# if (isFailed()){forceNeg = forcePos = momentNeg = momentPos = Vec3D<double>(0,0,0); return;}
# var b1=mat->_b1, b2=mat->_b2, b3=mat->_b3, a2=mat->_a2; //local copies //todo get from where i had
l = currentRestLength # ??
rho = E_density[edge]
A = E_area[edge]
E = E_stiffness[edge] # youngs modulus
G=1.0 # todo shear_modulus
ixx = 1.0 # todo section ixx
I=1.0
iyy = 1.0 # todo section.iyy//
# var l0=length.dataSync();
J=1.0;# todo check
# var l02 = l0 * l0;
# var l03 = l0 * l0 * l0;
b1= 12*E*I/(l*l*l)
b2= 6*E*I/(l*l)
b3= 2*E*I/(l)
a1= E*A/l
a2= G*J/l
nu=0
b1= 5e6
b2= 1.25e7
b3= 2.08333e+07
a1= E*A/l
a2= 1.04167e+07
L = 5;
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
# console.log("currentRestLength:"+currentRestLength);
# console.log("b1:"+b1/10e6);
# console.log("b2:"+b2/10e7);
# console.log("b3:"+b3/10e7);
# console.log("a2:"+a2/10e7);
# var b1= 5e6;
# var b2= 1.25e7;
# var b3= 2.08333e+07;
# var a1= E*A/l;
# var a2= 1.04167e+07;
currentTransverseArea=25.0 # todo ?? later change
currentTransverseArea=A
# Beam equations. All relevant terms are here, even though some are zero for small angle and others are zero for large angle (profiled as negligible performance penalty)
forceNeg = [(_stress*currentTransverseArea) (b1*E_pos2[edge][2]-b2*(E_angle1v[edge][3] + E_angle2v[edge][3])) (b1*E_pos2[edge][3] + b2*(E_angle1v[edge][2] + E_angle2v[edge][2]))] # Use Curstress instead of -a1*Pos2.x to account for non-linear deformation
forcePos = -forceNeg;
momentNeg = [(a2*(E_angle2v[edge][1]-E_angle1v[edge][1])) (-b2*E_pos2[edge][3]-b3*(2*E_angle1v[edge][2]+E_angle2v[edge][2])) (b2*E_pos2[edge][2] - b3*(2*E_angle1v[edge][3] + E_angle2v[edge][3]))]
momentPos = [(a2*(E_angle1v[edge][1]-E_angle2v[edge][1])) (-b2*E_pos2[edge][3]- b3*(E_angle1v[edge][2]+2*E_angle2v[edge][2])) (b2*E_pos2[edge][2] - b3*(E_angle1v[edge][3] + 2*E_angle2v[edge][3]))]
# //local damping:
# if (isLocalVelocityValid()){ //if we don't have the basis for a good damping calculation, don't do any damping.
# float sqA1=mat->_sqA1, sqA2xIp=mat->_sqA2xIp,sqB1=mat->_sqB1, sqB2xFMp=mat->_sqB2xFMp, sqB3xIp=mat->_sqB3xIp;
# Vec3D<double> posCalc( sqA1*dPos2.x,
# sqB1*dPos2.y - sqB2xFMp*(dAngle1.z+dAngle2.z),
# sqB1*dPos2.z + sqB2xFMp*(dAngle1.y+dAngle2.y));
# forceNeg += pVNeg->dampingMultiplier()*posCalc;
# forcePos -= pVPos->dampingMultiplier()*posCalc;
# momentNeg -= 0.5*pVNeg->dampingMultiplier()*Vec3D<>( -sqA2xIp*(dAngle2.x - dAngle1.x),
# sqB2xFMp*dPos2.z + sqB3xIp*(2*dAngle1.y + dAngle2.y),
# -sqB2xFMp*dPos2.y + sqB3xIp*(2*dAngle1.z + dAngle2.z));
# momentPos -= 0.5*pVPos->dampingMultiplier()*Vec3D<>( sqA2xIp*(dAngle2.x - dAngle1.x),
# sqB2xFMp*dPos2.z + sqB3xIp*(dAngle1.y + 2*dAngle2.y),
# -sqB2xFMp*dPos2.y + sqB3xIp*(dAngle1.z + 2*dAngle2.z));
# }
# else setBoolState(LOCAL_VELOCITY_VALID, true); //we're good for next go-around unless something changes
# transform forces and moments to local voxel coordinates
smallAngle=false # ?? todo check
if !smallAngle # ?? chech
forceNeg = RotateVec3DInv(E_angle1[edge],forceNeg)
momentNeg = RotateVec3DInv(E_angle1[edge],momentNeg)
end
forcePos = RotateVec3DInv(E_angle2[edge],forcePos);
momentPos = RotateVec3DInv(E_angle2[edge],momentPos);
# println(momentPos)
forceNeg =toAxisOriginalVector3(forceNeg,E_axis[edge]);
forcePos =toAxisOriginalVector3(forcePos,E_axis[edge]);
momentNeg=toAxisOriginalQuat(momentNeg,E_axis[edge]);# TODOO CHECKKKKKK
momentPos=toAxisOriginalQuat(momentPos,E_axis[edge]);
# println(momentPos[2]," ",momentPos[3]," ",momentPos[4]," ",momentPos[1]," ")
N_intForce[node1] =N_intForce[node1] +(forceNeg) ;
N_intForce[node2] =N_intForce[node2] +(forcePos) ;
# println(N_intMoment[node2])
N_intMoment[node1]=[(N_intMoment[node1][1]+momentNeg[2]) (N_intMoment[node1][2]+momentNeg[3]) (N_intMoment[node2][3]+momentPos[4])];
N_intMoment[node2]=[(N_intMoment[node2][1]+momentPos[2]) (N_intMoment[node2][2]+momentPos[3]) (N_intMoment[node2][3]+momentPos[4])];
# println(N_intMoment[node2])
# assert(!(forceNeg.x != forceNeg.x) || !(forceNeg.y != forceNeg.y) || !(forceNeg.z != forceNeg.z)); //assert non QNAN
# assert(!(forcePos.x != forcePos.x) || !(forcePos.y != forcePos.y) || !(forcePos.z != forcePos.z)); //assert non QNAN
end
function orientLink( edge) # updates pos2, angle1, angle2, and smallAngle //Quat3D<double> /*double restLength*/
node1=E_source[edge]
node2=E_target[edge]
currentRestLength=E_currentRestLength[edge]
pVNeg=copy(N_currPosition[node1])# todo change to be linked to edge not node
pVPos=copy(N_currPosition[node2])# todo change to be linked to edge not node
pos2 = toAxisXVector3(pVPos-pVNeg,E_axis[edge]) # digit truncation happens here...
# pos2.x = Math.round(pos2.x * 1e4) / 1e4;
angle1 = toAxisXQuat(N_orient[node1],E_axis[edge])
angle2 = toAxisXQuat(N_orient[node2],E_axis[edge])
# println(angle1[2]," ",angle1[3]," ",angle1[4]," ",angle1[1])
totalRot = conjugate(angle1) #keep track of the total rotation of this bond (after toAxisX()) # Quat3D<double>
# println(totalRot.x," ",totalRot.y," ",totalRot.z," ",totalRot.w)
pos2 = RotateVec3D(totalRot,pos2)
# angle2 = copy(totalRot) .* angle2 # todo .*
angle2=Quat(angle2.w*totalRot.w,angle2.x*totalRot.x,angle2.y*totalRot.y,angle2.z*totalRot.z)
angle1 = Quat(1.0,0.0,0.0,0.0)#new THREE.Quaternion() #zero for now...
# small angle approximation?
# var SmallTurn = ((Math.abs(pos2.z)+Math.abs(pos2.y))/pos2.x);
# var ExtendPerc = (Math.abs(1-pos2.x/currentRestLength));
# if (!smallAngle /*&& angle2.IsSmallAngle()*/ && SmallTurn < SA_BOND_BEND_RAD && ExtendPerc < SA_BOND_EXT_PERC){
# smallAngle = true;
# setBoolState(LOCAL_VELOCITY_VALID, false);
# }
# else if (smallAngle && (/*!angle2.IsSmallishAngle() || */SmallTurn > HYSTERESIS_FACTOR*SA_BOND_BEND_RAD || ExtendPerc > HYSTERESIS_FACTOR*SA_BOND_EXT_PERC)){
# smallAngle = false;
# setBoolState(LOCAL_VELOCITY_VALID, false);
# }
smallAngle=true #todo later remove
if (smallAngle) #Align so Angle1 is all zeros
pos2[1] =pos2[1]- currentRestLength #only valid for small angles
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)
# assert(!(angle1v.x != angle1v.x) || !(angle1v.y != angle1v.y) || !(angle1v.z != angle1v.z)); # assert non QNAN
# assert(!(angle2v.x != angle2v.x) || !(angle2v.y != angle2v.y) || !(angle2v.z != angle2v.z)); # assert non QNAN
E_pos2[edge]=copy(pos2)
E_angle1v[edge]=copy(angle1v)
E_angle2v[edge]=copy(angle2v)
E_angle1[edge]=copy(angle1)
E_angle2[edge]=copy(angle2)
return totalRot
end
function RotateVec3D(a, f)
fx= (f[1]==-0) ? 0 : f[1]
fy= (f[2]==-0) ? 0 : f[2]
fz= (f[3]==-0) ? 0 : f[3]
# fx= f[1]
# fy= f[2]
# fz= f[3]
tw = fx*a.x + fy*a.y + fz*a.z
tx = fx*a.w - fy*a.z + fz*a.y
ty = fx*a.z + fy*a.w - fz*a.x
tz = -fx*a.y + fy*a.x + fz*a.w
return [(a.w*tx+a.x*tw+a.y*tz-a.z*ty) (a.w*ty-a.x*tz+a.y*tw+a.z*tx) (a.w*tz+a.x*ty-a.y*tx + a.z*tw)]
end
#!< Returns a vector representing the specified vector "f" rotated by this quaternion. @param[in] f The vector to transform.
function RotateVec3DInv(a, f)
fx=f[1]
fy=f[2]
fz=f[3]
tw = a.x*fx + a.y*fy + a.z*fz
tx = a.w*fx - a.y*fz + a.z*fy
ty = a.w*fy + a.x*fz - a.z*fx
tz = a.w*fz - a.x*fy + a.y*fx
return [(tw*a.x + tx*a.w + ty*a.z - tz*a.y) (tw*a.y - tx*a.z + ty*a.w + tz*a.x) (tw*a.z + tx*a.y - ty*a.x + tz*a.w)]
end
#!< Returns a vector representing the specified vector "f" rotated by the inverse of this quaternion. This is the opposite of RotateVec3D. @param[in] f The vector to transform.
function setFromUnitVectors(vFrom, vTo )
# assumes direction vectors vFrom and vTo are normalized
EPS = 0.000001;
r = dot(vFrom,vTo)+1
if r < EPS
r = 0;
if abs( vFrom.x ) > abs( vFrom.z )
qx = - vFrom[2]
qy = vFrom[1]
qz = 0
qw = r
else
qx = 0
qy = - vFrom[3]
qz = vFrom[2]
qw = r
end
else
# crossVectors( vFrom, vTo ); // inlined to avoid cyclic dependency on Vector3
qx = vFrom[2] * vTo[3] - vFrom[3] * vTo[2]
qy = vFrom[3] * vTo[1] - vFrom[1] * vTo[3]
qz = vFrom[1] * vTo[2] - vFrom[2] * vTo[1]
qw = r
end
qx= (qx==-0) ? 0 : qx
qy= (qy==-0) ? 0 : qy
qz= (qz==-0) ? 0 : qz
qw= (qw==-0) ? 0 : qw
nn=normalize(collect(Iterators.flatten([qw,qx,qy,qz])))
return [nn[1] nn[2] nn[3] nn[4]]
# return normalizeQ(Quat(qw,qx,qy,qz))
# return Quat(nn[1], nn[2], nn[3], nn[4])
end
function normalizeQ(q)
l = norm(q)
if l === 0
qx = 0
qy = 0
qz = 0
qw = 1
else
l = 1 / l
qx = q.x * l
qy = q.y * l
qz = q.z * l
qw = q.w * l
end
return Quat(qw,qx,qy,qz)
end
function conjugate(q)
x= (-q.x==-0) ? 0 : -q.x
y= (-q.y==-0) ? 0 : -q.y
z= (-q.z==-0) ? 0 : -q.z
return Quat(q.w, x, y, z)
end
#Returns a quaternion that is the conjugate of this quaternion. This quaternion is not modified.
function applyQuaternion(q1,q2)
x = q1[2]
y = q1[3]
z = q1[4]
w = q1[1]
qx = q2[2]
qy = q2[3]
qz = q2[4]
qw = q2[1]
# 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
mm=normalize(collect(Iterators.flatten([xx yy zz])))
return [mm[1] mm[2] mm[3]]
end
function applyQuaternion1(e,q2)
x = e[1]
y = e[2]
z = e[3]
qx = q2[2]
qy = q2[3]
qz = q2[4]
qw = q2[1]
# 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
return [xx yy zz]
end
function setQuaternionFromEuler(euler)
x=euler[1]
y=euler[2]
z=euler[3]
c1 = cos( x / 2 )
c2 = cos( y / 2 )
c3 = cos( z / 2 )
s1 = sin( x / 2 )
s2 = sin( y / 2 )
s3 = sin( z / 2 )
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 [w x y z]
end
function quatToMatrix( quaternion )
te = zeros(16)
x = quaternion[2]
y = quaternion[3]
z = quaternion[4]
w = quaternion[1]
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
sy = 1
sz = 1
te[ 1 ] = ( 1 - ( yy + zz ) ) * sx
te[ 2 ] = ( xy + wz ) * sx
te[ 3 ] = ( xz - wy ) * sx
te[ 4 ] = 0;
te[ 5 ] = ( xy - wz ) * sy
te[ 6 ] = ( 1 - ( xx + zz ) ) * sy
te[ 7 ] = ( yz + wx ) * sy
te[ 8 ] = 0;
te[ 9 ] = ( xz + wy ) * sz
te[ 10 ] = ( yz - wx ) * sz
te[ 11 ] = ( 1 - ( xx + yy ) ) * sz
te[ 12 ] = 0
te[ 13 ] = 0 #position.x;
te[ 14 ] = 0 #position.y;
te[ 15 ] = 0 #position.z;
te[ 16 ] = 1
return te
end
function setFromRotationMatrix(m)
te = m
m11 = (te[ 1 ]== -0.0) ? 0.0 : te[ 1 ]
m12 = (te[ 5 ]== -0.0) ? 0.0 : te[ 5 ]
m13 = (te[ 9 ]== -0.0) ? 0.0 : te[ 9 ]
m21 = (te[ 2 ]== -0.0) ? 0.0 : te[ 2 ]
m22 = (te[ 6 ]== -0.0) ? 0.0 : te[ 6 ]
m23 = (te[ 10]== -0.0) ? 0.0 : te[ 10]
m31 = (te[ 3 ]== -0.0) ? 0.0 : te[ 3 ]
m32 = (te[ 7 ]== -0.0) ? 0.0 : te[ 7 ]
m33 = (te[ 11]== -0.0) ? 0.0 : te[ 11]
m11 = te[ 1 ]
m12 = te[ 5 ]
m13 = te[ 9 ]
m21 = te[ 2 ]
m22 = te[ 6 ]
m23 = te[ 10]
m31 = te[ 3 ]
m32 = te[ 7 ]
m33 = te[ 11]
y = asin( clamp( m13, - 1, 1 ) )
if ( abs( m13 ) < 0.9999999 )
x = atan( - m23, m33 )
z = atan( - m12, m11 )#-m12, m11
# if(m23==0.0)
# x = atan( m23, m33 )
# end
# if(m12==0.0)
# z = atan( m12, m11 )
# end
else
x = atan( m32, m22 )
z = 0;
end
return [x y z]
end
function toAxisOriginalVector3(pV,axis)
# xaxis=[1 0 0]
# vector=copy(axis)
# vector=normalize(collect(Iterators.flatten(vector)))
# p = SVector(pV[1],pV[2], pV[3])
# q=setFromUnitVectors(xaxis, vector)
# v= q * p
# return [v[1] v[2] v[3]]
xaxis=[1 0 0]
vector=copy(axis)
vector=normalize(collect(Iterators.flatten(vector)))
p = SVector(pV[1],pV[2], pV[3])
q=setFromUnitVectors(xaxis, vector)
qq=Quat(q[1],q[2],q[3],q[4])
d=17
qw=round(q[1], digits=d)
qx=round(q[2], digits=d)
qy=round(q[3], digits=d)
qz=round(q[4], digits=d)
rot=setFromRotationMatrix(copy(quatToMatrix( copy([qw qx qy qz]) )))
return applyQuaternion1( copy(pV) ,setQuaternionFromEuler(copy(rot)) )
end
function toAxisXVector3(pV,axis) #TODO CHANGE
# xaxis=[1 0 0]
# vector=copy(axis)
# vector=normalize(collect(Iterators.flatten(vector)))
# p = SVector(pV[1],pV[2], pV[3])
# q=setFromUnitVectors(vector,xaxis)
# v= q * p
# return [v[1] v[2] v[3]]
xaxis=[1 0 0]
vector=copy(axis)
vector=normalize(collect(Iterators.flatten(vector)))
p = SVector(pV[1],pV[2], pV[3])
q=setFromUnitVectors(vector,xaxis)
qq=Quat(q[1],q[2],q[3],q[4])
d=17
qw=round(q[1], digits=d)
qx=round(q[2], digits=d)
qy=round(q[3], digits=d)
qz=round(q[4], digits=d)
rot=setFromRotationMatrix(copy(quatToMatrix( copy([qw qx qy qz]) )))
return applyQuaternion1( copy(pV) ,setQuaternionFromEuler(copy(rot)) )
end
#transforms a vec3D in the original orientation of the bond to that as if the bond was in +X direction
function toAxisOriginalQuat(pQ,axis)
# xaxis=[1 0 0]
# vector=copy(axis)
# vector=normalize(collect(Iterators.flatten(vector)))
# p = SVector(pQ[1],pQ[2], pQ[3])
# q=setFromUnitVectors(xaxis, vector)
# v=q * p
# return Quat(1.0,v[1],v[2],v[3])
xaxis=[1 0 0]
vector=copy(axis)
vector=normalize(collect(Iterators.flatten(vector)))
p = SVector(pQ[1],pQ[2], pQ[3])
q=setFromUnitVectors(xaxis,vector)
qq=Quat(q[1],q[2],q[3],q[4])
d=17
qw=round(q[1], digits=d)
qx=round(q[2], digits=d)
qy=round(q[3], digits=d)
qz=round(q[4], digits=d)
rot=setFromRotationMatrix(copy(quatToMatrix( copy([qw qx qy qz]) )))
v=applyQuaternion1( copy([pQ[1] pQ[2] pQ[3]]) ,setQuaternionFromEuler(copy(rot)) )
return [1.0 v[1] v[2] v[3]]
end
function toAxisXQuat(pQ,axis)
# xaxis=[1 0 0]
# vector=copy(axis)
# vector=normalize(collect(Iterators.flatten(vector)))
# p = SVector(q.x,q.y, q.z)
# q=setFromUnitVectors(vector,xaxis)
# v=q * p
# return Quat(q.w,v[1],v[2],v[3])
xaxis=[1 0 0]
vector=copy(axis)
vector=normalize(collect(Iterators.flatten(vector)))
p = SVector(pQ.x,pQ.y, pQ.z)
q=setFromUnitVectors(vector,xaxis)
qq=Quat(q[1],q[2],q[3],q[4])
d=17
qw=round(q[1], digits=d)
qx=round(q[2], digits=d)
qy=round(q[3], digits=d)
qz=round(q[4], digits=d)
rot=setFromRotationMatrix(copy(quatToMatrix( copy([qw qx qy qz]) )))
v=applyQuaternion1( copy([pQ.x pQ.y pQ.z ]) ,setQuaternionFromEuler(copy(rot)) )
return Quat(1.0,v[1],v[2],v[3])
# return [1.0 v[1] v[2] v[3]]
end
#transforms a vec3D in the original orientation of the bond to that as if the bond was in +X direction
function ToRotationVector(a)
if (a.w >= 1.0 || a.w <= -1.0)
return [0 0 0]
end
squareLength = 1.0-a.w*a.w; # because x*x + y*y + z*z + w*w = 1.0, but more susceptible to w noise (when
SLTHRESH_ACOS2SQRT= 2.4e-3; # SquareLength threshhold for when we can use square root optimization for acos. From SquareLength = 1-w*w. (calculate according to 1.0-W_THRESH_ACOS2SQRT*W_THRESH_ACOS2SQRT
if (squareLength < SLTHRESH_ACOS2SQRT) # ???????
return [a.x a.y a.z] *(2.0*sqrt((2-2*a.w)/squareLength)); # acos(w) = sqrt(2*(1-x)) for w close to 1. for w=0.001, error is 1.317e-6
else
return [a.x a.y a.z] * (2.0*acos(a.w)/sqrt(squareLength));
end
end
# !< Returns a rotation vector representing this quaternion rotation. Adapted from http://www.euclideanspace.com/maths/geometry/rotations/conversions/quaternionToAngle/
function FromRotationVector(VecIn)
theta=VecIn/2.0
thetaMag2=norm(theta)*norm(theta)
DBL_EPSILONx24 =5.328e-15
if thetaMag2*thetaMag2 < DBL_EPSILONx24
qw=1.0 - 0.5*thetaMag2
s=1.0 - thetaMag2 / 6.0
else
thetaMag = sqrt(thetaMag2)
qw=cos(thetaMag)
s=sin(thetaMag) / thetaMag
end
qx=theta[1]*s
qy=theta[2]*s
qz=theta[3]*s;
return Quat(qw,qx,qy,qz)
end
function FromAngleToPosX(a, RotateFrom) #highly optimized at the expense of readability
SMALL_ANGLE_RAD= 1.732e-2 # Angles less than this get small angle approximations. To get: Root solve atan(t)/t-1+MAX_ERROR_PERCENT. From: MAX_ERROR_PERCENT = (t-atan(t))/t
SMALL_ANGLE_W =0.9999625 # quaternion W value corresponding to a SMALL_ANGLE_RAD. To calculate, cos(SMALL_ANGLE_RAD*0.5).
W_THRESH_ACOS2SQRT= 0.9988 # Threshhold of w above which we can approximate acos(w) with sqrt(2-2w). To get: Root solve 1-sqrt(2-2wt)/acos(wt) - MAX_ERROR_PERCENT. From MAX_ERROR_PERCENT = (acos(wt)-sqrt(2-2wt))/acos(wt)
if (RotateFrom[1]==0 && RotateFrom[2]==0 && RotateFrom[3]==0)
return 0 #leave off if it slows down too much!!
end
# Catch and handle small angle:
YoverX = RotateFrom[2]/RotateFrom[1]
ZoverX = RotateFrom[3]/RotateFrom[1]
if (YoverX<SMALL_ANGLE_RAD && YoverX>-SMALL_ANGLE_RAD && ZoverX<SMALL_ANGLE_RAD && ZoverX>-SMALL_ANGLE_RAD) # ??? //Intercept small angle and zero angle
ax=0
ay=0.5*ZoverX
az=-0.5*YoverX
aw = 1+0.5*(-a.y*a.y-a.z*a.z) # w=sqrt(1-x*x-y*y), small angle sqrt(1+x) ~= 1+x/2 at x near zero.
return Quat(aw,ax,ay,az)
end
# more accurate non-small angle:
RotFromNorm = copy(RotateFrom)
RotFromNorm=normalize(collect(Iterators.flatten(RotFromNorm))) # Normalize the input...
theta = acos(RotFromNorm[1]) # because RotFromNorm is normalized, 1,0,0 is normalized, and A.B = |A||B|cos(theta) = cos(theta)
if(theta >(π-DISCARD_ANGLE_RAD)) # ??????
aw=0
ax=0
ay=1
az=0
return Quat(aw,ax,ay,az)
end # 180 degree rotation (about y axis, since the vector must be pointing in -x direction
AxisMagInv = 1.0/sqrt(RotFromNorm[3]*RotFromNorm[3]+RotFromNorm[2]*RotFromNorm[2])
# Here theta is the angle, axis is RotFromNorm.Cross(Vec3D(1,0,0)) = Vec3D(0, RotFromNorm.z/AxisMagInv, -RotFromNorm.y/AxisMagInv), which is still normalized. (super rolled together)
aa = 0.5*theta
s = sin(a)
aw= cos(aa)
ax= 0
ay= RotFromNorm[3]*AxisMagInv*s
az = -RotFromNorm[2]*AxisMagInv*s # angle axis function, reduced
return Quat(aw,ax,ay,az)
end
# !< Overwrites this quaternion with the calculated rotation that would transform the specified RotateFrom vector to point in the positve X direction. Note: function changes this quaternion. @param[in] RotateFrom An arbitrary direction vector. Does not need to be normalized.
function axialStrain(positiveEnd)
# strainRatio = pVPos->material()->E/pVNeg->material()->E;
strainRatio=1.0
return positiveEnd ? (2.0 *strain*strainRatio/(1.0+strainRatio)) : (2.0*strain/(1.0+strainRatio))
end
function updateStrain( axialStrain,E) # ?from where strain
strain = axialStrain # redundant?
currentTransverseStrainSum=0.0 # ??? todo
linear=true
maxStrain=100000000000000000000;# ?? todo later change
if linear
if axialStrain > maxStrain
maxStrain = axialStrain # remember this maximum for easy reference
end
return stress(axialStrain,E)
else
if (axialStrain > maxStrain) # if new territory on the stress/strain curve
maxStrain = axialStrain # remember this maximum for easy reference
returnStress = stress(axialStrain,E) # ??currentTransverseStrainSum
if (nu != 0.0)
strainOffset = maxStrain-stress(axialStrain,E)/(_eHat*(1-nu)) # precalculate strain offset for when we back off
else
strainOffset = maxStrain-returnStress/E # precalculate strain offset for when we back off
end
else # backed off a non-linear material, therefore in linear region.
relativeStrain = axialStrain-strainOffset # treat the material as linear with a strain offset according to the maximum plastic deformation
if (nu != 0.0)
returnStress = stress(relativeStrain,E)
else
returnStress = E*relativeStrain
end
end
return returnStress
end
end
function stress( strain , E ) #end,transverseStrainSum, forceLinear){
# reference: http://www.colorado.edu/engineering/CAS/courses.d/Structures.d/IAST.Lect05.d/IAST.Lect05.pdf page 10
# if (isFailed(strain)) return 0.0f; //if a failure point is set and exceeded, we've broken!
# var E =setup.edges[0].stiffness; //todo change later to material ??
# var E=1000000;//todo change later to material ??
# var scaleFactor=1;
return E*strain;
# # if (strain <= strainData[1] || linear || forceLinear){ //for compression/first segment and linear materials (forced or otherwise), simple calculation
# if (nu==0.0) return E*strain;
# else return _eHat*((1-nu)*strain + nu*transverseStrainSum);
# else return eHat()*((1-nu)*strain + nu*transverseStrainSum);
# # }
# //the non-linear feature with non-zero poissons ratio is currently experimental
# int DataCount = modelDataPoints();
# for (int i=2; i<DataCount; i++){ //go through each segment in the material model (skipping the first segment because it has already been handled.
# if (strain <= strainData[i] || i==DataCount-1){ //if in the segment ending with this point (or if this is the last point extrapolate out)
# float Perc = (strain-strainData[i-1])/(strainData[i]-strainData[i-1]);
# float basicStress = stressData[i-1] + Perc*(stressData[i]-stressData[i-1]);
# if (nu==0.0f) return basicStress;
# else { //accounting for volumetric effects
# float modulus = (stressData[i]-stressData[i-1])/(strainData[i]-strainData[i-1]);
# float modulusHat = modulus/((1-2*nu)*(1+nu));
# float effectiveStrain = basicStress/modulus; //this is the strain at which a simple linear stress strain line would hit this point at the definied modulus
# float effectiveTransverseStrainSum = transverseStrainSum*(effectiveStrain/strain);
# return modulusHat*((1-nu)*effectiveStrain + nu*effectiveTransverseStrainSum);
# }
# }
# }
# assert(false); //should never reach this point
# return 0.0f;
end
function ToEulerAngles(q) # TODO I THINK WRONG
# roll (x-axis rotation)
sinr_cosp = (2 * (q.w * q.x + q.y * q.z) )== -0.0 ? 0.0 : (2 * (q.w * q.x + q.y * q.z) )
cosr_cosp = (1 - 2 * (q.x * q.x + q.y * q.y))== -0.0 ? 0.0 : (1 - 2 * (q.x * q.x + q.y * q.y))
roll = atan(sinr_cosp, cosr_cosp)
# pitch (y-axis rotation)
sinp = 2 * (q.w * q.y - q.z * q.x)
if (abs(sinp) >= 1)
pitch = copysign(π / 2, sinp) # use 90 degrees if out of range
else
pitch = asin(sinp)
end
# yaw (z-axis rotation)
siny_cosp = 2 * (q.w * q.z + q.x * q.y)
cosy_cosp = 1 - 2 * (q.y * q.y + q.z * q.z)
yaw = atan(siny_cosp, cosy_cosp)
return [roll pitch yaw]
end
# ToEulerAngles(Quat(1.0,1.0,1.0,1.0))
# http://klas-physics.googlecode.com/svn/trunk/src/general/Integrator.cpp (reference)
function timeStep(dt,node,static,currentTimeStep)
previousDt = dt
linMom=copy(N_linMom[node])
angMom=copy(N_angMom[node])
orient=copy(N_orient[node])
pos=copy(N_currPosition[node])
if (dt == 0.0)
return 0
end
if(all(N_restrained_degrees_of_freedom[node] .>=1))
# pos = originalPosition() + ext->translation();
# orient = ext->rotationQuat();
# haltMotion();
# # pos=copy(N_position[node])
# # node.currPosition=pos.clone();
# # linMom = new THREE.Vector3(0,0,0);
# # angMom = new THREE.Vector3(0,0,0);
# # node.displacement={x:0,y:0,z:0};
# node.orient=orient.clone();
# node.linMom=linMom.clone();
# node.angMom=angMom.clone();
return 0
end
# Translation
curForce = force(node,static,currentTimeStep)
# var fricForce = curForce.clone();
# if (isFloorEnabled()) floorForce(dt, &curForce); //floor force needs dt to calculate threshold to "stop" a slow voxel into static friction.
# fricForce = curForce - fricForce;
# assert(!(curForce.x != curForce.x) || !(curForce.y != curForce.y) || !(curForce.z != curForce.z)); //assert non QNAN
linMom=linMom+curForce*dt
massInverse=8e-6 # todo ?? later change
translate=linMom*(dt*massInverse) # ??massInverse
# // we need to check for friction conditions here (after calculating the translation) and stop things accordingly
# if (isFloorEnabled() && floorPenetration() >= 0){ //we must catch a slowing voxel here since it all boils down to needing access to the dt of this timestep.
# double work = fricForce.x*translate.x + fricForce.y*translate.y; //F dot disp
# double hKe = 0.5*mat->_massInverse*(linMom.x*linMom.x + linMom.y*linMom.y); //horizontal kinetic energy
# if(hKe + work <= 0) setFloorStaticFriction(true); //this checks for a change of direction according to the work-energy principle
# if (isFloorStaticFriction()){ //if we're in a state of static friction, zero out all horizontal motion
# linMom.x = linMom.y = 0;
# translate.x = translate.y = 0;
# }
# }
# else setFloorStaticFriction(false);
pos=pos+translate
N_currPosition[node]=copy(pos)
N_displacement[node]=N_displacement[node]+translate
# Rotation
curMoment = moment(node)
angMom=angMom+curMoment*dt
momentInertiaInverse=1.0 # todo ?? later change
# orient=orient.*FromRotationVector(copy(angMom)*(dt*momentInertiaInverse))
temp=FromRotationVector(copy(angMom)*(dt*momentInertiaInverse))
orient=Quat(orient.w*temp.w,orient.x*temp.x,orient.y*temp.y,orient.z*temp.z)
# orient.multiply(FromRotationVector(angMom.clone().multiplyScalar((dt*momentInertiaInverse)))) # tupdate the orientation //momentInertiaInverse
N_orient[node]=copy(orient)
eul = ToEulerAngles(orient) # TODO I THINK WRONG
N_angle[node]=copy(eul)
N_linMom[node]=copy(linMom)
N_angMom[node]=copy(angMom)
# if (ext){//?? todo fix
# var size = 1;//change
# if (ext->isFixed(X_TRANSLATE)) {pos.x = ix*size + ext->translation().x; linMom.x=0;}
# if (ext->isFixed(Y_TRANSLATE)) {pos.y = iy*size + ext->translation().y; linMom.y=0;}
# if (ext->isFixed(Z_TRANSLATE)) {pos.z = iz*size + ext->translation().z; linMom.z=0;}
# if (ext->isFixedAnyRotation()){ //if any rotation fixed, all are fixed
# if (ext->isFixedAllRotation()){
# orient = ext->rotationQuat();
# angMom = Vec3D<double>();
# }
# else { //partial fixes: slow!
# Vec3D<double> tmpRotVec = orient.ToRotationVector();
# if (ext->isFixed(X_ROTATE)){ tmpRotVec.x=0; angMom.x=0;}
# if (ext->isFixed(Y_ROTATE)){ tmpRotVec.y=0; angMom.y=0;}
# if (ext->isFixed(Z_ROTATE)){ tmpRotVec.z=0; angMom.z=0;}
# orient.FromRotationVector(tmpRotVec);
# }
# }
# }
# poissonsStrainInvalid = true;
end
function force(node,static,currentTimeStep)
# forces from internal bonds
totalForce=[0 0 0]
# new THREE.Vector3(node.force.x,node.force.y,node.force.z);
# todo
totalForce=totalForce+N_intForce[node]
# for (int i=0; i<6; i++){
# if (links[i]) totalForce += links[i]->force(isNegative((linkDirection)i)); # total force in LCS
# }
totalForce = RotateVec3D(N_orient[node],totalForce); # from local to global coordinates
# assert(!(totalForce.x != totalForce.x) || !(totalForce.y != totalForce.y) || !(totalForce.z != totalForce.z)); //assert non QNAN
# other forces
if(static)
totalForce=totalForce+N_force[node]
# }else if(currentTimeStep<50){
# totalForce.add(new THREE.Vector3(node.force.x,node.force.y,node.force.z));
else
# var ex=0.1;
# if(node.force.y!=0){
# var f=400*Math.sin(currentTimeStep*ex);
# totalForce.add(new THREE.Vector3(0,f,0));
# }
x=N_position[node][3]
t=currentTimeStep
wave=getForce(x,t)
totalForce=totalForce+[0 wave 0]
end
# if (externalExists()) totalForce += external()->force(); //external forces
# totalForce -= velocity()*mat->globalDampingTranslateC(); //global damping f-cv
# totalForce.z += mat->gravityForce(); //gravity, according to f=mg
# if (isCollisionsEnabled()){
# for (std::vector<CVX_Collision*>::iterator it=colWatch->begin(); it!=colWatch->end(); it++){
# totalForce -= (*it)->contactForce(this);
# }
# }
# todo make internal forces 0 again
N_intForce[node]=[0 0 0] # do i really need it?
# node.force.x=0;
# node.force.y=0;
# node.force.z=0;
return totalForce
end
function moment(node)
#moments from internal bonds
totalMoment=[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+N_intMoment[node]
totalMoment = RotateVec3D(N_orient[node],totalMoment);
totalMoment=totalMoment+N_moment[node]
#other moments
# if (externalExists()) totalMoment += external()->moment(); //external moments
# totalMoment -= angularVelocity()*mat->globalDampingRotateC(); //global damping
N_intMoment[node]=[0 0 0] # do i really need it?
return totalMoment
end
function updateDataAndSave(setup,fileName)
nodes = setup["nodes"]
edges = setup["edges"]
voxCount=size(nodes)[1]
linkCount=size(edges)[1]
i=1
for edge in edges
edge["stress"]=E_stress[i]
i=i+1
end
i=1
for node in nodes
node["displacement"]["x"]=N_displacement[i][1]
node["displacement"]["y"]=N_displacement[i][2]
node["displacement"]["z"]=N_displacement[i][3]
node["angle"]["x"]=N_angle[i][1]
node["angle"]["y"]=N_angle[i][2]
node["angle"]["z"]=N_angle[i][3]
i=i+1
end
# pass data as a json string (how it shall be displayed in a file)
stringdata = JSON.json(setup)
# write the file with the stringdata variable information
open(fileName, "w") do f
write(f, stringdata)
end
end
###############################################################################################
latticeSize=9
numTimeSteps=200
save=false
setup = Dict()
open("../json/setupTestUni9.json", "r") do f
global setup
dicttxt = String(read(f)) # file information to string
setup=JSON.parse(dicttxt) # parse and transform data
end
setup=setup["setup"]
############# nodes
N_position=[]
N_degrees_of_freedom=[]
N_restrained_degrees_of_freedom=[]
N_displacement=[]
N_angle=[]
N_currPosition=[]
N_linMom=[]
N_angMom=[]
N_intForce=[]
N_intMoment=[]
N_moment=[]
N_posTimeSteps=[]
N_angTimeSteps=[]
N_force=[]
N_orient=[]
############# edges
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_currentTransverseStrainSum=[]
E_stressTimeSteps=[]
########
voxCount=0
linkCount=0
nodes = setup["nodes"]
edges = setup["edges"]
voxCount=size(nodes)[1]
linkCount=size(edges)[1]
strain =0 #todooo moveeee
######## ######## ######## ########
# println(toAxisXVector3([-5,0,-5 ],[-0.7071067811865475,0,-0.7071067811865475 ]))
# println(toAxisXVector3([5,-5,0 ],[0.7071067811865475,-0.7071067811865475,0 ]))
# println(toAxisXVector3([-5,5,0 ],[-0.7071067811865475,0.7071067811865475,0 ]))
# println(toAxisXVector3([-5,-5,0 ],[-0.7071067811865475,-0.7071067811865475,0 ]))
# println(toAxisXVector3([5,0,5 ],[0.7071067811865475,0,0.7071067811865475 ]))
# println(toAxisXVector3([0,5,5 ],[0,0.7071067811865475,0.7071067811865475 ]))
# println()
# println(toAxisOriginalVector3([1 0 0],[0 0 1]))
# println(toAxisOriginalVector3([0 1 0],[0 0 1]))
# println(toAxisOriginalVector3([0 0 1],[0 0 1]))
# println()
# println(toAxisXVector3([1 0 0],[0 0 1]))
# println(toAxisXVector3([0 1 0],[0 0 1]))
# println(toAxisXVector3([0 0 1],[0 0 1]))
# println()
dt=0.0251646
t=@timed simulateParallel(setup,numTimeSteps,dt,true,10)
time=t[2]
println("ran latticeSize $latticeSize with $voxCount voxels and $linkCount edges for $numTimeSteps time steps took $time seconds")
# setup["animation"]["exaggeration"]=75444
if save
updateDataAndSave(setup,"../json/trialJuliaParallel.json")
end