Damped Springs on Array of Bones / Joints

IKChainSpring.js

// Return the a rotation needed to add to FROM to get to TO.
function quatDelta(qFrom, qTo, out=null){
  return qFrom.invert( out || new Quat() ).mul( qTo );
}

const QUAT_FWD2UP = new Quat().setAxisAngle(Vec3.LEFT, Maths.toRad(90));
class ChainSpring{
  constructor( chain ){
    this.links = new Array( chain.count );
    this.stiffness = 60;
    this.damping = 6.5;

    //Create Offset End Points to Keep Track off.
    let b;
    for(let i=0; i < chain.count; i++){
      b = chain.links[ 0 ].com.Bone;
      this.links[i] = { 
        offset		: Vec3.scale( Vec3.UP, b.length ),	// Bone's pointing direction and Vector Length
        velocity	: new Vec3(),	// Current speed moving from position to target.	
        target		: new Vec3(),	// Position to reach
        position 	: new Vec3(),	// Current World Position used for LookAt.
      };
    }
  }

  //set the Spring Point Position for each bone based on its current transform data.
  reset( chain ){
    let tran = new TransformData(),
      ct, cs;

    for(let i=0; i < chain.count; i++){
      ct = chain.links[i].com.Transform;
      cs = this.links[i];

      tran.add( ct.rotation, ct.position );
      tran.transformVec( cs.offset, cs.position );
    }
  }

  update( chain, follow ){
    const HACK_OFFSET = [ 0, 0.1, 0];

    let tran 		= new TransformData(),	// Build World Space Transform for each bone.
      v			= new Vec3(),	// Temp Vector
      dir 		= new Vec3(),	// Direction Vector
      boneWPos	= new Vec3(),	// Bone World Position
      qLook		= new Quat(),	// Look Rotation
      qDelta		= new Quat(),	// Delta Rotation
      lastIdx 	= chain.count - 1,
      up,		// vertex up position
      ct,		// chain link transform
      cs;		// chain link spring data

    //Start using the follow Object's Transform to set the first bones Position and Rotation.
    tran.set(follow.rotation, follow.position);

    // Loop through all the bones in the ik chain.
    for(let i=0; i < chain.count; i++){
      // Get some reference shortcuts
      ct = chain.links[i].com.Transform;
      cs = this.links[i];

      //Get the Bone's Starting Position in World Space
      tran.transformVec( ct.position, boneWPos ); // Transform Bone Local Position to World Position

      // Get Bone's ending position in World Space 
      //   With the Bone Local Direction and Length (cs.offset), Rotate it using the parent 
      //   transform data. With it pointing in the correct world direction, add it to 
      //   bone's world position
      Vec3.transformQuat( cs.offset, tran.rotation, cs.target ).add( boneWPos );

      // The Bone's end position is the target position we want to move the spring
      // point toward based on spring forces (damped acceleration)
      this.spring( cs ); 

      // The spring postion has been updated, Now we need to determine the new
      // direction the bone needs to point to. We do that by taking the spring
      // point position and compare it to the bone's starting world space position.
      // Then we use that direction as the basis to create a Look At Rotation.
      Vec3.sub( cs.position, boneWPos, dir ).nearZero( v );

      up	= ( v.x == 0 && v.z == 0 )? Vec3.BACK : Vec3.UP;

      Quat.lookRotation( dir, up, qLook );


      if( i != 0 ){
        // Look at Rotation is created from a World Space direction. But sub bones
        // live in local space and their position/rotation is governed by their parent
        // bone. So we need to take the extra steps to basicly subtract the parent's rotation
        // from the desired world rotation, which gives us how much rotation is needed with
        // the parent's rotation to look at that direction. Sounds complicated but its not really
        //   If looking at 90 degrees,  parent is already at 45 degrees, so the sub none only needs
        //   to look only 45 degrees to point at the 90 degree point.
        quatDelta(tran.rotation, qLook, qDelta );

        // Copy data to the bone's rotation. Because the bone points up but look rotation looks forward
        // All we need to do is mul (add) a rotation offset that makes forward look up.
        ct.rotation.copy( qDelta ).mul( QUAT_FWD2UP );

        // Update the Transform Data for the next Bone
        tran.add( ct.rotation, ct.position ); //if(i !== lastIdx) 
      }else{
        // HACK !!!
        // This else condition is kind of a hack because the chain's root lives in world space, it
        // has no parent. The follow object also lives in world space. So this part of the code
        // set the chain's root to follow the object of interest

        // Rotate the offset backed on the follow object's rotation then add its position
        // for the final world space position that the chain should appear.
        Vec3.transformQuat( HACK_OFFSET, follow.rotation, ct.position ).add( follow.position );

        quatDelta(tran.rotation, qLook, qDelta );
        ct.rotation.copy( qDelta ).mul( QUAT_FWD2UP );

        //ct.rotation.copy( qLook ).mul( QUAT_FWD2UP );

        tran.set( ct.rotation, ct.position );
      }

      ct.isModified = true;
    }

  }

  spring( dSpring ){
    //.........................................
    // Apply Spring force and dampening to create acceleration for the velocity
    // f = -kx			:: spring_force = -stiffness * displacement
    // a = f - dv		:: acceleration = spring_force - damping * velocity
    let a = dSpring.target,
      b = dSpring.position,
      v = dSpring.velocity;

    let dt = Fungi.deltaTime;


    // v += -kx - dv   :: calc acceleration then add it to current velocity
    v[0] += (-this.stiffness * ( b[0] - a[0] ) - this.damping * v[0] ) * dt;
    v[1] += (-this.stiffness * ( b[1] - a[1] ) - this.damping * v[1] ) * dt;
    v[2] += (-this.stiffness * ( b[2] - a[2] ) - this.damping * v[2] ) * dt;
    v[3] += (-this.stiffness * ( b[3] - a[3] ) - this.damping * v[3] ) * dt;

    //.........................................
    // Move position  closer to target. This will be the position for the LookAt Function
    b[0] += v[0] * dt;
    b[1] += v[1] * dt;
    b[2] += v[2] * dt;
    b[3] += v[3] * dt;
  }

}