$search
00001 #include <stdio.h> 00002 #include <stdlib.h> 00003 #include <string.h> 00004 #include <assert.h> 00005 #include <math.h> 00006 00063 #include <vector> // Include STL vector class. 00064 00065 #include "triangulate.h" 00066 00067 00068 namespace ConvexDecomposition 00069 { 00070 00071 00072 class Vec2d 00073 { 00074 public: 00075 Vec2d(const double *v) 00076 { 00077 mX = v[0]; 00078 mY = v[1]; 00079 } 00080 Vec2d(double x,double y) 00081 { 00082 Set(x,y); 00083 }; 00084 double GetX(void) const { return mX; }; 00085 double GetY(void) const { return mY; }; 00086 00087 void Set(double x,double y) 00088 { 00089 mX = x; 00090 mY = y; 00091 }; 00092 00093 private: 00094 double mX; 00095 double mY; 00096 };// Typedef an STL vector of vertices which are used to represent 00097 // a polygon/contour and a series of triangles. 00098 00099 typedef std::vector< Vec2d > Vec2dVector; 00100 00101 static bool Process(const Vec2dVector &contour,Vec2dVector &result); // compute area of a contour/polygon 00102 static double Area(const Vec2dVector &contour); // decide if point Px/Py is inside triangle defined by (Ax,Ay) (Bx,By) (Cx,Cy) 00103 static bool InsideTriangle(double Ax, double Ay,double Bx, double By,double Cx, double Cy,double Px, double Py); 00104 static bool Snip(const Vec2dVector &contour,int u,int v,int w,int n,int *V); 00105 00106 static const double EPSILON=0.0000000001f; 00107 00108 double Area(const Vec2dVector &contour) 00109 { 00110 int n = contour.size(); 00111 double A=0.0f; 00112 for(int p=n-1,q=0; q<n; p=q++) 00113 { 00114 A+= contour[p].GetX()*contour[q].GetY() - contour[q].GetX()*contour[p].GetY(); 00115 } 00116 return A*0.5f; 00117 } 00118 /* 00119 InsideTriangle decides if a point P is Inside of the triangle 00120 defined by A, B, C. 00121 */ 00122 bool InsideTriangle(double Ax, double Ay,double Bx, double By,double Cx, double Cy,double Px, double Py) 00123 { 00124 double ax, ay, bx, by, cx, cy, apx, apy, bpx, bpy, cpx, cpy; 00125 double cCROSSap, bCROSScp, aCROSSbp; ax = Cx - Bx; ay = Cy - By; 00126 bx = Ax - Cx; by = Ay - Cy; 00127 cx = Bx - Ax; cy = By - Ay; 00128 apx= Px - Ax; apy= Py - Ay; 00129 bpx= Px - Bx; bpy= Py - By; 00130 cpx= Px - Cx; cpy= Py - Cy; aCROSSbp = ax*bpy - ay*bpx; 00131 cCROSSap = cx*apy - cy*apx; 00132 bCROSScp = bx*cpy - by*cpx; return ((aCROSSbp >= 0.0f) && (bCROSScp >= 0.0f) && (cCROSSap >= 0.0f)); 00133 }; 00134 00135 bool Snip(const Vec2dVector &contour,int u,int v,int w,int n,int *V) 00136 { 00137 int p; 00138 double Ax, Ay, Bx, By, Cx, Cy, Px, Py; 00139 Ax = contour[V[u]].GetX(); 00140 Ay = contour[V[u]].GetY(); 00141 Bx = contour[V[v]].GetX(); 00142 By = contour[V[v]].GetY(); 00143 Cx = contour[V[w]].GetX(); 00144 Cy = contour[V[w]].GetY(); 00145 if ( EPSILON > (((Bx-Ax)*(Cy-Ay)) - ((By-Ay)*(Cx-Ax))) ) return false; for (p=0;p<n;p++) 00146 { 00147 if( (p == u) || (p == v) || (p == w) ) continue; 00148 Px = contour[V[p]].GetX(); 00149 Py = contour[V[p]].GetY(); 00150 if (InsideTriangle(Ax,Ay,Bx,By,Cx,Cy,Px,Py)) return false; 00151 } return true; 00152 } 00153 00154 bool Process(const Vec2dVector &contour,Vec2dVector &result) 00155 { 00156 /* allocate and initialize list of Vertices in polygon */ 00157 int n = contour.size(); 00158 if ( n < 3 ) return false; int *V = new int[n]; /* we want a counter-clockwise polygon in V */ if ( 0.0f < Area(contour) ) 00159 for (int v=0; v<n; v++) V[v] = v; 00160 else 00161 for(int v=0; v<n; v++) V[v] = (n-1)-v; int nv = n; /* remove nv-2 Vertices, creating 1 triangle every time */ 00162 00163 int count = 2*nv; /* error detection */ 00164 00165 for(int m=0, v=nv-1; nv>2; ) 00166 { 00167 /* if we loop, it is probably a non-simple polygon */ 00168 if (0 >= (count--)) 00169 { 00170 //** Triangulate: ERROR - probable bad polygon! 00171 return false; 00172 } /* three consecutive vertices in current polygon, <u,v,w> */ 00173 00174 int u = v ; 00175 if (nv <= u) u = 0; /* previous */ 00176 v = u+1; if (nv <= v) v = 0; /* new v */ 00177 int w = v+1; 00178 if (nv <= w) w = 0; /* next */ 00179 00180 if ( Snip(contour,u,v,w,nv,V) ) 00181 { 00182 int a,b,c,s,t; /* true names of the vertices */ 00183 00184 a = V[u]; 00185 b = V[v]; 00186 c = V[w]; /* output Triangle */ 00187 00188 result.push_back( contour[a] ); 00189 result.push_back( contour[b] ); 00190 result.push_back( contour[c] ); 00191 00192 m++; /* remove v from remaining polygon */ 00193 for(s=v,t=v+1;t<nv;s++,t++) V[s] = V[t]; nv--; /* resest error detection counter */ 00194 count = 2*nv; 00195 } 00196 00197 } 00198 delete V; 00199 return true; 00200 } 00201 00202 00203 unsigned int triangulate3d(unsigned int pcount, // number of points in the polygon 00204 const double *vertices, // array of 3d vertices. 00205 double *triangles, // memory to store output triangles 00206 unsigned int maxTri, // maximum triangles we are allowed to output. 00207 const double *plane) 00208 00209 { 00210 unsigned int ret = 0; 00211 00212 FILE *fph = fopen("debug.obj", "wb"); 00213 if ( fph ) 00214 { 00215 fprintf(fph,"v 10 10 0\r\n"); 00216 for (unsigned int i=0; i<pcount; i++) 00217 { 00218 fprintf(fph,"v %f %f %f\r\n", vertices[i*3+0], vertices[i*3+1], vertices[i*3+2]); 00219 } 00220 for (unsigned int i=0; i<pcount; i++) 00221 { 00222 unsigned int next = i+1; 00223 if ( next == pcount ) next = 0; 00224 fprintf(fph,"f %d %d %d\r\n", i+2, 1, next+2 ); 00225 } 00226 fclose(fph); 00227 } 00228 00229 if ( pcount >= 3 ) 00230 { 00231 double normal[3]; 00232 00233 normal[0] = plane[0]; 00234 normal[1] = plane[1]; 00235 normal[2] = plane[2]; 00236 double D = plane[3]; 00237 00238 unsigned int i0 = 0; 00239 unsigned int i1 = 1; 00240 unsigned int i2 = 2; 00241 unsigned int axis = 0; 00242 00243 00244 // find the dominant axis. 00245 double dx = fabs(normal[0]); 00246 double dy = fabs(normal[1]); 00247 double dz = fabs(normal[2]); 00248 00249 if ( dx > dy && dx > dz ) 00250 { 00251 axis = 0; 00252 i0 = 1; 00253 i1 = 2; 00254 i2 = 0; 00255 } 00256 else if ( dy > dx && dy > dz ) 00257 { 00258 i0 = 0; 00259 i1 = 2; 00260 i2 = 1; 00261 axis = 1; 00262 } 00263 else if ( dz > dx && dz > dy ) 00264 { 00265 i0 = 0; 00266 i1 = 1; 00267 i2 = 2; 00268 axis = 2; 00269 } 00270 00271 double *ptemp = new double[pcount*2]; 00272 double *ptri = new double[maxTri*2*3]; 00273 const double *source = vertices; 00274 double *dest = ptemp; 00275 00276 for (unsigned int i=0; i<pcount; i++) 00277 { 00278 00279 dest[0] = source[i0]; 00280 dest[1] = source[i1]; 00281 00282 dest+=2; 00283 source+=3; 00284 } 00285 00286 ret = triangulate2d(pcount, ptemp, ptri, maxTri ); 00287 00288 // ok..now we have to copy it back and project the 3d component. 00289 if ( ret ) 00290 { 00291 const double *source = ptri; 00292 double *dest = triangles; 00293 00294 double inverseZ = -1.0f / normal[i2]; 00295 00296 for (unsigned int i=0; i<ret*3; i++) 00297 { 00298 00299 dest[i0] = source[0]; 00300 dest[i1] = source[1]; 00301 00302 dest[i2] = (normal[i0]*source[0] + normal[i1]*source[1] + D ) * inverseZ; // solve for projected component 00303 00304 dest+=3; 00305 source+=2; 00306 } 00307 00308 00309 if ( 1 ) 00310 { 00311 FILE *fph = fopen("test.obj","wb"); 00312 const double *source = triangles; 00313 for (unsigned int i=0; i<ret*3; i++) 00314 { 00315 fprintf(fph,"v %f %f %f\r\n", source[0], source[1], source[2] ); 00316 source+=3; 00317 } 00318 int index = 1; 00319 for (unsigned int i=0; i<ret; i++) 00320 { 00321 fprintf(fph,"f %d %d %d\r\n", index, index+1, index+2 ); 00322 index+=3; 00323 } 00324 fclose(fph); 00325 } 00326 } 00327 00328 delete ptri; 00329 delete ptemp; 00330 00331 } 00332 00333 return ret; 00334 } 00335 00336 unsigned int triangulate3d(unsigned int pcount, // number of points in the polygon 00337 const unsigned int *indices, // polygon points using indices 00338 const double *vertices, // base address for array indexing 00339 double *triangles, // buffer to store output 3d triangles. 00340 unsigned int maxTri, // maximum triangles we can output. 00341 const double *plane) 00342 { 00343 unsigned int ret = 0; 00344 00345 if ( pcount ) 00346 { 00347 // copy the indexed polygon out as a flat array of vertices. 00348 double *ptemp = new double[pcount*3]; 00349 double *dest = ptemp; 00350 00351 for (unsigned int i=0; i<pcount; i++) 00352 { 00353 unsigned int index = indices[i]; 00354 const double *source = &vertices[index*3]; 00355 *dest++ = *source++; 00356 *dest++ = *source++; 00357 *dest++ = *source++; 00358 } 00359 00360 ret = triangulate3d(pcount,ptemp,triangles,maxTri,plane); 00361 00362 delete ptemp; 00363 } 00364 00365 return ret; 00366 } 00367 00368 unsigned int triangulate2d(unsigned int pcount, // number of points in the polygon 00369 const double *vertices, // address of input points (2d) 00370 double *triangles, // destination buffer for output triangles. 00371 unsigned int maxTri) // maximum number of triangles we can store. 00372 { 00373 unsigned int ret = 0; 00374 00375 const double *source = vertices; 00376 Vec2dVector vlist; 00377 00378 for (unsigned int i=0; i<pcount; i++) 00379 { 00380 Vec2d v(source); 00381 vlist.push_back(v); 00382 source+=2; 00383 } 00384 00385 Vec2dVector result; 00386 00387 bool ok = Process(vlist,result); 00388 if ( ok ) 00389 { 00390 ret = result.size()/3; 00391 if ( ret < maxTri ) 00392 { 00393 double *dest = triangles; 00394 for (unsigned int i=0; i<ret*3; i++) 00395 { 00396 dest[0] = result[i].GetX(); 00397 dest[1] = result[i].GetY(); 00398 dest+=2; 00399 } 00400 } 00401 else 00402 { 00403 ret = 0; 00404 } 00405 } 00406 00407 return ret; 00408 } 00409 00410 }; // end of namespace