CollisionDetector_impl.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 
17 #include "CollisionDetector_impl.h"
19 #include <iostream>
20 #include <string>
21 
22 
23 using namespace std;
24 using namespace hrp;
25 
26 
28  : orb(CORBA_ORB::_duplicate(orb))
29 {
30 
31 }
32 
33 
35 {
36 
37 }
38 
39 
41 {
42  PortableServer::POA_var poa = _default_POA();
43  PortableServer::ObjectId_var id = poa->servant_to_id(this);
44  poa->deactivate_object(id);
45 }
46 
47 
48 void CollisionDetector_impl::registerCharacter(const char* name, BodyInfo_ptr bodyInfo)
49 {
50  cout << "adding " << name << " ";
51  ColdetBodyPtr coldetBody;
52 
53  string bodyInfoId(orb->object_to_string(bodyInfo));
54 
55  // test
56  cout << "BodyInfo CORBA ID of " << name << " is " << bodyInfoId << endl;
57 
58  //StringToColdetBodyMap::iterator it = bodyInfoToColdetBodyMap.find(bodyInfoId);
59  //if(it != bodyInfoToColdetBodyMap.end()){
60  // coldetBody = new ColdetBody(*it->second);
61  //} else {
62  coldetBody = new ColdetBody(bodyInfo);
63  coldetBody->setName(name);
64  // bodyInfoToColdetBodyMap.insert(it, make_pair(bodyInfoId, coldetBody));
65  //}
66 
67  StringToColdetBodyMap::iterator it = nameToColdetBodyMap.find(name);
68  if(it != nameToColdetBodyMap.end()){
69  cout << "\n";
70  cout << "The model of the name " << name;
71  cout << " has already been registered. It is replaced." << endl;
72  nameToColdetBodyMap[name] = coldetBody;
73  } else {
74  nameToColdetBodyMap.insert(it, make_pair(name, coldetBody));
75  cout << " is ok !" << endl;
76  }
77 }
78 
79 
81 (const LinkPair& linkPair)
82 {
84 }
85 
86 
88 (const LinkPair& linkPair, vector<ColdetModelPairExPtr>& io_coldetPairs)
89 {
90  const char* bodyName[2];
91  bodyName[0] = linkPair.charName1;
92  bodyName[1] = linkPair.charName2;
93 
94  const char* linkName[2];
95  linkName[0] = linkPair.linkName1;
96  linkName[1] = linkPair.linkName2;
97 
98  bool notFound = false;
99  ColdetBodyPtr coldetBody[2];
100  ColdetModelPtr coldetModel[2];
101 
102  for(int i=0; i < 2; ++i){
103  StringToColdetBodyMap::iterator it = nameToColdetBodyMap.find(bodyName[i]);
104  if(it == nameToColdetBodyMap.end()){
105  cout << "CollisionDetector::addCollisionPair : Body ";
106  cout << bodyName[i] << " is not found." << endl;
107  notFound = true;
108  } else {
109  coldetBody[i] = it->second;
110  coldetModel[i] = coldetBody[i]->linkColdetModel(linkName[i]);
111  if(!coldetModel[i]){
112  cout << "CollisionDetector::addCollisionPair : Link ";
113  cout << linkName[i] << " is not found." << endl;
114  notFound = true;
115  }
116  }
117  }
118 
119  if(!notFound){
120  io_coldetPairs.push_back(
121  new ColdetModelPairEx(coldetBody[0], coldetModel[0], coldetBody[1], coldetModel[1], linkPair.tolerance));
122  }
123 }
124 
125 
127 (const CharacterPositionSequence& characterPositions, CollisionSequence_out out_collisions)
128 {
129  updateAllLinkPositions(characterPositions);
130  return detectAllCollisions(coldetModelPairs, out_collisions);
131 }
132 
133 
135 (const LinkPairSequence& checkPairs,
136  const CharacterPositionSequence& characterPositions,
137  CollisionSequence_out out_collisions)
138 {
139  updateAllLinkPositions(characterPositions);
140 
141  vector<ColdetModelPairExPtr> tmpColdetPairs;
142 
143  for(unsigned int i=0; i < checkPairs.length(); ++i){
144  const LinkPair& linkPair = checkPairs[i];
145  addCollisionPairSub(linkPair, tmpColdetPairs);
146  }
147 
148  return detectAllCollisions(tmpColdetPairs, out_collisions);
149 }
150 
151 
153 (const CharacterPositionSequence& characterPositions)
154 {
155  for(unsigned int i=0; i < characterPositions.length(); i++){
156  const CharacterPosition& characterPosition = characterPositions[i];
157  const string bodyName(characterPosition.characterName);
158  StringToColdetBodyMap::iterator it = nameToColdetBodyMap.find(bodyName);
159  if(it != nameToColdetBodyMap.end()){
160  ColdetBodyPtr& coldetBody = it->second;
161  coldetBody->setLinkPositions(characterPosition.linkPositions);
162  }
163  }
164 }
165 
166 
168 (vector<ColdetModelPairExPtr>& coldetPairs, CollisionSequence_out& out_collisions)
169 {
170  bool detected = false;
171  const int numColdetPairs = coldetPairs.size();
172  out_collisions = new CollisionSequence;
173  out_collisions->length(numColdetPairs);
174 
175  for(int i=0; i < numColdetPairs; ++i){
176 
177  ColdetModelPairEx& coldetPair = *coldetPairs[i];
178  Collision& collision = out_collisions[i];
179 
180  if(detectCollisionsOfLinkPair(coldetPair, collision.points, true)){
181  detected = true;
182  }
183 
184  collision.pair.charName1 = CORBA::string_dup(coldetPair.body0->name());
185  collision.pair.linkName1 = CORBA::string_dup(coldetPair.model(0)->name().c_str());
186  collision.pair.charName2 = CORBA::string_dup(coldetPair.body1->name());
187  collision.pair.linkName2 = CORBA::string_dup(coldetPair.model(1)->name().c_str());
188  }
189 
190  return detected;
191 }
192 
193 
195 (ColdetModelPairEx& coldetPair, CollisionPointSequence& out_collisionPoints, const bool addCollisionPoints)
196 {
197  bool detected = false;
198 
199  vector<collision_data>& cdata = coldetPair.detectCollisions();
200 
201  int npoints = 0;
202  for(unsigned int i=0; i < cdata.size(); i++) {
203  for(int j=0; j < cdata[i].num_of_i_points; j++){
204  if(cdata[i].i_point_new[j]){
205  npoints ++;
206  }
207  }
208  }
209  if(npoints > 0){
210  detected = true;
211  if(addCollisionPoints){
212  out_collisionPoints.length(npoints);
213  int index = 0;
214  for(unsigned int i=0; i < cdata.size(); i++) {
215  collision_data& cd = cdata[i];
216  for(int j=0; j < cd.num_of_i_points; j++){
217  if (cd.i_point_new[j]){
218  CollisionPoint& point = out_collisionPoints[index];
219  for(int k=0; k < 3; k++){
220  point.position[k] = cd.i_points[j][k];
221  }
222  for(int k=0; k < 3; k++){
223  point.normal[k] = cd.n_vector[k];
224  }
225  point.idepth = cd.depth;
226  index++;
227  }
228  }
229  }
230  }
231  }
232 
233  return detected;
234 }
235 
236 
238 (
239  CORBA::Boolean checkAll,
240  const CharacterPositionSequence& characterPositions,
241  LinkPairSequence_out out_collidedPairs
242  )
243 {
244  updateAllLinkPositions(characterPositions);
245  return detectIntersectingLinkPairs(coldetModelPairs, out_collidedPairs, checkAll);
246 }
247 
248 
250 (
251  CORBA::Boolean checkAll,
252  const LinkPairSequence& checkPairs,
253  const CharacterPositionSequence& characterPositions,
254  LinkPairSequence_out out_collidedPairs
255  )
256 {
257  updateAllLinkPositions(characterPositions);
258 
259  vector<ColdetModelPairExPtr> tmpColdetPairs;
260 
261  for(unsigned int i=0; i < checkPairs.length(); ++i){
262  const LinkPair& linkPair = checkPairs[i];
263  addCollisionPairSub(linkPair, tmpColdetPairs);
264  }
265 
266  return detectIntersectingLinkPairs(tmpColdetPairs, out_collidedPairs, checkAll);
267 }
268 
269 
271 (
272  const CharacterPositionSequence& characterPositions,
273  DistanceSequence_out out_distances
274  )
275 {
276  updateAllLinkPositions(characterPositions);
277  computeDistances(coldetModelPairs, out_distances);
278 }
279 
280 
282 (
283  const LinkPairSequence& checkPairs,
284  const CharacterPositionSequence& characterPositions,
285  DistanceSequence_out out_distances
286  )
287 {
288  updateAllLinkPositions(characterPositions);
289 
290  vector<ColdetModelPairExPtr> tmpColdetPairs;
291 
292  for(unsigned int i=0; i < checkPairs.length(); ++i){
293  const LinkPair& linkPair = checkPairs[i];
294  addCollisionPairSub(linkPair, tmpColdetPairs);
295  }
296 
297  computeDistances(tmpColdetPairs, out_distances);
298 }
299 
301 (
302  const DblArray3 point,
303  const DblArray3 dir
304  )
305 {
306  CORBA::Double D, minD=0;
307  StringToColdetBodyMap::iterator it = nameToColdetBodyMap.begin();
308  for (; it!=nameToColdetBodyMap.end(); it++){
309 #if 0
310  std::cout << it->first << std::endl;
311 #endif
312  ColdetBodyPtr body = it->second;
313  for (unsigned int i=0; i<body->numLinks(); i++){
314  D = body->linkColdetModel(i)->computeDistanceWithRay(point, dir);
315  if ((minD==0&&D>0)||(minD>0&&D>0&&minD>D)) minD = D;
316 #if 0
317  std::cout << "D = " << D << std::endl;
318 #endif
319  }
320  }
321 #if 0
322  std::cout << "minD = " << minD << std::endl;
323 #endif
324  return minD;
325 }
326 
327 DblSequence* CollisionDetector_impl::scanDistanceWithRay(const DblArray3 p, const DblArray9 R, CORBA::Double step, CORBA::Double range)
328 {
329  DblSequence *distances = new DblSequence();
330  int scan_half = (int)(range/2/step);
331  distances->length(scan_half*2+1);
332  double local[3], a;
333  DblArray3 dir;
334  local[1] = 0;
335  for (int i = -scan_half; i<= scan_half; i++){
336  a = i*step;
337  local[0] = -sin(a); local[2] = -cos(a);
338  dir[0] = R[0]*local[0]+R[1]*local[1]+R[2]*local[2];
339  dir[1] = R[3]*local[0]+R[4]*local[1]+R[5]*local[2];
340  dir[2] = R[6]*local[0]+R[7]*local[1]+R[8]*local[2];
341  (*distances)[i+scan_half] = queryDistanceWithRay(p, dir);
342 #if 0
343  printf("angle = %8.3f, distance = %8.3f\n", a, (*distances)[i+scan_half]);
344  fflush(stdout);
345 #endif
346  }
347  return distances;
348 }
349 
351 (vector<ColdetModelPairExPtr>& coldetPairs, LinkPairSequence_out& out_collidedPairs, const bool checkAll)
352 {
353  CollisionPointSequence dummy;
354 
355  bool detected = false;
356 
357  vector<int> collidedPairIndices;
358  collidedPairIndices.reserve(coldetPairs.size());
359 
360  for(unsigned int i=0; i < coldetPairs.size(); ++i){
361  if(detectCollisionsOfLinkPair(*coldetPairs[i], dummy, false)){
362  detected = true;
363  collidedPairIndices.push_back(i);
364  if(!checkAll){
365  break;
366  }
367  }
368  }
369 
370  out_collidedPairs = new LinkPairSequence();
371  out_collidedPairs->length(collidedPairIndices.size());
372 
373  for(CORBA::ULong i=0; i < collidedPairIndices.size(); ++i){
374  int pairIndex = collidedPairIndices[i];
375  ColdetModelPairEx& coldetPair = *coldetPairs[pairIndex];
376  LinkPair& linkPair = out_collidedPairs[i];
377  linkPair.charName1 = CORBA::string_dup(coldetPair.body0->name());
378  linkPair.linkName1 = CORBA::string_dup(coldetPair.model(0)->name().c_str());
379  linkPair.charName2 = CORBA::string_dup(coldetPair.body1->name());
380  linkPair.linkName2 = CORBA::string_dup(coldetPair.model(1)->name().c_str());
381  }
382 
383  return detected;
384 }
385 
387 (vector<ColdetModelPairExPtr>& coldetPairs, LinkPairSequence_out& out_collidedPairs, const bool checkAll)
388 {
389  bool detected = false;
390 
391  vector<int> collidedPairIndices;
392  collidedPairIndices.reserve(coldetPairs.size());
393 
394  for(unsigned int i=0; i < coldetPairs.size(); ++i){
395  if(coldetPairs[i]->detectIntersection()){
396  detected = true;
397  collidedPairIndices.push_back(i);
398  if(!checkAll){
399  break;
400  }
401  }
402  }
403 
404  out_collidedPairs = new LinkPairSequence();
405  out_collidedPairs->length(collidedPairIndices.size());
406 
407  for(CORBA::ULong i=0; i < collidedPairIndices.size(); ++i){
408  int pairIndex = collidedPairIndices[i];
409  ColdetModelPairEx& coldetPair = *coldetPairs[pairIndex];
410  LinkPair& linkPair = out_collidedPairs[i];
411  linkPair.charName1 = CORBA::string_dup(coldetPair.body0->name());
412  linkPair.linkName1 = CORBA::string_dup(coldetPair.model(0)->name().c_str());
413  linkPair.charName2 = CORBA::string_dup(coldetPair.body1->name());
414  linkPair.linkName2 = CORBA::string_dup(coldetPair.model(1)->name().c_str());
415  }
416 
417  return detected;
418 }
419 
421 (vector<ColdetModelPairExPtr>& coldetPairs, DistanceSequence_out& out_distances)
422 {
423  out_distances = new DistanceSequence();
424  out_distances->length(coldetPairs.size());
425 
426  for(unsigned int i=0; i < coldetPairs.size(); ++i){
427  ColdetModelPairEx& coldetPair = *coldetPairs[i];
428  Distance& dinfo = out_distances[i];
429  dinfo.minD = coldetPair.computeDistance(dinfo.point0, dinfo.point1);
430  LinkPair& linkPair = dinfo.pair;
431  linkPair.charName1 = CORBA::string_dup(coldetPair.body0->name());
432  linkPair.linkName1 = CORBA::string_dup(coldetPair.model(0)->name().c_str());
433  linkPair.charName2 = CORBA::string_dup(coldetPair.body1->name());
434  linkPair.linkName2 = CORBA::string_dup(coldetPair.model(1)->name().c_str());
435  }
436 }
437 
438 
440  : orb(CORBA_ORB::_duplicate(orb))
441 {
442 
443 }
444 
445 
447 {
448  PortableServer::POA_var poa = _default_POA();
449  PortableServer::ObjectId_var id = poa->servant_to_id(this);
450  poa->deactivate_object(id);
451 }
452 
453 
455 {
456  CollisionDetector_impl* collisionDetector = new CollisionDetector_impl(orb);
457  PortableServer::ServantBase_var collisionDetectorrServant = collisionDetector;
458  PortableServer::POA_var poa = _default_POA();
459  PortableServer::ObjectId_var id = poa->activate_object(collisionDetector);
460  return collisionDetector->_this();
461 }
462 
463 
465 {
466  orb->shutdown(false);
467 }
virtual CORBA::Boolean queryIntersectionForGivenPairs(CORBA::Boolean checkAll, const LinkPairSequence &checkPairs, const CharacterPositionSequence &characterPositions, LinkPairSequence_out collidedPairs)
#define CORBA_ORB_ptr
Definition: ORBwrap.h:16
#define CORBA_ORB
Definition: ORBwrap.h:15
void computeDistances(vector< ColdetModelPairExPtr > &coldetPairs, DistanceSequence_out &out_distances)
boost::intrusive_ptr< ColdetBody > ColdetBodyPtr
Definition: ColdetBody.h:72
bool detectCollisionsOfLinkPair(ColdetModelPairEx &coldetPair, CollisionPointSequence &out_collisionPoints, const bool addCollisionPoints)
bool detectCollidedLinkPairs(vector< ColdetModelPairExPtr > &coldetPairs, LinkPairSequence_out &out_collidedPairs, const bool checkAll)
CollisionDetectorFactory_impl(CORBA_ORB_ptr orb)
png_infop png_charpp name
Definition: png.h:2382
bool detectAllCollisions(vector< ColdetModelPairExPtr > &coldetPairs, CollisionSequence_out &out_collisions)
void updateAllLinkPositions(const CharacterPositionSequence &characterPositions)
png_uint_32 i
Definition: png.h:2735
bool detectIntersectingLinkPairs(vector< ColdetModelPairExPtr > &coldetPairs, LinkPairSequence_out &out_collidedPairs, const bool checkAll)
void addCollisionPairSub(const LinkPair &linkPair, vector< ColdetModelPairExPtr > &io_coldetPairs)
virtual void queryDistanceForGivenPairs(const LinkPairSequence &checkPairs, const CharacterPositionSequence &characterPositions, DistanceSequence_out distances)
double computeDistance(double *point0, double *point1)
virtual void queryDistanceForDefinedPairs(const CharacterPositionSequence &characterPositions, DistanceSequence_out distances)
StringToColdetBodyMap nameToColdetBodyMap
def j(str, encoding="cp932")
virtual DblSequence * scanDistanceWithRay(const DblArray3 p, const DblArray9 R, CORBA::Double step, CORBA::Double range)
list index
vector< ColdetModelPairExPtr > coldetModelPairs
string a
virtual CORBA::Boolean queryIntersectionForDefinedPairs(CORBA::Boolean checkAll, const CharacterPositionSequence &characterPositions, LinkPairSequence_out collidedPairs)
ColdetModel * model(int index)
virtual CORBA::Boolean queryContactDeterminationForGivenPairs(const LinkPairSequence &checkPairs, const CharacterPositionSequence &characterPositions, CollisionSequence_out collisions)
typedef int
Definition: png.h:1113
virtual CORBA::Double queryDistanceWithRay(const DblArray3 point, const DblArray3 dir)
CollisionDetector_impl(CORBA_ORB_ptr orb)
virtual void addCollisionPair(const LinkPair &colPair)
const std::string & name() const
get name of this model
Definition: ColdetModel.h:93
#define local
Definition: crc32.c:31
boost::intrusive_ptr< ColdetModel > ColdetModelPtr
Definition: ColdetModel.h:268
virtual CORBA::Boolean queryContactDeterminationForDefinedPairs(const CharacterPositionSequence &characterPositions, CollisionSequence_out collisions)
virtual void registerCharacter(const char *name, BodyInfo_ptr bodyInfo)
std::vector< collision_data > & detectCollisions()


openhrp3
Author(s): AIST, General Robotix Inc., Nakamura Lab of Dept. of Mechano Informatics at University of Tokyo
autogenerated on Sat Apr 13 2019 02:14:20