jacobian.js

jntCount     = ChainPose.count;
angles[]     = new Array( jntCount ).fill( 0 )

loop
    // ~~~~~~~~~~~~~~~~~~~~~~~~~~~~
    // Get world space transform (pos,rot,scl) of each bone in the chain
    // Then preallocate space for the jacobian matrix
    transforms[] = compute_WorldSpace_Transforms( ChainPose );
    jacobian     = Array( transforms.length * 3 * 3 ); // Each Joint gets 3 floats for 3 axes
    
    // ~~~~~~~~~~~~~~~~~~~~~~~~~~~~
    // Fill in Jacobian Matrix
    i=0;
    for t of each transform{
        toEffector      = effectorPos - t.pos;

        cross           = cross( [1,0,0], toEffector );
        jacobian[ i++ ] = cross[ 0 ]
        jacobian[ i++ ] = cross[ 1 ]
        jacobian[ i++ ] = cross[ 2 ]
        
        cross           = cross( [0,1,0], toEffector );
        jacobian[ i++ ] = cross[ 0 ]
        jacobian[ i++ ] = cross[ 1 ]
        jacobian[ i++ ] = cross[ 2 ]
        
        cross           = cross( [0,0,1], toEffector );
        jacobian[ i++ ] = cross[ 0 ]
        jacobian[ i++ ] = cross[ 1 ]
        jacobian[ i++ ] = cross[ 2 ]
    }
    
    // ~~~~~~~~~~~~~~~~~~~~~~~~~~~~
    // Apply the Transpose Matrix
    tJacobian = transpose( jacobian )
    toTarget  = targetPos - effectorPos;

    // theta = mul( tJacobian, toTarget )
    theta = [0,0,0]
    for( let i=0; i < tJacobian; i +=3 ){
        theta[ 0 ] += toTarget[ 0 ] * tJacobian[ i + 0 ];
        theta[ 1 ] += toTarget[ 1 ] * tJacobian[ i + 1 ];
        theta[ 2 ] += toTarget[ 2 ] * tJacobian[ i + 2 ];
    }

    inc     = 0.05;
    theta  *= inc;
    angles += theta;

    // ~~~~~~~~~~~~~~~~~~~~~~~~~~~~
    // Apply Axis+Angle rotations to worldspace joint rotation
    i=0;
    for t of each transform{
        q     = quat.setAxisAngle( [1,0,0], this.angles[ i ] ); // Create rotation
        t.rot = q * t.rot;                                      // Apply to joint's current ws rotation
        
        // Store it in local space for use
        if( i == 0 ) ChainPose.setLocalRotation( i, t.rot );
        else{
            pinv = quat.invert( transforms[ i-1 ].rot );    // Parent World Space Rotation Inverted
            ChainPose.setLocalRotation( i, pinv * t.rot );  // Convert to local space.
        }
        i++;
    }

    updateBoneView( ChainPose );
end loop