colinfo.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  * The University of Tokyo
8  */
14 #include "colinfo.h"
15 
16 //#define VERBOSE
17 
18 int ColModel::LineIntersection(PQP_CollideResult& cres, const fVec3& p1, const fVec3& p2)
19 {
20  static PQP_REAL T[3], RR[3][3];
21  static PQP_REAL P1[3], P2[3];
22  fVec3 pos(joint->abs_pos);
23  fMat33 att(joint->abs_att);
24  T[0] = pos(0);
25  T[1] = pos(1);
26  T[2] = pos(2);
27  RR[0][0] = att(0,0);
28  RR[0][1] = att(0,1);
29  RR[0][2] = att(0,2);
30  RR[1][0] = att(1,0);
31  RR[1][1] = att(1,1);
32  RR[1][2] = att(1,2);
33  RR[2][0] = att(2,0);
34  RR[2][1] = att(2,1);
35  RR[2][2] = att(2,2);
36  P1[0] = p1(0);
37  P1[1] = p1(1);
38  P1[2] = p1(2);
39  P2[0] = p2(0);
40  P2[1] = p2(1);
41  P2[2] = p2(2);
42 
43  return PQP_TriLineIntersect(&cres, RR, T, model, P1, P2);
44 }
45 
46 int ColPair::Collide(PQP_CollideResult& cres)
47 {
48  static PQP_REAL R1[3][3];
49  static PQP_REAL T1[3];
50  static PQP_REAL R2[3][3];
51  static PQP_REAL T2[3];
52  fVec3 p1(models[0]->joint->abs_pos);
53  fMat33 r1(models[0]->joint->abs_att);
54  fVec3 p2(models[1]->joint->abs_pos);
55  fMat33 r2(models[1]->joint->abs_att);
56  // memo: definition of R1 and R2 is row major in PQP
57  R1[0][0] = r1(0,0);
58  R1[0][1] = r1(0,1);
59  R1[0][2] = r1(0,2);
60  R1[1][0] = r1(1,0);
61  R1[1][1] = r1(1,1);
62  R1[1][2] = r1(1,2);
63  R1[2][0] = r1(2,0);
64  R1[2][1] = r1(2,1);
65  R1[2][2] = r1(2,2);
66  T1[0] = p1(0);
67  T1[1] = p1(1);
68  T1[2] = p1(2);
69 
70  R2[0][0] = r2(0,0);
71  R2[0][1] = r2(0,1);
72  R2[0][2] = r2(0,2);
73  R2[1][0] = r2(1,0);
74  R2[1][1] = r2(1,1);
75  R2[1][2] = r2(1,2);
76  R2[2][0] = r2(2,0);
77  R2[2][1] = r2(2,1);
78  R2[2][2] = r2(2,2);
79  T2[0] = p2(0);
80  T2[1] = p2(1);
81  T2[2] = p2(2);
82 
83  return PQP_Collide(&cres,
84  R1, T1, models[0]->model,
85  R2, T2, models[1]->model);
86 }
87 
88 int ColPair::Distance(PQP_DistanceResult& dres,
89  PQP_REAL rel_error, PQP_REAL abs_error,
90  int qsize)
91 {
92  static PQP_REAL R1[3][3];
93  static PQP_REAL T1[3];
94  static PQP_REAL R2[3][3];
95  static PQP_REAL T2[3];
96  fVec3 p1(models[0]->joint->abs_pos);
97  fMat33 r1(models[0]->joint->abs_att);
98  fVec3 p2(models[1]->joint->abs_pos);
99  fMat33 r2(models[1]->joint->abs_att);
100  // memo: definition of R1 and R2 is row major in PQP
101  R1[0][0] = r1(0,0);
102  R1[0][1] = r1(0,1);
103  R1[0][2] = r1(0,2);
104  R1[1][0] = r1(1,0);
105  R1[1][1] = r1(1,1);
106  R1[1][2] = r1(1,2);
107  R1[2][0] = r1(2,0);
108  R1[2][1] = r1(2,1);
109  R1[2][2] = r1(2,2);
110  T1[0] = p1(0);
111  T1[1] = p1(1);
112  T1[2] = p1(2);
113 
114  R2[0][0] = r2(0,0);
115  R2[0][1] = r2(0,1);
116  R2[0][2] = r2(0,2);
117  R2[1][0] = r2(1,0);
118  R2[1][1] = r2(1,1);
119  R2[1][2] = r2(1,2);
120  R2[2][0] = r2(2,0);
121  R2[2][1] = r2(2,1);
122  R2[2][2] = r2(2,2);
123  T2[0] = p2(0);
124  T2[1] = p2(1);
125  T2[2] = p2(2);
126 
127  return PQP_Distance(&dres,
128  R1, T1, models[0]->model,
129  R2, T2, models[1]->model,
130  rel_error, abs_error,
131  qsize);
132 }
133 
134 int ColPair::Tolerance(PQP_ToleranceResult& tres,
135  PQP_REAL tolerance)
136 {
137  static PQP_REAL R1[3][3];
138  static PQP_REAL T1[3];
139  static PQP_REAL R2[3][3];
140  static PQP_REAL T2[3];
141  fVec3 p1(models[0]->joint->abs_pos);
142  fMat33 r1(models[0]->joint->abs_att);
143  fVec3 p2(models[1]->joint->abs_pos);
144  fMat33 r2(models[1]->joint->abs_att);
145  // memo: definition of R1 and R2 is row major in PQP
146  R1[0][0] = r1(0,0);
147  R1[0][1] = r1(0,1);
148  R1[0][2] = r1(0,2);
149  R1[1][0] = r1(1,0);
150  R1[1][1] = r1(1,1);
151  R1[1][2] = r1(1,2);
152  R1[2][0] = r1(2,0);
153  R1[2][1] = r1(2,1);
154  R1[2][2] = r1(2,2);
155  T1[0] = p1(0);
156  T1[1] = p1(1);
157  T1[2] = p1(2);
158 
159  R2[0][0] = r2(0,0);
160  R2[0][1] = r2(0,1);
161  R2[0][2] = r2(0,2);
162  R2[1][0] = r2(1,0);
163  R2[1][1] = r2(1,1);
164  R2[1][2] = r2(1,2);
165  R2[2][0] = r2(2,0);
166  R2[2][1] = r2(2,1);
167  R2[2][2] = r2(2,2);
168  T2[0] = p2(0);
169  T2[1] = p2(1);
170  T2[2] = p2(2);
171 
172  return PQP_Tolerance(&tres,
173  R1, T1, models[0]->model,
174  R2, T2, models[1]->model,
175  tolerance);
176 }
177 
181 static void apply_transform(TransformNode* tnode, fVec3& pos, int scale_only = false)
182 {
183  static float fscale[3];
184  // scale
185  tnode->getScale(fscale);
186  pos(0) *= fscale[0];
187  pos(1) *= fscale[1];
188  pos(2) *= fscale[2];
189  // translation, rotation
190  if(!scale_only)
191  {
192  static fVec3 trans, tmp;
193  static fMat33 att;
194  static float fpos[3];
195  static float frot[4];
196  tnode->getTranslation(fpos);
197  tnode->getRotation(frot);
198  trans(0) = fpos[0];
199  trans(1) = fpos[1];
200  trans(2) = fpos[2];
201  att.rot2mat(fVec3(frot[0], frot[1], frot[2]), frot[3]);
202  tmp.mul(att, pos);
203  pos.add(trans, tmp);
204  }
205 }
206 
207 int ColModel::add_triangles(TransformNode* tnode)
208 {
209  if(!tnode) return 0;
210  // search all shape nodes
211  n_triangle = 0;
212  add_triangles(tnode, (Node*)tnode);
213  if(n_triangle > 0)
214  {
215 //#ifdef VERBOSE
216  cerr << "add_triangles(" << tnode->getName() << ")" << endl;
217 //#endif
218  model->BeginModel();
219  create_pqp_model();
220  model->EndModel();
221 //#ifdef VERBOSE
222  cerr << " total " << n_triangle << " triangles" << endl;
223 //#endif
224  }
225  return 0;
226 }
227 
228 int ColModel::add_triangles(TransformNode* tnode, Node* cur)
229 {
230  if(!cur) return -1;
231  add_triangles_sub(tnode, cur->getChildNodes());
232  return 0;
233 }
234 
235 int ColModel::add_triangles_sub(TransformNode* tnode, Node* cur)
236 {
237  if(!cur) return 0;
238 // if(cur->getName()) cerr << " " << cur->getName() << endl;
239  if(cur->isIndexedFaceSetNode())
240  add_triangles_face(tnode, (IndexedFaceSetNode*)cur);
241  else
242  {
243  add_triangles_sub(tnode, cur->next());
244  add_triangles_sub(tnode, cur->getChildNodes());
245  }
246  return 0;
247 }
248 
249 int ColModel::add_triangles_face(TransformNode* tnode, IndexedFaceSetNode* inode)
250 {
251  CoordinateNode* cnode = inode->getCoordinateNodes();
252  if(!cnode) return -1;
253  int n_my_vertex = cnode->getNPoints();
254  int n_index = inode->getNCoordIndexes();
255  if(n_my_vertex == 0 || n_index == 0) return 0;
256  int i;
257  int vertex_id_base = n_vertex;
258  n_vertex += n_my_vertex;
259  // reallocate memory for saving vertices and save current vertices
260 #if 1
261  fVec3* tmp_vertices = vertices;
262  vertices = new fVec3 [n_vertex];
263  if(tmp_vertices)
264  {
265  for(i=0; i<vertex_id_base; i++)
266  vertices[i].set(tmp_vertices[i]);
267  delete[] tmp_vertices;
268  }
269 #else
270  fVec3* tmp_vertices = 0;
271  if(vertices)
272  {
273  tmp_vertices = new fVec3 [vertex_id_base];
274  for(i=0; i<vertex_id_base; i++)
275  tmp_vertices[i].set(vertices[i]);
276  delete[] vertices;
277  }
278  vertices = new fVec3 [n_vertex];
279  for(i=0; i<vertex_id_base; i++)
280  vertices[i].set(tmp_vertices[i]);
281  if(tmp_vertices) delete[] tmp_vertices;
282 #endif
283  float fp[3];
284  for(i=0; i<n_my_vertex; i++)
285  {
286  cnode->getPoint(i, fp);
287  vertices[i+vertex_id_base](0) = fp[0];
288  vertices[i+vertex_id_base](1) = fp[1];
289  vertices[i+vertex_id_base](2) = fp[2];
290  apply_all_transforms(tnode, (Node*)cnode, vertices[i+vertex_id_base]);
291  }
292  // process polygons (all changed to ccw)
293  int ccw = inode->getCCW();
294  for(i=0; i<n_index; i++)
295  {
296  int c1, c2, c3;
297  c1 = inode->getCoordIndex(i);
298  i++;
299  while( (c2 = inode->getCoordIndex(i)) != -1 &&
300  (c3 = inode->getCoordIndex(i+1)) != -1 )
301  {
302  TriangleInfo* ti = 0;
303  if(ccw) ti = new TriangleInfo(c1+vertex_id_base, c2+vertex_id_base, c3+vertex_id_base);
304  else ti = new TriangleInfo(c1+vertex_id_base, c3+vertex_id_base, c2+vertex_id_base);
305  triangles.append(ti);
306  n_triangle++;
307  i++;
308  }
309  i += 1;
310  }
311  return 0;
312 }
313 
314 void ColModel::apply_all_transforms(TransformNode* tnode, Node* cur, fVec3& pos)
315 {
316  Node* node;
317  int scale_only = false;
318  for(node=cur; node; node=node->getParentNode())
319  {
320  if(node->isTransformNode())
321  {
322  // if node is the top transform node, apply scale only and break
323  if(node == tnode)
324  scale_only = true;
325  apply_transform((TransformNode*)node, pos, scale_only);
326  }
327  }
328 }
329 
330 int ColModel::create_pqp_model()
331 {
332  PQP_REAL p1[3], p2[3], p3[3];
333  PQP_REAL q1[3], q2[3], q3[3];
334  tListElement<TriangleInfo>* ti;
335  for(ti=triangles.first(); ti; ti=ti->next())
336  {
337  TriangleInfo* tri = ti->body();
338  tri->neighbor_vertex[0] = -1;
339  tri->neighbor_vertex[1] = -1;
340  tri->neighbor_vertex[2] = -1;
341  tri->neighbor_tri[0] = -1;
342  tri->neighbor_tri[1] = -1;
343  tri->neighbor_tri[2] = -1;
344  int c1 = tri->index[0];
345  int c2 = tri->index[1];
346  int c3 = tri->index[2];
347  tListElement<TriangleInfo>* a;
348  int vert;
349  for(a=triangles.first(); a; a=a->next())
350  {
351  if(a != ti)
352  {
353  if((vert = a->body()->have_edge(c1, c2)) >= 0)
354  {
355  tri->neighbor_vertex[0] = vert;
356  tri->neighbor_tri[0] = a->id();
357  }
358  else if((vert = a->body()->have_edge(c2, c3)) >= 0)
359  {
360  tri->neighbor_vertex[1] = vert;
361  tri->neighbor_tri[1] = a->id();
362  }
363  else if((vert = a->body()->have_edge(c3, c1)) >= 0)
364  {
365  tri->neighbor_vertex[2] = vert;
366  tri->neighbor_tri[2] = a->id();
367  }
368  }
369  if(tri->neighbor_vertex[0] >= 0 &&
370  tri->neighbor_vertex[1] >= 0 &&
371  tri->neighbor_vertex[2] >= 0)
372  break;
373  }
374  p1[0] = (PQP_REAL)vertices[c1](0);
375  p1[1] = (PQP_REAL)vertices[c1](1);
376  p1[2] = (PQP_REAL)vertices[c1](2);
377  p2[0] = (PQP_REAL)vertices[c2](0);
378  p2[1] = (PQP_REAL)vertices[c2](1);
379  p2[2] = (PQP_REAL)vertices[c2](2);
380  p3[0] = (PQP_REAL)vertices[c3](0);
381  p3[1] = (PQP_REAL)vertices[c3](1);
382  p3[2] = (PQP_REAL)vertices[c3](2);
383  if(tri->neighbor_vertex[0] >= 0)
384  {
385  q1[0] = (PQP_REAL)vertices[tri->neighbor_vertex[0]](0);
386  q1[1] = (PQP_REAL)vertices[tri->neighbor_vertex[0]](1);
387  q1[2] = (PQP_REAL)vertices[tri->neighbor_vertex[0]](2);
388  }
389  if(tri->neighbor_vertex[1] >= 0)
390  {
391  q2[0] = (PQP_REAL)vertices[tri->neighbor_vertex[1]](0);
392  q2[1] = (PQP_REAL)vertices[tri->neighbor_vertex[1]](1);
393  q2[2] = (PQP_REAL)vertices[tri->neighbor_vertex[1]](2);
394  }
395  if(tri->neighbor_vertex[2] >= 0)
396  {
397  q3[0] = (PQP_REAL)vertices[tri->neighbor_vertex[2]](0);
398  q3[1] = (PQP_REAL)vertices[tri->neighbor_vertex[2]](1);
399  q3[2] = (PQP_REAL)vertices[tri->neighbor_vertex[2]](2);
400  }
401  int vertex_id[3] = { c1, c2, c3 };
402  model->AddTri(p1, p2, p3, q1, q2, q3, vertex_id, tri->neighbor_tri, ti->id());
403 #ifdef VERBOSE
404  cerr << "triangle " << ti->id() << ": [" << c1 << ", " << c2 << ", " << c3 << "], neighbors = [" << tri->neighbor_tri[0] << ", " << tri->neighbor_tri[1] << ", " << tri->neighbor_tri[2] << "]" << endl;
405  cerr << vertices[c1] << endl;
406  cerr << vertices[c2] << endl;
407  cerr << vertices[c3] << endl;
408 #endif
409  }
410  return 0;
411 }
412 
416 void ColInfo::add_pair(ColPair* p)
417 {
418  if(n_pairs == n_allocated_pairs) allocate_pair(n_allocated_pairs + 10);
419  pairs[n_pairs] = p;
420  n_pairs++;
421 }
422 
423 void ColInfo::add_model(ColModel* m)
424 {
425  if(n_models == n_allocated_models) allocate_model(n_allocated_models + 10);
426  models[n_models] = m;
427  n_models++;
428  n_total_tri += m->n_triangle;
429 }
430 
431 int ColInfo::AddCharPairs(const char* char1, const char* char2, Chain* chain, SceneGraph* sg)
432 {
433  cerr << "AddCharPairs(" << char1 << ", " << char2 << ")" << endl;
434  add_char_pairs(chain->Root(), char1, char2, chain, sg);
435  return n_pairs;
436 }
437 
438 int ColInfo::add_char_pairs(Joint* cur, const char* char1, const char* char2, Chain* chain, SceneGraph* sg)
439 {
440  if(!cur) return 0;
441  char* charname;
442  if((charname = cur->CharName()) && !strcmp(charname, char1))
443  {
444  add_char_pairs(cur, chain->Root(), char2, sg);
445  }
446  add_char_pairs(cur->brother, char1, char2, chain, sg);
447  add_char_pairs(cur->child, char1, char2, chain, sg);
448  return 0;
449 }
450 
451 int ColInfo::add_char_pairs(Joint* j1, Joint* j2, const char* char2, SceneGraph* sg)
452 {
453  if(!j2) return 0;
454  char* charname;
455  if((charname = j2->CharName()) && !strcmp(charname, char2) && j1 != j2)
456  {
457  add_joint_pair(j1, j2, sg);
458  }
459  add_char_pairs(j1, j2->brother, char2, sg);
460  add_char_pairs(j1, j2->child, char2, sg);
461  return 0;
462 }
463 
464 int ColInfo::AddJointPair(const char* joint1, const char* joint2, Chain* chain, SceneGraph* sg)
465 {
466  Joint* j1, *j2;
467  j1 = chain->FindJoint(joint1);
468  j2 = chain->FindJoint(joint2);
469  if(!j1 || !j2) return 0;
470  if(j1 == j2) return 0;
471  add_joint_pair(j1, j2, sg);
472  return n_pairs;
473 }
474 
475 int ColInfo::add_joint_pair(Joint* j1, Joint* j2, SceneGraph* sg)
476 {
477  if(Pair(j1, j2)) return 0;
478  ColModel* m1, *m2;
479  m1 = Model(j1);
480  if(!m1)
481  {
482  m1 = new ColModel(j1, sg);
483  add_model(m1);
484  }
485  m2 = Model(j2);
486  if(!m2)
487  {
488  m2 = new ColModel(j2, sg);
489  add_model(m2);
490  }
491  if(m1->n_triangle == 0 || m2->n_triangle == 0) return -1;
492  if( (j1->parent == j2 && j1->n_dof == 0) || (j2->parent == j1 && j2->n_dof == 0) )
493  return 0;
494  cerr << "add joint pair: " << j1->name << ", " << j2->name << endl;
495  ColPair* p = new ColPair(m1, m2);
496  add_pair(p);
497  return 0;
498 }
499 
500 ColModel* ColInfo::AddModel(Joint* jref, SceneGraph* sg)
501 {
502  ColModel* ret = Model(jref);
503  if(!ret)
504  {
505  ret = new ColModel(jref, sg);
506  if(ret->n_triangle > 0)
507  add_model(ret);
508  else
509  {
510  delete ret;
511  ret = 0;
512  }
513  }
514  return ret;
515 }
PQP_CollideResult
Definition: PQP_Internal.h:92
apply_transform
static void apply_transform(TransformNode *tnode, fVec3 &pos, int scale_only=false)
Definition: colinfo.cpp:181
ColInfo::AddCharPairs
int AddCharPairs(const char *char1, const char *char2, Chain *chain, SceneGraph *sg)
Definition: colinfo.cpp:431
ColInfo::AddModel
ColModel * AddModel(Joint *jref, SceneGraph *sg)
Definition: colinfo.cpp:500
Joint::parent
Joint * parent
pointer to the parent joint
Definition: chain.h:683
i
png_uint_32 i
Definition: png.h:2732
ColInfo::allocate_model
void allocate_model(int n_new_alloc)
Definition: colinfo.h:122
fVec3::add
void add(const fVec3 &vec1, const fVec3 &vec2)
Definition: fMatrix3.cpp:888
fVec3
3-element vector class.
Definition: fMatrix3.h:206
ColInfo::AddJointPair
int AddJointPair(const char *joint1, const char *joint2, Chain *chain, SceneGraph *sg)
Definition: colinfo.cpp:464
Joint
The class for representing a joint.
Definition: chain.h:538
ColPair
Definition: colinfo.h:20
ColInfo::Pair
ColPair * Pair(int i)
Definition: colinfo.h:88
ColInfo::n_total_tri
int n_total_tri
Definition: colinfo.h:146
Chain
The class representing the whole mechanism. May contain multiple characters.
Definition: chain.h:144
ColInfo::add_pair
void add_pair(ColPair *p)
Definition: colinfo.cpp:416
ColInfo::allocate_pair
void allocate_pair(int n_new_alloc)
Definition: colinfo.h:112
colinfo.h
Joint::child
Joint * child
pointer to the child joint
Definition: chain.h:685
ColInfo::add_char_pairs
int add_char_pairs(Joint *cur, const char *char1, const char *char2, Chain *chain, SceneGraph *sg)
Definition: colinfo.cpp:438
Joint::n_dof
int n_dof
degrees of freedom (0/1/3/6)
Definition: chain.h:715
fMat33
3x3 matrix class.
Definition: fMatrix3.h:29
PQP_TriLineIntersect
int PQP_TriLineIntersect(PQP_CollideResult *result, PQP_REAL RR[3][3], PQP_REAL T[3], PQP_Model *o1, PQP_REAL P1[3], PQP_REAL P2[3], int flag=PQP_ALL_CONTACTS)
fMat33::rot2mat
void rot2mat(const fVec3 &, double)
Converts from/to equivalent rotation axis and angle.
Definition: fMatrix3.cpp:362
Chain::Root
Joint * Root()
Definition: chain.h:151
Joint::CharName
char * CharName() const
Returns the character name.
Definition: chain.h:648
ColInfo::Model
ColModel * Model(int i)
Definition: colinfo.h:91
trans
png_infop png_bytep * trans
Definition: png.h:2432
ColInfo::n_allocated_models
int n_allocated_models
Definition: colinfo.h:144
PQP_Collide
int PQP_Collide(PQP_CollideResult *result, PQP_REAL R1[3][3], PQP_REAL T1[3], PQP_Model *o1, PQP_REAL R2[3][3], PQP_REAL T2[3], PQP_Model *o2, int flag=PQP_ALL_CONTACTS)
ColInfo::models
ColModel ** models
Definition: colinfo.h:145
Chain::FindJoint
Joint * FindJoint(const char *jname, const char *charname=0)
Find a joint from name.
Definition: chain.cpp:391
ColInfo::n_pairs
int n_pairs
Definition: colinfo.h:139
ColInfo::n_allocated_pairs
int n_allocated_pairs
Definition: colinfo.h:140
ColInfo::ColModel
friend class ColModel
Definition: colinfo.h:41
ColInfo::add_model
void add_model(ColModel *m)
Definition: colinfo.cpp:423
Joint::name
char * name
joint name (including the character name)
Definition: chain.h:691
test.a
int a
Definition: test.py:1
ColInfo::n_models
int n_models
Definition: colinfo.h:143
Joint::brother
Joint * brother
pointer to the brother joint
Definition: chain.h:684
ColInfo::add_joint_pair
int add_joint_pair(Joint *j1, Joint *j2, SceneGraph *sg)
Definition: colinfo.cpp:475
fp
png_FILE_p fp
Definition: png.h:1948
ColInfo::ColPair
friend class ColPair
Definition: colinfo.h:40
ColInfo::pairs
ColPair ** pairs
Definition: colinfo.h:141
fVec3::mul
void mul(const fVec3 &vec, double d)
Definition: fMatrix3.cpp:916

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