void ComputeBVector(ref NVector bVector) { for (int i = 0; i < restingContact.Count; i++) { Collision c = restingContact[i]; RigidBodyClass obj1 = c.obj1; RigidBodyClass obj2 = c.obj2; Vector3 normal = c.NormalDirection; Vector3 ADistanceFromCenter = c.CollisionLocation - obj1.GetPosition(); Vector3 BDistanceFromCenter = c.CollisionLocation - obj2.GetPosition(); Vector3 AExtForce = obj1.GetForce(); Vector3 BExtForce = obj2.GetForce(); Vector3 AExtTorque = obj1.GetTorque(); Vector3 BExtTorque = obj2.GetTorque(); Vector3 AExtPart = AExtForce / (float)obj1.GetMass() + (Vector3.Cross((obj1.GetIInverse().MultiplyVector(AExtTorque)), ADistanceFromCenter)); Vector3 BExtPart = BExtForce / (float)obj2.GetMass() + (Vector3.Cross((obj2.GetIInverse().MultiplyVector(BExtTorque)), BDistanceFromCenter)); Vector3 AVelPart = (Vector3.Cross(obj1.GetAngularVelocity(), Vector3.Cross(obj1.GetAngularVelocity(), ADistanceFromCenter))); AVelPart += Vector3.Cross(obj1.GetIInverse().MultiplyVector(Vector3.Cross(obj1.GetAngularMomentum(), obj1.GetAngularVelocity())), ADistanceFromCenter); Vector3 BVelPart = (Vector3.Cross(obj2.GetAngularVelocity(), Vector3.Cross(obj2.GetAngularVelocity(), BDistanceFromCenter))); BVelPart += Vector3.Cross(obj2.GetIInverse().MultiplyVector(Vector3.Cross(obj2.GetAngularMomentum(), obj2.GetAngularVelocity())), BDistanceFromCenter); Vector3 PositionPartCombination = ((AExtPart + AVelPart) - (BExtPart - BVelPart)); double k1 = Vector3.Dot(normal, PositionPartCombination); Vector3 ndot = ComputeNDot(c); double k2 = 2 * Vector3.Dot(ndot, (VelocityOfPointOnBody(obj1, c.CollisionLocation)) - (VelocityOfPointOnBody(obj2, c.CollisionLocation))); bVector.SetPosition(i, (k1 + k2)); } }
void RestingContact() { NMatrix AMatrix = new NMatrix(restingContact.Count, restingContact.Count); NVector fVector = new NVector(restingContact.Count); NVector bVector = new NVector(restingContact.Count); ComputeAMatrix(ref AMatrix); ComputeBVector(ref bVector); //Now I need a QP Solver. Not completely sure which one to use, and how. Uncomment when you do this //QP_Solve(AMatrix,bVector,ref fVector); for (int i = 0; i < restingContact.Count; i++) { double force = fVector.GetPosition(i); Vector3 normalDirection = restingContact[i].CollisionLocation; RigidBodyClass classA = restingContact[i].obj1; RigidBodyClass classB = restingContact[i].obj2; //Applying force in the positive direction classA.AddForce((float)force * normalDirection); Vector3 TorqueDistance = restingContact[i].CollisionLocation - classA.GetPosition(); classA.AddTorque(Vector3.Scale(TorqueDistance, (float)force * normalDirection)); force = -1 * force; //Applying force in the negative direction classB.AddForce((float)force * normalDirection); TorqueDistance = restingContact[i].CollisionLocation - classB.GetPosition(); classB.AddTorque(Vector3.Scale(TorqueDistance, (float)force * normalDirection)); } }
float ComputeAij(int i, int j) { //Check if the bodies influence ea or not Collision ci = restingContact[i]; Collision cj = restingContact[j]; if ((ci.obj1 != cj.obj1) && (ci.obj1 != cj.obj2) && (ci.obj2 != cj.obj1) && (ci.obj2 != cj.obj2)) { return(0); } else { RigidBodyClass a = ci.obj1; RigidBodyClass b = ci.obj2; Vector3 ni = ci.NormalDirection; Vector3 nj = cj.NormalDirection; Vector3 pi = ci.CollisionLocation; Vector3 pj = cj.CollisionLocation; Vector3 ra = pi - a.GetPosition(); Vector3 rb = pi - a.GetPosition(); Vector3 forceOnA = Vector3.zero; Vector3 torqueOnA = Vector3.zero; Vector3 forceOnB = Vector3.zero; Vector3 torqueOnB = Vector3.zero; if (ci.obj1 == cj.obj1) { forceOnA = nj; torqueOnA = Vector3.Cross((pj - a.GetPosition()), nj); } else if (ci.obj1 == cj.obj2) { forceOnA = -nj; torqueOnA = Vector3.Cross((pj - a.GetPosition()), nj); } if (ci.obj2 == cj.obj1) { forceOnB = nj; torqueOnB = Vector3.Cross((pj - b.GetPosition()), nj); } else if (ci.obj2 == cj.obj2) { forceOnB = -nj; torqueOnB = Vector3.Cross((pj - b.GetPosition()), nj); } Vector3 a_linear = forceOnA / (float)a.GetMass(); Vector3 a_angular = Vector3.Cross(a.GetIInverse().MultiplyVector(torqueOnA), ra); Vector3 b_linear = forceOnB / (float)b.GetMass(); Vector3 b_angular = Vector3.Cross(b.GetIInverse().MultiplyVector(torqueOnB), rb); float result = Vector3.Dot(ni, ((a_linear + a_angular) - (b_linear + b_angular))); return(result); } }
public Collision(RigidBodyClass a1, RigidBodyClass b1, Vector3 p1, Vector3 n1, Vector3 ea1, Vector3 eb1, bool vf1) { obj1 = a1; obj2 = b1; CollisionLocation = p1; NormalDirection = n1; Edge1Direction = ea1; Edge2Direction = eb1; IsVertexFaceContact = vf1; }
void RandomForce() { GameObject[] listofBlocks = GameObject.FindGameObjectsWithTag("Block"); for (int i = 0; i < 10; i++) { int randBlock = Random.Range(0, listofBlocks.Length); print(randBlock); RigidBodyClass rb = listofBlocks[i].GetComponent <BlockRigidBody>().rigidBody; Vector3 randomForce = new Vector3(Random.value, Random.value, 0); rb.AddForce(randomForce); } }
bool AddCollision(int index1, int index2) { //Fetching GameObjects and their rigidbodies GameObject r1 = blockList[index1]; GameObject r2 = blockList[index2]; RigidBodyClass rb1 = rigidBodies[index1]; RigidBodyClass rb2 = rigidBodies[index2]; //Getting normals and vertices of each object Vector3[] verti_a = new Vector3[8]; Vector3[] verti_b = new Vector3[8]; Vector3[] norma_a = new Vector3[6]; Vector3[] norma_b = new Vector3[6]; GetNormalAndVertices(r1, r2, ref norma_a, ref norma_b, ref verti_a, ref verti_b); float min_p = 10000; Vector3 min = new Vector3(0, 0, 0); bool facev = false; Vector3 ea = new Vector3(0, 0, 0); Vector3 eb = new Vector3(0, 0, 0); //normals of a -> 3 int c = 0; for (int i = 0; i < norma_a.Length; i++) { if (norma_a[i][0] >= 0 && norma_a[i][1] >= 0 && norma_a[i][2] >= 0) { //Debug.Log(norma_a[i]); c++; float[] ans = IntersectsWhenProjected(verti_a, verti_b, norma_a[i]); if (ans[0] != 1) { return(false); } else { if (ans[2] - ans[1] < min_p) { min_p = ans[2] - ans[1]; min = norma_a[i]; facev = true; //print("FaceVertex"); } } } } //normals of b -> 3 for (int j = 0; j < norma_b.Length; j++) { if (norma_b[j][0] >= 0 && norma_b[j][1] >= 0 && norma_b[j][2] >= 0) { //Debug.Log(norma_b[j]); c++; float[] ans = IntersectsWhenProjected(verti_a, verti_b, norma_b[j]); if (ans[0] != 1) { return(false); } else { if (ans[2] - ans[1] < min_p) { min_p = ans[2] - ans[1]; min = norma_b[j]; facev = true; //print("FaceVertex"); } } } } //cross products of normals -> 9 for (int i = 0; i < norma_a.Length; i++) { for (int j = 0; j < norma_b.Length; j++) { if (norma_a[i][0] >= 0 && norma_b[i][1] >= 0 && norma_b[i][2] >= 0) { if (norma_b[j][0] >= 0 && norma_b[j][1] >= 0 && norma_b[j][2] >= 0) { c++; float[] ans = IntersectsWhenProjected(verti_a, verti_b, Vector3.Cross(norma_a[i], norma_b[j])); if (ans[0] != 1) { return(false); } else { if (ans[2] - ans[1] < min_p) { min_p = ans[2] - ans[1]; min = norma_b[j]; facev = false; ea = norma_a[i]; eb = norma_b[j]; //print("Edge/Edge"); } } } } } } Debug.Log("collision"); //Vector3 temp = new Vector3((rb1.GetPosition().x - rb2.GetPosition().x) * min.x, (rb1.GetPosition().y - rb2.GetPosition().y) * min.y, (rb1.GetPosition().z - rb2.GetPosition().z) * min.z); Vector3 p1 = (rb2.GetPosition() + ((rb1.GetPosition() - rb2.GetPosition()) / 2)); Vector3 temp = rb1.GetPosition() - rb2.GetPosition(); //if (Vector3.Dot(temp, min) < 0) //{ // min = -1 * min; //} Collision col = new Collision(rb1, rb2, p1, min, ea, eb, facev); collisions.Add(col); //Collision Detected between i and j //rb1.CollisionTest(); //rb2.CollisionTest(); return(true); }
Vector3 VelocityOfPointOnBody(RigidBodyClass rb, Vector3 point) { return(rb.GetVelocity() + Vector3.Cross(rb.GetAngularVelocity(), (point - rb.GetPosition()))); }