00001
00002
00003
00004
00005
00006
00007
00008
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
00056 cout << "BodyInfo CORBA ID of " << name << " is " << bodyInfoId << endl;
00057
00058
00059
00060
00061
00062 coldetBody = new ColdetBody(bodyInfo);
00063 coldetBody->setName(name);
00064
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 }