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