public static bool IsInLineOfSight(Vector3 position1, Vector3 position2, float collisionThreshold, ShapeCollection shapeCollection)
{
Segment segment = new Segment(new FlatRedBall.Math.Geometry.Point(ref position1),
new FlatRedBall.Math.Geometry.Point(ref position2));
for(int i = 0; i < shapeCollection.Polygons.Count; i++)
{
Polygon polygon = shapeCollection.Polygons[i];
if (polygon.CollideAgainst(segment) ||
(collisionThreshold > 0 && segment.DistanceTo(polygon) < collisionThreshold))
{
return false;
}
}
for (int i = 0; i < shapeCollection.AxisAlignedRectangles.Count; i++)
{
AxisAlignedRectangle rectangle = shapeCollection.AxisAlignedRectangles[i];
FlatRedBall.Math.Geometry.Point throwaway;
if (rectangle.Intersects(segment, out throwaway) ||
(collisionThreshold > 0 && segment.DistanceTo(rectangle) < collisionThreshold))
{
return false;
}
}
for (int i = 0; i < shapeCollection.Circles.Count; i++)
{
Circle circle = shapeCollection.Circles[i];
if (segment.DistanceTo(circle) < collisionThreshold)
{
return false;
}
}
#if DEBUG
if (shapeCollection.Capsule2Ds.Count != 0)
{
throw new Exception("IsInLineOfSight does not support ShapeCollections with Capsule2Ds");
}
#endif
for (int i = 0; i < shapeCollection.Lines.Count; i++)
{
Line line = shapeCollection.Lines[i];
if (segment.DistanceTo(line.AsSegment()) < collisionThreshold)
{
return false;
}
}
return true;
}
/// <summary>
/// Tests if two vector positions are within line of sight given a collision map.
/// </summary>
/// <param name="position1">The first world-coordinate position.</param>
/// <param name="position2">The second world-coordinate position.</param>
/// <param name="collisionThreshold">Distance from position2 to the polygon it's colliding against.
/// If a polygon is within this threshold, this will return false.</param>
/// <param name="collisionMap">The list of polygons used to test if the two positions are within line of sight.</param>
/// <returns></returns>
#endregion
public static bool IsInLineOfSight(Vector3 position1, Vector3 position2, float collisionThreshold, PositionedObjectList<Polygon> collisionMap)
{
Segment segment = new Segment(new FlatRedBall.Math.Geometry.Point(ref position1),
new FlatRedBall.Math.Geometry.Point(ref position2));
foreach (Polygon polygon in collisionMap)
{
if (polygon.CollideAgainst(segment) ||
(collisionThreshold > 0 && segment.DistanceTo(polygon) < collisionThreshold))
{
return false;
}
}
return true;
}
private static List<ContactPoint> GetContactPoints(Polygon firstPolygon, Polygon secondPolygon,
out Segment[] firstSegments, out Segment[] secondSegments)
{
firstSegments = GetSegments(firstPolygon);
secondSegments = GetSegments(secondPolygon);
List<ContactPoint> contactPoints = GetContactPoints(firstSegments, secondSegments);
return contactPoints;
}
private static Vector3 GetShift(Segment[] segmentsTestingAgainst, int index)
{
Segment secondSegment = segmentsTestingAgainst[index];
Vector3 secondAsVector = secondSegment.ToVector3();
secondAsVector.Normalize();
float temp = secondAsVector.X;
secondAsVector.X = -secondAsVector.Y;
secondAsVector.Y = temp;
secondAsVector *= ShiftAmount;
return secondAsVector;
}
private static void ShiftEndpointsToEliminatePointOnSegment(Segment[] firstSegments, Segment[] secondSegments)
{
const double error = .1f ;
for (int i = 0; i < firstSegments.Length; i++)
{
for (int j = 0; j < secondSegments.Length; j++)
{
Point firstPoint = firstSegments[i].Point1;
Point secondPoint = firstSegments[i].Point2;
if (secondSegments[j].DistanceTo(firstPoint) < error)
{
Vector3 shiftVector = GetShift(secondSegments, j);
Segment segment = firstSegments[i];
segment.Point1.X += shiftVector.X;
segment.Point1.Y += shiftVector.Y;
firstSegments[i] = segment;
int index = i - 1;
if (i == 0)
{
index = firstSegments.Length - 1;
}
segment = firstSegments[index];
segment.Point2 = firstSegments[i].Point1;
firstSegments[index] = segment;
}
if (secondSegments[j].DistanceTo(secondPoint) < error)
{
Vector3 shiftVector = GetShift(secondSegments, j);
Segment segment = firstSegments[i];
segment.Point2.X += shiftVector.X;
segment.Point2.Y += shiftVector.Y;
firstSegments[i] = segment;
int index = i + 1;
if (i == firstSegments.Length - 1)
{
index = 0;
}
segment = firstSegments[index];
segment.Point1 = firstSegments[i].Point2;
firstSegments[index] = segment;
}
firstPoint = secondSegments[j].Point1;
secondPoint = secondSegments[j].Point2;
if (firstSegments[i].DistanceTo(firstPoint) < error)
{
Vector3 shiftVector = GetShift(firstSegments, i);
Segment segment = secondSegments[j];
segment.Point1.X += shiftVector.X;
segment.Point1.Y += shiftVector.Y;
secondSegments[j] = segment;
int index = j - 1;
if (j == 0)
{
index = secondSegments.Length - 1;
}
segment = secondSegments[index];
segment.Point2 = secondSegments[j].Point1;
secondSegments[index] = segment;
}
if (firstSegments[i].DistanceTo(secondPoint) < error)
{
Vector3 shiftVector = GetShift(firstSegments, i);
Segment segment = secondSegments[j];
segment.Point2.X += shiftVector.X;
segment.Point2.Y += shiftVector.Y;
secondSegments[j] = segment;
int index = j + 1;
if (j == secondSegments.Length - 1)
{
index = 0;
}
segment = secondSegments[index];
segment.Point1 = secondSegments[j].Point2;
secondSegments[index] = segment;
}
}
}
}
private static List<Vector3> GetAbsoluteVertices(Segment[] segments)
{
List<Vector3> thisVertices = new List<Vector3>(segments.Length + 1);
thisVertices.Add(new Vector3((float)segments[0].Point1.X, (float)segments[0].Point1.Y, 0));
for (int i = 0; i < segments.Length; i++)
{
thisVertices.Add(new Vector3((float)segments[i].Point2.X, (float)segments[i].Point2.Y, 0));
}
return thisVertices;
}
/// <summary>
/// Returns the point where this segment intersects the argument segment.
/// </summary>
/// <param name="s2">The segment to test for intersection.</param>
/// <returns>The point where this segment intersects the
/// argument segment. If the two segments do not touch, the point
/// will have both values be double.NAN.
/// </returns>
#endregion
public void IntersectionPoint(ref Segment s2, out Point intersectionPoint)
{
// code borrowed from
// http://www.topcoder.com/tc?module=Static&d1=tutorials&d2=geometry2
double A1 = Point2.Y - Point1.Y;
double B1 = Point1.X - Point2.X;
double C1 = A1 * Point1.X + B1 * Point1.Y;
double A2 = s2.Point2.Y - s2.Point1.Y;
double B2 = s2.Point1.X - s2.Point2.X;
double C2 = A2 * s2.Point1.X + B2 * s2.Point1.Y;
double det = A1 * B2 - A2 * B1;
if (det == 0)
{
//Lines are parallel
intersectionPoint.X = double.NaN;
intersectionPoint.Y = double.NaN;
}
else
{
intersectionPoint.X = (B2 * C1 - B1 * C2) / det;
intersectionPoint.Y = (A1 * C2 - A2 * C1) / det;
if (!this.IsClosestPointOnEndpoint(ref intersectionPoint) &&
!s2.IsClosestPointOnEndpoint(ref intersectionPoint))
{
// do nothing
;
}
else
{
// The closest point is on an endpoint, but we may have a situation where
// one segment is touching another one like a T. If that's the case,
// let's still consider it an intersection.
double distanceFromThis = this.DistanceTo(intersectionPoint);
double distanceFromOther = s2.DistanceTo(intersectionPoint);
if (distanceFromOther > .000000001 ||
distanceFromThis > .000000001)
{
intersectionPoint.X = float.NaN;
intersectionPoint.Y = float.NaN;
}
}
}
}
private static Segment[] GetSegments(AxisAlignedRectangle rectangle)
{
Segment[] rectangleSegments = new Segment[4];
rectangleSegments[0] = new Segment(rectangle.Left, rectangle.Top, rectangle.Right, rectangle.Top); // top
rectangleSegments[1] = new Segment(rectangle.Right, rectangle.Top, rectangle.Right, rectangle.Bottom); // right
rectangleSegments[2] = new Segment(rectangle.Right, rectangle.Bottom, rectangle.Left, rectangle.Bottom); // bottom
rectangleSegments[3] = new Segment(rectangle.Left, rectangle.Bottom, rectangle.Left, rectangle.Top); // left
return rectangleSegments;
}
public float DistanceToSquared(ref Vector3 vector, out Segment connectingSegment)
{
float segmentLength = (float)this.GetLength();
Vector2 normalizedLine = new Vector2(
(float)(Point2.X - Point1.X) / segmentLength,
(float)(Point2.Y - Point1.Y) / segmentLength);
Vector2 pointVector = new Vector2((float)(vector.X - Point1.X), (float)(vector.Y - Point1.Y));
float length = Vector2.Dot(pointVector, normalizedLine);
if (length < 0)
{
connectingSegment.Point1 = new Point(ref vector);
connectingSegment.Point2 = Point1;
return (float) connectingSegment.GetLengthSquared();
}
else if (length > segmentLength)
{
connectingSegment.Point1 = new Point(ref vector);
connectingSegment.Point2 = Point2;
return (float) connectingSegment.GetLengthSquared();
}
else
{
connectingSegment.Point1 = new Point(ref vector);
connectingSegment.Point2 = new Point(Point1.X + length * normalizedLine.X,
Point1.Y + length * normalizedLine.Y);
return (float)connectingSegment.GetLengthSquared();
}
}
public float DistanceTo(Segment otherSegment)
{
if (otherSegment.Intersects(this))
{
return 0;
}
else
{
float minDistance = float.PositiveInfinity;
minDistance = System.Math.Min(minDistance, this.DistanceTo(otherSegment.Point1));
minDistance = System.Math.Min(minDistance, this.DistanceTo(otherSegment.Point2));
minDistance = System.Math.Min(minDistance, otherSegment.DistanceTo(this.Point1));
minDistance = System.Math.Min(minDistance, otherSegment.DistanceTo(this.Point2));
return minDistance;
}
}
public float DistanceTo(Point point, out Segment connectingSegment)
{
double segmentLength = GetLength();
Point normalizedLine = new Point(
(Point2.X - Point1.X) / segmentLength,
(Point2.Y - Point1.Y) / segmentLength);
Point pointVector = new Point(point.X - Point1.X, point.Y - Point1.Y);
double length = Point.Dot(pointVector, normalizedLine);
connectingSegment.Point1 = point;
if (length < 0)
{
connectingSegment.Point2 = Point1;
}
else if (length > segmentLength)
{
connectingSegment.Point2 = Point2;
}
else
{
connectingSegment.Point2 = new Point(Point1.X + length * normalizedLine.X,
Point1.Y + length * normalizedLine.Y);
}
return (float)connectingSegment.GetLength();
}
public float DistanceTo(AxisAlignedRectangle rectangle)
{
float finalDistance = float.MaxValue;
finalDistance = System.Math.Min(finalDistance, DistanceTo(rectangle.Left, rectangle.Top));
finalDistance = System.Math.Min(finalDistance, DistanceTo(rectangle.Left, rectangle.Bottom));
finalDistance = System.Math.Min(finalDistance, DistanceTo(rectangle.Right, rectangle.Top));
finalDistance = System.Math.Min(finalDistance, DistanceTo(rectangle.Right, rectangle.Bottom));
Segment rectSegment;
rectSegment = new Segment(rectangle.Left, rectangle.Top, rectangle.Left, rectangle.Bottom);
finalDistance = System.Math.Min(finalDistance, DistanceTo(rectSegment));
rectSegment = new Segment(rectangle.Left, rectangle.Top, rectangle.Right, rectangle.Top);
finalDistance = System.Math.Min(finalDistance, DistanceTo(rectSegment));
rectSegment = new Segment(rectangle.Right, rectangle.Top, rectangle.Right, rectangle.Bottom);
finalDistance = System.Math.Min(finalDistance, DistanceTo(rectSegment));
rectSegment = new Segment(rectangle.Left, rectangle.Bottom, rectangle.Right, rectangle.Bottom);
finalDistance = System.Math.Min(finalDistance, DistanceTo(rectSegment));
return finalDistance;
}
void GetSegmentsAboveAndBelow(int index, out Segment aboveSegment, out Segment belowSegment)
{
Segment segment = GetHorizontalSegmentAt(index);
segment.ScaleBy(2); // so they intersect
Vector3 normalVector = segment.Point2.ToVector3() - segment.Point1.ToVector3();
normalVector = new Vector3(-normalVector.Y, normalVector.X, 0);
normalVector.Normalize();
normalVector = normalVector * Width / 2.0f;
aboveSegment = segment;
aboveSegment.MoveBy(normalVector.X, normalVector.Y);
belowSegment = segment;
belowSegment.MoveBy(-normalVector.X, -normalVector.Y);
}
Segment GetHorizontalSegmentAt(int index)
{
Vector3 pointBefore = this.mPoints[index];
Vector3 pointAfter = this.mPoints[index + 1];
Segment connectingSegment = new Segment(pointBefore, pointAfter);
return connectingSegment;
}
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 Segment[] GetSegments(Polygon polygon)
{
polygon.FillVertexArray();
Segment polygonSegment;
Segment[] segments = new Segment[polygon.Points.Count - 1];
for (int i = 0; i < polygon.Points.Count - 1; i++)
{
Vector3 vectorAtI = polygon.AbsolutePointPosition(i);
polygonSegment = new Segment(vectorAtI, polygon.AbsolutePointPosition(i + 1));
segments[i] = polygonSegment;
}
return segments;
}
public bool IsParallelAndTouching(Segment s2, out Point intersectionPoint)
{
double thisAngle = this.Angle;
double otherAngle = s2.Angle;
double distance = this.DistanceTo(s2);
const float maximumAngleVariation = .00001f;
const double maximumDistance = .00001f;
intersectionPoint = new Point(double.NaN, double.NaN);
if (System.Math.Abs(thisAngle - otherAngle) < maximumAngleVariation &&
distance < maximumDistance)
{
if (s2.DistanceTo(this.Point1) < maximumDistance)
{
intersectionPoint = Point1;
}
else if (s2.DistanceTo(this.Point2) < maximumDistance)
{
intersectionPoint = Point2;
}
else if (this.DistanceTo(s2.Point1) < maximumDistance)
{
intersectionPoint = s2.Point1;
}
else// if (this.DistanceTo(s2.Point2) < maximumDistance)
{
intersectionPoint = s2.Point2;
}
// They're parallel and touching
return true;
}
return false;
}
private void GetLinkOver(ref Link linkOver, ref PositionedNode linkOverParent)
{
Cursor cursor = GuiManager.Cursor;
float worldX;
float worldY;
if (EditorData.NodeNetwork.Nodes.Count != 0)
{
float tolerance = 5 / SpriteManager.Camera.PixelsPerUnitAt(0);
for (int i = 0; i < EditorData.NodeNetwork.Nodes.Count; i++)
{
PositionedNode node = EditorData.NodeNetwork.Nodes[i];
worldX = cursor.WorldXAt(node.Z);
worldY = cursor.WorldYAt(node.Z);
for (int linkIndex = 0; linkIndex < node.Links.Count; linkIndex++)
{
Segment segment = new Segment(
node.Position, node.Links[linkIndex].NodeLinkingTo.Position);
float distance = segment.DistanceTo(worldX, worldY);
if (distance < tolerance)
{
linkOverParent = node;
linkOver = node.Links[linkIndex];
return;
}
}
}
}
linkOverParent = null;
linkOver = null;
}
public bool Intersects(Segment s2, out Point intersectionPoint)
{
IntersectionPoint(ref s2, out intersectionPoint);
return !double.IsNaN(intersectionPoint.X);
}
