Commit 32ac237d authored by Joseph Mirabel's avatar Joseph Mirabel Committed by Joseph Mirabel
Browse files

Sort disabled collision pairs to allow faster retrieval.

* This decreases the time when there are a lot of disabled collisions.
parent f0c5f478
......@@ -125,11 +125,26 @@ namespace hpp
return in.substr (prefix_.size ());
}
void sortCollisionPairs ();
urdf::Parser* urdfParser_;
::srdf::Model srdfModel_;
RobotPtrType robot_;
std::string prefix_;
typedef ::srdf::Model::DisabledCollision DisabledCollision;
std::vector < DisabledCollision > sortedDisabledCollisions_;
struct {
bool operator () (const DisabledCollision& c1, const DisabledCollision& c2)
{
int res_link1 = c1.link1_.compare (c2.link1_);
if (res_link1 == 0)
return c2.link2_.compare (c2.link2_) > 0;
else
return res_link1 > 0;
}
} disabledCollisionComp_;
}; // class Parser
} // end of namespace srdf.
......
......@@ -64,11 +64,25 @@ namespace hpp
}
}
void Parser::sortCollisionPairs ()
{
// Retrieve collision pairs that will NOT be taken into account.
const CollisionPairsType& disabledColPairs
= srdfModel_.getDisabledCollisionPairs ();
sortedDisabledCollisions_.resize (disabledColPairs.size());
std::partial_sort_copy (disabledColPairs.begin (), disabledColPairs.end (),
sortedDisabledCollisions_.begin (), sortedDisabledCollisions_.end (),
disabledCollisionComp_);
}
void Parser::addCollisionPairs ()
{
typedef urdf::Parser::MapHppJointType MapHppJointType;
MapHppJointType& jmap = urdfParser_->jointsMap_;
sortCollisionPairs ();
// Cycle through all joint pairs
for (MapHppJointType::const_iterator it1 = jmap.begin ();
it1 != jmap.end (); it1++) {
......@@ -111,22 +125,16 @@ namespace hpp
Parser::isCollisionPairDisabled (const std::string& bodyName_1,
const std::string& bodyName_2)
{
// Retrieve collision pairs that will NOT be taken into account.
CollisionPairsType disabledColPairs
= srdfModel_.getDisabledCollisionPairs ();
// Cycle through disabled collision pairs.
BOOST_FOREACH (CollisionPairType disabledColPair, disabledColPairs)
{
std::string disabled1 = disabledColPair.link1_;
std::string disabled2 = disabledColPair.link2_;
if ((bodyName_1 == disabled1 && bodyName_2 == disabled2)
|| (bodyName_1 == disabled2 && bodyName_2 == disabled1))
return true;
}
return false;
DisabledCollision dc;
dc.link1_ = bodyName_1;
dc.link2_ = bodyName_2;
if (std::binary_search (sortedDisabledCollisions_.begin (),
sortedDisabledCollisions_.end (), dc, disabledCollisionComp_))
return true;
dc.link2_ = bodyName_1;
dc.link1_ = bodyName_2;
return std::binary_search (sortedDisabledCollisions_.begin (),
sortedDisabledCollisions_.end (), dc, disabledCollisionComp_);
}
bool
......
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment