Skip to content

Commit

Permalink
use _mapListNonCollidingInterGrabbedLinkPairsWhenGrabbed and _listNon…
Browse files Browse the repository at this point in the history 
…CollidingGrabbedGrabberLinkPairsWhenGrabbed in CheckSelfCollisionChecking
  • Loading branch information
Shunichi Nozawa committed Sep 25, 2024
1 parent 0cd3346 commit a942ee1
Showing 1 changed file with 35 additions and 67 deletions.
102 changes: 35 additions & 67 deletions src/libopenrave/kinbodycollision.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -83,8 +83,7 @@ bool KinBody::CheckSelfCollision(CollisionReportPtr report, CollisionCheckerBase
RAVELOG_WARN_FORMAT("env=%s, grabbed body on %s has already been destroyed, ignoring.", GetEnv()->GetNameId()%GetName());
continue;
}
const KinBody& grabbedBody = *pGrabbedBody;
if( !grabbedBody.IsEnabled() ) {
if( !pGrabbedBody->IsEnabled() ) {
continue;
}
vGrabbedBodies.emplace_back(grabPair.second.get());
Expand All @@ -95,14 +94,13 @@ bool KinBody::CheckSelfCollision(CollisionReportPtr report, CollisionCheckerBase
const size_t numGrabbed = _grabbedBodiesByEnvironmentIndex.size();
// RAVELOG_INFO_FORMAT("env=%s, checking self collision for %s with grabbed bodies: numgrabbed=%d", GetEnv()->GetNameId()%GetName()%numGrabbed);
for (size_t indexGrabbed1 = 0; indexGrabbed1 < numGrabbed; indexGrabbed1++) {
const KinBodyPtr& pGrabbedBody1 = vLockedGrabbedBodiesCache[indexGrabbed1];
vGrabbedBodies[indexGrabbed1]->ComputeListNonCollidingLinks();

const std::list<KinBody::LinkConstPtr>& nonCollidingLinks1 = vGrabbedBodies[indexGrabbed1]->_listNonCollidingLinksWhenGrabbed;
const ListNonCollidingLinkPairs& nonCollidingLinkPairs = vGrabbedBodies[indexGrabbed1]->_listNonCollidingGrabbedGrabberLinkPairsWhenGrabbed;

KinBodyPtr pLinkParent;

for (const KinBody::LinkConstPtr& probotlinkFromNonColliding : nonCollidingLinks1) {
FOREACHC(itNonCollidingLinkPairs, nonCollidingLinkPairs) {
const KinBody::LinkConstPtr& probotlinkFromNonColliding = (*itNonCollidingLinkPairs).second;
const KinBody::Link& robotlinkFromNonColliding = *probotlinkFromNonColliding;
pLinkParent = robotlinkFromNonColliding.GetParent(true);
if( !pLinkParent ) {
Expand All @@ -111,29 +109,24 @@ bool KinBody::CheckSelfCollision(CollisionReportPtr report, CollisionCheckerBase
const KinBody::LinkConstPtr& probotlink = (!!pLinkParent) ? probotlinkFromNonColliding : _veclinks.at(robotlinkFromNonColliding.GetIndex());

// have to use link/link collision since link/body checks attached bodies
for (const KinBody::LinkPtr& pGrabbedBodylink : grabbedBody1.GetLinks()) {
if( collisionchecker->CheckCollision(probotlink, pGrabbedBodylink, pusereport) ) {
bCollision = true;
if( !bAllLinkCollisions ) { // if checking all collisions, have to continue
break;
}
}
if( !!pusereport && pusereport->minDistance < report->minDistance ) {
*report = *pusereport;
}
}
if( bCollision ) {
const KinBody::LinkConstPtr& pGrabbedBodylink = (*itNonCollidingLinkPairs).first;
if( collisionchecker->CheckCollision(probotlink, pGrabbedBodylink, pusereport) ) {
bCollision = true;
if( !bAllLinkCollisions ) { // if checking all collisions, have to continue
break;
}
}
if( !!pusereport && pusereport->minDistance < report->minDistance ) {
*report = *pusereport;
}
}
if( bCollision ) {
if( !bAllLinkCollisions ) { // if checking all collisions, have to continue
break;
}
}

const KinBody& grabbedBody1 = *vLockedGrabbedBodiesCache[indexGrabbed1];
if( grabbedBody1.CheckSelfCollision(pusereport, collisionchecker) ) {
bCollision = true;
if( !bAllLinkCollisions ) { // if checking all collisions, have to continue
Expand All @@ -143,62 +136,37 @@ bool KinBody::CheckSelfCollision(CollisionReportPtr report, CollisionCheckerBase
if( !!pusereport && pusereport->minDistance < report->minDistance ) {
*report = *pusereport;
}
} // end for indexGrabbed1

if( numGrabbed > 1 ) {
// Since collision checking is commutative (i.e. CheckCollision(link1, link2) == CheckCollision(link2, link1)), checking it once per pair is sufficient.
for( size_t indexGrabbed2 = indexGrabbed1 + 1; indexGrabbed2 < numGrabbed; ++indexGrabbed2 ) {
const KinBodyPtr& pGrabbedBody2 = vLockedGrabbedBodiesCache[indexGrabbed2];
vGrabbedBodies[indexGrabbed2]->ComputeListNonCollidingLinks();

const std::list<KinBody::LinkConstPtr>& nonCollidingLinks2 = vGrabbedBodies[indexGrabbed2]->_listNonCollidingLinksWhenGrabbed;

for( const KinBody::LinkPtr& pGrabbedBody2Link : grabbedBody2.GetLinks() ) {
if( !pGrabbedBody2Link->IsEnabled() ) {
continue;
}
// See if these two links were initially colliding. If they are, then no further
// check is need (as this link pair should be skipped).
// if the link is in nonCollidingLinks1, collision has already been checked.
if( std::find(nonCollidingLinks1.begin(), nonCollidingLinks1.end(), pGrabbedBody2Link) != nonCollidingLinks1.end() ) {
continue;
}
for( const KinBody::LinkPtr& pGrabbedBody1Link : grabbedBody1.GetLinks() ) {
if( !pGrabbedBody1Link->IsEnabled() ) {
continue;
}
if( std::find(nonCollidingLinks2.begin(), nonCollidingLinks2.end(), pGrabbedBody1Link) != nonCollidingLinks2.end() ) {
if( collisionchecker->CheckCollision(KinBody::LinkConstPtr(pGrabbedBody1Link),
KinBody::LinkConstPtr(pGrabbedBody2Link),
pusereport) ) {
bCollision = true;
if( !bAllLinkCollisions ) { // if checking all collisions, have to continue
break;
}
} // end if CheckCollision
if( !!pusereport && pusereport->minDistance < report->minDistance ) {
*report = *pusereport;
}
} // end if pGrabbedBody1Link in nonCollidingLinks2
} // end for pGrabbedBody1Link in nonCollidingLinks2
if( bCollision ) {
if( !bAllLinkCollisions ) { // if checking all collisions, have to continue
break;
}
}
} // end for pGrabbedBody2Link
if( bCollision ) {
if( !bAllLinkCollisions ) { // if checking all collisions, have to continue
break;
}
// Check grabbed vs grabbed collision
// TODO
FOREACHC(itInfo, _mapListNonCollidingInterGrabbedLinkPairsWhenGrabbed) {
const ListNonCollidingLinkPairs& pairs = (*itInfo).second;
if( pairs.empty() ) {
continue;
}
const KinBodyPtr pLink1 = pairs.front().first->GetParent(true);
const KinBodyPtr pLink2 = pairs.front().second->GetParent(true);
if( !pLink1 || !pLink2 ) {
continue;
}
FOREACHC(itLinks, pairs) {
if( collisionchecker->CheckCollision((*itLinks).first, (*itLinks).second, pusereport) ) {
bCollision = true;
if( !bAllLinkCollisions ) { // if checking all collisions, have to continue
break;
}
} // end for indexGrabbed2
} // end if numGrabbed > 1
}
if( !!pusereport && pusereport->minDistance < report->minDistance ) {
*report = *pusereport;
}
}
if( bCollision ) {
if( !bAllLinkCollisions ) { // if checking all collisions, have to continue
break;
}
}
} // end for indexGrabbed1
}

if( bCollision && !!report ) {
if( report != pusereport ) {
Expand Down

0 comments on commit a942ee1

Please sign in to comment.