public void SetMotorTarget(ref IndexedQuaternion qAinB, float dt) // qAinB is rotation of body A wrt body B.
{
// convert target from body to constraint space
IndexedQuaternion qConstraint = MathUtil.QuaternionInverse(m_rbBFrame.GetRotation()) * qAinB * m_rbAFrame.GetRotation();
qConstraint.Normalize();
// extract "pure" hinge component
IndexedVector3 vNoHinge = MathUtil.QuatRotate(ref qConstraint, ref vHinge);
vNoHinge.Normalize();
IndexedQuaternion qNoHinge = MathUtil.ShortestArcQuat(ref vHinge, ref vNoHinge);
IndexedQuaternion qHinge = MathUtil.QuaternionInverse(ref qNoHinge) * qConstraint;
qHinge.Normalize();
// compute angular target, clamped to limits
float targetAngle = MathUtil.QuatAngle(ref qHinge);
if (targetAngle > MathUtil.SIMD_PI) // long way around. flip quat and recalculate.
{
qHinge = -qHinge;
targetAngle = MathUtil.QuatAngle(ref qHinge);
}
if (qHinge.Z < 0)
{
targetAngle = -targetAngle;
}
SetMotorTarget(targetAngle, dt);
}
public static void CalculateDiffAxisAngle(ref IndexedMatrix transform0, ref IndexedMatrix transform1, out IndexedVector3 axis, out float angle)
{
//IndexedMatrix dmat = GetRotateMatrix(ref transform1) * IndexedMatrix.Invert(GetRotateMatrix(ref transform0));
IndexedBasisMatrix dmat = transform1._basis * transform0._basis.Inverse();
IndexedQuaternion dorn = IndexedQuaternion.Identity;
GetRotation(ref dmat, out dorn);
///floating point inaccuracy can lead to w component > 1..., which breaks
dorn.Normalize();
angle = MathUtil.QuatAngle(ref dorn);
axis = new IndexedVector3(dorn.X, dorn.Y, dorn.Z);
//axis[3] = float(0.);
//check for axis length
float len = axis.LengthSquared();
if (len < MathUtil.SIMD_EPSILON * MathUtil.SIMD_EPSILON)
{
axis = new IndexedVector3(1,0,0);
}
else
{
axis.Normalize();
}
}
public static void IntegrateTransform(ref IndexedMatrix curTrans,ref IndexedVector3 linvel,ref IndexedVector3 angvel,float timeStep,out IndexedMatrix predictedTransform)
{
predictedTransform = IndexedMatrix.CreateTranslation(curTrans._origin + linvel * timeStep);
// #define QUATERNION_DERIVATIVE
#if QUATERNION_DERIVATIVE
IndexedVector3 pos;
IndexedQuaternion predictedOrn;
IndexedVector3 scale;
curTrans.Decompose(ref scale, ref predictedOrn, ref pos);
predictedOrn += (angvel * predictedOrn) * (timeStep * .5f));
predictedOrn.Normalize();
#else
//Exponential map
//google for "Practical Parameterization of Rotations Using the Exponential Map", F. Sebastian Grassia
IndexedVector3 axis;
float fAngle = angvel.Length();
//limit the angular motion
if (fAngle*timeStep > ANGULAR_MOTION_THRESHOLD)
{
fAngle = ANGULAR_MOTION_THRESHOLD / timeStep;
}
if ( fAngle < 0.001f )
{
// use Taylor's expansions of sync function
axis = angvel*( 0.5f*timeStep-(timeStep*timeStep*timeStep)*(0.020833333333f)*fAngle*fAngle );
}
else
{
// sync(fAngle) = sin(c*fAngle)/t
axis = angvel*( (float)Math.Sin(0.5f*fAngle*timeStep)/fAngle );
}
IndexedQuaternion dorn = new IndexedQuaternion(axis.X,axis.Y,axis.Z,(float)Math.Cos( fAngle*timeStep*.5f) );
IndexedQuaternion orn0 = curTrans.GetRotation();
IndexedQuaternion predictedOrn = dorn * orn0;
predictedOrn.Normalize();
#endif
IndexedMatrix newMatrix = IndexedMatrix.CreateFromQuaternion(predictedOrn);
predictedTransform._basis = newMatrix._basis;
}
public static void CalculateDiffAxisAngleQuaternion(ref IndexedQuaternion orn0, ref IndexedQuaternion orn1a, out IndexedVector3 axis, out float angle)
{
IndexedQuaternion orn1 = MathUtil.QuatFurthest(ref orn0, ref orn1a);
IndexedQuaternion dorn = orn1 * MathUtil.QuaternionInverse(ref orn0);
///floating point inaccuracy can lead to w component > 1..., which breaks
dorn.Normalize();
angle = MathUtil.QuatAngle(ref dorn);
axis = new IndexedVector3(dorn.X, dorn.Y, dorn.Z);
//check for axis length
float len = axis.LengthSquared();
if (len < MathUtil.SIMD_EPSILON * MathUtil.SIMD_EPSILON)
{
axis = new IndexedVector3(1f, 0, 0);
}
else
{
axis.Normalize();
}
}