parallelFEA.jl

# 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;