|
|
@@ -1,1591 +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) {
|
|
|
- // -- GODOT start --
|
|
|
- delete[] nodes;
|
|
|
- // -- GODOT end --
|
|
|
- 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;
|
|
|
-}
|
|
|
-}
|
Rémi Verschelde