parallelFea.js

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

//BASED ON https://github.com/jonhiller/Voxelyze


var DBL_EPSILONx24 =5.328e-15; //DBL_EPSILON*24
var DISCARD_ANGLE_RAD= 1e-7; //Anything less than this angle can be considered 0
var 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 
var SMALL_ANGLE_W =0.9999625; //quaternion W value corresponding to a SMALL_ANGLE_RAD. To calculate, cos(SMALL_ANGLE_RAD*0.5).
var 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)
var X_AXIS = 0; 			//!< X Axis
var Y_AXIS = 1; 			//!< Y Axis
var Z_AXIS = 2; 		    //!< Z Axis
var currentTime=0;
var maxStrain=0;




function simulateParallel(setup,numTimeSteps,dt,static=true,saveInterval=10){
	// var instuctionsDiv=document.getElementById("footer2").innerHTML;
	initialize(setup);
    for(var i=0;i<numTimeSteps;i++){
        var t0 = performance.now();
        doTimeStep(setup,dt,static,i,saveInterval);
        var t1 = performance.now();
		console.log("TimeStep "+i+" took " + (t1 - t0) + " milliseconds.");
		
		// document.getElementById("footer2").innerHTML = "Timestep "+i +" out of "+ numTimeSteps+".";
	}
	// updateColors();
}

function initialize(setup){
	
	// pre-calculate current position
	var voxCount = setup.nodes.length;
	for(var i=0;i<voxCount;i++){
		setup.nodes[i].currPosition=new THREE.Vector3(setup.nodes[i].position.x,setup.nodes[i].position.y,setup.nodes[i].position.z);
		setup.nodes[i].orient= new THREE.Quaternion();
        setup.nodes[i].linMom=new THREE.Vector3(0,0,0);
        setup.nodes[i].angMom=new THREE.Vector3(0,0,0);
        setup.nodes[i].intForce=new THREE.Vector3(0,0,0);
        setup.nodes[i].intMoment=new THREE.Vector3(0,0,0);
		setup.nodes[i].moment={ x: 0, y: 0,z:0 };
		setup.nodes[i].displacement={ x: 0, y: 0,z:0 };
		//for dynamic simulations
		setup.nodes[i].posTimeSteps=[];
		setup.nodes[i].angTimeSteps=[];
		setup.nodes[i].nomSize=1.0;
		setup.nodes[i].massInverse=8e-6;
		setup.nodes[i].mass=1/8e-6;
		setup.nodes[i].FloorStaticFriction=false;
	} 
	// pre-calculate the axis
	var linkCount = setup.edges.length;
	for(var i=0;i<linkCount;i++){
		var node1=setup.nodes[setup.edges[i].source];
		var node2=setup.nodes[setup.edges[i].target];
		var pVNeg=new THREE.Vector3(node1.position.x,node1.position.y,node1.position.z);
		var pVPos=new THREE.Vector3(node2.position.x,node2.position.y,node2.position.z);
		var axis=pVPos.clone().sub(pVNeg).normalize();
		setup.edges[i].axis=axis.clone();


		setup.edges[i].currentRestLength=0;
        setup.edges[i].pos2= new THREE.Vector3(0,0,0);
        setup.edges[i].angle1v= new THREE.Vector3(0,0,0);
        setup.edges[i].angle2v= new THREE.Vector3(0,0,0);
        setup.edges[i].angle1=new THREE.Quaternion();
        setup.edges[i].angle2=new THREE.Quaternion();
        setup.edges[i].currentTransverseArea=0;
		setup.edges[i].currentTransverseStrainSum=0;
		//todo update stresses
		setup.edges[i].stressTimeSteps=[];
		
	} 
	
}

