public SplitComplexPolygonNode GetRightestConnection(SplitComplexPolygonNode incoming)
{
if (NumConnected == 0)
{
throw new Exception("the connection graph is inconsistent");
}
if (NumConnected == 1)
{
//b2Assert(false);
// Because of the possibility of collapsing nearby points,
// we may end up with "spider legs" dangling off of a region.
// The correct behavior here is to turn around.
return incoming;
}
Point2D inDir = mPosition - incoming.mPosition;
double inLength = inDir.Magnitude();
inDir.Normalize();
if (inLength <= MathUtil.EPSILON)
{
throw new Exception("Length too small");
}
SplitComplexPolygonNode result = null;
for (int i = 0; i < NumConnected; ++i)
{
if (mConnected[i] == incoming)
{
continue;
}
Point2D testDir = mConnected[i].mPosition - mPosition;
double testLengthSqr = testDir.MagnitudeSquared();
testDir.Normalize();
/*
if (testLengthSqr < COLLAPSE_DIST_SQR) {
printf("Problem with connection %d\n",i);
printf("This node has %d connections\n",nConnected);
printf("That one has %d\n",connected[i].nConnected);
if (this == connected[i]) printf("This points at itself.\n");
}*/
if (testLengthSqr <= (MathUtil.EPSILON * MathUtil.EPSILON))
{
throw new Exception("Length too small");
}
double myCos = Point2D.Dot(inDir, testDir);
double mySin = Point2D.Cross(inDir, testDir);
if (result != null)
{
Point2D resultDir = result.mPosition - mPosition;
resultDir.Normalize();
double resCos = Point2D.Dot(inDir, resultDir);
double resSin = Point2D.Cross(inDir, resultDir);
if (IsRighter(mySin, myCos, resSin, resCos))
{
result = mConnected[i];
}
}
else
{
result = mConnected[i];
}
}
//if (B2_POLYGON_REPORT_ERRORS && result != null)
//{
// printf("nConnected = %d\n", nConnected);
// for (int i = 0; i < nConnected; ++i)
// {
// printf("connected[%d] @ %d\n", i, (int)connected[i]);
// }
//}
//Debug.Assert(result != null);
return result;
}
public SplitComplexPolygonNode GetRightestConnection(Point2D incomingDir)
{
Point2D diff = mPosition - incomingDir;
SplitComplexPolygonNode temp = new SplitComplexPolygonNode(diff);
SplitComplexPolygonNode res = GetRightestConnection(temp);
//Debug.Assert(res != null);
return res;
}
private bool IsConnectedTo(SplitComplexPolygonNode me)
{
return mConnected.Contains(me);
}
public void RemoveConnection(SplitComplexPolygonNode fromMe)
{
mConnected.Remove(fromMe);
}
public void AddConnection(SplitComplexPolygonNode toMe)
{
// Ignore duplicate additions
if (!mConnected.Contains(toMe) && toMe != this)
{
mConnected.Add(toMe);
}
}
public bool Equals(SplitComplexPolygonNode pn)
{
if ((Object)pn == null)
{
return false;
}
if (mPosition == null || pn.Position == null)
{
return false;
}
return mPosition.Equals(pn.Position);
}
/// <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"></param>
/// <returns></returns>
public static List<Point2DList> SplitComplexPolygon(Point2DList verts, double epsilon)
{
int numVerts = verts.Count;
int nNodes = 0;
List<SplitComplexPolygonNode> nodes = new List<SplitComplexPolygonNode>();
//Add base nodes (raw outline)
for (int i = 0; i < verts.Count; ++i)
{
SplitComplexPolygonNode newNode = new SplitComplexPolygonNode(new Point2D(verts[i].X, verts[i].Y));
nodes.Add(newNode);
}
for (int i = 0; i < verts.Count; ++i)
{
int iplus = (i == numVerts - 1) ? 0 : i + 1;
int iminus = (i == 0) ? numVerts - 1 : i - 1;
nodes[i].AddConnection(nodes[iplus]);
nodes[i].AddConnection(nodes[iminus]);
}
nNodes = nodes.Count;
//Process intersection nodes - horribly inefficient
bool dirty = true;
int counter = 0;
while (dirty)
{
dirty = false;
for (int i = 0; !dirty && i < nNodes; ++i)
{
for (int j = 0; !dirty && j < nodes[i].NumConnected; ++j)
{
for (int k = 0; !dirty && k < nNodes; ++k)
{
if (k == i || nodes[k] == nodes[i][j])
{
continue;
}
for (int l = 0; !dirty && l < nodes[k].NumConnected; ++l)
{
if (nodes[k][l] == nodes[i][j] || nodes[k][l] == nodes[i])
{
continue;
}
//Check intersection
Point2D intersectPt = new Point2D();
//if (counter > 100) printf("checking intersection: %d, %d, %d, %d\n",i,j,k,l);
bool crosses = TriangulationUtil.LinesIntersect2D( nodes[i].Position,
nodes[i][j].Position,
nodes[k].Position,
nodes[k][l].Position,
true, true, true,
ref intersectPt,
epsilon);
if (crosses)
{
/*if (counter > 100) {
printf("Found crossing at %f, %f\n",intersectPt.x, intersectPt.y);
printf("Locations: %f,%f - %f,%f | %f,%f - %f,%f\n",
nodes[i].position.x, nodes[i].position.y,
nodes[i].connected[j].position.x, nodes[i].connected[j].position.y,
nodes[k].position.x,nodes[k].position.y,
nodes[k].connected[l].position.x,nodes[k].connected[l].position.y);
printf("Memory addresses: %d, %d, %d, %d\n",(int)&nodes[i],(int)nodes[i].connected[j],(int)&nodes[k],(int)nodes[k].connected[l]);
}*/
dirty = true;
//Destroy and re-hook connections at crossing point
SplitComplexPolygonNode intersectionNode = new SplitComplexPolygonNode(intersectPt);
int idx = nodes.IndexOf(intersectionNode);
if (idx >= 0 && idx < nodes.Count)
{
intersectionNode = nodes[idx];
}
else
{
nodes.Add(intersectionNode);
nNodes = nodes.Count;
}
SplitComplexPolygonNode nodei = nodes[i];
SplitComplexPolygonNode connij = nodes[i][j];
SplitComplexPolygonNode nodek = nodes[k];
SplitComplexPolygonNode connkl = nodes[k][l];
connij.RemoveConnection(nodei);
nodei.RemoveConnection(connij);
connkl.RemoveConnection(nodek);
nodek.RemoveConnection(connkl);
if (!intersectionNode.Position.Equals(nodei.Position, epsilon))
{
intersectionNode.AddConnection(nodei);
nodei.AddConnection(intersectionNode);
}
if (!intersectionNode.Position.Equals(nodek.Position, epsilon))
{
intersectionNode.AddConnection(nodek);
nodek.AddConnection(intersectionNode);
}
if (!intersectionNode.Position.Equals(connij.Position, epsilon))
{
intersectionNode.AddConnection(connij);
connij.AddConnection(intersectionNode);
}
if (!intersectionNode.Position.Equals(connkl.Position, epsilon))
{
intersectionNode.AddConnection(connkl);
connkl.AddConnection(intersectionNode);
}
}
}
}
}
}
++counter;
//if (counter > 100) printf("Counter: %d\n",counter);
}
// /*
// // Debugging: check for connection consistency
// for (int i=0; i<nNodes; ++i) {
// int nConn = nodes[i].nConnected;
// for (int j=0; j<nConn; ++j) {
// if (nodes[i].connected[j].nConnected == 0) Assert(false);
// SplitComplexPolygonNode* connect = nodes[i].connected[j];
// bool found = false;
// for (int k=0; k<connect.nConnected; ++k) {
// if (connect.connected[k] == &nodes[i]) found = true;
// }
// Assert(found);
// }
// }*/
//Collapse duplicate points
bool foundDupe = true;
int nActive = nNodes;
double epsilonSquared = epsilon * epsilon;
while (foundDupe)
{
foundDupe = false;
for (int i = 0; i < nNodes; ++i)
{
if (nodes[i].NumConnected == 0)
{
continue;
}
for (int j = i + 1; j < nNodes; ++j)
{
if (nodes[j].NumConnected == 0)
{
continue;
}
Point2D diff = nodes[i].Position - nodes[j].Position;
if (diff.MagnitudeSquared() <= epsilonSquared)
{
if (nActive <= 3)
{
throw new Exception("Eliminated so many duplicate points that resulting polygon has < 3 vertices!");
}
//printf("Found dupe, %d left\n",nActive);
--nActive;
foundDupe = true;
SplitComplexPolygonNode inode = nodes[i];
SplitComplexPolygonNode jnode = nodes[j];
//Move all of j's connections to i, and remove j
int njConn = jnode.NumConnected;
for (int k = 0; k < njConn; ++k)
{
SplitComplexPolygonNode knode = jnode[k];
//Debug.Assert(knode != jnode);
if (knode != inode)
{
inode.AddConnection(knode);
knode.AddConnection(inode);
}
knode.RemoveConnection(jnode);
//printf("knode %d on node %d now has %d connections\n",k,j,knode.nConnected);
//printf("Found duplicate point.\n");
}
jnode.ClearConnections(); // to help with garbage collection
nodes.RemoveAt(j);
--nNodes;
}
}
}
}
// /*
// // Debugging: check for connection consistency
// for (int i=0; i<nNodes; ++i) {
// int nConn = nodes[i].nConnected;
// printf("Node %d has %d connections\n",i,nConn);
// for (int j=0; j<nConn; ++j) {
// if (nodes[i].connected[j].nConnected == 0) {
// printf("Problem with node %d connection at address %d\n",i,(int)(nodes[i].connected[j]));
// Assert(false);
// }
// SplitComplexPolygonNode* connect = nodes[i].connected[j];
// bool found = false;
// for (int k=0; k<connect.nConnected; ++k) {
// if (connect.connected[k] == &nodes[i]) found = true;
// }
// if (!found) printf("Connection %d (of %d) on node %d (of %d) did not have reciprocal connection.\n",j,nConn,i,nNodes);
// Assert(found);
// }
// }//*/
//Now walk the edge of the list
//Find node with minimum y value (max x if equal)
double minY = double.MaxValue;
double maxX = -double.MaxValue;
int minYIndex = -1;
for (int i = 0; i < nNodes; ++i)
{
if (nodes[i].Position.Y < minY && nodes[i].NumConnected > 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].NumConnected > 1)
{
minYIndex = i;
maxX = nodes[i].Position.X;
}
}
Point2D origDir = new Point2D(1.0f, 0.0f);
List<Point2D> resultVecs = new List<Point2D>();
SplitComplexPolygonNode currentNode = nodes[minYIndex];
SplitComplexPolygonNode startNode = currentNode;
//Debug.Assert(currentNode.nConnected > 0);
SplitComplexPolygonNode nextNode = currentNode.GetRightestConnection(origDir);
if (nextNode == null)
{
// Borked, clean up our mess and return
return PolygonUtil.SplitComplexPolygonCleanup(verts);
}
resultVecs.Add(startNode.Position);
while (nextNode != startNode)
{
if (resultVecs.Count > (4 * nNodes))
{
//printf("%d, %d, %d\n",(int)startNode,(int)currentNode,(int)nextNode);
//printf("%f, %f . %f, %f\n",currentNode.position.x,currentNode.position.y, nextNode.position.x, nextNode.position.y);
//verts.printFormatted();
//printf("Dumping connection graph: \n");
//for (int i=0; i<nNodes; ++i)
//{
// printf("nodex[%d] = %f; nodey[%d] = %f;\n",i,nodes[i].position.x,i,nodes[i].position.y);
// printf("//connected to\n");
// for (int j=0; j<nodes[i].nConnected; ++j)
// {
// printf("connx[%d][%d] = %f; conny[%d][%d] = %f;\n",i,j,nodes[i].connected[j].position.x, i,j,nodes[i].connected[j].position.y);
// }
//}
//printf("Dumping results thus far: \n");
//for (int i=0; i<nResultVecs; ++i)
//{
// printf("x[%d]=map(%f,-3,3,0,width); y[%d] = map(%f,-3,3,height,0);\n",i,resultVecs[i].x,i,resultVecs[i].y);
//}
//Debug.Assert(false);
//nodes should never be visited four times apiece (proof?), so we've probably hit a loop...crap
throw new Exception("nodes should never be visited four times apiece (proof?), so we've probably hit a loop...crap");
}
resultVecs.Add(nextNode.Position);
SplitComplexPolygonNode oldNode = currentNode;
currentNode = nextNode;
//printf("Old node connections = %d; address %d\n",oldNode.nConnected, (int)oldNode);
//printf("Current node connections = %d; address %d\n",currentNode.nConnected, (int)currentNode);
nextNode = currentNode.GetRightestConnection(oldNode);
if (nextNode == null)
{
return PolygonUtil.SplitComplexPolygonCleanup(resultVecs);
}
// There was a problem, so jump out of the loop and use whatever garbage we've generated so far
//printf("nextNode address: %d\n",(int)nextNode);
}
if (resultVecs.Count < 1)
{
// Borked, clean up our mess and return
return PolygonUtil.SplitComplexPolygonCleanup(verts);
}
else
{
return PolygonUtil.SplitComplexPolygonCleanup(resultVecs);
}
}
