slerp method

Quaternion slerp(Quaternion q, double t)

Spherically interpolates between this quaternion and the given quaternion by t. The parameter t is clamped to the range 0, 1.

Implementation

Quaternion slerp(Quaternion  q, double t ) {
	if ( t == 0 ) return this;
	if ( t == 1 ) return copy( q );

	final x = this.x, y = this.y, z = this.z, w = this.w;

	double cosHalfTheta = w * q.w + x * q.x + y * q.y + z * q.z;

	if ( cosHalfTheta < 0 ) {
		this.w = - q.w;
		this.x = - q.x;
		this.y = - q.y;
		this.z = - q.z;

		cosHalfTheta = - cosHalfTheta;
	}
    else {
		copy( q );
	}

	if ( cosHalfTheta >= 1.0 ) {
		this.w = w;
		this.x = x;
		this.y = y;
		this.z = z;

		return this;
	}

	final sinHalfTheta = math.sqrt( 1.0 - cosHalfTheta * cosHalfTheta );

	if ( sinHalfTheta.abs() < 0.001 ) {
		this.w = 0.5 * ( w + this.w );
		this.x = 0.5 * ( x + this.x );
		this.y = 0.5 * ( y + this.y );
		this.z = 0.5 * ( z + this.z );

		return this;
	}

	final halfTheta = math.atan2( sinHalfTheta, cosHalfTheta );
	final ratioA = math.sin( ( 1 - t ) * halfTheta ) / sinHalfTheta;
	final ratioB = math.sin( t * halfTheta ) / sinHalfTheta;

	this.w = ( w * ratioA ) + ( this.w * ratioB );
	this.x = ( x * ratioA ) + ( this.x * ratioB );
	this.y = ( y * ratioA ) + ( this.y * ratioB );
	this.z = ( z * ratioA ) + ( this.z * ratioB );

	return this;
}