$search
00001 #include <stdio.h> 00002 #include <stdlib.h> 00003 #include <string.h> 00004 #include <assert.h> 00005 00062 #include "planetri.h" 00063 00064 namespace ConvexDecomposition 00065 { 00066 00067 00068 static inline double DistToPt(const double *p,const double *plane) 00069 { 00070 double x = p[0]; 00071 double y = p[1]; 00072 double z = p[2]; 00073 double d = x*plane[0] + y*plane[1] + z*plane[2] + plane[3]; 00074 return d; 00075 } 00076 00077 00078 static PlaneTriResult getSidePlane(const double *p,const double *plane,double epsilon) 00079 { 00080 00081 double d = DistToPt(p,plane); 00082 00083 if ( (d+epsilon) > 0 ) 00084 return PTR_FRONT; // it is 'in front' within the provided epsilon value. 00085 00086 return PTR_BACK; 00087 } 00088 00089 static void add(const double *p,double *dest,unsigned int tstride,unsigned int &pcount) 00090 { 00091 char *d = (char *) dest; 00092 d = d + pcount*tstride; 00093 dest = (double *) d; 00094 dest[0] = p[0]; 00095 dest[1] = p[1]; 00096 dest[2] = p[2]; 00097 pcount++; 00098 assert( pcount <= 4 ); 00099 } 00100 00101 00102 // assumes that the points are on opposite sides of the plane! 00103 static void intersect(const double *p1,const double *p2,double *split,const double *plane) 00104 { 00105 00106 double dp1 = DistToPt(p1,plane); 00107 double dp2 = DistToPt(p2,plane); 00108 00109 double dir[3]; 00110 00111 dir[0] = p2[0] - p1[0]; 00112 dir[1] = p2[1] - p1[1]; 00113 dir[2] = p2[2] - p1[2]; 00114 00115 double dot1 = dir[0]*plane[0] + dir[1]*plane[1] + dir[2]*plane[2]; 00116 double dot2 = dp1 - plane[3]; 00117 00118 double t = -(plane[3] + dot2 ) / dot1; 00119 00120 split[0] = (dir[0]*t)+p1[0]; 00121 split[1] = (dir[1]*t)+p1[1]; 00122 split[2] = (dir[2]*t)+p1[2]; 00123 00124 } 00125 00126 #define MAXPTS 256 00127 00128 class point 00129 { 00130 public: 00131 00132 void set(const double *p) 00133 { 00134 x = p[0]; 00135 y = p[1]; 00136 z = p[2]; 00137 } 00138 00139 double x; 00140 double y; 00141 double z; 00142 }; 00143 class polygon 00144 { 00145 public: 00146 polygon(void) 00147 { 00148 mVcount = 0; 00149 } 00150 00151 polygon(const double *p1,const double *p2,const double *p3) 00152 { 00153 mVcount = 3; 00154 mVertices[0].set(p1); 00155 mVertices[1].set(p2); 00156 mVertices[2].set(p3); 00157 } 00158 00159 00160 int NumVertices(void) const { return mVcount; }; 00161 00162 const point& Vertex(int index) 00163 { 00164 if ( index < 0 ) index+=mVcount; 00165 return mVertices[index]; 00166 }; 00167 00168 00169 void set(const point *pts,int count) 00170 { 00171 for (int i=0; i<count; i++) 00172 { 00173 mVertices[i] = pts[i]; 00174 } 00175 mVcount = count; 00176 } 00177 00178 int mVcount; 00179 point mVertices[MAXPTS]; 00180 }; 00181 00182 class plane 00183 { 00184 public: 00185 plane(const double *p) 00186 { 00187 normal.x = p[0]; 00188 normal.y = p[1]; 00189 normal.z = p[2]; 00190 D = p[3]; 00191 } 00192 00193 double Classify_Point(const point &p) 00194 { 00195 return p.x*normal.x + p.y*normal.y + p.z*normal.z + D; 00196 } 00197 00198 point normal; 00199 double D; 00200 }; 00201 00202 void Split_Polygon(polygon *poly, plane *part, polygon &front, polygon &back) 00203 { 00204 int count = poly->NumVertices (); 00205 int out_c = 0, in_c = 0; 00206 point ptA, ptB,outpts[MAXPTS],inpts[MAXPTS]; 00207 double sideA, sideB; 00208 ptA = poly->Vertex (count - 1); 00209 sideA = part->Classify_Point (ptA); 00210 for (int i = -1; ++i < count;) 00211 { 00212 ptB = poly->Vertex(i); 00213 sideB = part->Classify_Point(ptB); 00214 if (sideB > 0) 00215 { 00216 if (sideA < 0) 00217 { 00218 point v; 00219 intersect(&ptB.x, &ptA.x, &v.x, &part->normal.x ); 00220 outpts[out_c++] = inpts[in_c++] = v; 00221 } 00222 outpts[out_c++] = ptB; 00223 } 00224 else if (sideB < 0) 00225 { 00226 if (sideA > 0) 00227 { 00228 point v; 00229 intersect(&ptB.x, &ptA.x, &v.x, &part->normal.x ); 00230 outpts[out_c++] = inpts[in_c++] = v; 00231 } 00232 inpts[in_c++] = ptB; 00233 } 00234 else 00235 outpts[out_c++] = inpts[in_c++] = ptB; 00236 ptA = ptB; 00237 sideA = sideB; 00238 } 00239 00240 front.set(&outpts[0], out_c); 00241 back.set(&inpts[0], in_c); 00242 } 00243 00244 PlaneTriResult planeTriIntersection(const double *_plane, // the plane equation in Ax+By+Cz+D format 00245 const double *triangle, // the source triangle. 00246 unsigned int tstride, // stride in bytes of the input and output *vertices* 00247 double epsilon, // the co-planer epsilon value. 00248 double *front, // the triangle in front of the 00249 unsigned int &fcount, // number of vertices in the 'front' triangle 00250 double *back, // the triangle in back of the plane 00251 unsigned int &bcount) // the number of vertices in the 'back' triangle. 00252 { 00253 00254 fcount = 0; 00255 bcount = 0; 00256 00257 const char *tsource = (const char *) triangle; 00258 00259 // get the three vertices of the triangle. 00260 const double *p1 = (const double *) (tsource); 00261 const double *p2 = (const double *) (tsource+tstride); 00262 const double *p3 = (const double *) (tsource+tstride*2); 00263 00264 00265 PlaneTriResult r1 = getSidePlane(p1,_plane,epsilon); // compute the side of the plane each vertex is on 00266 PlaneTriResult r2 = getSidePlane(p2,_plane,epsilon); 00267 PlaneTriResult r3 = getSidePlane(p3,_plane,epsilon); 00268 00269 if ( r1 == r2 && r1 == r3 ) // if all three vertices are on the same side of the plane. 00270 { 00271 if ( r1 == PTR_FRONT ) // if all three are in front of the plane, then copy to the 'front' output triangle. 00272 { 00273 add(p1,front,tstride,fcount); 00274 add(p2,front,tstride,fcount); 00275 add(p3,front,tstride,fcount); 00276 } 00277 else 00278 { 00279 add(p1,back,tstride,bcount); // if all three are in 'back' then copy to the 'back' output triangle. 00280 add(p2,back,tstride,bcount); 00281 add(p3,back,tstride,bcount); 00282 } 00283 return r1; // if all three points are on the same side of the plane return result 00284 } 00285 00286 00287 polygon pi(p1,p2,p3); 00288 polygon pfront,pback; 00289 00290 plane part(_plane); 00291 Split_Polygon(&pi,&part,pfront,pback); 00292 00293 for (int i=0; i<pfront.mVcount; i++) 00294 { 00295 add( &pfront.mVertices[i].x, front, tstride, fcount ); 00296 } 00297 00298 for (int i=0; i<pback.mVcount; i++) 00299 { 00300 add( &pback.mVertices[i].x, back, tstride, bcount ); 00301 } 00302 00303 PlaneTriResult ret = PTR_SPLIT; 00304 00305 if ( fcount == 0 && bcount ) 00306 ret = PTR_BACK; 00307 00308 if ( bcount == 0 && fcount ) 00309 ret = PTR_FRONT; 00310 00311 return ret; 00312 } 00313 00314 };