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