function doTimeStep(setup,dt,static=true,currentTimeStep,saveInterval){
	if (dt==0) 
		return true;
	else if (dt<0) 
		dt = recommendedTimeStep();

	// if (collisions) updateCollisions();
	

	var collisions=false;

	//Euler integration:
	var Diverged = false;
	var linkCount = setup.edges.length;
	for(var i=0;i<linkCount;i++){
        updateForces(setup,setup.edges[i],setup.nodes[setup.edges[i].source],setup.nodes[setup.edges[i].target],static);
        // todo: update forces and whatever
		if (axialStrain(setup.edges[i]) > 100) {
			Diverged = true; //catch divergent condition! (if any thread sets true we will fail, so don't need mutex...
		}

	}
    if (Diverged){
		console.log("Divergedd!!!!!")
		return false;

	}
	var voxCount = setup.nodes.length;

	for(var i=0;i<voxCount;i++){
		timeStep(dt,setup.nodes[i],static,currentTimeStep);
		if(!static&& currentTimeStep%saveInterval==0){
			setup.nodes[i].posTimeSteps.push(setup.nodes[i].displacement);
			setup.nodes[i].angTimeSteps.push(setup.nodes[i].angle);

		}
		// todo: update linMom,angMom, orient and whatever
	} 

	
	currentTime += dt;
	return true;

}

function updateForces(setup,edge,node1,node2,static=true){
	
	var pVNeg=new THREE.Vector3(node1.position.x,node1.position.y,node1.position.z);
	var pVPos=new THREE.Vector3(node2.position.x,node2.position.y,node2.position.z);
    var currentRestLength=pVPos.clone().sub(pVNeg).length();
	edge.currentRestLength=currentRestLength; //todo make sure updated
	

	pVNeg=node1.currPosition.clone();
	pVPos=node2.currPosition.clone();

	// Vec3D<double> three
	var oldPos2 = edge.pos2.clone();//??
	var oldAngle1v = edge.angle1v.clone();
	var oldAngle2v = edge.angle2v.clone(); //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,node1,node2); //sets pos2, angle1, angle2 /*restLength*/

	var dPos2=edge.pos2.clone().sub(oldPos2).multiplyScalar(0.5);
	var dAngle1=edge.angle1v.clone().sub(oldAngle1v).multiplyScalar(0.5);
	var dAngle2=edge.angle2v.clone().sub(oldAngle2v).multiplyScalar(0.5);

	
	
	
	//if volume effects...
    //if (!mat->isXyzIndependent() || currentTransverseStrainSum != 0) 
    //updateTransverseInfo(); //currentTransverseStrainSum != 0 catches when we disable poissons mid-simulation
	
	
	var _stress=updateStrain((edge.pos2.x/edge.currentRestLength),edge.stiffness);
	// var _stress=updateStrain(1.0);
	edge.stress = _stress;
	if(!static){
		edge.stressTimeSteps.push(_stress);
	}

	// console.log("Stress:"+edge.stress)
	if(setup.viz.minStress>edge.stress){
		setup.viz.minStress=edge.stress;
	}else if (setup.viz.maxStress<edge.stress){
		setup.viz.maxStress=edge.stress;
	}

	// 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
	
	var l   = currentRestLength;//??
	var rho = edge.density;
	var A = edge.area;
	var E = edge.stiffness;// youngs modulus
	var G=1.0;//todo shear_modulus
	var ixx = 1.0;//todo section ixx
	var I=1.0;
	var iyy = 1.0;//todo section.iyy//
	// var l0=length.dataSync();
	var J=1.0;//todo check
	// var l02 = l0 * l0;
	// var l03 = l0 * l0 * l0;
	var b1= 12*E*I/(l*l*l);
	var b2= 6*E*I/(l*l);
	var b3= 2*E*I/(l);
	var a1= E*A/l;
	var a2= G*J/l;
	var nu=0;

	// var b1= 5e6;
	// var b2= 1.25e7;
	// var b3= 2.08333e+07;
	// var a1= E*A/l;
	// var a2= 1.04167e+07;

	var E=1000000;
	var E=edge.stiffness;
	var L = 5;
	var a1 = E*L; //EA/L : Units of N/m
	var a2 = E * L*L*L / (12.0*(1+nu)); //GJ/L : Units of N-m
	var b1 = E*L; //12EI/L^3 : Units of N/m
	var b2 = E*L*L/2.0; //6EI/L^2 : Units of N (or N-m/m: torque related to linear distance)
	var 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;

	var currentTransverseArea=25.0;// todo ?? later change
	var currentTransverseArea=edge.area;


	//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)
	var forceNeg = new THREE.Vector3 (	_stress*currentTransverseArea, //currentA1*pos2.x,
								b1*edge.pos2.y - b2*(edge.angle1v.z + edge.angle2v.z),
								b1*edge.pos2.z + b2*(edge.angle1v.y + edge.angle2v.y)); //Use Curstress instead of -a1*Pos2.x to account for non-linear deformation 
	var forcePos = forceNeg.clone().negate();

	var momentNeg = new THREE.Vector3 (	a2*(edge.angle2v.x - edge.angle1v.x),
								-b2*edge.pos2.z - b3*(2*edge.angle1v.y + edge.angle2v.y),
								b2*edge.pos2.y - b3*(2*edge.angle1v.z + edge.angle2v.z));
	var momentPos = new THREE.Vector3 (	a2*(edge.angle1v.x - edge.angle2v.x),
								-b2*edge.pos2.z - b3*(edge.angle1v.y + 2*edge.angle2v.y),
								b2*edge.pos2.y - b3*(edge.angle1v.z + 2*edge.angle2v.z));
	
								
	// //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
	var smallAngle=false;//?? todo check
	var forceNeg,momentNeg,forcePos,momentPos;

	
	
	if (!smallAngle){//?? chech
		forceNeg = RotateVec3DInv(edge.angle1,forceNeg);
		momentNeg = RotateVec3DInv(edge.angle1,momentNeg);
	}
	forcePos = RotateVec3DInv(edge.angle2,forcePos);
	momentPos = RotateVec3DInv(edge.angle2,momentPos);
	

	forceNeg =toAxisOriginalVector3(forceNeg,edge.axis);
	forcePos =toAxisOriginalVector3(forcePos,edge.axis);
	momentNeg=toAxisOriginalQuat(momentNeg,edge.axis);
	momentPos=toAxisOriginalQuat(momentPos,edge.axis);
	
	

	node1.intForce.add(forceNeg.clone());
	node2.intForce.add(forcePos.clone());
	node1.intMoment.add(momentNeg.clone());
	node2.intMoment.add(momentPos.clone());

	// 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


}

