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 ); }