ColdetModelPair.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  */
14 #include <iostream>
15 #include <math.h>
16 #include "ColdetModelPair.h"
17 #include "ColdetModelSharedDataSet.h"
18 #include "CollisionPairInserter.h"
19 #include "Opcode/Opcode.h"
20 #include "SSVTreeCollider.h"
21 
22 using namespace hrp;
23 
24 
25 ColdetModelPair::ColdetModelPair()
26 {
27  collisionPairInserter = new CollisionPairInserter;
28 }
29 
30 
31 ColdetModelPair::ColdetModelPair(ColdetModelPtr model0, ColdetModelPtr model1,
32  double tolerance)
33 {
34  collisionPairInserter = new CollisionPairInserter;
35  set(model0, model1);
36  tolerance_ = tolerance;
37 }
38 
39 
40 ColdetModelPair::ColdetModelPair(const ColdetModelPair& org)
41 {
42  collisionPairInserter = new CollisionPairInserter;
43  set(org.models[0], org.models[1]);
44  tolerance_ = org.tolerance_;
45 }
46 
47 
48 ColdetModelPair::~ColdetModelPair()
49 {
50  delete collisionPairInserter;
51 }
52 
53 
54 void ColdetModelPair::set(ColdetModelPtr model0, ColdetModelPtr model1)
55 {
56  models[0] = model0;
57  models[1] = model1;
58  // inverse order because of historical background
59  // this should be fixed.(note that the direction of normal is inversed when the order inversed
60  if(model0 && model1)
61  collisionPairInserter->set(model1->dataSet, model0->dataSet);
62 }
63 
64 
65 std::vector<collision_data>& ColdetModelPair::detectCollisionsSub(bool detectAllContacts)
66 {
67  collisionPairInserter->clear();
68 
69  int pt0 = models[0]->getPrimitiveType();
70  int pt1 = models[1]->getPrimitiveType();
71  bool detected;
72  bool detectPlaneSphereCollisions(bool detectAllContacts);
73 
74  if (( pt0 == ColdetModel::SP_PLANE && pt1 == ColdetModel::SP_CYLINDER)
75  || (pt1 == ColdetModel::SP_PLANE && pt0 == ColdetModel::SP_CYLINDER)){
76  detected = detectPlaneCylinderCollisions(detectAllContacts);
77  }
78  else if (pt0 == ColdetModel::SP_PLANE || pt1 == ColdetModel::SP_PLANE){
79  detected = detectPlaneMeshCollisions(detectAllContacts);
80  }
81  else if (pt0 == ColdetModel::SP_SPHERE && pt1 == ColdetModel::SP_SPHERE) {
82  detected = detectSphereSphereCollisions(detectAllContacts);
83  }
84 
85  else if (pt0 == ColdetModel::SP_SPHERE || pt1 == ColdetModel::SP_SPHERE) {
86  detected = detectSphereMeshCollisions(detectAllContacts);
87  }
88  else {
89  detected = detectMeshMeshCollisions(detectAllContacts);
90  }
91 
92  if(!detected){
93  collisionPairInserter->clear();
94  }
95 
96  return collisionPairInserter->collisions();
97 }
98 
99 
100 bool ColdetModelPair::detectPlaneMeshCollisions(bool detectAllContacts)
101 {
102  bool result = false;
103 
104  ColdetModelPtr plane, mesh;
105  bool reversed=false;
106  if (models[0]->getPrimitiveType() == ColdetModel::SP_PLANE){
107  plane = models[0];
108  mesh = models[1];
109  }
110  if (models[1]->getPrimitiveType() == ColdetModel::SP_PLANE){
111  plane = models[1];
112  mesh = models[0];
113  reversed = true;
114  }
115  if (!plane || !mesh || !mesh->dataSet->model.GetMeshInterface()) return false;
116 
117 
118  PlanesCollider PC;
119  if(!detectAllContacts) PC.SetFirstContact(true);
120  PC.setCollisionPairInserter(collisionPairInserter);
121  IceMaths::Matrix4x4 mTrans = *(mesh->transform);
122  for(udword i=0; i<3; i++){
123  for(udword j=0; j<3; j++){
124  collisionPairInserter->CD_Rot1(i,j) = mTrans[j][i];
125  }
126  collisionPairInserter->CD_Trans1[i] = mTrans[3][i];
127  }
128  collisionPairInserter->CD_s1 = 1.0;
129 
130  PlanesCache Cache;
131  IceMaths::Matrix4x4 pTrans = (*(plane->pTransform)) * (*(plane->transform));
132  IceMaths::Point p, nLocal(0,0,1), n;
133  IceMaths::TransformPoint3x3(n, nLocal, pTrans);
134  pTrans.GetTrans(p);
135  Plane Planes[] = {Plane(p, n)};
136  bool IsOk = PC.Collide(Cache, Planes, 1, mesh->dataSet->model,
137  mesh->transform);
138  if (!IsOk){
139  std::cerr << "PlanesCollider::Collide() failed" << std::endl;
140  }else{
141  result = PC.GetContactStatus();
142  }
143  if (reversed){
144  std::vector<collision_data>& cdata
145  = collisionPairInserter->collisions();
146  for (size_t i=0; i<cdata.size(); i++){
147  cdata[i].n_vector *= -1;
148  }
149  }
150 
151  return result;
152 }
153 
154 bool ColdetModelPair::detectMeshMeshCollisions(bool detectAllContacts)
155 {
156  bool result = false;
157 
158  if(models[0]->isValid() && models[1]->isValid()){
159 
160  Opcode::BVTCache colCache;
161 
162  // inverse order because of historical background
163  // this should be fixed.(note that the direction of normal is inversed when the order inversed
164  colCache.Model0 = &models[1]->dataSet->model;
165  colCache.Model1 = &models[0]->dataSet->model;
166 
167  if(colCache.Model0->HasSingleNode() || colCache.Model1->HasSingleNode())
168  return result;
169 
170  Opcode::AABBTreeCollider collider;
171  collider.setCollisionPairInserter(collisionPairInserter);
172 
173  if(!detectAllContacts){
174  collider.SetFirstContact(true);
175  }
176 
177  bool isOk = collider.Collide(colCache, models[1]->transform, models[0]->transform);
178 
179  if (!isOk)
180  std::cerr << "AABBTreeCollider::Collide() failed" << std::endl;
181 
182  result = collider.GetContactStatus();
183 
184  boxTestsCount = collider.GetNbBVBVTests();
185  triTestsCount = collider.GetNbPrimPrimTests();
186  }
187 
188  return result;
189 }
190 
191 bool ColdetModelPair::detectSphereSphereCollisions(bool detectAllContacts) {
192 
193  bool result = false;
194  int sign = 1;
195 
196  if (models[0]->isValid() && models[1]->isValid()) {
197 
198  ColdetModelPtr sphereA, sphereB;
199  sphereA = models[0];
200  sphereB = models[1];
201 
202  IceMaths::Matrix4x4 sATrans = (*(sphereA->pTransform)) * (*(sphereA->transform));
203  IceMaths::Matrix4x4 sBTrans = (*(sphereB->pTransform)) * (*(sphereB->transform));
204 
205  float radiusA, radiusB;
206  sphereA->getPrimitiveParam(0, radiusA);
207  sphereB->getPrimitiveParam(0, radiusB);
208 
209  IceMaths::Point centerA = sATrans.GetTrans();
210  IceMaths::Point centerB = sBTrans.GetTrans();
211 
212  IceMaths::Point D = centerB - centerA;
213 
214  float depth = radiusA + radiusB - D.Magnitude();
215 
216  if (D.Magnitude() <= (radiusA + radiusB)) {
217 
218  result = true;
219 
220  float x = (pow(D.Magnitude(), 2) + pow(radiusA, 2) - pow(radiusB, 2)) / (2 * D.Magnitude());
221 
222  IceMaths::Point n = D / D.Magnitude();
223 
224  IceMaths::Point q = centerA + n * x;
225 
226  std::vector<collision_data>& cdata = collisionPairInserter->collisions();
227  cdata.clear();
228 
229  collision_data col;
230  col.depth = depth;
231  col.num_of_i_points = 1;
232  col.i_point_new[0] = 1;
233  col.i_point_new[1] = 0;
234  col.i_point_new[2] = 0;
235  col.i_point_new[3] = 0;
236  col.n_vector[0] = sign * n.x;
237  col.n_vector[1] = sign * n.y;
238  col.n_vector[2] = sign * n.z;
239  col.i_points[0][0] = q.x;
240  col.i_points[0][1] = q.y;
241  col.i_points[0][2] = q.z;
242  cdata.push_back(col);
243  }
244  }
245 
246  return result;
247 }
248 
249 bool ColdetModelPair::detectSphereMeshCollisions(bool detectAllContacts) {
250 
251  bool result = false;
252  int sign = 1;
253 
254  if (models[0]->isValid() && models[1]->isValid()) {
255 
256  ColdetModelPtr sphere, mesh;
257 
258  if (models[0]->getPrimitiveType() == ColdetModel::SP_SPHERE) {
259  sphere = models[0];
260  mesh = models[1];
261  sign = -1;
262  }
263  else if (models[1]->getPrimitiveType() == ColdetModel::SP_SPHERE) {
264  sphere = models[1];
265  mesh = models[0];
266  }
267 
268  if (!sphere || !mesh)
269  return false;
270 
271  IceMaths::Matrix4x4 sTrans = (*(sphere->pTransform)) * (*(sphere->transform));
272 
273  float radius;
274  sphere->getPrimitiveParam(0, radius);
275 
276  IceMaths::Sphere sphere_def(IceMaths::Point(0, 0, 0), radius);
277 
278  Opcode::SphereCache colCache;
279 
280  Opcode::SphereCollider collider;
281 
282  if (!detectAllContacts) {
283  collider.SetFirstContact(true);
284  }
285 
286  bool isOk = collider.Collide(colCache, sphere_def, mesh->dataSet->model, &sTrans, mesh->transform);
287 
288  if (isOk) {
289 
290  if (collider.GetContactStatus()) {
291 
292  int TouchedPrimCount = collider.GetNbTouchedPrimitives();
293  const udword* TouchedPrim = collider.GetTouchedPrimitives();
294 
295  if (TouchedPrimCount) {
296 
297  result = true;
298 
299  std::vector< std::vector<IceMaths::Point> > triangle(TouchedPrimCount); // Triangle of each face in world's coordinates
300  std::vector<IceMaths::Plane> face(TouchedPrimCount); // Plane of each face in world's coordinates
301 
302  std::vector<float> depth(TouchedPrimCount);
303 
304  std::vector<IceMaths::Point> q(TouchedPrimCount);
305  std::vector<float> A(TouchedPrimCount);
306 
307  IceMaths::Matrix4x4 sTransInv;
308  IceMaths::InvertPRMatrix(sTransInv, sTrans);
309 
310  std::vector<collision_data>& cdata = collisionPairInserter->collisions();
311  cdata.clear();
312 
313  for (int i = 0; i < TouchedPrimCount; i++) {
314 
315  int vertex_index[3];
316  std::vector<IceMaths::Point> vertex(3);
317 
318  float x, y, z;
319  float R;
320 
321  mesh->getTriangle(TouchedPrim[i], vertex_index[0], vertex_index[1], vertex_index[2]);
322 
323  for (int j = 0; j < 3; j++) {
324  mesh->getVertex(vertex_index[j], x, y, z);
325  TransformPoint4x3(vertex[j], IceMaths::Point(x, y, z), *(mesh->transform));
326  }
327 
328  triangle[i] = std::vector<IceMaths::Point> (vertex);
329 
330  face[i] = IceMaths::Plane(vertex[0], vertex[1], vertex[2]);
331  face[i].Normalize();
332 
333  IceMaths::Plane face_s; // Plane of each face in sphere's coordinates
334  IceMaths::TransformPlane(face_s, face[i], sTransInv);
335  face_s.Normalize();
336 
337  if (abs(face_s.d) > radius)
338  cout << "No intersection";
339  else {
340 
341  R = sqrt(pow(radius, 2) - pow(face_s.d, 2));
342  depth[i] = radius - abs(face_s.d);
343 
344  IceMaths::Point U, V;
345 
346  TransformPoint3x3(U, vertex[1] - vertex[0], sTransInv);
347  U.Normalize();
348  V = face_s.n ^ U;
349  V.Normalize();
350 
351  IceMaths::Matrix4x4 scTrans;
352  scTrans.SetRow(0, U);
353  scTrans.SetRow(1, V);
354  scTrans.SetRow(2, face_s.n);
355  scTrans.SetRow(3, face_s.n * -face_s.d);
356 
357  IceMaths::Matrix4x4 scTransInv;
358  IceMaths::InvertPRMatrix(scTransInv, scTrans);
359 
360  IceMaths::Point vertex_c[3];
361  std::vector<float> vx, vy;
362 
363  for (int j = 0; j < 3; j++) {
364  TransformPoint4x3(vertex_c[j], vertex[j], sTransInv * scTransInv);
365  vx.push_back(vertex_c[j].x);
366  vy.push_back(vertex_c[j].y);
367  }
368 
369  float cx, cy;
370  calculateCentroidIntersection(cx, cy, A[i], R, vx, vy);
371 
372  TransformPoint4x3(q[i], IceMaths::Point (cx, cy, 0), scTrans * sTrans);
373  }
374  }
375 
376  std::vector<bool> considered_checklist(TouchedPrimCount, false);
377  std::vector<int> sameplane;
378 
379  std::vector<IceMaths::Point> new_q;
380  std::vector<IceMaths::Point> new_n;
381  std::vector<float> new_depth;
382 
383  // The following procedure is needed to merge components from the same plane (but different triangles)
384 
385  for (int i = 0; i < TouchedPrimCount; i++) {
386 
387  if (!considered_checklist[i]) {
388 
389  for (int j = i + 1; j < TouchedPrimCount; j++) {
390  IceMaths::Point normdiff(face[i].n - face[j].n);
391  if (normdiff.Magnitude() < LOCAL_EPSILON && (face[i].d - face[j].d) < LOCAL_EPSILON) {
392  if (!sameplane.size()) sameplane.push_back(i); // In order to consider it just once
393  sameplane.push_back(j);
394  }
395  }
396 
397  if (!sameplane.size()) {
398  new_q.push_back(q[i]);
399  new_n.push_back(face[i].n);
400  new_depth.push_back(depth[i]);
401  considered_checklist[i] = true;
402  }
403  else {
404 
405  float sum_xA, sum_yA, sum_zA, sum_A;
406  sum_xA = sum_yA = sum_zA = sum_A = 0;
407 
408  for (unsigned int k = 0; k < sameplane.size(); k++) {
409  sum_xA += q[sameplane[k]].x * A[sameplane[k]];
410  sum_yA += q[sameplane[k]].y * A[sameplane[k]];
411  sum_zA += q[sameplane[k]].z * A[sameplane[k]];
412  sum_A += A[sameplane[k]];
413  considered_checklist[sameplane[k]] = true;
414  }
415 
416  IceMaths::Point q_temp;
417  q_temp.x = sum_xA / sum_A;
418  q_temp.y = sum_yA / sum_A;
419  q_temp.z = sum_zA / sum_A;
420  new_q.push_back(q_temp);
421  new_n.push_back(face[i].n);
422  new_depth.push_back(depth[i]);
423 
424  sameplane.clear();
425  }
426  }
427  }
428 
429  for (unsigned int i = 0; i < new_q.size(); i++) {
430  collision_data col;
431  col.depth = new_depth[i];
432  col.num_of_i_points = 1;
433  col.i_point_new[0] = 1;
434  col.i_point_new[1] = 0;
435  col.i_point_new[2] = 0;
436  col.i_point_new[3] = 0;
437  col.n_vector[0] = sign * new_n[i].x;
438  col.n_vector[1] = sign * new_n[i].y;
439  col.n_vector[2] = sign * new_n[i].z;
440  col.i_points[0][0] = new_q[i].x;
441  col.i_points[0][1] = new_q[i].y;
442  col.i_points[0][2] = new_q[i].z;
443  cdata.push_back(col);
444  }
445  }
446  }
447  }
448 
449  else
450  std::cerr << "SphereCollider::Collide() failed" << std::endl;
451 
452  }
453 
454  return result;
455 }
456 
457 bool ColdetModelPair::detectPlaneCylinderCollisions(bool detectAllContacts) {
458 
459  ColdetModelPtr plane, cylinder;
460  bool reversed=false;
461  if (models[0]->getPrimitiveType() == ColdetModel::SP_PLANE){
462  plane = models[0];
463  }else if(models[0]->getPrimitiveType() == ColdetModel::SP_CYLINDER){
464  cylinder = models[0];
465  }
466  if (models[1]->getPrimitiveType() == ColdetModel::SP_PLANE){
467  plane = models[1];
468  reversed = true;
469  }else if(models[1]->getPrimitiveType() == ColdetModel::SP_CYLINDER){
470  cylinder = models[1];
471  }
472  if (!plane || !cylinder) return false;
473 
474  IceMaths::Matrix4x4 pTrans = (*(plane->pTransform)) * (*(plane->transform));
475  IceMaths::Matrix4x4 cTrans = (*(cylinder->pTransform)) * (*(cylinder->transform));
476 
477  float radius, height; // height and radius of cylinder
478  cylinder->getPrimitiveParam(0, radius);
479  cylinder->getPrimitiveParam(1, height);
480 
481  IceMaths::Point pTopLocal(0, height/2, 0), pBottomLocal(0, -height/2, 0);
482  IceMaths::Point pTop, pBottom; // center points of top and bottom discs
483  IceMaths::TransformPoint4x3(pTop, pTopLocal, cTrans);
484  IceMaths::TransformPoint4x3(pBottom, pBottomLocal, cTrans);
485 
486  IceMaths::Point pOnPlane, nLocal(0,0,1), n;
487  IceMaths::TransformPoint3x3(n, nLocal, pTrans);
488  pTrans.GetTrans(pOnPlane);
489  float d = pOnPlane|n; // distance between origin and plane
490 
491  float dTop = (pTop|n) - d;
492  float dBottom = (pBottom|n) - d;
493 
494  if (dTop > radius && dBottom > radius) return false;
495 
496  double theta = asin((dTop - dBottom)/height);
497  double rcosth = radius*cos(theta);
498 
499  int contactsCount = 0;
500  if (rcosth >= dTop) contactsCount+=2;
501  if (rcosth >= dBottom) contactsCount+=2;
502 
503  if (contactsCount){
504  std::vector<collision_data>& cdata = collisionPairInserter->collisions();
505  cdata.resize(contactsCount);
506  for (int i=0; i<contactsCount; i++){
507  cdata[i].num_of_i_points = 1;
508  cdata[i].i_point_new[0]=1;
509  cdata[i].i_point_new[1]=0;
510  cdata[i].i_point_new[2]=0;
511  cdata[i].i_point_new[3]=0;
512  if (reversed){
513  cdata[i].n_vector[0] = -n.x;
514  cdata[i].n_vector[1] = -n.y;
515  cdata[i].n_vector[2] = -n.z;
516  }else{
517  cdata[i].n_vector[0] = n.x;
518  cdata[i].n_vector[1] = n.y;
519  cdata[i].n_vector[2] = n.z;
520  }
521  }
522  IceMaths::Point vBottomTop = pTop - pBottom;
523  IceMaths::Point v = vBottomTop^n;
524  v.Normalize();
525  IceMaths::Point w = v^n;
526  w.Normalize();
527 
528  unsigned int index=0;
529  if (rcosth >= dBottom){ // bottom disc collides
530  double depth = rcosth - dBottom;
531  IceMaths::Point iPoint = pBottom - dBottom*n - dBottom*tan(theta)*w;
532  double x = dBottom/cos(theta);
533  IceMaths::Point dv = sqrt(radius*radius - x*x)*v;
534  cdata[index].i_points[0][0] = iPoint.x + dv.x;
535  cdata[index].i_points[0][1] = iPoint.y + dv.y;
536  cdata[index].i_points[0][2] = iPoint.z + dv.z;
537  cdata[index].depth = depth;
538  index++;
539  cdata[index].i_points[0][0] = iPoint.x - dv.x;
540  cdata[index].i_points[0][1] = iPoint.y - dv.y;
541  cdata[index].i_points[0][2] = iPoint.z - dv.z;
542  cdata[index].depth = depth;
543  index++;
544  }
545  if (rcosth >= dTop){ // top disc collides
546  double depth = rcosth - dTop;
547  IceMaths::Point iPoint = pTop - dTop*n - dTop*tan(theta)*w;
548  double x = dTop/cos(theta);
549  IceMaths::Point dv = sqrt(radius*radius - x*x)*v;
550  cdata[index].i_points[0][0] = iPoint.x + dv.x;
551  cdata[index].i_points[0][1] = iPoint.y + dv.y;
552  cdata[index].i_points[0][2] = iPoint.z + dv.z;
553  cdata[index].depth = depth;
554  index++;
555  cdata[index].i_points[0][0] = iPoint.x - dv.x;
556  cdata[index].i_points[0][1] = iPoint.y - dv.y;
557  cdata[index].i_points[0][2] = iPoint.z - dv.z;
558  cdata[index].depth = depth;
559  index++;
560  }
561 
562  return true;
563  }
564  return false;
565 }
566 
567 
568 double ColdetModelPair::computeDistance(double *point0, double *point1)
569 {
570  if(models[0]->isValid() && models[1]->isValid()){
571 
572  Opcode::BVTCache colCache;
573 
574  colCache.Model0 = &models[1]->dataSet->model;
575  colCache.Model1 = &models[0]->dataSet->model;
576 
577  SSVTreeCollider collider;
578 
579  float d;
580  Point p0, p1;
581  collider.Distance(colCache, d, p0, p1,
582  models[1]->transform, models[0]->transform);
583  point0[0] = p1.x;
584  point0[1] = p1.y;
585  point0[2] = p1.z;
586  point1[0] = p0.x;
587  point1[1] = p0.y;
588  point1[2] = p0.z;
589  return d;
590  }
591 
592  return -1;
593 }
594 
595 double ColdetModelPair::computeDistance(int& triangle0, double* point0, int& triangle1, double* point1)
596 {
597  if(models[0]->isValid() && models[1]->isValid()){
598 
599  Opcode::BVTCache colCache;
600 
601  colCache.Model0 = &models[1]->dataSet->model;
602  colCache.Model1 = &models[0]->dataSet->model;
603 
604  SSVTreeCollider collider;
605 
606  float d;
607  Point p0, p1;
608  collider.Distance(colCache, d, p0, p1,
609  models[1]->transform, models[0]->transform);
610  point0[0] = p1.x;
611  point0[1] = p1.y;
612  point0[2] = p1.z;
613  point1[0] = p0.x;
614  point1[1] = p0.y;
615  point1[2] = p0.z;
616  triangle1 = colCache.id0;
617  triangle0 = colCache.id1;
618  return d;
619  }
620 
621  return -1;
622 }
623 
624 
625 bool ColdetModelPair::detectIntersection()
626 {
627  if(models[0]->isValid() && models[1]->isValid()){
628 
629  Opcode::BVTCache colCache;
630 
631  colCache.Model0 = &models[1]->dataSet->model;
632  colCache.Model1 = &models[0]->dataSet->model;
633 
634  SSVTreeCollider collider;
635 
636  return collider.Collide(colCache, tolerance_,
637  models[1]->transform, models[0]->transform);
638  }
639 
640  return false;
641 }
642 
643 void ColdetModelPair::setCollisionPairInserter(CollisionPairInserterBase *inserter)
644 {
645  delete collisionPairInserter;
646  collisionPairInserter = inserter;
647  // inverse order because of historical background
648  // this should be fixed.(note that the direction of normal is inversed when the order inversed
649  collisionPairInserter->set(models[1]->dataSet, models[0]->dataSet);
650 }
651 
652 int ColdetModelPair::calculateCentroidIntersection(float &cx, float &cy, float &A, float radius, std::vector<float> vx, std::vector<float> vy) {
653 
654  unsigned int i; // Vertex and Side
655  int j[5]; // Point ID
656  unsigned int k; // Section
657 
658  int isOk = ColdetModelPair::makeCCW(vx, vy);
659 
660  if (isOk) {
661 
662  std::vector<pointStruct> point;
663  pointStruct p;
664  unsigned int numInter;
665  std::vector<float> x_int(2), y_int(2);
666 
667  for (i = 0; i < vx.size(); i++) {
668 
669  // Recording of the vertex
670 
671  p.x = vx[i];
672  p.y = vy[i];
673  p.angle = atan2(vy[i], vx[i]);
674  if (p.angle < 0) p.angle += TWOPI;
675  p.type = vertex;
676 
677  p.code = isInsideCircle(radius, p.x, p.y);
678  point.push_back(p);
679 
680  // Recording of the intersections
681 
682  numInter = calculateIntersection(x_int, y_int, radius, vx[i], vy[i], vx[(i + 1) % vx.size()], vy[(i + 1) % vx.size()]);
683 
684  for (k = 0; k < numInter; k++) {
685  p.x = x_int[k];
686  p.y = y_int[k];
687  p.angle = atan2(y_int[k], x_int[k]);
688  if (p.angle < 0) p.angle += TWOPI;
689  p.type = inter;
690  p.code = i + 1;
691  point.push_back(p);
692  }
693  }
694 
695  j[0] = 0;
696 
697  int start = -1;
698  bool finished = false;
699 
700  std::vector<figStruct> figure;
701  figStruct f;
702 
703  while (!finished) {
704 
705  for (int cont = 1; cont <= 4; cont++)
706  j[cont] = (j[0] + cont) % point.size();
707 
708  if (point[j[0]].code) {
709 
710  if (start == -1) start = j[0];
711 
712  if (point[j[1]].code) {
713 
714  f.p1 = j[0];
715  f.p2 = j[1];
716  f.type = tri;
717  figure.push_back(f);
718  j[0] = f.p2;
719  }
720 
721  else if (point[j[2]].code || point[j[3]].code || point[j[4]].code) {
722 
723  f.type = sector;
724  f.p1 = j[0];
725 
726  if (point[j[2]].code) f.p2 = j[2];
727  else if (point[j[3]].code) f.p2 = j[3];
728  else if (point[j[4]].code) f.p2 = j[4];
729 
730  figure.push_back(f);
731  j[0] = f.p2;
732  }
733 
734  else {
735  cout << "Error: No intersection detected" << endl;
736  return 0;
737  }
738  }
739 
740  else {
741  j[0] = j[1];
742  }
743 
744  if (((j[0] == 0) && (start == -1)) || (j[0] == start))
745  finished = true;
746  }
747 
748  if (figure.size()) {
749 
750  std::vector<float> x(3, 0);
751  std::vector<float> y(3, 0);
752  float sumx, sumy;
753  float th;
754 
755  for (k = 0; k < figure.size(); k++) {
756  if (figure[k].type == tri) {
757  x[1] = point[figure[k].p1].x;
758  y[1] = point[figure[k].p1].y;
759  x[2] = point[figure[k].p2].x;
760  y[2] = point[figure[k].p2].y;
761  figure[k].area = calculatePolygonArea(x, y);
762  sumx = sumy = 0;
763  for (int cont = 0; cont < 3; cont++) {
764  sumx += x[cont];
765  sumy += y[cont];
766  }
767  figure[k].cx = sumx / 3;
768  figure[k].cy = sumy / 3;
769  }
770  else if (figure[k].type == sector) {
771  th = point[figure[k].p2].angle - point[figure[k].p1].angle;
772  if (th < 0) th += TWOPI;
773  figure[k].area = pow(radius, 2) * th / 2;
774  calculateSectorCentroid(figure[k].cx, figure[k].cy, radius, point[figure[k].p1].angle, point[figure[k].p2].angle);
775  }
776  }
777 
778  float sum_xA, sum_yA, sum_A;
779  sum_xA = sum_yA = sum_A = 0;
780 
781  for (k = 0; k < figure.size(); k++) {
782  sum_xA += figure[k].cx * figure[k].area;
783  sum_yA += figure[k].cy * figure[k].area;
784  sum_A += figure[k].area;
785  }
786 
787  if ((figure.size() == 1) && (sum_A == 0)) {
788  cx = point[figure[0].p1].x;
789  cy = point[figure[0].p1].y;
790  }
791  else {
792  cx = sum_xA / sum_A;
793  cy = sum_yA / sum_A;
794  }
795 
796  A = sum_A;
797 
798  return 1;
799  }
800 
801  else {
802  if (isInsideTriangle(0, 0, vx, vy)) {
803  cx = cy = 0;
804  A = TWOPI * pow(radius, 2);
805  return 1;
806  }
807  else{
808  cx = cy = 0;
809  A = TWOPI * pow(radius, 2);
810  return 0;
811  }
812  }
813  }
814 
815  else
816  return 0;
817 }
818 
819 int ColdetModelPair::makeCCW(std::vector<float> &vx, std::vector<float> &vy) {
820 
821  if ((vx.size() == 3) && (vy.size() == 3)) {
822  if (ColdetModelPair::calculatePolygonArea(vx, vy) < 0) {
823  vx[0] = vx[1];
824  vy[0] = vy[1];
825  }
826  return 1;
827  }
828  else {
829  cout << "The number of vertices does not correspond to a triangle" << endl;
830  return 0;
831  }
832 }
833 
834 float ColdetModelPair::calculatePolygonArea(const std::vector<float> &vx, const std::vector<float> &vy) {
835 
836  float area = 0;
837 
838  if (vx.size() == vy.size()) {
839  for (unsigned int i = 0; i < vx.size(); i++) {
840  area += vx[i] * vy[(i + 1) % vx.size()] - vy[i] * vx[(i + 1) % vx.size()];
841  }
842  return area / 2;
843  }
844  else {
845  cout << "The number of coordinates does not match" << endl;
846  return 0;
847  }
848 }
849 
850 #if defined(WIN32) || defined(_WIN32) || defined(__WIN32__) || defined(__NT__)
851 #define trunc(x) ((int)(x))
852 #endif
853 void ColdetModelPair::calculateSectorCentroid(float &cx, float &cy, float radius, float th1, float th2) {
854 
855  float th, psi, phi, g;
856 
857  th = th2 - th1;
858  if (th2 < th1) th += TWOPI;
859 
860  g = (abs(th) > LOCAL_EPSILON) ? 4.0 / 3.0 * radius / th * sin(th / 2) : 2.0 / 3.0 * radius;
861 
862  psi = th1 + th2;
863  if (th2 < th1) psi += TWOPI;
864 
865  phi = psi / 2 - trunc(psi / 2 / TWOPI) * TWOPI;
866 
867  cx = g * cos(phi);
868  cy = g * sin(phi);
869 }
870 
871 bool ColdetModelPair::isInsideTriangle(float x, float y, const std::vector<float> &vx, const std::vector<float> &vy) {
872 
873  IceMaths::Point v1, v2;
874  double m1, m2;
875  double anglesum = 0;
876 
877  for (int i = 0; i < 3; i++) {
878 
879  v1 = IceMaths::Point(vx[i], vy[i], 0) - IceMaths::Point(x, y, 0);
880  v2 = IceMaths::Point(vx[(i + 1) % vx.size()], vy[(i + 1) % vy.size()], 0) - IceMaths::Point(x, y, 0);
881 
882  m1 = v1.Magnitude();
883  m2 = v2.Magnitude();
884 
885  if (m1 * m2 <= LOCAL_EPSILON) {
886  anglesum = TWOPI;
887  break;
888  }
889  else
890  anglesum += acos((v1 | v2) / (m1 * m2));
891  }
892 
893  return (abs(TWOPI - anglesum) < LOCAL_EPSILON);
894 }
895 
896 int ColdetModelPair::calculateIntersection(std::vector<float> &x, std::vector<float> &y, float radius, float x1, float y1, float x2, float y2) {
897 
898  int numint;
899 
900  float x_test, y_test;
901  x.clear();
902  y.clear();
903 
904  float xmin = min(x1, x2);
905  float xmax = max(x1, x2);
906  float ymin = min(y1, y2);
907  float ymax = max(y1, y2);
908 
909  float v_norm, proy_norm;
910  float x_temp, y_temp;
911 
912  std::vector<float> t;
913 
914  if ((sqrt(pow(x1, 2) + pow(y1, 2)) != radius) && (sqrt(pow(x2, 2) + pow(y2, 2)) != radius)) {
915 
916  float m, b;
917  float D;
918 
919  if (abs(x2 - x1) > LOCAL_EPSILON) {
920 
921  m = (y2 - y1) / (x2 - x1);
922  b = y1 - m * x1;
923 
924  D = 4 * pow(m, 2) * pow(b, 2) - 4 * (1 + pow(m, 2)) * (pow(b, 2) - pow(radius, 2));
925  }
926  else
927  D = pow(radius, 2) - pow(x1, 2);
928 
929  numint = D < 0 ? 0 : (D > 0 ? 2 : 1);
930 
931  if (numint > 0) {
932 
933  for (int i = 0; i < numint; i++) {
934 
935  if (abs(x2 - x1) > LOCAL_EPSILON) {
936  x_test = (-2 * m * b + pow(-1.0, i) * sqrt(D)) / (2 * (1 + pow(m, 2)));
937  y_test = m * x_test + b;
938  }
939  else {
940  x_test = x1;
941  y_test = pow(-1.0, i) * sqrt(D);
942  }
943 
944  cout.flush();
945 
946  if ((xmin <= x_test) && (x_test <= xmax) && (ymin <= y_test) && (y_test <= ymax)) {
947  x.push_back(x_test);
948  y.push_back(y_test);
949  v_norm = sqrt(pow(x2 - x1, 2) + pow(y2 - y1, 2));
950  proy_norm = sqrt(pow(x_test - x1, 2) + pow(y_test - y1, 2));
951  t.push_back(proy_norm / v_norm);
952  }
953  }
954 
955  if (t.size() > 1) {
956  if (t[0] > t[1]) {
957  x_temp = x[0];
958  y_temp = y[0];
959  x[0] = x[1];
960  y[0] = y[1];
961  x[1] = x_temp;
962  y[1] = y_temp;
963  }
964  }
965  }
966  }
967 
968  return t.size();
969 }
970 
971 ColdetModelPair& ColdetModelPair::operator=(const ColdetModelPair& org)
972 {
973  collisionPairInserter = new CollisionPairInserter;
974  set(org.models[0], org.models[1]);
975  tolerance_ = org.tolerance_;
976 }
hrp::CollisionPairInserterBase::CD_Trans1
Vector3 CD_Trans1
translation of the first mesh
Definition: CollisionPairInserterBase.h:106
Opcode::BaseModel::HasSingleNode
inline_ BOOL HasSingleNode() const
Definition: Opcode.h:140
hrp::CollisionPairInserterBase::collisions
std::vector< collision_data > & collisions()
get collision information
Definition: CollisionPairInserterBase.h:95
hrp::ColdetModelPair::detectSphereSphereCollisions
bool detectSphereSphereCollisions(bool detectAllContacts)
Definition: ColdetModelPair.cpp:191
hrp::CollisionPairInserterBase::CD_s1
double CD_s1
scale of the first mesh
Definition: CollisionPairInserterBase.h:107
type
png_infop png_charp png_int_32 png_int_32 int * type
Definition: png.h:2332
x
* x
Definition: IceUtils.h:98
hrp::collision_data
Definition: CollisionData.h:20
hrp::ColdetModelPair::tolerance
double tolerance() const
Definition: ColdetModelPair.h:69
hrp::ColdetModelPair::collisionPairInserter
CollisionPairInserterBase * collisionPairInserter
Definition: ColdetModelPair.h:102
hrp::min
static int min(int a, int b)
Definition: MatrixSolvers.cpp:55
hrp::collision_data::num_of_i_points
int num_of_i_points
Definition: CollisionData.h:26
Opcode::VolumeCollider::GetTouchedPrimitives
inline_ const udword * GetTouchedPrimitives() const
Definition: Opcode.h:60
LOCAL_EPSILON
#define LOCAL_EPSILON
Definition: ColdetModelPair.h:17
hrp::ColdetModelPair::makeCCW
int makeCCW(std::vector< float > &vx, std::vector< float > &vy)
Definition: ColdetModelPair.cpp:819
hrp::ColdetModelPair::setCollisionPairInserter
void setCollisionPairInserter(CollisionPairInserterBase *inserter)
Definition: ColdetModelPair.cpp:643
hrp::ColdetModelPair::detectCollisionsSub
std::vector< collision_data > & detectCollisionsSub(bool detectAllContacts)
Definition: ColdetModelPair.cpp:65
hrp::ColdetModelPair::calculatePolygonArea
float calculatePolygonArea(const std::vector< float > &vx, const std::vector< float > &vy)
Definition: ColdetModelPair.cpp:834
hrp::ColdetModelPair::detectPlaneMeshCollisions
bool detectPlaneMeshCollisions(bool detectAllContacts)
Definition: ColdetModelPair.cpp:100
IceMaths::Plane::Normalize
inline_ void Normalize()
Definition: OPC_IceHook.h:44
hrp::ColdetModelPair::isInsideTriangle
bool isInsideTriangle(float x, float y, const std::vector< float > &vx, const std::vector< float > &vy)
Definition: ColdetModelPair.cpp:871
hrp::CollisionPairInserterBase::set
void set(ColdetModelSharedDataSet *model0, ColdetModelSharedDataSet *model1)
Definition: CollisionPairInserterBase.h:99
IceMaths::Point::Magnitude
inline_ float Magnitude() const
Computes magnitude.
Definition: OPC_IceHook.h:220
w
w
i
png_uint_32 i
Definition: png.h:2735
Opcode::BVTCache::Model1
const Model * Model1
Model for second object.
Definition: Opcode.h:61
Opcode::SphereCollider::Collide
bool Collide(SphereCache &cache, const Sphere &sphere, const Model &model, const Matrix4x4 *worlds=null, const Matrix4x4 *worldm=null)
Definition: OPC_SphereCollider.cpp:94
b
long b
Definition: jpegint.h:371
Opcode::AABBTreeCollider::Collide
bool Collide(BVTCache &cache, const Matrix4x4 *world0=null, const Matrix4x4 *world1=null)
Definition: OPC_TreeCollider.cpp:97
TWOPI
#define TWOPI
2.0 * PI
Definition: IceTypes.h:34
hrp::ColdetModelPair::calculateIntersection
int calculateIntersection(std::vector< float > &x, std::vector< float > &y, float radius, float x1, float y1, float x2, float y2)
Definition: ColdetModelPair.cpp:896
hrp::ColdetModelPair::detectPlaneCylinderCollisions
bool detectPlaneCylinderCollisions(bool detectAllContacts)
Definition: ColdetModelPair.cpp:457
hrp::ColdetModelPair::isInsideCircle
bool isInsideCircle(float r, float x, float y)
Definition: ColdetModelPair.h:80
hrp::collision_data::n_vector
Vector3 n_vector
Definition: CollisionData.h:30
IceCore::Pair::id1
udword id1
Second index of the pair.
Definition: OPC_IceHook.h:23
hrp::collision_data::depth
double depth
Definition: CollisionData.h:31
Opcode::PlanesCollider::setCollisionPairInserter
void setCollisionPairInserter(hrp::CollisionPairInserterBase *collisionPairInserter)
Definition: Opcode.h:38
IceMaths::Matrix4x4::GetTrans
inline_ const HPoint & GetTrans() const
Returns the translation part of the matrix.
Definition: OPC_IceHook.h:96
Opcode::Collider::GetContactStatus
inline_ BOOL GetContactStatus() const
Definition: Opcode.h:53
Opcode::Collider::SetFirstContact
inline_ void SetFirstContact(bool flag)
Definition: Opcode.h:105
udword
unsigned int udword
sizeof(udword) must be 4
Definition: IceTypes.h:65
hrp::ColdetModelPair::computeDistance
double computeDistance(double *point0, double *point1)
Definition: ColdetModelPair.cpp:568
IceMaths::Plane::n
Point n
The normal to the plane.
Definition: OPC_IceHook.h:54
hrp::ColdetModelPair::models
ColdetModelPtr models[2]
Definition: ColdetModelPair.h:99
IceMaths::TransformPlane
inline_ void TransformPlane(Plane &transformed, const Plane &plane, const Matrix4x4 &transform)
Definition: OPC_IceHook.h:88
height
png_infop png_uint_32 png_uint_32 * height
Definition: png.h:2309
TransformPoint4x3
inline_ void TransformPoint4x3(Point &dest, const Point &source, const Matrix4x4 &rot)
Quickly rotates & translates a vector, using the 4x3 part of a 4x4 matrix.
Definition: IceMatrix4x4.h:437
IceMaths::TransformPoint4x3
inline_ void TransformPoint4x3(Point &dest, const Point &source, const Matrix4x4 &rot)
Quickly rotates & translates a vector, using the 4x3 part of a 4x4 matrix.
Definition: OPC_IceHook.h:438
hrp::CollisionPairInserterBase::clear
void clear()
clear collision information
Definition: CollisionPairInserterBase.h:35
j
def j(str, encoding="cp932")
index
list index
hrp::ColdetModelPair::calculateCentroidIntersection
int calculateCentroidIntersection(float &cx, float &cy, float &A, float radius, std::vector< float > vx, std::vector< float > vy)
Definition: ColdetModelPair.cpp:652
IceCore::Pair::id0
udword id0
First index of the pair.
Definition: OPC_IceHook.h:22
Opcode::AABBTreeCollider::setCollisionPairInserter
void setCollisionPairInserter(hrp::CollisionPairInserterBase *collisionPairInserter)
Definition: Opcode.h:77
t
t
hrp::ColdetModelPair::detectMeshMeshCollisions
bool detectMeshMeshCollisions(bool detectAllContacts)
Definition: ColdetModelPair.cpp:154
TransformPoint3x3
inline_ void TransformPoint3x3(Point &dest, const Point &source, const Matrix4x4 &rot)
Quickly rotates a vector, using the 3x3 part of a 4x4 matrix.
Definition: IceMatrix4x4.h:445
Opcode::AABBTreeCollider::GetNbBVBVTests
inline_ udword GetNbBVBVTests() const
Definition: Opcode.h:135
start
png_size_t start
Definition: png.h:1496
code
Definition: inftrees.h:24
Opcode::VolumeCollider::GetNbTouchedPrimitives
inline_ udword GetNbTouchedPrimitives() const
Definition: Opcode.h:50
IceMaths::InvertPRMatrix
ICEMATHS_API void InvertPRMatrix(Matrix4x4 &dest, const Matrix4x4 &src)
Definition: IceMatrix4x4.cpp:54
hrp::ColdetModelPair::calculateSectorCentroid
void calculateSectorCentroid(float &cx, float &cy, float radius, float th1, float th2)
Definition: ColdetModelPair.cpp:853
hrp::collision_data::i_point_new
int i_point_new[4]
Definition: CollisionData.h:28
Opcode::BVTCache::Model0
const Model * Model0
Model for first object.
Definition: Opcode.h:60
hrp::collision_data::i_points
Vector3 i_points[4]
Definition: CollisionData.h:27
SSVTreeCollider::Distance
bool Distance(BVTCache &cache, float &minD, Point &point0, Point &point1, const Matrix4x4 *world0=null, const Matrix4x4 *world1=null)
compute the minimum distance and the closest points
Definition: SSVTreeCollider.cpp:11
hrp::ColdetModelPair::set
void set(ColdetModelPtr model0, ColdetModelPtr model1)
Definition: ColdetModelPair.cpp:54
hrp::CollisionPairInserterBase::CD_Rot1
Matrix33 CD_Rot1
rotation of the first mesh
Definition: CollisionPairInserterBase.h:105
SSVTreeCollider
collision detector based on SSV(Sphere Swept Volume)
Definition: SSVTreeCollider.h:12
IceMaths::Point::Normalize
inline_ Point & Normalize()
Normalizes the vector.
Definition: OPC_IceHook.h:270
Opcode::PlanesCollider::Collide
bool Collide(PlanesCache &cache, const Plane *planes, udword nb_planes, const Model &model, const Matrix4x4 *worldm=null)
Definition: OPC_PlanesCollider.cpp:104
SSVTreeCollider::Collide
bool Collide(BVTCache &cache, double tolerance, const Matrix4x4 *world0=null, const Matrix4x4 *world1=null)
detect collision between links.
Definition: SSVTreeCollider.cpp:72
hrp::ColdetModelPtr
boost::intrusive_ptr< ColdetModel > ColdetModelPtr
Definition: ColdetModel.h:268
y
* y
Definition: IceUtils.h:97
IceMaths::Matrix4x4::SetRow
inline_ void SetRow(const udword r, const HPoint &p)
Sets a row.
Definition: OPC_IceHook.h:82
hrp::ColdetModelPair::transform
IceMaths::Matrix4x4 * transform(int index)
Definition: ColdetModelPair.h:41
IceMaths::TransformPoint3x3
inline_ void TransformPoint3x3(Point &dest, const Point &source, const Matrix4x4 &rot)
Quickly rotates a vector, using the 3x3 part of a 4x4 matrix.
Definition: OPC_IceHook.h:446
hrp::ColdetModelPair::detectSphereMeshCollisions
bool detectSphereMeshCollisions(bool detectAllContacts)
Definition: ColdetModelPair.cpp:249
Opcode::AABBTreeCollider::GetNbPrimPrimTests
inline_ udword GetNbPrimPrimTests() const
Definition: Opcode.h:145
hrp::ColdetModelPair::operator=
ColdetModelPair & operator=(const ColdetModelPair &cmp)
Definition: ColdetModelPair.cpp:971
hrp::max
static int max(int a, int b)
Definition: MatrixSolvers.cpp:54
IceMaths::Plane::d
float d
The distance from the origin.
Definition: OPC_IceHook.h:55

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:37