function orientLink( edge,node1,node2){ //updates pos2, angle1, angle2, and smallAngle //Quat3D<double> /*double restLength*/

    var pVPos=node2.currPosition.clone();
	var pVNeg=node1.currPosition.clone();
	

    
	var currentRestLength=edge.currentRestLength;
	// var currentRestLength=0;
	
	
	var pos2 = toAxisXVector3(pVPos.clone().sub(pVNeg),edge.axis); //digit truncation happens here...

	

	// pos2.x = Math.round(pos2.x * 1e4) / 1e4; 
	
	var angle1 = toAxisXQuat(node1.orient,edge.axis);
	var angle2 = toAxisXQuat(node2.orient,edge.axis);


	var totalRot = angle1.conjugate(); //keep track of the total rotation of this bond (after toAxisX()) //Quat3D<double>
	pos2 = RotateVec3D(totalRot,pos2);
	angle2 = totalRot.clone().multiply(angle2);
	angle1 = 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);
    // }
    
    var smallAngle=true; //todo later remove

	if (smallAngle)	{ //Align so Angle1 is all zeros
		pos2.x -= 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); 
	}

	
	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

    edge.pos2=pos2.clone();
    edge.angle1v=angle1v.clone();
    edge.angle2v=angle2v.clone();
    edge.angle1=angle1.clone();
	edge.angle2=angle2.clone();
	
	

	return totalRot;
}

function RotateVec3D(a, f)  { 
	var fx=f.x, fy=f.y, fz=f.z;
	var tw = fx*a.x + fy*a.y + fz*a.z;
	var tx = fx*a.w - fy*a.z + fz*a.y;
	var ty = fx*a.z + fy*a.w - fz*a.x;
	var tz = -fx*a.y + fy*a.x + fz*a.w;

	return new THREE.Vector3(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);
} //!< Returns a vector representing the specified vector "f" rotated by this quaternion. @param[in] f The vector to transform.


