private static void SetPointsFromContactPointsAndVertices(Polygon polygon, Polygon otherPolygon, List<ContactPoint> contactPoints, int polygonPointToStartAt, List<Vector3> thisVertices, List<Vector3> otherVertices)
{
Polygon currentPolygon = polygon;
List<Vector3> newPolygonPoints = new List<Vector3>();
int otherIndex = 0;
int otherIndexToStartAt = -1;
int thisIndex = polygonPointToStartAt;
while (true)
{
if (newPolygonPoints.Count > 3 * (polygon.Points.Count + otherPolygon.Points.Count))
{
// just break here for now
break;
}
bool isThereAContactPoint = false;
if (currentPolygon == polygon)
{
newPolygonPoints.Add(thisVertices[thisIndex]);
isThereAContactPoint = DoesContactPointsHaveThisIndex(thisIndex, contactPoints);
}
else
{
newPolygonPoints.Add(otherVertices[otherIndex]);
isThereAContactPoint = DoesContactPointsHaveOtherIndex(otherIndex, contactPoints);
}
if (isThereAContactPoint)
{
ContactPoint cp = new ContactPoint();
if (currentPolygon == polygon)
{
cp = GetContactPointAtThisIndex(thisIndex, contactPoints, thisVertices[thisIndex]);
}
else
{
cp = GetContactPointAtOtherIndex(otherIndex, contactPoints, otherVertices[otherIndex]);
}
#region If it's a segment intersection, drop the point and swap the current polygon
if (cp.ContactType == ContactType.SegmentIntersection)
{
if (currentPolygon == polygon)
{
currentPolygon = otherPolygon;
otherIndex = cp.OtherIndex + 1;
if (otherIndex == otherVertices.Count - 1)
{
otherIndex = 0;
}
if (otherIndex == otherIndexToStartAt)
{
newPolygonPoints.Add(newPolygonPoints[0]);
break;
}
if (otherIndexToStartAt == -1)
{
otherIndexToStartAt = otherIndex;
}
}
else
{
currentPolygon = polygon;
thisIndex = cp.ThisIndex + 1;
if (thisIndex == polygon.Points.Count - 1)
{
thisIndex = 0;
}
if (thisIndex == polygonPointToStartAt)
{
// close off the polygon and stop adding points
newPolygonPoints.Add(newPolygonPoints[0]);
break;
}
}
newPolygonPoints.Add(cp.Position);
}
#endregion
else if(cp.ContactType == ContactType.PointOnSegment)
{
// See which next point is the furthest away, and decide based off of that.
int nextOtherIndex = cp.OtherIndex + 1;
if (cp.OtherEndpoint != -1)
{
nextOtherIndex = cp.OtherEndpoint + 1;
}
if (nextOtherIndex >= otherVertices.Count - 1)
{
nextOtherIndex -= (otherVertices.Count - 1);
}
int nextThisIndex = cp.ThisIndex + 1;
if (cp.ThisEndpoint != -1)
{
nextThisIndex = cp.ThisEndpoint + 1;
}
if (nextThisIndex >= thisVertices.Count - 1)
{
nextThisIndex -= (thisVertices.Count - 1);
}
Vector3 nextThisVector = thisVertices[nextThisIndex] - cp.Position;
Vector3 nextOtherVector = otherVertices[nextOtherIndex] - cp.Position;
double distanceAwayFromThis = 0;
double distanceAwayFromOther = 0;
bool doWhile = true;
int numberOfExtraDoWhiles = 0;
int thisIndexBeforeDoWhile = nextThisIndex;
int otherIndexBeforeDoWhile = nextOtherIndex;
// The doWhile section will find out which path will take us
// furtest away from the other Polygon. This works most of the time
// but in some cases (like if the initial two paths are parallel), we'll
// want to special case which we use
bool forceUseThis = false;
bool forceUseOther = false;
while (doWhile)
{
if (nextThisVector.Length() < .0001f)
{
nextThisIndex++;
if (nextThisIndex == thisVertices.Count - 1)
{
nextThisIndex = 0;
}
nextThisVector = thisVertices[nextThisIndex] - cp.Position;
}
if (nextOtherVector.Length() < .0001f)
{
nextOtherIndex++;
if (nextOtherIndex == otherVertices.Count - 1)
{
nextOtherIndex = 0;
}
nextOtherVector = otherVertices[nextOtherIndex] - cp.Position;
}
float thisVectorLength = nextThisVector.Length();
float otherVectorLength = nextOtherVector.Length();
float smallestDistance = System.Math.Min(thisVectorLength, otherVectorLength);
nextThisVector.Normalize();
nextOtherVector.Normalize();
nextThisVector *= smallestDistance / 2.0f;
nextOtherVector *= smallestDistance / 2.0f;
nextThisVector += cp.Position;
nextOtherVector += cp.Position;
if (nextThisVector == nextOtherVector)
{
forceUseThis = thisVectorLength < otherVectorLength;
forceUseOther = !forceUseThis;
break;
}
double minimumDistance = .00001;
if (polygon.IsPointInside(ref nextOtherVector))
{
distanceAwayFromThis = 0;
}
else
{
distanceAwayFromThis = polygon.VectorFrom(nextOtherVector.X, nextOtherVector.Y).Length();
if (distanceAwayFromThis < minimumDistance)
{
distanceAwayFromThis = 0;
}
}
if (otherPolygon.IsPointInside(ref nextThisVector))
{
distanceAwayFromOther = 0;
}
else
{
distanceAwayFromOther = otherPolygon.VectorFrom(nextThisVector.X, nextThisVector.Y).Length();
if (distanceAwayFromOther < minimumDistance)
{
distanceAwayFromOther = 0;
}
}
if (distanceAwayFromOther == distanceAwayFromThis)
{
// We need a tiebreaker. Let's move an extra index and see what happens, shall we?
nextThisIndex++;
if (nextThisIndex == thisVertices.Count - 1)
{
nextThisIndex = 0;
}
nextThisVector = thisVertices[nextThisIndex] - cp.Position;
nextOtherIndex++;
if (nextOtherIndex == otherVertices.Count - 1)
{
nextOtherIndex = 0;
}
nextOtherVector = otherVertices[nextOtherIndex] - cp.Position;
numberOfExtraDoWhiles++;
}
else
{
doWhile = false;
}
}
nextThisIndex = thisIndexBeforeDoWhile;
nextOtherIndex = otherIndexBeforeDoWhile;
bool useThis = distanceAwayFromThis < distanceAwayFromOther;
if (forceUseThis)
useThis = true;
else if (forceUseOther)
useThis = false;
if (useThis && currentPolygon == polygon)
{
// make sure there are no other contact points on the current segment on this
for (int i = 0; i < contactPoints.Count; i++)
{
ContactPoint otherCp = contactPoints[i];
if (otherCp.Position != cp.Position && otherCp.ThisIndex == cp.ThisIndex)
{
useThis = false;
break;
}
}
}
else if (!useThis && currentPolygon == otherPolygon)
{
// make sure there are no other contact points on the current segment on this
for (int i = 0; i < contactPoints.Count; i++)
{
ContactPoint otherCp = contactPoints[i];
if (otherCp.Position != cp.Position && otherCp.OtherIndex == cp.OtherIndex)
{
useThis = true;
break;
}
}
}
newPolygonPoints.Add(cp.Position);
if (distanceAwayFromThis == distanceAwayFromOther)
{
useThis = currentPolygon == polygon;
}
if (useThis)
{
currentPolygon = polygon;
thisIndex = nextThisIndex;
if (thisIndex == polygonPointToStartAt)
{
// close off the polygon and stop adding points
newPolygonPoints.Add(newPolygonPoints[0]);
break;
}
}
else
{
currentPolygon = otherPolygon;
otherIndex = nextOtherIndex;
if (otherIndex == otherIndexToStartAt)
{
newPolygonPoints.Add(newPolygonPoints[0]);
break;
}
if (otherIndexToStartAt == -1)
{
otherIndexToStartAt = otherIndex;
}
}
}
}
else
{
if (currentPolygon == polygon)
{
thisIndex++;
if (thisIndex == polygon.Points.Count - 1)
{
thisIndex = 0;
}
if (thisIndex == polygonPointToStartAt)
{
// close off the polygon and stop adding points
newPolygonPoints.Add(newPolygonPoints[0]);
break;
}
}
else
{
otherIndex++;
if (otherIndex == otherPolygon.Points.Count - 1)
{
otherIndex = 0;
}
if (otherIndex == otherIndexToStartAt)
{
// close off the polygon and stop adding points
newPolygonPoints.Add(newPolygonPoints[0]);
break;
}
}
}
}
SetPolygonPoints(polygon, newPolygonPoints);
}
private static List<ContactPoint> GetContactPoints(Segment[] firstSegments, Segment[] secondSegments)
{
if (shouldEliminatePointOnEnd)
{
ShiftEndpointsToEliminatePointOnSegment(firstSegments, secondSegments);
}
Point intersectionPoint = new Point();
List<ContactPoint> contactPoints = new List<ContactPoint>();
Segment firstSegment = new Segment();
Segment secondSegment = new Segment();
const double allowedError = .0005;
bool debug = false;
#if !SILVERLIGHT && !MONOGAME && !XBOX360 && !WINDOWS_PHONE
if (debug)
{
FlatRedBall.Content.Polygon.PolygonSaveList psl = new FlatRedBall.Content.Polygon.PolygonSaveList();
psl.PolygonSaves.Add(
FlatRedBall.Content.Polygon.PolygonSave.FromPolygon(
Polygon.FromSegments(firstSegments)));
psl.PolygonSaves.Add(
FlatRedBall.Content.Polygon.PolygonSave.FromPolygon(
Polygon.FromSegments(secondSegments)));
psl.Save(FlatRedBall.IO.FileManager.MyDocuments + "modifiedPolygons.plylstx");
}
#endif
#region Handle the two-parallel segment special case
List<ContactPoint> parallelContactPoints = new List<ContactPoint>();
for (int firstIndex = 0; firstIndex < firstSegments.Length; firstIndex++)
{
firstSegment = firstSegments[firstIndex];
for (int secondIndex = 0; secondIndex < secondSegments.Length; secondIndex++)
{
secondSegment = secondSegments[secondIndex];
Point parallelIntersectionPoint;
if (firstSegment.IsParallelAndTouching(secondSegment, out parallelIntersectionPoint))
{
ContactPoint cp = new ContactPoint();
cp.ThisIndex = firstIndex;
cp.OtherIndex = secondIndex;
// Technically this is a point on segment intersection,
// but we treat it like a segment intersection because we're
// going to eliminate duplicates
cp.ContactType = ContactType.SegmentIntersection;
cp.Position = new Vector3((float)parallelIntersectionPoint.X, (float)parallelIntersectionPoint.Y, 0);
parallelContactPoints.Add(cp);
}
}
}
#endregion
#region else, do regular intersection tests
List<int> indexesToRemove = new List<int>();
for (int firstSegmentIndex = 0; firstSegmentIndex < firstSegments.Length; firstSegmentIndex++)
{
firstSegment = firstSegments[firstSegmentIndex];
for (int secondSegmentIndex = 0; secondSegmentIndex < secondSegments.Length; secondSegmentIndex++)
{
secondSegment = secondSegments[secondSegmentIndex];
bool intersects = firstSegment.Intersects(secondSegment, out intersectionPoint);
if ( intersects ||
firstSegment.IsParallelAndTouching(secondSegment, out intersectionPoint))
{
ContactPoint cp = new ContactPoint();
cp.Position = new Vector3((float)intersectionPoint.X, (float)intersectionPoint.Y, 0);
cp.ThisIndex = firstSegmentIndex;
cp.OtherIndex = secondSegmentIndex;
cp.ThisEndpoint = -1;
cp.OtherEndpoint = -1;
if(intersectionPoint.EqualdWithin(firstSegment.Point1, allowedError))
{
cp.ThisEndpoint = firstSegmentIndex;
}
else if(intersectionPoint.EqualdWithin(firstSegment.Point2, allowedError))
{
cp.ThisEndpoint = firstSegmentIndex + 1;
}
else if(intersectionPoint.EqualdWithin(secondSegment.Point1, allowedError))
{
cp.OtherEndpoint = secondSegmentIndex;
}
else if(intersectionPoint.EqualdWithin(secondSegment.Point2, allowedError))
{
cp.OtherEndpoint = secondSegmentIndex + 1;
}
if (!intersects ||
cp.ThisEndpoint != -1 ||
cp.OtherEndpoint != -1
)
{
if (shouldEliminatePointOnEnd)
{
throw new Exception();
}
cp.ContactType = ContactType.PointOnSegment;
}
else
{
cp.ContactType = ContactType.SegmentIntersection;
}
if (!intersects)
{
indexesToRemove.Add(contactPoints.Count);
}
contactPoints.Add(cp);
}
}
}
#if false // Not sure why this is here, but it causes warnings and we don't use it
if (false && parallelContactPoints.Count == 2)
{
const float minimumLength = .01f;
bool isParallelContactPoint = false;
for (int i = 0; i < contactPoints.Count; i++)
{
float closestToPair = float.PositiveInfinity;
Vector3 position = contactPoints[i].Position;
for (int parallelIndex = 0; parallelIndex < parallelContactPoints.Count; parallelIndex++)
{
Segment segment1 = firstSegments[parallelContactPoints[parallelIndex].ThisIndex];
Segment segment2 = secondSegments[parallelContactPoints[parallelIndex].OtherIndex];
float firstDistance = segment1.DistanceTo(position.X, position.Y);
float secondDistance = segment2.DistanceTo(position.X, position.Y);
float furthestAway = System.Math.Max(firstDistance, secondDistance);
closestToPair = System.Math.Min(furthestAway, closestToPair);
}
if (closestToPair > minimumLength)
{
isParallelContactPoint = true;
break;
}
}
if (!isParallelContactPoint)
{
return parallelContactPoints;
}
}
#endif
int numberKilled = 0;
List<int> intersectionIndexes = new List<int>();
for (int i = 0; i < firstSegments.Length; i++)
{
intersectionIndexes.Clear();
for (int cpIndex = 0; cpIndex < contactPoints.Count; cpIndex++)
{
if (contactPoints[cpIndex].ThisIndex == i)
{
intersectionIndexes.Add(cpIndex);
}
}
if (intersectionIndexes.Count > 2 && intersectionIndexes.Count % 2 == 1)
{
contactPoints.RemoveAt(intersectionIndexes[1]);
}
}
for (int i = 0; i < secondSegments.Length; i++)
{
intersectionIndexes.Clear();
for (int cpIndex = 0; cpIndex < contactPoints.Count; cpIndex++)
{
if (contactPoints[cpIndex].OtherIndex == i)
{
intersectionIndexes.Add(cpIndex);
}
}
if (intersectionIndexes.Count > 2 && intersectionIndexes.Count % 2 == 1)
{
contactPoints.RemoveAt(intersectionIndexes[1]);
}
}
// See if any contact points are the same. If so, kill them
for (int firstCP = 0; firstCP < contactPoints.Count - 1; firstCP++)
{
for (int secondCP = 1; secondCP < contactPoints.Count; secondCP++)
{
if (firstCP == secondCP)
{
continue;
}
float distanceApart = (contactPoints[firstCP].Position - contactPoints[secondCP].Position).Length();
if (distanceApart < .002)
{
contactPoints.RemoveAt(secondCP);
firstCP = -1; // start over.
numberKilled++;
break;
}
}
}
if (contactPoints.Count == 1)
{
// Is this bad? What do we do?
//int m = 3;
}
if (contactPoints.Count % 2 != 0 && contactPoints.Count > 1)
{
// Oh, no, an odd number of contact points? Let's kill the closest pair
ReactToOddContactPointCount(contactPoints);
}
#endregion
return contactPoints;
}
private static void SetPointsFromContactPointsAndVertices(Polygon polygon, Polygon otherPolygon, List <ContactPoint> contactPoints, int polygonPointToStartAt, List <Vector3> thisVertices, List <Vector3> otherVertices)
{
Polygon currentPolygon = polygon;
List <Vector3> newPolygonPoints = new List <Vector3>();
int otherIndex = 0;
int otherIndexToStartAt = -1;
int thisIndex = polygonPointToStartAt;
while (true)
{
if (newPolygonPoints.Count > 3 * (polygon.Points.Count + otherPolygon.Points.Count))
{
// just break here for now
break;
}
bool isThereAContactPoint = false;
if (currentPolygon == polygon)
{
newPolygonPoints.Add(thisVertices[thisIndex]);
isThereAContactPoint = DoesContactPointsHaveThisIndex(thisIndex, contactPoints);
}
else
{
newPolygonPoints.Add(otherVertices[otherIndex]);
isThereAContactPoint = DoesContactPointsHaveOtherIndex(otherIndex, contactPoints);
}
if (isThereAContactPoint)
{
ContactPoint cp = new ContactPoint();
if (currentPolygon == polygon)
{
cp = GetContactPointAtThisIndex(thisIndex, contactPoints, thisVertices[thisIndex]);
}
else
{
cp = GetContactPointAtOtherIndex(otherIndex, contactPoints, otherVertices[otherIndex]);
}
#region If it's a segment intersection, drop the point and swap the current polygon
if (cp.ContactType == ContactType.SegmentIntersection)
{
if (currentPolygon == polygon)
{
currentPolygon = otherPolygon;
otherIndex = cp.OtherIndex + 1;
if (otherIndex == otherVertices.Count - 1)
{
otherIndex = 0;
}
if (otherIndex == otherIndexToStartAt)
{
newPolygonPoints.Add(newPolygonPoints[0]);
break;
}
if (otherIndexToStartAt == -1)
{
otherIndexToStartAt = otherIndex;
}
}
else
{
currentPolygon = polygon;
thisIndex = cp.ThisIndex + 1;
if (thisIndex == polygon.Points.Count - 1)
{
thisIndex = 0;
}
if (thisIndex == polygonPointToStartAt)
{
// close off the polygon and stop adding points
newPolygonPoints.Add(newPolygonPoints[0]);
break;
}
}
newPolygonPoints.Add(cp.Position);
}
#endregion
else if (cp.ContactType == ContactType.PointOnSegment)
{
// See which next point is the furthest away, and decide based off of that.
int nextOtherIndex = cp.OtherIndex + 1;
if (cp.OtherEndpoint != -1)
{
nextOtherIndex = cp.OtherEndpoint + 1;
}
if (nextOtherIndex >= otherVertices.Count - 1)
{
nextOtherIndex -= (otherVertices.Count - 1);
}
int nextThisIndex = cp.ThisIndex + 1;
if (cp.ThisEndpoint != -1)
{
nextThisIndex = cp.ThisEndpoint + 1;
}
if (nextThisIndex >= thisVertices.Count - 1)
{
nextThisIndex -= (thisVertices.Count - 1);
}
Vector3 nextThisVector = thisVertices[nextThisIndex] - cp.Position;
Vector3 nextOtherVector = otherVertices[nextOtherIndex] - cp.Position;
double distanceAwayFromThis = 0;
double distanceAwayFromOther = 0;
bool doWhile = true;
int numberOfExtraDoWhiles = 0;
int thisIndexBeforeDoWhile = nextThisIndex;
int otherIndexBeforeDoWhile = nextOtherIndex;
// The doWhile section will find out which path will take us
// furtest away from the other Polygon. This works most of the time
// but in some cases (like if the initial two paths are parallel), we'll
// want to special case which we use
bool forceUseThis = false;
bool forceUseOther = false;
while (doWhile)
{
if (nextThisVector.Length() < .0001f)
{
nextThisIndex++;
if (nextThisIndex == thisVertices.Count - 1)
{
nextThisIndex = 0;
}
nextThisVector = thisVertices[nextThisIndex] - cp.Position;
}
if (nextOtherVector.Length() < .0001f)
{
nextOtherIndex++;
if (nextOtherIndex == otherVertices.Count - 1)
{
nextOtherIndex = 0;
}
nextOtherVector = otherVertices[nextOtherIndex] - cp.Position;
}
float thisVectorLength = nextThisVector.Length();
float otherVectorLength = nextOtherVector.Length();
float smallestDistance = System.Math.Min(thisVectorLength, otherVectorLength);
nextThisVector.Normalize();
nextOtherVector.Normalize();
nextThisVector *= smallestDistance / 2.0f;
nextOtherVector *= smallestDistance / 2.0f;
nextThisVector += cp.Position;
nextOtherVector += cp.Position;
if (nextThisVector == nextOtherVector)
{
forceUseThis = thisVectorLength < otherVectorLength;
forceUseOther = !forceUseThis;
break;
}
double minimumDistance = .00001;
if (polygon.IsPointInside(ref nextOtherVector))
{
distanceAwayFromThis = 0;
}
else
{
distanceAwayFromThis = polygon.VectorFrom(nextOtherVector.X, nextOtherVector.Y).Length();
if (distanceAwayFromThis < minimumDistance)
{
distanceAwayFromThis = 0;
}
}
if (otherPolygon.IsPointInside(ref nextThisVector))
{
distanceAwayFromOther = 0;
}
else
{
distanceAwayFromOther = otherPolygon.VectorFrom(nextThisVector.X, nextThisVector.Y).Length();
if (distanceAwayFromOther < minimumDistance)
{
distanceAwayFromOther = 0;
}
}
if (distanceAwayFromOther == distanceAwayFromThis)
{
// We need a tiebreaker. Let's move an extra index and see what happens, shall we?
nextThisIndex++;
if (nextThisIndex == thisVertices.Count - 1)
{
nextThisIndex = 0;
}
nextThisVector = thisVertices[nextThisIndex] - cp.Position;
nextOtherIndex++;
if (nextOtherIndex == otherVertices.Count - 1)
{
nextOtherIndex = 0;
}
nextOtherVector = otherVertices[nextOtherIndex] - cp.Position;
numberOfExtraDoWhiles++;
}
else
{
doWhile = false;
}
}
nextThisIndex = thisIndexBeforeDoWhile;
nextOtherIndex = otherIndexBeforeDoWhile;
bool useThis = distanceAwayFromThis < distanceAwayFromOther;
if (forceUseThis)
{
useThis = true;
}
else if (forceUseOther)
{
useThis = false;
}
if (useThis && currentPolygon == polygon)
{
// make sure there are no other contact points on the current segment on this
for (int i = 0; i < contactPoints.Count; i++)
{
ContactPoint otherCp = contactPoints[i];
if (otherCp.Position != cp.Position && otherCp.ThisIndex == cp.ThisIndex)
{
useThis = false;
break;
}
}
}
else if (!useThis && currentPolygon == otherPolygon)
{
// make sure there are no other contact points on the current segment on this
for (int i = 0; i < contactPoints.Count; i++)
{
ContactPoint otherCp = contactPoints[i];
if (otherCp.Position != cp.Position && otherCp.OtherIndex == cp.OtherIndex)
{
useThis = true;
break;
}
}
}
newPolygonPoints.Add(cp.Position);
if (distanceAwayFromThis == distanceAwayFromOther)
{
useThis = currentPolygon == polygon;
}
if (useThis)
{
currentPolygon = polygon;
thisIndex = nextThisIndex;
if (thisIndex == polygonPointToStartAt)
{
// close off the polygon and stop adding points
newPolygonPoints.Add(newPolygonPoints[0]);
break;
}
}
else
{
currentPolygon = otherPolygon;
otherIndex = nextOtherIndex;
if (otherIndex == otherIndexToStartAt)
{
newPolygonPoints.Add(newPolygonPoints[0]);
break;
}
if (otherIndexToStartAt == -1)
{
otherIndexToStartAt = otherIndex;
}
}
}
}
else
{
if (currentPolygon == polygon)
{
thisIndex++;
if (thisIndex == polygon.Points.Count - 1)
{
thisIndex = 0;
}
if (thisIndex == polygonPointToStartAt)
{
// close off the polygon and stop adding points
newPolygonPoints.Add(newPolygonPoints[0]);
break;
}
}
else
{
otherIndex++;
if (otherIndex == otherPolygon.Points.Count - 1)
{
otherIndex = 0;
}
if (otherIndex == otherIndexToStartAt)
{
// close off the polygon and stop adding points
newPolygonPoints.Add(newPolygonPoints[0]);
break;
}
}
}
}
SetPolygonPoints(polygon, newPolygonPoints);
}
private static ContactPoint GetContactPointAtOtherIndex(int index, List<ContactPoint> contactPoints, Vector3 closestPoint)
{
ContactPoint closestContact = new ContactPoint();
float closestDistance = float.PositiveInfinity;
for (int i = 0; i < contactPoints.Count; i++)
{
if (contactPoints[i].OtherIndex == index)
{
float distanceSquared = (contactPoints[i].Position - closestPoint).LengthSquared();
if (distanceSquared < closestDistance)
{
closestContact = contactPoints[i];
closestDistance = distanceSquared;
}
}
}
if (float.IsPositiveInfinity(closestDistance))
{
throw new Exception();
}
else
{
return closestContact;
}
}
private static List <ContactPoint> GetContactPoints(Segment[] firstSegments, Segment[] secondSegments)
{
if (shouldEliminatePointOnEnd)
{
ShiftEndpointsToEliminatePointOnSegment(firstSegments, secondSegments);
}
Point intersectionPoint = new Point();
List <ContactPoint> contactPoints = new List <ContactPoint>();
Segment firstSegment = new Segment();
Segment secondSegment = new Segment();
const double allowedError = .0005;
bool debug = false;
#if !SILVERLIGHT && !MONOGAME && !XBOX360 && !WINDOWS_PHONE
if (debug)
{
FlatRedBall.Content.Polygon.PolygonSaveList psl = new FlatRedBall.Content.Polygon.PolygonSaveList();
psl.PolygonSaves.Add(
FlatRedBall.Content.Polygon.PolygonSave.FromPolygon(
Polygon.FromSegments(firstSegments)));
psl.PolygonSaves.Add(
FlatRedBall.Content.Polygon.PolygonSave.FromPolygon(
Polygon.FromSegments(secondSegments)));
psl.Save(FlatRedBall.IO.FileManager.MyDocuments + "modifiedPolygons.plylstx");
}
#endif
#region Handle the two-parallel segment special case
List <ContactPoint> parallelContactPoints = new List <ContactPoint>();
for (int firstIndex = 0; firstIndex < firstSegments.Length; firstIndex++)
{
firstSegment = firstSegments[firstIndex];
for (int secondIndex = 0; secondIndex < secondSegments.Length; secondIndex++)
{
secondSegment = secondSegments[secondIndex];
Point parallelIntersectionPoint;
if (firstSegment.IsParallelAndTouching(secondSegment, out parallelIntersectionPoint))
{
ContactPoint cp = new ContactPoint();
cp.ThisIndex = firstIndex;
cp.OtherIndex = secondIndex;
// Technically this is a point on segment intersection,
// but we treat it like a segment intersection because we're
// going to eliminate duplicates
cp.ContactType = ContactType.SegmentIntersection;
cp.Position = new Vector3((float)parallelIntersectionPoint.X, (float)parallelIntersectionPoint.Y, 0);
parallelContactPoints.Add(cp);
}
}
}
#endregion
#region else, do regular intersection tests
List <int> indexesToRemove = new List <int>();
for (int firstSegmentIndex = 0; firstSegmentIndex < firstSegments.Length; firstSegmentIndex++)
{
firstSegment = firstSegments[firstSegmentIndex];
for (int secondSegmentIndex = 0; secondSegmentIndex < secondSegments.Length; secondSegmentIndex++)
{
secondSegment = secondSegments[secondSegmentIndex];
bool intersects = firstSegment.Intersects(secondSegment, out intersectionPoint);
if (intersects ||
firstSegment.IsParallelAndTouching(secondSegment, out intersectionPoint))
{
ContactPoint cp = new ContactPoint();
cp.Position = new Vector3((float)intersectionPoint.X, (float)intersectionPoint.Y, 0);
cp.ThisIndex = firstSegmentIndex;
cp.OtherIndex = secondSegmentIndex;
cp.ThisEndpoint = -1;
cp.OtherEndpoint = -1;
if (intersectionPoint.EqualdWithin(firstSegment.Point1, allowedError))
{
cp.ThisEndpoint = firstSegmentIndex;
}
else if (intersectionPoint.EqualdWithin(firstSegment.Point2, allowedError))
{
cp.ThisEndpoint = firstSegmentIndex + 1;
}
else if (intersectionPoint.EqualdWithin(secondSegment.Point1, allowedError))
{
cp.OtherEndpoint = secondSegmentIndex;
}
else if (intersectionPoint.EqualdWithin(secondSegment.Point2, allowedError))
{
cp.OtherEndpoint = secondSegmentIndex + 1;
}
if (!intersects ||
cp.ThisEndpoint != -1 ||
cp.OtherEndpoint != -1
)
{
if (shouldEliminatePointOnEnd)
{
throw new Exception();
}
cp.ContactType = ContactType.PointOnSegment;
}
else
{
cp.ContactType = ContactType.SegmentIntersection;
}
if (!intersects)
{
indexesToRemove.Add(contactPoints.Count);
}
contactPoints.Add(cp);
}
}
}
#if false // Not sure why this is here, but it causes warnings and we don't use it
if (false && parallelContactPoints.Count == 2)
{
const float minimumLength = .01f;
bool isParallelContactPoint = false;
for (int i = 0; i < contactPoints.Count; i++)
{
float closestToPair = float.PositiveInfinity;
Vector3 position = contactPoints[i].Position;
for (int parallelIndex = 0; parallelIndex < parallelContactPoints.Count; parallelIndex++)
{
Segment segment1 = firstSegments[parallelContactPoints[parallelIndex].ThisIndex];
Segment segment2 = secondSegments[parallelContactPoints[parallelIndex].OtherIndex];
float firstDistance = segment1.DistanceTo(position.X, position.Y);
float secondDistance = segment2.DistanceTo(position.X, position.Y);
float furthestAway = System.Math.Max(firstDistance, secondDistance);
closestToPair = System.Math.Min(furthestAway, closestToPair);
}
if (closestToPair > minimumLength)
{
isParallelContactPoint = true;
break;
}
}
if (!isParallelContactPoint)
{
return(parallelContactPoints);
}
}
#endif
int numberKilled = 0;
List <int> intersectionIndexes = new List <int>();
for (int i = 0; i < firstSegments.Length; i++)
{
intersectionIndexes.Clear();
for (int cpIndex = 0; cpIndex < contactPoints.Count; cpIndex++)
{
if (contactPoints[cpIndex].ThisIndex == i)
{
intersectionIndexes.Add(cpIndex);
}
}
if (intersectionIndexes.Count > 2 && intersectionIndexes.Count % 2 == 1)
{
contactPoints.RemoveAt(intersectionIndexes[1]);
}
}
for (int i = 0; i < secondSegments.Length; i++)
{
intersectionIndexes.Clear();
for (int cpIndex = 0; cpIndex < contactPoints.Count; cpIndex++)
{
if (contactPoints[cpIndex].OtherIndex == i)
{
intersectionIndexes.Add(cpIndex);
}
}
if (intersectionIndexes.Count > 2 && intersectionIndexes.Count % 2 == 1)
{
contactPoints.RemoveAt(intersectionIndexes[1]);
}
}
// See if any contact points are the same. If so, kill them
for (int firstCP = 0; firstCP < contactPoints.Count - 1; firstCP++)
{
for (int secondCP = 1; secondCP < contactPoints.Count; secondCP++)
{
if (firstCP == secondCP)
{
continue;
}
float distanceApart = (contactPoints[firstCP].Position - contactPoints[secondCP].Position).Length();
if (distanceApart < .002)
{
contactPoints.RemoveAt(secondCP);
firstCP = -1; // start over.
numberKilled++;
break;
}
}
}
if (contactPoints.Count == 1)
{
// Is this bad? What do we do?
//int m = 3;
}
if (contactPoints.Count % 2 != 0 && contactPoints.Count > 1)
{
// Oh, no, an odd number of contact points? Let's kill the closest pair
ReactToOddContactPointCount(contactPoints);
}
#endregion
return(contactPoints);
}