planetri.cpp
Go to the documentation of this file.
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 };


convex_decomposition
Author(s): John W. Ratcliff
autogenerated on Thu Feb 11 2016 22:42:23