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