//TODO make multi-path colliders work......
/// <summary>
/// Generates the fine details between two known-colliding Collider2D objects.
/// One collider will be a CircleCollider2D and the other will be a PolygonCollider2D that belongs to a JelloBody.
/// </summary>
/// <param name="collA">The CircleCollider2D involved in the collision.</param>
/// <param name="info">The SupplementartyColliderInfo representing the PolygonCollider2D that belongs to a JelloBody involved in the collision.</param>
/// <param name="contacts">The JelloContact list to add any generated JelloContact to.</param>
public void BodyCollide(CircleCollider2D collA, SupplementaryColliderInfo info, ref List<JelloContact> contacts)
{
JelloContact contact;
Vector2 pt;
Vector2 hitPt;
Vector2 norm;
float scalarAB;
float dist;
float radius;
//contact = new JelloContact();
radius = collA.radius * Mathf.Max(collA.transform.localScale.x, collA.transform.localScale.y);
pt = (Vector2)collA.transform.TransformPoint(collA.center);
//for each pointmass in body B
for (int j = 0; j < info.ColliderVertices.Length - 2; j++)
{
//get collision info
dist = JelloVectorTools.getClosestPointOnSegmentSquared(pt, info.ColliderVertices[j + 1], info.ColliderVertices[j + 2], out hitPt, out norm, out scalarAB); //TODO make this work with fully penetrated circles.
//fill out collisioninfo
if(dist < radius * radius)
{
contact = new JelloContact();
contact.bodyA = null;
contact.colliderA = collA;
contact.rigidbodyA = collA.rigidbody2D;
contact.transformA = collA.transform;
contact.bodyB = info.body;
contact.colliderB = info.collider;
contact.rigidbodyB = info.collider.rigidbody2D;
contact.transformB = info.collider.transform;
contact.bodyBpmA = j;
contact.bodyBpmB = j + 1 > info.ColliderVertices.Length - 3 ? 0 : j + 1;
contact.scalarAB = scalarAB;
contact.hitPoint = hitPt;
contact.normal = (hitPt - pt).normalized * (JelloShapeTools.Contains(info.ColliderVertices, pt) ? 1f : -1f);
contact.mtv = contact.normal;
contact.penetration = radius - Mathf.Sqrt(dist);
contact.R = contact.hitPoint - (Vector2)collA.transform.position;
contact.R2 = contact.hitPoint - (Vector2)info.collider.transform.position;
contacts.Add(contact);
}
}
}
//public void BodyCollide(PolygonCollider2D collA, EdgeCollider2D collB, ref List<JelloContact> contacts){}
//public void BodyCollide(EdgeCollider2D collA, PolygonCollider2D collB, ref List<JelloContact> contacts){}
public void BodyCollide(SupplementaryColliderInfo infoA, SupplementaryColliderInfo infoB, ref List<JelloContact> contacts)
{
List<JelloContact> closestLineContacts = new List<JelloContact>();
//find each point of the colliders that are penetrating each other.
FindPenetratingPoints(infoA, infoB, ref contacts);
//find each contact that satisfies the parametric contact method
FindParametricContactDetails(infoA, infoB, ref contacts);
//decide the detection method for each contact, parametric, closest line, or hybrid
FinalizeContactTypes(infoA, infoB, ref contacts, ref closestLineContacts);
//get details (using the closest line method) for each contact that does not satisfy the parametric or hybrid detection methods.
FindClosestLineContactDetails(infoA, infoB, ref contacts, ref closestLineContacts);
}
private void FindPenetratingPoints(SupplementaryColliderInfo infoA, SupplementaryColliderInfo infoB, ref List<JelloContact> contacts)
{
//TODO i have an idea of how to optimise this a bit maybe.
//have separate lists for each body and iterate better over each of them...
//loop through both bodies at the same time, identifying penetrating points
for (int i = 0; i < Mathf.Max ( infoA.ColliderVertices.Length - 2, infoB.ColliderVertices.Length - 2); i++)
{
//check for potential collisions in both bodies
if(i < infoA.ColliderVertices.Length - 2)
{
//only consider this point if it is insdie the other body's AABB and inside the other body.
if(infoB.AABB.contains(infoA.ColliderVertices[i + 1]) && JelloShapeTools.Contains(infoB.ColliderVertices, infoA.ColliderVertices[i + 1]))
{
contacts.Add (new JelloContact());
contacts[contacts.Count - 1].penetration = Mathf.Infinity;
contacts[contacts.Count - 1].toi = Mathf.Infinity;
contacts[contacts.Count - 1].bodyApm = i;
contacts[contacts.Count - 1].bodyA = infoA.body;
contacts[contacts.Count - 1].colliderA = infoA.collider;
contacts[contacts.Count - 1].rigidbodyA = infoA.rigidbody;
contacts[contacts.Count - 1].transformA = infoA.transform;
contacts[contacts.Count - 1].bodyB = infoB.body;
contacts[contacts.Count - 1].colliderB = infoB.collider;
contacts[contacts.Count - 1].rigidbodyB = infoB.rigidbody;
contacts[contacts.Count - 1].transformB = infoB.transform;
}
}
if(i < infoB.ColliderVertices.Length - 2)
{
//only consider this point if it is insdie the other body's AABB and inside the other body.
if(infoA.AABB.contains(infoB.ColliderVertices[i + 1]) && JelloShapeTools.Contains(infoA.ColliderVertices, infoB.ColliderVertices[i + 1]))
{
contacts.Add (new JelloContact());
contacts[contacts.Count - 1].penetration = Mathf.Infinity;
contacts[contacts.Count - 1].toi = Mathf.Infinity;
contacts[contacts.Count - 1].bodyApm = i;
contacts[contacts.Count - 1].bodyA = infoB.body;
contacts[contacts.Count - 1].colliderA = infoB.collider;
contacts[contacts.Count - 1].rigidbodyA = infoB.rigidbody;
contacts[contacts.Count - 1].transformA = infoB.transform;
contacts[contacts.Count - 1].bodyB = infoA.body;
contacts[contacts.Count - 1].colliderB = infoA.collider;
contacts[contacts.Count - 1].rigidbodyB = infoA.rigidbody;
contacts[contacts.Count - 1].transformB = infoA.transform;
}
}
}
}
private void FindParametricContactDetails(SupplementaryColliderInfo infoA, SupplementaryColliderInfo infoB, ref List<JelloContact> contacts)
{
Vector2 A, B, prevA, prevB, vA, vB;
//after looping through each point and checking for potential collisions, loop through one more time and get fine detail collision.
for (int i = 0; i < Mathf.Max (infoA.ColliderVertices.Length - 2, infoB.ColliderVertices.Length - 2); i++)
{
//handle the first collider
if(i < infoA.ColliderVertices.Length - 2)
{
//get the information for the infoa edge
A = infoA.ColliderVertices[i + 1];
B = infoA.ColliderVertices[i + 2];
prevA = infoA.PrevColliderVertices[i + 1];
prevB = infoA.PrevColliderVertices[i + 2];
vA = A - prevA;
vB = B - prevB;
//test collision for each infoB contact against the infoa edge
for(int a = 0; a < contacts.Count; a++)
{
if(contacts[a].colliderA == infoA.collider) //this is where it could be beneficial to have separate lists, its not a big check, but it would also save on an iteration...
continue;
ParametricContactTest
(
A,
B,
infoB.ColliderVertices[contacts[a].bodyApm + 1],
prevA,
prevB,
infoB.PrevColliderVertices[contacts[a].bodyApm + 1],
vA - (infoB.ColliderVertices[contacts[a].bodyApm + 1] - infoB.PrevColliderVertices[contacts[a].bodyApm + 1]),
vB - (infoB.ColliderVertices[contacts[a].bodyApm + 1] - infoB.PrevColliderVertices[contacts[a].bodyApm + 1]),
i,
contacts[a]
);
}
}
//handle the second collider
if(i < infoB.ColliderVertices.Length - 2)
{
A = infoB.ColliderVertices[i + 1];
B = infoB.ColliderVertices[i + 2];
prevA = infoB.PrevColliderVertices[i + 1];
prevB = infoB.PrevColliderVertices[i + 2];
vA = A - prevA;
vB = B - prevB;
for(int a = 0; a < contacts.Count; a++)
{
if(contacts[a].colliderA == infoB.collider)
continue;
ParametricContactTest
(
A,
B,
infoA.ColliderVertices[contacts[a].bodyApm + 1],
prevA,
prevB,
infoA.PrevColliderVertices[contacts[a].bodyApm + 1],
vA - (infoA.ColliderVertices[contacts[a].bodyApm + 1] - infoA.PrevColliderVertices[contacts[a].bodyApm + 1]),
vB - (infoA.ColliderVertices[contacts[a].bodyApm + 1] - infoA.PrevColliderVertices[contacts[a].bodyApm + 1]),
i,
contacts[a]
);
}
}
}
}
private void FindClosestLineContactDetails(SupplementaryColliderInfo infoA, SupplementaryColliderInfo infoB, ref List<JelloContact> contacts, ref List<JelloContact> closestLineContacts)
{
Vector2 A, B, edgeNormal;
for (int i = 0; i < Mathf.Max (infoA.ColliderVertices.Length - 2, infoB.ColliderVertices.Length - 2); i++)
{
if(i < infoA.ColliderVertices.Length - 2)
{
A = infoA.ColliderVertices[i + 1];
B = infoA.ColliderVertices[i + 2];
//edge normal
edgeNormal = infoA.EdgeNormals[i];
for(int a = 0; a < closestLineContacts.Count; a+=2)
{
if(closestLineContacts[a].colliderA == infoA.collider)
continue;
ClosestLineContactTest
(
A,
B,
infoB.ColliderVertices[closestLineContacts[a].bodyApm + 1],
i,
edgeNormal,
infoB.PointNormals[closestLineContacts[a].bodyApm],
infoA.InverseSquaredEdgeLengths[i],
closestLineContacts[a],
closestLineContacts[a + 1]
);
}
}
if(i < infoB.ColliderVertices.Length - 2)
{
A = infoB.ColliderVertices[i + 1];
B = infoB.ColliderVertices[i + 2];
// normal
edgeNormal = infoB.EdgeNormals[i];
for(int a = 0; a < closestLineContacts.Count; a+=2)
{
if(closestLineContacts[a].colliderA == infoB.collider)
continue;
ClosestLineContactTest
(
A,
B,
infoA.ColliderVertices[closestLineContacts[a].bodyApm + 1],
i,
edgeNormal,
infoA.PointNormals[closestLineContacts[a].bodyApm],
infoB.InverseSquaredEdgeLengths[i],
closestLineContacts[a],
closestLineContacts[a + 1]
);
}
}
}
//TODO place the following code into another method?
for(int i = 0; i < closestLineContacts.Count; i++)
{
if (closestLineContacts[i].penetration > PenetrationThreshold && closestLineContacts[i + 1].penetration < closestLineContacts[i].penetration && closestLineContacts[i + 1].penetration != Mathf.Infinity)
{
closestLineContacts[i + 1].penetration = Mathf.Sqrt(closestLineContacts[i + 1].penetration);
float eLength;
if(closestLineContacts[i + 1].colliderA == infoA.collider)
{
eLength = Vector2.Distance(infoB.ColliderVertices[closestLineContacts[i + 1].bodyBpmA + 1], infoB.ColliderVertices[closestLineContacts[i + 1].bodyBpmB + 1]);
if(closestLineContacts[i + 1].bodyBpmB == infoB.ColliderVertices.Length - 2)
closestLineContacts[i + 1].bodyBpmB = 0;
}
else
{
eLength = Vector2.Distance(infoA.ColliderVertices[closestLineContacts[i + 1].bodyBpmA + 1], infoA.ColliderVertices[closestLineContacts[i + 1].bodyBpmB + 1]);
if(closestLineContacts[i + 1].bodyBpmB == infoA.ColliderVertices.Length - 2)
closestLineContacts[i + 1].bodyBpmB = 0;
}
if(eLength != 0)
eLength = 1 / eLength;
closestLineContacts[i + 1].normal *= eLength;
closestLineContacts[i + 1].mtv = closestLineContacts[i + 1].normal;
closestLineContacts[i + 1].R = closestLineContacts[i + 1].hitPoint - (Vector2)closestLineContacts[i + 1].transformA.position;
closestLineContacts[i + 1].R2 = closestLineContacts[i + 1].hitPoint - (Vector2)closestLineContacts[i + 1].transformB.position;
closestLineContacts.Remove(closestLineContacts[i]);
}
else if(closestLineContacts[i].penetration != Mathf.Infinity)
{
closestLineContacts[i].penetration = Mathf.Sqrt(closestLineContacts[i].penetration);
float eLength;
if(closestLineContacts[i].colliderA == infoA.collider)
{
eLength = Vector2.Distance(infoB.ColliderVertices[closestLineContacts[i].bodyBpmA + 1], infoB.ColliderVertices[closestLineContacts[i].bodyBpmB + 1]);
if(closestLineContacts[i].bodyBpmB >= infoB.ColliderVertices.Length - 2)
closestLineContacts[i].bodyBpmB = 0;
}
else
{
eLength = Vector2.Distance(infoA.ColliderVertices[closestLineContacts[i].bodyBpmA + 1], infoA.ColliderVertices[closestLineContacts[i].bodyBpmB + 1]);
if(closestLineContacts[i].bodyBpmB >= infoA.ColliderVertices.Length - 2)
closestLineContacts[i].bodyBpmB = 0;
}
if(eLength != 0)
eLength = 1 / eLength;
closestLineContacts[i].normal *= eLength;
closestLineContacts[i].mtv = closestLineContacts[i].normal;
closestLineContacts[i].R = closestLineContacts[i].hitPoint - (Vector2)closestLineContacts[i].transformA.position;
closestLineContacts[i].R2 = closestLineContacts[i].hitPoint - (Vector2)closestLineContacts[i].transformB.position;
closestLineContacts.Remove(closestLineContacts[i + 1]);
}
else
{
closestLineContacts.Remove(closestLineContacts[i]);
closestLineContacts.Remove(closestLineContacts[i]);
i-=1;
}
}
//now place the closet line contacs back into the original contacts list.
for(int i = 0; i < closestLineContacts.Count; i++)
{
contacts.Add(closestLineContacts[i]);
}
}
private void FinalizeContactTypes(SupplementaryColliderInfo infoA, SupplementaryColliderInfo infoB, ref List<JelloContact> contacts, ref List<JelloContact> closestLineContacts)
{
//loop through each contact and fill out the rest of the details.
for(int i = contacts.Count - 1; i >= 0; i--)
{
//if the time of impact is infinity then this contact point fialed the parametric contact tests and the closest line method will need to be used instead.
if(contacts[i].toi == Mathf.Infinity)
{
//add two copies of this conact to the closest line contact list.
//representative of closestSame
closestLineContacts.Add (contacts[i]);
closestLineContacts[closestLineContacts.Count - 1].penetration = Mathf.Infinity;
//representative of closestAway
closestLineContacts.Add(new JelloContact());
closestLineContacts[closestLineContacts.Count - 1].penetration = Mathf.Infinity;
closestLineContacts[closestLineContacts.Count - 1].bodyA = contacts[i].bodyA;
closestLineContacts[closestLineContacts.Count - 1].bodyB = contacts[i].bodyB;
closestLineContacts[closestLineContacts.Count - 1].bodyApm = contacts[i].bodyApm;
closestLineContacts[closestLineContacts.Count - 1].colliderA = contacts[i].colliderA;
closestLineContacts[closestLineContacts.Count - 1].colliderB = contacts[i].colliderB;
closestLineContacts[closestLineContacts.Count - 1].rigidbodyA = contacts[i].rigidbodyA;
closestLineContacts[closestLineContacts.Count - 1].rigidbodyB = contacts[i].rigidbodyB;
closestLineContacts[closestLineContacts.Count - 1].transformA = contacts[i].transformA;
closestLineContacts[closestLineContacts.Count - 1].transformB = contacts[i].transformB;
//remove this contact from the contacts list.
//one of the two above will be returned to this list later at the end of this.
contacts.RemoveAt(i);
continue;
}
if(contacts[i].colliderA == infoA.collider)
{
//assign the normal
contacts[i].normal = infoB.EdgeNormals[contacts[i].bodyBpmA];
//test hybrid
HybridContactTest
(
infoB.ColliderVertices[contacts[i].bodyBpmA + 1],
infoB.ColliderVertices[contacts[i].bodyBpmB + 1],
infoA.ColliderVertices[contacts[i].bodyApm + 1],
infoB.InverseSquaredEdgeLengths[contacts[i].bodyBpmA],
contacts[i]
);
if(contacts[i].bodyBpmB >= infoB.ColliderVertices.Length - 2)
contacts[i].bodyBpmB = 0;
}
else
{
//assign the normal
contacts[i].normal = infoA.EdgeNormals[contacts[i].bodyBpmA];
//test hybrid
HybridContactTest
(
infoA.ColliderVertices[contacts[i].bodyBpmA + 1],
infoA.ColliderVertices[contacts[i].bodyBpmB + 1],
infoB.ColliderVertices[contacts[i].bodyApm + 1],
infoA.InverseSquaredEdgeLengths[contacts[i].bodyBpmA],
contacts[i]
);
if(contacts[i].bodyBpmB >= infoA.ColliderVertices.Length - 2)
contacts[i].bodyBpmB = 0;
}
contacts[i].R = contacts[i].hitPoint - (Vector2)contacts[i].transformA.position;
contacts[i].R2 = contacts[i].hitPoint - (Vector2)contacts[i].transformB.position;
}
}
/// <summary>
/// Start tracking the Collider2D.
/// Adds it to the JelloWorld.trackedColliders list and JelloWorld.TrackedColliderDictionary.
/// </summary>
/// <param name="collider">The Collider2D to start tracking.</param>
/// <param name="colliderInfo">The SupplementaryColliderInfo that describes the now tracked Collider2D.</param>
/// <param name="jelloBody">The JelloBody, if any, the Collider2D belongs to.</param>
/// <returns>Whether the collider is already being tracked. True if already being tracked, false if not.</returns>
public bool StartTrackingCollider(Collider2D collider, out SupplementaryColliderInfo colliderInfo, JelloBody jelloBody = null)
{
if(trackedColliders.Contains(collider))
{
colliderInfo = World.TrackedColliderDictionary[collider.GetInstanceID()];
return true;
}
trackedColliders.Add (collider);
colliderInfo = new SupplementaryColliderInfo(collider, jelloBody);
TrackedColliderDictionary.Add (collider.GetInstanceID(), colliderInfo);
return false;
}
