Spring Physics - Oscillation and Critical Dampening on Quaternions

QuaterionSpring.js

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