using
System;
using
System.Collections.Generic;
using
System.Diagnostics;
using
System.Text;
using
FarseerPhysics.Collision;
using
Microsoft.Xna.Framework;
namespace
FarseerPhysics.Common
{
#if !(XBOX360)
[DebuggerDisplay(
"Count = {Count} Vertices = {ToString()}"
)]
#endif
public
class
Vertices : List<Vector2>
{
public
Vertices()
{
}
public
Vertices(
int
capacity)
{
Capacity = capacity;
}
public
Vertices(Vector2[] vector2)
{
for
(
int
i = 0; i < vector2.Length; i++)
{
Add(vector2[i]);
}
}
public
Vertices(IList<Vector2> vertices)
{
for
(
int
i = 0; i < vertices.Count; i++)
{
Add(vertices[i]);
}
}
/// <summary>
/// Nexts the index.
/// </summary>
/// <param name="index">The index.</param>
/// <returns></returns>
public
int
NextIndex(
int
index)
{
if
(index == Count - 1)
{
return
0;
}
return
index + 1;
}
public
Vector2 NextVertex(
int
index)
{
return
this
[NextIndex(index)];
}
/// <summary>
/// Gets the previous index.
/// </summary>
/// <param name="index">The index.</param>
/// <returns></returns>
public
int
PreviousIndex(
int
index)
{
if
(index == 0)
{
return
Count - 1;
}
return
index - 1;
}
public
Vector2 PreviousVertex(
int
index)
{
return
this
[PreviousIndex(index)];
}
/// <summary>
/// Gets the signed area.
/// </summary>
/// <returns></returns>
public
float
GetSignedArea()
{
int
i;
float
area = 0;
for
(i = 0; i < Count; i++)
{
int
j = (i + 1) % Count;
area +=
this
[i].X *
this
[j].Y;
area -=
this
[i].Y *
this
[j].X;
}
area /= 2.0f;
return
area;
}
/// <summary>
/// Gets the area.
/// </summary>
/// <returns></returns>
public
float
GetArea()
{
int
i;
float
area = 0;
for
(i = 0; i < Count; i++)
{
int
j = (i + 1) % Count;
area +=
this
[i].X *
this
[j].Y;
area -=
this
[i].Y *
this
[j].X;
}
area /= 2.0f;
return
(area < 0 ? -area : area);
}
/// <summary>
/// Gets the centroid.
/// </summary>
/// <returns></returns>
public
Vector2 GetCentroid()
{
Vector2 c = Vector2.Zero;
float
area = 0.0f;
const
float
inv3 = 1.0f / 3.0f;
Vector2 pRef = Vector2.Zero;
for
(
int
i = 0; i < Count; ++i)
{
Vector2 p1 = pRef;
Vector2 p2 =
this
[i];
Vector2 p3 = i + 1 < Count ?
this
[i + 1] :
this
[0];
Vector2 e1 = p2 - p1;
Vector2 e2 = p3 - p1;
float
D = MathUtils.Cross(e1, e2);
float
triangleArea = 0.5f * D;
area += triangleArea;
c += triangleArea * inv3 * (p1 + p2 + p3);
}
c *= 1.0f / area;
return
c;
}
/// <summary>
/// Gets the radius based on area.
/// </summary>
/// <returns></returns>
public
float
GetRadius()
{
float
area = GetSignedArea();
double
radiusSqrd = (
double
)area / MathHelper.Pi;
if
(radiusSqrd < 0)
{
radiusSqrd *= -1;
}
return
(
float
)Math.Sqrt(radiusSqrd);
}
/// <summary>
/// Returns an AABB for vertex.
/// </summary>
/// <returns></returns>
public
AABB GetCollisionBox()
{
AABB aabb;
Vector2 lowerBound =
new
Vector2(
float
.MaxValue,
float
.MaxValue);
Vector2 upperBound =
new
Vector2(
float
.MinValue,
float
.MinValue);
for
(
int
i = 0; i < Count; ++i)
{
if
(
this
[i].X < lowerBound.X)
{
lowerBound.X =
this
[i].X;
}
if
(
this
[i].X > upperBound.X)
{
upperBound.X =
this
[i].X;
}
if
(
this
[i].Y < lowerBound.Y)
{
lowerBound.Y =
this
[i].Y;
}
if
(
this
[i].Y > upperBound.Y)
{
upperBound.Y =
this
[i].Y;
}
}
aabb.LowerBound = lowerBound;
aabb.UpperBound = upperBound;
return
aabb;
}
public
void
Translate(Vector2 vector)
{
Translate(
ref
vector);
}
/// <summary>
/// Translates the vertices with the specified vector.
/// </summary>
/// <param name="vector">The vector.</param>
public
void
Translate(
ref
Vector2 vector)
{
for
(
int
i = 0; i < Count; i++)
this
[i] = Vector2.Add(
this
[i], vector);
}
/// <summary>
/// Scales the vertices with the specified vector.
/// </summary>
/// <param name="value">The Value.</param>
public
void
Scale(
ref
Vector2 value)
{
for
(
int
i = 0; i < Count; i++)
this
[i] = Vector2.Multiply(
this
[i], value);
}
/// <summary>
/// Rotate the vertices with the defined value in radians.
/// </summary>
/// <param name="value">The amount to rotate by in radians.</param>
public
void
Rotate(
float
value)
{
Matrix rotationMatrix;
Matrix.CreateRotationZ(value,
out
rotationMatrix);
for
(
int
i = 0; i < Count; i++)
this
[i] = Vector2.Transform(
this
[i], rotationMatrix);
}
/// <summary>
/// Assuming the polygon is simple; determines whether the polygon is convex.
/// NOTE: It will also return false if the input contains colinear edges.
/// </summary>
/// <returns>
/// <c>true</c> if it is convex; otherwise, <c>false</c>.
/// </returns>
public
bool
IsConvex()
{
for
(
int
i = 0; i < Count; ++i)
{
int
i1 = i;
int
i2 = i + 1 < Count ? i + 1 : 0;
Vector2 edge =
this
[i2] -
this
[i1];
for
(
int
j = 0; j < Count; ++j)
{
if
(j == i1 || j == i2)
{
continue
;
}
Vector2 r =
this
[j] -
this
[i1];
float
s = edge.X * r.Y - edge.Y * r.X;
if
(s <= 0.0f)
return
false
;
}
}
return
true
;
}
public
bool
IsCounterClockWise()
{
if
(Count < 3)
return
true
;
return
(GetSignedArea() > 0.0f);
}
/// <summary>
/// Forces counter clock wise order.
/// </summary>
public
void
ForceCounterClockWise()
{
if
(!IsCounterClockWise())
{
Reverse();
}
}
/// <summary>
/// Check for edge crossings
/// </summary>
/// <returns></returns>
public
bool
IsSimple()
{
for
(
int
i = 0; i < Count; ++i)
{
int
iplus = (i + 1 > Count - 1) ? 0 : i + 1;
Vector2 a1 =
new
Vector2(
this
[i].X,
this
[i].Y);
Vector2 a2 =
new
Vector2(
this
[iplus].X,
this
[iplus].Y);
for
(
int
j = i + 1; j < Count; ++j)
{
int
jplus = (j + 1 > Count - 1) ? 0 : j + 1;
Vector2 b1 =
new
Vector2(
this
[j].X,
this
[j].Y);
Vector2 b2 =
new
Vector2(
this
[jplus].X,
this
[jplus].Y);
Vector2 temp;
if
(LineTools.LineIntersect2(a1, a2, b1, b2,
out
temp))
{
return
false
;
}
}
}
return
true
;
}
public
bool
IsSimple2()
{
for
(
int
i = 0; i < Count; ++i)
{
if
(i < Count - 1)
{
for
(
int
h = i + 1; h < Count; ++h)
{
if
(
this
[i] ==
this
[h])
{
return
true
;
}
}
}
int
j = (i + 1) % Count;
Vector2 iToj =
this
[j] -
this
[i];
Vector2 iTojNormal =
new
Vector2(iToj.Y, -iToj.X);
int
startK = (j + 1) % Count;
int
endK = (i - 1 + Count) % Count;
endK += startK < endK ? 0 : startK + 1;
int
k = startK;
Vector2 iTok =
this
[k] -
this
[i];
bool
onLeftSide = Vector2.Dot(iTok, iTojNormal) >= 0;
Vector2 prevK =
this
[k];
++k;
for
(; k <= endK; ++k)
{
int
modK = k % Count;
iTok =
this
[modK] -
this
[i];
if
(onLeftSide != Vector2.Dot(iTok, iTojNormal) >= 0)
{
Vector2 prevKtoK =
this
[modK] - prevK;
Vector2 prevKtoKNormal =
new
Vector2(prevKtoK.Y, -prevKtoK.X);
if
((Vector2.Dot(
this
[i] - prevK, prevKtoKNormal) >= 0) !=
(Vector2.Dot(
this
[j] - prevK, prevKtoKNormal) >= 0))
{
return
true
;
}
}
onLeftSide = Vector2.Dot(iTok, iTojNormal) > 0;
prevK =
this
[modK];
}
}
return
false
;
}
/// <summary>
/// Checks if polygon is valid for use in Box2d engine.
/// Last ditch effort to ensure no invalid polygons are
/// added to world geometry.
///
/// Performs a full check, for simplicity, convexity,
/// orientation, minimum angle, and volume. This won't
/// be very efficient, and a lot of it is redundant when
/// other tools in this section are used.
/// </summary>
/// <returns></returns>
public
bool
CheckPolygon()
{
int
error = -1;
if
(Count < 3 || Count > Settings.MaxPolygonVertices)
{
error = 0;
}
if
(!IsConvex())
{
error = 1;
}
if
(!IsSimple())
{
error = 2;
}
if
(GetArea() < Settings.Epsilon)
{
error = 3;
}
Vector2[] normals =
new
Vector2[Count];
Vertices vertices =
new
Vertices(Count);
for
(
int
i = 0; i < Count; ++i)
{
vertices.Add(
new
Vector2(
this
[i].X,
this
[i].Y));
int
i1 = i;
int
i2 = i + 1 < Count ? i + 1 : 0;
Vector2 edge =
new
Vector2(
this
[i2].X -
this
[i1].X,
this
[i2].Y -
this
[i1].Y);
normals[i] = MathUtils.Cross(edge, 1.0f);
normals[i].Normalize();
}
for
(
int
i = 0; i < Count; ++i)
{
int
iminus = (i == 0) ? Count - 1 : i - 1;
float
cross = MathUtils.Cross(normals[iminus], normals[i]);
cross = MathUtils.Clamp(cross, -1.0f, 1.0f);
float
angle = (
float
)Math.Asin(cross);
if
(angle <= Settings.AngularSlop)
{
error = 4;
break
;
}
for
(
int
j = 0; j < Count; ++j)
{
if
(j == i || j == (i + 1) % Count)
{
continue
;
}
float
s = Vector2.Dot(normals[i], vertices[j] - vertices[i]);
if
(s >= -Settings.LinearSlop)
{
error = 5;
}
}
Vector2 centroid = vertices.GetCentroid();
Vector2 n1 = normals[iminus];
Vector2 n2 = normals[i];
Vector2 v = vertices[i] - centroid;
Vector2 d =
new
Vector2();
d.X = Vector2.Dot(n1, v);
d.Y = Vector2.Dot(n2, v);
if
((d.X < 0.0f) || (d.Y < 0.0f))
{
error = 6;
}
}
if
(error != -1)
{
Debug.WriteLine(
"Found invalid polygon, "
);
switch
(error)
{
case
0:
Debug.WriteLine(
string
.Format(
"must have between 3 and {0} vertices.\n"
,
Settings.MaxPolygonVertices));
break
;
case
1:
Debug.WriteLine(
"must be convex.\n"
);
break
;
case
2:
Debug.WriteLine(
"must be simple (cannot intersect itself).\n"
);
break
;
case
3:
Debug.WriteLine(
"area is too small.\n"
);
break
;
case
4:
Debug.WriteLine(
"sides are too close to parallel.\n"
);
break
;
case
5:
Debug.WriteLine(
"polygon is too thin.\n"
);
break
;
case
6:
Debug.WriteLine(
"core shape generation would move edge past centroid (too thin).\n"
);
break
;
default
:
Debug.WriteLine(
"don't know why.\n"
);
break
;
}
}
return
error != -1;
}
/// <summary>
/// Trace the edge of a non-simple polygon and return a simple polygon.
///
/// Method:
/// Start at vertex with minimum y (pick maximum x one if there are two).
/// We aim our "lastDir" vector at (1.0, 0)
/// We look at the two rays going off from our start vertex, and follow whichever
/// has the smallest angle (in -Pi . Pi) wrt lastDir ("rightest" turn)
/// Loop until we hit starting vertex:
/// We add our current vertex to the list.
/// We check the seg from current vertex to next vertex for intersections
/// - if no intersections, follow to next vertex and continue
/// - if intersections, pick one with minimum distance
/// - if more than one, pick one with "rightest" next point (two possibilities for each)
/// </summary>
/// <param name="verts">The vertices.</param>
/// <returns></returns>
public
Vertices TraceEdge(Vertices verts)
{
PolyNode[] nodes =
new
PolyNode[verts.Count * verts.Count];
int
nNodes = 0;
for
(
int
i = 0; i < verts.Count; ++i)
{
Vector2 pos =
new
Vector2(verts[i].X, verts[i].Y);
nodes[i].Position = pos;
++nNodes;
int
iplus = (i == verts.Count - 1) ? 0 : i + 1;
int
iminus = (i == 0) ? verts.Count - 1 : i - 1;
nodes[i].AddConnection(nodes[iplus]);
nodes[i].AddConnection(nodes[iminus]);
}
bool
dirty =
true
;
int
counter = 0;
while
(dirty)
{
dirty =
false
;
for
(
int
i = 0; i < nNodes; ++i)
{
for
(
int
j = 0; j < nodes[i].NConnected; ++j)
{
for
(
int
k = 0; k < nNodes; ++k)
{
if
(k == i || nodes[k] == nodes[i].Connected[j])
continue
;
for
(
int
l = 0; l < nodes[k].NConnected; ++l)
{
if
(nodes[k].Connected[l] == nodes[i].Connected[j] ||
nodes[k].Connected[l] == nodes[i])
continue
;
Vector2 intersectPt;
bool
crosses = LineTools.LineIntersect(nodes[i].Position, nodes[i].Connected[j].Position,
nodes[k].Position, nodes[k].Connected[l].Position,
out
intersectPt);
if
(crosses)
{
dirty =
true
;
PolyNode connj = nodes[i].Connected[j];
PolyNode connl = nodes[k].Connected[l];
nodes[i].Connected[j].RemoveConnection(nodes[i]);
nodes[i].RemoveConnection(connj);
nodes[k].Connected[l].RemoveConnection(nodes[k]);
nodes[k].RemoveConnection(connl);
nodes[nNodes] =
new
PolyNode(intersectPt);
nodes[nNodes].AddConnection(nodes[i]);
nodes[i].AddConnection(nodes[nNodes]);
nodes[nNodes].AddConnection(nodes[k]);
nodes[k].AddConnection(nodes[nNodes]);
nodes[nNodes].AddConnection(connj);
connj.AddConnection(nodes[nNodes]);
nodes[nNodes].AddConnection(connl);
connl.AddConnection(nodes[nNodes]);
++nNodes;
goto
SkipOut;
}
}
}
}
}
SkipOut:
++counter;
}
bool
foundDupe =
true
;
int
nActive = nNodes;
while
(foundDupe)
{
foundDupe =
false
;
for
(
int
i = 0; i < nNodes; ++i)
{
if
(nodes[i].NConnected == 0)
continue
;
for
(
int
j = i + 1; j < nNodes; ++j)
{
if
(nodes[j].NConnected == 0)
continue
;
Vector2 diff = nodes[i].Position - nodes[j].Position;
if
(diff.LengthSquared() <= Settings.Epsilon * Settings.Epsilon)
{
if
(nActive <= 3)
return
new
Vertices();
--nActive;
foundDupe =
true
;
PolyNode inode = nodes[i];
PolyNode jnode = nodes[j];
int
njConn = jnode.NConnected;
for
(
int
k = 0; k < njConn; ++k)
{
PolyNode knode = jnode.Connected[k];
Debug.Assert(knode != jnode);
if
(knode != inode)
{
inode.AddConnection(knode);
knode.AddConnection(inode);
}
knode.RemoveConnection(jnode);
}
jnode.NConnected = 0;
}
}
}
}
float
minY =
float
.MaxValue;
float
maxX = -
float
.MaxValue;
int
minYIndex = -1;
for
(
int
i = 0; i < nNodes; ++i)
{
if
(nodes[i].Position.Y < minY && nodes[i].NConnected > 1)
{
minY = nodes[i].Position.Y;
minYIndex = i;
maxX = nodes[i].Position.X;
}
else
if
(nodes[i].Position.Y == minY && nodes[i].Position.X > maxX && nodes[i].NConnected > 1)
{
minYIndex = i;
maxX = nodes[i].Position.X;
}
}
Vector2 origDir =
new
Vector2(1.0f, 0.0f);
Vector2[] resultVecs =
new
Vector2[4 * nNodes];
int
nResultVecs = 0;
PolyNode currentNode = nodes[minYIndex];
PolyNode startNode = currentNode;
Debug.Assert(currentNode.NConnected > 0);
PolyNode nextNode = currentNode.GetRightestConnection(origDir);
if
(nextNode ==
null
)
{
Vertices vertices =
new
Vertices(nResultVecs);
for
(
int
i = 0; i < nResultVecs; ++i)
{
vertices.Add(resultVecs[i]);
}
return
vertices;
}
resultVecs[0] = startNode.Position;
++nResultVecs;
while
(nextNode != startNode)
{
if
(nResultVecs > 4 * nNodes)
{
Debug.Assert(
false
);
}
resultVecs[nResultVecs++] = nextNode.Position;
PolyNode oldNode = currentNode;
currentNode = nextNode;
nextNode = currentNode.GetRightestConnection(oldNode);
if
(nextNode ==
null
)
{
Vertices vertices =
new
Vertices(nResultVecs);
for
(
int
i = 0; i < nResultVecs; ++i)
{
vertices.Add(resultVecs[i]);
}
return
vertices;
}
}
return
new
Vertices();
}
private
class
PolyNode
{
private
const
int
MaxConnected = 32;
public
PolyNode[] Connected =
new
PolyNode[MaxConnected];
public
int
NConnected;
public
Vector2 Position;
public
PolyNode(Vector2 pos)
{
Position = pos;
NConnected = 0;
}
private
bool
IsRighter(
float
sinA,
float
cosA,
float
sinB,
float
cosB)
{
if
(sinA < 0)
{
if
(sinB > 0 || cosA <= cosB)
return
true
;
else
return
false
;
}
else
{
if
(sinB < 0 || cosA <= cosB)
return
false
;
else
return
true
;
}
}
public
void
AddConnection(PolyNode toMe)
{
Debug.Assert(NConnected < MaxConnected);
for
(
int
i = 0; i < NConnected; ++i)
{
if
(Connected[i] == toMe)
return
;
}
Connected[NConnected] = toMe;
++NConnected;
}
public
void
RemoveConnection(PolyNode fromMe)
{
bool
isFound =
false
;
int
foundIndex = -1;
for
(
int
i = 0; i < NConnected; ++i)
{
if
(fromMe == Connected[i])
{
isFound =
true
;
foundIndex = i;
break
;
}
}
Debug.Assert(isFound);
--NConnected;
for
(
int
i = foundIndex; i < NConnected; ++i)
{
Connected[i] = Connected[i + 1];
}
}
public
PolyNode GetRightestConnection(PolyNode incoming)
{
if
(NConnected == 0) Debug.Assert(
false
);
if
(NConnected == 1)
{
return
incoming;
}
Vector2 inDir = Position - incoming.Position;
float
inLength = inDir.Length();
inDir.Normalize();
Debug.Assert(inLength > Settings.Epsilon);
PolyNode result =
null
;
for
(
int
i = 0; i < NConnected; ++i)
{
if
(Connected[i] == incoming)
continue
;
Vector2 testDir = Connected[i].Position - Position;
float
testLengthSqr = testDir.LengthSquared();
testDir.Normalize();
Debug.Assert(testLengthSqr >= Settings.Epsilon * Settings.Epsilon);
float
myCos = Vector2.Dot(inDir, testDir);
float
mySin = MathUtils.Cross(inDir, testDir);
if
(result !=
null
)
{
Vector2 resultDir = result.Position - Position;
resultDir.Normalize();
float
resCos = Vector2.Dot(inDir, resultDir);
float
resSin = MathUtils.Cross(inDir, resultDir);
if
(IsRighter(mySin, myCos, resSin, resCos))
{
result = Connected[i];
}
}
else
{
result = Connected[i];
}
}
Debug.Assert(result !=
null
);
return
result;
}
public
PolyNode GetRightestConnection(Vector2 incomingDir)
{
Vector2 diff = Position - incomingDir;
PolyNode temp =
new
PolyNode(diff);
PolyNode res = GetRightestConnection(temp);
Debug.Assert(res !=
null
);
return
res;
}
}
public
override
string
ToString()
{
StringBuilder builder =
new
StringBuilder();
for
(
int
i = 0; i < Count; i++)
{
builder.Append(
this
[i].ToString());
if
(i < Count - 1)
{
builder.Append(
" "
);
}
}
return
builder.ToString();
}
/// <summary>
/// Projects to axis.
/// </summary>
/// <param name="axis">The axis.</param>
/// <param name="min">The min.</param>
/// <param name="max">The max.</param>
public
void
ProjectToAxis(
ref
Vector2 axis,
out
float
min,
out
float
max)
{
float
dotProduct = Vector2.Dot(axis,
this
[0]);
min = dotProduct;
max = dotProduct;
for
(
int
i = 0; i < Count; i++)
{
dotProduct = Vector2.Dot(
this
[i], axis);
if
(dotProduct < min)
{
min = dotProduct;
}
else
{
if
(dotProduct > max)
{
max = dotProduct;
}
}
}
}
/// <summary>
/// Winding number test for a point in a polygon.
/// </summary>
/// <param name="point">The point to be tested.</param>
/// <returns>-1 if the winding number is zero and the point is outside
/// the polygon, 1 if the point is inside the polygon, and 0 if the point
/// is on the polygons edge.</returns>
public
int
PointInPolygon(
ref
Vector2 point)
{
int
wn = 0;
for
(
int
i = 0; i < Count; i++)
{
Vector2 p1 =
this
[i];
Vector2 p2 =
this
[NextIndex(i)];
Vector2 edge = p2 - p1;
float
area = MathUtils.Area(
ref
p1,
ref
p2,
ref
point);
if
(area == 0f && Vector2.Dot(point - p1, edge) >= 0f && Vector2.Dot(point - p2, edge) <= 0f)
{
return
0;
}
if
(p1.Y <= point.Y)
{
if
(p2.Y > point.Y && area > 0f)
{
++wn;
}
}
else
{
if
(p2.Y <= point.Y && area < 0f)
{
--wn;
}
}
}
return
(wn == 0 ? -1 : 1);
}
/// <summary>
/// Compute the sum of the angles made between the test point and each pair of points making up the polygon.
/// If this sum is 2pi then the point is an interior point, if 0 then the point is an exterior point.
/// </summary>
public
bool
PointInPolygonAngle(
ref
Vector2 point)
{
double
angle = 0;
for
(
int
i = 0; i < Count; i++)
{
Vector2 p1 =
this
[i] - point;
Vector2 p2 =
this
[NextIndex(i)] - point;
angle += MathUtils.VectorAngle(
ref
p1,
ref
p2);
}
if
(Math.Abs(angle) < Math.PI)
{
return
false
;
}
return
true
;
}
}
}