Extras/ConvexDecomposition/splitplane.cpp

   1 #include "float_math.h"
   2 #include <stdio.h>
   3 #include <stdlib.h>
   4 #include <string.h>
   5 #include <assert.h>
   6 #include <float.h>
   7 #include <math.h>
   8
   9
  10 /*----------------------------------------------------------------------
  11                 Copyright (c) 2004 Open Dynamics Framework Group
  12                                         www.physicstools.org
  13                 All rights reserved.
  14
  15                 Redistribution and use in source and binary forms, with or without modification, are permitted provided
  16                 that the following conditions are met:
  17
  18                 Redistributions of source code must retain the above copyright notice, this list of conditions
  19                 and the following disclaimer.
  20
  21                 Redistributions in binary form must reproduce the above copyright notice,
  22                 this list of conditions and the following disclaimer in the documentation
  23                 and/or other materials provided with the distribution.
  24
  25                 Neither the name of the Open Dynamics Framework Group nor the names of its contributors may
  26                 be used to endorse or promote products derived from this software without specific prior written permission.
  27
  28                 THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 'AS IS' AND ANY EXPRESS OR IMPLIED WARRANTIES,
  29                 INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
  30                 DISCLAIMED. IN NO EVENT SHALL THE INTEL OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
  31                 EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
  32                 LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER
  33                 IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF
  34                 THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
  35 -----------------------------------------------------------------------*/
  36
  37 // http://codesuppository.blogspot.com
  38 //
  39 // mailto: jratcliff@infiniplex.net
  40 //
  41 // http://www.amillionpixels.us
  42 //
  43
  44 #include "splitplane.h"
  45 #include "ConvexDecomposition.h"
  46 #include "cd_vector.h"
  47 #include "cd_hull.h"
  48 #include "cd_wavefront.h"
  49 #include "bestfit.h"
  50 #include "planetri.h"
  51 #include "vlookup.h"
  52 #include "meshvolume.h"
  53
  54 namespace ConvexDecomposition
  55 {
  56
  57 static void computePlane(const float *A,const float *B,const float *C,float *plane)
  58 {
  59
  60         float vx = (B[0] - C[0]);
  61         float vy = (B[1] - C[1]);
  62         float vz = (B[2] - C[2]);
  63
  64         float wx = (A[0] - B[0]);
  65         float wy = (A[1] - B[1]);
  66         float wz = (A[2] - B[2]);
  67
  68         float vw_x = vy * wz - vz * wy;
  69         float vw_y = vz * wx - vx * wz;
  70         float vw_z = vx * wy - vy * wx;
  71
  72         float mag = sqrtf((vw_x * vw_x) + (vw_y * vw_y) + (vw_z * vw_z));
  73
  74         if ( mag < 0.000001f )
  75         {
  76                 mag = 0;
  77         }
  78         else
  79         {
  80                 mag = 1.0f/mag;
  81         }
  82
  83         float x = vw_x * mag;
  84         float y = vw_y * mag;
  85         float z = vw_z * mag;
  86
  87
  88         float D = 0.0f - ((x*A[0])+(y*A[1])+(z*A[2]));
  89
  90   plane[0] = x;
  91   plane[1] = y;
  92   plane[2] = z;
  93   plane[3] = D;
  94
  95 }
  96
  97 class Rect3d
  98 {
  99 public:
 100   Rect3d(void) { };
 101
 102   Rect3d(const float *bmin,const float *bmax)
 103   {
 104
 105     mMin[0] = bmin[0];
 106     mMin[1] = bmin[1];
 107     mMin[2] = bmin[2];
 108
 109     mMax[0] = bmax[0];
 110     mMax[1] = bmax[1];
 111     mMax[2] = bmax[2];
 112
 113   }
 114
 115   void SetMin(const float *bmin)
 116   {
 117     mMin[0] = bmin[0];
 118     mMin[1] = bmin[1];
 119     mMin[2] = bmin[2];
 120   }
 121
 122   void SetMax(const float *bmax)
 123   {
 124     mMax[0] = bmax[0];
 125     mMax[1] = bmax[1];
 126     mMax[2] = bmax[2];
 127   }
 128
 129         void SetMin(float x,float y,float z)
 130         {
 131                 mMin[0] = x;
 132                 mMin[1] = y;
 133                 mMin[2] = z;
 134         }
 135
 136         void SetMax(float x,float y,float z)
 137         {
 138                 mMax[0] = x;
 139                 mMax[1] = y;
 140                 mMax[2] = z;
 141         }
 142
 143   float mMin[3];
 144   float mMax[3];
 145 };
 146
 147 void splitRect(unsigned int axis,
 148                                                    const Rect3d &source,
 149                                                          Rect3d &b1,
 150                                                          Rect3d &b2,
 151                                                          const float *midpoint)
 152 {
 153         switch ( axis )
 154         {
 155                 case 0:
 156                         b1.SetMin(source.mMin);
 157                         b1.SetMax( midpoint[0], source.mMax[1], source.mMax[2] );
 158
 159                         b2.SetMin( midpoint[0], source.mMin[1], source.mMin[2] );
 160                         b2.SetMax(source.mMax);
 161
 162                         break;
 163                 case 1:
 164                         b1.SetMin(source.mMin);
 165                         b1.SetMax( source.mMax[0], midpoint[1], source.mMax[2] );
 166
 167                         b2.SetMin( source.mMin[0], midpoint[1], source.mMin[2] );
 168                         b2.SetMax(source.mMax);
 169
 170                         break;
 171                 case 2:
 172                         b1.SetMin(source.mMin);
 173                         b1.SetMax( source.mMax[0], source.mMax[1], midpoint[2] );
 174
 175                         b2.SetMin( source.mMin[0], source.mMin[1], midpoint[2] );
 176                         b2.SetMax(source.mMax);
 177
 178                         break;
 179         }
 180 }
 181
 182 bool computeSplitPlane(unsigned int vcount,
 183                        const float *vertices,
 184                        unsigned int tcount,
 185                        const unsigned int *indices,
 186                        ConvexDecompInterface *callback,
 187                        float *plane)
 188 {
 189   float bmin[3] = { 1e9, 1e9, 1e9 };
 190   float bmax[3] = { -1e9, -1e9, -1e9 };
 191
 192         for (unsigned int i=0; i<vcount; i++)
 193         {
 194     const float *p = &vertices[i*3];
 195
 196                 if ( p[0] < bmin[0] ) bmin[0] = p[0];
 197                 if ( p[1] < bmin[1] ) bmin[1] = p[1];
 198                 if ( p[2] < bmin[2] ) bmin[2] = p[2];
 199
 200                 if ( p[0] > bmax[0] ) bmax[0] = p[0];
 201                 if ( p[1] > bmax[1] ) bmax[1] = p[1];
 202                 if ( p[2] > bmax[2] ) bmax[2] = p[2];
 203
 204   }
 205
 206   float dx = bmax[0] - bmin[0];
 207   float dy = bmax[1] - bmin[1];
 208   float dz = bmax[2] - bmin[2];
 209
 210
 211         float laxis = dx;
 212
 213         unsigned int axis = 0;
 214
 215         if ( dy > dx )
 216         {
 217                 axis = 1;
 218                 laxis = dy;
 219         }
 220
 221         if ( dz > dx && dz > dy )
 222         {
 223                 axis = 2;
 224                 laxis = dz;
 225         }
 226
 227   float p1[3];
 228   float p2[3];
 229   float p3[3];
 230
 231   p3[0] = p2[0] = p1[0] = bmin[0] + dx*0.5f;
 232   p3[1] = p2[1] = p1[1] = bmin[1] + dy*0.5f;
 233   p3[2] = p2[2] = p1[2] = bmin[2] + dz*0.5f;
 234
 235   Rect3d b(bmin,bmax);
 236
 237   Rect3d b1,b2;
 238
 239   splitRect(axis,b,b1,b2,p1);
 240
 241
 242 //  callback->ConvexDebugBound(b1.mMin,b1.mMax,0x00FF00);
 243 //  callback->ConvexDebugBound(b2.mMin,b2.mMax,0xFFFF00);
 244
 245   switch ( axis )
 246   {
 247     case 0:
 248       p2[1] = bmin[1];
 249       p2[2] = bmin[2];
 250
 251       if ( dz > dy )
 252       {
 253         p3[1] = bmax[1];
 254         p3[2] = bmin[2];
 255       }
 256       else
 257       {
 258         p3[1] = bmin[1];
 259         p3[2] = bmax[2];
 260       }
 261
 262       break;
 263     case 1:
 264       p2[0] = bmin[0];
 265       p2[2] = bmin[2];
 266
 267       if ( dx > dz )
 268       {
 269         p3[0] = bmax[0];
 270         p3[2] = bmin[2];
 271       }
 272       else
 273       {
 274         p3[0] = bmin[0];
 275         p3[2] = bmax[2];
 276       }
 277
 278       break;
 279     case 2:
 280       p2[0] = bmin[0];
 281       p2[1] = bmin[1];
 282
 283       if ( dx > dy )
 284       {
 285         p3[0] = bmax[0];
 286         p3[1] = bmin[1];
 287       }
 288       else
 289       {
 290         p3[0] = bmin[0];
 291         p3[1] = bmax[1];
 292       }
 293
 294       break;
 295   }
 296
 297 //  callback->ConvexDebugTri(p1,p2,p3,0xFF0000);
 298
 299         computePlane(p1,p2,p3,plane);
 300
 301   return true;
 302
 303 }
 304
 305
 306 }