public RigidBody_c(Body_c b, Shape cg, HullMaker hull, float radius, Microsoft.Xna.Framework.Graphics.Color c) { maxRadius = radius; body = b; collideModel = cg; renderModel = hull; id = idcounter; colour = c; idcounter++; }
public ContactConstraint_c(ref Body_c b1, ref Body_c b2, ref Vector3 r1, ref Vector3 r2, ref Vector3 normal) { m_body1 = b1; m_body2 = b2; m_r1 = r1; m_r2 = r2; m_velocityConstraintDirection = normal; m_beta = 1.0f; m_cachedMomentum = 0; m_cachedTangentMomentum = Vector3.Zero; }
public Body_c(Body_c copy) { com = copy.com; x = copy.x; v = copy.v; m = copy.m; inv_m = copy.inv_m; inv_m_back = copy.inv_m_back; q = copy.q; omega = copy.omega; I = copy.I; inv_I = copy.inv_I; }
public void StoreState() { store = new Body_c(); store.com = com; store.x = x; store.v = v; store.m = m; store.inv_m = inv_m; store.inv_m_back = inv_m_back; store.q = q; store.omega = omega; store.I = I; store.inv_I = inv_I; }
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 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()); }
RigidBody_c CreateRigidBody(Shape collideModel, Quaternion q, Vector3 x, float inv_m) { //RenderPolytope renderModel = rb.renderModel; HullMaker renderModel = new HullMaker(collideModel, -1); //if (!renderModel) //{ // renderModel = CreateRenderModel(collideModel); //} Body_c body = new Body_c(); body.q = q; body.x = x; body.inv_I = Matrix.Identity; body.inv_I *= inv_m / 25.0f; //body.inv_I.M33 = 1; body.inv_I.M44 = 1; body.inv_m = inv_m; if (inv_m == 0.0f) { body.m = 0.0f; } if (inv_m > 0.0f) { HelperRigidBody_c.ComputeMassProperties(body, renderModel, 1.0f); } float radiusNegX = Math.Abs(collideModel.GetSupportPoint(new Vector3(-1, 0, 0)).X); float radiusPosX = Math.Abs(collideModel.GetSupportPoint(new Vector3(1, 0, 0)).X); float radiusNegY = Math.Abs(collideModel.GetSupportPoint(new Vector3(0, -1, 0)).Y); float radiusPosY = Math.Abs(collideModel.GetSupportPoint(new Vector3(0, 1, 0)).Y); float radiusNegZ = Math.Abs(collideModel.GetSupportPoint(new Vector3(0, 0, -1)).Z); float radiusPosZ = Math.Abs(collideModel.GetSupportPoint(new Vector3(0, 0, 1)).Z); Vector3 maxRadiusVector = new Vector3 ( Math.Max(radiusNegX, radiusPosX), Math.Max(radiusNegY, radiusPosY), Math.Max(radiusNegZ, radiusPosZ) ); float maxRadius = maxRadiusVector.Length(); Microsoft.Xna.Framework.Graphics.Color[] randColour = { Microsoft.Xna.Framework.Graphics.Color.Red, Microsoft.Xna.Framework.Graphics.Color.Green, Microsoft.Xna.Framework.Graphics.Color.PaleGoldenrod, Microsoft.Xna.Framework.Graphics.Color.OliveDrab, Microsoft.Xna.Framework.Graphics.Color.OrangeRed, Microsoft.Xna.Framework.Graphics.Color.Wheat, Microsoft.Xna.Framework.Graphics.Color.YellowGreen }; randColourIndx++; if (randColourIndx > (randColour.Count() - 1)) { randColourIndx = 0; } body.inv_m_back = body.inv_m; return(new RigidBody_c(body, collideModel, renderModel, maxRadius, randColour[randColourIndx])); }