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