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