public static void integrateTransform( ref btTransform curTrans, ref btVector3 linvel, ref btVector3 angvel
, double timeStep, out btTransform predictedTransform )
{
btVector3 tmp;
btVector3 tmp2;
linvel.Mult( timeStep, out tmp );
curTrans.getOrigin( out tmp2 );
tmp2.Add( ref tmp, out predictedTransform.m_origin );
#if QUATERNION_DERIVATIVE
btQuaternion predictedOrn = curTrans.getRotation();
predictedOrn += (angvel predictedOrn) * (timeStep 0.5);
predictedOrn.normalize();
#else
//Exponential map
//google for "Practical Parameterization of Rotations Using the Exponential Map", F. Sebastian Grassia
btVector3 axis;
double fAngle = angvel.length();
//limit the angular motion
if( fAngle * timeStep > ANGULAR_MOTION_THRESHOLD )
{
fAngle = ANGULAR_MOTION_THRESHOLD / timeStep;
}
if( fAngle < 0.001 )
{
// use Taylor's expansions of sync function
angvel.Mult( ( btScalar.BT_HALF * timeStep - ( timeStep * timeStep * timeStep ) * ( 0.020833333333 ) * fAngle * fAngle ), out axis );
}
else
{
// sync(fAngle) = sin(cfAngle)/t
angvel.Mult( ( btScalar.btSin( btScalar.BT_HALF * fAngle * timeStep ) / fAngle ), out axis );
}
btQuaternion dorn = new btQuaternion( axis.x, axis.y, axis.z, btScalar.btCos( fAngle * timeStep * 0.5 ) );
btQuaternion orn0;
curTrans.getRotation( out orn0 );
btQuaternion predictedOrn;
dorn.Mult( ref orn0, out predictedOrn );
predictedOrn.normalize();
#endif
btMatrix3x3.setRotation( out predictedTransform.m_basis, ref predictedOrn);
//predictedTransform.setRotation( ref predictedOrn );
}
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 an inversely scaled versionof this quaternion
@param s The inverse scale factor */
public static void Div( ref btQuaternion q, double s, out btQuaternion result )
{
//Debug.Assert( s != 0.0 );
q.Mult( ( 1.0 / s ), out result );
}
/*@brief Return an inversely scaled versionof this quaternion
@param s The inverse scale factor */
public static void Div( ref btQuaternion q, float s, out btQuaternion result )
{
//Debug.Assert( s != 0.0 );
q.Mult( ( btScalar.BT_ONE / s ), out result );
}