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;
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
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,
00245 const double *triangle,
00246 unsigned int tstride,
00247 double epsilon,
00248 double *front,
00249 unsigned int &fcount,
00250 double *back,
00251 unsigned int &bcount)
00252 {
00253
00254 fcount = 0;
00255 bcount = 0;
00256
00257 const char *tsource = (const char *) triangle;
00258
00259
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);
00266 PlaneTriResult r2 = getSidePlane(p2,_plane,epsilon);
00267 PlaneTriResult r3 = getSidePlane(p3,_plane,epsilon);
00268
00269 if ( r1 == r2 && r1 == r3 )
00270 {
00271 if ( r1 == PTR_FRONT )
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);
00280 add(p2,back,tstride,bcount);
00281 add(p3,back,tstride,bcount);
00282 }
00283 return r1;
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 };