CollisionPairInserter.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 #include "CollisionPairInserter.h"
13 #include "Opcode/Opcode.h"
14 #include <cstdio>
15 #include <iostream>
16 #include <vector>
17 
18 using namespace std;
19 using namespace Opcode;
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 namespace {
33  const bool COLLIDE_DEBUG = false;
34  // �R���p�C�����Ɂ@-DDEPTH_CHECK�Ƃ���΁Adepth�l�ɂ��ڐG�_�I�����L��ɂȂ�܂��B�@�@//
35 #ifdef DEPTH_CHECK
36  const double MAX_DEPTH = 0.1;
37 #endif
38  const int CD_OK = 0;
39  const int CD_ALL_CONTACTS = 1;
40  const int CD_FIRST_CONTACT = 2;
41  const int CD_ERR_COLLIDE_OUT_OF_MEMORY = 2;
42 
43  enum {
44  FV = 1,
45  VF,
46  EE
47  };
48 }
49 
50 
51 CollisionPairInserter::CollisionPairInserter()
52 {
53 
54 }
55 
56 
57 CollisionPairInserter::~CollisionPairInserter()
58 {
59 
60 }
61 
62 
63 void CollisionPairInserter::copy_tri(col_tri* t1, tri* t2)
64 {
65  t1->p1 = t2->p1;
66  t1->p2 = t2->p2;
67  t1->p3 = t2->p3;
68 }
69 
70 
71 void CollisionPairInserter::copy_tri(col_tri* t1, col_tri* t2)
72 {
73  t1->p1 = t2->p1;
74  t1->p2 = t2->p2;
75  t1->p3 = t2->p3;
76 
77  if(t2->n[0] && t2->n[1] && t2->n[2]){
78  t1->n = t2->n;
79  }
80 }
81 
82 
83 void CollisionPairInserter::calc_normal_vector(col_tri* t)
84 {
85  if(t->status == 0){
86  const Vector3 e1(t->p2 - t->p1);
87  const Vector3 e2(t->p3 - t->p2);
88  t->n = e1.cross(e2).normalized();
89  t->status = 1;
90  }
91 }
92 
93 
94 int CollisionPairInserter::is_convex_neighbor(col_tri* t1, col_tri* t2)
95 {
96  const double EPS = 1.0e-12; // a small number
97 
98  calc_normal_vector(t2);
99 
100  // printf("normal vector1 = %f %f %f\n", t1->n[0], t1->n[1], t1->n[2]);
101  // printf("normal vector2 = %f %f %f\n", t2->n[0], t2->n[1], t2->n[2]);
102 
103  const Vector3 vec1(t1->p1 - t2->p1);
104  const Vector3 vec2(t1->p2 - t2->p2);
105  const Vector3 vec3(t1->p3 - t2->p3);
106 
107  // printf("is_convex_neighbor = %f %f %f\n",innerProd(t1->n,vec1),innerProd(t1->n,vec2),innerProd(t1->n,vec3));
108 
109  if(t2->n.dot(vec1) < EPS && t2->n.dot(vec2) < EPS && t2->n.dot(vec3) < EPS){
110  return 1;
111  } else {
112  return 0;
113  }
114 }
115 
116 void CollisionPairInserter::triangleIndexToPoint(ColdetModelSharedDataSet* model, int id, col_tri& tri){
117  IceMaths::IndexedTriangle indextriangle = model->triangles[id];
118  IceMaths::Point point0 = model->vertices[indextriangle.mVRef[0]];
119  IceMaths::Point point1 = model->vertices[indextriangle.mVRef[1]];
120  IceMaths::Point point2 = model->vertices[indextriangle.mVRef[2]];
121  tri.p1[0] = point0.x; tri.p1[1] = point0.y; tri.p1[2] = point0.z;
122  tri.p2[0] = point1.x; tri.p2[1] = point1.y; tri.p2[2] = point1.z;
123  tri.p3[0] = point2.x; tri.p3[1] = point2.y; tri.p3[2] = point2.z;
124 }
125 
126 void CollisionPairInserter::get_triangles_in_convex_neighbor(ColdetModelSharedDataSet* model, int id, col_tri* tri_convex_neighbor, std::vector<int>& foundTriangles, int& count){
127  int k;
128  for(unsigned int i=0; i<foundTriangles.size(); i++)
129  if(foundTriangles[i] == id){
130  k = i;
131  break;
132  }
133 
134  for(int i=0; i<3; i++){
135  int nei = model->neighbor[id].triangles[i];
136  if(nei < 0)
137  continue;
138  unsigned int j=0;
139  for(; j<foundTriangles.size(); j++)
140  if(foundTriangles[j] == nei)
141  break;
142  if(j<foundTriangles.size())
143  continue;
144 
145  col_tri tri_nei;
146  triangleIndexToPoint(model, nei, tri_nei);
147 
148  if(is_convex_neighbor( &tri_nei, &tri_convex_neighbor[k])){
149  if(k!=0){
150  Vector3 p1 = tri_nei.p1 - tri_convex_neighbor[0].p1;
151  if(p1.dot(tri_convex_neighbor[0].n) > 0)
152  continue;
153  Vector3 p2 = tri_nei.p2 - tri_convex_neighbor[0].p1;
154  if(p2.dot(tri_convex_neighbor[0].n) > 0)
155  continue;
156  Vector3 p3 = tri_nei.p3 - tri_convex_neighbor[0].p1;
157  if(p3.dot(tri_convex_neighbor[0].n) > 0)
158  continue;
159  }
160  foundTriangles.push_back(nei);
161  tri_convex_neighbor[count].status = 0;
162  copy_tri(&tri_convex_neighbor[count++], &tri_nei);
163  }
164  }
165 }
166 
167 int CollisionPairInserter::get_triangles_in_convex_neighbor(ColdetModelSharedDataSet* model, int id, col_tri* tri_convex_neighbor, int min_num){
168 
169  std::vector<int> foundTriangles;
170  int count=0;
171  triangleIndexToPoint(model, id, tri_convex_neighbor[count++]);
172  tri_convex_neighbor[0].status = 0;
173  foundTriangles.push_back(id);
174 
175  int start = 0;
176  int end = 1;
177 
178  int j=0;
179  while(count < min_num && j<2){
180  for(int i=start; i< end; i++)
181  get_triangles_in_convex_neighbor(model, foundTriangles[i], tri_convex_neighbor, foundTriangles, count);
182  start = end;
183  end = count;
184  j++;
185  }
186 
187  return count;
188 }
189 
190 
191 void CollisionPairInserter::examine_normal_vector(int id1, int id2, int ctype)
192 {
193  check_separability(id1, id2, ctype);
194 }
195 
196 void CollisionPairInserter::check_separability(int id1, int id2, int ctype){
197  int contactIndex = cdContact.size() - 1;
198  Vector3 signed_distance;
199  Vector3 signed_distance1(99999999.0,99999999.0,99999999.0);
200  Vector3 signed_distance2(-99999999.0,-99999999.0,-99999999.0);
201 
202  ColdetModelSharedDataSet* model0 = ((CollisionPairInserterBase*)this)->models[0];
203  ColdetModelSharedDataSet* model1 = ((CollisionPairInserterBase*)this)->models[1];
204  find_signed_distance(signed_distance1, model0, id1, contactIndex, ctype, 1);
205  find_signed_distance(signed_distance2, model1, id2, contactIndex, ctype, 2);
206 
207  int max = (2 < ctype) ? ctype : 2;
208 
209  for(int i=0; i < max; ++i){
210  signed_distance[i] = signed_distance1[i] - signed_distance2[i];
211  if(COLLIDE_DEBUG) printf("signed distance %d = %f\n", i, signed_distance[i]);
212  }
213 
214  switch(ctype){
215 
216  case FV:
217  if(signed_distance[0] < signed_distance[1]){
218  cdContact[contactIndex].n_vector = cdContact[contactIndex].m;
219  cdContact[contactIndex].depth = fabs(signed_distance[1]);
220  if(COLLIDE_DEBUG) printf("normal replaced\n");
221  } else {
222  cdContact[contactIndex].depth = fabs(signed_distance[0]);
223  }
224  break;
225 
226  case VF:
227  if(signed_distance[0] < signed_distance[1]){
228  cdContact[contactIndex].n_vector = - cdContact[contactIndex].n;
229  cdContact[contactIndex].depth = fabs(signed_distance[1]);
230  if(COLLIDE_DEBUG) printf("normal replaced\n");
231  } else{
232  cdContact[contactIndex].depth = fabs(signed_distance[0]);
233  }
234  break;
235 
236  case EE:
237  cdContact[contactIndex].num_of_i_points = 1;
238  if(signed_distance[0] < signed_distance[1] && signed_distance[2] <= signed_distance[1]){
239  cdContact[contactIndex].n_vector = cdContact[contactIndex].m;
240  cdContact[contactIndex].depth = fabs(signed_distance[1]);
241  if(COLLIDE_DEBUG) printf("normal replaced\n");
242  } else if(signed_distance[0] < signed_distance[2] && signed_distance[1] < signed_distance[2]){
243  cdContact[contactIndex].n_vector = - cdContact[contactIndex].n;
244  cdContact[contactIndex].depth = fabs(signed_distance[2]);
245  if(COLLIDE_DEBUG) printf("normal replaced\n");
246  } else {
247  cdContact[contactIndex].depth = fabs(signed_distance[0]);
248  // cout << "depth in InsertCollisionPair.cpp = " << signed_distance[0] << endl;
249  }
250  cdContact[contactIndex].i_points[0] += cdContact[contactIndex].i_points[1];
251  cdContact[contactIndex].i_points[0] *= 0.5;
252  break;
253  }
254 
255  if(COLLIDE_DEBUG){
256  printf("final normal = %f %f %f\n", cdContact[contactIndex].n_vector[0],
257  cdContact[contactIndex].n_vector[1], cdContact[contactIndex].n_vector[2]);
258  }
259  if(COLLIDE_DEBUG){
260  for(int i=0; i < cdContact[contactIndex].num_of_i_points; ++i){
261  cout << "final depth = " << cdContact[contactIndex].depth << endl;
262  cout << "final i_point = " << cdContact[contactIndex].i_points[i][0] << " "
263  << cdContact[contactIndex].i_points[i][1] << " " << cdContact[contactIndex].i_points[i][2]
264  << endl;
265  }
266  }
267 
268  if(COLLIDE_DEBUG) cout << endl;
269 }
270 
271 void CollisionPairInserter::find_signed_distance(
272  Vector3& signed_distance, col_tri* trp, int nth, int ctype, int obj)
273 {
274  find_signed_distance(signed_distance, trp->p1, nth, ctype, obj);
275  find_signed_distance(signed_distance, trp->p2, nth, ctype, obj);
276  find_signed_distance(signed_distance, trp->p3, nth, ctype, obj);
277 }
278 
279 void CollisionPairInserter::find_signed_distance(
280  Vector3& signed_distance, const Vector3& vert, int nth, int ctype, int obj)
281 {
282  Vector3 vert_w;
283  if(obj==1){
284  vert_w = CD_s1 * (CD_Rot1 * vert + CD_Trans1);
285  } else {
286  vert_w = CD_s2 * (CD_Rot2 * vert + CD_Trans2);
287  }
288 
289  if(COLLIDE_DEBUG) printf("vertex = %f %f %f\n", vert_w[0], vert_w[1], vert_w[2]);
290 
291  // use the first intersecting point to find the distance
292  const Vector3 vec(vert_w - cdContact[nth].i_points[0]);
293  //vecNormalize(cdContact[nth].n_vector);
294  cdContact[nth].n_vector.normalize();
295 
296  double dis0 = cdContact[nth].n_vector.dot(vec);
297 
298 #if 0
299  switch(ctype){
300  case FV:
301  if(dot(cdContact[nth].n_vector, cdContact[nth].n) > 0.0) dis0 = - dis0;
302  break;
303  case VF:
304  if(dot(cdContact[nth].n_vector, cdContact[nth].m) < 0.0) dis0 = - dis0;
305  break;
306  case EE:
307  if(dot(cdContact[nth].n_vector, cdContact[nth].n) > 0.0 ||
308  dot(cdContact[nth].n_vector, cdContact[nth].m) < 0.0)
309  dis0 = - dis0;
310  }
311 #endif
312 
313  if(COLLIDE_DEBUG) printf("dis0 = %f\n", dis0);
314 
315  double dis1 = dis0;
316  double dis2 = dis0;
317 
318  switch(ctype){
319  case FV:
320  dis1 = cdContact[nth].m.dot(vec);
321  if(COLLIDE_DEBUG) printf("dis1 = %f\n", dis1);
322  break;
323  case VF:
324  dis1 = - cdContact[nth].n.dot(vec);
325  if(COLLIDE_DEBUG) printf("dis1 = %f\n", dis1);
326  break;
327  case EE:
328  dis1 = cdContact[nth].m.dot(vec);
329  dis2 = - cdContact[nth].n.dot(vec);
330  if(COLLIDE_DEBUG){
331  printf("dis1 = %f\n", dis1);
332  printf("dis2 = %f\n", dis2);
333  }
334  }
335 
336  if(COLLIDE_DEBUG) printf("obj = %d\n", obj);
337  if(obj == 1){
338  if(dis0 < signed_distance[0]) signed_distance[0] = dis0;
339  if(dis1 < signed_distance[1]) signed_distance[1] = dis1;
340  if(ctype==EE)
341  if(dis2 < signed_distance[2]) signed_distance[2] = dis2;
342  }
343  else{
344  if(signed_distance[0] < dis0) signed_distance[0] = dis0;
345  if(signed_distance[1] < dis1) signed_distance[1] = dis1;
346  if(ctype==EE)
347  if(signed_distance[2] < dis2) signed_distance[2] = dis2;
348  }
349 }
350 
351 void CollisionPairInserter::find_signed_distance(
352  Vector3& signed_distance,
354  int id,
355  int contactIndex,
356  int ctype,
357  int obj)
358 {
359  const int MIN_NUM_NEIGHBOR = 10;
360  col_tri* tri_convex_neighbor = new col_tri[22];
361  int num = get_triangles_in_convex_neighbor(model, id, tri_convex_neighbor, MIN_NUM_NEIGHBOR);
362 
363  for(int i=0; i<num; ++i){
364  find_signed_distance(signed_distance, &tri_convex_neighbor[i], contactIndex, ctype, obj);
365  }
366 
367  delete [] tri_convex_neighbor;
368 }
369 
370 int CollisionPairInserter::new_point_test(int k)
371 {
372  const double eps = 1.0e-12; // 1 micro meter to judge two contact points are identical
373 
374  int last = cdContact.size()-1;
375 
376  for(int i=0; i < last; ++i){
377  for(int j=0; j < cdContact[i].num_of_i_points; ++j){
378  Vector3 dv(cdContact[i].i_points[j] - cdContact[last].i_points[k]);
379  double d = cdContact[i].depth - cdContact[last].depth;
380  if(dv.dot(dv) < eps && d*d < eps) return 0;
381  }
382  }
383  return 1;
384 }
385 
386 
387 //
388 // obsolute signatures
389 //
390 int CollisionPairInserter::apply(
391  const Opcode::AABBCollisionNode* b1,
392  const Opcode::AABBCollisionNode* b2,
393  int id1, int id2,
394  int num_of_i_points,
395  Vector3 i_points[4],
396  Vector3& n_vector,
397  double depth,
398  Vector3& n1,
399  Vector3& m1,
400  int ctype,
401  Opcode::MeshInterface* mesh1,
402  Opcode::MeshInterface* mesh2)
403 {
404  cdContact.push_back(collision_data());
405  collision_data& contact = cdContact.back();
406  contact.id1 = id1;
407  contact.id2 = id2;
408  contact.depth = depth;
409  contact.num_of_i_points = num_of_i_points;
410 
411  if(COLLIDE_DEBUG) printf("num_of_i_points = %d\n", num_of_i_points);
412 
413  for(int i=0; i < num_of_i_points; ++i){
414  contact.i_points[i].noalias() = CD_s2 * ((CD_Rot2 * i_points[i]) + CD_Trans2);
415  }
416 
417  contact.n_vector.noalias() = CD_Rot2 * n_vector;
418  contact.n.noalias() = CD_Rot2 * n1;
419  contact.m.noalias() = CD_Rot2 * m1;
420  if (normalVectorCorrection){
421  examine_normal_vector(id1, id2, ctype);
422  }
423 
424 #ifdef DEPTH_CHECK
425  // analyze_neighborhood_of_i_point(b1, b2, cdContactsCount, ctype);
426  // remove the intersecting point if depth is deeper than MAX_DEPTH meter
427  if(fabs(contact.depth) < MAX_DEPTH){
428  for(int i=0; i < num_of_i_points; ++i){
429  contact.i_point_new[i] = new_point_test(i);
430  }
431  } else {
432  for(int i=0; i < num_of_i_points; ++i){
433  contact.i_point_new[i] = 0;
434  }
435  }
436 #else
437  for(int i=0; i < num_of_i_points; ++i){
438  contact.i_point_new[i] = 1;
439  }
440 #endif
441 
442  return CD_OK;
443 }
444 
445 
446 int CollisionPairInserter::detectTriTriOverlap(
447  const Vector3& P1,
448  const Vector3& P2,
449  const Vector3& P3,
450  const Vector3& Q1,
451  const Vector3& Q2,
452  const Vector3& Q3,
453  collision_data* col_p)
454 {
455  return tri_tri_overlap(P1, P2, P3, Q1, Q2, Q3, col_p, this);
456 }
hrp::collision_data::n
Vector3 n
Definition: CollisionData.h:33
i
png_uint_32 i
Definition: png.h:2732
hrp::collision_data::num_of_i_points
int num_of_i_points
Definition: CollisionData.h:26
Opcode.h
hrp::collision_data::depth
double depth
Definition: CollisionData.h:31
hrp::CollisionPairInserter::tri::p2
Vector3 p2
Definition: CollisionPairInserter.h:52
hrp::CollisionPairInserter::col_tri
Definition: CollisionPairInserter.h:55
hrp::CollisionPairInserter::col_tri::p2
Vector3 p2
Definition: CollisionPairInserter.h:59
hrp::CollisionPairInserter::col_tri::status
int status
Definition: CollisionPairInserter.h:58
hrp::ColdetModelSharedDataSet::vertices
vector< IceMaths::Point > vertices
Definition: ColdetModelSharedDataSet.h:70
hrp::collision_data::n_vector
Vector3 n_vector
Definition: CollisionData.h:30
autoplay.n
n
Definition: autoplay.py:12
start
png_size_t start
Definition: png.h:1493
hrp
Definition: ColdetModel.h:28
hrp::CollisionPairInserter::tri::p3
Vector3 p3
Definition: CollisionPairInserter.h:52
tri_tri_overlap
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
hrp::max
static int max(int a, int b)
Definition: MatrixSolvers.cpp:54
hrp::CollisionPairInserter::col_tri::n
Vector3 n
Definition: CollisionPairInserter.h:60
hrp::Vector3
Eigen::Vector3d Vector3
Definition: EigenTypes.h:11
EPS
#define EPS
hrp::collision_data::id1
int id1
Definition: CollisionData.h:23
hrp::CollisionPairInserter::tri
Definition: CollisionPairInserter.h:48
eps
#define eps
Definition: fEulerPara.cpp:17
hrp::ColdetModelSharedDataSet
Definition: ColdetModelSharedDataSet.h:29
hrp::CollisionPairInserter::col_tri::p1
Vector3 p1
Definition: CollisionPairInserter.h:59
hrp::collision_data
Definition: CollisionData.h:20
hrp::ColdetModelSharedDataSet::triangles
vector< IceMaths::IndexedTriangle > triangles
Definition: ColdetModelSharedDataSet.h:71
viewSimTest.obj
obj
Definition: viewSimTest.py:6
hrp::ColdetModelSharedDataSet::neighbor
NeighborTriangleSetArray neighbor
Definition: ColdetModelSharedDataSet.h:76
HRP_COLLISION_EXPORT
#define HRP_COLLISION_EXPORT
Definition: hrplib/hrpCollision/config.h:26
hrp::dot
double dot(const Vector3 &v1, const Vector3 &v2)
Definition: Tvmet2Eigen.h:19
CollisionPairInserter.h
ColdetModelSharedDataSet.h
hrp::CollisionPairInserter::tri::p1
Vector3 p1
Definition: CollisionPairInserter.h:52
hrp::collision_data::id2
int id2
Definition: CollisionData.h:24
hrp::collision_data::i_points
Vector3 i_points[4]
Definition: CollisionData.h:27
hrp::CollisionPairInserter::col_tri::p3
Vector3 p3
Definition: CollisionPairInserter.h:59
hrp::CollisionPairInserterBase
Definition: CollisionPairInserterBase.h:27
hrp::collision_data::m
Vector3 m
Definition: CollisionData.h:34
hrp::collision_data::i_point_new
int i_point_new[4]
Definition: CollisionData.h:28
num
int num
Definition: png.h:1499
Opcode
Definition: CollisionPairInserterBase.h:18


openhrp3
Author(s): AIST, General Robotix Inc., Nakamura Lab of Dept. of Mechano Informatics at University of Tokyo
autogenerated on Wed Sep 7 2022 02:51:02