summaryrefslogtreecommitdiff
path: root/thirdparty/b2d_convexdecomp/b2Polygon.cpp
diff options
context:
space:
mode:
Diffstat (limited to 'thirdparty/b2d_convexdecomp/b2Polygon.cpp')
-rw-r--r--thirdparty/b2d_convexdecomp/b2Polygon.cpp1586
1 files changed, 1586 insertions, 0 deletions
diff --git a/thirdparty/b2d_convexdecomp/b2Polygon.cpp b/thirdparty/b2d_convexdecomp/b2Polygon.cpp
new file mode 100644
index 0000000000..b6ead62c63
--- /dev/null
+++ b/thirdparty/b2d_convexdecomp/b2Polygon.cpp
@@ -0,0 +1,1586 @@
+/*
+ * Copyright (c) 2007 Eric Jordan
+ *
+ * This software is provided 'as-is', without any express or implied
+ * warranty. In no event will the authors be held liable for any damages
+ * arising from the use of this software.
+ * Permission is granted to anyone to use this software for any purpose,
+ * including commercial applications, and to alter it and redistribute it
+ * freely, subject to the following restrictions:
+ * 1. The origin of this software must not be misrepresented; you must not
+ * claim that you wrote the original software. If you use this software
+ * in a product, an acknowledgment in the product documentation would be
+ * appreciated but is not required.
+ * 2. Altered source versions must be plainly marked as such, and must not be
+ * misrepresented as being the original software.
+ * 3. This notice may not be removed or altered from any source distribution.
+ */
+
+// This utility works with Box2d version 2.0 (or higher), and not with 1.4.3
+
+#include "b2Triangle.h"
+#include "b2Polygon.h"
+
+#include <math.h>
+#include <limits.h>
+#include <assert.h>
+#define b2Assert assert
+
+namespace b2ConvexDecomp {
+
+
+//If you're using 1.4.3, b2_toiSlop won't exist, so set this equal to 0
+static const float32 toiSlop = 0.0f;
+
+/*
+ * Check if the lines a0->a1 and b0->b1 cross.
+ * If they do, intersectionPoint will be filled
+ * with the point of crossing.
+ *
+ * Grazing lines should not return true.
+ */
+bool intersect(const b2Vec2& a0, const b2Vec2& a1,
+ const b2Vec2& b0, const b2Vec2& b1,
+ b2Vec2& intersectionPoint) {
+
+ if (a0 == b0 || a0 == b1 || a1 == b0 || a1 == b1) return false;
+ float x1 = a0.x; float y1 = a0.y;
+ float x2 = a1.x; float y2 = a1.y;
+ float x3 = b0.x; float y3 = b0.y;
+ float x4 = b1.x; float y4 = b1.y;
+
+ //AABB early exit
+ if (b2Max(x1,x2) < b2Min(x3,x4) || b2Max(x3,x4) < b2Min(x1,x2) ) return false;
+ if (b2Max(y1,y2) < b2Min(y3,y4) || b2Max(y3,y4) < b2Min(y1,y2) ) return false;
+
+ float ua = ((x4 - x3) * (y1 - y3) - (y4 - y3) * (x1 - x3));
+ float ub = ((x2 - x1) * (y1 - y3) - (y2 - y1) * (x1 - x3));
+ float denom = (y4 - y3) * (x2 - x1) - (x4 - x3) * (y2 - y1);
+ if (b2Abs(denom) < CMP_EPSILON) {
+ //Lines are too close to parallel to call
+ return false;
+ }
+ ua /= denom;
+ ub /= denom;
+
+ if ((0 < ua) && (ua < 1) && (0 < ub) && (ub < 1)) {
+ //if (intersectionPoint){
+ intersectionPoint.x = (x1 + ua * (x2 - x1));
+ intersectionPoint.y = (y1 + ua * (y2 - y1));
+ //}
+ //printf("%f, %f -> %f, %f crosses %f, %f -> %f, %f\n",x1,y1,x2,y2,x3,y3,x4,y4);
+ return true;
+ }
+
+ return false;
+}
+
+/*
+ * True if line from a0->a1 intersects b0->b1
+ */
+bool intersect(const b2Vec2& a0, const b2Vec2& a1,
+ const b2Vec2& b0, const b2Vec2& b1) {
+ b2Vec2 myVec(0.0f,0.0f);
+ return intersect(a0, a1, b0, b1, myVec);
+}
+
+b2Polygon::b2Polygon(float32* _x, float32* _y, int32 nVert) {
+ nVertices = nVert;
+ x = new float32[nVertices];
+ y = new float32[nVertices];
+ for (int32 i = 0; i < nVertices; ++i) {
+ x[i] = _x[i];
+ y[i] = _y[i];
+ }
+ areaIsSet = false;
+}
+
+b2Polygon::b2Polygon(b2Vec2* v, int32 nVert) {
+ nVertices = nVert;
+ x = new float32[nVertices];
+ y = new float32[nVertices];
+ for (int32 i = 0; i < nVertices; ++i) {
+ x[i] = v[i].x;
+ y[i] = v[i].y;
+
+ }
+ areaIsSet = false;
+}
+
+b2Polygon::b2Polygon() {
+ x = NULL;
+ y = NULL;
+ nVertices = 0;
+ areaIsSet = false;
+}
+
+b2Polygon::~b2Polygon() {
+ //printf("About to delete poly with %d vertices\n",nVertices);
+ delete[] x;
+ delete[] y;
+}
+
+float32 b2Polygon::GetArea() {
+ // TODO: fix up the areaIsSet caching so that it can be used
+ //if (areaIsSet) return area;
+ area = 0.0f;
+
+ //First do wraparound
+ area += x[nVertices-1]*y[0]-x[0]*y[nVertices-1];
+ for (int i=0; i<nVertices-1; ++i){
+ area += x[i]*y[i+1]-x[i+1]*y[i];
+ }
+ area *= .5f;
+ areaIsSet = true;
+ return area;
+}
+
+bool b2Polygon::IsCCW() {
+ return (GetArea() > 0.0f);
+}
+
+void b2Polygon::MergeParallelEdges(float32 tolerance) {
+ if (nVertices <= 3) return; //Can't do anything useful here to a triangle
+ bool* mergeMe = new bool[nVertices];
+ int32 newNVertices = nVertices;
+ for (int32 i = 0; i < nVertices; ++i) {
+ int32 lower = (i == 0) ? (nVertices - 1) : (i - 1);
+ int32 middle = i;
+ int32 upper = (i == nVertices - 1) ? (0) : (i + 1);
+ float32 dx0 = x[middle] - x[lower];
+ float32 dy0 = y[middle] - y[lower];
+ float32 dx1 = x[upper] - x[middle];
+ float32 dy1 = y[upper] - y[middle];
+ float32 norm0 = sqrtf(dx0*dx0+dy0*dy0);
+ float32 norm1 = sqrtf(dx1*dx1+dy1*dy1);
+ if ( !(norm0 > 0.0f && norm1 > 0.0f) && newNVertices > 3 ) {
+ //Merge identical points
+ mergeMe[i] = true;
+ --newNVertices;
+ }
+ dx0 /= norm0; dy0 /= norm0;
+ dx1 /= norm1; dy1 /= norm1;
+ float32 cross = dx0 * dy1 - dx1 * dy0;
+ float32 dot = dx0 * dx1 + dy0 * dy1;
+ if (fabs(cross) < tolerance && dot > 0 && newNVertices > 3) {
+ mergeMe[i] = true;
+ --newNVertices;
+ } else {
+ mergeMe[i] = false;
+ }
+ }
+ if(newNVertices == nVertices || newNVertices == 0) {
+ delete[] mergeMe;
+ return;
+ }
+ float32* newx = new float32[newNVertices];
+ float32* newy = new float32[newNVertices];
+ int32 currIndex = 0;
+ for (int32 i=0; i < nVertices; ++i) {
+ if (mergeMe[i] || newNVertices == 0 || currIndex == newNVertices) continue;
+ b2Assert(currIndex < newNVertices);
+ newx[currIndex] = x[i];
+ newy[currIndex] = y[i];
+ ++currIndex;
+ }
+ delete[] x;
+ delete[] y;
+ delete[] mergeMe;
+ x = newx;
+ y = newy;
+ nVertices = newNVertices;
+ //printf("%d \n", newNVertices);
+}
+
+ /*
+ * Allocates and returns pointer to vector vertex array.
+ * Length of array is nVertices.
+ */
+b2Vec2* b2Polygon::GetVertexVecs() {
+ b2Vec2* out = new b2Vec2[nVertices];
+ for (int32 i = 0; i < nVertices; ++i) {
+ out[i].Set(x[i], y[i]);
+ }
+ return out;
+}
+
+b2Polygon::b2Polygon(b2Triangle& t) {
+ nVertices = 3;
+ x = new float[nVertices];
+ y = new float[nVertices];
+ for (int32 i = 0; i < nVertices; ++i) {
+ x[i] = t.x[i];
+ y[i] = t.y[i];
+ }
+}
+
+void b2Polygon::Set(const b2Polygon& p) {
+ if (nVertices != p.nVertices){
+ nVertices = p.nVertices;
+ delete[] x;
+ delete[] y;
+ x = new float32[nVertices];
+ y = new float32[nVertices];
+ }
+
+ for (int32 i = 0; i < nVertices; ++i) {
+ x[i] = p.x[i];
+ y[i] = p.y[i];
+ }
+ areaIsSet = false;
+}
+
+ /*
+ * Assuming the polygon is simple, checks if it is convex.
+ */
+bool b2Polygon::IsConvex() {
+ bool isPositive = false;
+ for (int32 i = 0; i < nVertices; ++i) {
+ int32 lower = (i == 0) ? (nVertices - 1) : (i - 1);
+ int32 middle = i;
+ int32 upper = (i == nVertices - 1) ? (0) : (i + 1);
+ float32 dx0 = x[middle] - x[lower];
+ float32 dy0 = y[middle] - y[lower];
+ float32 dx1 = x[upper] - x[middle];
+ float32 dy1 = y[upper] - y[middle];
+ float32 cross = dx0 * dy1 - dx1 * dy0;
+ // Cross product should have same sign
+ // for each vertex if poly is convex.
+ bool newIsP = (cross >= 0) ? true : false;
+ if (i == 0) {
+ isPositive = newIsP;
+ }
+ else if (isPositive != newIsP) {
+ return false;
+ }
+ }
+ return true;
+}
+
+/*
+ * Pulled from b2Shape.cpp, assertions removed
+ */
+static b2Vec2 PolyCentroid(const b2Vec2* vs, int32 count)
+{
+ b2Vec2 c; c.Set(0.0f, 0.0f);
+ float32 area = 0.0f;
+
+ const float32 inv3 = 1.0f / 3.0f;
+ b2Vec2 pRef(0.0f, 0.0f);
+ for (int32 i = 0; i < count; ++i)
+ {
+ // Triangle vertices.
+ b2Vec2 p1 = pRef;
+ b2Vec2 p2 = vs[i];
+ b2Vec2 p3 = i + 1 < count ? vs[i+1] : vs[0];
+
+ b2Vec2 e1 = p2 - p1;
+ b2Vec2 e2 = p3 - p1;
+
+ float32 D = b2Cross(e1, e2);
+
+ float32 triangleArea = 0.5f * D;
+ area += triangleArea;
+
+ // Area weighted centroid
+ c += (p1 + p2 + p3) * triangleArea * inv3;
+ }
+
+ // Centroid
+ c *= 1.0f / area;
+ return c;
+}
+
+
+/*
+ * 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.
+ */
+bool b2Polygon::IsUsable(bool printErrors){
+ int32 error = -1;
+ bool noError = true;
+ if (nVertices < 3 || nVertices > b2_maxPolygonVertices) {noError = false; error = 0;}
+ if (!IsConvex()) {noError = false; error = 1;}
+ if (!IsSimple()) {noError = false; error = 2;}
+ if (GetArea() < CMP_EPSILON) {noError = false; error = 3;}
+
+ //Compute normals
+ b2Vec2* normals = new b2Vec2[nVertices];
+ b2Vec2* vertices = new b2Vec2[nVertices];
+ for (int32 i = 0; i < nVertices; ++i){
+ vertices[i].Set(x[i],y[i]);
+ int32 i1 = i;
+ int32 i2 = i + 1 < nVertices ? i + 1 : 0;
+ b2Vec2 edge(x[i2]-x[i1],y[i2]-y[i1]);
+ normals[i] = b2Cross(edge, 1.0f);
+ normals[i].Normalize();
+ }
+
+ //Required side checks
+ for (int32 i=0; i<nVertices; ++i){
+ int32 iminus = (i==0)?nVertices-1:i-1;
+ //int32 iplus = (i==nVertices-1)?0:i+1;
+
+ //Parallel sides check
+ float32 cross = b2Cross(normals[iminus], normals[i]);
+ cross = b2Clamp(cross, -1.0f, 1.0f);
+ float32 angle = asinf(cross);
+ if(angle <= b2_angularSlop){
+ noError = false;
+ error = 4;
+ break;
+ }
+
+ //Too skinny check
+ for (int32 j=0; j<nVertices; ++j){
+ if (j == i || j == (i + 1) % nVertices){
+ continue;
+ }
+ float32 s = b2Dot(normals[i], vertices[j] - vertices[i]);
+ if (s >= -b2_linearSlop){
+ noError = false;
+ error = 5;
+ }
+ }
+
+
+ b2Vec2 centroid = PolyCentroid(vertices,nVertices);
+ b2Vec2 n1 = normals[iminus];
+ b2Vec2 n2 = normals[i];
+ b2Vec2 v = vertices[i] - centroid;
+
+ b2Vec2 d;
+ d.x = b2Dot(n1, v) - toiSlop;
+ d.y = b2Dot(n2, v) - toiSlop;
+
+ // Shifting the edge inward by b2_toiSlop should
+ // not cause the plane to pass the centroid.
+ if ((d.x < 0.0f)||(d.y < 0.0f)){
+ noError = false;
+ error = 6;
+ }
+
+ }
+ delete[] vertices;
+ delete[] normals;
+
+ if (!noError && printErrors){
+ printf("Found invalid polygon, ");
+ switch(error){
+ case 0:
+ printf("must have between 3 and %d vertices.\n",b2_maxPolygonVertices);
+ break;
+ case 1:
+ printf("must be convex.\n");
+ break;
+ case 2:
+ printf("must be simple (cannot intersect itself).\n");
+ break;
+ case 3:
+ printf("area is too small.\n");
+ break;
+ case 4:
+ printf("sides are too close to parallel.\n");
+ break;
+ case 5:
+ printf("polygon is too thin.\n");
+ break;
+ case 6:
+ printf("core shape generation would move edge past centroid (too thin).\n");
+ break;
+ default:
+ printf("don't know why.\n");
+ }
+ }
+ return noError;
+}
+
+
+bool b2Polygon::IsUsable(){
+ return IsUsable(B2_POLYGON_REPORT_ERRORS);
+}
+
+//Check for edge crossings
+bool b2Polygon::IsSimple() {
+ for (int32 i=0; i<nVertices; ++i){
+ int32 iplus = (i+1 > nVertices-1)?0:i+1;
+ b2Vec2 a1(x[i],y[i]);
+ b2Vec2 a2(x[iplus],y[iplus]);
+ for (int32 j=i+1; j<nVertices; ++j){
+ int32 jplus = (j+1 > nVertices-1)?0:j+1;
+ b2Vec2 b1(x[j],y[j]);
+ b2Vec2 b2(x[jplus],y[jplus]);
+ if (intersect(a1,a2,b1,b2)){
+ return false;
+ }
+ }
+ }
+ return true;
+}
+
+ /*
+ * Tries to add a triangle to the polygon. Returns null if it can't connect
+ * properly, otherwise returns a pointer to the new Polygon. Assumes bitwise
+ * equality of joined vertex positions.
+ *
+ * Remember to delete the pointer afterwards.
+ * Todo: Make this return a b2Polygon instead
+ * of a pointer to a heap-allocated one.
+ *
+ * For internal use.
+ */
+b2Polygon* b2Polygon::Add(b2Triangle& t) {
+ // First, find vertices that connect
+ int32 firstP = -1;
+ int32 firstT = -1;
+ int32 secondP = -1;
+ int32 secondT = -1;
+ for (int32 i = 0; i < nVertices; i++) {
+ if (t.x[0] == x[i] && t.y[0] == y[i]) {
+ if (firstP == -1) {
+ firstP = i;
+ firstT = 0;
+ }
+ else {
+ secondP = i;
+ secondT = 0;
+ }
+ }
+ else if (t.x[1] == x[i] && t.y[1] == y[i]) {
+ if (firstP == -1) {
+ firstP = i;
+ firstT = 1;
+ }
+ else {
+ secondP = i;
+ secondT = 1;
+ }
+ }
+ else if (t.x[2] == x[i] && t.y[2] == y[i]) {
+ if (firstP == -1) {
+ firstP = i;
+ firstT = 2;
+ }
+ else {
+ secondP = i;
+ secondT = 2;
+ }
+ }
+ else {
+ }
+ }
+ // Fix ordering if first should be last vertex of poly
+ if (firstP == 0 && secondP == nVertices - 1) {
+ firstP = nVertices - 1;
+ secondP = 0;
+ }
+
+ // Didn't find it
+ if (secondP == -1) {
+ return NULL;
+ }
+
+ // Find tip index on triangle
+ int32 tipT = 0;
+ if (tipT == firstT || tipT == secondT)
+ tipT = 1;
+ if (tipT == firstT || tipT == secondT)
+ tipT = 2;
+
+ float32* newx = new float[nVertices + 1];
+ float32* newy = new float[nVertices + 1];
+ int32 currOut = 0;
+ for (int32 i = 0; i < nVertices; i++) {
+ newx[currOut] = x[i];
+ newy[currOut] = y[i];
+ if (i == firstP) {
+ ++currOut;
+ newx[currOut] = t.x[tipT];
+ newy[currOut] = t.y[tipT];
+ }
+ ++currOut;
+ }
+ b2Polygon* result = new b2Polygon(newx, newy, nVertices+1);
+ delete[] newx;
+ delete[] newy;
+ return result;
+}
+
+ /**
+ * Adds this polygon to a PolyDef.
+ */
+#if 0
+void b2Polygon::AddTo(b2FixtureDef& pd) {
+ if (nVertices < 3) return;
+
+ b2Assert(nVertices <= b2_maxPolygonVertices);
+
+ b2Vec2* vecs = GetVertexVecs();
+ b2Vec2* vecsToAdd = new b2Vec2[nVertices];
+
+ int32 offset = 0;
+
+ b2PolygonShape *polyShape = new b2PolygonShape;
+ int32 ind;
+
+ for (int32 i = 0; i < nVertices; ++i) {
+
+ //Omit identical neighbors (including wraparound)
+ ind = i - offset;
+ if (vecs[i].x==vecs[remainder(i+1,nVertices)].x &&
+ vecs[i].y==vecs[remainder(i+1,nVertices)].y){
+ offset++;
+ continue;
+ }
+
+ vecsToAdd[ind] = vecs[i];
+
+ }
+
+ polyShape->Set((const b2Vec2*)vecsToAdd, ind+1);
+ pd.shape = polyShape;
+
+ delete[] vecs;
+ delete[] vecsToAdd;
+}
+#endif
+ /**
+ * Finds and fixes "pinch points," points where two polygon
+ * vertices are at the same point.
+ *
+ * If a pinch point is found, pin is broken up into poutA and poutB
+ * and true is returned; otherwise, returns false.
+ *
+ * Mostly for internal use.
+ */
+bool ResolvePinchPoint(const b2Polygon& pin, b2Polygon& poutA, b2Polygon& poutB){
+ if (pin.nVertices < 3) return false;
+ float32 tol = .001f;
+ bool hasPinchPoint = false;
+ int32 pinchIndexA = -1;
+ int32 pinchIndexB = -1;
+ for (int i=0; i<pin.nVertices; ++i){
+ for (int j=i+1; j<pin.nVertices; ++j){
+ //Don't worry about pinch points where the points
+ //are actually just dupe neighbors
+ if (b2Abs(pin.x[i]-pin.x[j])<tol&&b2Abs(pin.y[i]-pin.y[j])<tol&&j!=i+1){
+ pinchIndexA = i;
+ pinchIndexB = j;
+ //printf("pinch: %f, %f == %f, %f\n",pin.x[i],pin.y[i],pin.x[j],pin.y[j]);
+ //printf("at indexes %d, %d\n",i,j);
+ hasPinchPoint = true;
+ break;
+ }
+ }
+ if (hasPinchPoint) break;
+ }
+ if (hasPinchPoint){
+ //printf("Found pinch point\n");
+ int32 sizeA = pinchIndexB - pinchIndexA;
+ if (sizeA == pin.nVertices) return false;//has dupe points at wraparound, not a problem here
+ float32* xA = new float32[sizeA];
+ float32* yA = new float32[sizeA];
+ for (int32 i=0; i < sizeA; ++i){
+ int32 ind = remainder(pinchIndexA+i,pin.nVertices);
+ xA[i] = pin.x[ind];
+ yA[i] = pin.y[ind];
+ }
+ b2Polygon tempA(xA,yA,sizeA);
+ poutA.Set(tempA);
+ delete[] xA;
+ delete[] yA;
+
+ int32 sizeB = pin.nVertices - sizeA;
+ float32* xB = new float32[sizeB];
+ float32* yB = new float32[sizeB];
+ for (int32 i=0; i<sizeB; ++i){
+ int32 ind = remainder(pinchIndexB+i,pin.nVertices);
+ xB[i] = pin.x[ind];
+ yB[i] = pin.y[ind];
+ }
+ b2Polygon tempB(xB,yB,sizeB);
+ poutB.Set(tempB);
+ //printf("Size of a: %d, size of b: %d\n",sizeA,sizeB);
+ delete[] xB;
+ delete[] yB;
+ }
+ return hasPinchPoint;
+}
+
+ /**
+ * Triangulates a polygon using simple ear-clipping algorithm. Returns
+ * size of Triangle array unless the polygon can't be triangulated.
+ * This should only happen if the polygon self-intersects,
+ * though it will not _always_ return null for a bad polygon - it is the
+ * caller's responsibility to check for self-intersection, and if it
+ * doesn't, it should at least check that the return value is non-null
+ * before using. You're warned!
+ *
+ * Triangles may be degenerate, especially if you have identical points
+ * in the input to the algorithm. Check this before you use them.
+ *
+ * This is totally unoptimized, so for large polygons it should not be part
+ * of the simulation loop.
+ *
+ * Returns:
+ * -1 if algorithm fails (self-intersection most likely)
+ * 0 if there are not enough vertices to triangulate anything.
+ * Number of triangles if triangulation was successful.
+ *
+ * results will be filled with results - ear clipping always creates vNum - 2
+ * or fewer (due to pinch point polygon snipping), so allocate an array of
+ * this size.
+ */
+
+int32 TriangulatePolygon(float32* xv, float32* yv, int32 vNum, b2Triangle* results) {
+ if (vNum < 3)
+ return 0;
+
+ //Recurse and split on pinch points
+ b2Polygon pA,pB;
+ b2Polygon pin(xv,yv,vNum);
+ if (ResolvePinchPoint(pin,pA,pB)){
+ b2Triangle* mergeA = new b2Triangle[pA.nVertices];
+ b2Triangle* mergeB = new b2Triangle[pB.nVertices];
+ int32 nA = TriangulatePolygon(pA.x,pA.y,pA.nVertices,mergeA);
+ int32 nB = TriangulatePolygon(pB.x,pB.y,pB.nVertices,mergeB);
+ if (nA==-1 || nB==-1){
+ delete[] mergeA;
+ delete[] mergeB;
+ return -1;
+ }
+ for (int32 i=0; i<nA; ++i){
+ results[i].Set(mergeA[i]);
+ }
+ for (int32 i=0; i<nB; ++i){
+ results[nA+i].Set(mergeB[i]);
+ }
+ delete[] mergeA;
+ delete[] mergeB;
+ return (nA+nB);
+ }
+
+ b2Triangle* buffer = new b2Triangle[vNum-2];
+ int32 bufferSize = 0;
+ float32* xrem = new float32[vNum];
+ float32* yrem = new float32[vNum];
+ for (int32 i = 0; i < vNum; ++i) {
+ xrem[i] = xv[i];
+ yrem[i] = yv[i];
+ }
+
+ int xremLength = vNum;
+
+ while (vNum > 3) {
+ // Find an ear
+ int32 earIndex = -1;
+ //float32 earVolume = -1.0f;
+ float32 earMaxMinCross = -10.0f;
+ for (int32 i = 0; i < vNum; ++i) {
+ if (IsEar(i, xrem, yrem, vNum)) {
+ int32 lower = remainder(i-1,vNum);
+ int32 upper = remainder(i+1,vNum);
+ b2Vec2 d1(xrem[upper]-xrem[i],yrem[upper]-yrem[i]);
+ b2Vec2 d2(xrem[i]-xrem[lower],yrem[i]-yrem[lower]);
+ b2Vec2 d3(xrem[lower]-xrem[upper],yrem[lower]-yrem[upper]);
+
+ d1.Normalize();
+ d2.Normalize();
+ d3.Normalize();
+ float32 cross12 = b2Abs( b2Cross(d1,d2) );
+ float32 cross23 = b2Abs( b2Cross(d2,d3) );
+ float32 cross31 = b2Abs( b2Cross(d3,d1) );
+ //Find the maximum minimum angle
+ float32 minCross = b2Min(cross12, b2Min(cross23,cross31));
+ if (minCross > earMaxMinCross){
+ earIndex = i;
+ earMaxMinCross = minCross;
+ }
+
+ /*//This bit chooses the ear with greatest volume first
+ float32 testVol = b2Abs( d1.x*d2.y-d2.x*d1.y );
+ if (testVol > earVolume){
+ earIndex = i;
+ earVolume = testVol;
+ }*/
+ }
+ }
+
+ // If we still haven't found an ear, we're screwed.
+ // Note: sometimes this is happening because the
+ // remaining points are collinear. Really these
+ // should just be thrown out without halting triangulation.
+ if (earIndex == -1){
+ if (B2_POLYGON_REPORT_ERRORS){
+ b2Polygon dump(xrem,yrem,vNum);
+ printf("Couldn't find an ear, dumping remaining poly:\n");
+ dump.printFormatted();
+ printf("Please submit this dump to ewjordan at Box2d forums\n");
+ }
+ for (int32 i = 0; i < bufferSize; i++) {
+ results[i].Set(buffer[i]);
+ }
+
+ delete[] buffer;
+
+ if (bufferSize > 0) return bufferSize;
+ else return -1;
+ }
+
+ // Clip off the ear:
+ // - remove the ear tip from the list
+
+ --vNum;
+ float32* newx = new float32[vNum];
+ float32* newy = new float32[vNum];
+ int32 currDest = 0;
+ for (int32 i = 0; i < vNum; ++i) {
+ if (currDest == earIndex) ++currDest;
+ newx[i] = xrem[currDest];
+ newy[i] = yrem[currDest];
+ ++currDest;
+ }
+
+ // - add the clipped triangle to the triangle list
+ int32 under = (earIndex == 0) ? (vNum) : (earIndex - 1);
+ int32 over = (earIndex == vNum) ? 0 : (earIndex + 1);
+ b2Triangle toAdd = b2Triangle(xrem[earIndex], yrem[earIndex], xrem[over], yrem[over], xrem[under], yrem[under]);
+ buffer[bufferSize].Set(toAdd);
+ ++bufferSize;
+
+ // - replace the old list with the new one
+ delete[] xrem;
+ delete[] yrem;
+ xrem = newx;
+ yrem = newy;
+ }
+
+ b2Triangle toAdd = b2Triangle(xrem[1], yrem[1], xrem[2], yrem[2],
+ xrem[0], yrem[0]);
+ buffer[bufferSize].Set(toAdd);
+ ++bufferSize;
+
+ delete[] xrem;
+ delete[] yrem;
+
+ b2Assert(bufferSize == xremLength-2);
+
+ for (int32 i = 0; i < bufferSize; i++) {
+ results[i].Set(buffer[i]);
+ }
+
+ delete[] buffer;
+
+ return bufferSize;
+}
+
+ /**
+ * Turns a list of triangles into a list of convex polygons. Very simple
+ * method - start with a seed triangle, keep adding triangles to it until
+ * you can't add any more without making the polygon non-convex.
+ *
+ * Returns an integer telling how many polygons were created. Will fill
+ * polys array up to polysLength entries, which may be smaller or larger
+ * than the return value.
+ *
+ * Takes O(N*P) where P is the number of resultant polygons, N is triangle
+ * count.
+ *
+ * The final polygon list will not necessarily be minimal, though in
+ * practice it works fairly well.
+ */
+int32 PolygonizeTriangles(b2Triangle* triangulated, int32 triangulatedLength, b2Polygon* polys, int32 polysLength) {
+ int32 polyIndex = 0;
+
+ if (triangulatedLength <= 0) {
+ return 0;
+ }
+ else {
+ int* covered = new int[triangulatedLength];
+ for (int32 i = 0; i < triangulatedLength; ++i) {
+ covered[i] = 0;
+ //Check here for degenerate triangles
+ if ( ( (triangulated[i].x[0] == triangulated[i].x[1]) && (triangulated[i].y[0] == triangulated[i].y[1]) )
+ || ( (triangulated[i].x[1] == triangulated[i].x[2]) && (triangulated[i].y[1] == triangulated[i].y[2]) )
+ || ( (triangulated[i].x[0] == triangulated[i].x[2]) && (triangulated[i].y[0] == triangulated[i].y[2]) ) ) {
+ covered[i] = 1;
+ }
+ }
+
+ bool notDone = true;
+ while (notDone) {
+ int32 currTri = -1;
+ for (int32 i = 0; i < triangulatedLength; ++i) {
+ if (covered[i])
+ continue;
+ currTri = i;
+ break;
+ }
+ if (currTri == -1) {
+ notDone = false;
+ }
+ else {
+ b2Polygon poly(triangulated[currTri]);
+ covered[currTri] = 1;
+ int32 index = 0;
+ for (int32 i = 0; i < 2*triangulatedLength; ++i,++index) {
+ while (index >= triangulatedLength) index -= triangulatedLength;
+ if (covered[index]) {
+ continue;
+ }
+ b2Polygon* newP = poly.Add(triangulated[index]);
+ if (!newP) {
+ continue;
+ }
+ if (newP->nVertices > b2Polygon::maxVerticesPerPolygon) {
+ delete newP;
+ newP = NULL;
+ continue;
+ }
+ if (newP->IsConvex()) { //Or should it be IsUsable? Maybe re-write IsConvex to apply the angle threshold from Box2d
+ poly.Set(*newP);
+ delete newP;
+ newP = NULL;
+ covered[index] = 1;
+ } else {
+ delete newP;
+ newP = NULL;
+ }
+ }
+ if (polyIndex < polysLength){
+ poly.MergeParallelEdges(b2_angularSlop);
+ //If identical points are present, a triangle gets
+ //borked by the MergeParallelEdges function, hence
+ //the vertex number check
+ if (poly.nVertices >= 3) polys[polyIndex].Set(poly);
+ //else printf("Skipping corrupt poly\n");
+ }
+ if (poly.nVertices >= 3) polyIndex++; //Must be outside (polyIndex < polysLength) test
+ }
+ //printf("MEMCHECK: %d\n",_CrtCheckMemory());
+ }
+ delete[] covered;
+ }
+ return polyIndex;
+}
+
+ /**
+ * Checks if vertex i is the tip of an ear in polygon defined by xv[] and
+ * yv[].
+ *
+ * Assumes clockwise orientation of polygon...ick
+ */
+bool IsEar(int32 i, float32* xv, float32* yv, int32 xvLength) {
+ float32 dx0, dy0, dx1, dy1;
+ dx0 = dy0 = dx1 = dy1 = 0;
+ if (i >= xvLength || i < 0 || xvLength < 3) {
+ return false;
+ }
+ int32 upper = i + 1;
+ int32 lower = i - 1;
+ if (i == 0) {
+ dx0 = xv[0] - xv[xvLength - 1];
+ dy0 = yv[0] - yv[xvLength - 1];
+ dx1 = xv[1] - xv[0];
+ dy1 = yv[1] - yv[0];
+ lower = xvLength - 1;
+ }
+ else if (i == xvLength - 1) {
+ dx0 = xv[i] - xv[i - 1];
+ dy0 = yv[i] - yv[i - 1];
+ dx1 = xv[0] - xv[i];
+ dy1 = yv[0] - yv[i];
+ upper = 0;
+ }
+ else {
+ dx0 = xv[i] - xv[i - 1];
+ dy0 = yv[i] - yv[i - 1];
+ dx1 = xv[i + 1] - xv[i];
+ dy1 = yv[i + 1] - yv[i];
+ }
+ float32 cross = dx0 * dy1 - dx1 * dy0;
+ if (cross > 0)
+ return false;
+ b2Triangle myTri(xv[i], yv[i], xv[upper], yv[upper],
+ xv[lower], yv[lower]);
+ for (int32 j = 0; j < xvLength; ++j) {
+ if (j == i || j == lower || j == upper)
+ continue;
+ if (myTri.IsInside(xv[j], yv[j]))
+ return false;
+ }
+ return true;
+}
+
+void ReversePolygon(b2Polygon& p){
+ ReversePolygon(p.x,p.y,p.nVertices);
+}
+
+void ReversePolygon(float* x, float* y, int n) {
+ if (n == 1)
+ return;
+ int32 low = 0;
+ int32 high = n - 1;
+ while (low < high) {
+ float32 buffer = x[low];
+ x[low] = x[high];
+ x[high] = buffer;
+ buffer = y[low];
+ y[low] = y[high];
+ y[high] = buffer;
+ ++low;
+ --high;
+ }
+}
+
+ /**
+ * Decomposes a non-convex polygon into a number of convex polygons, up
+ * to maxPolys (remaining pieces are thrown out, but the total number
+ * is returned, so the return value can be greater than maxPolys).
+ *
+ * Each resulting polygon will have no more than maxVerticesPerPolygon
+ * vertices (set to b2MaxPolyVertices by default, though you can change
+ * this).
+ *
+ * Returns -1 if operation fails (usually due to self-intersection of
+ * polygon).
+ */
+int32 DecomposeConvex(b2Polygon* p, b2Polygon* results, int32 maxPolys) {
+ if (p->nVertices < 3) return 0;
+
+ b2Triangle* triangulated = new b2Triangle[p->nVertices - 2];
+ int32 nTri;
+ if (p->IsCCW()) {
+ //printf("It is ccw \n");
+ b2Polygon tempP;
+ tempP.Set(*p);
+ ReversePolygon(tempP.x, tempP.y, tempP.nVertices);
+ nTri = TriangulatePolygon(tempP.x, tempP.y, tempP.nVertices, triangulated);
+ //ReversePolygon(p->x, p->y, p->nVertices); //reset orientation
+ } else {
+ //printf("It is not ccw \n");
+ nTri = TriangulatePolygon(p->x, p->y, p->nVertices, triangulated);
+ }
+ if (nTri < 1) {
+ //Still no luck? Oh well...
+ delete[] triangulated;
+ return -1;
+ }
+ int32 nPolys = PolygonizeTriangles(triangulated, nTri, results, maxPolys);
+ delete[] triangulated;
+ return nPolys;
+}
+
+ /**
+ * Decomposes a polygon into convex polygons and adds all pieces to a b2BodyDef
+ * using a prototype b2PolyDef. All fields of the prototype are used for every
+ * shape except the vertices (friction, restitution, density, etc).
+ *
+ * If you want finer control, you'll have to add everything by hand.
+ *
+ * This is the simplest method to add a complicated polygon to a body.
+ *
+ * Until Box2D's b2BodyDef behavior changes, this method returns a pointer to
+ * a heap-allocated array of b2PolyDefs, which must be deleted by the user
+ * after the b2BodyDef is added to the world.
+ */
+#if 0
+void DecomposeConvexAndAddTo(b2Polygon* p, b2Body* bd, b2FixtureDef* prototype) {
+
+ if (p->nVertices < 3) return;
+ b2Polygon* decomposed = new b2Polygon[p->nVertices - 2]; //maximum number of polys
+ int32 nPolys = DecomposeConvex(p, decomposed, p->nVertices - 2);
+ //printf("npolys: %d",nPolys);
+ b2FixtureDef* pdarray = new b2FixtureDef[2*p->nVertices];//extra space in case of splits
+ int32 extra = 0;
+ for (int32 i = 0; i < nPolys; ++i) {
+ b2FixtureDef* toAdd = &pdarray[i+extra];
+ *toAdd = *prototype;
+ //Hmm, shouldn't have to do all this...
+ /*
+ toAdd->type = prototype->type;
+ toAdd->friction = prototype->friction;
+ toAdd->restitution = prototype->restitution;
+ toAdd->density = prototype->density;
+ toAdd->userData = prototype->userData;
+ toAdd->categoryBits = prototype->categoryBits;
+ toAdd->maskBits = prototype->maskBits;
+ toAdd->groupIndex = prototype->groupIndex;//*/
+ //decomposed[i].print();
+ b2Polygon curr = decomposed[i];
+ //TODO ewjordan: move this triangle handling to a better place so that
+ //it happens even if this convenience function is not called.
+ if (curr.nVertices == 3){
+ //Check here for near-parallel edges, since we can't
+ //handle this in merge routine
+ for (int j=0; j<3; ++j){
+ int32 lower = (j == 0) ? (curr.nVertices - 1) : (j - 1);
+ int32 middle = j;
+ int32 upper = (j == curr.nVertices - 1) ? (0) : (j + 1);
+ float32 dx0 = curr.x[middle] - curr.x[lower]; float32 dy0 = curr.y[middle] - curr.y[lower];
+ float32 dx1 = curr.x[upper] - curr.x[middle]; float32 dy1 = curr.y[upper] - curr.y[middle];
+ float32 norm0 = sqrtf(dx0*dx0+dy0*dy0); float32 norm1 = sqrtf(dx1*dx1+dy1*dy1);
+ if ( !(norm0 > 0.0f && norm1 > 0.0f) ) {
+ //Identical points, don't do anything!
+ goto Skip;
+ }
+ dx0 /= norm0; dy0 /= norm0;
+ dx1 /= norm1; dy1 /= norm1;
+ float32 cross = dx0 * dy1 - dx1 * dy0;
+ float32 dot = dx0*dx1 + dy0*dy1;
+ if (fabs(cross) < b2_angularSlop && dot > 0) {
+ //Angle too close, split the triangle across from this point.
+ //This is guaranteed to result in two triangles that satify
+ //the tolerance (one of the angles is 90 degrees)
+ float32 dx2 = curr.x[lower] - curr.x[upper]; float32 dy2 = curr.y[lower] - curr.y[upper];
+ float32 norm2 = sqrtf(dx2*dx2+dy2*dy2);
+ if (norm2 == 0.0f) {
+ goto Skip;
+ }
+ dx2 /= norm2; dy2 /= norm2;
+ float32 thisArea = curr.GetArea();
+ float32 thisHeight = 2.0f * thisArea / norm2;
+ float32 buffer2 = dx2;
+ dx2 = dy2; dy2 = -buffer2;
+ //Make two new polygons
+ //printf("dx2: %f, dy2: %f, thisHeight: %f, middle: %d\n",dx2,dy2,thisHeight,middle);
+ float32 newX1[3] = { curr.x[middle]+dx2*thisHeight, curr.x[lower], curr.x[middle] };
+ float32 newY1[3] = { curr.y[middle]+dy2*thisHeight, curr.y[lower], curr.y[middle] };
+ float32 newX2[3] = { newX1[0], curr.x[middle], curr.x[upper] };
+ float32 newY2[3] = { newY1[0], curr.y[middle], curr.y[upper] };
+ b2Polygon p1(newX1,newY1,3);
+ b2Polygon p2(newX2,newY2,3);
+ if (p1.IsUsable()){
+ p1.AddTo(*toAdd);
+
+
+ bd->CreateFixture(toAdd);
+ ++extra;
+ } else if (B2_POLYGON_REPORT_ERRORS){
+ printf("Didn't add unusable polygon. Dumping vertices:\n");
+ p1.print();
+ }
+ if (p2.IsUsable()){
+ p2.AddTo(pdarray[i+extra]);
+
+ bd->CreateFixture(toAdd);
+ } else if (B2_POLYGON_REPORT_ERRORS){
+ printf("Didn't add unusable polygon. Dumping vertices:\n");
+ p2.print();
+ }
+ goto Skip;
+ }
+ }
+
+ }
+ if (decomposed[i].IsUsable()){
+ decomposed[i].AddTo(*toAdd);
+
+ bd->CreateFixture((const b2FixtureDef*)toAdd);
+ } else if (B2_POLYGON_REPORT_ERRORS){
+ printf("Didn't add unusable polygon. Dumping vertices:\n");
+ decomposed[i].print();
+ }
+Skip:
+ ;
+ }
+ delete[] pdarray;
+ delete[] decomposed;
+ return;// pdarray; //needs to be deleted after body is created
+}
+
+#endif
+ /**
+ * Find the convex hull of a point cloud using "Gift-wrap" algorithm - start
+ * with an extremal point, and walk around the outside edge by testing
+ * angles.
+ *
+ * Runs in O(N*S) time where S is number of sides of resulting polygon.
+ * Worst case: point cloud is all vertices of convex polygon -> O(N^2).
+ *
+ * There may be faster algorithms to do this, should you need one -
+ * this is just the simplest. You can get O(N log N) expected time if you
+ * try, I think, and O(N) if you restrict inputs to simple polygons.
+ *
+ * Returns null if number of vertices passed is less than 3.
+ *
+ * Results should be passed through convex decomposition afterwards
+ * to ensure that each shape has few enough points to be used in Box2d.
+ *
+ * FIXME?: May be buggy with colinear points on hull. Couldn't find a test
+ * case that resulted in wrong behavior. If one turns up, the solution is to
+ * supplement angle check with an equality resolver that always picks the
+ * longer edge. I think the current solution is working, though it sometimes
+ * creates an extra edge along a line.
+ */
+
+b2Polygon ConvexHull(b2Vec2* v, int nVert) {
+ float32* cloudX = new float32[nVert];
+ float32* cloudY = new float32[nVert];
+ for (int32 i = 0; i < nVert; ++i) {
+ cloudX[i] = v[i].x;
+ cloudY[i] = v[i].y;
+ }
+ b2Polygon result = ConvexHull(cloudX, cloudY, nVert);
+ delete[] cloudX;
+ delete[] cloudY;
+ return result;
+}
+
+b2Polygon ConvexHull(float32* cloudX, float32* cloudY, int32 nVert) {
+ b2Assert(nVert > 2);
+ int32* edgeList = new int32[nVert];
+ int32 numEdges = 0;
+
+ float32 minY = 1e10;
+ int32 minYIndex = nVert;
+ for (int32 i = 0; i < nVert; ++i) {
+ if (cloudY[i] < minY) {
+ minY = cloudY[i];
+ minYIndex = i;
+ }
+ }
+
+ int32 startIndex = minYIndex;
+ int32 winIndex = -1;
+ float32 dx = -1.0f;
+ float32 dy = 0.0f;
+ while (winIndex != minYIndex) {
+ float32 newdx = 0.0f;
+ float32 newdy = 0.0f;
+ float32 maxDot = -2.0f;
+ for (int32 i = 0; i < nVert; ++i) {
+ if (i == startIndex)
+ continue;
+ newdx = cloudX[i] - cloudX[startIndex];
+ newdy = cloudY[i] - cloudY[startIndex];
+ float32 nrm = sqrtf(newdx * newdx + newdy * newdy);
+ nrm = (nrm == 0.0f) ? 1.0f : nrm;
+ newdx /= nrm;
+ newdy /= nrm;
+
+ //Cross and dot products act as proxy for angle
+ //without requiring inverse trig.
+ //FIXED: don't need cross test
+ //float32 newCross = newdx * dy - newdy * dx;
+ float32 newDot = newdx * dx + newdy * dy;
+ if (newDot > maxDot) {//newCross >= 0.0f && newDot > maxDot) {
+ maxDot = newDot;
+ winIndex = i;
+ }
+ }
+ edgeList[numEdges++] = winIndex;
+ dx = cloudX[winIndex] - cloudX[startIndex];
+ dy = cloudY[winIndex] - cloudY[startIndex];
+ float32 nrm = sqrtf(dx * dx + dy * dy);
+ nrm = (nrm == 0.0f) ? 1.0f : nrm;
+ dx /= nrm;
+ dy /= nrm;
+ startIndex = winIndex;
+ }
+
+ float32* xres = new float32[numEdges];
+ float32* yres = new float32[numEdges];
+ for (int32 i = 0; i < numEdges; i++) {
+ xres[i] = cloudX[edgeList[i]];
+ yres[i] = cloudY[edgeList[i]];
+ //("%f, %f\n",xres[i],yres[i]);
+ }
+
+ b2Polygon returnVal(xres, yres, numEdges);
+
+ delete[] xres;
+ delete[] yres;
+ delete[] edgeList;
+ returnVal.MergeParallelEdges(b2_angularSlop);
+ return returnVal;
+}
+
+
+/*
+ * Given sines and cosines, tells if A's angle is less than B's on -Pi, Pi
+ * (in other words, is A "righter" than B)
+ */
+bool IsRighter(float32 sinA, float32 cosA, float32 sinB, float32 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;
+ }
+}
+
+//Fix for obnoxious behavior for the % operator for negative numbers...
+int32 remainder(int32 x, int32 modulus){
+ int32 rem = x % modulus;
+ while (rem < 0){
+ rem += modulus;
+ }
+ return rem;
+}
+
+/*
+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)
+
+*/
+
+b2Polygon TraceEdge(b2Polygon* p){
+ b2PolyNode* nodes = new b2PolyNode[p->nVertices*p->nVertices];//overkill, but sufficient (order of mag. is right)
+ int32 nNodes = 0;
+
+ //Add base nodes (raw outline)
+ for (int32 i=0; i < p->nVertices; ++i){
+ b2Vec2 pos(p->x[i],p->y[i]);
+ nodes[i].position = pos;
+ ++nNodes;
+ int32 iplus = (i==p->nVertices-1)?0:i+1;
+ int32 iminus = (i==0)?p->nVertices-1:i-1;
+ nodes[i].AddConnection(nodes[iplus]);
+ nodes[i].AddConnection(nodes[iminus]);
+ }
+
+ //Process intersection nodes - horribly inefficient
+ bool dirty = true;
+ int counter = 0;
+ while (dirty){
+ dirty = false;
+ for (int32 i=0; i < nNodes; ++i){
+ for (int32 j=0; j < nodes[i].nConnected; ++j){
+ for (int32 k=0; k < nNodes; ++k){
+ if (k==i || &nodes[k] == nodes[i].connected[j]) continue;
+ for (int32 l=0; l < nodes[k].nConnected; ++l){
+
+ if ( nodes[k].connected[l] == nodes[i].connected[j] ||
+ nodes[k].connected[l] == &nodes[i]) continue;
+ //Check intersection
+ b2Vec2 intersectPt;
+ //if (counter > 100) printf("checking intersection: %d, %d, %d, %d\n",i,j,k,l);
+ bool crosses = intersect(nodes[i].position,nodes[i].connected[j]->position,
+ nodes[k].position,nodes[k].connected[l]->position,
+ intersectPt);
+ 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
+ b2PolyNode* connj = nodes[i].connected[j];
+ b2PolyNode* 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] = b2PolyNode(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;
+ //if (counter > 100) printf("Counter: %d\n",counter);
+ }
+
+ /*
+ // Debugging: check for connection consistency
+ for (int32 i=0; i<nNodes; ++i) {
+ int32 nConn = nodes[i].nConnected;
+ for (int32 j=0; j<nConn; ++j) {
+ if (nodes[i].connected[j]->nConnected == 0) b2Assert(false);
+ b2PolyNode* connect = nodes[i].connected[j];
+ bool found = false;
+ for (int32 k=0; k<connect->nConnected; ++k) {
+ if (connect->connected[k] == &nodes[i]) found = true;
+ }
+ b2Assert(found);
+ }
+ }*/
+
+ //Collapse duplicate points
+ bool foundDupe = true;
+ int nActive = nNodes;
+ while (foundDupe){
+ foundDupe = false;
+ for (int32 i=0; i < nNodes; ++i){
+ if (nodes[i].nConnected == 0) continue;
+ for (int32 j=i+1; j < nNodes; ++j){
+ if (nodes[j].nConnected == 0) continue;
+ b2Vec2 diff = nodes[i].position - nodes[j].position;
+ if (diff.LengthSquared() <= COLLAPSE_DIST_SQR){
+ if (nActive <= 3) return b2Polygon();
+ //printf("Found dupe, %d left\n",nActive);
+ --nActive;
+ foundDupe = true;
+ b2PolyNode* inode = &nodes[i];
+ b2PolyNode* jnode = &nodes[j];
+ //Move all of j's connections to i, and orphan j
+ int32 njConn = jnode->nConnected;
+ for (int32 k=0; k < njConn; ++k){
+ b2PolyNode* knode = jnode->connected[k];
+ b2Assert(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");
+ }
+ /*
+ printf("Orphaning node at address %d\n",(int)jnode);
+ for (int32 k=0; k<njConn; ++k) {
+ if (jnode->connected[k]->IsConnectedTo(*jnode)) printf("Problem!!!\n");
+ }
+ for (int32 k=0; k < njConn; ++k){
+ jnode->RemoveConnectionByIndex(k);
+ }
+ */
+ jnode->nConnected = 0;
+ }
+ }
+ }
+ }
+
+ /*
+ // Debugging: check for connection consistency
+ for (int32 i=0; i<nNodes; ++i) {
+ int32 nConn = nodes[i].nConnected;
+ printf("Node %d has %d connections\n",i,nConn);
+ for (int32 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]));
+ b2Assert(false);
+ }
+ b2PolyNode* connect = nodes[i].connected[j];
+ bool found = false;
+ for (int32 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);
+ b2Assert(found);
+ }
+ }//*/
+
+ //Now walk the edge of the list
+
+ //Find node with minimum y value (max x if equal)
+ float32 minY = 1e10;
+ float32 maxX = -1e10;
+ int32 minYIndex = -1;
+ for (int32 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;
+ }
+ }
+
+ b2Vec2 origDir(1.0f,0.0f);
+ b2Vec2* resultVecs = new b2Vec2[4*nNodes];// nodes may be visited more than once, unfortunately - change to growable array!
+ int32 nResultVecs = 0;
+ b2PolyNode* currentNode = &nodes[minYIndex];
+ b2PolyNode* startNode = currentNode;
+ b2Assert(currentNode->nConnected > 0);
+ b2PolyNode* nextNode = currentNode->GetRightestConnection(origDir);
+ if (!nextNode) goto CleanUp; // Borked, clean up our mess and return
+ resultVecs[0] = startNode->position;
+ ++nResultVecs;
+ while (nextNode != startNode){
+ if (nResultVecs > 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);
+ p->printFormatted();
+ printf("Dumping connection graph: \n");
+ for (int32 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 (int32 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 (int32 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);
+ }
+ //*/
+ b2Assert(false); //nodes should never be visited four times apiece (proof?), so we've probably hit a loop...crap
+ }
+ resultVecs[nResultVecs++] = nextNode->position;
+ b2PolyNode* 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) goto CleanUp; // 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);
+ }
+
+ CleanUp:
+
+ float32* xres = new float32[nResultVecs];
+ float32* yres = new float32[nResultVecs];
+ for (int32 i=0; i<nResultVecs; ++i){
+ xres[i] = resultVecs[i].x;
+ yres[i] = resultVecs[i].y;
+ }
+ b2Polygon retval(xres,yres,nResultVecs);
+ delete[] resultVecs;
+ delete[] yres;
+ delete[] xres;
+ delete[] nodes;
+ return retval;
+}
+
+b2PolyNode::b2PolyNode(){
+ nConnected = 0;
+ visited = false;
+}
+b2PolyNode::b2PolyNode(b2Vec2& pos){
+ position = pos;
+ nConnected = 0;
+ visited = false;
+}
+
+void b2PolyNode::AddConnection(b2PolyNode& toMe){
+ b2Assert(nConnected < MAX_CONNECTED);
+ // Ignore duplicate additions
+ for (int32 i=0; i<nConnected; ++i) {
+ if (connected[i] == &toMe) return;
+ }
+ connected[nConnected] = &toMe;
+ ++nConnected;
+}
+
+void b2PolyNode::RemoveConnection(b2PolyNode& fromMe){
+ bool isFound = false;
+ int32 foundIndex = -1;
+ for (int32 i=0; i<nConnected; ++i){
+ if (&fromMe == connected[i]) {//.position == connected[i]->position){
+ isFound = true;
+ foundIndex = i;
+ break;
+ }
+ }
+ b2Assert(isFound);
+ --nConnected;
+ //printf("nConnected: %d\n",nConnected);
+ for (int32 i=foundIndex; i < nConnected; ++i){
+ connected[i] = connected[i+1];
+ }
+}
+void b2PolyNode::RemoveConnectionByIndex(int32 index){
+ --nConnected;
+ //printf("New nConnected = %d\n",nConnected);
+ for (int32 i=index; i < nConnected; ++i){
+ connected[i] = connected[i+1];
+ }
+}
+bool b2PolyNode::IsConnectedTo(b2PolyNode& me){
+ bool isFound = false;
+ for (int32 i=0; i<nConnected; ++i){
+ if (&me == connected[i]) {//.position == connected[i]->position){
+ isFound = true;
+ break;
+ }
+ }
+ return isFound;
+}
+b2PolyNode* b2PolyNode::GetRightestConnection(b2PolyNode* incoming){
+ if (nConnected == 0) b2Assert(false); // This means the connection graph is inconsistent
+ if (nConnected == 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;
+ }
+ b2Vec2 inDir = position - incoming->position;
+ float32 inLength = inDir.Normalize();
+ b2Assert(inLength > CMP_EPSILON);
+
+ b2PolyNode* result = NULL;
+ for (int32 i=0; i<nConnected; ++i){
+ if (connected[i] == incoming) continue;
+ b2Vec2 testDir = connected[i]->position - position;
+ float32 testLengthSqr = testDir.LengthSquared();
+ 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");
+ }*/
+ b2Assert (testLengthSqr >= COLLAPSE_DIST_SQR);
+ float32 myCos = b2Dot(inDir,testDir);
+ float32 mySin = b2Cross(inDir,testDir);
+ if (result){
+ b2Vec2 resultDir = result->position - position;
+ resultDir.Normalize();
+ float32 resCos = b2Dot(inDir,resultDir);
+ float32 resSin = b2Cross(inDir,resultDir);
+ if (IsRighter(mySin,myCos,resSin,resCos)){
+ result = connected[i];
+ }
+ } else{
+ result = connected[i];
+ }
+ }
+ if (B2_POLYGON_REPORT_ERRORS && !result) {
+ printf("nConnected = %d\n",nConnected);
+ for (int32 i=0; i<nConnected; ++i) {
+ printf("connected[%d] @ %d\n",i,0);//(int)connected[i]);
+ }
+ }
+ b2Assert(result);
+
+ return result;
+}
+
+b2PolyNode* b2PolyNode::GetRightestConnection(b2Vec2& incomingDir){
+ b2Vec2 diff = position-incomingDir;
+ b2PolyNode temp(diff);
+ b2PolyNode* res = GetRightestConnection(&temp);
+ b2Assert(res);
+ return res;
+}
+}
generated by cgit v1.2.3 (git 2.25.1) at 2024-11-22 06:19:59 +0000