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