Limb Inverse Kinematic Solver

limb_ik.js

// https://www.mathsisfun.com/algebra/trig-solving-sss-triangles.html
static lawcos_sss( aLen, bLen, cLen ){
	// Law of Cosines - SSS : cos(C) = (a^2 + b^2 - c^2) / 2ab
	// The Angle between A and B with C being the opposite length of the angle.
	return Math.acos( (aLen*aLen + bLen*bLen - cLen*cLen) / (2 * aLen * bLen) );
}

function limb( chain, target, pose, wt ){
	//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
	// Using law of cos SSS, so need the length of all sides of the triangle
	let aLen	= chain.lens[ 0 ],
		bLen	= chain.lens[ 1 ],
		cLen	= Math.sqrt( target.distanceSqr ),
		wq 		= new Quat(),
		rot 	= new Quat(),	
		tmp		= new Quat(),
		rad;

	//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
	// FIRST BONE - Aim then rotate by the angle.
	_aimBone( chain, target, wt, rot );						// Aime the first bone toward the target oriented with the bend direction.
	
	rad	= Maths.lawcos_sss( aLen, cLen, bLen );				// Get the Angle between First Bone and Target.
	tmp.setAxisAngle( target.axis.x, -rad );				// Use the Target's X axis for rotation
	rot.pmul( tmp );										// Rotate the the aimed bone by the angle from SSS
	wq.copy( rot );											// Save a Copy as World Rotation before converting to local space

	rot.pmul( wt.rot.invert( tmp ) );						// Convert to Bone's Local Space by mul invert of parent bone rotation

	if( pose )	pose.updateBone( chain.idx[ 0 ], rot );		// Save result to bone.
	else		chain.updateBone( 0, rot );

	//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
	// SECOND BONE
	// Need to rotate from Right to Left, So take the angle and subtract it from 180 to rotate from 
	// the other direction. Ex. L->R 70 degrees == R->L 110 degrees
	rad	= Math.PI - Maths.lawcos_sss( aLen, bLen, cLen );

	Quat.mul( wq, chain.bind[ 1 ].rot, rot ) 				// Add Bone 2's Local Bind Rotation to Bone 1's new World Rotation.
		.pmul( tmp.setAxisAngle( target.axis.x, rad ) )		// Rotate it by the target's x-axis
		.pmul( wq.invert( tmp ) );							// Convert to Bone's Local Space

	if( pose )	pose.updateBone( chain.idx[ 1 ], rot );
	else		chain.updateBone( 1, rot );
}

function _aimBone( chain, target, wt, out ){
	// Create the rotation that aligns to the target's z Axis, that also points the bending at targets y axis
	// -- Targets Forward becomes Bone's Up
	// -- Depending on the chain, the new forward can be either X or Y.
	// -- Calc the bone's new Left based on its new Up/Forward directions
	let zAxis	= target.axis[ chain.ikTargetFwd ],
		xAxis	= Vec3.cross( target.axis.z, zAxis ),
		qTarget	= Quat.fromAxis( xAxis, target.axis.z, zAxis ),
		tmp		= new Quat();

	Quat.mul( wt.rot, chain.bind[ 0 ].rot, out );			// Get Worldspace rotation of the first bone
	if( Quat.dot( out, qTarget ) < 0 ) qTarget.flip();		// Axis can be pointing in the wrong direction, Flip the signs: Thx @gszauer
	Quat.mul( qTarget, out.invert( tmp ), tmp );			// Diff between starting rotation and final rotation
	out.pmul( tmp );										// Add that difference to starting rotation

	return out;
}