function RotateVec3DInv(a, f) { 
    var fx=f.x, fy=f.y, fz=f.z;
    var tw = a.x*fx + a.y*fy + a.z*fz;
    var tx = a.w*fx - a.y*fz + a.z*fy;
    var ty = a.w*fy + a.x*fz - a.z*fx;
    var tz = a.w*fz - a.x*fy + a.y*fx;
    return new THREE.Vector3(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);	
} //!< 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 toAxisOriginalVector3(pV,axis){
	// switch (axis){
	// 	case Y_AXIS:{
	// 		var tmp = pV.y; 
	// 		pV.y=pV.x; 
	// 		pV.x = -tmp; 
	// 		break;
	// 	} 
	// 	case Z_AXIS: {
	// 		var tmp = pV.z; 
	// 		pV.z=pV.x; 
	// 		pV.x = -tmp;
	// 		 break;
	// 	} 
	// 	default: 
	// 		break;
	// }
	// if(axis.equals(new THREE.Vector3(0,1,0))){
	// 	var tmp = pV.y; 
	// 	pV.y=pV.x; 
	// 	pV.x = -tmp;  

	// }else if(axis.equals(new THREE.Vector3(0,0,1))){
	// 	var tmp = pV.z; 
	// 	pV.z=pV.x; 
	// 	pV.x = -tmp; 

	// }

	var vector=axis.clone();
	vector.normalize();
	var xaxis=new THREE.Vector3(1,0,0);
	var geometry = new THREE.BoxGeometry( 1, 1, 1 );
	var material = new THREE.MeshBasicMaterial( {color: 0x00ff00} );
	var cube = new THREE.Mesh( geometry, material );
	var quaternion = new THREE.Quaternion(); // create one and reuse it
	quaternion.setFromUnitVectors(  xaxis,vector );
	cube.applyQuaternion( quaternion );
	pV.applyEuler(cube.rotation);


	return pV;
}

function toAxisOriginalQuat(pQ,axis){
	// switch (axis){
	// 	case Y_AXIS: {
	// 		var tmp = pQ.y; 
	// 		pQ.y=pQ.x; 
	// 		pQ.x = -tmp; 
	// 		break;
	// 	} 
	// 	case Z_AXIS: {
	// 		var tmp = pQ.z; 
	// 		pQ.z=pQ.x; 
    //         pQ.x = -tmp; 
    //         break;
	// 	} 
	// 	default: 
	// 		break;
	// }
	// if(axis.equals(new THREE.Vector3(0,1,0))){
	// 	var tmp = pQ.y; 
	// 	pQ.y=pQ.x; 
	// 	pQ.x = -tmp; 

	// }else if(axis.equals(new THREE.Vector3(0,0,1))){
	// 	var tmp = pQ.z; 
	// 	pQ.z=pQ.x; 
	// 	pQ.x = -tmp; 

	// }

	var v=new THREE.Vector3(pQ.x,pQ.y,pQ.z);
	var vector=axis.clone();
	vector.normalize();
	var xaxis=new THREE.Vector3(1,0,0);
	var geometry = new THREE.BoxGeometry( 1, 1, 1 );
	var material = new THREE.MeshBasicMaterial( {color: 0x00ff00} );
	var cube = new THREE.Mesh( geometry, material );
	var quaternion = new THREE.Quaternion(); // create one and reuse it
	// quaternion.setFromUnitVectors( vector, xaxis );
	quaternion.setFromUnitVectors( xaxis, vector ); //amira changed to see 3 march 2020
	cube.applyQuaternion( quaternion );
	v.applyEuler(cube.rotation);

	return new THREE.Quaternion(v.x,v.y,v.z,pQ.w);

}

