OPC_TriTriOverlap.h
Go to the documentation of this file.
00001 
00003 #define LOCAL_EPSILON 0.000001f
00004 
00006 #define SORT(a,b)                       \
00007         if(a>b)                                 \
00008         {                                               \
00009                 const float c=a;        \
00010                 a=b;                            \
00011                 b=c;                            \
00012         }
00013 
00015 #define EDGE_EDGE_TEST(V0, U0, U1)                                              \
00016         Bx = U0[i0] - U1[i0];                                                           \
00017         By = U0[i1] - U1[i1];                                                           \
00018         Cx = V0[i0] - U0[i0];                                                           \
00019         Cy = V0[i1] - U0[i1];                                                           \
00020         f  = Ay*Bx - Ax*By;                                                                     \
00021         d  = By*Cx - Bx*Cy;                                                                     \
00022         if((f>0.0f && d>=0.0f && d<=f) || (f<0.0f && d<=0.0f && d>=f))  \
00023         {                                                                                                       \
00024                 const float e=Ax*Cy - Ay*Cx;                                    \
00025                 if(f>0.0f)                                                                              \
00026                 {                                                                                               \
00027                         if(e>=0.0f && e<=f) return TRUE;                        \
00028                 }                                                                                               \
00029                 else                                                                                    \
00030                 {                                                                                               \
00031                         if(e<=0.0f && e>=f) return TRUE;                        \
00032                 }                                                                                               \
00033         }
00034 
00036 #define EDGE_AGAINST_TRI_EDGES(V0, V1, U0, U1, U2)              \
00037 {                                                                                                               \
00038         float Bx,By,Cx,Cy,d,f;                                                          \
00039         const float Ax = V1[i0] - V0[i0];                                       \
00040         const float Ay = V1[i1] - V0[i1];                                       \
00041         /* test edge U0,U1 against V0,V1 */                                     \
00042         EDGE_EDGE_TEST(V0, U0, U1);                                                     \
00043         /* test edge U1,U2 against V0,V1 */                                     \
00044         EDGE_EDGE_TEST(V0, U1, U2);                                                     \
00045         /* test edge U2,U1 against V0,V1 */                                     \
00046         EDGE_EDGE_TEST(V0, U2, U0);                                                     \
00047 }
00048 
00050 #define POINT_IN_TRI(V0, U0, U1, U2)                                    \
00051 {                                                                                                               \
00052         /* is T1 completly inside T2? */                                        \
00053         /* check if V0 is inside tri(U0,U1,U2) */                       \
00054         float a  = U1[i1] - U0[i1];                                                     \
00055         float b  = -(U1[i0] - U0[i0]);                                          \
00056         float c  = -a*U0[i0] - b*U0[i1];                                        \
00057         float d0 = a*V0[i0] + b*V0[i1] + c;                                     \
00058                                                                                                                 \
00059         a  = U2[i1] - U1[i1];                                                           \
00060         b  = -(U2[i0] - U1[i0]);                                                        \
00061         c  = -a*U1[i0] - b*U1[i1];                                                      \
00062         const float d1 = a*V0[i0] + b*V0[i1] + c;                       \
00063                                                                                                                 \
00064         a  = U0[i1] - U2[i1];                                                           \
00065         b  = -(U0[i0] - U2[i0]);                                                        \
00066         c  = -a*U2[i0] - b*U2[i1];                                                      \
00067         const float d2 = a*V0[i0] + b*V0[i1] + c;                       \
00068         if(d0*d1>0.0f)                                                                          \
00069         {                                                                                                       \
00070                 if(d0*d2>0.0f) return TRUE;                                             \
00071         }                                                                                                       \
00072 }
00073 
00075 BOOL CoplanarTriTri(const Point& n, const Point& v0, const Point& v1, const Point& v2, const Point& u0, const Point& u1, const Point& u2)
00076 {
00077         float A[3];
00078         short i0,i1;
00079         /* first project onto an axis-aligned plane, that maximizes the area */
00080         /* of the triangles, compute indices: i0,i1. */
00081         A[0] = fabsf(n[0]);
00082         A[1] = fabsf(n[1]);
00083         A[2] = fabsf(n[2]);
00084         if(A[0]>A[1])
00085         {
00086                 if(A[0]>A[2])
00087                 {
00088                         i0=1;      /* A[0] is greatest */
00089                         i1=2;
00090                 }
00091                 else
00092                 {
00093                         i0=0;      /* A[2] is greatest */
00094                         i1=1;
00095                 }
00096         }
00097         else   /* A[0]<=A[1] */
00098         {
00099                 if(A[2]>A[1])
00100                 {
00101                         i0=0;      /* A[2] is greatest */
00102                         i1=1;
00103                 }
00104                 else
00105                 {
00106                         i0=0;      /* A[1] is greatest */
00107                         i1=2;
00108                 }
00109         }
00110 
00111         /* test all edges of triangle 1 against the edges of triangle 2 */
00112         EDGE_AGAINST_TRI_EDGES(v0, v1, u0, u1, u2);
00113         EDGE_AGAINST_TRI_EDGES(v1, v2, u0, u1, u2);
00114         EDGE_AGAINST_TRI_EDGES(v2, v0, u0, u1, u2);
00115 
00116         /* finally, test if tri1 is totally contained in tri2 or vice versa */
00117         POINT_IN_TRI(v0, u0, u1, u2);
00118         POINT_IN_TRI(u0, v0, v1, v2);
00119 
00120         return FALSE;
00121 }
00122 
00124 #define NEWCOMPUTE_INTERVALS(VV0, VV1, VV2, D0, D1, D2, D0D1, D0D2, A, B, C, X0, X1)    \
00125 {                                                                                                                                                                               \
00126         if(D0D1>0.0f)                                                                                                                                           \
00127         {                                                                                                                                                                       \
00128                 /* here we know that D0D2<=0.0 */                                                                                               \
00129                 /* that is D0, D1 are on the same side, D2 on the other or on the plane */              \
00130                 A=VV2; B=(VV0 - VV2)*D2; C=(VV1 - VV2)*D2; X0=D2 - D0; X1=D2 - D1;                              \
00131         }                                                                                                                                                                       \
00132         else if(D0D2>0.0f)                                                                                                                                      \
00133         {                                                                                                                                                                       \
00134                 /* here we know that d0d1<=0.0 */                                                                                               \
00135                 A=VV1; B=(VV0 - VV1)*D1; C=(VV2 - VV1)*D1; X0=D1 - D0; X1=D1 - D2;                              \
00136         }                                                                                                                                                                       \
00137         else if(D1*D2>0.0f || D0!=0.0f)                                                                                                         \
00138         {                                                                                                                                                                       \
00139                 /* here we know that d0d1<=0.0 or that D0!=0.0 */                                                               \
00140                 A=VV0; B=(VV1 - VV0)*D0; C=(VV2 - VV0)*D0; X0=D0 - D1; X1=D0 - D2;                              \
00141         }                                                                                                                                                                       \
00142         else if(D1!=0.0f)                                                                                                                                       \
00143         {                                                                                                                                                                       \
00144                 A=VV1; B=(VV0 - VV1)*D1; C=(VV2 - VV1)*D1; X0=D1 - D0; X1=D1 - D2;                              \
00145         }                                                                                                                                                                       \
00146         else if(D2!=0.0f)                                                                                                                                       \
00147         {                                                                                                                                                                       \
00148                 A=VV2; B=(VV0 - VV2)*D2; C=(VV1 - VV2)*D2; X0=D2 - D0; X1=D2 - D1;                              \
00149         }                                                                                                                                                                       \
00150         else                                                                                                                                                            \
00151         {                                                                                                                                                                       \
00152                 /* triangles are coplanar */                                                                                                    \
00153                 return CoplanarTriTri(N1, V0, V1, V2, U0, U1, U2);                                                              \
00154         }                                                                                                                                                                       \
00155 }
00156 
00158 
00178 
00179 BOOL AABBTreeCollider::TriTriOverlap(const Point& V0, const Point& V1, const Point& V2, const Point& U0, const Point& U1, const Point& U2)
00180 {
00181 
00182         // Stats
00183         mNbPrimPrimTests++;
00184 
00185         // Modified by S-cubed, Inc.
00186         // Detect collisions by using TriOverlap.cpp instead of the collision detection by OPCODE
00187 
00188         hrp::collision_data c_pair;
00189         hrp::Vector3 i0, i1, i2;
00190         hrp::Vector3 p0, p1, p2;
00191 
00192         // Convert coordinates to match the interface of tri_tri_overlap()@TriOerlap.cpp
00193         // Ice/IcePointer => Vector3
00194 
00195         i0[0] = V0.x;        i0[1] = V0.y;        i0[2] = V0.z;
00196         i1[0] = V1.x;        i1[1] = V1.y;        i1[2] = V1.z;
00197         i2[0] = V2.x;        i2[1] = V2.y;        i2[2] = V2.z;
00198 
00199         p0[0] = U0.x;        p0[1] = U0.y;        p0[2] = U0.z;
00200         p1[0] = U1.x;        p1[1] = U1.y;        p1[2] = U1.z;
00201         p2[0] = U2.x;        p2[1] = U2.y;        p2[2] = U2.z;
00202         
00203         if(collisionPairInserter &&
00204            collisionPairInserter->detectTriTriOverlap(i0, i1, i2, p0, p1, p2, &c_pair)){
00205             /*
00206                 insert_collision_pair(mNowNode0, mNowNode1, mId0, mId1,
00207                                       c_pair.num_of_i_points,
00208                                       c_pair.i_points,
00209                                       c_pair.n_vector,
00210                                       c_pair.depth,
00211                                       c_pair.n,
00212                                       c_pair.m,
00213                                       c_pair.c_type,
00214                                       (MeshInterface*)mIMesh0,
00215                                       (MeshInterface*)mIMesh1);
00216             */
00217             collisionPairInserter->apply(mNowNode0, mNowNode1, mId0, mId1,
00218                                          c_pair.num_of_i_points,
00219                                          c_pair.i_points,
00220                                          c_pair.n_vector,
00221                                          c_pair.depth,
00222                                          c_pair.n,
00223                                          c_pair.m,
00224                                          c_pair.c_type,
00225                                          (MeshInterface*)mIMesh0,
00226                                          (MeshInterface*)mIMesh1);
00227             return TRUE;
00228         }
00229         return FALSE;
00230 }


openhrp3
Author(s): AIST, General Robotix Inc., Nakamura Lab of Dept. of Mechano Informatics at University of Tokyo
autogenerated on Sun Apr 2 2017 03:43:56