/*@brief Return the quaternion which is the result of Spherical Linear Interpolation between this and the other quaternion
* @param q The other quaternion to interpolate with
* @param t The ratio between this and q to interpolate. * If t = 0 the result is this, if t=1 the result is q.
* Slerp interpolates assuming constant velocity. */
void slerp( ref btQuaternion q, double t, out btQuaternion result )
{
double magnitude = btScalar.btSqrt( length2() * q.length2() );
//Debug.Assert( magnitude > (double)( 0 ) );
double product = dot( ref q ) / magnitude;
if( btScalar.btFabs( product ) < 1 )
{
// Take care of long angle case see http://en.wikipedia.org/wiki/Slerp
double sign = ( product < 0 ) ? -1 : 1;
double theta = btScalar.btAcos( sign * product );
double s1 = btScalar.btSin( sign * t * theta );
double d = 1.0 / btScalar.btSin( theta );
double s0 = btScalar.btSin( ( 1.0 - t ) * theta );
result.x = ( x * s0 + q.x * s1 ) * d;
result.y = ( y * s0 + q.y * s1 ) * d;
result.z = ( z * s0 + q.z * s1 ) * d;
result.w = ( w * s0 + q.w * s1 ) * d;
}
else
{
result = this;
}
}
/*@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;
}
/*@brief Return the []half[] angle between this quaternion and the other
@param q The other quaternion */
double angle( ref btQuaternion q )
{
double s = btScalar.btSqrt( length2() * q.length2() );
//Debug.Assert( s != 0.0 );
return btScalar.btAcos( dot( ref q ) / s );
}
public static void setRotation( out btMatrix3x3 result, ref btQuaternion q )
{
double d = q.length2();
#if PARANOID_ASSERTS
Debug.Assert( d != 0 );
#endif
double s = btScalar.BT_TWO / d;
double xs = q.x * s, ys = q.y * s, zs = q.z * s;
double wx = q.w * xs, wy = q.w * ys, wz = q.w * zs;
double xx = q.x * xs, xy = q.x * ys, xz = q.x * zs;
double yy = q.y * ys, yz = q.y * zs, zz = q.z * zs;
btMatrix3x3.setValue( out result,
btScalar.BT_ONE - ( yy + zz ), xy - wz, xz + wy,
xy + wz, btScalar.BT_ONE - ( xx + zz ), yz - wx,
xz - wy, yz + wx, btScalar.BT_ONE - ( xx + yy ) );
}
//public btVector3 m_el3;
// explicit btMatrix3x3(stringbtScalar m) { setFromOpenGLSubMatrix(m); }
/*@brief Constructor from Quaternion */
public btMatrix3x3( ref btQuaternion q )
{
//setRotation( q );
double d = q.length2();
//btFullAssert(d != (double)(0.0));
double s = 2.0 / d;
double xs = q.x * s, ys = q.y * s, zs = q.z * s;
double wx = q.w * xs, wy = q.w * ys, wz = q.w * zs;
double xx = q.x * xs, xy = q.x * ys, xz = q.x * zs;
double yy = q.y * ys, yz = q.y * zs, zz = q.z * zs;
m_el0.x = 1.0 - ( yy + zz ); m_el0.y = xy - wz; m_el0.z = xz + wy; m_el0.w = 0;
m_el1.x = xy + wz; m_el1.y = 1.0 - ( xx + zz ); m_el1.z = yz - wx; m_el1.w = 0;
m_el2.x = xz - wy; m_el2.y = yz + wx; m_el2.z = 1.0 - ( xx + yy ); m_el2.w = 0;
#if !DISABLE_ROW4
m_el3.x = 0; m_el3.y = 0; m_el3.z = 0; m_el3.w = 1;
#endif
}
/*@brief Return the quaternion which is the result of Spherical Linear Interpolation between this and the other quaternion
* @param q The other quaternion to interpolate with
* @param t The ratio between this and q to interpolate. * If t = 0 the result is this, if t=1 the result is q.
* Slerp interpolates assuming constant velocity. */
void slerp( ref btQuaternion q, float t, out btQuaternion result )
{
float magnitude = btScalar.btSqrt( length2() * q.length2() );
//Debug.Assert( magnitude > (float)( 0 ) );
float product = dot( ref q ) / magnitude;
if( btScalar.btFabs( product ) < 1 )
{
// Take care of long angle case see http://en.wikipedia.org/wiki/Slerp
float sign = ( product < 0 ) ? -1 : 1;
float theta = btScalar.btAcos( sign * product );
float s1 = btScalar.btSin( sign * t * theta );
float d = btScalar.BT_ONE / btScalar.btSin( theta );
float s0 = btScalar.btSin( ( btScalar.BT_ONE - t ) * theta );
result.x = ( x * s0 + q.x * s1 ) * d;
result.y = ( y * s0 + q.y * s1 ) * d;
result.z = ( z * s0 + q.z * s1 ) * d;
result.w = ( w * s0 + q.w * s1 ) * d;
}
else
{
result = this;
}
}