Extras/CDTestFramework/Opcode/OPC_TriTriOverlap.h

   1 /*
   2  *      OPCODE - Optimized Collision Detection
   3  * http://www.codercorner.com/Opcode.htm
   4  *
   5  * Copyright (c) 2001-2008 Pierre Terdiman,  pierre@codercorner.com
   6
   7 This software is provided 'as-is', without any express or implied warranty.
   8 In no event will the authors be held liable for any damages arising from the use of this software.
   9 Permission is granted to anyone to use this software for any purpose,
  10 including commercial applications, and to alter it and redistribute it freely,
  11 subject to the following restrictions:
  12
  13 1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
  14 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
  15 3. This notice may not be removed or altered from any source distribution.
  16 */
  17
  18 //! if OPC_TRITRI_EPSILON_TEST is true then we do a check (if |dv|<EPSILON then dv=0.0;) else no check is done (which is less robust, but faster)
  19 #define LOCAL_EPSILON 0.000001f
  20
  21 //! sort so that a<=b
  22 #define SORT(a,b)                       \
  23         if(a>b)                                 \
  24         {                                               \
  25                 const float c=a;        \
  26                 a=b;                            \
  27                 b=c;                            \
  28         }
  29
  30 //! Edge to edge test based on Franlin Antonio's gem: "Faster Line Segment Intersection", in Graphics Gems III, pp. 199-202
  31 #define EDGE_EDGE_TEST(V0, U0, U1)                                              \
  32         Bx = U0[i0] - U1[i0];                                                           \
  33         By = U0[i1] - U1[i1];                                                           \
  34         Cx = V0[i0] - U0[i0];                                                           \
  35         Cy = V0[i1] - U0[i1];                                                           \
  36         f  = Ay*Bx - Ax*By;                                                                     \
  37         d  = By*Cx - Bx*Cy;                                                                     \
  38         if((f>0.0f && d>=0.0f && d<=f) || (f<0.0f && d<=0.0f && d>=f))  \
  39         {                                                                                                       \
  40                 const float e=Ax*Cy - Ay*Cx;                                    \
  41                 if(f>0.0f)                                                                              \
  42                 {                                                                                               \
  43                         if(e>=0.0f && e<=f) return TRUE;                        \
  44                 }                                                                                               \
  45                 else                                                                                    \
  46                 {                                                                                               \
  47                         if(e<=0.0f && e>=f) return TRUE;                        \
  48                 }                                                                                               \
  49         }
  50
  51 //! TO BE DOCUMENTED
  52 #define EDGE_AGAINST_TRI_EDGES(V0, V1, U0, U1, U2)              \
  53 {                                                                                                               \
  54         float Bx,By,Cx,Cy,d,f;                                                          \
  55         const float Ax = V1[i0] - V0[i0];                                       \
  56         const float Ay = V1[i1] - V0[i1];                                       \
  57         /* test edge U0,U1 against V0,V1 */                                     \
  58         EDGE_EDGE_TEST(V0, U0, U1);                                                     \
  59         /* test edge U1,U2 against V0,V1 */                                     \
  60         EDGE_EDGE_TEST(V0, U1, U2);                                                     \
  61         /* test edge U2,U1 against V0,V1 */                                     \
  62         EDGE_EDGE_TEST(V0, U2, U0);                                                     \
  63 }
  64
  65 //! TO BE DOCUMENTED
  66 #define POINT_IN_TRI(V0, U0, U1, U2)                                    \
  67 {                                                                                                               \
  68         /* is T1 completly inside T2? */                                        \
  69         /* check if V0 is inside tri(U0,U1,U2) */                       \
  70         float a  = U1[i1] - U0[i1];                                                     \
  71         float b  = -(U1[i0] - U0[i0]);                                          \
  72         float c  = -a*U0[i0] - b*U0[i1];                                        \
  73         float d0 = a*V0[i0] + b*V0[i1] + c;                                     \
  74                                                                                                                 \
  75         a  = U2[i1] - U1[i1];                                                           \
  76         b  = -(U2[i0] - U1[i0]);                                                        \
  77         c  = -a*U1[i0] - b*U1[i1];                                                      \
  78         const float d1 = a*V0[i0] + b*V0[i1] + c;                       \
  79                                                                                                                 \
  80         a  = U0[i1] - U2[i1];                                                           \
  81         b  = -(U0[i0] - U2[i0]);                                                        \
  82         c  = -a*U2[i0] - b*U2[i1];                                                      \
  83         const float d2 = a*V0[i0] + b*V0[i1] + c;                       \
  84         if(d0*d1>0.0f)                                                                          \
  85         {                                                                                                       \
  86                 if(d0*d2>0.0f) return TRUE;                                             \
  87         }                                                                                                       \
  88 }
  89
  90 //! TO BE DOCUMENTED
  91 BOOL CoplanarTriTri(const Point& n, const Point& v0, const Point& v1, const Point& v2, const Point& u0, const Point& u1, const Point& u2)
  92 {
  93         float A[3];
  94         short i0,i1;
  95         /* first project onto an axis-aligned plane, that maximizes the area */
  96         /* of the triangles, compute indices: i0,i1. */
  97         A[0] = fabsf(n[0]);
  98         A[1] = fabsf(n[1]);
  99         A[2] = fabsf(n[2]);
 100         if(A[0]>A[1])
 101         {
 102                 if(A[0]>A[2])
 103                 {
 104                         i0=1;      /* A[0] is greatest */
 105                         i1=2;
 106                 }
 107                 else
 108                 {
 109                         i0=0;      /* A[2] is greatest */
 110                         i1=1;
 111                 }
 112         }
 113         else   /* A[0]<=A[1] */
 114         {
 115                 if(A[2]>A[1])
 116                 {
 117                         i0=0;      /* A[2] is greatest */
 118                         i1=1;
 119                 }
 120                 else
 121                 {
 122                         i0=0;      /* A[1] is greatest */
 123                         i1=2;
 124                 }
 125         }
 126
 127         /* test all edges of triangle 1 against the edges of triangle 2 */
 128         EDGE_AGAINST_TRI_EDGES(v0, v1, u0, u1, u2);
 129         EDGE_AGAINST_TRI_EDGES(v1, v2, u0, u1, u2);
 130         EDGE_AGAINST_TRI_EDGES(v2, v0, u0, u1, u2);
 131
 132         /* finally, test if tri1 is totally contained in tri2 or vice versa */
 133         POINT_IN_TRI(v0, u0, u1, u2);
 134         POINT_IN_TRI(u0, v0, v1, v2);
 135
 136         return FALSE;
 137 }
 138
 139 //! TO BE DOCUMENTED
 140 #define NEWCOMPUTE_INTERVALS(VV0, VV1, VV2, D0, D1, D2, D0D1, D0D2, A, B, C, X0, X1)    \
 141 {                                                                                                                                                                               \
 142         if(D0D1>0.0f)                                                                                                                                           \
 143         {                                                                                                                                                                       \
 144                 /* here we know that D0D2<=0.0 */                                                                                               \
 145                 /* that is D0, D1 are on the same side, D2 on the other or on the plane */              \
 146                 A=VV2; B=(VV0 - VV2)*D2; C=(VV1 - VV2)*D2; X0=D2 - D0; X1=D2 - D1;                              \
 147         }                                                                                                                                                                       \
 148         else if(D0D2>0.0f)                                                                                                                                      \
 149         {                                                                                                                                                                       \
 150                 /* here we know that d0d1<=0.0 */                                                                                               \
 151                 A=VV1; B=(VV0 - VV1)*D1; C=(VV2 - VV1)*D1; X0=D1 - D0; X1=D1 - D2;                              \
 152         }                                                                                                                                                                       \
 153         else if(D1*D2>0.0f || D0!=0.0f)                                                                                                         \
 154         {                                                                                                                                                                       \
 155                 /* here we know that d0d1<=0.0 or that D0!=0.0 */                                                               \
 156                 A=VV0; B=(VV1 - VV0)*D0; C=(VV2 - VV0)*D0; X0=D0 - D1; X1=D0 - D2;                              \
 157         }                                                                                                                                                                       \
 158         else if(D1!=0.0f)                                                                                                                                       \
 159         {                                                                                                                                                                       \
 160                 A=VV1; B=(VV0 - VV1)*D1; C=(VV2 - VV1)*D1; X0=D1 - D0; X1=D1 - D2;                              \
 161         }                                                                                                                                                                       \
 162         else if(D2!=0.0f)                                                                                                                                       \
 163         {                                                                                                                                                                       \
 164                 A=VV2; B=(VV0 - VV2)*D2; C=(VV1 - VV2)*D2; X0=D2 - D0; X1=D2 - D1;                              \
 165         }                                                                                                                                                                       \
 166         else                                                                                                                                                            \
 167         {                                                                                                                                                                       \
 168                 /* triangles are coplanar */                                                                                                    \
 169                 return CoplanarTriTri(N1, V0, V1, V2, U0, U1, U2);                                                              \
 170         }                                                                                                                                                                       \
 171 }
 172
 173 ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
 174 /**
 175  *      Triangle/triangle intersection test routine,
 176  *      by Tomas Moller, 1997.
 177  *      See article "A Fast Triangle-Triangle Intersection Test",
 178  *      Journal of Graphics Tools, 2(2), 1997
 179  *
 180  *      Updated June 1999: removed the divisions -- a little faster now!
 181  *      Updated October 1999: added {} to CROSS and SUB macros
 182  *
 183  *      int NoDivTriTriIsect(float V0[3],float V1[3],float V2[3],
 184  *                      float U0[3],float U1[3],float U2[3])
 185  *
 186  *      \param          V0              [in] triangle 0, vertex 0
 187  *      \param          V1              [in] triangle 0, vertex 1
 188  *      \param          V2              [in] triangle 0, vertex 2
 189  *      \param          U0              [in] triangle 1, vertex 0
 190  *      \param          U1              [in] triangle 1, vertex 1
 191  *      \param          U2              [in] triangle 1, vertex 2
 192  *      \return         true if triangles overlap
 193  */
 194 ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
 195 inline_ BOOL AABBTreeCollider::TriTriOverlap(const Point& V0, const Point& V1, const Point& V2, const Point& U0, const Point& U1, const Point& U2)
 196 {
 197         // Stats
 198         mNbPrimPrimTests++;
 199
 200         // Compute plane equation of triangle(V0,V1,V2)
 201         Point E1 = V1 - V0;
 202         Point E2 = V2 - V0;
 203         const Point N1 = E1 ^ E2;
 204         const float d1 =-N1 | V0;
 205         // Plane equation 1: N1.X+d1=0
 206
 207         // Put U0,U1,U2 into plane equation 1 to compute signed distances to the plane
 208         float du0 = (N1|U0) + d1;
 209         float du1 = (N1|U1) + d1;
 210         float du2 = (N1|U2) + d1;
 211
 212         // Coplanarity robustness check
 213 #ifdef OPC_TRITRI_EPSILON_TEST
 214         if(fabsf(du0)<LOCAL_EPSILON) du0 = 0.0f;
 215         if(fabsf(du1)<LOCAL_EPSILON) du1 = 0.0f;
 216         if(fabsf(du2)<LOCAL_EPSILON) du2 = 0.0f;
 217 #endif
 218         const float du0du1 = du0 * du1;
 219         const float du0du2 = du0 * du2;
 220
 221         if(du0du1>0.0f && du0du2>0.0f)  // same sign on all of them + not equal 0 ?
 222                 return FALSE;                           // no intersection occurs
 223
 224         // Compute plane of triangle (U0,U1,U2)
 225         E1 = U1 - U0;
 226         E2 = U2 - U0;
 227         const Point N2 = E1 ^ E2;
 228         const float d2=-N2 | U0;
 229         // plane equation 2: N2.X+d2=0
 230
 231         // put V0,V1,V2 into plane equation 2
 232         float dv0 = (N2|V0) + d2;
 233         float dv1 = (N2|V1) + d2;
 234         float dv2 = (N2|V2) + d2;
 235
 236 #ifdef OPC_TRITRI_EPSILON_TEST
 237         if(fabsf(dv0)<LOCAL_EPSILON) dv0 = 0.0f;
 238         if(fabsf(dv1)<LOCAL_EPSILON) dv1 = 0.0f;
 239         if(fabsf(dv2)<LOCAL_EPSILON) dv2 = 0.0f;
 240 #endif
 241
 242         const float dv0dv1 = dv0 * dv1;
 243         const float dv0dv2 = dv0 * dv2;
 244
 245         if(dv0dv1>0.0f && dv0dv2>0.0f)  // same sign on all of them + not equal 0 ?
 246                 return FALSE;                           // no intersection occurs
 247
 248         // Compute direction of intersection line
 249         const Point D = N1^N2;
 250
 251         // Compute and index to the largest component of D
 252         float max=fabsf(D[0]);
 253         short index=0;
 254         float bb=fabsf(D[1]);
 255         float cc=fabsf(D[2]);
 256         if(bb>max) max=bb,index=1;
 257         if(cc>max) max=cc,index=2;
 258
 259         // This is the simplified projection onto L
 260         const float vp0 = V0[index];
 261         const float vp1 = V1[index];
 262         const float vp2 = V2[index];
 263
 264         const float up0 = U0[index];
 265         const float up1 = U1[index];
 266         const float up2 = U2[index];
 267
 268         // Compute interval for triangle 1
 269         float a,b,c,x0,x1;
 270         NEWCOMPUTE_INTERVALS(vp0,vp1,vp2,dv0,dv1,dv2,dv0dv1,dv0dv2,a,b,c,x0,x1);
 271
 272         // Compute interval for triangle 2
 273         float d,e,f,y0,y1;
 274         NEWCOMPUTE_INTERVALS(up0,up1,up2,du0,du1,du2,du0du1,du0du2,d,e,f,y0,y1);
 275
 276         const float xx=x0*x1;
 277         const float yy=y0*y1;
 278         const float xxyy=xx*yy;
 279
 280         float isect1[2], isect2[2];
 281
 282         float tmp=a*xxyy;
 283         isect1[0]=tmp+b*x1*yy;
 284         isect1[1]=tmp+c*x0*yy;
 285
 286         tmp=d*xxyy;
 287         isect2[0]=tmp+e*xx*y1;
 288         isect2[1]=tmp+f*xx*y0;
 289
 290         SORT(isect1[0],isect1[1]);
 291         SORT(isect2[0],isect2[1]);
 292
 293         if(isect1[1]<isect2[0] || isect2[1]<isect1[0]) return FALSE;
 294         return TRUE;
 295 }