axiosengine/axios/Common/ConvexHull/Melkman.cs

122 lines
5.1 KiB
C#
Raw Normal View History

2012-03-19 23:57:59 +00:00
using Microsoft.Xna.Framework;
namespace FarseerPhysics.Common.ConvexHull
{
public static class Melkman
{
//Melkman based convex hull algorithm contributed by Cowdozer
/// <summary>
/// Creates a convex hull.
/// Note:
/// 1. Vertices must be of a simple polygon, i.e. edges do not overlap.
/// 2. Melkman does not work on point clouds
/// </summary>
/// <remarks>
/// Implemented using Melkman's Convex Hull Algorithm - O(n) time complexity.
/// Reference: http://www.ams.sunysb.edu/~jsbm/courses/345/melkman.pdf
/// </remarks>
/// <returns>A convex hull in counterclockwise winding order.</returns>
public static Vertices GetConvexHull(Vertices vertices)
{
//With less than 3 vertices, this is about the best we can do for a convex hull
if (vertices.Count < 3)
return vertices;
//We'll never need a queue larger than the current number of Vertices +1
//Create double-ended queue
Vector2[] deque = new Vector2[vertices.Count + 1];
int qf = 3, qb = 0; //Queue front index, queue back index
int qfm1, qbm1; //qfm1 = second element, qbm1 = second last element
//Start by placing first 3 vertices in convex CCW order
int startIndex = 3;
float k = MathUtils.Area(vertices[0], vertices[1], vertices[2]);
if (k == 0)
{
//Vertices are collinear.
deque[0] = vertices[0];
deque[1] = vertices[2]; //We can skip vertex 1 because it should be between 0 and 2
deque[2] = vertices[0];
qf = 2;
//Go until the end of the collinear sequence of vertices
for (startIndex = 3; startIndex < vertices.Count; startIndex++)
{
Vector2 tmp = vertices[startIndex];
if (MathUtils.Area(ref deque[0], ref deque[1], ref tmp) == 0) //This point is also collinear
deque[1] = vertices[startIndex];
else break;
}
}
else
{
deque[0] = deque[3] = vertices[2];
if (k > 0)
{
//Is Left. Set deque = {2, 0, 1, 2}
deque[1] = vertices[0];
deque[2] = vertices[1];
}
else
{
//Is Right. Set deque = {2, 1, 0, 2}
deque[1] = vertices[1];
deque[2] = vertices[0];
}
}
qfm1 = qf == 0 ? deque.Length - 1 : qf - 1; //qfm1 = qf - 1;
qbm1 = qb == deque.Length - 1 ? 0 : qb + 1; //qbm1 = qb + 1;
//Add vertices one at a time and adjust convex hull as needed
for (int i = startIndex; i < vertices.Count; i++)
{
Vector2 nextPt = vertices[i];
//Ignore if it is already within the convex hull we have constructed
if (MathUtils.Area(ref deque[qfm1], ref deque[qf], ref nextPt) > 0 &&
MathUtils.Area(ref deque[qb], ref deque[qbm1], ref nextPt) > 0)
continue;
//Pop front until convex
while (!(MathUtils.Area(ref deque[qfm1], ref deque[qf], ref nextPt) > 0))
{
//Pop the front element from the queue
qf = qfm1; //qf--;
qfm1 = qf == 0 ? deque.Length - 1 : qf - 1; //qfm1 = qf - 1;
}
//Add vertex to the front of the queue
qf = qf == deque.Length - 1 ? 0 : qf + 1; //qf++;
qfm1 = qf == 0 ? deque.Length - 1 : qf - 1; //qfm1 = qf - 1;
deque[qf] = nextPt;
//Pop back until convex
while (!(MathUtils.Area(ref deque[qb], ref deque[qbm1], ref nextPt) > 0))
{
//Pop the back element from the queue
qb = qbm1; //qb++;
qbm1 = qb == deque.Length - 1 ? 0 : qb + 1; //qbm1 = qb + 1;
}
//Add vertex to the back of the queue
qb = qb == 0 ? deque.Length - 1 : qb - 1; //qb--;
qbm1 = qb == deque.Length - 1 ? 0 : qb + 1; //qbm1 = qb + 1;
deque[qb] = nextPt;
}
//Create the convex hull from what is left in the deque
Vertices convexHull = new Vertices(vertices.Count + 1);
if (qb < qf)
for (int i = qb; i < qf; i++)
convexHull.Add(deque[i]);
else
{
for (int i = 0; i < qf; i++)
convexHull.Add(deque[i]);
for (int i = qb; i < deque.Length; i++)
convexHull.Add(deque[i]);
}
return convexHull;
}
}
}