function toAxisXVector3(v,axis){ //TODO CHANGE

	// var vector=new THREE.Vector3(1,0,0);

	// switch (axis){
	// 	case Y_AXIS: 
	// 		return new THREE.Vector3(v.y, -v.x, v.z); 
	// 		break;
	// 	case Z_AXIS: 
	// 		return new THREE.Vector3(v.z, v.y, -v.x); 
	// 		break;
	// 	default: 
	// 		
	// 		return v;
	//		break;
	// }

	// switch (axis){
	// 	case Y_AXIS: 
	// 		vector=new THREE.Vector3(0,1,0);
	// 		break;
	// 	case Z_AXIS: 
	// 		vector=new THREE.Vector3(0,0,1);
	// 		break;
	// 	default: 
	// 		vector=new THREE.Vector3(1,0,0);
	// 		break;
	// }

	var vector=axis.clone();
	vector.normalize();
	var xaxis=new THREE.Vector3(1,0,0);
	var geometry = new THREE.BoxGeometry( 1, 1, 1 );
	var material = new THREE.MeshBasicMaterial( {color: 0x00ff00} );
	var cube = new THREE.Mesh( geometry, material );
	var quaternion = new THREE.Quaternion(); // create one and reuse it
	quaternion.setFromUnitVectors( vector, xaxis );
	cube.applyQuaternion( quaternion );
	v.applyEuler(cube.rotation);
	// var res=6;
	// v=new THREE.Vector3( parseFloat(v.x.toFixed(res)),parseFloat(v.y.toFixed(res)),parseFloat(v.z.toFixed(res)))
	
	
	
	return v.clone();

} //transforms a vec3D in the original orientation of the bond to that as if the bond was in +X direction

function  toAxisXQuat(q,axis){
	// var vector=new THREE.Vector3(1,0,0);
	// switch (axis){
	// 	case Y_AXIS: 
	// 		return new THREE.Quaternion( q.y, -q.x, q.z,q.w); 
	// 	case Z_AXIS: 
	// 		return new THREE.Quaternion( q.z, q.y, -q.x,q.w); 
	// 	default: 
	// 		return q;
	// }
	
	// switch (axis){
	// 	case Y_AXIS: 
	// 		vector=new THREE.Vector3(0,1,0);
	// 		break;
	// 	case Z_AXIS: 
	// 		vector=new THREE.Vector3(0,0,1);
	// 		break;
	// 	default: 
	// 		vector=new THREE.Vector3(1,0,0);
	// 		break;
	// }
	var v=new THREE.Vector3(q.x,q.y,q.z);
	var vector=axis.clone();
	vector.normalize();
	var xaxis=new THREE.Vector3(1,0,0);
	var geometry = new THREE.BoxGeometry( 1, 1, 1 );
	var material = new THREE.MeshBasicMaterial( {color: 0x00ff00} );
	var cube = new THREE.Mesh( geometry, material );
	var quaternion = new THREE.Quaternion(); // create one and reuse it
	quaternion.setFromUnitVectors( vector, xaxis );
	cube.applyQuaternion( quaternion );
	v.applyEuler(cube.rotation);

	return new THREE.Quaternion(v.x,v.y,v.z,q.w);

} //transforms a vec3D in the original orientation of the bond to that as if the bond was in +X direction


