summaryrefslogtreecommitdiff
path: root/drivers/convex_decomp/b2Polygon.cpp
diff options
context:
space:
mode:
Diffstat (limited to 'drivers/convex_decomp/b2Polygon.cpp')
-rw-r--r--drivers/convex_decomp/b2Polygon.cpp1586
1 files changed, 0 insertions, 1586 deletions
diff --git a/drivers/convex_decomp/b2Polygon.cpp b/drivers/convex_decomp/b2Polygon.cpp
deleted file mode 100644
index b6ead62c63..0000000000
--- a/drivers/convex_decomp/b2Polygon.cpp
+++ /dev/null
@@ -1,1586 +0,0 @@
-/*
- * 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-12-26 22:03:33 +0000