OPC_TriTriOverlap.h
Go to the documentation of this file.
1 
3 #define LOCAL_EPSILON 0.000001f
4 
6 #define SORT(a,b) \
7  if(a>b) \
8  { \
9  const float c=a; \
10  a=b; \
11  b=c; \
12  }
13 
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 
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 
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 
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 
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 
158 
178 BOOL AABBTreeCollider::TriTriOverlap(const Point& V0, const Point& V1, const Point& V2, const Point& U0, const Point& U1, const Point& U2)
180 {
181 
182  // Stats
183  mNbPrimPrimTests++;
184 
185  // Modified by S-cubed, Inc.
186  // Detect collisions by using TriOverlap.cpp instead of the collision detection by OPCODE
187 
188  hrp::collision_data c_pair;
189  hrp::Vector3 i0, i1, i2;
190  hrp::Vector3 p0, p1, p2;
191 
192  // Convert coordinates to match the interface of tri_tri_overlap()@TriOerlap.cpp
193  // Ice/IcePointer => Vector3
194 
195  i0[0] = V0.x; i0[1] = V0.y; i0[2] = V0.z;
196  i1[0] = V1.x; i1[1] = V1.y; i1[2] = V1.z;
197  i2[0] = V2.x; i2[1] = V2.y; i2[2] = V2.z;
198 
199  p0[0] = U0.x; p0[1] = U0.y; p0[2] = U0.z;
200  p1[0] = U1.x; p1[1] = U1.y; p1[2] = U1.z;
201  p2[0] = U2.x; p2[1] = U2.y; p2[2] = U2.z;
202 
203  if(collisionPairInserter &&
204  collisionPairInserter->detectTriTriOverlap(i0, i1, i2, p0, p1, p2, &c_pair)){
205  /*
206  insert_collision_pair(mNowNode0, mNowNode1, mId0, mId1,
207  c_pair.num_of_i_points,
208  c_pair.i_points,
209  c_pair.n_vector,
210  c_pair.depth,
211  c_pair.n,
212  c_pair.m,
213  c_pair.c_type,
214  (MeshInterface*)mIMesh0,
215  (MeshInterface*)mIMesh1);
216  */
217  collisionPairInserter->apply(mNowNode0, mNowNode1, mId0, mId1,
218  c_pair.num_of_i_points,
219  c_pair.i_points,
220  c_pair.n_vector,
221  c_pair.depth,
222  c_pair.n,
223  c_pair.m,
224  c_pair.c_type,
225  (MeshInterface*)mIMesh0,
226  (MeshInterface*)mIMesh1);
227  return TRUE;
228  }
229  return FALSE;
230 }
AABBTreeCollider::mNowNode0
const AABBCollisionNode * mNowNode0
Definition: OPC_TreeCollider.h:212
FALSE
#define FALSE
Definition: OPC_IceHook.h:9
hrp::collision_data
Definition: CollisionData.h:20
AABBTreeCollider::TriTriOverlap
BOOL TriTriOverlap(const Point &V0, const Point &V1, const Point &V2, const Point &U0, const Point &U1, const Point &U2)
Definition: OPC_TriTriOverlap.h:179
hrp::collision_data::num_of_i_points
int num_of_i_points
Definition: CollisionData.h:26
hrp::collision_data::c_type
int c_type
Definition: CollisionData.h:35
TRUE
#define TRUE
Definition: OPC_IceHook.h:13
AABBTreeCollider::mNbPrimPrimTests
udword mNbPrimPrimTests
Number of Primitive-Primitive tests.
Definition: OPC_TreeCollider.h:195
AABBTreeCollider::collisionPairInserter
hrp::CollisionPairInserterBase * collisionPairInserter
Definition: OPC_TreeCollider.h:220
Point::z
float z
Definition: IcePoint.h:524
EDGE_AGAINST_TRI_EDGES
#define EDGE_AGAINST_TRI_EDGES(V0, V1, U0, U1, U2)
TO BE DOCUMENTED.
Definition: OPC_TriTriOverlap.h:36
Point
Definition: IcePoint.h:25
hrp::collision_data::n_vector
Vector3 n_vector
Definition: CollisionData.h:30
hrp::collision_data::depth
double depth
Definition: CollisionData.h:31
AABBTreeCollider::mIMesh0
const MeshInterface * mIMesh0
User-defined mesh interface for object0.
Definition: OPC_TreeCollider.h:191
hrp::CollisionPairInserterBase::detectTriTriOverlap
virtual int detectTriTriOverlap(const Vector3 &P1, const Vector3 &P2, const Vector3 &P3, const Vector3 &Q1, const Vector3 &Q2, const Vector3 &Q3, collision_data *col_p)=0
detect collsiion between triangles
hrp::Vector3
Eigen::Vector3d Vector3
Definition: EigenTypes.h:11
BOOL
int BOOL
Another boolean type.
Definition: IceTypes.h:102
Point::x
float x
Definition: IcePoint.h:524
hrp::collision_data::m
Vector3 m
Definition: CollisionData.h:34
POINT_IN_TRI
#define POINT_IN_TRI(V0, U0, U1, U2)
TO BE DOCUMENTED.
Definition: OPC_TriTriOverlap.h:50
AABBTreeCollider::mNowNode1
const AABBCollisionNode * mNowNode1
Definition: OPC_TreeCollider.h:213
CoplanarTriTri
BOOL CoplanarTriTri(const Point &n, const Point &v0, const Point &v1, const Point &v2, const Point &u0, const Point &u1, const Point &u2)
TO BE DOCUMENTED.
Definition: OPC_TriTriOverlap.h:75
Point::y
float y
Definition: IcePoint.h:524
hrp::CollisionPairInserterBase::apply
virtual int apply(const Opcode::AABBCollisionNode *b1, const Opcode::AABBCollisionNode *b2, int id1, int id2, int num_of_i_points, Vector3 i_points[4], Vector3 &n_vector, double depth, Vector3 &n1, Vector3 &m1, int ctype, Opcode::MeshInterface *mesh1, Opcode::MeshInterface *mesh2)=0
refine collision information using neighboring triangls
hrp::collision_data::n
Vector3 n
Definition: CollisionData.h:33
hrp::collision_data::i_points
Vector3 i_points[4]
Definition: CollisionData.h:27
AABBTreeCollider::mIMesh1
const MeshInterface * mIMesh1
User-defined mesh interface for object1.
Definition: OPC_TreeCollider.h:192

openhrp3
Author(s): AIST, General Robotix Inc., Nakamura Lab of Dept. of Mechano Informatics at University of Tokyo
autogenerated on Thu Sep 8 2022 02:24:04