jake · December 8, 2019 21:49 · Nov 17, 2018
diff --git a/QuaterionSpring.js b/QuaterionSpring.js
@@ -0,0 +1,106 @@
+// Resources
+// https://burakkanber.com/blog/physics-in-javascript-car-suspension-part-1-spring-mass-damper/
+// https://gafferongames.com/post/spring_physics/
+// https://gafferongames.com/post/physics_in_3d/
+// http://digitalopus.ca/site/pd-controllers/
+//		.. Has things about Torque
+
+class QuaterionSpring{
+  constructor( damping=5, stiffness=30 ){
+    this.velocity	= new Float32Array(4);
+    this.stiffness	= stiffness;
+    this.damping	= damping;;
+  }
+
+  _velLenSqr(){
+    return 	this.velocity[0] ** 2 + this.velocity[1] ** 2 + 
+        this.velocity[2] ** 2 + this.velocity[3] ** 2;
+  }
+
+  // Harmonic oscillation 
+  // https://stackoverflow.com/questions/44688112/spring-physics-applied-to-quaternions-using-python
+  oscillationStep(cq, target, dt){
+    // Check when the spring is done.
+    let dot = Quat.dot(cq, target);
+    if( dot >= 0.9999  && this._velLenSqr() < 0.000001 ){
+      cq.copy( target );
+      return;
+    }
+
+    //.........................................
+    let tq = new Quat();
+    if( dot < 0 ){ // Use the closest rotation
+      tq[0] = -target[0];
+      tq[1] = -target[1];
+      tq[2] = -target[2];
+      tq[3] = -target[3];
+    }else tq.copy( target );
+
+    //.........................................
+    // displacement = current - target;
+    // spring_force = (stiffness * displacement - damper * velocity ) / mass
+    // velocity += spring_force * deltaTime;	// Acceleration
+    // current += velocity * deltaTime 			
+
+    //tried with MASS, took it out cause no need for it at the moment.
+    //this.velocity[0] += ((k * ( cq[0] - tq[0] ) - d * this.velocity[0]) / m) * dt;
+    //this.velocity[1] += ((k * ( cq[1] - tq[1] ) - d * this.velocity[1]) / m) * dt;
+    //this.velocity[2] += ((k * ( cq[2] - tq[2] ) - d * this.velocity[2]) / m) * dt;
+    //this.velocity[3] += ((k * ( cq[3] - tq[3] ) - d * this.velocity[3]) / m) * dt;
+
+    this.velocity[0] += (-this.stiffness * ( cq[0] - tq[0] ) - this.damping * this.velocity[0])  * dt;
+    this.velocity[1] += (-this.stiffness * ( cq[1] - tq[1] ) - this.damping * this.velocity[1])  * dt;
+    this.velocity[2] += (-this.stiffness * ( cq[2] - tq[2] ) - this.damping * this.velocity[2])  * dt;
+    this.velocity[3] += (-this.stiffness * ( cq[3] - tq[3] ) - this.damping * this.velocity[3])  * dt;
+
+    //.........................................
+    cq[0] += this.velocity[0] * dt;
+    cq[1] += this.velocity[1] * dt;
+    cq[2] += this.velocity[2] * dt;
+    cq[3] += this.velocity[3] * dt;
+    //console.log(cq);
+    cq.normalize();
+  }
+
+  // Critically Damped Spring
+  criticallyStep(cq, target, dt){
+    // Check when the spring is done.
+    let dot = Quat.dot(cq, target);
+    if( dot >= 0.9999  && this._velLenSqr() < 0.000001 ){
+      cq.copy( target );
+      return;
+    }
+
+    //.........................................
+    let tq = new Quat();
+    if( dot < 0 ){ // Use the closest rotation
+      tq[0] = -target[0];
+      tq[1] = -target[1];
+      tq[2] = -target[2];
+      tq[3] = -target[3];
+    }else tq.copy( target );
+
+    //.........................................
+    // n1 = velocity - ( currentRot - targerRot  ) * ( omega * omega * dt );
+    // n2 = 1 + omega * dt;
+    // newVelocity = n1 / ( n2 * n2 );
+    // currentRot += newVelocity * dt;
+    let dSqrDt	= this.damping * this.damping * dt,
+      n2		= 1 + this.damping * dt,
+      n2Sqr	= n2 * n2;
+
+    this.velocity[0] = ( this.velocity[0] - ( cq[0] - tq[0] ) * dSqrDt ) / n2Sqr;
+    this.velocity[1] = ( this.velocity[1] - ( cq[1] - tq[1] ) * dSqrDt ) / n2Sqr;
+    this.velocity[2] = ( this.velocity[2] - ( cq[2] - tq[2] ) * dSqrDt ) / n2Sqr;
+    this.velocity[3] = ( this.velocity[3] - ( cq[3] - tq[3] ) * dSqrDt ) / n2Sqr;
+
+    //.........................................
+    cq[0] += this.velocity[0] * dt;
+    cq[1] += this.velocity[1] * dt;
+    cq[2] += this.velocity[2] * dt;
+    cq[3] += this.velocity[3] * dt;
+    cq.normalize();
+
+    return cq;
+  }
+}
No results found