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 void updateSeparatingDistance( ref btTransform transA, ref btTransform transB )
{
btVector3 toPosA; transA.getOrigin( out toPosA );
btVector3 toPosB; transB.getOrigin( out toPosB );
btQuaternion toOrnA;
transA.getRotation( out toOrnA );
btQuaternion toOrnB;
transB.getRotation( out toOrnB );
if( m_separatingDistance > 0 )
{
btVector3 linVelA, angVelA, linVelB, angVelB;
btTransformUtil.calculateVelocityQuaternion( ref m_posA, ref toPosA, ref m_ornA, ref toOrnA, 1, out linVelA, out angVelA );
btTransformUtil.calculateVelocityQuaternion( ref m_posB, ref toPosB, ref m_ornB, ref toOrnB, 1, out linVelB, out angVelB );
double maxAngularProjectedVelocity = angVelA.length() * m_boundingRadiusA + angVelB.length() * m_boundingRadiusB;
btVector3 relLinVel;
linVelB.Sub( ref linVelA, out relLinVel );
double relLinVelocLength = relLinVel.dot( ref m_separatingNormal );
if( relLinVelocLength < 0 )
{
relLinVelocLength = 0;
}
double projectedMotion = maxAngularProjectedVelocity + relLinVelocLength;
m_separatingDistance -= projectedMotion;
}
m_posA = toPosA;
m_posB = toPosB;
m_ornA = toOrnA;
m_ornB = toOrnB;
}
public void initSeparatingDistance( ref btVector3 separatingVector, double separatingDistance, ref btTransform transA, ref btTransform transB )
{
m_separatingDistance = separatingDistance;
if( m_separatingDistance > 0 )
{
m_separatingNormal = separatingVector;
m_posA = transA.m_origin;
m_posB = transB.m_origin;
transA.getRotation( out m_ornA );
transB.getRotation( out m_ornB );
}
}
/*@brief Return the transform of the btQuaternion */
public static void Apply( ref btTransform t, ref btQuaternion q, out btQuaternion result )
{
btQuaternion tmp;
t.getRotation( out tmp );
tmp.Mult( ref q, out result );
}
