OPCODE/OPC_TriTriOverlap.h

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