public static void quatRotate( ref btQuaternion rotation, ref btVector3 v, out btVector3 result )
{
btQuaternion q;
btQuaternion tmp;
rotation.Mult( ref v, out q );
rotation.inverse( out tmp );
q.Mult( ref tmp, out result );
}
/*@brief Return the inverse of a quaternion*/
public static void inverse( ref btQuaternion q, out btQuaternion result )
{
q.inverse( out result );
}
/*@brief Return the angle between this quaternion and the other along the shortest path
@param q The other quaternion */
public double angleShortestPath( ref btQuaternion q )
{
double s = btScalar.btSqrt( length2() * q.length2() );
//Debug.Assert( s != 0.0 );
if( dot( ref q ) < 0 ) // Take care of long angle case see http://en.wikipedia.org/wiki/Slerp
{
btQuaternion b;
q.inverse( out b );
return btScalar.btAcos( dot( ref b ) / s ) * 2.0;
}
else
return btScalar.btAcos( dot( ref q ) / s ) * 2.0;
}
/*@todo document this and it's use */
public void nearest( ref btQuaternion qd, out btQuaternion result )
{
btQuaternion diff, sum;
this.Sub( ref qd, out diff );
this.Add( ref qd, out sum );
if( diff.dot( ref diff ) < sum.dot( ref sum ) )
result = qd;
qd.inverse( out result );
}
internal static void calculateDiffAxisAngleQuaternion( ref btQuaternion orn0, ref btQuaternion orn1a, out btVector3 axis, out double angle )
{
btQuaternion orn1;
orn0.nearest( ref orn1a, out orn1 );
btQuaternion dorn;
btQuaternion tmp;
orn0.inverse( out tmp );
orn1.Mult( ref tmp, out dorn );
angle = dorn.getAngle();
axis.x = dorn.x; axis.y = dorn.y; axis.z = dorn.z; axis.w = 0;
//check for axis length
double len = axis.length2();
if( len < btScalar.SIMD_EPSILON * btScalar.SIMD_EPSILON )
axis = btVector3.xAxis;
else
axis.Div( btScalar.btSqrt( len ), out axis );
}