Quaternion Mult(Quaternion a, Quaternion b) { #if true Quaternion c; float aW = a.W, aX = a.X, aY = a.Y, aZ = a.Z; float bW = b.W, bX = b.X, bY = b.Y, bZ = b.Z; c.X = (aW * bX) + (aX * bW) + (aY * bZ) - (aZ * bY); c.Y = (aW * bY) + (aY * bW) + (aZ * bX) - (aX * bZ); c.Z = (aW * bZ) + (aZ * bW) + (aX * bY) - (aY * bX); c.W = (aW * bW) - (aX * bX) - (aY * bY) - (aZ * bZ); // Debug Speedup Checks - slight error difference between our // custom Quaternion calculation and the API one - possibly due // to internal xna speedups? Quaternion check = Quaternion.Multiply(a, b); float err = 0.01f; Debug_c.Assert((Math.Abs(check.X - c.X) < err) && (Math.Abs(check.Y - c.Y) < err) && (Math.Abs(check.Z - c.Z) < err) && (Math.Abs(check.W - c.W) < err)); c = check; return(c); #else return(Quaternion.Multiply(a, b)); #endif }
public Matrix inv_I; // inverse intertia tensor public void UpdateVel(float dt) { if (m > 0.0f) { v += new Vector3(0, -109.8f, 0) * dt; Debug_c.Valid(v); } }
void Valid(Matrix m) { MyMatrix temp = new MyMatrix(m); for (int i = 0; i < 4; i++) { for (int k = 0; k < 4; k++) { Debug_c.Assert(float.IsNaN(temp[i, k]) == false); } } }
public void UpdatePos(float dt) { if (m > 0.0f) { x += v * dt; Debug_c.Valid(x); Quaternion temp = MyMath.Mult(new Quaternion(omega.X, omega.Y, omega.Z, 0), q) * 0.5f; q = q + temp * dt; q.Normalize(); } }
void Update(float dt) { Contact_c.gTimeStamp++; //float linDrag = 0.99f; //float angDrag = 0.98f; /* * //*****Integrate****** * for (int i=0; i < m_rigidBodies.Count; i++) * { * Body_c b = m_rigidBodies[i].body; * * b.x += b.v * dt; * Debug_c.Valid(b.x); * * b.v += new Vector3(0, -400.8f, 0) * dt * b.m; * Debug_c.Valid(b.v); * * Quaternion temp = MyMath.Mult(new Quaternion(b.omega.X, b.omega.Y, b.omega.Z, 0), b.q) * 0.5f; * b.q = b.q + temp * dt; * b.q.Normalize(); * * b.v *= linDrag; * b.omega *= angDrag; * Debug_c.Valid(b.omega); * } */ #if USE_IMPULSES m_prevSteps[m_curStep].Clear(); for (int i = 0; i < m_rigidBodies.Count; i++) { m_prevSteps[m_curStep].Add(new Body_c(m_rigidBodies[i].body)); } m_curStep = (m_curStep + 1) % m_prevSteps.Count(); // Process all collisions //if (false) { for (int i = 0; i < m_rigidBodies.Count; i++) { Body_c b = m_rigidBodies[i].body; b.StoreState(); } for (int i = 0; i < m_rigidBodies.Count; i++) { Body_c b = m_rigidBodies[i].body; b.UpdateVel(dt); b.UpdatePos(dt); } CheckCollisions(); ArbiterContainer_c.SortInYDirection(); for (int i = 0; i < m_rigidBodies.Count; i++) { Body_c b = m_rigidBodies[i].body; b.RestoreState(); } for (int iteration = 0; iteration < 4; iteration++) { for (int k = 0; k < ArbiterContainer_c.arbiterArray.Count; k++) { for (int a = 0; a < ArbiterContainer_c.arbiterArray[k].arbiter.contacts.Count; a++) { Contact_c contact = ArbiterContainer_c.arbiterArray[k].arbiter.contacts[a]; bool zapIt = false; if (contact.Distance() > 0.01f && Contact_c.gTimeStamp != contact.timeStamp) { zapIt = true; } if (zapIt) { ArbiterContainer_c.arbiterArray[k].arbiter.contacts.RemoveAt(a); a--; continue; } contact.constraint.GenerateImpulse(0.9f); } } } } // Update Velocity for (int i = 0; i < m_rigidBodies.Count; i++) { Body_c b = m_rigidBodies[i].body; b.UpdateVel(dt); } // Process Contacts //if (false) { for (int i = 0; i < m_rigidBodies.Count; i++) { Body_c b = m_rigidBodies[i].body; b.StoreState(); } for (int i = 0; i < m_rigidBodies.Count; i++) { Body_c b = m_rigidBodies[i].body; b.UpdatePos(dt); } CheckCollisions(); ArbiterContainer_c.SortInYDirection(); for (int i = 0; i < m_rigidBodies.Count; i++) { Body_c b = m_rigidBodies[i].body; b.RestoreState(); } // For the shock propogation - should do a sort on order // but since this is our test code, I know I've added the rigidbodies // to the list in the order from bottom to top //if (false) for (int iteration = 0; iteration < 90; iteration++) { for (int i = 0; i < m_rigidBodies.Count; i++) { RigidBody_c b1 = m_rigidBodies[i]; b1.body.inv_m = 0.0f; //int j = i+1; //if (j>m_rigidBodies.Count-1) continue; for (int j = i + 1; j < m_rigidBodies.Count; j++) { RigidBody_c b2 = m_rigidBodies[j]; float cullingRadius = b1.maxRadius + b2.maxRadius; if ((b1.body.x - b2.body.x).LengthSquared() > cullingRadius * cullingRadius) { continue; } b1.body.StoreState(); b2.body.StoreState(); b1.body.UpdatePos(dt); b2.body.UpdatePos(dt); /* * for (int mm=0; mm < m_rigidBodies.Count; mm++) * { * Body_c b = m_rigidBodies[mm].body; * b.StoreState(); * } * for (int mm=0; mm < m_rigidBodies.Count; mm++) * { * Body_c b = m_rigidBodies[mm].body; * b.UpdatePos(dt); * } */ Vector3 n = Vector3.Zero; Vector3 p1 = Vector3.Zero; Vector3 p2 = Vector3.Zero; bool haveHit = Intersection_c.HasIntersection(b1.collideModel, b1.body.q, b1.body.x - MyMath.Rotate(b1.body.q, b1.body.com), b2.collideModel, b2.body.q, b2.body.x - MyMath.Rotate(b2.body.q, b2.body.com), out n, out p1, out p2); /* * for (int mm=0; mm < m_rigidBodies.Count; mm++) * { * Body_c b = m_rigidBodies[mm].body; * b.RestoreState(); * } */ b1.body.RestoreState(); b2.body.RestoreState(); if (haveHit) { Contact_c c = new Contact_c(b1, b2, p1, p2, n); c.constraint.GenerateImpulse(0.0f); } } } for (int i = 0; i < m_rigidBodies.Count; i++) { m_rigidBodies[i].body.inv_m = m_rigidBodies[i].body.inv_m_back; } } int numContactSteps = 2; if (false) { for (int step = 0; step < numContactSteps; step++) { for (int iteration = 0; iteration < 5; iteration++) { for (int k = 0; k < ArbiterContainer_c.arbiterArray.Count; k++) { for (int a = 0; a < ArbiterContainer_c.arbiterArray[k].arbiter.contacts.Count; a++) { /* * for (int i=0; i < m_rigidBodies.Count; i++) * { * Body_c b = m_rigidBodies[i].body; * b.StoreState(); * } * for (int i=0; i < m_rigidBodies.Count; i++) * { * Body_c b = m_rigidBodies[i].body; * b.UpdatePos(dt); * } * CheckCollisions(); * for (int i=0; i < m_rigidBodies.Count; i++) * { * Body_c b = m_rigidBodies[i].body; * b.RestoreState(); * } */ Contact_c contact = ArbiterContainer_c.arbiterArray[k].arbiter.contacts[a]; bool zapIt = false; if (contact.Distance() > 0.01f && Contact_c.gTimeStamp != contact.timeStamp) { zapIt = true; } if (zapIt) { ArbiterContainer_c.arbiterArray[k].arbiter.contacts.RemoveAt(a); a--; continue; } float ee = (numContactSteps - step - 1) * -1.0f / (float)numContactSteps; //if ( Math.Abs(contact.timeStamp - Contact_c.gTimeStamp) > 2 ) ee = -0.8f; //ee = 0.0f; contact.constraint.GenerateImpulse(ee); } } } } } // Shock propogation if (false) { for (int iteration = 0; iteration < 5; iteration++) { for (int k = 0; k < ArbiterContainer_c.arbiterArray.Count; k++) { for (int a = 0; a < ArbiterContainer_c.arbiterArray[k].arbiter.contacts.Count; a++) { Contact_c contact = ArbiterContainer_c.arbiterArray[k].arbiter.contacts[a]; if (ArbiterContainer_c.arbiterArray[k].arbiter.contacts.Count > 0) { ArbiterContainer_c.arbiterArray[k].arbiter.contacts[0].b1.body.inv_m = 0.0f; } bool zapIt = false; if (contact.Distance() > 0.1f && Contact_c.gTimeStamp != contact.timeStamp) { zapIt = true; } //if ( Math.Abs(contact.timeStamp - Contact_c.gTimeStamp) > 30 ) zapIt = true; if (zapIt) { ArbiterContainer_c.arbiterArray[k].arbiter.contacts.RemoveAt(a); a--; continue; } contact.constraint.GenerateImpulse(0.0f); } } for (int i = 0; i < m_rigidBodies.Count; i++) { m_rigidBodies[i].body.inv_m = m_rigidBodies[i].body.inv_m_back; } } } } // Update Positions for (int i = 0; i < m_rigidBodies.Count; i++) { Body_c b = m_rigidBodies[i].body; b.UpdatePos(dt); //b.v *= linDrag; b.omega *= 0.95f; //angDrag; Debug_c.Valid(b.omega); } #endif //USE_IMPULSES // Resolve momentum exchanges #if !USE_IMPULSES for (int i = 0; i < m_rigidBodies.Count; i++) { Body_c b = m_rigidBodies[i].body; b.UpdateVel(dt); b.UpdatePos(dt); } CheckCollisions(); ArbiterContainer_c.SortInYDirection(); for (int iteration = 0; iteration < 10; iteration++) { for (int k = 0; k < ArbiterContainer_c.arbiterArray.Count; k++) { for (int a = 0; a < ArbiterContainer_c.arbiterArray[k].arbiter.contacts.Count; a++) { Contact_c contact = ArbiterContainer_c.arbiterArray[k].arbiter.contacts[a]; bool zapIt = false; if (contact.Distance() > 0.1f && Contact_c.gTimeStamp != contact.timeStamp) { zapIt = true; } if (zapIt) { ArbiterContainer_c.arbiterArray[k].arbiter.contacts.RemoveAt(a); a--; continue; } if (iteration == 0) { contact.constraint.PrepareForIteration(); } else { contact.constraint.Iterate(); } } } } #endif }
void Valid(float v) { Debug_c.Assert(float.IsNaN(v) == false); }
void Valid(Vector3 v) { Debug_c.Assert(float.IsNaN(v.X) == false); Debug_c.Assert(float.IsNaN(v.Y) == false); Debug_c.Assert(float.IsNaN(v.Z) == false); }
void ComputeMassProperties(Body_c body, HullMaker model, float density) { MyVector3 diag = new MyVector3(Vector3.Zero); MyVector3 offDiag = new MyVector3(Vector3.Zero); Vector3 weightedCenterOfMass = Vector3.Zero; float volume = 0; float mass = 0; // Iterate through the faces for (int faceIndex = 0; faceIndex < model.surfaceTriList.Count; faceIndex++) { HullMaker.ClipTri face = model.surfaceTriList[faceIndex]; // Iterate through the tris in the face for (int triIndex = 0; triIndex < 3; triIndex++) { MyVector3 v0 = new MyVector3(face.n1); MyVector3 v1 = new MyVector3(face.n2); MyVector3 v2 = new MyVector3(face.n3); float det = Det(v0.V3(), v1.V3(), v2.V3()); // Volume float tetVolume = det / 6.0f; volume += tetVolume; // Mass float tetMass = tetVolume * density; mass += tetMass; // Center of Mass Vector3 tetCenterOfMass = ((v0 + v1 + v2) / 4.0f).V3(); // Note: includes origin (0, 0, 0) as fourth vertex weightedCenterOfMass += tetMass * tetCenterOfMass; // Inertia Tensor for (int i = 0; i < 3; i++) { int j = (i + 1) % 3; int k = (i + 2) % 3; diag[i] += det * (v0[i] * v1[i] + v1[i] * v2[i] + v2[i] * v0[i] + v0[i] * v0[i] + v1[i] * v1[i] + v2[i] * v2[i]) / 60.0f; offDiag[i] += det * ( v0[j] * v1[k] + v1[j] * v2[k] + v2[j] * v0[k] + v0[j] * v2[k] + v1[j] * v0[k] + v2[j] * v1[k] + 2 * v0[j] * v0[k] + 2 * v1[j] * v1[k] + 2 * v2[j] * v2[k]) / 120.0f; } } } Debug_c.Assert(mass > 0); if (mass == 0.0f) { mass = 5.0f; } Vector3 centerOfMass = weightedCenterOfMass / mass; diag *= density; offDiag *= density; MyMatrix I = new MyMatrix(Matrix.Identity); I[0, 0] = diag[1] + diag[2]; I[1, 1] = diag[2] + diag[0]; I[2, 2] = diag[0] + diag[1]; I[1, 2] = I[2, 1] = -offDiag[0]; I[0, 2] = I[2, 0] = -offDiag[1]; I[0, 1] = I[1, 0] = -offDiag[2]; /// // Move inertia tensor to be relative to center of mass (rather than origin) // Translate intertia to center of mass float x = centerOfMass.X; float y = centerOfMass.Y; float z = centerOfMass.Z; //Debug_c.Assert(Math.Abs(x)>0); //Debug_c.Assert(Math.Abs(y)>0); //Debug_c.Assert(Math.Abs(z)>0); //if (x==0.0f) x = 1.0f; //if (y==0.0f) y = 1.0f; //if (z==0.0f) z = 1.0f; I[0, 0] -= mass * (y * y + z * z); I[0, 1] -= mass * (-x * y); I[0, 2] -= mass * (-x * z); I[1, 1] -= mass * (x * x + z * z); I[1, 2] -= mass * (-y * z); I[2, 2] -= mass * (x * x + y * y); // Symmetry I[1, 0] = I[0, 1]; I[2, 0] = I[0, 2]; I[2, 1] = I[1, 2]; float check = 0.0f; for (int r = 0; r < 3; r++) { for (int c = 0; c < 3; c++) { check += I[r, c]; } } Debug_c.Assert(Math.Abs(check) > 0.0f); if (check == 0.0f) { I = new MyMatrix(Matrix.Identity); } body.com = centerOfMass; body.inv_m = 1.0f / mass; body.inv_m_back = body.inv_m; body.I = I.Get(); GeneralInverse4x4(out body.inv_I, ref I); //body.inv_I = Matrix.Invert( I.Get() ); Debug_c.Valid(body.com); Debug_c.Valid(body.inv_m); Debug_c.Valid(body.inv_I); Debug_c.Valid(body.I); Matrix test = Matrix.Identity; test = Matrix.Invert(I.Get()); }
public void GenerateImpulse(float e /*Bouncyness (Coefficient of restitution)*/) { if (m_body1.inv_m == 0.0f && m_body2.inv_m == 0.0f) { return; } Vector3 v1 = m_body1.v + Vector3.Cross(m_body1.omega, MyMath.Rotate(m_body1.q, m_r1)); Vector3 v2 = m_body2.v + Vector3.Cross(m_body2.omega, MyMath.Rotate(m_body2.q, m_r2)); // Compute the relative velocity between the bodies Vector3 relativeVelocity = v2 - v1; // If the objects are moving away from each other we dont need to apply an impulse float relativeMovement = Vector3.Dot(relativeVelocity, m_velocityConstraintDirection); if (relativeMovement < -0.01f) { return; } Vector3 r1 = MyMath.Rotate(m_body1.q, m_r1); Vector3 r2 = MyMath.Rotate(m_body2.q, m_r2); //Vector3 r1 = m_body1.x + MyMath.Rotate(m_body1.q, m_r1); //Vector3 r2 = m_body2.x + MyMath.Rotate(m_body2.q, m_r2); //Vector3 r1 = m_r1; //Vector3 r2 = m_r2; Matrix orientationMatrix1 = Matrix.CreateFromQuaternion(m_body1.q); Matrix inverseOrientationMatrix1 = Matrix.Transpose(orientationMatrix1); Matrix inverseWorldInertiaMatrix1 = orientationMatrix1 * m_body1.inv_I * inverseOrientationMatrix1; //Matrix inverseWorldInertiaMatrix1 = inverseOrientationMatrix1 * m_body1.inv_I * orientationMatrix1; //Matrix inverseWorldInertiaMatrix1 = m_body1.inv_I; Matrix orientationMatrix2 = Matrix.CreateFromQuaternion(m_body2.q); Matrix inverseOrientationMatrix2 = Matrix.Transpose(orientationMatrix2); Matrix inverseWorldInertiaMatrix2 = orientationMatrix2 * m_body2.inv_I * inverseOrientationMatrix2; //Matrix inverseWorldInertiaMatrix2 = inverseOrientationMatrix2 * m_body2.inv_I * orientationMatrix2; //Matrix inverseWorldInertiaMatrix2 = m_body2.inv_I; Vector3 a1 = Vector3.Transform(Vector3.Cross(r1, m_velocityConstraintDirection), inverseWorldInertiaMatrix1); Vector3 a2 = Vector3.Transform(Vector3.Cross(r2, m_velocityConstraintDirection), inverseWorldInertiaMatrix2); //Vector3 a1 = Vector3.Transform(Vector3.Cross(r1, m_velocityConstraintDirection), m_body1.inv_I); //Vector3 a2 = Vector3.Transform(Vector3.Cross(r2, m_velocityConstraintDirection), m_body2.inv_I); float kn = m_body1.inv_m + m_body2.inv_m + Vector3.Dot(m_velocityConstraintDirection, Vector3.Cross(a1, r1)) + Vector3.Dot(m_velocityConstraintDirection, Vector3.Cross(a2, r2)); float pn = (1 + e) / kn; Vector3 J = -m_velocityConstraintDirection * relativeMovement * (1 + e) / kn; //m_velocityConstraintDirection = Vector3.Normalize( m_velocityConstraintDirection ); //J = Vector3.Dot( J, m_velocityConstraintDirection ) * m_velocityConstraintDirection; //J = Vector3.Normalize( J ) * len; //float len2 = J.Length(); m_body1.v -= J * m_body1.inv_m; m_body2.v += J * m_body2.inv_m; Vector3 oldOmega1 = m_body1.omega; Vector3 oldOmega2 = m_body2.omega; m_body1.omega = oldOmega1 - Vector3.Transform(Vector3.Cross(r1, J), inverseWorldInertiaMatrix1); m_body2.omega = oldOmega2 + Vector3.Transform(Vector3.Cross(r2, J), inverseWorldInertiaMatrix2); //m_body1.omega = oldOmega1 - Vector3.Transform(Vector3.Cross(r1, J), m_body1.inv_I); //m_body2.omega = oldOmega2 + Vector3.Transform(Vector3.Cross(r2, J), m_body2.inv_I); Debug_c.Valid(m_body1.v); Debug_c.Valid(m_body2.v); Debug_c.Valid(m_body1.omega); Debug_c.Valid(m_body2.omega); // Tangent Friction //if (false) { // Work out our tangent vector, with is perpendicular // to our collision normal Vector3 tangent = relativeVelocity - Vector3.Dot(relativeVelocity, m_velocityConstraintDirection) * m_velocityConstraintDirection; if (tangent.LengthSquared() < 0.00001f) { return; } tangent.Normalize(); Vector3 at1 = Vector3.Transform(Vector3.Cross(r1, tangent), inverseWorldInertiaMatrix1); Vector3 at2 = Vector3.Transform(Vector3.Cross(r2, tangent), inverseWorldInertiaMatrix2); float ktn = m_body1.inv_m + m_body2.inv_m + Vector3.Dot(tangent, Vector3.Cross(at1, r1)) + Vector3.Dot(tangent, Vector3.Cross(at2, r2)); //float mu = 0.5f; float pt = (1 + e) / ktn; pt = MathHelper.Clamp(pt, -0.3f * pn, 0.3f * pn); Vector3 Jt = -tangent * pt; m_body1.v -= Jt * m_body1.inv_m; m_body2.v += Jt * m_body2.inv_m; Vector3 oldOmegat1 = m_body1.omega; Vector3 oldOmegat2 = m_body2.omega; m_body1.omega = oldOmegat1 - Vector3.Transform(Vector3.Cross(r1, Jt), inverseWorldInertiaMatrix1); m_body2.omega = oldOmegat2 + Vector3.Transform(Vector3.Cross(r2, Jt), inverseWorldInertiaMatrix2); Debug_c.Valid(m_body1.v); Debug_c.Valid(m_body2.v); Debug_c.Valid(m_body1.omega); Debug_c.Valid(m_body2.omega); } }
public override void Iterate() { if (m_body1.inv_m == 0.0f && m_body2.inv_m == 0.0f) { return; } // Compute the relative velocity between the bodies Vector3 relativeVelocity = (m_body2.v + Vector3.Cross(m_body2.omega, MyMath.Rotate(m_body2.q, m_r2))) - (m_body1.v + Vector3.Cross(m_body1.omega, MyMath.Rotate(m_body1.q, m_r1))); // Project the relative velocity onto the constraint direction float velocityError = Vector3.Dot(relativeVelocity, m_velocityConstraintDirection); // Compute the velocity delta needed to satisfy the constraint float deltaVelocity = -velocityError - m_positionError; // Compute the momentum to be exchanged to correct velocities float momentumPacket = deltaVelocity * m_effectiveMass; // Clamp the momentum packet to reflect the fact that the contact can only push the objects apart momentumPacket = Math.Min(momentumPacket, -m_cachedMomentum); Vector3 momentumPacketWithDir = momentumPacket * m_velocityConstraintDirection; // Exchange the correctional momentum between the bodies m_body1.v -= momentumPacketWithDir * m_body1.inv_m; m_body2.v += momentumPacketWithDir * m_body2.inv_m; m_body1.omega -= momentumPacket * m_invMoment1; m_body2.omega += momentumPacket * m_invMoment2; Debug_c.Valid(m_body1.omega); Debug_c.Valid(m_body2.omega); // Test code //Vector3 newRelativeVelocity = (m_body2.v + Vector3.Cross(m_body2.omega, MyMath.Rotate(m_body2.q, m_r2))) - // (m_body1.v + Vector3.Cross(m_body1.omega, MyMath.Rotate(m_body1.q, m_r1))); //float newVelocityError = Vector3.Dot(newRelativeVelocity, m_velocityConstraintDirection); // Accumulate the momentum for next frame m_cachedMomentum += momentumPacket; /// // FRICTION //if (gFriction) { relativeVelocity = (m_body2.v + Vector3.Cross(m_body2.omega, MyMath.Rotate(m_body2.q, m_r2))) - (m_body1.v + Vector3.Cross(m_body1.omega, MyMath.Rotate(m_body1.q, m_r1))); Vector3 tangentVelocityDirection = relativeVelocity - Vector3.Dot(relativeVelocity, m_velocityConstraintDirection) * m_velocityConstraintDirection; if (tangentVelocityDirection.LengthSquared() < 0.0001f) { return; } Debug_c.Valid(tangentVelocityDirection); tangentVelocityDirection.Normalize(); float tangentVelocityError = Vector3.Dot(relativeVelocity, tangentVelocityDirection); Vector3 r1 = MyMath.Rotate(m_body1.q, m_r1); Vector3 r2 = MyMath.Rotate(m_body2.q, m_r2); Vector3 invMoment1 = Vector3.Transform(Vector3.Cross(r1, tangentVelocityDirection), m_body1.inv_I); Vector3 invMoment2 = Vector3.Transform(Vector3.Cross(r2, tangentVelocityDirection), m_body2.inv_I); Debug_c.Valid(invMoment1); Debug_c.Valid(invMoment2); // Compute effective mass of the constraint system -- this is a measure of how easy it // is to accelerate the contact points apart along the constraint direction -- it's analogous // to effective resistance in an electric circuit [i.e., 1 / (1/R1 + 1/R2)] float effectiveMass = 1.0f / ( m_body1.inv_m + m_body2.inv_m + Vector3.Dot(tangentVelocityDirection, ( Vector3.Cross(invMoment1, r1) + Vector3.Cross(invMoment2, r2) )) ); float tangentDeltaVelocity = -tangentVelocityError; float tangentMomentumPacket = tangentDeltaVelocity * effectiveMass; tangentMomentumPacket = Math.Max(tangentMomentumPacket, m_cachedMomentum * 0.5f); Vector3 tangentMomentumPacketWithDir = tangentMomentumPacket * tangentVelocityDirection; // Exchange the correctional momentum between the bodies m_body1.v -= tangentMomentumPacketWithDir * m_body1.inv_m; m_body2.v += tangentMomentumPacketWithDir * m_body2.inv_m; m_body1.omega -= tangentMomentumPacket * invMoment1; m_body2.omega += tangentMomentumPacket * invMoment2; Debug_c.Valid(m_body1.v); Debug_c.Valid(m_body2.v); Debug_c.Valid(m_body1.omega); Debug_c.Valid(m_body2.omega); m_cachedTangentMomentum = tangentMomentumPacketWithDir; } }
public override void PrepareForIteration() { if (m_body1.inv_m == 0.0f && m_body2.inv_m == 0.0f) { return; } Vector3 r1 = MyMath.Rotate(m_body1.q, m_r1); Vector3 r2 = MyMath.Rotate(m_body2.q, m_r2); Vector3 x1 = m_body1.x + r1; Vector3 x2 = m_body2.x + r2; // Compute the positional constraint error (scaled by the Baumgarte coefficient 'm_beta') // m_beta = 0.98f / gTimeStep; //m_beta = 0.5f / Contact_c.gTimeStep; m_beta = 0.5f / Contact_c.gTimeStep; m_positionError = m_beta * Vector3.Dot((x2 - x1), m_velocityConstraintDirection); // Add a boundary layer to the position error -- this will ensure the objects remain in contact //m_positionError -= 1.0f; m_positionError -= 1.0f; // The velocity constraint direction is aligned with the contact normal // (This represents how much angular velocity we get for every unit of momentum transferred along the constraint direction) m_invMoment1 = Vector3.Transform(Vector3.Cross(r1, m_velocityConstraintDirection), m_body1.inv_I); m_invMoment2 = Vector3.Transform(Vector3.Cross(r2, m_velocityConstraintDirection), m_body2.inv_I); Debug_c.Valid(m_invMoment1); Debug_c.Valid(m_invMoment2); // Compute effective mass of the constraint system -- this is a measure of how easy it // is to accelerate the contact points apart along the constraint direction -- it's analogous // to effective resistance in an electric circuit [i.e., 1 / (1/R1 + 1/R2)] m_effectiveMass = 1.0f / ( m_body1.inv_m + m_body2.inv_m + Vector3.Dot(m_velocityConstraintDirection, ( Vector3.Cross(m_invMoment1, r1) + Vector3.Cross(m_invMoment2, r2) )) ); // Convert last frame's momentum to momentum for the new time step float timeRatio = Contact_c.gTimeRatio; m_cachedMomentum *= Contact_c.gTimeRatio; m_cachedTangentMomentum *= Contact_c.gTimeRatio; // Apply last frame's momentum m_body1.v -= m_cachedMomentum * m_velocityConstraintDirection * m_body1.inv_m; m_body2.v += m_cachedMomentum * m_velocityConstraintDirection * m_body2.inv_m; m_body1.omega -= m_cachedMomentum * m_invMoment1; m_body2.omega += m_cachedMomentum * m_invMoment2; Debug_c.Valid(m_body1.v); Debug_c.Valid(m_body2.v); Debug_c.Valid(m_body1.omega); Debug_c.Valid(m_body2.omega); m_cachedTangentMomentum = Vector3.Zero; }
bool HasIntersection(Shape p1, Quaternion q1, Vector3 t1, Shape p2, Quaternion q2, Vector3 t2, out Vector3 returnNormal, out Vector3 point1, out Vector3 point2) { returnNormal = Vector3.Zero; point1 = Vector3.Zero; point2 = Vector3.Zero; const float kCollideEpsilon = 1e-3f; // v0 = center of Minkowski sum Vector3 v01 = MyMath.Rotate(q1, p1.GetCenter()) + t1; Vector3 v02 = MyMath.Rotate(q2, p2.GetCenter()) + t2; Vector3 v0 = v02 - v01; Debug_c.Valid(v02); Debug_c.Valid(v01); Debug_c.Valid(v0); // Avoid case where centers overlap -- any direction is fine in this case if (v0.LengthSquared() < 0.0001f) { v0 = new Vector3(0.00001f, 0, 0); } // v1 = support in direction of origin Vector3 n = -v0; Vector3 v11 = Collision.TransformSupportVert(p1, q1, t1, -n); Vector3 v12 = Collision.TransformSupportVert(p2, q2, t2, n); Debug_c.Valid(v11); Debug_c.Valid(v12); Vector3 v1 = v12 - v11; if (Vector3.Dot(v1, n) <= 0.0f) { return(false); } Debug_c.Valid(v0); Debug_c.Valid(v1); // v2 - support perpendicular to v1,v0 n = Vector3.Cross(v1, v0); Debug_c.Valid(n); if (n.LengthSquared() < 0.0001f) { n = v1 - v0; n.Normalize(); returnNormal = n; point1 = v11; point2 = v12; return(true); } Vector3 v21 = Collision.TransformSupportVert(p1, q1, t1, -n); Vector3 v22 = Collision.TransformSupportVert(p2, q2, t2, n); Vector3 v2 = v22 - v21; if (Vector3.Dot(v2, n) <= 0.0f) { return(false); } Debug_c.Valid(v21); Debug_c.Valid(v22); // Determine whether origin is on + or - side of plane (v1,v0,v2) n = Vector3.Cross(v1 - v0, v2 - v0); Debug_c.Valid(n); float dist = Vector3.Dot(n, v0); Debug_c.Assert(n.LengthSquared() > 0.0001f); // If the origin is on the - side of the plane, reverse the direction of the plane if (dist > 0) { Swap(ref v1, ref v2); Swap(ref v11, ref v21); Swap(ref v12, ref v22); n = -n; } bool hit = false; /// // Phase One: Identify a portal // while (true) { // Obtain the support point in a direction perpendicular to the existing plane // Note: This point is guaranteed to lie off the plane Vector3 v31 = Collision.TransformSupportVert(p1, q1, t1, -n); Vector3 v32 = Collision.TransformSupportVert(p2, q2, t2, n); Vector3 v3 = v32 - v31; if (Vector3.Dot(v3, n) <= 0) { return(false); } // If origin is outside (v1,v0,v3), then eliminate v2 and loop if (Vector3.Dot(Vector3.Cross(v1, v3), v0) < 0) { v2 = v3; v21 = v31; v22 = v32; n = Vector3.Cross(v1 - v0, v3 - v0); continue; } // If origin is outside (v3,v0,v2), then eliminate v1 and loop if (Vector3.Dot(Vector3.Cross(v3, v2), v0) < 0) { v1 = v3; v11 = v31; v12 = v32; n = Vector3.Cross(v3 - v0, v2 - v0); continue; } /// // Phase Two: Refine the portal // We are now inside of a wedge... while (true) { // Compute normal of the wedge face n = Vector3.Cross(v2 - v1, v3 - v1); // Can this happen??? Can it be handled more cleanly? if (n.LengthSquared() < 0.00001f) { Debug_c.Assert(false); return(false); } n.Normalize(); // Compute distance from origin to wedge face float d = Vector3.Dot(n, v1); // If the origin is inside the wedge, we have a hit if (d >= 0 && !hit) { returnNormal = n; // Compute the barycentric coordinates of the origin float b0 = Vector3.Dot(Vector3.Cross(v1, v2), v3); float b1 = Vector3.Dot(Vector3.Cross(v3, v2), v0); float b2 = Vector3.Dot(Vector3.Cross(v0, v1), v3); float b3 = Vector3.Dot(Vector3.Cross(v2, v1), v0); float sum = b0 + b1 + b2 + b3; if (sum <= 0) { b0 = 0; b1 = Vector3.Dot(Vector3.Cross(v2, v3), n); b2 = Vector3.Dot(Vector3.Cross(v3, v1), n); b3 = Vector3.Dot(Vector3.Cross(v1, v2), n); sum = b1 + b2 + b3; } float inv = 1.0f / sum; point1 = (b0 * v01 + b1 * v11 + b2 * v21 + b3 * v31) * inv; point2 = (b0 * v02 + b1 * v12 + b2 * v22 + b3 * v32) * inv; // HIT!!! hit = true; } // Find the support point in the direction of the wedge face Vector3 v41 = Collision.TransformSupportVert(p1, q1, t1, -n); Vector3 v42 = Collision.TransformSupportVert(p2, q2, t2, n); Vector3 v4 = v42 - v41; float delta = Vector3.Dot((v4 - v3), n); float separation = -Vector3.Dot(v4, n); // If the boundary is thin enough or the origin is outside the support plane for the // newly discovered vertex, then we can terminate if (delta <= kCollideEpsilon || separation >= 0) { returnNormal = n; return(hit); } // Compute the tetrahedron dividing face (v4,v0,v1) float d1 = Vector3.Dot(Vector3.Cross(v4, v1), v0); // Compute the tetrahedron dividing face (v4,v0,v2) float d2 = Vector3.Dot(Vector3.Cross(v4, v2), v0); // Compute the tetrahedron dividing face (v4,v0,v3) float d3 = Vector3.Dot(Vector3.Cross(v4, v3), v0); if (d1 < 0) { if (d2 < 0) { // Inside d1 & inside d2 ==> eliminate v1 v1 = v4; v11 = v41; v12 = v42; } else { // Inside d1 & outside d2 ==> eliminate v3 v3 = v4; v31 = v41; v32 = v42; } } else { if (d3 < 0) { // Outside d1 & inside d3 ==> eliminate v2 v2 = v4; v21 = v41; v22 = v42; } else { // Outside d1 & outside d3 ==> eliminate v1 v1 = v4; v11 = v41; v12 = v42; } } } } }