Extras/ConvexDecomposition/planetri.cpp

   1 #include "float_math.h"
   2 #include <stdio.h>
   3 #include <stdlib.h>
   4 #include <string.h>
   5 #include <assert.h>
   6
   7 #include "planetri.h"
   8
   9 /*----------------------------------------------------------------------
  10                 Copyright (c) 2004 Open Dynamics Framework Group
  11                                         www.physicstools.org
  12                 All rights reserved.
  13
  14                 Redistribution and use in source and binary forms, with or without modification, are permitted provided
  15                 that the following conditions are met:
  16
  17                 Redistributions of source code must retain the above copyright notice, this list of conditions
  18                 and the following disclaimer.
  19
  20                 Redistributions in binary form must reproduce the above copyright notice,
  21                 this list of conditions and the following disclaimer in the documentation
  22                 and/or other materials provided with the distribution.
  23
  24                 Neither the name of the Open Dynamics Framework Group nor the names of its contributors may
  25                 be used to endorse or promote products derived from this software without specific prior written permission.
  26
  27                 THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 'AS IS' AND ANY EXPRESS OR IMPLIED WARRANTIES,
  28                 INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
  29                 DISCLAIMED. IN NO EVENT SHALL THE INTEL OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
  30                 EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
  31                 LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER
  32                 IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF
  33                 THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
  34 -----------------------------------------------------------------------*/
  35
  36 // http://codesuppository.blogspot.com
  37 //
  38 // mailto: jratcliff@infiniplex.net
  39 //
  40 // http://www.amillionpixels.us
  41 //
  42
  43 static inline float DistToPt(const float *p,const float *plane)
  44 {
  45         float x = p[0];
  46         float y = p[1];
  47         float z = p[2];
  48         float d = x*plane[0] + y*plane[1] + z*plane[2] + plane[3];
  49         return d;
  50 }
  51
  52
  53 static PlaneTriResult getSidePlane(const float *p,const float *plane,float epsilon)
  54 {
  55
  56   float d = DistToPt(p,plane);
  57
  58   if ( (d+epsilon) > 0 )
  59                 return PTR_FRONT; // it is 'in front' within the provided epsilon value.
  60
  61   return PTR_BACK;
  62 }
  63
  64 static void add(const float *p,float *dest,unsigned int tstride,unsigned int &pcount)
  65 {
  66   char *d = (char *) dest;
  67   d = d + pcount*tstride;
  68   dest = (float *) d;
  69   dest[0] = p[0];
  70   dest[1] = p[1];
  71   dest[2] = p[2];
  72   pcount++;
  73         assert( pcount <= 4 );
  74 }
  75
  76
  77 // assumes that the points are on opposite sides of the plane!
  78 static void intersect(const float *p1,const float *p2,float *split,const float *plane)
  79 {
  80
  81   float dp1 = DistToPt(p1,plane);
  82
  83   float dir[3];
  84
  85   dir[0] = p2[0] - p1[0];
  86   dir[1] = p2[1] - p1[1];
  87   dir[2] = p2[2] - p1[2];
  88
  89   float dot1 = dir[0]*plane[0] + dir[1]*plane[1] + dir[2]*plane[2];
  90   float dot2 = dp1 - plane[3];
  91
  92   float    t = -(plane[3] + dot2 ) / dot1;
  93
  94   split[0] = (dir[0]*t)+p1[0];
  95   split[1] = (dir[1]*t)+p1[1];
  96   split[2] = (dir[2]*t)+p1[2];
  97
  98 }
  99
 100 PlaneTriResult planeTriIntersection(const float *plane,    // the plane equation in Ax+By+Cz+D format
 101                                     const float *triangle, // the source triangle.
 102                                     unsigned int tstride,  // stride in bytes of the input and output triangles
 103                                     float        epsilon,  // the co-planer epsilon value.
 104                                     float       *front,    // the triangle in front of the
 105                                     unsigned int &fcount,  // number of vertices in the 'front' triangle
 106                                     float       *back,     // the triangle in back of the plane
 107                                     unsigned int &bcount) // the number of vertices in the 'back' triangle.
 108 {
 109   fcount = 0;
 110   bcount = 0;
 111
 112   const char *tsource = (const char *) triangle;
 113
 114   // get the three vertices of the triangle.
 115   const float *p1     = (const float *) (tsource);
 116   const float *p2     = (const float *) (tsource+tstride);
 117   const float *p3     = (const float *) (tsource+tstride*2);
 118
 119
 120   PlaneTriResult r1   = getSidePlane(p1,plane,epsilon); // compute the side of the plane each vertex is on
 121   PlaneTriResult r2   = getSidePlane(p2,plane,epsilon);
 122   PlaneTriResult r3   = getSidePlane(p3,plane,epsilon);
 123
 124   if ( r1 == r2 && r1 == r3 ) // if all three vertices are on the same side of the plane.
 125   {
 126     if ( r1 == PTR_FRONT ) // if all three are in front of the plane, then copy to the 'front' output triangle.
 127     {
 128       add(p1,front,tstride,fcount);
 129       add(p2,front,tstride,fcount);
 130       add(p3,front,tstride,fcount);
 131     }
 132     else
 133     {
 134       add(p1,back,tstride,bcount); // if all three are in 'abck' then copy to the 'back' output triangle.
 135       add(p2,back,tstride,bcount);
 136       add(p3,back,tstride,bcount);
 137     }
 138     return r1; // if all three points are on the same side of the plane return result
 139   }
 140
 141   // ok.. we need to split the triangle at the plane.
 142
 143   // First test ray segment P1 to P2
 144   if ( r1 == r2 ) // if these are both on the same side...
 145   {
 146     if ( r1 == PTR_FRONT )
 147     {
 148       add( p1, front, tstride, fcount );
 149       add( p2, front, tstride, fcount );
 150     }
 151     else
 152     {
 153       add( p1, back, tstride, bcount );
 154       add( p2, back, tstride, bcount );
 155     }
 156   }
 157   else
 158   {
 159     float split[3]; // split the point
 160     intersect(p1,p2,split,plane);
 161
 162     if ( r1 == PTR_FRONT )
 163     {
 164
 165       add(p1, front, tstride, fcount );
 166       add(split, front, tstride, fcount );
 167
 168       add(split, back, tstride, bcount );
 169       add(p2, back, tstride, bcount );
 170
 171     }
 172     else
 173     {
 174       add(p1, back, tstride, bcount );
 175       add(split, back, tstride, bcount );
 176
 177       add(split, front, tstride, fcount );
 178       add(p2, front, tstride, fcount );
 179     }
 180
 181   }
 182
 183   // Next test ray segment P2 to P3
 184   if ( r2 == r3 ) // if these are both on the same side...
 185   {
 186     if ( r3 == PTR_FRONT )
 187     {
 188       add( p3, front, tstride, fcount );
 189     }
 190     else
 191     {
 192       add( p3, back, tstride, bcount );
 193     }
 194   }
 195   else
 196   {
 197     float split[3]; // split the point
 198     intersect(p2,p3,split,plane);
 199
 200     if ( r3 == PTR_FRONT )
 201     {
 202       add(split, front, tstride, fcount );
 203       add(split, back, tstride, bcount );
 204
 205       add(p3, front, tstride, fcount );
 206     }
 207     else
 208     {
 209       add(split, front, tstride, fcount );
 210       add(split, back, tstride, bcount );
 211
 212       add(p3, back, tstride, bcount );
 213     }
 214   }
 215
 216   // Next test ray segment P3 to P1
 217   if ( r3 != r1 ) // if these are both on the same side...
 218   {
 219     float split[3]; // split the point
 220
 221     intersect(p3,p1,split,plane);
 222
 223     if ( r1 == PTR_FRONT )
 224     {
 225       add(split, front, tstride, fcount );
 226       add(split, back, tstride, bcount );
 227     }
 228     else
 229     {
 230       add(split, front, tstride, fcount );
 231       add(split, back, tstride, bcount );
 232     }
 233   }
 234
 235
 236
 237   return PTR_SPLIT;
 238 }