Extras/ConvexDecomposition/raytri.cpp

   1 #include "float_math.h"
   2 #include <stdio.h>
   3 #include <stdlib.h>
   4 #include <string.h>
   5 #include <assert.h>
   6 #include <math.h>
   7
   8 #include "raytri.h"
   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
  45 /* a = b - c */
  46 #define vector(a,b,c) \
  47         (a)[0] = (b)[0] - (c)[0];       \
  48         (a)[1] = (b)[1] - (c)[1];       \
  49         (a)[2] = (b)[2] - (c)[2];
  50
  51
  52
  53 #define innerProduct(v,q) \
  54                 ((v)[0] * (q)[0] + \
  55                 (v)[1] * (q)[1] + \
  56                 (v)[2] * (q)[2])
  57
  58 #define crossProduct(a,b,c) \
  59         (a)[0] = (b)[1] * (c)[2] - (c)[1] * (b)[2]; \
  60         (a)[1] = (b)[2] * (c)[0] - (c)[2] * (b)[0]; \
  61         (a)[2] = (b)[0] * (c)[1] - (c)[0] * (b)[1];
  62
  63 bool rayIntersectsTriangle(const float *p,const float *d,const float *v0,const float *v1,const float *v2,float &t)
  64 {
  65
  66         float e1[3],e2[3],h[3],s[3],q[3];
  67         float a,f,u,v;
  68
  69         vector(e1,v1,v0);
  70         vector(e2,v2,v0);
  71         crossProduct(h,d,e2);
  72         a = innerProduct(e1,h);
  73
  74         if (a > -0.00001 && a < 0.00001)
  75                 return(false);
  76
  77         f = 1/a;
  78         vector(s,p,v0);
  79         u = f * (innerProduct(s,h));
  80
  81         if (u < 0.0 || u > 1.0)
  82                 return(false);
  83
  84         crossProduct(q,s,e1);
  85         v = f * innerProduct(d,q);
  86         if (v < 0.0 || u + v > 1.0)
  87                 return(false);
  88         // at this stage we can compute t to find out where
  89         // the intersection point is on the line
  90         t = f * innerProduct(e2,q);
  91         if (t > 0) // ray intersection
  92                 return(true);
  93         else // this means that there is a line intersection
  94                  // but not a ray intersection
  95                  return (false);
  96 }
  97
  98
  99 bool lineIntersectsTriangle(const float *rayStart,const float *rayEnd,const float *p1,const float *p2,const float *p3,float *sect)
 100 {
 101         float dir[3];
 102
 103   dir[0] = rayEnd[0] - rayStart[0];
 104   dir[1] = rayEnd[1] - rayStart[1];
 105   dir[2] = rayEnd[2] - rayStart[2];
 106
 107   float d = sqrtf(dir[0]*dir[0] + dir[1]*dir[1] + dir[2]*dir[2]);
 108   float r = 1.0f / d;
 109
 110   dir[0]*=r;
 111   dir[1]*=r;
 112   dir[2]*=r;
 113
 114
 115   float t;
 116
 117         bool ret = rayIntersectsTriangle(rayStart, dir, p1, p2, p3, t );
 118
 119         if ( ret )
 120         {
 121                 if ( t > d )
 122                 {
 123                         sect[0] = rayStart[0] + dir[0]*t;
 124                         sect[1] = rayStart[1] + dir[1]*t;
 125                         sect[2] = rayStart[2] + dir[2]*t;
 126                 }
 127                 else
 128                 {
 129                         ret = false;
 130                 }
 131         }
 132
 133   return ret;
 134 }