b2Polygon.cpp
上传用户:gb3593
上传日期:2022-01-07
资源大小:3028k
文件大小:49k
源码类别:

游戏引擎

开发平台:

Visual C++

  1. /*
  2.  * Copyright (c) 2007 Eric Jordan
  3.  *
  4.  * This software is provided 'as-is', without any express or implied
  5.  * warranty.  In no event will the authors be held liable for any damages
  6.  * arising from the use of this software.
  7.  * Permission is granted to anyone to use this software for any purpose,
  8.  * including commercial applications, and to alter it and redistribute it
  9.  * freely, subject to the following restrictions:
  10.  * 1. The origin of this software must not be misrepresented; you must not
  11.  * claim that you wrote the original software. If you use this software
  12.  * in a product, an acknowledgment in the product documentation would be
  13.  * appreciated but is not required.
  14.  * 2. Altered source versions must be plainly marked as such, and must not be
  15.  * misrepresented as being the original software.
  16.  * 3. This notice may not be removed or altered from any source distribution.
  17.  */
  18. // This utility works with Box2d version 2.0 (or higher), and not with 1.4.3
  19. #include "b2Triangle.h"
  20. #include "b2Polygon.h"
  21. #include <cmath>
  22. #include <climits>
  23. static bool B2_POLYGON_REPORT_ERRORS = false;
  24. //If you're using 1.4.3, b2_toiSlop won't exist, so set this equal to 0
  25. static const float32 toiSlop = 0.0f;
  26. /*
  27.  * Check if the lines a0->a1 and b0->b1 cross.
  28.  * If they do, intersectionPoint will be filled
  29.  * with the point of crossing.
  30.  *
  31.  * Grazing lines should not return true.
  32.  */
  33. bool intersect(const b2Vec2& a0, const b2Vec2& a1,
  34.    const b2Vec2& b0, const b2Vec2& b1, 
  35.    b2Vec2& intersectionPoint) {
  36. if (a0 == b0 || a0 == b1 || a1 == b0 || a1 == b1) return false;
  37. float x1 = a0.x; float y1 = a0.y;
  38. float x2 = a1.x; float y2 = a1.y;
  39. float x3 = b0.x; float y3 = b0.y;
  40. float x4 = b1.x; float y4 = b1.y;
  41. //AABB early exit
  42. if (b2Max(x1,x2) < b2Min(x3,x4) || b2Max(x3,x4) < b2Min(x1,x2) ) return false;
  43. if (b2Max(y1,y2) < b2Min(y3,y4) || b2Max(y3,y4) < b2Min(y1,y2) ) return false;
  44. float ua = ((x4 - x3) * (y1 - y3) - (y4 - y3) * (x1 - x3));
  45. float ub = ((x2 - x1) * (y1 - y3) - (y2 - y1) * (x1 - x3));
  46. float denom = (y4 - y3) * (x2 - x1) - (x4 - x3) * (y2 - y1);
  47. if (b2Abs(denom) < FLT_EPSILON) {
  48. //Lines are too close to parallel to call
  49. return false;
  50. }
  51. ua /= denom;
  52. ub /= denom;
  53. if ((0 < ua) && (ua < 1) && (0 < ub) && (ub < 1)) {
  54. //if (intersectionPoint){
  55. intersectionPoint.x = (x1 + ua * (x2 - x1));
  56. intersectionPoint.y = (y1 + ua * (y2 - y1));
  57. //}
  58. //printf("%f, %f -> %f, %f crosses %f, %f -> %f, %fn",x1,y1,x2,y2,x3,y3,x4,y4);
  59. return true;
  60. }
  61. return false;
  62. }
  63. /*
  64.  * True if line from a0->a1 intersects b0->b1
  65.  */
  66. bool intersect(const b2Vec2& a0, const b2Vec2& a1,
  67.    const b2Vec2& b0, const b2Vec2& b1) {
  68. b2Vec2 myVec(0.0f,0.0f);
  69. return intersect(a0, a1, b0, b1, myVec);
  70. }
  71. b2Polygon::b2Polygon(float32* _x, float32* _y, int32 nVert) {
  72.         nVertices = nVert;
  73.         x = new float32[nVertices];
  74.         y = new float32[nVertices];
  75.         for (int32 i = 0; i < nVertices; ++i) {
  76.             x[i] = _x[i];
  77.             y[i] = _y[i];
  78.         }
  79. areaIsSet = false;
  80. }
  81. b2Polygon::b2Polygon(b2Vec2* v, int32 nVert) {
  82.         nVertices = nVert;
  83.         x = new float32[nVertices];
  84.         y = new float32[nVertices];
  85.         for (int32 i = 0; i < nVertices; ++i) {
  86.             x[i] = v[i].x;
  87.             y[i] = v[i].y;
  88.         }
  89. areaIsSet = false;
  90. }
  91. b2Polygon::b2Polygon() {
  92. x = NULL;
  93. y = NULL;
  94. nVertices = 0;
  95. areaIsSet = false;
  96. }
  97. b2Polygon::~b2Polygon() {
  98. //printf("About to delete poly with %d verticesn",nVertices);
  99. delete[] x;
  100. delete[] y;
  101. }
  102. float32 b2Polygon::GetArea() {
  103. // TODO: fix up the areaIsSet caching so that it can be used
  104. //if (areaIsSet) return area;
  105. area = 0.0f;
  106. //First do wraparound
  107. area += x[nVertices-1]*y[0]-x[0]*y[nVertices-1];
  108. for (int i=0; i<nVertices-1; ++i){
  109. area += x[i]*y[i+1]-x[i+1]*y[i];
  110. }
  111. area *= .5f;
  112. areaIsSet = true;
  113. return area;
  114. }
  115. bool b2Polygon::IsCCW() {
  116. return (GetArea() > 0.0f);
  117. }
  118. void b2Polygon::MergeParallelEdges(float32 tolerance) {
  119. if (nVertices <= 3) return; //Can't do anything useful here to a triangle
  120. bool* mergeMe = new bool[nVertices];
  121. int32 newNVertices = nVertices;
  122. for (int32 i = 0; i < nVertices; ++i) {
  123. int32 lower = (i == 0) ? (nVertices - 1) : (i - 1);
  124. int32 middle = i;
  125. int32 upper = (i == nVertices - 1) ? (0) : (i + 1);
  126. float32 dx0 = x[middle] - x[lower];
  127. float32 dy0 = y[middle] - y[lower];
  128. float32 dx1 = x[upper] - x[middle];
  129. float32 dy1 = y[upper] - y[middle];
  130. float32 norm0 = sqrtf(dx0*dx0+dy0*dy0);
  131. float32 norm1 = sqrtf(dx1*dx1+dy1*dy1);
  132. if ( !(norm0 > 0.0f && norm1 > 0.0f) && newNVertices > 3 ) {
  133. //Merge identical points
  134. mergeMe[i] = true;
  135. --newNVertices;
  136. }
  137. dx0 /= norm0; dy0 /= norm0;
  138. dx1 /= norm1; dy1 /= norm1;
  139. float32 cross = dx0 * dy1 - dx1 * dy0;
  140. float32 dot = dx0 * dx1 + dy0 * dy1;
  141. if (fabs(cross) < tolerance && dot > 0 && newNVertices > 3) {
  142. mergeMe[i] = true;
  143. --newNVertices;
  144. } else {
  145. mergeMe[i] = false;
  146. }
  147. }
  148. if(newNVertices == nVertices || newNVertices == 0) {
  149. delete[] mergeMe;
  150. return;
  151. }
  152. float32* newx = new float32[newNVertices];
  153. float32* newy = new float32[newNVertices];
  154. int32 currIndex = 0;
  155. for (int32 i=0; i < nVertices; ++i) {
  156. if (mergeMe[i] || newNVertices == 0 || currIndex == newNVertices) continue;
  157. b2Assert(currIndex < newNVertices);
  158. newx[currIndex] = x[i];
  159. newy[currIndex] = y[i];
  160. ++currIndex;
  161. }
  162. delete[] x;
  163. delete[] y;
  164. delete[] mergeMe;
  165. x = newx;
  166. y = newy;
  167. nVertices = newNVertices;
  168. // printf("%d n", newNVertices);
  169. }
  170.     /* 
  171.  * Allocates and returns pointer to vector vertex array.
  172.      *  Length of array is nVertices.
  173.  */
  174. b2Vec2* b2Polygon::GetVertexVecs() {
  175.         b2Vec2* out = new b2Vec2[nVertices];
  176.         for (int32 i = 0; i < nVertices; ++i) {
  177.             out[i].Set(x[i], y[i]);
  178.         }
  179.         return out;
  180. }
  181. b2Polygon::b2Polygon(b2Triangle& t) {
  182. nVertices = 3;
  183. x = new float[nVertices];
  184. y = new float[nVertices];
  185. for (int32 i = 0; i < nVertices; ++i) {
  186. x[i] = t.x[i];
  187. y[i] = t.y[i];
  188. }
  189. }
  190. void b2Polygon::Set(const b2Polygon& p) {
  191.         if (nVertices != p.nVertices){
  192. nVertices = p.nVertices;
  193. if (x) delete[] x;
  194. if (y) delete[] y;
  195. x = new float32[nVertices];
  196. y = new float32[nVertices];
  197.         }
  198.         for (int32 i = 0; i < nVertices; ++i) {
  199.             x[i] = p.x[i];
  200.             y[i] = p.y[i];
  201.         }
  202. areaIsSet = false;
  203. }
  204.     /*
  205.      * Assuming the polygon is simple, checks if it is convex.
  206.      */
  207. bool b2Polygon::IsConvex() {
  208.         bool isPositive = false;
  209.         for (int32 i = 0; i < nVertices; ++i) {
  210.             int32 lower = (i == 0) ? (nVertices - 1) : (i - 1);
  211.             int32 middle = i;
  212.             int32 upper = (i == nVertices - 1) ? (0) : (i + 1);
  213.             float32 dx0 = x[middle] - x[lower];
  214.             float32 dy0 = y[middle] - y[lower];
  215.             float32 dx1 = x[upper] - x[middle];
  216.             float32 dy1 = y[upper] - y[middle];
  217.             float32 cross = dx0 * dy1 - dx1 * dy0;
  218.             // Cross product should have same sign
  219.             // for each vertex if poly is convex.
  220.             bool newIsP = (cross >= 0) ? true : false;
  221.             if (i == 0) {
  222.                 isPositive = newIsP;
  223.             }
  224.             else if (isPositive != newIsP) {
  225.                 return false;
  226.             }
  227.         }
  228.         return true;
  229. }
  230. /*
  231.  * Pulled from b2Shape.cpp, assertions removed
  232.  */
  233. static b2Vec2 PolyCentroid(const b2Vec2* vs, int32 count)
  234. {
  235. b2Vec2 c; c.Set(0.0f, 0.0f);
  236. float32 area = 0.0f;
  237. const float32 inv3 = 1.0f / 3.0f;
  238. b2Vec2 pRef(0.0f, 0.0f);
  239. for (int32 i = 0; i < count; ++i)
  240. {
  241. // Triangle vertices.
  242. b2Vec2 p1 = pRef;
  243. b2Vec2 p2 = vs[i];
  244. b2Vec2 p3 = i + 1 < count ? vs[i+1] : vs[0];
  245. b2Vec2 e1 = p2 - p1;
  246. b2Vec2 e2 = p3 - p1;
  247. float32 D = b2Cross(e1, e2);
  248. float32 triangleArea = 0.5f * D;
  249. area += triangleArea;
  250. // Area weighted centroid
  251. c += triangleArea * inv3 * (p1 + p2 + p3);
  252. }
  253. // Centroid
  254. c *= 1.0f / area;
  255. return c;
  256. }
  257. /*
  258.  * Checks if polygon is valid for use in Box2d engine.
  259.  * Last ditch effort to ensure no invalid polygons are
  260.  * added to world geometry.
  261.  *
  262.  * Performs a full check, for simplicity, convexity,
  263.  * orientation, minimum angle, and volume.  This won't
  264.  * be very efficient, and a lot of it is redundant when
  265.  * other tools in this section are used.
  266.  */
  267. bool b2Polygon::IsUsable(bool printErrors){
  268. int32 error = -1;
  269. bool noError = true;
  270. if (nVertices < 3 || nVertices > b2_maxPolygonVertices) {noError = false; error = 0;}
  271. if (!IsConvex()) {noError = false; error = 1;}
  272. if (!IsSimple()) {noError = false; error = 2;}
  273. if (GetArea() < FLT_EPSILON) {noError = false; error = 3;}
  274. //Compute normals
  275. b2Vec2* normals = new b2Vec2[nVertices];
  276. b2Vec2* vertices = new b2Vec2[nVertices];
  277. for (int32 i = 0; i < nVertices; ++i){
  278. vertices[i].Set(x[i],y[i]);
  279. int32 i1 = i;
  280. int32 i2 = i + 1 < nVertices ? i + 1 : 0;
  281. b2Vec2 edge(x[i2]-x[i1],y[i2]-y[i1]);
  282. normals[i] = b2Cross(edge, 1.0f);
  283. normals[i].Normalize();
  284. }
  285. //Required side checks
  286. for (int32 i=0; i<nVertices; ++i){
  287. int32 iminus = (i==0)?nVertices-1:i-1;
  288. //int32 iplus = (i==nVertices-1)?0:i+1;
  289. //Parallel sides check
  290. float32 cross = b2Cross(normals[iminus], normals[i]);
  291. cross = b2Clamp(cross, -1.0f, 1.0f);
  292. float32 angle = asinf(cross);
  293. if(angle <= b2_angularSlop){
  294. noError = false;
  295. error = 4;
  296. break;
  297. }
  298. //Too skinny check
  299. for (int32 j=0; j<nVertices; ++j){
  300. if (j == i || j == (i + 1) % nVertices){
  301. continue;
  302. }
  303. float32 s = b2Dot(normals[i], vertices[j] - vertices[i]);
  304. if (s >= -b2_linearSlop){
  305. noError = false;
  306. error = 5;
  307. }
  308. }
  309. b2Vec2 centroid = PolyCentroid(vertices,nVertices);
  310. b2Vec2 n1 = normals[iminus];
  311. b2Vec2 n2 = normals[i];
  312. b2Vec2 v = vertices[i] - centroid;;
  313. b2Vec2 d;
  314. d.x = b2Dot(n1, v) - toiSlop;
  315. d.y = b2Dot(n2, v) - toiSlop;
  316. // Shifting the edge inward by b2_toiSlop should
  317. // not cause the plane to pass the centroid.
  318. if ((d.x < 0.0f)||(d.y < 0.0f)){
  319. noError = false;
  320. error = 6;
  321. }
  322. }
  323. delete[] vertices;
  324. delete[] normals;
  325. if (!noError && printErrors){
  326. printf("Found invalid polygon, ");
  327. switch(error){
  328. case 0:
  329. printf("must have between 3 and %d vertices.n",b2_maxPolygonVertices);
  330. break;
  331. case 1:
  332. printf("must be convex.n");
  333. break;
  334. case 2:
  335. printf("must be simple (cannot intersect itself).n");
  336. break;
  337. case 3:
  338. printf("area is too small.n");
  339. break;
  340. case 4:
  341. printf("sides are too close to parallel.n");
  342. break;
  343. case 5:
  344. printf("polygon is too thin.n");
  345. break;
  346. case 6:
  347. printf("core shape generation would move edge past centroid (too thin).n");
  348. break;
  349. default:
  350. printf("don't know why.n");
  351. }
  352. }
  353. return noError;
  354. }
  355. bool b2Polygon::IsUsable(){
  356. return IsUsable(B2_POLYGON_REPORT_ERRORS);
  357. }
  358. //Check for edge crossings
  359. bool b2Polygon::IsSimple() {
  360. for (int32 i=0; i<nVertices; ++i){
  361. int32 iplus = (i+1 > nVertices-1)?0:i+1;
  362. b2Vec2 a1(x[i],y[i]);
  363. b2Vec2 a2(x[iplus],y[iplus]);
  364. for (int32 j=i+1; j<nVertices; ++j){
  365. int32 jplus = (j+1 > nVertices-1)?0:j+1;
  366. b2Vec2 b1(x[j],y[j]);
  367. b2Vec2 b2(x[jplus],y[jplus]);
  368. if (intersect(a1,a2,b1,b2)){
  369. return false;
  370. }
  371. }
  372. }
  373. return true;
  374. }
  375.     
  376.     /*
  377.      * Tries to add a triangle to the polygon. Returns null if it can't connect
  378.      * properly, otherwise returns a pointer to the new Polygon. Assumes bitwise
  379.      * equality of joined vertex positions.
  380.  *
  381.  * Remember to delete the pointer afterwards.
  382.  * Todo: Make this return a b2Polygon instead
  383.  * of a pointer to a heap-allocated one.
  384.  *
  385.  * For internal use.
  386.      */
  387. b2Polygon* b2Polygon::Add(b2Triangle& t) {
  388. // float32 equalTol = .001f;
  389.         // First, find vertices that connect
  390.         int32 firstP = -1;
  391.         int32 firstT = -1;
  392.         int32 secondP = -1;
  393.         int32 secondT = -1;
  394.         for (int32 i = 0; i < nVertices; i++) {
  395.             if (t.x[0] == x[i] && t.y[0] == y[i]) {
  396.                 if (firstP == -1) {
  397.                     firstP = i;
  398.                     firstT = 0;
  399.                 }
  400.                 else {
  401.                     secondP = i;
  402.                     secondT = 0;
  403.                 }
  404.             }
  405.             else if (t.x[1] == x[i] && t.y[1] == y[i]) {
  406.                 if (firstP == -1) {
  407.                     firstP = i;
  408.                     firstT = 1;
  409.                 }
  410.                 else {
  411.                     secondP = i;
  412.                     secondT = 1;
  413.                 }
  414.             }
  415.             else if (t.x[2] == x[i] && t.y[2] == y[i]) {
  416.                 if (firstP == -1) {
  417.                     firstP = i;
  418.                     firstT = 2;
  419.                 }
  420.                 else {
  421.                     secondP = i;
  422.                     secondT = 2;
  423.                 }
  424.             }
  425.             else {
  426.             }
  427.         }
  428.         // Fix ordering if first should be last vertex of poly
  429.         if (firstP == 0 && secondP == nVertices - 1) {
  430.             firstP = nVertices - 1;
  431.             secondP = 0;
  432.         }
  433.         // Didn't find it
  434.         if (secondP == -1) {
  435.     return NULL;
  436. }
  437.         // Find tip index on triangle
  438.         int32 tipT = 0;
  439.         if (tipT == firstT || tipT == secondT)
  440.             tipT = 1;
  441.         if (tipT == firstT || tipT == secondT)
  442.             tipT = 2;
  443.         float32* newx = new float[nVertices + 1];
  444.         float32* newy = new float[nVertices + 1];
  445.         int32 currOut = 0;
  446.         for (int32 i = 0; i < nVertices; i++) {
  447.             newx[currOut] = x[i];
  448.             newy[currOut] = y[i];
  449.             if (i == firstP) {
  450.                 ++currOut;
  451.                 newx[currOut] = t.x[tipT];
  452.                 newy[currOut] = t.y[tipT];
  453.             }
  454.             ++currOut;
  455.         }
  456.         b2Polygon* result = new b2Polygon(newx, newy, nVertices+1);
  457.         delete[] newx;
  458.         delete[] newy;
  459.         return result;
  460. }
  461.     /**
  462.      * Adds this polygon to a PolyDef.
  463.      */
  464. void b2Polygon::AddTo(b2FixtureDef& pd) {
  465. if (nVertices < 3) return;
  466. b2Assert(nVertices <= b2_maxPolygonVertices);
  467. b2Vec2* vecs = GetVertexVecs();
  468. b2Vec2* vecsToAdd = new b2Vec2[nVertices];
  469. int32 offset = 0;
  470. b2PolygonShape *polyShape = new b2PolygonShape;
  471. int32 ind;
  472.     for (int32 i = 0; i < nVertices; ++i) {
  473. //Omit identical neighbors (including wraparound)
  474. ind = i - offset;
  475. if (vecs[i].x==vecs[remainder(i+1,nVertices)].x &&
  476. vecs[i].y==vecs[remainder(i+1,nVertices)].y){
  477. offset++;
  478. continue;
  479. }
  480. vecsToAdd[ind] = vecs[i];
  481.     }
  482. polyShape->Set((const b2Vec2*)vecsToAdd, ind+1);
  483. pd.shape = polyShape;
  484.     delete[] vecs;
  485. delete[] vecsToAdd;
  486. }
  487. /**
  488.  * Finds and fixes "pinch points," points where two polygon
  489.  * vertices are at the same point.
  490.  *
  491.  * If a pinch point is found, pin is broken up into poutA and poutB
  492.  * and true is returned; otherwise, returns false.
  493.  *
  494.  * Mostly for internal use.
  495.  */
  496. bool ResolvePinchPoint(const b2Polygon& pin, b2Polygon& poutA, b2Polygon& poutB){
  497. if (pin.nVertices < 3) return false;
  498. float32 tol = .001f;
  499. bool hasPinchPoint = false;
  500. int32 pinchIndexA = -1;
  501. int32 pinchIndexB = -1;
  502. for (int i=0; i<pin.nVertices; ++i){
  503. for (int j=i+1; j<pin.nVertices; ++j){
  504. //Don't worry about pinch points where the points
  505. //are actually just dupe neighbors
  506. if (b2Abs(pin.x[i]-pin.x[j])<tol&&b2Abs(pin.y[i]-pin.y[j])<tol&&j!=i+1){
  507. pinchIndexA = i;
  508. pinchIndexB = j;
  509. //printf("pinch: %f, %f == %f, %fn",pin.x[i],pin.y[i],pin.x[j],pin.y[j]);
  510. //printf("at indexes %d, %dn",i,j);
  511. hasPinchPoint = true;
  512. break;
  513. }
  514. }
  515. if (hasPinchPoint) break;
  516. }
  517. if (hasPinchPoint){
  518. //printf("Found pinch pointn");
  519. int32 sizeA = pinchIndexB - pinchIndexA;
  520. if (sizeA == pin.nVertices) return false;//has dupe points at wraparound, not a problem here
  521. float32* xA = new float32[sizeA];
  522. float32* yA = new float32[sizeA];
  523. for (int32 i=0; i < sizeA; ++i){
  524. int32 ind = remainder(pinchIndexA+i,pin.nVertices);
  525. xA[i] = pin.x[ind];
  526. yA[i] = pin.y[ind];
  527. }
  528. b2Polygon tempA(xA,yA,sizeA);
  529. poutA.Set(tempA);
  530. delete[] xA;
  531. delete[] yA;
  532. int32 sizeB = pin.nVertices - sizeA;
  533. float32* xB = new float32[sizeB];
  534. float32* yB = new float32[sizeB];
  535. for (int32 i=0; i<sizeB; ++i){
  536. int32 ind = remainder(pinchIndexB+i,pin.nVertices);
  537. xB[i] = pin.x[ind];
  538. yB[i] = pin.y[ind];
  539. }
  540. b2Polygon tempB(xB,yB,sizeB);
  541. poutB.Set(tempB);
  542. //printf("Size of a: %d, size of b: %dn",sizeA,sizeB);
  543. delete[] xB;
  544. delete[] yB;
  545. }
  546. return hasPinchPoint;
  547. }
  548.     /**
  549.      * Triangulates a polygon using simple ear-clipping algorithm. Returns
  550.      * size of Triangle array unless the polygon can't be triangulated.
  551.      * This should only happen if the polygon self-intersects,
  552.      * though it will not _always_ return null for a bad polygon - it is the
  553.      * caller's responsibility to check for self-intersection, and if it
  554.      * doesn't, it should at least check that the return value is non-null
  555.      * before using. You're warned!
  556.  *
  557.  * Triangles may be degenerate, especially if you have identical points
  558.  * in the input to the algorithm.  Check this before you use them.
  559.      *
  560.      * This is totally unoptimized, so for large polygons it should not be part
  561.      * of the simulation loop.
  562.      *
  563.      * Returns:
  564.      * -1 if algorithm fails (self-intersection most likely)
  565.      * 0 if there are not enough vertices to triangulate anything.
  566.      * Number of triangles if triangulation was successful.
  567.      *
  568.      * results will be filled with results - ear clipping always creates vNum - 2
  569.      * or fewer (due to pinch point polygon snipping), so allocate an array of
  570.  * this size.
  571.      */
  572. int32 TriangulatePolygon(float32* xv, float32* yv, int32 vNum, b2Triangle* results) {
  573.         if (vNum < 3)
  574.             return 0;
  575. //Recurse and split on pinch points
  576. b2Polygon pA,pB;
  577. b2Polygon pin(xv,yv,vNum);
  578. if (ResolvePinchPoint(pin,pA,pB)){
  579. b2Triangle* mergeA = new b2Triangle[pA.nVertices];
  580. b2Triangle* mergeB = new b2Triangle[pB.nVertices];
  581. int32 nA = TriangulatePolygon(pA.x,pA.y,pA.nVertices,mergeA);
  582. int32 nB = TriangulatePolygon(pB.x,pB.y,pB.nVertices,mergeB);
  583. if (nA==-1 || nB==-1){
  584. delete[] mergeA;
  585. delete[] mergeB;
  586. return -1;
  587. }
  588. for (int32 i=0; i<nA; ++i){
  589. results[i].Set(mergeA[i]);
  590. }
  591. for (int32 i=0; i<nB; ++i){
  592. results[nA+i].Set(mergeB[i]);
  593. }
  594. delete[] mergeA;
  595. delete[] mergeB;
  596. return (nA+nB);
  597. }
  598.         b2Triangle* buffer = new b2Triangle[vNum-2];
  599.         int32 bufferSize = 0;
  600.         float32* xrem = new float32[vNum];
  601.         float32* yrem = new float32[vNum];
  602.         for (int32 i = 0; i < vNum; ++i) {
  603.             xrem[i] = xv[i];
  604.             yrem[i] = yv[i];
  605.         }
  606. int xremLength = vNum;
  607.         while (vNum > 3) {
  608.             // Find an ear
  609.             int32 earIndex = -1;
  610. //float32 earVolume = -1.0f;
  611. float32 earMaxMinCross = -10.0f;
  612.             for (int32 i = 0; i < vNum; ++i) {
  613.                 if (IsEar(i, xrem, yrem, vNum)) {
  614. int32 lower = remainder(i-1,vNum);
  615. int32 upper = remainder(i+1,vNum);
  616. b2Vec2 d1(xrem[upper]-xrem[i],yrem[upper]-yrem[i]);
  617. b2Vec2 d2(xrem[i]-xrem[lower],yrem[i]-yrem[lower]);
  618. b2Vec2 d3(xrem[lower]-xrem[upper],yrem[lower]-yrem[upper]);
  619. d1.Normalize();
  620. d2.Normalize();
  621. d3.Normalize();
  622. float32 cross12 = b2Abs( b2Cross(d1,d2) );
  623. float32 cross23 = b2Abs( b2Cross(d2,d3) );
  624. float32 cross31 = b2Abs( b2Cross(d3,d1) );
  625. //Find the maximum minimum angle
  626. float32 minCross = b2Min(cross12, b2Min(cross23,cross31));
  627. if (minCross > earMaxMinCross){
  628. earIndex = i;
  629. earMaxMinCross = minCross;
  630. }
  631. /*//This bit chooses the ear with greatest volume first
  632. float32 testVol = b2Abs( d1.x*d2.y-d2.x*d1.y );
  633. if (testVol > earVolume){
  634. earIndex = i;
  635. earVolume = testVol;
  636. }*/
  637.                 }
  638.             }
  639.             // If we still haven't found an ear, we're screwed.
  640.             // Note: sometimes this is happening because the
  641. // remaining points are collinear.  Really these
  642. // should just be thrown out without halting triangulation.
  643. if (earIndex == -1){
  644. if (B2_POLYGON_REPORT_ERRORS){
  645. b2Polygon dump(xrem,yrem,vNum);
  646. printf("Couldn't find an ear, dumping remaining poly:n");
  647. dump.printFormatted();
  648. printf("Please submit this dump to ewjordan at Box2d forumsn");
  649. }
  650. for (int32 i = 0; i < bufferSize; i++) {
  651. results[i].Set(buffer[i]);
  652. }
  653. delete[] buffer;
  654. if (bufferSize > 0) return bufferSize;
  655.                 else return -1;
  656. }
  657.             // Clip off the ear:
  658.             // - remove the ear tip from the list
  659.             --vNum;
  660.             float32* newx = new float32[vNum];
  661.             float32* newy = new float32[vNum];
  662.             int32 currDest = 0;
  663.             for (int32 i = 0; i < vNum; ++i) {
  664.                 if (currDest == earIndex) ++currDest;
  665.                 newx[i] = xrem[currDest];
  666.                 newy[i] = yrem[currDest];
  667.                 ++currDest;
  668.             }
  669.             // - add the clipped triangle to the triangle list
  670.             int32 under = (earIndex == 0) ? (vNum) : (earIndex - 1);
  671.             int32 over = (earIndex == vNum) ? 0 : (earIndex + 1);
  672.             b2Triangle toAdd = b2Triangle(xrem[earIndex], yrem[earIndex], xrem[over], yrem[over], xrem[under], yrem[under]);
  673.             buffer[bufferSize].Set(toAdd);
  674.             ++bufferSize;
  675.             // - replace the old list with the new one
  676.             delete[] xrem;
  677.             delete[] yrem;
  678.             xrem = newx;
  679.             yrem = newy;
  680.         }
  681.         b2Triangle toAdd = b2Triangle(xrem[1], yrem[1], xrem[2], yrem[2],
  682.   xrem[0], yrem[0]);
  683.         buffer[bufferSize].Set(toAdd);
  684.         ++bufferSize;
  685.         delete[] xrem;
  686.         delete[] yrem;
  687.         b2Assert(bufferSize == xremLength-2);
  688.         for (int32 i = 0; i < bufferSize; i++) {
  689.             results[i].Set(buffer[i]);
  690.         }
  691.         delete[] buffer;
  692.         return bufferSize;
  693. }
  694.     /**
  695.  * Turns a list of triangles into a list of convex polygons. Very simple
  696.      * method - start with a seed triangle, keep adding triangles to it until
  697.      * you can't add any more without making the polygon non-convex.
  698.      *
  699.      * Returns an integer telling how many polygons were created.  Will fill
  700.      * polys array up to polysLength entries, which may be smaller or larger
  701.      * than the return value.
  702.      * 
  703.      * Takes O(N*P) where P is the number of resultant polygons, N is triangle
  704.      * count.
  705.      * 
  706.      * The final polygon list will not necessarily be minimal, though in
  707.      * practice it works fairly well.
  708.      */
  709. int32 PolygonizeTriangles(b2Triangle* triangulated, int32 triangulatedLength, b2Polygon* polys, int32 polysLength) {
  710.         int32 polyIndex = 0;
  711.         if (triangulatedLength <= 0) {
  712.             return 0;
  713.         }
  714.         else {
  715.             int* covered = new int[triangulatedLength];
  716.             for (int32 i = 0; i < triangulatedLength; ++i) {
  717. covered[i] = 0;
  718. //Check here for degenerate triangles
  719. if ( ( (triangulated[i].x[0] == triangulated[i].x[1]) && (triangulated[i].y[0] == triangulated[i].y[1]) )
  720.  || ( (triangulated[i].x[1] == triangulated[i].x[2]) && (triangulated[i].y[1] == triangulated[i].y[2]) )
  721.  || ( (triangulated[i].x[0] == triangulated[i].x[2]) && (triangulated[i].y[0] == triangulated[i].y[2]) ) ) {
  722. covered[i] = 1;
  723. }
  724.             }
  725.             bool notDone = true;
  726.             while (notDone) {
  727.                 int32 currTri = -1;
  728.                 for (int32 i = 0; i < triangulatedLength; ++i) {
  729.                     if (covered[i])
  730.                         continue;
  731.                     currTri = i;
  732.                     break;
  733.                 }
  734.                 if (currTri == -1) {
  735.                     notDone = false;
  736.                 }
  737.                 else {
  738.                     b2Polygon poly(triangulated[currTri]);
  739. covered[currTri] = 1;
  740. int32 index = 0;
  741.                     for (int32 i = 0; i < 2*triangulatedLength; ++i,++index) {
  742. while (index >= triangulatedLength) index -= triangulatedLength;
  743.                         if (covered[index]) {
  744.                             continue;
  745. }
  746.                         b2Polygon* newP = poly.Add(triangulated[index]);
  747.                         if (!newP) {
  748.                             continue;
  749. }
  750. if (newP->nVertices > b2Polygon::maxVerticesPerPolygon) {
  751. delete newP;
  752. newP = NULL;
  753.                             continue;
  754. }
  755.                         if (newP->IsConvex()) { //Or should it be IsUsable?  Maybe re-write IsConvex to apply the angle threshold from Box2d
  756.                             poly.Set(*newP);
  757. delete newP;
  758. newP = NULL;
  759.                             covered[index] = 1;
  760.                         } else {
  761. delete newP;
  762. newP = NULL;
  763. }
  764.                     }
  765.                     if (polyIndex < polysLength){
  766. poly.MergeParallelEdges(b2_angularSlop);
  767. //If identical points are present, a triangle gets
  768. //borked by the MergeParallelEdges function, hence
  769. //the vertex number check
  770. if (poly.nVertices >= 3) polys[polyIndex].Set(poly);
  771. //else printf("Skipping corrupt polyn");
  772. }
  773.                     if (poly.nVertices >= 3) polyIndex++; //Must be outside (polyIndex < polysLength) test
  774.                 }
  775. //printf("MEMCHECK: %dn",_CrtCheckMemory());
  776.             }
  777.             delete[] covered;
  778.         }
  779.         return polyIndex;
  780. }
  781.     /**
  782.  * Checks if vertex i is the tip of an ear in polygon defined by xv[] and
  783.      * yv[].
  784.  *
  785.  * Assumes clockwise orientation of polygon...ick
  786.      */
  787. bool IsEar(int32 i, float32* xv, float32* yv, int32 xvLength) {
  788.         float32 dx0, dy0, dx1, dy1;
  789.         dx0 = dy0 = dx1 = dy1 = 0;
  790.         if (i >= xvLength || i < 0 || xvLength < 3) {
  791.             return false;
  792.         }
  793.         int32 upper = i + 1;
  794.         int32 lower = i - 1;
  795.         if (i == 0) {
  796.             dx0 = xv[0] - xv[xvLength - 1];
  797.             dy0 = yv[0] - yv[xvLength - 1];
  798.             dx1 = xv[1] - xv[0];
  799.             dy1 = yv[1] - yv[0];
  800.             lower = xvLength - 1;
  801.         }
  802.         else if (i == xvLength - 1) {
  803.             dx0 = xv[i] - xv[i - 1];
  804.             dy0 = yv[i] - yv[i - 1];
  805.             dx1 = xv[0] - xv[i];
  806.             dy1 = yv[0] - yv[i];
  807.             upper = 0;
  808.         }
  809.         else {
  810.             dx0 = xv[i] - xv[i - 1];
  811.             dy0 = yv[i] - yv[i - 1];
  812.             dx1 = xv[i + 1] - xv[i];
  813.             dy1 = yv[i + 1] - yv[i];
  814.         }
  815.         float32 cross = dx0 * dy1 - dx1 * dy0;
  816.         if (cross > 0)
  817.             return false;
  818.         b2Triangle myTri(xv[i], yv[i], xv[upper], yv[upper],
  819.   xv[lower], yv[lower]);
  820.         for (int32 j = 0; j < xvLength; ++j) {
  821.             if (j == i || j == lower || j == upper)
  822.                 continue;
  823.             if (myTri.IsInside(xv[j], yv[j]))
  824.                 return false;
  825.         }
  826.         return true;
  827. }
  828. void ReversePolygon(b2Polygon& p){
  829. ReversePolygon(p.x,p.y,p.nVertices);
  830. }
  831. void ReversePolygon(float* x, float* y, int n) {
  832.         if (n == 1)
  833.             return;
  834.         int32 low = 0;
  835.         int32 high = n - 1;
  836.         while (low < high) {
  837.             float32 buffer = x[low];
  838.             x[low] = x[high];
  839.             x[high] = buffer;
  840.             buffer = y[low];
  841.             y[low] = y[high];
  842.             y[high] = buffer;
  843.             ++low;
  844.             --high;
  845.         }
  846. }
  847.     /**
  848.  * Decomposes a non-convex polygon into a number of convex polygons, up
  849.      * to maxPolys (remaining pieces are thrown out, but the total number
  850.  * is returned, so the return value can be greater than maxPolys).
  851.      *
  852.      * Each resulting polygon will have no more than maxVerticesPerPolygon
  853.      * vertices (set to b2MaxPolyVertices by default, though you can change
  854.  * this).
  855.      * 
  856.      * Returns -1 if operation fails (usually due to self-intersection of
  857.  * polygon).
  858.      */
  859. int32 DecomposeConvex(b2Polygon* p, b2Polygon* results, int32 maxPolys) {
  860.         if (p->nVertices < 3) return 0;
  861.         b2Triangle* triangulated = new b2Triangle[p->nVertices - 2];
  862. int32 nTri;
  863.         if (p->IsCCW()) {
  864. //printf("It is ccw n");
  865. b2Polygon tempP;
  866. tempP.Set(*p);
  867. ReversePolygon(tempP.x, tempP.y, tempP.nVertices);
  868. nTri = TriangulatePolygon(tempP.x, tempP.y, tempP.nVertices, triangulated);
  869. // ReversePolygon(p->x, p->y, p->nVertices); //reset orientation
  870. } else {
  871. //printf("It is not ccw n");
  872. nTri = TriangulatePolygon(p->x, p->y, p->nVertices, triangulated);
  873. }
  874. if (nTri < 1) {
  875.             //Still no luck?  Oh well...
  876.             return -1;
  877.         }
  878.         int32 nPolys = PolygonizeTriangles(triangulated, nTri, results, maxPolys);
  879.         delete[] triangulated;
  880.         return nPolys;
  881. }
  882.     /**
  883.  * Decomposes a polygon into convex polygons and adds all pieces to a b2BodyDef
  884.      * using a prototype b2PolyDef. All fields of the prototype are used for every
  885.      * shape except the vertices (friction, restitution, density, etc).
  886.      * 
  887.      * If you want finer control, you'll have to add everything by hand.
  888.      * 
  889.      * This is the simplest method to add a complicated polygon to a body.
  890.  *
  891.  * Until Box2D's b2BodyDef behavior changes, this method returns a pointer to
  892.  * a heap-allocated array of b2PolyDefs, which must be deleted by the user
  893.  * after the b2BodyDef is added to the world.
  894.      */
  895. void DecomposeConvexAndAddTo(b2Polygon* p, b2Body* bd, b2FixtureDef* prototype) {
  896.         if (p->nVertices < 3) return;
  897.         b2Polygon* decomposed = new b2Polygon[p->nVertices - 2]; //maximum number of polys
  898.         int32 nPolys = DecomposeConvex(p, decomposed, p->nVertices - 2);
  899. // printf("npolys: %d",nPolys);
  900. b2FixtureDef* pdarray = new b2FixtureDef[2*p->nVertices];//extra space in case of splits
  901. int32 extra = 0;
  902.         for (int32 i = 0; i < nPolys; ++i) {
  903.             b2FixtureDef* toAdd = &pdarray[i+extra];
  904.  *toAdd = *prototype;
  905.  //Hmm, shouldn't have to do all this...
  906.  /*
  907.  toAdd->type = prototype->type;
  908.  toAdd->friction = prototype->friction;
  909.  toAdd->restitution = prototype->restitution;
  910.  toAdd->density = prototype->density;
  911.  toAdd->userData = prototype->userData;
  912.  toAdd->categoryBits = prototype->categoryBits;
  913.  toAdd->maskBits = prototype->maskBits;
  914.  toAdd->groupIndex = prototype->groupIndex;//*/
  915.  //decomposed[i].print();
  916. b2Polygon curr = decomposed[i];
  917. //TODO ewjordan: move this triangle handling to a better place so that
  918. //it happens even if this convenience function is not called.
  919. if (curr.nVertices == 3){
  920. //Check here for near-parallel edges, since we can't
  921. //handle this in merge routine
  922. for (int j=0; j<3; ++j){
  923. int32 lower = (j == 0) ? (curr.nVertices - 1) : (j - 1);
  924. int32 middle = j;
  925. int32 upper = (j == curr.nVertices - 1) ? (0) : (j + 1);
  926. float32 dx0 = curr.x[middle] - curr.x[lower]; float32 dy0 = curr.y[middle] - curr.y[lower];
  927. float32 dx1 = curr.x[upper] - curr.x[middle]; float32 dy1 = curr.y[upper] - curr.y[middle];
  928. float32 norm0 = sqrtf(dx0*dx0+dy0*dy0); float32 norm1 = sqrtf(dx1*dx1+dy1*dy1);
  929. if ( !(norm0 > 0.0f && norm1 > 0.0f) ) {
  930. //Identical points, don't do anything!
  931. goto Skip;
  932. }
  933. dx0 /= norm0; dy0 /= norm0;
  934. dx1 /= norm1; dy1 /= norm1;
  935. float32 cross = dx0 * dy1 - dx1 * dy0;
  936. float32 dot = dx0*dx1 + dy0*dy1;
  937. if (fabs(cross) < b2_angularSlop && dot > 0) {
  938. //Angle too close, split the triangle across from this point.
  939. //This is guaranteed to result in two triangles that satify
  940. //the tolerance (one of the angles is 90 degrees)
  941. float32 dx2 = curr.x[lower] - curr.x[upper]; float32 dy2 = curr.y[lower] - curr.y[upper];
  942. float32 norm2 = sqrtf(dx2*dx2+dy2*dy2);
  943. if (norm2 == 0.0f) {
  944. goto Skip;
  945. }
  946. dx2 /= norm2; dy2 /= norm2;
  947. float32 thisArea = curr.GetArea();
  948. float32 thisHeight = 2.0f * thisArea / norm2;
  949. float32 buffer2 = dx2;
  950. dx2 = dy2; dy2 = -buffer2;
  951. //Make two new polygons
  952. //printf("dx2: %f, dy2: %f, thisHeight: %f, middle: %dn",dx2,dy2,thisHeight,middle);
  953. float32 newX1[3] = { curr.x[middle]+dx2*thisHeight, curr.x[lower], curr.x[middle] };
  954. float32 newY1[3] = { curr.y[middle]+dy2*thisHeight, curr.y[lower], curr.y[middle] };
  955. float32 newX2[3] = { newX1[0], curr.x[middle], curr.x[upper] };
  956. float32 newY2[3] = { newY1[0], curr.y[middle], curr.y[upper] };
  957. b2Polygon p1(newX1,newY1,3);
  958. b2Polygon p2(newX2,newY2,3);
  959. if (p1.IsUsable()){
  960. p1.AddTo(*toAdd);
  961. bd->CreateFixture(toAdd);
  962. ++extra;
  963. } else if (B2_POLYGON_REPORT_ERRORS){
  964. printf("Didn't add unusable polygon.  Dumping vertices:n");
  965. p1.print();
  966. }
  967. if (p2.IsUsable()){
  968. p2.AddTo(pdarray[i+extra]);
  969. bd->CreateFixture(toAdd);
  970. } else if (B2_POLYGON_REPORT_ERRORS){
  971. printf("Didn't add unusable polygon.  Dumping vertices:n");
  972. p2.print();
  973. }
  974. goto Skip;
  975. }
  976. }
  977. }
  978. if (decomposed[i].IsUsable()){
  979. decomposed[i].AddTo(*toAdd);
  980. bd->CreateFixture((const b2FixtureDef*)toAdd);
  981. } else if (B2_POLYGON_REPORT_ERRORS){
  982. printf("Didn't add unusable polygon.  Dumping vertices:n");
  983. decomposed[i].print();
  984. }
  985. Skip:
  986. ;
  987.         }
  988. delete[] pdarray;
  989.         delete[] decomposed;
  990. return;// pdarray; //needs to be deleted after body is created
  991. }
  992.     /**
  993.  * Find the convex hull of a point cloud using "Gift-wrap" algorithm - start
  994.      * with an extremal point, and walk around the outside edge by testing
  995.      * angles.
  996.      * 
  997.      * Runs in O(N*S) time where S is number of sides of resulting polygon.
  998.      * Worst case: point cloud is all vertices of convex polygon -> O(N^2).
  999.      * 
  1000.      * There may be faster algorithms to do this, should you need one -
  1001.      * this is just the simplest. You can get O(N log N) expected time if you
  1002.      * try, I think, and O(N) if you restrict inputs to simple polygons.
  1003.      * 
  1004.      * Returns null if number of vertices passed is less than 3.
  1005.      * 
  1006.  * Results should be passed through convex decomposition afterwards
  1007.  * to ensure that each shape has few enough points to be used in Box2d.
  1008.  *
  1009.      * FIXME?: May be buggy with colinear points on hull. Couldn't find a test
  1010.      * case that resulted in wrong behavior. If one turns up, the solution is to
  1011.      * supplement angle check with an equality resolver that always picks the
  1012.      * longer edge. I think the current solution is working, though it sometimes
  1013.      * creates an extra edge along a line.
  1014.      */
  1015. b2Polygon ConvexHull(b2Vec2* v, int nVert) {
  1016.         float32* cloudX = new float32[nVert];
  1017.         float32* cloudY = new float32[nVert];
  1018.         for (int32 i = 0; i < nVert; ++i) {
  1019.             cloudX[i] = v[i].x;
  1020.             cloudY[i] = v[i].y;
  1021.         }
  1022.         b2Polygon result = ConvexHull(cloudX, cloudY, nVert);
  1023. delete[] cloudX;
  1024. delete[] cloudY;
  1025. return result;
  1026. }
  1027. b2Polygon ConvexHull(float32* cloudX, float32* cloudY, int32 nVert) {
  1028. b2Assert(nVert > 2);
  1029.         int32* edgeList = new int32[nVert];
  1030.         int32 numEdges = 0;
  1031.         float32 minY = FLT_MAX;
  1032.         int32 minYIndex = nVert;
  1033.         for (int32 i = 0; i < nVert; ++i) {
  1034.             if (cloudY[i] < minY) {
  1035.                 minY = cloudY[i];
  1036.                 minYIndex = i;
  1037.             }
  1038.         }
  1039.         int32 startIndex = minYIndex;
  1040.         int32 winIndex = -1;
  1041.         float32 dx = -1.0f;
  1042.         float32 dy = 0.0f;
  1043.         while (winIndex != minYIndex) {
  1044.             float32 newdx = 0.0f;
  1045.             float32 newdy = 0.0f;
  1046.             float32 maxDot = -2.0f;
  1047.             for (int32 i = 0; i < nVert; ++i) {
  1048.                 if (i == startIndex)
  1049.                     continue;
  1050.                 newdx = cloudX[i] - cloudX[startIndex];
  1051.                 newdy = cloudY[i] - cloudY[startIndex];
  1052.                 float32 nrm = sqrtf(newdx * newdx + newdy * newdy);
  1053.                 nrm = (nrm == 0.0f) ? 1.0f : nrm;
  1054.                 newdx /= nrm;
  1055.                 newdy /= nrm;
  1056.                 
  1057.                 //Cross and dot products act as proxy for angle
  1058.                 //without requiring inverse trig.
  1059.                 //FIXED: don't need cross test
  1060.                 //float32 newCross = newdx * dy - newdy * dx;
  1061.                 float32 newDot = newdx * dx + newdy * dy;
  1062.                 if (newDot > maxDot) {//newCross >= 0.0f && newDot > maxDot) {
  1063.                     maxDot = newDot;
  1064.                     winIndex = i;
  1065.                 }
  1066.             }
  1067.             edgeList[numEdges++] = winIndex;
  1068.             dx = cloudX[winIndex] - cloudX[startIndex];
  1069.             dy = cloudY[winIndex] - cloudY[startIndex];
  1070.             float32 nrm = sqrtf(dx * dx + dy * dy);
  1071.             nrm = (nrm == 0.0f) ? 1.0f : nrm;
  1072.             dx /= nrm;
  1073.             dy /= nrm;
  1074.             startIndex = winIndex;
  1075.         }
  1076.         float32* xres = new float32[numEdges];
  1077.         float32* yres = new float32[numEdges];
  1078.         for (int32 i = 0; i < numEdges; i++) {
  1079.             xres[i] = cloudX[edgeList[i]];
  1080.             yres[i] = cloudY[edgeList[i]];
  1081. //("%f, %fn",xres[i],yres[i]);
  1082.         }
  1083.         b2Polygon returnVal(xres, yres, numEdges);
  1084.         delete[] xres;
  1085. delete[] yres;
  1086.         delete[] edgeList;
  1087. returnVal.MergeParallelEdges(b2_angularSlop);
  1088. return returnVal;
  1089. }
  1090. /*
  1091.  * Given sines and cosines, tells if A's angle is less than B's on -Pi, Pi
  1092.  * (in other words, is A "righter" than B)
  1093.  */
  1094. bool IsRighter(float32 sinA, float32 cosA, float32 sinB, float32 cosB){
  1095. if (sinA < 0){
  1096. if (sinB > 0 || cosA <= cosB) return true;
  1097. else return false;
  1098. } else {
  1099. if (sinB < 0 || cosA <= cosB) return false;
  1100. else return true;
  1101. }
  1102. }
  1103. //Fix for obnoxious behavior for the % operator for negative numbers...
  1104. int32 remainder(int32 x, int32 modulus){
  1105. int32 rem = x % modulus;
  1106. while (rem < 0){
  1107. rem += modulus;
  1108. }
  1109. return rem;
  1110. }
  1111. /*
  1112. Method:
  1113. Start at vertex with minimum y (pick maximum x one if there are two).  
  1114. We aim our "lastDir" vector at (1.0, 0)
  1115. We look at the two rays going off from our start vertex, and follow whichever
  1116. has the smallest angle (in -Pi -> Pi) wrt lastDir ("rightest" turn)
  1117. Loop until we hit starting vertex:
  1118. We add our current vertex to the list.
  1119. We check the seg from current vertex to next vertex for intersections
  1120.   - if no intersections, follow to next vertex and continue
  1121.   - if intersections, pick one with minimum distance
  1122.     - if more than one, pick one with "rightest" next point (two possibilities for each)
  1123. */
  1124. b2Polygon TraceEdge(b2Polygon* p){
  1125. b2PolyNode* nodes = new b2PolyNode[p->nVertices*p->nVertices];//overkill, but sufficient (order of mag. is right)
  1126. int32 nNodes = 0;
  1127. //Add base nodes (raw outline)
  1128. for (int32 i=0; i < p->nVertices; ++i){
  1129. b2Vec2 pos(p->x[i],p->y[i]);
  1130. nodes[i].position = pos;
  1131. ++nNodes;
  1132. int32 iplus = (i==p->nVertices-1)?0:i+1;
  1133. int32 iminus = (i==0)?p->nVertices-1:i-1;
  1134. nodes[i].AddConnection(nodes[iplus]);
  1135. nodes[i].AddConnection(nodes[iminus]);
  1136. }
  1137. //Process intersection nodes - horribly inefficient
  1138. bool dirty = true;
  1139. int counter = 0;
  1140. while (dirty){
  1141. dirty = false;
  1142. for (int32 i=0; i < nNodes; ++i){
  1143. for (int32 j=0; j < nodes[i].nConnected; ++j){
  1144. for (int32 k=0; k < nNodes; ++k){
  1145. if (k==i || &nodes[k] == nodes[i].connected[j]) continue;
  1146. for (int32 l=0; l < nodes[k].nConnected; ++l){
  1147. if ( nodes[k].connected[l] == nodes[i].connected[j] ||
  1148.  nodes[k].connected[l] == &nodes[i]) continue;
  1149. //Check intersection
  1150. b2Vec2 intersectPt;
  1151. //if (counter > 100) printf("checking intersection: %d, %d, %d, %dn",i,j,k,l);
  1152. bool crosses = intersect(nodes[i].position,nodes[i].connected[j]->position,
  1153.  nodes[k].position,nodes[k].connected[l]->position,
  1154.  intersectPt);
  1155. if (crosses){
  1156. /*if (counter > 100) {
  1157. printf("Found crossing at %f, %fn",intersectPt.x, intersectPt.y);
  1158. printf("Locations: %f,%f - %f,%f | %f,%f - %f,%fn",
  1159. nodes[i].position.x, nodes[i].position.y,
  1160. nodes[i].connected[j]->position.x, nodes[i].connected[j]->position.y,
  1161. nodes[k].position.x,nodes[k].position.y,
  1162. nodes[k].connected[l]->position.x,nodes[k].connected[l]->position.y);
  1163. printf("Memory addresses: %d, %d, %d, %dn",(int)&nodes[i],(int)nodes[i].connected[j],(int)&nodes[k],(int)nodes[k].connected[l]);
  1164. }*/
  1165. dirty = true;
  1166. //Destroy and re-hook connections at crossing point
  1167. b2PolyNode* connj = nodes[i].connected[j];
  1168. b2PolyNode* connl = nodes[k].connected[l];
  1169. nodes[i].connected[j]->RemoveConnection(nodes[i]);
  1170. nodes[i].RemoveConnection(*connj);
  1171. nodes[k].connected[l]->RemoveConnection(nodes[k]);
  1172. nodes[k].RemoveConnection(*connl);
  1173. nodes[nNodes] = b2PolyNode(intersectPt);
  1174. nodes[nNodes].AddConnection(nodes[i]);
  1175. nodes[i].AddConnection(nodes[nNodes]);
  1176. nodes[nNodes].AddConnection(nodes[k]);
  1177. nodes[k].AddConnection(nodes[nNodes]);
  1178. nodes[nNodes].AddConnection(*connj);
  1179. connj->AddConnection(nodes[nNodes]);
  1180. nodes[nNodes].AddConnection(*connl);
  1181. connl->AddConnection(nodes[nNodes]);
  1182. ++nNodes;
  1183. goto SkipOut;
  1184. }
  1185. }
  1186. }
  1187. }
  1188. }
  1189. SkipOut:
  1190. ++counter;
  1191. //if (counter > 100) printf("Counter: %dn",counter);
  1192. }
  1193. /*
  1194. // Debugging: check for connection consistency
  1195. for (int32 i=0; i<nNodes; ++i) {
  1196. int32 nConn = nodes[i].nConnected;
  1197. for (int32 j=0; j<nConn; ++j) {
  1198. if (nodes[i].connected[j]->nConnected == 0) b2Assert(false);
  1199. b2PolyNode* connect = nodes[i].connected[j];
  1200. bool found = false;
  1201. for (int32 k=0; k<connect->nConnected; ++k) {
  1202. if (connect->connected[k] == &nodes[i]) found = true;
  1203. }
  1204. b2Assert(found);
  1205. }
  1206. }*/
  1207. //Collapse duplicate points
  1208. bool foundDupe = true;
  1209. int nActive = nNodes;
  1210. while (foundDupe){
  1211. foundDupe = false;
  1212. for (int32 i=0; i < nNodes; ++i){
  1213. if (nodes[i].nConnected == 0) continue;
  1214. for (int32 j=i+1; j < nNodes; ++j){
  1215. if (nodes[j].nConnected == 0) continue;
  1216. b2Vec2 diff = nodes[i].position - nodes[j].position;
  1217. if (diff.LengthSquared() <= COLLAPSE_DIST_SQR){
  1218. if (nActive <= 3) return b2Polygon();
  1219. //printf("Found dupe, %d leftn",nActive);
  1220. --nActive;
  1221. foundDupe = true;
  1222. b2PolyNode* inode = &nodes[i];
  1223. b2PolyNode* jnode = &nodes[j];
  1224. //Move all of j's connections to i, and orphan j
  1225. int32 njConn = jnode->nConnected;
  1226. for (int32 k=0; k < njConn; ++k){
  1227. b2PolyNode* knode = jnode->connected[k];
  1228. b2Assert(knode != jnode);
  1229. if (knode != inode) {
  1230. inode->AddConnection(*knode);
  1231. knode->AddConnection(*inode);
  1232. }
  1233. knode->RemoveConnection(*jnode);
  1234. //printf("knode %d on node %d now has %d connectionsn",k,j,knode->nConnected);
  1235. //printf("Found duplicate point.n");
  1236. }
  1237. //printf("Orphaning node at address %dn",(int)jnode);
  1238. //for (int32 k=0; k<njConn; ++k) {
  1239. // if (jnode->connected[k]->IsConnectedTo(*jnode)) printf("Problem!!!n");
  1240. //}
  1241. /*
  1242. for (int32 k=0; k < njConn; ++k){
  1243. jnode->RemoveConnectionByIndex(k);
  1244. }*/
  1245. jnode->nConnected = 0;
  1246. }
  1247. }
  1248. }
  1249. }
  1250. /*
  1251. // Debugging: check for connection consistency
  1252. for (int32 i=0; i<nNodes; ++i) {
  1253. int32 nConn = nodes[i].nConnected;
  1254. printf("Node %d has %d connectionsn",i,nConn);
  1255. for (int32 j=0; j<nConn; ++j) {
  1256. if (nodes[i].connected[j]->nConnected == 0) {
  1257. printf("Problem with node %d connection at address %dn",i,(int)(nodes[i].connected[j]));
  1258. b2Assert(false);
  1259. }
  1260. b2PolyNode* connect = nodes[i].connected[j];
  1261. bool found = false;
  1262. for (int32 k=0; k<connect->nConnected; ++k) {
  1263. if (connect->connected[k] == &nodes[i]) found = true;
  1264. }
  1265. if (!found) printf("Connection %d (of %d) on node %d (of %d) did not have reciprocal connection.n",j,nConn,i,nNodes);
  1266. b2Assert(found);
  1267. }
  1268. }//*/
  1269. //Now walk the edge of the list
  1270. //Find node with minimum y value (max x if equal)
  1271. float32 minY = FLT_MAX;
  1272. float32 maxX = -FLT_MAX;
  1273. int32 minYIndex = -1;
  1274. for (int32 i = 0; i < nNodes; ++i) {
  1275. if (nodes[i].position.y < minY && nodes[i].nConnected > 1) {
  1276. minY = nodes[i].position.y;
  1277. minYIndex = i;
  1278. maxX = nodes[i].position.x;
  1279. } else if (nodes[i].position.y == minY && nodes[i].position.x > maxX && nodes[i].nConnected > 1) {
  1280. minYIndex = i;
  1281. maxX = nodes[i].position.x;
  1282. }
  1283. }
  1284. b2Vec2 origDir(1.0f,0.0f);
  1285. b2Vec2* resultVecs = new b2Vec2[4*nNodes];// nodes may be visited more than once, unfortunately - change to growable array!
  1286. int32 nResultVecs = 0;
  1287. b2PolyNode* currentNode = &nodes[minYIndex];
  1288. b2PolyNode* startNode = currentNode;
  1289. b2Assert(currentNode->nConnected > 0);
  1290. b2PolyNode* nextNode = currentNode->GetRightestConnection(origDir);
  1291. if (!nextNode) goto CleanUp; // Borked, clean up our mess and return
  1292. resultVecs[0] = startNode->position;
  1293. ++nResultVecs;
  1294. while (nextNode != startNode){
  1295. if (nResultVecs > 4*nNodes){
  1296. /*
  1297. printf("%d, %d, %dn",(int)startNode,(int)currentNode,(int)nextNode);
  1298. printf("%f, %f -> %f, %fn",currentNode->position.x,currentNode->position.y, nextNode->position.x, nextNode->position.y);
  1299. p->printFormatted();
  1300. printf("Dumping connection graph: n");
  1301. for (int32 i=0; i<nNodes; ++i) {
  1302. printf("nodex[%d] = %f; nodey[%d] = %f;n",i,nodes[i].position.x,i,nodes[i].position.y);
  1303. printf("//connected ton");
  1304. for (int32 j=0; j<nodes[i].nConnected; ++j) {
  1305. 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);
  1306. }
  1307. }
  1308. printf("Dumping results thus far: n");
  1309. for (int32 i=0; i<nResultVecs; ++i) {
  1310. 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);
  1311. }
  1312. //*/
  1313. b2Assert(false); //nodes should never be visited four times apiece (proof?), so we've probably hit a loop...crap
  1314. }
  1315. resultVecs[nResultVecs++] = nextNode->position;
  1316. b2PolyNode* oldNode = currentNode;
  1317. currentNode = nextNode;
  1318. //printf("Old node connections = %d; address %dn",oldNode->nConnected, (int)oldNode);
  1319. //printf("Current node connections = %d; address %dn",currentNode->nConnected, (int)currentNode);
  1320. nextNode = currentNode->GetRightestConnection(oldNode);
  1321. if (!nextNode) goto CleanUp; // There was a problem, so jump out of the loop and use whatever garbage we've generated so far
  1322. //printf("nextNode address: %dn",(int)nextNode);
  1323. }
  1324. CleanUp:
  1325. float32* xres = new float32[nResultVecs];
  1326. float32* yres = new float32[nResultVecs];
  1327. for (int32 i=0; i<nResultVecs; ++i){
  1328. xres[i] = resultVecs[i].x;
  1329. yres[i] = resultVecs[i].y;
  1330. }
  1331. b2Polygon retval(xres,yres,nResultVecs);
  1332. delete[] resultVecs;
  1333. delete[] yres;
  1334. delete[] xres;
  1335. delete[] nodes;
  1336. return retval;
  1337. }
  1338. b2PolyNode::b2PolyNode(){
  1339. nConnected = 0;
  1340. visited = false;
  1341. }
  1342. b2PolyNode::b2PolyNode(b2Vec2& pos){
  1343. position = pos;
  1344. nConnected = 0;
  1345. visited = false;
  1346. }
  1347. void b2PolyNode::AddConnection(b2PolyNode& toMe){
  1348. b2Assert(nConnected < MAX_CONNECTED);
  1349. // Ignore duplicate additions
  1350. for (int32 i=0; i<nConnected; ++i) {
  1351. if (connected[i] == &toMe) return;
  1352. }
  1353. connected[nConnected] = &toMe;
  1354. ++nConnected;
  1355. }
  1356. void b2PolyNode::RemoveConnection(b2PolyNode& fromMe){
  1357. bool isFound = false;
  1358. int32 foundIndex = -1;
  1359. for (int32 i=0; i<nConnected; ++i){
  1360. if (&fromMe == connected[i]) {//.position == connected[i]->position){
  1361. isFound = true;
  1362. foundIndex = i;
  1363. break;
  1364. }
  1365. }
  1366. b2Assert(isFound);
  1367. --nConnected;
  1368. //printf("nConnected: %dn",nConnected);
  1369. for (int32 i=foundIndex; i < nConnected; ++i){
  1370. connected[i] = connected[i+1];
  1371. }
  1372. }
  1373. void b2PolyNode::RemoveConnectionByIndex(int32 index){
  1374. --nConnected;
  1375. //printf("New nConnected = %dn",nConnected);
  1376. for (int32 i=index; i < nConnected; ++i){
  1377. connected[i] = connected[i+1];
  1378. }
  1379. }
  1380. bool b2PolyNode::IsConnectedTo(b2PolyNode& me){
  1381. bool isFound = false;
  1382. for (int32 i=0; i<nConnected; ++i){
  1383. if (&me == connected[i]) {//.position == connected[i]->position){
  1384. isFound = true;
  1385. break;
  1386. }
  1387. }
  1388. return isFound;
  1389. }
  1390. b2PolyNode* b2PolyNode::GetRightestConnection(b2PolyNode* incoming){
  1391. if (nConnected == 0) b2Assert(false); // This means the connection graph is inconsistent
  1392. if (nConnected == 1) {
  1393. //b2Assert(false);
  1394. // Because of the possibility of collapsing nearby points,
  1395. // we may end up with "spider legs" dangling off of a region.
  1396. // The correct behavior here is to turn around.
  1397. return incoming;
  1398. }
  1399. b2Vec2 inDir = position - incoming->position;
  1400. float32 inLength = inDir.Normalize();
  1401. b2Assert(inLength > FLT_EPSILON);
  1402. b2PolyNode* result = NULL;
  1403. for (int32 i=0; i<nConnected; ++i){
  1404. if (connected[i] == incoming) continue;
  1405. b2Vec2 testDir = connected[i]->position - position;
  1406. float32 testLengthSqr = testDir.LengthSquared();
  1407. testDir.Normalize();
  1408. /*
  1409. if (testLengthSqr < COLLAPSE_DIST_SQR) {
  1410. printf("Problem with connection %dn",i);
  1411. printf("This node has %d connectionsn",nConnected);
  1412. printf("That one has %dn",connected[i]->nConnected);
  1413. if (this == connected[i]) printf("This points at itself.n");
  1414. }*/
  1415. b2Assert (testLengthSqr >= COLLAPSE_DIST_SQR);
  1416. float32 myCos = b2Dot(inDir,testDir);
  1417. float32 mySin = b2Cross(inDir,testDir);
  1418. if (result){
  1419. b2Vec2 resultDir = result->position - position;
  1420. resultDir.Normalize();
  1421. float32 resCos = b2Dot(inDir,resultDir);
  1422. float32 resSin = b2Cross(inDir,resultDir);
  1423. if (IsRighter(mySin,myCos,resSin,resCos)){
  1424. result = connected[i];
  1425. }
  1426. } else{
  1427. result = connected[i];
  1428. }
  1429. }
  1430. if (B2_POLYGON_REPORT_ERRORS && !result) {
  1431. printf("nConnected = %dn",nConnected);
  1432. for (int32 i=0; i<nConnected; ++i) {
  1433. printf("connected[%d] @ %dn",i,0);//(int)connected[i]);
  1434. }
  1435. }
  1436. b2Assert(result);
  1437. return result;
  1438. }
  1439. b2PolyNode* b2PolyNode::GetRightestConnection(b2Vec2& incomingDir){
  1440. b2Vec2 diff = position-incomingDir;
  1441. b2PolyNode temp(diff);
  1442. b2PolyNode* res = GetRightestConnection(&temp);
  1443. b2Assert(res);
  1444. return res;
  1445. }