//const Quat3D Conjugate() const {return Quat3D(w, -x, -y, -z);} //!< Returns a quaternion that is the conjugate of this quaternion. This quaternion is not modified.
function ToRotationVector(a)  {
	if (a.w >= 1.0 || a.w <= -1.0) {
		return new THREE.Vector3(0,0,0);
	}
	var 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 
	var 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 new THREE.Vector3(a.x, a.y, a.z).multiplyScalar(2.0*Math.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 new THREE.Vector3(a.x, a.y, a.z).multiplyScalar(2.0*Math.acos(a.w)/Math.sqrt(squareLength));
} //!< Returns a rotation vector representing this quaternion rotation. Adapted from http://www.euclideanspace.com/maths/geometry/rotations/conversions/quaternionToAngle/

function FromRotationVector( VecIn) { 
	var q=new THREE.Quaternion();
	var theta = VecIn.clone().divideScalar(2.0);
	var s, thetaMag2 = theta.length()*theta.length();
	if (thetaMag2*thetaMag2 < DBL_EPSILONx24 ){ //if the 4th taylor expansion term is negligible
		q.w=1.0 - 0.5*thetaMag2;
		s=1.0 - thetaMag2 / 6.0;
	}
	else {
		var thetaMag = Math.sqrt(thetaMag2);
		q.w=Math.cos(thetaMag);
		s=Math.sin(thetaMag) / thetaMag;
	}
	q.x=theta.x*s;
	q.y=theta.y*s;
	q.z=theta.z*s;
	return q;
} //!< Overwrites this quaternion with values from the specified rotation vector. Adapted from http://physicsforgames.blogspot.com/2010/02/quaternions.html.  Note: function changes this quaternion. @param[in] VecIn A rotation vector to calculate this quaternion from.


function FromAngleToPosX(a, RotateFrom){ //highly optimized at the expense of readability
	if (new THREE.Vector3(0,0,0).equals(RotateFrom)) 
		return; //leave off if it slows down too much!!

    //Catch and handle small angle:
    var YoverX = RotateFrom.y/RotateFrom.x;
    var ZoverX = RotateFrom.z/RotateFrom.x;
    if (YoverX<SMALL_ANGLE_RAD && YoverX>-SMALL_ANGLE_RAD && ZoverX<SMALL_ANGLE_RAD && ZoverX>-SMALL_ANGLE_RAD){ //??? //Intercept small angle and zero angle
		a.x=0; 
		a.y=0.5*ZoverX; 
		a.z=-0.5*YoverX;
        a.w = 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 a;
    }

    //more accurate non-small angle:
    var RotFromNorm = RotateFrom.clone();
    RotFromNorm.normalize(); //Normalize the input...

    var theta = Math.acos(RotFromNorm.x); //because RotFromNorm is normalized, 1,0,0 is normalized, and A.B = |A||B|cos(theta) = cos(theta)
    if (theta > Math.PI-DISCARD_ANGLE_RAD) {//??????
		a.w=0; 
		a.x=0; 
		a.y=1; 
		a.z=0; 
		return a;
	} //180 degree rotation (about y axis, since the vector must be pointing in -x direction

    var AxisMagInv = 1.0/Math.sqrt(RotFromNorm.z*RotFromNorm.z+RotFromNorm.y*RotFromNorm.y);
    //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)
    var aa = 0.5*theta;
    var s = Math.sin(a);
	a.w=Math.cos(aa); 
	a.x=0; 
	a.y=RotFromNorm.z*AxisMagInv*s; 
	a.z = -RotFromNorm.y*AxisMagInv*s; //angle axis function, reduced
	return a;

} //!< 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()  {
	return strain;
} //!< returns the current overall axial strain (unitless) between the two voxels.

function axialStrain( positiveEnd) {
	//strainRatio = pVPos->material()->E/pVNeg->material()->E;
	var strainRatio=1.0;
	return positiveEnd ? 2.0 *strain*strainRatio/(1.0+strainRatio) : 2.0*strain/(1.0+strainRatio);
}

function updateStrain( axialStrain,E){ //?from where strain
	strain = axialStrain; //redundant?
	var currentTransverseStrainSum=0.0; //??? todo
    var linear=true;
    // var maxStrain=100000000000000000000;//?? todo later change
	if (linear){
		if (axialStrain > maxStrain) 
			maxStrain = axialStrain; //remember this maximum for easy reference

		return stress(axialStrain,E);
	}
	else {
		var returnStress;

		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

		}
		else { //backed off a non-linear material, therefore in linear region.
			var 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;
		}
		return returnStress;

	}

}

function stress( strain , E ){//,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;
}

function updateTransverseInfo(edge){
	// currentTransverseArea = 0.5*(pVNeg->transverseArea(edge.axis)+pVPos->transverseArea(edge.axis));
    // currentTransverseStrainSum = 0.5*(pVNeg->transverseStrainSum(edge.axis)+pVPos->transverseStrainSum(edge.axis));
    edge.currentTransverseArea = 1; //or 0
	edge.currentTransverseStrainSum = 1;//or 0

}

