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 void UpdateSeparatingDistance(ref IndexedMatrix transA, ref IndexedMatrix transB)
{
IndexedVector3 toPosA = transA._origin;
IndexedVector3 toPosB = transB._origin;
IndexedQuaternion toOrnA = transA.GetRotation();
IndexedQuaternion toOrnB = transB.GetRotation();
if (m_separatingDistance > 0.0f)
{
IndexedVector3 linVelA;
IndexedVector3 angVelA;
IndexedVector3 linVelB;
IndexedVector3 angVelB;
TransformUtil.CalculateVelocityQuaternion(ref m_posA, ref toPosA, ref m_ornA, ref toOrnA, 1f, out linVelA, out angVelA);
TransformUtil.CalculateVelocityQuaternion(ref m_posB, ref toPosB, ref m_ornB, ref toOrnB, 1f, out linVelB, out angVelB);
float maxAngularProjectedVelocity = angVelA.Length() * m_boundingRadiusA + angVelB.Length() * m_boundingRadiusB;
IndexedVector3 relLinVel = (linVelB - linVelA);
float relLinVelocLength = IndexedVector3.Dot((linVelB - linVelA), m_separatingNormal);
if (relLinVelocLength < 0f)
{
relLinVelocLength = 0f;
}
float projectedMotion = maxAngularProjectedVelocity + relLinVelocLength;
m_separatingDistance -= projectedMotion;
}
m_posA = toPosA;
m_posB = toPosB;
m_ornA = toOrnA;
m_ornB = toOrnB;
}
public SimMotionState(CollisionWorld pWorld, uint id, IndexedMatrix starTransform, object frameUpdates)
{
m_properties = new EntityProperties()
{
ID = id,
Position = starTransform._origin,
Rotation = starTransform.GetRotation()
};
m_lastProperties = new EntityProperties()
{
ID = id,
Position = starTransform._origin,
Rotation = starTransform.GetRotation()
};
m_world = pWorld;
m_xform = starTransform;
}
public EntityProperties FromTransform(uint id, IndexedMatrix startTransform)
{
EntityProperties ret = new EntityProperties();
ID = id;
Position = startTransform._origin;
Rotation = startTransform.GetRotation();
return(ret);
}
public void ConvexSweepTest(ConvexShape castShape, ref IndexedMatrix convexFromWorld, ref IndexedMatrix convexToWorld, ConvexResultCallback resultCallback, float allowedCcdPenetration)
{
IndexedMatrix convexFromTrans = convexFromWorld;
IndexedMatrix convexToTrans = convexToWorld;
IndexedVector3 castShapeAabbMin;
IndexedVector3 castShapeAabbMax;
/* Compute AABB that encompasses angular movement */
IndexedVector3 linVel, angVel;
TransformUtil.CalculateVelocity(ref convexFromTrans, ref convexToTrans, 1.0f, out linVel, out angVel);
// FIXME MAN check this - should be a get/set rotation call, basis copy like this may break with scale?
IndexedMatrix R = IndexedMatrix.Identity;
R.SetRotation(convexFromTrans.GetRotation());
castShape.CalculateTemporalAabb(ref R, ref linVel, ref angVel, 1.0f, out castShapeAabbMin, out castShapeAabbMax);
/// go over all objects, and if the ray intersects their aabb + cast shape aabb,
// do a ray-shape query using convexCaster (CCD)
for (int i = 0; i < m_overlappingObjects.Count; i++)
{
CollisionObject collisionObject = m_overlappingObjects[i];
//only perform raycast if filterMask matches
if (resultCallback.NeedsCollision(collisionObject.GetBroadphaseHandle()))
{
//RigidcollisionObject* collisionObject = ctrl->GetRigidcollisionObject();
IndexedVector3 collisionObjectAabbMin;
IndexedVector3 collisionObjectAabbMax;
IndexedMatrix t = collisionObject.GetWorldTransform();
collisionObject.GetCollisionShape().GetAabb(ref t, out collisionObjectAabbMin, out collisionObjectAabbMax);
AabbUtil2.AabbExpand(ref collisionObjectAabbMin, ref collisionObjectAabbMax, ref castShapeAabbMin, ref castShapeAabbMax);
float hitLambda = 1f; //could use resultCallback.m_closestHitFraction, but needs testing
IndexedVector3 hitNormal;
if (AabbUtil2.RayAabb(convexFromWorld._origin, convexToWorld._origin, ref collisionObjectAabbMin, ref collisionObjectAabbMax, ref hitLambda, out hitNormal))
{
IndexedMatrix wt = collisionObject.GetWorldTransform();
CollisionWorld.ObjectQuerySingle(castShape, ref convexFromTrans, ref convexToTrans,
collisionObject,
collisionObject.GetCollisionShape(),
ref wt,
resultCallback,
allowedCcdPenetration);
}
}
}
}
void InitSeparatingDistance(ref IndexedVector3 separatingVector, float separatingDistance, ref IndexedMatrix transA, ref IndexedMatrix transB)
{
m_separatingNormal = separatingVector;
m_separatingDistance = separatingDistance;
IndexedVector3 toPosA = transA._origin;
IndexedVector3 toPosB = transB._origin;
IndexedQuaternion toOrnA = transA.GetRotation();
IndexedQuaternion toOrnB = transB.GetRotation();
m_posA = toPosA;
m_posB = toPosB;
m_ornA = toOrnA;
m_ornB = toOrnB;
}
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 void SetWorldTransform(ref IndexedMatrix worldTrans, bool force)
{
m_xform = worldTrans;
// Put the new transform into m_properties
m_properties.Position = m_xform._origin;
m_properties.Rotation = m_xform.GetRotation();
// A problem with stock Bullet is that we don't get an event when an object is deactivated.
// This means that the last non-zero values for linear and angular velocity
// are left in the viewer who does dead reconning and the objects look like
// they float off.
// BulletSim ships with a patch to Bullet which creates such an event.
m_properties.Velocity = Rigidbody.GetLinearVelocity();
m_properties.AngularVelocity = Rigidbody.GetAngularVelocity();
if (force
|| !AlmostEqual(ref m_lastProperties.Position, ref m_properties.Position, POSITION_TOLERANCE) ||
!AlmostEqual(ref m_properties.Rotation, ref m_lastProperties.Rotation, ROTATION_TOLERANCE)
// If the Velocity and AngularVelocity are zero, most likely the object has
// been deactivated. If they both are zero and they have become zero recently,
// make sure a property update is sent so the zeros make it to the viewer.
|| ((m_properties.Velocity == ZeroVect && m_properties.AngularVelocity == ZeroVect)
&&
(m_properties.Velocity != m_lastProperties.Velocity ||
m_properties.AngularVelocity != m_lastProperties.AngularVelocity))
// If Velocity and AngularVelocity are non-zero but have changed, send an update.
|| !AlmostEqual(ref m_properties.Velocity, ref m_lastProperties.Velocity, VELOCITY_TOLERANCE)
||
!AlmostEqual(ref m_properties.AngularVelocity, ref m_lastProperties.AngularVelocity,
ANGULARVELOCITY_TOLERANCE)
)
{
// Add this update to the list of updates for this frame.
m_lastProperties = m_properties;
if (m_world.LastEntityProperty < m_world.UpdatedObjects.Length)
{
m_world.UpdatedObjects[m_world.LastEntityProperty++] = (m_properties);
}
//(*m_updatesThisFrame)[m_properties.ID] = &m_properties;
}
}
public static void runTests()
{
String filename = @"e:\users\man\bullet\xna-math-debug-output.txt";
FileStream filestream = File.Open(filename, FileMode.Create, FileAccess.Write, FileShare.None);
StreamWriter streamWriter = new StreamWriter(filestream);
streamWriter.WriteLine("Identity");
IndexedMatrix m = IndexedMatrix.Identity;
MathUtil.PrintMatrix(streamWriter, m);
streamWriter.WriteLine("setEuler 2,0,0");
m._basis.SetEulerZYX(2, 0, 0);
MathUtil.PrintMatrix(streamWriter, m);
streamWriter.WriteLine("setEuler 0,2,0");
m = IndexedMatrix.Identity;
m._basis.SetEulerZYX(0, 2, 0);
MathUtil.PrintMatrix(streamWriter, m);
streamWriter.WriteLine("setEuler 0,0,2");
m = IndexedMatrix.Identity;
m._basis.SetEulerZYX(0, 0, 2);
MathUtil.PrintMatrix(streamWriter, m);
streamWriter.WriteLine("setEuler 2,2,0");
m = IndexedMatrix.Identity;
m._basis.SetEulerZYX(2, 2, 0);
MathUtil.PrintMatrix(streamWriter, m);
streamWriter.WriteLine("setEuler 2,0,2");
m = IndexedMatrix.Identity;
m._basis.SetEulerZYX(2, 0, 2);
MathUtil.PrintMatrix(streamWriter, m);
streamWriter.WriteLine("setEuler 2,2,2");
m = IndexedMatrix.Identity;
m._basis.SetEulerZYX(2, 2, 2);
MathUtil.PrintMatrix(streamWriter, m);
streamWriter.WriteLine("setEuler 0.5*PI,0,0 Trans 100,100,100 MULTIPLIED BY setEuler 0,0,0.5*PI Trans -10,10,0");
m = IndexedMatrix.Identity;
m._basis.SetEulerZYX(MathUtil.SIMD_HALF_PI, 0, 0);
m._origin = new IndexedVector3(100, 100, 100);
IndexedMatrix m2 = IndexedMatrix.Identity;
m2._basis.SetEulerZYX(0, 0, MathUtil.SIMD_HALF_PI);
m2._origin = new IndexedVector3(-10, 10, 0);
IndexedMatrix m3 = m * m2;
MathUtil.PrintMatrix(streamWriter, m3);
//streamWriter.WriteLine("Broken axis comparison");
//m = IndexedMatrix.Identity;
//m.Right = new IndexedVector3(1, 0, 0);
//m.Up = new IndexedVector3(0, 1, 0);
//m.Backward = new IndexedVector3(0, 0, 1);
//m._origin = new IndexedVector3(0, 20, 0);
//m2 = IndexedMatrix.Identity;
//m2.Right = new IndexedVector3(0, -1, 0);
//m2.Up = new IndexedVector3(1, 0, 0);
//m2.Backward = new IndexedVector3(0, 0, 1);
//m2._origin = new IndexedVector3(1, -1, -1);
//m3 = m * m2;
//MathUtil.PrintMatrix(streamWriter, m3);
//streamWriter.WriteLine("setEuler 0.5*PI,0,0 Trans 0,0,0 MULTIPLIED BY setEuler 0,0,0.5*PI Trans -10,10,0");
//m = IndexedMatrix.Identity;
//m._basis.SetEulerZYX(MathUtil.SIMD_HALF_PI, 0, 0);
//m._origin = new IndexedVector3(0, 0, 0);
//m2 = IndexedMatrix.Identity;
//m2._basis.SetEulerZYX(0, 0, MathUtil.SIMD_HALF_PI);
//m2._origin = new IndexedVector3(-10, 10, 0);
//m3 = m * m2;
//MathUtil.PrintMatrix(streamWriter, m3);
//streamWriter.WriteLine("setEuler 0.25*PI,0,0 Trans 33,0,0 MULTIPLIED BY setEuler 0,0,0.5*PI Trans 0,0,0");
//m = IndexedMatrix.Identity;
//m._basis.SetEulerZYX(MathUtil.SIMD_QUARTER_PI, 0, 0);
//m._origin = new IndexedVector3(33, 0, 0);
//m2 = IndexedMatrix.Identity;
//m2._basis.SetEulerZYX(0, 0, MathUtil.SIMD_HALF_PI);
//m2._origin = new IndexedVector3(0, 0, 0);
//m3 = m *m2;
//MathUtil.PrintMatrix(streamWriter, m3);
streamWriter.WriteLine("transposeTimes");
m = IndexedMatrix.Identity;
m._basis.SetEulerZYX(2, 1, 2);
m._origin = new IndexedVector3(3, 3, 3);
streamWriter.WriteLine("");
//MathUtil.PrintMatrix(streamWriter, m);
m2 = IndexedMatrix.Identity;
m2._basis.SetEulerZYX(1, 2, -2);
m2._origin = new IndexedVector3(5, 2, 13);
//MathUtil.PrintMatrix(streamWriter, m2);
IndexedBasisMatrix im3 = m._basis.TransposeTimes(m2._basis);
MathUtil.PrintMatrix(streamWriter, im3);
streamWriter.WriteLine("inverseTransform.");
m = IndexedMatrix.Identity;
m._basis.SetEulerZYX(1, -2, -1);
m._origin = new IndexedVector3(-1, 2, -3);
IndexedVector3 v = new IndexedVector3(20, 25, 30);
IndexedVector3 result = MathUtil.InverseTransform(ref m, ref v);
MathUtil.PrintVector3(streamWriter, result);
streamWriter.WriteLine("");
streamWriter.WriteLine("inverseTimes.");
m = IndexedMatrix.Identity;
m._basis.SetEulerZYX(1, -2, -1);
m._origin = new IndexedVector3(-1, 2, -3);
m2 = IndexedMatrix.Identity;
m2._basis.SetEulerZYX(0.3f, 0.8f, 2.3f);
m2._origin = new IndexedVector3(20, 25, 30);
m3 = m.InverseTimes(ref m2);
MathUtil.PrintMatrix(streamWriter, m3);
streamWriter.WriteLine("Transform.");
m = IndexedMatrix.Identity;
m._basis.SetEulerZYX(1, -2, -1);
m._origin = new IndexedVector3(-1, 2, -3);
v = new IndexedVector3(20, 25, 30);
result = m * v;
MathUtil.PrintVector3(streamWriter, result);
streamWriter.WriteLine("");
streamWriter.WriteLine("TransformNormal.");
m = IndexedMatrix.Identity;
m._basis.SetEulerZYX(1, -2, -1);
m._origin = new IndexedVector3(-1, 2, -3);
v = new IndexedVector3(20, 25, 30);
//IndexedMatrix tm = IndexedMatrix.Transpose(m);
//result = IndexedVector3.TransformNormal(v,tm);
result = m._basis * v;
MathUtil.PrintVector3(streamWriter, result);
streamWriter.WriteLine("");
streamWriter.WriteLine("quatAngle");
Quaternion q = Quaternion.CreateFromYawPitchRoll(0.3f, 0.2f, 0.7f);
float fresult = MathUtil.QuatAngle(ref q);
streamWriter.WriteLine(String.Format("{0:0.00000000}", fresult));
streamWriter.WriteLine("quatRotate");
v = new IndexedVector3(20, -25, 30);
result = MathUtil.QuatRotate(q, v);
MathUtil.PrintVector3(streamWriter, result);
streamWriter.WriteLine("");
streamWriter.WriteLine("shortestArcQuat");
v = new IndexedVector3(2f, -1f, 3f);
IndexedVector3 v2 = new IndexedVector3(0.5f, 0.1f, 0.7f);
q = MathUtil.ShortestArcQuat(v, v2);
MathUtil.PrintQuaternion(streamWriter, q);
streamWriter.WriteLine("");
streamWriter.WriteLine("quaternionMultiply");
q = Quaternion.CreateFromYawPitchRoll(0.3f, 0.2f, 0.7f);
Quaternion q2 = Quaternion.CreateFromYawPitchRoll(-0.71f, 0.8f, 0.3f);
q = MathUtil.QuaternionMultiply(q, q2);
MathUtil.PrintQuaternion(streamWriter, q);
streamWriter.WriteLine("");
//streamWriter.WriteLine("matrixToEulerXYZ");
//m = IndexedMatrix.Identity;
//m._basis..SetEulerZYX(1, -2, -1);
//m._origin = new IndexedVector3(-1, 2, -3);
//MathUtil.MatrixToEulerXYZ(ref m, out result);
//MathUtil.PrintVector3(streamWriter, result);
//streamWriter.WriteLine("");
streamWriter.WriteLine("getSkewSymmetrixMatrix");
v = new IndexedVector3(0.2f, 0.7f, -0.3f);
IndexedVector3 v3;
IndexedVector3 v4;;
MathUtil.GetSkewSymmetricMatrix(ref v, out v2, out v3, out v4);
MathUtil.PrintVector3(streamWriter, v2);
streamWriter.WriteLine("");
MathUtil.PrintVector3(streamWriter, v3);
streamWriter.WriteLine("");
MathUtil.PrintVector3(streamWriter, v4);
streamWriter.WriteLine("");
streamWriter.WriteLine("quaternion create");
m = IndexedMatrix.Identity;
m._basis.SetEulerZYX(0, 0, MathUtil.SIMD_HALF_PI);
m._origin = new IndexedVector3(0.0f, 0.30f, 0.0f);
m2 = IndexedMatrix.Identity;
m2._basis.SetEulerZYX(0, 0, MathUtil.SIMD_HALF_PI);
m2._origin = new IndexedVector3(0.0f, -0.14f, 0.0f);
q = m.GetRotation();
q2 = m2.GetRotation();
MathUtil.PrintQuaternion(streamWriter, q);
streamWriter.WriteLine("");
MathUtil.PrintQuaternion(streamWriter, q2);
streamWriter.WriteLine("");
streamWriter.WriteLine("row and column");
m = IndexedMatrix.Identity;
m._basis.SetEulerZYX(0.3f, 0.7f, 0.5f);
m._origin = new IndexedVector3(11, 22, 33);
MathUtil.PrintVector3(streamWriter, "col0", m._basis.GetColumn(0));
MathUtil.PrintVector3(streamWriter, "col1", m._basis.GetColumn(1));
MathUtil.PrintVector3(streamWriter, "col2", m._basis.GetColumn(2));
//MathUtil.PrintVector3(streamWriter, "col3", MathUtil.MatrixColumn(m, 0));
MathUtil.PrintVector3(streamWriter, "row0", m._basis.GetRow(0));
MathUtil.PrintVector3(streamWriter, "row1", m._basis.GetRow(1));
MathUtil.PrintVector3(streamWriter, "row2", m._basis.GetRow(2));
//MathUtil.PrintVector3(streamWriter, "row3", MathUtil.MatrixRow(ref m, 3));
Matrix lookat1 = Matrix.CreateLookAt(new Vector3(5, 5, 5), new Vector3(10, 10, 10), new Vector3(0, 1, 0));
IndexedMatrix lookat2 = IndexedMatrix.CreateLookAt(new IndexedVector3(5, 5, 5), new IndexedVector3(10, 10, 10), new IndexedVector3(0, 1, 0));
Matrix compare = lookat2.ToMatrix();
MathUtil.PrintMatrix(streamWriter, "lookat1", lookat1);
MathUtil.PrintMatrix(streamWriter, "lookat2", lookat2);
MathUtil.PrintMatrix(streamWriter, "lookat compare", compare);
float aspect = (float)(800.0f / 600.0f);
float fov = MathHelper.ToRadians(40.0f);
float near = 1f;
float far = 500f;
Matrix pov = Matrix.CreatePerspectiveFieldOfView(fov, aspect, near, far);
IndexedMatrix pov2 = IndexedMatrix.CreatePerspectiveFieldOfView(fov, aspect, near, far);
Matrix pov3 = pov2.ToMatrix();
MathUtil.PrintMatrix(streamWriter, "pov1", pov);
MathUtil.PrintMatrix(streamWriter, "pov2", pov2);
MathUtil.PrintMatrix(streamWriter, "pov compare", pov3);
streamWriter.WriteLine("Complete.");
streamWriter.Flush();
filestream.Close();
}
