| 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 | if(fabsf(du0)<LOCAL_EPSILON) du0 = 0.0f; |
|---|
| 199 | if(fabsf(du1)<LOCAL_EPSILON) du1 = 0.0f; |
|---|
| 200 | if(fabsf(du2)<LOCAL_EPSILON) du2 = 0.0f; |
|---|
| 201 | #endif |
|---|
| 202 | const float du0du1 = du0 * du1; |
|---|
| 203 | const float du0du2 = du0 * du2; |
|---|
| 204 | |
|---|
| 205 | if(du0du1>0.0f && du0du2>0.0f) // same sign on all of them + not equal 0 ? |
|---|
| 206 | return FALSE; // no intersection occurs |
|---|
| 207 | |
|---|
| 208 | // Compute plane of triangle (U0,U1,U2) |
|---|
| 209 | E1 = U1 - U0; |
|---|
| 210 | E2 = U2 - U0; |
|---|
| 211 | const Point N2 = E1 ^ E2; |
|---|
| 212 | const float d2=-N2 | U0; |
|---|
| 213 | // plane equation 2: N2.X+d2=0 |
|---|
| 214 | |
|---|
| 215 | // put V0,V1,V2 into plane equation 2 |
|---|
| 216 | float dv0 = (N2|V0) + d2; |
|---|
| 217 | float dv1 = (N2|V1) + d2; |
|---|
| 218 | float dv2 = (N2|V2) + d2; |
|---|
| 219 | |
|---|
| 220 | #ifdef OPC_TRITRI_EPSILON_TEST |
|---|
| 221 | if(fabsf(dv0)<LOCAL_EPSILON) dv0 = 0.0f; |
|---|
| 222 | if(fabsf(dv1)<LOCAL_EPSILON) dv1 = 0.0f; |
|---|
| 223 | if(fabsf(dv2)<LOCAL_EPSILON) dv2 = 0.0f; |
|---|
| 224 | #endif |
|---|
| 225 | |
|---|
| 226 | const float dv0dv1 = dv0 * dv1; |
|---|
| 227 | const float dv0dv2 = dv0 * dv2; |
|---|
| 228 | |
|---|
| 229 | if(dv0dv1>0.0f && dv0dv2>0.0f) // same sign on all of them + not equal 0 ? |
|---|
| 230 | return FALSE; // no intersection occurs |
|---|
| 231 | |
|---|
| 232 | // Compute direction of intersection line |
|---|
| 233 | const Point D = N1^N2; |
|---|
| 234 | |
|---|
| 235 | // Compute and index to the largest component of D |
|---|
| 236 | float max=fabsf(D[0]); |
|---|
| 237 | short index=0; |
|---|
| 238 | float bb=fabsf(D[1]); |
|---|
| 239 | float cc=fabsf(D[2]); |
|---|
| 240 | if(bb>max) max=bb,index=1; |
|---|
| 241 | if(cc>max) max=cc,index=2; |
|---|
| 242 | |
|---|
| 243 | // This is the simplified projection onto L |
|---|
| 244 | const float vp0 = V0[index]; |
|---|
| 245 | const float vp1 = V1[index]; |
|---|
| 246 | const float vp2 = V2[index]; |
|---|
| 247 | |
|---|
| 248 | const float up0 = U0[index]; |
|---|
| 249 | const float up1 = U1[index]; |
|---|
| 250 | const float up2 = U2[index]; |
|---|
| 251 | |
|---|
| 252 | // Compute interval for triangle 1 |
|---|
| 253 | float a,b,c,x0,x1; |
|---|
| 254 | NEWCOMPUTE_INTERVALS(vp0,vp1,vp2,dv0,dv1,dv2,dv0dv1,dv0dv2,a,b,c,x0,x1); |
|---|
| 255 | |
|---|
| 256 | // Compute interval for triangle 2 |
|---|
| 257 | float d,e,f,y0,y1; |
|---|
| 258 | NEWCOMPUTE_INTERVALS(up0,up1,up2,du0,du1,du2,du0du1,du0du2,d,e,f,y0,y1); |
|---|
| 259 | |
|---|
| 260 | const float xx=x0*x1; |
|---|
| 261 | const float yy=y0*y1; |
|---|
| 262 | const float xxyy=xx*yy; |
|---|
| 263 | |
|---|
| 264 | float isect1[2], isect2[2]; |
|---|
| 265 | |
|---|
| 266 | float tmp=a*xxyy; |
|---|
| 267 | isect1[0]=tmp+b*x1*yy; |
|---|
| 268 | isect1[1]=tmp+c*x0*yy; |
|---|
| 269 | |
|---|
| 270 | tmp=d*xxyy; |
|---|
| 271 | isect2[0]=tmp+e*xx*y1; |
|---|
| 272 | isect2[1]=tmp+f*xx*y0; |
|---|
| 273 | |
|---|
| 274 | SORT(isect1[0],isect1[1]); |
|---|
| 275 | SORT(isect2[0],isect2[1]); |
|---|
| 276 | |
|---|
| 277 | if(isect1[1]<isect2[0] || isect2[1]<isect1[0]) return FALSE; |
|---|
| 278 | return TRUE; |
|---|
| 279 | } |
|---|