function transverseArea( axis){
	var size = 1.0;//??(float)mat->nominalSize();
    //if (mat->poissonsRatio() == 0) return size*size;
    if (true) return size*size;

	// var psVec = poissonsStrain();

	// switch (axis){
	// case X_AXIS: return (float)(size*size*(1+psVec.y)*(1+psVec.z));
	// case Y_AXIS: return (float)(size*size*(1+psVec.x)*(1+psVec.z));
	// case Z_AXIS: return (float)(size*size*(1+psVec.x)*(1+psVec.y));
	// default: return size*size;
	// }
}

//http://klas-physics.googlecode.com/svn/trunk/src/general/Integrator.cpp (reference)
function timeStep( dt,node,static=true,currentTimeStep){
	var previousDt = dt;
	var linMom=node.linMom.clone();
    var angMom=node.angMom.clone();
    var orient=node.orient.clone();
	var pos=new THREE.Vector3(node.currPosition.x,node.currPosition.y,node.currPosition.z);
	if (dt == 0.0) 
		return;

	var isTrue = (currentValue) => currentValue ==true;


	if (node.restrained_degrees_of_freedom.every(isTrue)){
		// pos = originalPosition() + ext->translation();
		// orient = ext->rotationQuat();
		// haltMotion();
		pos=new THREE.Vector3(node.position.x,node.position.y,node.position.z);
		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;
	}

	/////////////////////////
	var gravity=true;
	var isFloorEnabled=true;
	// node.FloorStaticFriction=false;

	//Translation
	var curForce = force(node,static,currentTimeStep);
	

	//add gravity
	var grav=-node.mass*9.80665*10.0;
	if(gravity&&!static){
		curForce.y+=grav;

	}

	var fricForce = curForce.clone();
	if (isFloorEnabled) {
		curForce=floorForce(node,dt, curForce).multiplyScalar(1.0);
	}
	fricForce = curForce.clone().sub(fricForce);
	// console.log(fricForce);



	linMom.add(curForce).multiplyScalar(dt);
	var translate=linMom.clone().multiplyScalar(dt*node.massInverse);//??massInverse

	

	//	we need to check for friction conditions here (after calculating the translation) and stop things accordingly
	if (isFloorEnabled && floorPenetration(node) >= 0 &&!static){ //we must catch a slowing voxel here since it all boils down to needing access to the dt of this timestep.
		var work = fricForce.x*translate.x + fricForce.z*translate.z; //F dot disp
		var hKe = 0.5*node.massInverse*(linMom.x*linMom.x + linMom.z*linMom.z); //horizontal kinetic energy
		
		if((hKe + work) <= 0) {
			node.FloorStaticFriction=true; //this checks for a change of direction according to the work-energy principle
			// console.log("dvdfvfdbvd");
		}
		
		if(node.FloorStaticFriction){ 
			//if we're in a state of static friction, zero out all horizontal motion
			
			console.log("hereeee");
			linMom.x = 0;
			linMom.z = 0;
			translate.x = 0
			translate.z = 0;
		}
	}
	else {
		
		node.FloorStaticFriction=false;
	}

	///////////////////////////////////////////////



	pos.add(translate);
	node.currPosition=pos.clone();
	node.displacement={
		x:translate.x+node.displacement.x,
		y:translate.y+node.displacement.y,
		z:translate.z+node.displacement.z};
	
	// pos += translate;

	//Rotation
	var curMoment = moment(node); 
	angMom.add(curMoment*dt);

	var momentInertiaInverse=1.0;//todo ?? later change
	orient.multiply(FromRotationVector(angMom.clone().multiplyScalar((dt*momentInertiaInverse)))); //update the orientation //momentInertiaInverse

	node.orient=orient.clone();
	
	var eulerOrder = "ZYX"; //TODO SEE IF CORRECT
	var eul = new THREE.Euler().setFromQuaternion( orient, eulerOrder ); 

	node.angle={
		x:eul.x,
		y:eul.y,
		z:eul.z};


	node.linMom=linMom.clone();
	node.angMom=angMom.clone();
	
	// 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;
}



