CollisionDetector_impl.cpp
Go to the documentation of this file.
00001 /*
00002  * Copyright (c) 2008, AIST, the University of Tokyo and General Robotix Inc.
00003  * All rights reserved. This program is made available under the terms of the
00004  * Eclipse Public License v1.0 which accompanies this distribution, and is
00005  * available at http://www.eclipse.org/legal/epl-v10.html
00006  * Contributors:
00007  * National Institute of Advanced Industrial Science and Technology (AIST)
00008  * General Robotix Inc. 
00009  */
00010 
00017 #include "CollisionDetector_impl.h"
00018 #include <hrpCollision/ColdetModel.h>
00019 #include <iostream>
00020 #include <string>
00021 
00022 
00023 using namespace std;
00024 using namespace hrp;
00025 
00026 
00027 CollisionDetector_impl::CollisionDetector_impl(CORBA_ORB_ptr orb)
00028     : orb(CORBA_ORB::_duplicate(orb))
00029 {
00030 
00031 }
00032 
00033 
00034 CollisionDetector_impl::~CollisionDetector_impl()
00035 {
00036 
00037 }
00038 
00039 
00040 void CollisionDetector_impl::destroy()
00041 {
00042     PortableServer::POA_var poa = _default_POA();
00043     PortableServer::ObjectId_var id = poa->servant_to_id(this);
00044     poa->deactivate_object(id);
00045 }
00046 
00047 
00048 void CollisionDetector_impl::registerCharacter(const char* name,        BodyInfo_ptr bodyInfo)
00049 {
00050     cout << "adding " << name << " ";
00051     ColdetBodyPtr coldetBody;
00052         
00053     string bodyInfoId(orb->object_to_string(bodyInfo));
00054 
00055     // test
00056     cout << "BodyInfo CORBA ID of " << name << " is " << bodyInfoId << endl;
00057         
00058     //StringToColdetBodyMap::iterator it = bodyInfoToColdetBodyMap.find(bodyInfoId);
00059     //if(it != bodyInfoToColdetBodyMap.end()){
00060     //    coldetBody = new ColdetBody(*it->second);
00061     //} else {
00062         coldetBody = new ColdetBody(bodyInfo);
00063         coldetBody->setName(name);
00064       //  bodyInfoToColdetBodyMap.insert(it, make_pair(bodyInfoId, coldetBody));
00065     //}
00066 
00067     StringToColdetBodyMap::iterator it = nameToColdetBodyMap.find(name);
00068     if(it != nameToColdetBodyMap.end()){
00069         cout << "\n";
00070         cout << "The model of the name " << name;
00071         cout << " has already been registered. It is replaced." << endl;
00072         nameToColdetBodyMap[name] = coldetBody;
00073     } else {
00074         nameToColdetBodyMap.insert(it, make_pair(name, coldetBody));
00075         cout << " is ok !" << endl;
00076     }
00077 }
00078 
00079 
00080 void CollisionDetector_impl::addCollisionPair
00081 (const LinkPair& linkPair)
00082 {
00083     addCollisionPairSub(linkPair, coldetModelPairs);
00084 }
00085 
00086 
00087 void CollisionDetector_impl::addCollisionPairSub
00088 (const LinkPair& linkPair, vector<ColdetModelPairExPtr>& io_coldetPairs)
00089 {
00090     const char* bodyName[2];
00091     bodyName[0] = linkPair.charName1;
00092     bodyName[1] = linkPair.charName2;
00093         
00094     const char* linkName[2];
00095     linkName[0] = linkPair.linkName1;
00096     linkName[1] = linkPair.linkName2;
00097 
00098     bool notFound = false;
00099     ColdetBodyPtr coldetBody[2];
00100     ColdetModelPtr coldetModel[2];
00101 
00102     for(int i=0; i < 2; ++i){
00103         StringToColdetBodyMap::iterator it = nameToColdetBodyMap.find(bodyName[i]);
00104         if(it == nameToColdetBodyMap.end()){
00105             cout << "CollisionDetector::addCollisionPair : Body ";
00106             cout << bodyName[i] << " is not found." << endl;
00107             notFound = true;
00108         } else {
00109             coldetBody[i] = it->second;
00110             coldetModel[i] = coldetBody[i]->linkColdetModel(linkName[i]);
00111             if(!coldetModel[i]){
00112                 cout << "CollisionDetector::addCollisionPair : Link ";
00113                 cout << linkName[i] << " is not found." << endl;
00114                 notFound = true;
00115             }
00116         }
00117     }
00118 
00119     if(!notFound){
00120         io_coldetPairs.push_back(
00121             new ColdetModelPairEx(coldetBody[0], coldetModel[0], coldetBody[1], coldetModel[1], linkPair.tolerance));
00122     }
00123 }       
00124 
00125 
00126 CORBA::Boolean CollisionDetector_impl::queryContactDeterminationForDefinedPairs
00127 (const CharacterPositionSequence& characterPositions, CollisionSequence_out out_collisions)
00128 {
00129     updateAllLinkPositions(characterPositions);
00130     return detectAllCollisions(coldetModelPairs, out_collisions);
00131 }
00132 
00133 
00134 CORBA::Boolean CollisionDetector_impl::queryContactDeterminationForGivenPairs
00135 (const LinkPairSequence& checkPairs,
00136  const CharacterPositionSequence& characterPositions,
00137  CollisionSequence_out out_collisions)
00138 {
00139     updateAllLinkPositions(characterPositions);
00140 
00141     vector<ColdetModelPairExPtr> tmpColdetPairs;
00142         
00143     for(unsigned int i=0; i < checkPairs.length(); ++i){
00144         const LinkPair& linkPair = checkPairs[i];
00145         addCollisionPairSub(linkPair, tmpColdetPairs);
00146     }
00147 
00148     return detectAllCollisions(tmpColdetPairs, out_collisions);
00149 }
00150 
00151 
00152 void CollisionDetector_impl::updateAllLinkPositions
00153 (const CharacterPositionSequence& characterPositions)
00154 {
00155     for(unsigned int i=0; i < characterPositions.length(); i++){
00156         const CharacterPosition& characterPosition = characterPositions[i];
00157         const string bodyName(characterPosition.characterName);
00158         StringToColdetBodyMap::iterator it = nameToColdetBodyMap.find(bodyName);
00159         if(it != nameToColdetBodyMap.end()){
00160             ColdetBodyPtr& coldetBody = it->second;
00161             coldetBody->setLinkPositions(characterPosition.linkPositions);
00162         }
00163     }
00164 }
00165 
00166 
00167 bool CollisionDetector_impl::detectAllCollisions
00168 (vector<ColdetModelPairExPtr>& coldetPairs, CollisionSequence_out& out_collisions)
00169 {
00170     bool detected = false;
00171     const int numColdetPairs = coldetPairs.size();
00172     out_collisions = new CollisionSequence;
00173     out_collisions->length(numColdetPairs);
00174         
00175     for(int i=0; i < numColdetPairs; ++i){
00176 
00177         ColdetModelPairEx& coldetPair = *coldetPairs[i];
00178         Collision& collision = out_collisions[i];
00179 
00180         if(detectCollisionsOfLinkPair(coldetPair, collision.points, true)){
00181             detected = true;
00182         }
00183                 
00184         collision.pair.charName1 = CORBA::string_dup(coldetPair.body0->name());
00185         collision.pair.linkName1 = CORBA::string_dup(coldetPair.model(0)->name().c_str());
00186         collision.pair.charName2 = CORBA::string_dup(coldetPair.body1->name());
00187         collision.pair.linkName2 = CORBA::string_dup(coldetPair.model(1)->name().c_str());
00188     }
00189 
00190     return detected;
00191 }
00192 
00193 
00194 bool CollisionDetector_impl::detectCollisionsOfLinkPair
00195 (ColdetModelPairEx& coldetPair, CollisionPointSequence& out_collisionPoints, const bool addCollisionPoints)
00196 {
00197     bool detected = false;
00198 
00199     vector<collision_data>& cdata = coldetPair.detectCollisions();
00200 
00201     int npoints = 0;
00202     for(unsigned int i=0; i < cdata.size(); i++) {
00203         for(int j=0; j < cdata[i].num_of_i_points; j++){
00204             if(cdata[i].i_point_new[j]){
00205                 npoints ++;
00206             }
00207         }
00208     }
00209     if(npoints > 0){
00210         detected = true;
00211         if(addCollisionPoints){
00212             out_collisionPoints.length(npoints);
00213             int index = 0;
00214             for(unsigned int i=0; i < cdata.size(); i++) {
00215                 collision_data& cd = cdata[i];
00216                 for(int j=0; j < cd.num_of_i_points; j++){
00217                     if (cd.i_point_new[j]){
00218                         CollisionPoint& point = out_collisionPoints[index];
00219                         for(int k=0; k < 3; k++){
00220                             point.position[k] = cd.i_points[j][k];
00221                         }
00222                         for(int k=0; k < 3; k++){
00223                             point.normal[k] = cd.n_vector[k];
00224                         }
00225                         point.idepth = cd.depth;
00226                         index++;
00227                     }
00228                 }
00229             }
00230         }
00231     }
00232         
00233     return detected;
00234 }
00235 
00236 
00237 CORBA::Boolean CollisionDetector_impl::queryIntersectionForDefinedPairs
00238 (
00239     CORBA::Boolean checkAll,
00240     const CharacterPositionSequence& characterPositions,
00241     LinkPairSequence_out out_collidedPairs
00242     )
00243 {
00244     updateAllLinkPositions(characterPositions);
00245     return detectIntersectingLinkPairs(coldetModelPairs, out_collidedPairs, checkAll);
00246 }
00247 
00248 
00249 CORBA::Boolean CollisionDetector_impl::queryIntersectionForGivenPairs
00250 (
00251     CORBA::Boolean checkAll,
00252     const LinkPairSequence& checkPairs,
00253     const CharacterPositionSequence& characterPositions,
00254     LinkPairSequence_out out_collidedPairs
00255     )
00256 {
00257     updateAllLinkPositions(characterPositions);
00258 
00259     vector<ColdetModelPairExPtr> tmpColdetPairs;
00260         
00261     for(unsigned int i=0; i < checkPairs.length(); ++i){
00262         const LinkPair& linkPair = checkPairs[i];
00263         addCollisionPairSub(linkPair, tmpColdetPairs);
00264     }
00265 
00266     return detectIntersectingLinkPairs(tmpColdetPairs, out_collidedPairs, checkAll);
00267 }
00268 
00269 
00270 void CollisionDetector_impl::queryDistanceForDefinedPairs
00271 (
00272     const CharacterPositionSequence& characterPositions,
00273     DistanceSequence_out out_distances
00274     )
00275 {
00276     updateAllLinkPositions(characterPositions);
00277     computeDistances(coldetModelPairs, out_distances);
00278 }
00279 
00280 
00281 void CollisionDetector_impl::queryDistanceForGivenPairs
00282 (
00283     const LinkPairSequence& checkPairs,
00284     const CharacterPositionSequence& characterPositions,
00285     DistanceSequence_out out_distances
00286     )
00287 {
00288     updateAllLinkPositions(characterPositions);
00289 
00290     vector<ColdetModelPairExPtr> tmpColdetPairs;
00291         
00292     for(unsigned int i=0; i < checkPairs.length(); ++i){
00293         const LinkPair& linkPair = checkPairs[i];
00294         addCollisionPairSub(linkPair, tmpColdetPairs);
00295     }
00296 
00297     computeDistances(tmpColdetPairs, out_distances);
00298 }
00299 
00300 CORBA::Double CollisionDetector_impl::queryDistanceWithRay
00301 (
00302  const DblArray3 point,
00303  const DblArray3 dir
00304  )
00305 {
00306     CORBA::Double D, minD=0;
00307     StringToColdetBodyMap::iterator it = nameToColdetBodyMap.begin();
00308     for (; it!=nameToColdetBodyMap.end(); it++){
00309 #if 0
00310         std::cout << it->first << std::endl;
00311 #endif
00312         ColdetBodyPtr body = it->second;
00313         for (unsigned int i=0; i<body->numLinks(); i++){
00314             D = body->linkColdetModel(i)->computeDistanceWithRay(point, dir);
00315             if ((minD==0&&D>0)||(minD>0&&D>0&&minD>D)) minD = D;
00316 #if 0
00317             std::cout << "D = " << D << std::endl;
00318 #endif
00319         }
00320     }
00321 #if 0
00322             std::cout << "minD = " << minD << std::endl;
00323 #endif
00324     return minD;
00325 }
00326 
00327 DblSequence* CollisionDetector_impl::scanDistanceWithRay(const DblArray3 p, const DblArray9 R, CORBA::Double step, CORBA::Double range)
00328 {
00329     DblSequence *distances = new DblSequence();
00330     int scan_half = (int)(range/2/step);
00331     distances->length(scan_half*2+1);
00332     double local[3], a;
00333     DblArray3 dir;
00334     local[1] = 0; 
00335     for (int i = -scan_half; i<= scan_half; i++){
00336         a = i*step;
00337         local[0] = -sin(a); local[2] = -cos(a); 
00338         dir[0] = R[0]*local[0]+R[1]*local[1]+R[2]*local[2]; 
00339         dir[1] = R[3]*local[0]+R[4]*local[1]+R[5]*local[2]; 
00340         dir[2] = R[6]*local[0]+R[7]*local[1]+R[8]*local[2]; 
00341         (*distances)[i+scan_half] = queryDistanceWithRay(p, dir); 
00342 #if 0
00343         printf("angle = %8.3f, distance = %8.3f\n", a, (*distances)[i+scan_half]);
00344         fflush(stdout);
00345 #endif
00346     }
00347     return distances;
00348 }
00349 
00350 bool CollisionDetector_impl::detectCollidedLinkPairs
00351 (vector<ColdetModelPairExPtr>& coldetPairs, LinkPairSequence_out& out_collidedPairs, const bool checkAll)
00352 {
00353     CollisionPointSequence dummy;
00354         
00355     bool detected = false;
00356         
00357     vector<int> collidedPairIndices;
00358     collidedPairIndices.reserve(coldetPairs.size());
00359 
00360     for(unsigned int i=0; i < coldetPairs.size(); ++i){
00361         if(detectCollisionsOfLinkPair(*coldetPairs[i], dummy, false)){
00362             detected = true;
00363             collidedPairIndices.push_back(i);
00364             if(!checkAll){
00365                 break;
00366             }
00367         }
00368     }
00369 
00370     out_collidedPairs = new LinkPairSequence();
00371     out_collidedPairs->length(collidedPairIndices.size());
00372 
00373     for(CORBA::ULong i=0; i < collidedPairIndices.size(); ++i){
00374         int pairIndex = collidedPairIndices[i];
00375         ColdetModelPairEx& coldetPair = *coldetPairs[pairIndex];
00376         LinkPair& linkPair = out_collidedPairs[i];
00377         linkPair.charName1 = CORBA::string_dup(coldetPair.body0->name());
00378         linkPair.linkName1 = CORBA::string_dup(coldetPair.model(0)->name().c_str());
00379         linkPair.charName2 = CORBA::string_dup(coldetPair.body1->name());
00380         linkPair.linkName2 = CORBA::string_dup(coldetPair.model(1)->name().c_str());
00381     }
00382 
00383     return detected;
00384 }
00385 
00386 bool CollisionDetector_impl::detectIntersectingLinkPairs
00387 (vector<ColdetModelPairExPtr>& coldetPairs, LinkPairSequence_out& out_collidedPairs, const bool checkAll)
00388 {
00389     bool detected = false;
00390         
00391     vector<int> collidedPairIndices;
00392     collidedPairIndices.reserve(coldetPairs.size());
00393 
00394     for(unsigned int i=0; i < coldetPairs.size(); ++i){
00395         if(coldetPairs[i]->detectIntersection()){
00396             detected = true;
00397             collidedPairIndices.push_back(i);
00398             if(!checkAll){
00399                 break;
00400             }
00401         }
00402     }
00403 
00404     out_collidedPairs = new LinkPairSequence();
00405     out_collidedPairs->length(collidedPairIndices.size());
00406 
00407     for(CORBA::ULong i=0; i < collidedPairIndices.size(); ++i){
00408         int pairIndex = collidedPairIndices[i];
00409         ColdetModelPairEx& coldetPair = *coldetPairs[pairIndex];
00410         LinkPair& linkPair = out_collidedPairs[i];
00411         linkPair.charName1 = CORBA::string_dup(coldetPair.body0->name());
00412         linkPair.linkName1 = CORBA::string_dup(coldetPair.model(0)->name().c_str());
00413         linkPair.charName2 = CORBA::string_dup(coldetPair.body1->name());
00414         linkPair.linkName2 = CORBA::string_dup(coldetPair.model(1)->name().c_str());
00415     }
00416 
00417     return detected;
00418 }
00419 
00420 void CollisionDetector_impl::computeDistances
00421 (vector<ColdetModelPairExPtr>& coldetPairs, DistanceSequence_out& out_distances)
00422 {
00423     out_distances = new DistanceSequence();
00424     out_distances->length(coldetPairs.size());
00425 
00426     for(unsigned int i=0; i < coldetPairs.size(); ++i){
00427         ColdetModelPairEx& coldetPair = *coldetPairs[i];
00428         Distance& dinfo = out_distances[i];
00429         dinfo.minD = coldetPair.computeDistance(dinfo.point0, dinfo.point1);
00430         LinkPair& linkPair = dinfo.pair;
00431         linkPair.charName1 = CORBA::string_dup(coldetPair.body0->name());
00432         linkPair.linkName1 = CORBA::string_dup(coldetPair.model(0)->name().c_str());
00433         linkPair.charName2 = CORBA::string_dup(coldetPair.body1->name());
00434         linkPair.linkName2 = CORBA::string_dup(coldetPair.model(1)->name().c_str());
00435     }
00436 }
00437 
00438 
00439 CollisionDetectorFactory_impl::CollisionDetectorFactory_impl(CORBA_ORB_ptr orb)
00440     : orb(CORBA_ORB::_duplicate(orb))
00441 {
00442     
00443 }
00444 
00445 
00446 CollisionDetectorFactory_impl::~CollisionDetectorFactory_impl()
00447 {
00448     PortableServer::POA_var poa = _default_POA();
00449     PortableServer::ObjectId_var id = poa->servant_to_id(this);
00450     poa->deactivate_object(id);
00451 }
00452 
00453 
00454 CollisionDetector_ptr CollisionDetectorFactory_impl::create()
00455 {
00456     CollisionDetector_impl* collisionDetector = new CollisionDetector_impl(orb);
00457     PortableServer::ServantBase_var collisionDetectorrServant = collisionDetector;
00458     PortableServer::POA_var poa = _default_POA();
00459     PortableServer::ObjectId_var id = poa->activate_object(collisionDetector);
00460     return collisionDetector->_this();
00461 }
00462 
00463 
00464 void CollisionDetectorFactory_impl::shutdown()
00465 {
00466     orb->shutdown(false);
00467 }


openhrp3
Author(s): AIST, General Robotix Inc., Nakamura Lab of Dept. of Mechano Informatics at University of Tokyo
autogenerated on Sun Apr 2 2017 03:43:53