TriOverlap.cpp
Go to the documentation of this file.
1 /*
2  * Copyright (c) 2008, AIST, the University of Tokyo and General Robotix Inc.
3  * All rights reserved. This program is made available under the terms of the
4  * Eclipse Public License v1.0 which accompanies this distribution, and is
5  * available at http://www.eclipse.org/legal/epl-v10.html
6  * Contributors:
7  * National Institute of Advanced Industrial Science and Technology (AIST)
8  * General Robotix Inc.
9  */
10 //
11 // TriOverlap.cpp
12 //
13 
15 #include <cmath>
16 #include <cstdio>
17 #include <iostream>
18 
19 using namespace std;
20 using namespace hrp;
21 
23  const Vector3& P1,
24  const Vector3& P2,
25  const Vector3& P3,
26  const Vector3& Q1,
27  const Vector3& Q2,
28  const Vector3& Q3,
29  collision_data* col_p,
30  CollisionPairInserterBase* collisionPairInserter);
31 
32 
33 namespace {
34 
35  const bool HIRUKAWA_DEBUG = false;
36 
37  /* used in normal_test */
38  enum { NOT_INTERSECT = 0,
39  EDGE1_NOT_INTERSECT = 1,
40  EDGE2_NOT_INTERSECT = 2,
41  EDGE3_NOT_INTERSECT = 3 };
42 
43  /* used in cross_test */
44  const int INTERSECT = 1;
45 }
46 
47 
48 /**********************************************************
49  separability test by the supporting plane of a triangle
50  return value
51  0 : not intersect
52  1 : f1 or e1 doesn't intersect
53  2 : f2 or e2 doesn't intersect
54  3 : f3 or e3 doesn't intersect
55 **********************************************************/
56 static int separability_test_by_face(const Vector3& nm)
57 {
58  if(nm[0] < 0.0 && nm[1] < 0.0 && nm[2] < 0.0 ||
59  nm[0] > 0.0 && nm[1] > 0.0 && nm[2] > 0.0){
60  return NOT_INTERSECT;
61  }
62  if(nm[0] < 0.0 && nm[1] < 0.0 && nm[2] > 0.0 ||
63  nm[0] > 0.0 && nm[1] > 0.0 && nm[2] < 0.0){
64  return EDGE1_NOT_INTERSECT;
65  }
66  if(nm[0] < 0.0 && nm[1] > 0.0 && nm[2] > 0.0 ||
67  nm[0] > 0.0 && nm[1] < 0.0 && nm[2] < 0.0){
68  return EDGE2_NOT_INTERSECT;
69  }
70  if(nm[0] > 0.0 && nm[1] < 0.0 && nm[2] > 0.0 ||
71  nm[0] < 0.0 && nm[1] > 0.0 && nm[2] < 0.0){
72  return EDGE3_NOT_INTERSECT;
73  }
74  return 0;
75 }
76 
77 
78 /**********************************************************
79  triangle inside test:
80  normal vector is cross product of ei*fj
81 ***********************************************************/
83  const Vector3& ef1,
84  const Vector3& ef2,
85  const Vector3& ef3,
86  const Vector3& P3,
87  const Vector3& P1,
88  const Vector3& P2,
89  const Vector3& Q)
90 {
91  double ef1P1 = ef1.dot(P1); /*project P1 on ef1*/
92  double ef1P3 = ef1.dot(P3); /*project P3 on ef1*/
93  double ef1Q = ef1.dot(Q); /*project Q on ef1*/
94 
95  double ef2P2 = ef2.dot(P2); /*project P2 on ef2*/
96  double ef2P1 = ef2.dot(P1); /*project P1 on ef2*/
97  double ef2Q = ef2.dot(Q); /*project Q on ef2*/
98 
99  double ef3P3 = ef3.dot(P3); /*project P3 on ef3*/
100  double ef3P2 = ef3.dot(P2); /*project P2 on ef3*/
101  double ef3Q = ef3.dot(Q); /*project Q on ef3*/
102 
103  if((ef1P3 > ef1P1 && ef1Q > ef1P1 ||
104  ef1P3 < ef1P1 && ef1Q < ef1P1 )
105  &&
106  (ef2P1 > ef2P2 && ef2Q > ef2P2 ||
107  ef2P1 < ef2P2 && ef2Q < ef2P2 )
108  &&
109  (ef3P2 > ef3P3 && ef3Q > ef3P3 ||
110  ef3P2 < ef3P3 && ef3Q < ef3P3 )) {
111  return INTERSECT;
112  }
113 
114  return NOT_INTERSECT;
115 }
116 
117 
119  Vector3& ipt,
120  const Vector3& x1,
121  const Vector3& x2,
122  double mn1,
123  double mn2)
124 {
125  if(mn1 == mn2) /*exit(0);*/ return;
126 
127  if(mn1 >0 && mn2 < 0){
128  ipt = (-(mn2*x1) + mn1*x2)/(mn1-mn2);
129  }else if(mn1 < 0 && mn2 > 0){
130  ipt = (mn2*x1 - mn1*x2)/(-mn1+mn2);
131  }
132 }
133 
134 
135 static inline void find_intersection_pt(
136  Vector3& ipt,
137  const Vector3& x1,
138  const Vector3& x2,
139  double p)
140 {
141  ipt = (1.0 - p) * x1 + p * x2;
142 
143  if(HIRUKAWA_DEBUG){
144  cout << "v1 = " << x1[0] << ", " << x1[1] << ", " << x1[2] << endl;
145  cout << "v2 = " << x2[0] << ", " << x2[1] << ", " << x2[2] << endl;
146  cout << "edge = " << x2[0]-x1[0] << ", " << x2[1]-x1[1] << ", "
147  << x2[2]-x1[2] << endl;
148  cout << "common pt = " << ipt[0] << " " << ipt[1] << " " << ipt[2] << endl;
149  }
150 }
151 
152 
153 //
154 // Calculate the depth of the intersection between two triangles
155 //
156 static inline double calc_depth(
157  const Vector3& ip1,
158  const Vector3& ip2,
159  const Vector3& n)
160 {
161  // vecNormalize(n);
162 
163  if(HIRUKAWA_DEBUG){
164  cout << "calc_depth 1 = " << (ip1 - ip2).dot(n) << endl;
165  }
166 
167  return fabs((ip1 - ip2).dot(n));
168 }
169 
170 
171 static double calc_depth(
172  const Vector3& ip1,
173  const Vector3& ip2,
174  const Vector3& pt1,
175  const Vector3& pt2,
176  const Vector3& pt3,
177  const Vector3& n)
178 {
179  // when a triangle penetrates another triangle at two intersection points
180  // and the separating plane is the supporting plane of the penetrated triangle
181 
182  // vecNormalize(n);
183 
184  double d1 = fabs((ip1 - pt1).dot(n));
185  double d2 = fabs((ip2 - pt2).dot(n));
186  double d3 = fabs((ip1 - pt3).dot(n)); // ip1 can be either ip1 or ip2
187 
188  double depth = (d1 < d2) ? d2 : d1;
189  if(d3 < depth){
190  depth = d3;
191  }
192 
193  if(HIRUKAWA_DEBUG){
194  cout << "calc_depth 3 = " << depth << endl;
195  }
196 
197  return depth;
198 }
199 
200 
201 static void find_foot(
202  const Vector3& ip,
203  const Vector3& pt1,
204  const Vector3& pt2,
205  Vector3& f)
206 {
207  /*
208  double u, v, w, p;
209 
210  u = pt2[0] - pt1[0]; v = pt2[1] - pt1[1]; w = pt2[2] - pt1[2];
211 
212  p = u * (ip[0] - pt1[0]) + v * (ip[1] - pt1[1]) + w * (ip[2] - pt1[2]);
213  p /= u * u + v * v + w * w;
214 
215  f[0] = pt1[0] + u * p; f[1] = pt1[1] + v * p; f[2] = pt1[2] + w * p;
216  */
217 
218  const Vector3 pt(pt2 - pt1);
219  const double p = pt.dot(ip - pt1) / pt.dot(pt);
220  f = pt1 + p * pt;
221 }
222 
223 
224 static double calc_depth(
225  const Vector3& ip,
226  const Vector3& pt1,
227  const Vector3& pt2,
228  const Vector3& pt3,
229  const Vector3& n)
230 {
231  Vector3 f12, f23, f31;
232 
233  find_foot(ip, pt1, pt2, f12);
234  find_foot(ip, pt2, pt3, f23);
235  find_foot(ip, pt3, pt1, f31);
236 
237  if(HIRUKAWA_DEBUG){
238  cout << "ip = " << ip[0] << " " << ip[1] << " " << ip[2] << endl;
239  cout << "f12 = " << f12[0] << " " << f12[1] << " " << f12[2] << endl;
240  cout << "f23 = " << f23[0] << " " << f23[1] << " " << f23[2] << endl;
241  cout << "f31 = " << f31[0] << " " << f31[1] << " " << f31[2] << endl;
242  }
243 
244  // fabs() is taken to cope with numerical error of find_foot()
245  const double d1 = fabs((f12 - ip).dot(n));
246  const double d2 = fabs((f23 - ip).dot(n));
247  const double d3 = fabs((f31 - ip).dot(n));
248 
249  // cout << "d1 d2 d3 = " << d1 << " " << d2 << " " << d3 << endl;
250  // dsum = fabs(d1)+fabs(d2)+fabs(d3);
251  // if(d1<0.0) d1=dsum; if(d2<0.0) d2=dsum; if(d3<0.0) d3=dsum;
252 
253  double depth = (d1 < d2) ? d1 : d2;
254  if(d3 < depth){
255  depth = d3;
256  }
257 
258  if(HIRUKAWA_DEBUG){
259  cout << "calc_depth 4 = " << depth << endl;
260  }
261 
262  return depth;
263 }
264 
265 
266 static double calc_depth2(
267  const Vector3& ip1,
268  const Vector3& ip2,
269  const Vector3& pt1,
270  const Vector3& pt2,
271  const Vector3& pt3,
272  const Vector3& n)
273 {
274  // when a triangle penetrates another triangle at two intersecting points
275  // and the separating plane is the supporting plane of the penetrating triangle
276 
277  const Vector3 nn(n); // vecNormalize(nn);
278 
279  const double depth1 = calc_depth(ip1, pt1, pt2, pt3, nn);
280  const double depth2 = calc_depth(ip2, pt1, pt2, pt3, nn);
281 
282  // cout << "depth1 depth2 = " << depth1 << " " << depth2 << endl;
283  const double depth = (depth1 < depth2) ? depth2 : depth1;
284 
285  if(HIRUKAWA_DEBUG){
286  cout << "calc_depth 5 = " << depth << endl;
287  }
288 
289  return depth;
290 }
291 
292 
293 static void calcNormal(
294  Vector3& vec,
295  const Vector3& v1,
296  const Vector3& v2,
297  const Vector3& v3,
298  double sgn)
299 {
300  // find the vector from v1 to the mid point of v2 and v3 when 0<sgn
301 
302  if(sgn < 0){
303  vec = -(v1 - 0.5 * (v2 + v3)).normalized();
304  } else {
305  vec = (v1 - 0.5 * (v2 + v3)).normalized();
306  }
307 }
308 
309 
311  const Vector3& p1,
312  const Vector3& p2,
313  const Vector3& q1,
314  const Vector3& q2,
315  const Vector3& ip1,
316  const Vector3& ip2,
317  const Vector3& n1,
318  const Vector3& m1,
319  const Vector3& n_vector,
320  double& dp)
321 {
322  const double eps = 1.0e-3; // threshold to consider two edges are parallel
323  //const double vn = 1.0e-2; // threshold to judge an intersecting point is near a vertex
324 
325  const Vector3 e(p2 - p1);
326  const Vector3 f(q2 - q1);
327 
328  const double c11 = e.dot(e);
329  const double c12 = - e.dot(f);
330  const double c21 = - c12;
331  const double c22 = - f.dot(f);
332 
333  const double det = c11 * c22 - c12 * c21;
334  // cout << "det = " << det << endl;
335 
336  if(fabs(det) < eps){
337  return 0;
338  } else {
339  const Vector3 g(q1 - p1);
340  const double a = e.dot(g);
341  const double b = f.dot(g);
342  const double t1 = ( c22 * a - c12 * b) / det;
343  const double t2 = (-c21 * a + c11 * b) / det;
344 
345  // quit if the foot of the common perpendicular is not on an edge
346  if(t1<0 || 1<t1 || t2<0 || 1<t2) return 0;
347 
348  // when two edges are in contact near a vertex of an edge
349  // if(t1<vn || 1.0-vn<t1 || t2<vn || 1.0-vn<t2) return 0;
350 
351  // find_intersection_pt(v1, p1, p2, t1);
352  // find_intersection_pt(v2, q1, q2, t2);
353 
354  dp = calc_depth(ip1, ip2, n_vector);
355 
356  return 1;
357  }
358 }
359 
360 
361 // for vertex-face contact
362 static inline int get_normal_vector_test(
363  const Vector3& ip1,
364  const Vector3& v1,
365  const Vector3& ip2,
366  const Vector3& v2,
367  const Vector3& n1,
368  const Vector3& m1)
369 {
370  // ip1 and ip2 are the intersecting points
371  // v1 and v2 are the vertices of the penetrating triangle
372  // note that (v1-ip1) (v2-ip2) lies on the penetrating triangle
373 
374  // eps_applicability = 0.965926; // Cos(15) threshold to consider two triangles face
375  const double eps_applicability = 0.5; // Cos(60) threshold to consider two triangles face
376 
377  // This condition should be checked mildly because the whole sole of a foot
378  // may sink inside the floor and then no collision is detected.
379  return (eps_applicability < n1.dot(m1)) ? 0 : 1;
380 }
381 
382 
383 // for edge-edge contact
385  Vector3& n_vector,
386  const Vector3& ef0,
387  const Vector3& ip,
388  const Vector3& iq,
389  const Vector3& v1,
390  const Vector3& v2,
391  const Vector3& n1,
392  const Vector3& m1,
393  const Vector3& va1,
394  const Vector3& va2,
395  const Vector3& vb1,
396  const Vector3& vb2)
397 {
398  // ip is the intersecting point on triangle p1p2p3
399  // iq is the intersecting point on triangle q1q2q3
400  // v1 is the vertex of p1p2p3 which is not on the intersecting edge
401  // v2 is the vertex of q1q2q3 which is not on the intersecting edge
402  // note that (v1-ip) lies on triangle p1p2p3 and (v2-iq) on q1q2q3
403 
404  const double eps_applicability = 0.0; // 1.52e-2; // threshold to consider two triangles face
405  const double eps_length = 1.0e-3; // 1mm: thereshold to consider the intersecting edge is short
406  const double eps_theta = 1.0e-1; // threshold to consider cos(theta) is too small
407 
408  // quit if two triangles does not satifsy the applicability condition
409  // i.e. two triangles do not face each other
410  if(- eps_applicability < n1.dot(m1)) return 0;
411 
412  const double ea_length = (va1 - va2).norm();
413  const double eb_length = (vb1 - vb2).norm();
414 
415  // return the normal vector of a triangle if an intersecting edge is too short
416  if(ea_length < eps_length || eb_length < eps_length){
417  // cout << "edge is too short" << endl;
418  if(ea_length < eb_length) {
419  n_vector = m1;
420  } else {
421  n_vector = -n1;
422  }
423  return 1;
424  }
425 
426  const Vector3 sv1(v1 - ip);
427  const double sv1_norm = sv1.norm();
428  const Vector3 sv2(v2 - iq);
429  const double sv2_norm = sv2.norm();
430 
431  if(eps_length < sv1_norm && eps_length < sv2_norm){
432  // quit if two triangles do not satisfy the applicability conditions
433  if(- eps_applicability < sv1.dot(sv2) / (sv1_norm * sv2_norm)){
434  return 0;
435  }
436  }
437 
438  // now neither of two edges is not too short
439  Vector3 ef(ef0.normalized());
440 
441  // Triangle p1p2p3
442  const double theta1 = ef.dot(n1) / n1.norm();
443  const double theta1_abs = fabs(theta1);
444 
445  double theta2;
446  double theta2_abs;
447  if(eps_length < sv1_norm){
448  theta2 = ef.dot(sv1) / sv1_norm;
449  theta2_abs = fabs(theta2);
450  } else {
451  theta2 = 0.0;
452  theta2_abs = 0.0;
453  }
454 
455  // triangle q1q2q3
456  const double theta3 = ef.dot(m1) / m1.norm();
457  const double theta3_abs = fabs(theta3);
458 
459  double theta4;
460  double theta4_abs;
461  if(eps_length < sv2_norm){
462  theta4 = ef.dot(sv2) / sv2_norm;
463  theta4_abs = fabs(theta4);
464  } else {
465  theta4 = 0.0;
466  theta4_abs = 0.0;
467  }
468 
469  if(sv1_norm < eps_length || sv2_norm < eps_length){
470  // when sv1 or sv2 is too short
471  // cout << "sv is too short" << endl;
472  if(theta1_abs < theta3_abs){
473  n_vector = m1;
474  } else {
475  n_vector = -n1;
476  }
477  return 1;
478  }
479 
480  if(theta2_abs < eps_theta && theta4_abs < eps_theta){
481  // when two triangles are coplanar
482  // proof.
483  // ef = (va2-va1)x(vb2-vb1) (1)
484  // sv1 * ef = 0 (2)
485  // sv2 * ef = 0
486  // substituting (1) to (2),
487  // sv1 * (va2-va1) x (vb2-vb1) = 0
488  // (vb2 - vb1) * sv1 x (va2 - va1) = 0
489  // considering sv1 x (va2 - va1) = n,
490  // (vb2 - vb1) * n = 0
491  // in the same way
492  // (va2 - va1) * m = 0
493  // q.e.d.
494 
495  if(theta1_abs < theta3_abs){
496  n_vector = m1;
497  return 1;
498  } else{
499  n_vector = -n1;
500  return 1;
501  }
502  }
503 
504  // return 0 if the plane which passes through ip with normal vector ef
505  // does not separate v1 and v2
506  if(-eps_applicability < theta2 * theta4) return 0;
507 
508  //
509  // regular case
510  //
511  double theta12;
512  if(theta1_abs < theta2_abs){
513  theta12 = theta2;
514  } else {
515  theta12 = -1.0 * theta1;
516  }
517 
518  double theta34;
519  if(theta3_abs < theta4_abs){
520  theta34 = -1.0 * theta4;
521  } else {
522  theta34 = theta3;
523  }
524 
525  double theta;
526  if(fabs(theta12) < fabs(theta34)){
527  theta = theta34;
528  } else {
529  theta = theta12;
530  }
531 
532  if(0 < theta){
533  n_vector = ef;
534  } else {
535  n_vector = -ef;
536  }
537 
538  if(HIRUKAWA_DEBUG){
539  cout << "va1=" << va1[0] << " " << va1[1] << " " << va1[2] << endl;
540  cout << "va2=" << va2[0] << " " << va2[1] << " " << va2[2] << endl;
541  cout << "va3=" << v1[0] << " " << v1[1] << " " << v1[2] << endl;
542  cout << "vb1=" << vb1[0] << " " << vb1[1] << " " << vb1[2] << endl;
543  cout << "vb2=" << vb2[0] << " " << vb2[1] << " " << vb2[2] << endl;
544  cout << "vb3=" << v2[0] << " " << v2[1] << " " << v2[2] << endl;
545  cout << "n1=" << n1[0] << " " << n1[1] << " " << n1[2] << endl;
546  cout << "m1=" << m1[0] << " " << m1[1] << " " << m1[2] << endl;
547  cout << "ef=" << ef[0] << " " << ef[1] << " " << ef[2] << endl;
548  cout << "sv1=" << sv1[0] << " " << sv1[1] << " " << sv1[2] << endl;
549  cout << "sv2=" << sv2[0] << " " << sv2[1] << " " << sv2[2] << endl;
550  cout << endl;
551  }
552 
553  if(n_vector.dot(sv1) < eps_applicability || - eps_applicability < n_vector.dot(sv2)){
554  // when the separating plane separates the outsides of the triangles
555  return 0;
556  } else {
557  return 1;
558  }
559 }
560 
561 
562 //
563 // find the collision info when a vertex penetrates a face
564 //
566  const Vector3& p1,
567  const Vector3& p2,
568  const Vector3& p3,
569  double mp0,
570  double mp1,
571  double mp2,
572  const Vector3& q1,
573  const Vector3& q2,
574  const Vector3& q3,
575  const Vector3& f1,
576  const Vector3& f2,
577  const Vector3& f3,
578  const Vector3& n1,
579  const Vector3& m1,
580  Vector3& ip3,
581  Vector3& ip4,
582  Vector3& ip5, /* unused ? */
583  Vector3& ip6, /* unused ? */
584  collision_data* col_p, double pq)
585 {
586  Vector3 ip1;
587  find_intersection_pt(ip1, p1, p2, mp0, mp1);
588  Vector3 ip2;
589  find_intersection_pt(ip2, p3, p1, mp2, mp0);
590 
591  if(get_normal_vector_test(ip1, p2, ip2, p3, m1, n1)){
592 
593  Vector3 vec;
594  calcNormal(vec, p1, p2, p3, mp0);
595 
596  col_p->n_vector = m1 * pq;
597 
598  //
599  // The depth is estimated in InsertCollisionPair.cpp
600  // The following depth calculation is done only for debugging purpose
601  //
602  col_p->depth = calc_depth(ip1, ip2, p2, p3, p1, col_p->n_vector);
603  const Vector3 nv(-n1 * pq);
604  const double dp = calc_depth2(ip1, ip2, q1, q2, q3, nv);
605  if(dp < col_p->depth){
606  col_p->depth = dp;
607  }
608 
609  ip3 = ip1; ip4 = ip2;
610  col_p->num_of_i_points = 2;
611 
612  return 1;
613  }
614 
615  return 0;
616 }
617 
618 
619 //
620 // find the collision info when an edges penetrate a face each other
621 //
623  const Vector3& p1,
624  const Vector3& p2,
625  const Vector3& p3,
626  double mp0,
627  double mp1,
628  const Vector3& q1,
629  const Vector3& q2,
630  const Vector3& q3,
631  double nq0,
632  double nq1,
633  const Vector3& ef11,
634  const Vector3& n1,
635  const Vector3& m1,
636  Vector3& ip3,
637  Vector3& ip4,
638  collision_data *col_p,
639  bool test_normal=true)
640 {
641  Vector3 ip1;
642  find_intersection_pt(ip1, q1, q2, nq0, nq1);
643  Vector3 ip2;
644  find_intersection_pt(ip2, p1, p2, mp0, mp1);
645 
646  double dp;
647  if(!test_normal
648  || (get_normal_vector_test(col_p->n_vector, ef11, ip2, ip1, p3, q3, n1, m1, p1, p2, q1, q2) &&
649  find_common_perpendicular(p1, p2, q1, q2, ip1, ip2, n1, m1, col_p->n_vector, dp))){
650 
651  ip3 = ip1; ip4 = ip2;
652  col_p->num_of_i_points = 2;
653  col_p->depth = dp;
654  return 1;
655  }
656 
657  return 0;
658 }
659 
660 
661 // very robust triangle intersection test
662 // uses no divisions
663 // works on coplanar triangles
664 
666  const Vector3& P1,
667  const Vector3& P2,
668  const Vector3& P3,
669  const Vector3& Q1,
670  const Vector3& Q2,
671  const Vector3& Q3,
672  collision_data* col_p,
673  CollisionPairInserterBase* collisionPairInserter)
674 {
675  /*
676  One triangle is (p1,p2,p3). Other is (q1,q2,q3).
677  Edges are (e1,e2,e3) and (f1,f2,f3).
678  Normals are n1 and m1
679  Outwards are (g1,g2,g3) and (h1,h2,h3).
680 
681  We assume that the triangle vertices are in the same coordinate system.
682 
683  First thing we do is establish a new c.s. so that p1 is at (0,0,0).
684 
685  */
686  Vector3 p1, p2, p3;
687  Vector3 q1, q2, q3;
688  Vector3 e1, e2, e3;
689  Vector3 f1, f2, f3;
690  // Vector3 g1, g2, g3;
691  // Vector3 h1, h2, h3;
692  Vector3 n1, m1;
693  Vector3 z;
694  Vector3 nq, mp;
695 
696  int triP,triQ;
697  int edf1, edf2, edf3, ede1, ede2, ede3;
698 
699  Vector3 ef11, ef12, ef13;
700  Vector3 ef21, ef22, ef23;
701  Vector3 ef31, ef32, ef33;
702 
703  /* intersection point R is a flag which tri P & Q correspond or not */
704  Vector3 ip,ip3,ip4,ip5,ip6;
705  Vector3 i_pts_w[4];
706 
707  const int FV = 1; // face-vertex contact type
708  const int VF = 2; // vertex-face contact type
709  const int EE = 3; // edge-edge contact type
710 
711  z << 0.0,0.0,0.0;
712 
713  p1 = P1 - P1;
714  p2 = P2 - P1;
715  p3 = P3 - P1;
716 
717  q1 = Q1 - P1;
718  q2 = Q2 - P1;
719  q3 = Q3 - P1;
720 
721  e1 = p2 - p1;
722  e2 = p3 - p2;
723  e3 = p1 - p3;
724 
725  f1 = q2 - q1;
726  f2 = q3 - q2;
727  f3 = q1 - q3;
728 
729  n1 = e1.cross(e2);
730  m1 = f1.cross(f2);
731 
732  // now begin the series of tests
733 
734  /*************************************
735  separability test by face
736  ************************************/
737 
738  nq[0] = n1.dot(q1);
739  nq[1] = n1.dot(q2);
740  nq[2] = n1.dot(q3);
741  triQ = separability_test_by_face(nq);
742 
743  if(triQ == NOT_INTERSECT) return 0;
744 
745  double mq = m1.dot(q1);
746  mp[0] = m1.dot(p1) - mq;
747  mp[1] = m1.dot(p2) - mq;
748  mp[2] = m1.dot(p3) - mq;
749  triP = separability_test_by_face(mp);
750  if(triP == NOT_INTERSECT) return 0;
751 
752  ef11 = e1.cross(f1);
753  ef12 = e1.cross(f2);
754  ef13 = e1.cross(f3);
755  ef21 = e2.cross(f1);
756  ef22 = e2.cross(f2);
757  ef23 = e2.cross(f3);
758  ef31 = e3.cross(f1);
759  ef32 = e3.cross(f2);
760  ef33 = e3.cross(f3);
761 
762  edf1 = 0; edf2 = 0; edf3 = 0; ede1 = 0; ede2 = 0; ede3 = 0;
763 
764  /********************************
765  triangle inside test
766  *********************************/
767  switch(triQ)
768  {
769  case NOT_INTERSECT:
770  return 0;
771 
772  case EDGE1_NOT_INTERSECT:
773  edf2 = triangle_inside_test(ef12,ef22,ef32,p3,p1,p2,q2);
774  edf3 = triangle_inside_test(ef13,ef23,ef33,p3,p1,p2,q3);
775  break;
776 
777  case EDGE2_NOT_INTERSECT:
778  edf1 = triangle_inside_test(ef11,ef21,ef31,p3,p1,p2,q1);
779  edf3 = triangle_inside_test(ef13,ef23,ef33,p3,p1,p2,q3);
780  break;
781 
782  case EDGE3_NOT_INTERSECT:
783  edf1 = triangle_inside_test(ef11,ef21,ef31,p3,p1,p2,q1);
784  edf2 = triangle_inside_test(ef12,ef22,ef32,p3,p1,p2,q2);
785  break;
786  }
787 
788  int num_of_edges = edf1 + edf2 + edf3;
789  if(num_of_edges == 3){
790  //exit(1);
791  return 0;
792  }
793 
794  if(num_of_edges < 2){
795  switch(triP)
796  {
797  case EDGE1_NOT_INTERSECT:
798  ede2 = triangle_inside_test(ef21,ef22,ef23,q3,q1,q2,p2);
799  ede3 = triangle_inside_test(ef31,ef32,ef33,q3,q1,q2,p3);
800  if(ede2+ede3==2){
801  edf1= NOT_INTERSECT;
802  edf2= NOT_INTERSECT;
803  edf3= NOT_INTERSECT;
804  }
805  break;
806 
807  case EDGE2_NOT_INTERSECT:
808  ede1 = triangle_inside_test(ef11,ef12,ef13,q3,q1,q2,p1);
809  ede3 = triangle_inside_test(ef31,ef32,ef33,q3,q1,q2,p3);
810  if(ede1+ede3==2){
811  edf1= NOT_INTERSECT;
812  edf2= NOT_INTERSECT;
813  edf3= NOT_INTERSECT;
814  }
815  break;
816 
817  case EDGE3_NOT_INTERSECT:
818  ede1 = triangle_inside_test(ef11,ef12,ef13,q3,q1,q2,p1);
819  ede2 = triangle_inside_test(ef21,ef22,ef23,q3,q1,q2,p2);
820  if(ede1+ede2 == 2){
821  edf1= NOT_INTERSECT;
822  edf2= NOT_INTERSECT;
823  edf3= NOT_INTERSECT;
824  }
825  break;
826  }
827  if(num_of_edges == 0 && ede1+ede2+ede3 == 3){
828  //exit(1);
829  return 0;
830  }
831  }
832 
833  int num = edf1+edf2+edf3+ede1+ede2+ede3;
834  if(num == 0){
835  // cout << "no edge intersect" << endl;
836  return 0;
837  }
838  else if(num > 2){
839  printf("err of edge detection....");
840  //exit(1);
841  return 0;
842  }
843 
844  n1.normalize();
845  m1.normalize();
846 
847  /*********************************
848  find intersection points
849  **********************************/
850  if(num==1){
851  if(edf1==INTERSECT){
852  find_intersection_pt(ip,q1,q2,nq[0],nq[1]);
853  ip3 = ip;
854  col_p->n_vector = -n1;
855  col_p->depth = 0.0;
856  col_p->c_type = FV;
857  }
858  else if(edf2==INTERSECT){
859  find_intersection_pt(ip,q2,q3,nq[1],nq[2]);
860  ip3 = ip;
861  col_p->n_vector = -n1;
862  col_p->depth = 0.0;
863  col_p->c_type = FV;
864  }
865  else if(edf3==INTERSECT){
866  find_intersection_pt(ip,q3,q1,nq[2],nq[0]);
867  ip3 = ip;
868  col_p->n_vector = -n1;
869  col_p->depth = 0.0;
870  col_p->c_type = FV;
871  }
872  else if(ede1==INTERSECT){
873  find_intersection_pt(ip,p1,p2,mp[0],mp[1]);
874  ip3 = ip;
875  col_p->n_vector = m1;
876  col_p->depth = 0.0;
877  col_p->c_type = VF;
878  }
879  else if(ede2==INTERSECT){
880  find_intersection_pt(ip,p2,p3,mp[1],mp[2]);
881  ip3 = ip;
882  col_p->n_vector = m1;
883  col_p->depth = 0.0;
884  col_p->c_type = VF;
885  }
886  else if(ede3==INTERSECT){
887  find_intersection_pt(ip,p3,p1,mp[2],mp[0]);
888  ip3 = ip;
889  col_p->n_vector = m1;
890  col_p->depth = 0.0;
891  col_p->c_type = VF;
892  }
893  col_p->num_of_i_points = 1;
894  }
895  else if(num==2)
896  {
897  bool nvc = collisionPairInserter->normalVectorCorrection;
898  if(edf1==INTERSECT && edf2==INTERSECT){
899  if(HIRUKAWA_DEBUG) cout << "f1 f2" << endl;
900  col_p->c_type = FV;
901  if(!find_collision_info(q2,q1,q3,nq[1],nq[0],nq[2],p1,p2,p3,e1,e2,e3,
902  m1,n1,ip3,ip4,ip5,ip6,col_p,-1.0))
903  return 0;
904  }
905  else if(edf1==INTERSECT && edf3==INTERSECT){
906  if(HIRUKAWA_DEBUG) cout << "f1 f3" << endl;
907  col_p->c_type = FV;
908  if(!find_collision_info(q1,q2,q3,nq[0],nq[1],nq[2],p1,p2,p3,e1,e2,e3,
909  m1,n1,ip3,ip4,ip5,ip6,col_p,-1.0))
910  return 0;
911  }
912  else if(ede1==INTERSECT && edf1==INTERSECT){
913  if(HIRUKAWA_DEBUG) cout << "e1 f1" << endl;
914  col_p->c_type = EE;
915  if(!find_collision_info(p1,p2,p3,mp[0],mp[1],q1,q2,q3,nq[0],nq[1],ef11,
916  n1,m1,ip3,ip4,col_p,nvc))
917  return 0;
918  }
919  else if(ede2==INTERSECT && edf1==INTERSECT){
920  if(HIRUKAWA_DEBUG) cout << "e2 f1" << endl;
921  col_p->c_type = EE;
922  if(!find_collision_info(p2,p3,p1,mp[1],mp[2],q1,q2,q3,nq[0],nq[1],ef21,
923  n1,m1,ip3,ip4,col_p,nvc))
924  return 0;
925  }
926  else if(ede3==INTERSECT && edf1==INTERSECT){
927  if(HIRUKAWA_DEBUG) cout << "e3 f1" << endl;
928  col_p->c_type = EE;
929  if(!find_collision_info(p3,p1,p2,mp[2],mp[0],q1,q2,q3,nq[0],nq[1],ef31,
930  n1,m1,ip3,ip4,col_p,nvc))
931  return 0;
932  }
933  else if(edf2==INTERSECT && edf3==INTERSECT){
934  if(HIRUKAWA_DEBUG) cout << "f2 f3" << endl;
935  col_p->c_type = FV;
936  if(!find_collision_info(q3,q2,q1,nq[2],nq[1],nq[0],p1,p2,p3,e1,e2,e3,
937  m1,n1,ip3,ip4,ip5,ip6,col_p,-1.0))
938  return 0;
939  }
940  else if(ede1==INTERSECT && edf2==INTERSECT){
941  if(HIRUKAWA_DEBUG) cout << "e1 f2" << endl;
942  col_p->c_type = EE;
943  if(!find_collision_info(p1,p2,p3,mp[0],mp[1],q2,q3,q1,nq[1],nq[2],ef12,
944  n1,m1,ip3,ip4,col_p,nvc))
945  return 0;
946  }
947  else if(ede2==INTERSECT && edf2==INTERSECT){
948  if(HIRUKAWA_DEBUG) cout << "e2 f2" << endl;
949  col_p->c_type = EE;
950  if(!find_collision_info(p2,p3,p1,mp[1],mp[2],q2,q3,q1,nq[1],nq[2],ef22,
951  n1,m1,ip3,ip4,col_p,nvc))
952  return 0;
953  }
954  else if(ede3==INTERSECT && edf2==INTERSECT){
955  if(HIRUKAWA_DEBUG) cout << "e3 f2" << endl;
956  col_p->c_type = EE;
957  if(!find_collision_info(p3,p1,p2,mp[2],mp[0],q2,q3,q1,nq[1],nq[2],ef32,
958  n1,m1,ip3,ip4,col_p,nvc))
959  return 0;
960  }
961  else if(ede1==INTERSECT && edf3==INTERSECT){
962  if(HIRUKAWA_DEBUG) cout << "e1 f3" << endl;
963  col_p->c_type = EE;
964  if(!find_collision_info(p1,p2,p3,mp[0],mp[1],q3,q1,q2,nq[2],nq[0],ef13,
965  n1,m1,ip3,ip4,col_p,nvc))
966  return 0;
967  }
968  else if(ede2==INTERSECT && edf3==INTERSECT){
969  if(HIRUKAWA_DEBUG) cout << "e2 f3" << endl;
970  col_p->c_type = EE;
971  if(!find_collision_info(p2,p3,p1,mp[1],mp[2],q3,q1,q2,nq[2],nq[0],ef23,
972  n1,m1,ip3,ip4,col_p,nvc))
973  return 0;
974  }
975  else if(ede3==INTERSECT && edf3==INTERSECT){
976  if(HIRUKAWA_DEBUG) cout << "e3 f3" << endl;
977  col_p->c_type = EE;
978  if(!find_collision_info(p3,p1,p2,mp[2],mp[0],q3,q1,q2,nq[2],nq[0],ef33,
979  n1,m1,ip3,ip4,col_p,nvc))
980  return 0;
981  }
982  else if(ede1==INTERSECT && ede2==INTERSECT){
983  if(HIRUKAWA_DEBUG) cout << "e1 e2" << endl;
984  col_p->c_type = VF;
985  if(!find_collision_info(p2,p1,p3,mp[1],mp[0],mp[2],q1,q2,q3,f1,f2,f3,
986  n1,m1,ip3,ip4,ip5,ip6,col_p,1.0))
987  return 0;
988  }
989  else if(ede1==INTERSECT && ede3==INTERSECT){
990  if(HIRUKAWA_DEBUG) cout << "e1 e3" << endl;
991  col_p->c_type = VF;
992  if(!find_collision_info(p1,p2,p3,mp[0],mp[1],mp[2],q1,q2,q3,f1,f2,f3,
993  n1,m1,ip3,ip4,ip5,ip6,col_p,1.0))
994  return 0;
995  }
996  else if(ede2==INTERSECT && ede3==INTERSECT){
997  if(HIRUKAWA_DEBUG) cout << "e2 e3" << endl;
998  col_p->c_type = VF;
999  if(!find_collision_info(p3,p2,p1,mp[2],mp[1],mp[0],q1,q2,q3,f1,f2,f3,
1000  n1,m1,ip3,ip4,ip5,ip6,col_p,1.0))
1001  return 0;
1002  }
1003  }
1004 
1005  if(col_p->num_of_i_points == 1){
1006  col_p->i_points[0] = ip3 + P1;
1007  }
1008  else if(col_p->num_of_i_points == 2){
1009  col_p->i_points[0] = ip3 + P1;
1010  col_p->i_points[1] = ip4 + P1;
1011  }
1012  else if(col_p->num_of_i_points == 3){
1013  col_p->i_points[0] = ip3 + P1;
1014  col_p->i_points[1] = ip4 + P1;
1015  col_p->i_points[2] = ip5 + P1;
1016  }
1017  else if(col_p->num_of_i_points == 4){
1018  col_p->i_points[0] = ip3 + P1;
1019  col_p->i_points[1] = ip4 + P1;
1020  col_p->i_points[2] = ip5 + P1;
1021  col_p->i_points[3] = ip5 + P1;
1022  }
1023 
1024  col_p->n = n1;
1025  col_p->m = m1;
1026 
1027  if(HIRUKAWA_DEBUG){
1028 
1029  CollisionPairInserterBase& c = *collisionPairInserter;
1030 
1031  Vector3 p1w(c.CD_s2 * (c.CD_Rot2 * P1 + c.CD_Trans2));
1032  Vector3 p2w(c.CD_s2 * (c.CD_Rot2 * P2 + c.CD_Trans2));
1033  Vector3 p3w(c.CD_s2 * (c.CD_Rot2 * P3 + c.CD_Trans2));
1034  Vector3 q1w(c.CD_s2 * (c.CD_Rot2 * Q1 + c.CD_Trans2));
1035  Vector3 q2w(c.CD_s2 * (c.CD_Rot2 * Q2 + c.CD_Trans2));
1036  Vector3 q3w(c.CD_s2 * (c.CD_Rot2 * Q3 + c.CD_Trans2));
1037  cout << "P1 = " << p1w[0] << " " << p1w[1] << " " << p1w[2] << endl;
1038  cout << "P2 = " << p2w[0] << " " << p2w[1] << " " << p2w[2] << endl;
1039  cout << "P3 = " << p3w[0] << " " << p3w[1] << " " << p3w[2] << endl;
1040  cout << "Q1 = " << q1w[0] << " " << q1w[1] << " " << q1w[2] << endl;
1041  cout << "Q2 = " << q2w[0] << " " << q2w[1] << " " << q2w[2] << endl;
1042  cout << "Q3 = " << q3w[0] << " " << q3w[1] << " " << q3w[2] << endl;
1043 
1044  for(int i=0; i<col_p->num_of_i_points; i++){
1045  i_pts_w[i] = c.CD_s2 * ((c.CD_Rot2 * col_p->i_points[i]) + c.CD_Trans2);
1046  cout << i << "-th intersecting point = ";
1047  cout << i_pts_w[i][0] << " " << i_pts_w[i][1] << " " << i_pts_w[i][2] << endl;
1048  }
1049 
1050  cout << "n1 = " << n1[0] << " " << n1[1] << " " << n1[2] << endl;
1051  cout << "m1 = " << m1[0] << " " << m1[1] << " " << m1[2] << endl;
1052  cout << "mp[0] mp[1] mp[2] = " << mp[0] << " " << mp[1] << " " << mp[2] << endl;
1053  cout << "nq[0] nq[1] nq[2] = " << nq[0] << " " << nq[1] << " " << nq[2] << endl;
1054  cout << "n_vector = " << col_p->n_vector[0] << " " << col_p->n_vector[1]
1055  << " " << col_p->n_vector[2] << endl;
1056  cout << "depth = " << col_p->depth << endl << endl;;
1057 
1058  }
1059 
1060 #if TRACE1
1061  printf("intersect point...in tri_contact..\n");
1062  printf(" ip1x = %f ip1y = %f ip1z = %f\n ip2x = %f ip2y = %f ip2z = %f\n",
1063  col_p->i_points[0][0],col_p->i_points[0][1],col_p->i_points[0][2],
1064  col_p->i_points[1][0],col_p->i_points[1][1],col_p->i_points[1][2]);
1065 
1066  printf("normal vector....it tri_conctact..\n");
1067  printf("N[0] = %f,N[1] = %f,N[2] = %f\n",col_p->n_vector[0],col_p->n_vector[1],col_p->n_vector[2]);
1068 #endif
1069 
1070  return 1;
1071 }
bool normalVectorCorrection
flag to enable/disable normal vector correction
int c
Definition: autoplay.py:16
static int find_collision_info(const Vector3 &p1, const Vector3 &p2, const Vector3 &p3, double mp0, double mp1, double mp2, const Vector3 &q1, const Vector3 &q2, const Vector3 &q3, const Vector3 &f1, const Vector3 &f2, const Vector3 &f3, const Vector3 &n1, const Vector3 &m1, Vector3 &ip3, Vector3 &ip4, Vector3 &ip5, Vector3 &ip6, collision_data *col_p, double pq)
Definition: TriOverlap.cpp:565
static void find_intersection_pt(Vector3 &ipt, const Vector3 &x1, const Vector3 &x2, double mn1, double mn2)
Definition: TriOverlap.cpp:118
static void find_foot(const Vector3 &ip, const Vector3 &pt1, const Vector3 &pt2, Vector3 &f)
Definition: TriOverlap.cpp:201
Matrix33 CD_Rot2
rotation of the second mesh
png_uint_32 i
Definition: png.h:2735
long b
Definition: jpegint.h:371
double det(const dmatrix &_a)
Eigen::Vector3d Vector3
Definition: EigenTypes.h:11
static void calcNormal(Vector3 &vec, const Vector3 &v1, const Vector3 &v2, const Vector3 &v3, double sgn)
Definition: TriOverlap.cpp:293
nv
double CD_s2
scale of the second mesh
#define HRP_COLLISION_EXPORT
Definition: Opcode.h:27
string a
Vector3 CD_Trans2
translation of the second mesh
HRP_COLLISION_EXPORT int tri_tri_overlap(const Vector3 &P1, const Vector3 &P2, const Vector3 &P3, const Vector3 &Q1, const Vector3 &Q2, const Vector3 &Q3, collision_data *col_p, CollisionPairInserterBase *collisionPairInserter)
Definition: TriOverlap.cpp:665
double dot(const Vector3 &v1, const Vector3 &v2)
Definition: Tvmet2Eigen.h:19
double sgn(double x)
Definition: fMatrix3.cpp:246
static double calc_depth(const Vector3 &ip1, const Vector3 &ip2, const Vector3 &n)
Definition: TriOverlap.cpp:156
int num
Definition: png.h:1502
static int triangle_inside_test(const Vector3 &ef1, const Vector3 &ef2, const Vector3 &ef3, const Vector3 &P3, const Vector3 &P1, const Vector3 &P2, const Vector3 &Q)
Definition: TriOverlap.cpp:82
static double calc_depth2(const Vector3 &ip1, const Vector3 &ip2, const Vector3 &pt1, const Vector3 &pt2, const Vector3 &pt3, const Vector3 &n)
Definition: TriOverlap.cpp:266
#define eps
Definition: fEulerPara.cpp:17
static int get_normal_vector_test(const Vector3 &ip1, const Vector3 &v1, const Vector3 &ip2, const Vector3 &v2, const Vector3 &n1, const Vector3 &m1)
Definition: TriOverlap.cpp:362
static int separability_test_by_face(const Vector3 &nm)
Definition: TriOverlap.cpp:56
static int find_common_perpendicular(const Vector3 &p1, const Vector3 &p2, const Vector3 &q1, const Vector3 &q2, const Vector3 &ip1, const Vector3 &ip2, const Vector3 &n1, const Vector3 &m1, const Vector3 &n_vector, double &dp)
Definition: TriOverlap.cpp:310


openhrp3
Author(s): AIST, General Robotix Inc., Nakamura Lab of Dept. of Mechano Informatics at University of Tokyo
autogenerated on Sat May 8 2021 02:42:41