function force(node,static=true,currentTimeStep) {
	//forces from internal bonds
	var totalForce=new THREE.Vector3(0,0,0);
	//new THREE.Vector3(node.force.x,node.force.y,node.force.z);
	// todo 
	
	totalForce.add(node.intForce);

	// for (int i=0; i<6; i++){ 
	// 	if (links[i]) totalForce += links[i]->force(isNegative((linkDirection)i)); //total force in LCS
	// }
	totalForce = RotateVec3D(node.orient,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.add(new THREE.Vector3(node.force.x,node.force.y,node.force.z));
	// }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));
			
		// }
		var ff=new THREE.Vector3(node.force.x,node.force.y,node.force.z);
		if(ff.length()>0){
			// var x=node.position.z;
			// var t=currentTimeStep;
			// var wave=getForce(x,t);
			// totalForce.add(new THREE.Vector3(0,wave,0));
			var t=currentTimeStep;
			totalForce.add(getForce(node.currPosition,currentTimeStep));

		}
		
	}
	

	// 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
	node.intForce=new THREE.Vector3(0,0,0);
	// node.force.x=0;
	// node.force.y=0;
	// node.force.z=0;
	return totalForce;
}

function floorForce(node, dt,  pTotalForce){
    var CurPenetration = floorPenetration(node); //for now use the average.
    

    var muStatic=0.2;
    var muKinetic=0.01;//normal force = 1e3*0.001
    var zetaGlobal=1.0;
    // pMat1->setInternalDamping(1.0);
	// pMat1->setGlobalDamping(0.2f);
    // pMat1->setStaticFriction(1.0f);
	// pMat1->setKineticFriction(0.1f); //normal force = 1e3*0.001
	// pMat1->setGlobalDamping(1.0f);

	if (CurPenetration>=0){ 
		
		var vel = velocity(node);
		var horizontalVel= new THREE.Vector3(vel.x, 0, vel.z);
		// console.log(CurPenetration);
		var normalForce = penetrationStiffness(node)*CurPenetration;
        
		pTotalForce.y += normalForce - collisionDampingTranslateC(node)*vel.y; //in the z direction: k*x-C*v - spring and damping

		if (node.FloorStaticFriction){ //If this voxel is currently in static friction mode (no lateral motion) 
			// assert(horizontalVel.Length2() == 0);
			var surfaceForceSq = (pTotalForce.x*pTotalForce.x + pTotalForce.z*pTotalForce.z); //use squares to avoid a square root
			var frictionForceSq = (muStatic*normalForce)*(muStatic*normalForce);
		
			if (surfaceForceSq > frictionForceSq) {
                node.FloorStaticFriction=false; //if we're breaking static friction, leave the forces as they currently have been calculated to initiate motion this time step
            }
		}
        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
            pTotalForce.sub(horizontalVel.normalize().multiplyScalar(muKinetic*normalForce*1.0))//add a friction force opposing velocity according to the normal force and the kinetic coefficient of friction
            //pTotalForce -=  muKinetic*normalForce*horizontalVel.Normalized(); 
        }
	}
	else {
        node.FloorStaticFriction=false;
    }
    return pTotalForce;

}

function floorPenetration(node) {
	var floor=-3.5;
	if(node.currPosition.y-floor<0){
		
		return -(node.currPosition.y-floor);
	}else{
		return 0;
	}
} //!< 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 velocity(node) {
    return node.linMom.clone().multiplyScalar(node.massInverse);
} //!< Returns the 3D velocity of this voxel in m/s (GCS)
function penetrationStiffness(node)  {
	var E=10000000;
    return (2*E*node.nomSize);
} //!< returns the stiffness with which this voxel will resist penetration. This is calculated according to E*A/L with L = voxelSize/2.
function collisionDampingTranslateC(node)  {
	var E=10000000;