public void setGravity( ref btVector3 acceleration )
{
if( m_inverseMass != btScalar.BT_ZERO )
{
acceleration.Div( m_inverseMass, out m_gravity );
//m_gravity = acceleration * ( (double)( 1.0 ) / m_inverseMass );
}
m_gravity_acceleration = acceleration;
}
internal static void calculateDiffAxisAngle( ref btTransform transform0, ref btTransform transform1, out btVector3 axis, out double angle )
{
btMatrix3x3 tmp_m;
transform0.m_basis.inverse( out tmp_m );
btMatrix3x3 dmat;
transform1.m_basis.Mult( ref tmp_m, out dmat );
btQuaternion dorn;
dmat.getRotation( out dorn );
///floating point inaccuracy can lead to w component > 1..., which breaks
dorn.normalize();
angle = dorn.getAngle();
axis.x = dorn.x; axis.y = dorn.y; axis.z = dorn.z; axis.w = 0;
//check for axis length
double len = axis.length2();
if( len < btScalar.SIMD_EPSILON * btScalar.SIMD_EPSILON )
axis = btVector3.xAxis;
else
axis.Div( btScalar.btSqrt( len ), out axis );
}
internal static void calculateDiffAxisAngleQuaternion( ref btQuaternion orn0, ref btQuaternion orn1a, out btVector3 axis, out double angle )
{
btQuaternion orn1;
orn0.nearest( ref orn1a, out orn1 );
btQuaternion dorn;
btQuaternion tmp;
orn0.inverse( out tmp );
orn1.Mult( ref tmp, out dorn );
angle = dorn.getAngle();
axis.x = dorn.x; axis.y = dorn.y; axis.z = dorn.z; axis.w = 0;
//check for axis length
double len = axis.length2();
if( len < btScalar.SIMD_EPSILON * btScalar.SIMD_EPSILON )
axis = btVector3.xAxis;
else
axis.Div( btScalar.btSqrt( len ), out axis );
}
internal static void calculateVelocity( ref btTransform transform0, ref btTransform transform1, double timeStep, out btVector3 linVel, out btVector3 angVel )
{
transform1.m_origin.Sub( ref transform0.m_origin, out linVel );
linVel.Div( timeStep, out linVel );
btVector3 axis;
double angle;
calculateDiffAxisAngle( ref transform0, ref transform1, out axis, out angle );
axis.Mult( angle, out angVel );
angVel.Div( timeStep, out angVel );
}