// =====
#region Methods
// Check two convex polygons
public void Check(ConvexPolygon polygonA, ConvexPolygon polygonB, float dt)
{
// We will use the Separating Axis Theorem to determine whether or not
// two convex polygons will collide. This theorem states that if there
// exists an edge, belonging to either polygon, such that the polygons'
// edge-normal projection intervals do NOT overlap, then the polygons
// themselves do not overlap. We are essentially searching for a
// direction on which we can cleanly draw a line between these two
// polygons. If such a direction exists, then the polygons do not
// overlap. After all, if we can draw a line between them, surely they
// are not overlapping.
//
// This is the intuition behind the Separating Axis Theorem. However, we
// are not simply interested in whether or not two polygons intersect.
// Rather, we are interested in the TIME at which they interesect, i.e.
// the timeOfImpact. We will do this by investigating the motion of the
// polygons' projection intervals on every edge-normal axis.
//
// SAT says that two polygons are separated if there is at least one
// axis on which their projections do not overlap. Then, it follows that
// two polygons are intersecting if and only if their projections overlap
// on EVERY edge-normal axis! Thus, we will monitor the polygons'
// projections across all axes and find the time of the LAST overlap.
// This is the polygon-polygon timeOfImpact.
// The algorithm may be summarized as follows.
//
// 1.) Broad phase: Bounding box test
// 2.) Narrow phase: Separating axis test
#region Initialization
// Entry logging
#if IS_LOGGING_METHODS
Log.Write(String.Format("Entering method for {0} with {1} and {2}", this.Name, BodyA.Name, BodyB.Name);
#endif
// Physics logging
#if IS_LOGGING_PHYSICS
if (Log.Subject1 == null || (BodyA.Name == Log.Subject1 && (Log.Subject2 == null || BodyB.Name == Log.Subject2)))
{
Log.Write(String.Format("{0} entering collision check with {1} over time interval [0, {2:0.0000}]", this.BodyA.Name, this.BodyB.Name, dt));
}
#endif
// Initialize fromLeft, which equals true if intervalA approaches
// intervalB from the left. This value is re-evaluated for each axis.
bool fromLeft = false;
// Use short-hand notation
Vector2 vA = BodyA.Velocity;
Vector2 vB = BodyB.Velocity;
Vector2 cA = BodyA.RotationalAxis;
Vector2 cB = BodyB.RotationalAxis;
float wA = BodyA.AngularVelocity;
float wB = BodyB.AngularVelocity;
// Get sweep boxes
Box boxA = polygonA.GetSweepBox(wA * dt, cA, vA * dt);
Box boxB = polygonB.GetSweepBox(wB * dt, cB, vB * dt);
#endregion
#region [1] Broad phase: Bounding box test
// Physics logging
#if IS_LOGGING_PHYSICS
if (Log.Subject1 == null || (BodyA.Name == Log.Subject1 && (Log.Subject2 == null || BodyB.Name == Log.Subject2)))
{
// Log.Write(String.Format("boxA = {0}", boxA));
// Log.Write(String.Format("boxB = {0}", boxB));
}
#endif
// If the polygons' bounding boxes will not intersect, then surely the
// polygons themselves won't either.
if (!boxA.IntersectsWith(boxB))
{
// Case 1: Boxes do not intersect
// Physics logging
#if IS_LOGGING_PHYSICS
if (Log.Subject1 == null || (BodyA.Name == Log.Subject1 && (Log.Subject2 == null || BodyB.Name == Log.Subject2)))
{
Log.Write("Boxes do NOT intersect! We are done.");
}
#endif
// Go to exit
this.Time = float.PositiveInfinity;
goto exit;
}
else
{
// Case 2: Boxes do intersect
// Physics logging
#if IS_LOGGING_PHYSICS
if (Log.Subject1 == null || (BodyA.Name == Log.Subject1 && (Log.Subject2 == null || BodyB.Name == Log.Subject2)))
{
Log.Write("Boxes do intersect... Keep going...");
}
#endif
}
#endregion
#region [2] Narrow phase: Separating axis test
// Count this polygon's vertices
int n = polygonA.Vertices.Count;
// Count other polygon's vertices
int m = polygonB.Vertices.Count;
// Get this polygon's edges
List <LineSegment> edgesA = polygonA.Edges;
// Get other polygon's edges
List <LineSegment> edgesB = polygonB.Edges;
// Initialize list of previous axes' initial slopes
List <float> pastSlopes1 = new List <float>();
// Initialize list of previous axes' final slopes
List <float> pastSlopes2 = new List <float>();
// Initialize list of 'remembered' axes
List <int> axes = new List <int>();
// Initialize list of 'remembered' axes' initial interval distances
List <float> distances = new List <float>();
// Initialize list of 'remembered' axes' 'isResting' indicators
List <bool> isRestingStates = new List <bool>();
// Initialize list of 'remembered' axes' 'fromLeft' indicators
List <bool> fromLeftStates = new List <bool>();
// Initialize minimum interval overlap distance
float minOverlapDistance = float.NegativeInfinity;
// Initialize axis on which minOverlapDistance occurs
int minOverlapAxis = -1;
// Equals true if fromLeft holds on minOverlapAxis
bool minOverlapFromLeft = false;
// Get 'initial' and 'final' states
this.BodyA.Sweep.AddState(0);
this.BodyB.Sweep.AddState(0);
this.BodyA.Sweep.AddState(dt);
this.BodyB.Sweep.AddState(dt);
#region SAT loop
// Loop through edges
for (int i = 0; i < n + m; i++)
{
// Declare current edge
LineSegment edge;
// Declare edge's unit normal vector i.e. the 'axis'
Vector2 axis1;
// Declare edge's unit normal vector after dt elapsed seconds
Vector2 axis2;
// Proceed according to current iteration number
if (i < n)
{
// Case 1: This edge belongs to polygonA
edge = edgesA[i];
axis1 = (edge.Point2 - edge.Point1).Unit.Normal;
int timeIndex = this.BodyA.Sweep.GetIndex(dt);
Vector2 p1 = this.BodyA.Sweep.States[timeIndex][i];
Vector2 p2 = this.BodyA.Sweep.States[timeIndex][Globals.MathHelper.Mod(i + 1, n)];
axis2 = (p2 - p1).Unit.Normal;
}
else
{
// Case 2: This edge belongs to polygonB
edge = edgesB[i - n];
axis1 = (edge.Point2 - edge.Point1).Unit.Normal;
int timeIndex = this.BodyB.Sweep.GetIndex(dt);
Vector2 p1 = this.BodyB.Sweep.States[timeIndex][i - n];
Vector2 p2 = this.BodyB.Sweep.States[timeIndex][Globals.MathHelper.Mod(i - n + 1, m)];
axis2 = (p2 - p1).Unit.Normal;
}
// Physics logging
#if IS_LOGGING_PHYSICS
if (Log.Subject1 == null || (BodyA.Name == Log.Subject1 && (Log.Subject2 == null || BodyB.Name == Log.Subject2)))
{
Log.Write(String.Format("Entering iteration {0} of {1} for axis1 = {2}, axis2 = {3}", i + 1, n + m, axis1, axis2));
}
#endif
#region Skip axes with similar slopes
// Get current slope
float slope1 = axis1.Slope;
// Get future slope
float slope2 = axis2.Slope;
// Initialize alreadyTested as false
bool alreadyTested = false;
// Loop through archive
for (int j = 0; j < pastSlopes1.Count; j++)
{
// Check for a match
if (slope1 == pastSlopes1[j] && slope2 == pastSlopes2[j])
{
// If a match exists, we raise a flag
alreadyTested = true;
break;
}
}
// Check if a flag has been raised
if (alreadyTested)
{
// If so, we may skip this iteration
// Physics logging
#if IS_LOGGING_PHYSICS
if (Log.Subject1 == null || (BodyA.Name == Log.Subject1 && (Log.Subject2 == null || BodyB.Name == Log.Subject2)))
{
Log.Write("Slope already tested. Skip this iteration.");
}
#endif
// Skip
continue;
}
else
{
// Otherwise, update the archive
pastSlopes1.Add(slope1);
pastSlopes2.Add(slope2);
}
#endregion
#region Classify initial and final intervals
// Get initial projections
Interval intervalA1 = this.BodyA.Sweep.GetProjection(axis1, 0);
Interval intervalB1 = this.BodyB.Sweep.GetProjection(axis1, 0);
// Get final projections
Interval intervalA2 = this.BodyA.Sweep.GetProjection(axis2, dt);
Interval intervalB2 = this.BodyB.Sweep.GetProjection(axis2, dt);
// Get d1, the initial interval separation distance
float d1 = Interval.DistanceBetween(intervalA1, intervalB1);
if (d1 == 0)
{
d1 = -intervalA1.IntersectionWith(intervalB1).Length;
}
// Get d2, the final interval separation distance
float d2 = Interval.DistanceBetween(intervalA2, intervalB2);
if (d2 == 0)
{
d2 = -intervalA2.IntersectionWith(intervalB2).Length;
}
// Classify this pair of initial intervals as either 'not overlapping',
// 'touching', or 'overlapping', i.e. 0, 1, or 2, respectively
byte isOverlap1 = 0;
if (d1 >= -CollisionResult.Epsilon && d1 <= CollisionResult.Epsilon)
{
isOverlap1 = 1;
}
else if (d1 < -CollisionResult.Epsilon)
{
isOverlap1 = 2;
}
// Classify this pair of final intervals as either 'not overlapping',
// 'touching', or 'overlapping', i.e. 0, 1, or 2, respectively
byte isOverlap2 = 0;
if (d2 >= -CollisionResult.Epsilon && d2 <= CollisionResult.Epsilon)
{
isOverlap2 = 1;
}
else if (d2 < -CollisionResult.Epsilon)
{
isOverlap2 = 2;
}
// Check if intervalA is initially to the left of intervalB
fromLeft = false;
if (d1 >= 0 && intervalA1.Max <= intervalB1.Min)
{
fromLeft = true;
}
else if (d1 < 0 && intervalA1.Min < intervalB1.Min)
{
fromLeft = true;
}
// Update minOverlapDistance. Recall that positive and negative
// distances represent separation and penetration, respectively.
if (d2 < -CollisionResult.Epsilon && d2 > minOverlapDistance)
{
minOverlapDistance = d2;
minOverlapAxis = i;
minOverlapFromLeft = fromLeft;
}
// Physics logging
#if IS_LOGGING_PHYSICS
if (Log.Subject1 == null || (BodyA.Name == Log.Subject1 && (Log.Subject2 == null || BodyB.Name == Log.Subject2)))
{
Log.Write(String.Format("intervalA1 = {0}, intervalB1 = {1}, d1 = {2}, isOverlap1 = {3}, fromLeft = {4}", intervalA1, intervalB1, d1, isOverlap1, fromLeft));
Log.Write(String.Format("intervalA2 = {0}, intervalB2 = {1}, d2 = {2}, isOverlap2 = {3}", intervalA2, intervalB2, d2, isOverlap2));
}
#endif
#endregion
#region Classify interval pair
// Proceed according to initial and final overlap states
if (isOverlap1 == 0)
{
if (isOverlap2 == 0)
{
#region Case 1: Initially not overlapping, future not overlapping
// Physics logging
#if IS_LOGGING_PHYSICS
if (Log.Subject1 == null || (BodyA.Name == Log.Subject1 && (Log.Subject2 == null || BodyB.Name == Log.Subject2)))
{
Log.Write(String.Format("case 1: A separating axis exists... No collision!"));
}
#endif
// We are done
this.Time = float.PositiveInfinity;
goto exit;
#endregion
}
else if (isOverlap2 == 1)
{
#region Case 2: Initially not overlapping, future touching
// Physics logging
#if IS_LOGGING_PHYSICS
if (Log.Subject1 == null || (BodyA.Name == Log.Subject1 && (Log.Subject2 == null || BodyB.Name == Log.Subject2)))
{
Log.Write(String.Format("case 2: These intervals will eventually collide... Remember this collisionAxis!"));
}
#endif
// Remember collisionAxis
axes.Add(i);
distances.Add(d1);
isRestingStates.Add(false);
fromLeftStates.Add(fromLeft);
#endregion
}
else
{
#region Case 3: Initially not overlapping, future overlapping
// Physics logging
#if IS_LOGGING_PHYSICS
if (Log.Subject1 == null || (BodyA.Name == Log.Subject1 && (Log.Subject2 == null || BodyB.Name == Log.Subject2)))
{
Log.Write(String.Format("case 3: These intervals will eventually collide... Remember this collisionAxis!"));
}
#endif
// Remember collisionAxis
axes.Add(i);
distances.Add(d1);
isRestingStates.Add(false);
fromLeftStates.Add(fromLeft);
#endregion
}
}
else if (isOverlap1 == 1)
{
if (isOverlap2 == 0)
{
#region Case 4: Initially touching, future not overlapping
// Physics logging
#if IS_LOGGING_PHYSICS
if (Log.Subject1 == null || (BodyA.Name == Log.Subject1 && (Log.Subject2 == null || BodyB.Name == Log.Subject2)))
{
Log.Write(String.Format("case 4: A separating axis exists... No collision!"));
}
#endif
// We are done
this.Time = float.PositiveInfinity;
goto exit;
#endregion
}
else if (isOverlap2 == 1)
{
#region Case 5: Initially touching, future touching
// Physics logging
#if IS_LOGGING_PHYSICS
if (Log.Subject1 == null || (BodyA.Name == Log.Subject1 && (Log.Subject2 == null || BodyB.Name == Log.Subject2)))
{
Log.Write(String.Format("case 5: These intervals are resting! Remember this restingAxis!"));
}
#endif
// Remember restingAxis
axes.Add(i);
distances.Add(d2);
isRestingStates.Add(true);
fromLeftStates.Add(fromLeft);
#endregion
}
else
{
#region Case 6: Initially touching, future overlapping
// This case is difficult to classify. We could have resting
// intervals that are gradually pulled together over time. In this
// case, we should still consider them as 'resting.' However, it
// is also possible that an interval has abruptly sped up, and this
// is a potential collision axis. We will proceed according to a
// pre-determined threshold, i.e. CollisionResult.Delta.
// Get interval displacement
float dd; if (d2 > d1)
{
dd = d2 - d1;
}
else
{
dd = d1 - d2;
}
// Compare against threshold
if (dd < 5.0f)
{
// Physics logging
#if IS_LOGGING_PHYSICS
if (Log.Subject1 == null || (BodyA.Name == Log.Subject1 && (Log.Subject2 == null || BodyB.Name == Log.Subject2)))
{
Log.Write(String.Format("case 6: This is tricky... Too slow for collision... Remember this restingAxis..."));
}
#endif
// Remember restingAxis
axes.Add(i);
distances.Add(d2);
isRestingStates.Add(true);
fromLeftStates.Add(fromLeft);
}
else
{
// Physics logging
#if IS_LOGGING_PHYSICS
if (Log.Subject1 == null || (BodyA.Name == Log.Subject1 && (Log.Subject2 == null || BodyB.Name == Log.Subject2)))
{
Log.Write(String.Format("case 6: This is tricky... Too fast for resting... Remember this collisionAxis..."));
}
#endif
// Remember collisionAxis
axes.Add(i);
distances.Add(d1);
isRestingStates.Add(false);
fromLeftStates.Add(fromLeft);
}
#endregion
}
}
else
{
if (isOverlap2 == 0)
{
#region Case 7: Initially overlapping, future not overlapping
// Physics logging
#if IS_LOGGING_PHYSICS
if (Log.Subject1 == null || (BodyA.Name == Log.Subject1 && (Log.Subject2 == null || BodyB.Name == Log.Subject2)))
{
Log.Write(String.Format("case 7: A separating axis exists... No collision!"));
}
#endif
// Question: Does this imply a contact is released?
// ... Not necessarily, but perhaps a useful event to monitor...
// We are done
this.Time = float.PositiveInfinity;
goto exit;
#endregion
}
else if (isOverlap2 == 1)
{
#region Case 8: Initially overlapping, future touching
// This case is difficult to classify. We could have resting
// intervals that were gradually pulled together over time, and are
// now trying to re-separate in response to the penalty forces. In
// this case, we should still consider them as 'resting.' However,
// it is also possible that the collision axis has changed, which
// would make this axis a potential candidate. We will proceed
// according to CollisionResult.Delta.
// Get interval displacement
float dd; if (d2 > d1)
{
dd = d2 - d1;
}
else
{
dd = d1 - d2;
}
// Compare against threshold
if (dd < 1.0f)
{
// Physics logging
#if IS_LOGGING_PHYSICS
if (Log.Subject1 == null || (BodyA.Name == Log.Subject1 && (Log.Subject2 == null || BodyB.Name == Log.Subject2)))
{
Log.Write(String.Format("case 8: This is tricky... Too slow for collision... Remember this restingAxis..."));
}
#endif
// Remember restingAxis
axes.Add(i);
distances.Add(d2);
isRestingStates.Add(true);
fromLeftStates.Add(fromLeft);
}
else
{
// Physics logging
#if IS_LOGGING_PHYSICS
if (Log.Subject1 == null || (BodyA.Name == Log.Subject1 && (Log.Subject2 == null || BodyB.Name == Log.Subject2)))
{
Log.Write(String.Format("case 8: This is tricky... Too fast for resting... Let's say 'overlapping intervals'... Skip this iteration."));
}
#endif
// Skip
continue;
}
#endregion
}
else
{
#region Case 9: Initially overlapping, future overlapping
// Physics logging
#if IS_LOGGING_PHYSICS
if (Log.Subject1 == null || (BodyA.Name == Log.Subject1 && (Log.Subject2 == null || BodyB.Name == Log.Subject2)))
{
Log.Write(String.Format("case 9: We have 'overlapping intervals'... Skip this iteration."));
}
#endif
// Skip
continue;
#endregion
}
}
#endregion
}
#endregion
// If no 'potential' axes were found, then these bodies are initially
// overlapping. We will apply a spring-like 'penalty' force on the axis
// of minimum pentration.
#region (Old) Error: Initially overlapping bodies
#if IS_ERROR_CHECKING
// Check 'remembered' axes count
// if (axes.Count == 0)
if (false)
{
// Create error string
String s = "Collision error\n";
s += "Bodies are initially overlapping!\n";
s += String.Format("bodyA.Name = {0}, bodyB.Name = {1}\n", this.BodyA.Name, this.BodyB.Name);
// Throw exception
throw new SystemException(s);
}
#endif
#endregion
// Penalty force testing
if (axes.Count == 0)
{
this.Time = float.PositiveInfinity;
this.Index = minOverlapAxis;
this.Distance = minOverlapDistance;
this.FromLeft = minOverlapFromLeft;
this.IsResting = true;
goto exit;
}
// If we've made it this far, we have a list of potential axes on which
// a collision or resting contact may occur. If 'resting' intervals
// occur on every 'remembered' axis, then this is a resting contact.
// Otherwise, this is not a resting contact, but rather a true collision.
#region Resting contact
// Physics logging
#if IS_LOGGING_PHYSICS
if (Log.Subject1 == null || (BodyA.Name == Log.Subject1 && (Log.Subject2 == null || BodyB.Name == Log.Subject2)))
{
Log.Write(String.Format("Potential contact axis count: {0}", axes.Count));
for (int i = 0; i < axes.Count; i++)
{
Log.Write(String.Format("axes[{0}] = {1}, distances[{2}] = {3}, isRestingStates[{4}] = {5}, fromLeftStates[{6}] = {7}",
i, axes[i], i, distances[i], i, isRestingStates[i], i, fromLeftStates[i]));
}
}
#endif
// Check for a resting contact
bool isResting = true;
for (int i = 0; i < isRestingStates.Count; i++)
{
if (isRestingStates[i] == false)
{
isResting = false; break;
}
}
// Physics logging
#if IS_LOGGING_PHYSICS
if (Log.Subject1 == null || (BodyA.Name == Log.Subject1 && (Log.Subject2 == null || BodyB.Name == Log.Subject2)))
{
if (isResting)
{
Log.Write(String.Format("This is a resting contact!"));
}
else
{
Log.Write(String.Format("This is not a resting contact!"));
}
}
#endif
// If all axes have resting intervals, this is a resting contact.
// We must find the optimal resting contact axis. We will pick
// whichever axis has the smallest absolute interval distance.
if (isResting)
{
// Initialize minimum absolute distance
float minDistance = float.PositiveInfinity;
for (int i = distances.Count - 1; i >= 0; i--)
{
// Get absolute distance
float absDistance = distances[i];
if (absDistance < 0)
{
absDistance = -absDistance;
}
// Check if optimal
if (absDistance < minDistance)
{
// If so, update optimum
minDistance = absDistance;
}
else
{
// Otherwise, remove from list
axes.RemoveAt(i);
distances.RemoveAt(i);
isRestingStates.RemoveAt(i);
fromLeftStates.RemoveAt(i);
}
}
// Since ties aren't allowed, there will be exactly one solution.
this.Time = float.PositiveInfinity;
this.Index = axes[0];
this.Distance = distances[0];
this.IsResting = isRestingStates[0];
this.FromLeft = fromLeftStates[0];
// We are done
goto exit;
}
#endregion
#region Collision contact
// If we've made it this far, this isn't a resting contact. We have a
// list of potential axes on which a true collision will occur.
this.Time = Sweep.GetTimeOfImpact(this.BodyA.Sweep, this.BodyB.Sweep, dt, ref axes, ref distances, ref isRestingStates, ref fromLeftStates);
this.Index = axes[0];
this.Distance = distances[0];
this.IsResting = isRestingStates[0];
this.FromLeft = fromLeftStates[0];
#endregion
#endregion
#region [*] Exit trap
// [*] Exit trap
exit:
// Physics logging
#if IS_LOGGING_PHYSICS
if (Log.Subject1 == null || (BodyA.Name == Log.Subject1 && (Log.Subject2 == null || BodyB.Name == Log.Subject2)))
{
Log.Write(String.Format("returning Time = {0}, Index = {1}, Distance = {2}, FromLeft = {3}", this.Time, this.Index, this.Distance, this.FromLeft));
Log.Write(String.Format("{0} exiting collision check with {1}", this.BodyA.Name, this.BodyB.Name));
}
#endif
// Exit logging
#if IS_LOGGING_METHODS
Log.Write (String.Format("Exiting method for {0} with {1} and {2}", this.Name, BodyA.Name, BodyB.Name);
#endif
// Null op
return;
#endregion
}
