/**
\todo reduce the number of calling this function when the project is loaded.
*/
void WorldItemImpl::updateCollisionDetector(bool forceUpdate)
{
if(TRACE_FUNCTIONS){
os << "WorldItemImpl::updateCollisionDetector()" << endl;
}
if(!isCollisionDetectionEnabled){
return;
}
if(forceUpdate){
updateColdetBodyInfos(coldetBodyInfos);
} else {
vector<ColdetBodyInfo> infos;
updateColdetBodyInfos(infos);
if(infos.size() == coldetBodyInfos.size()){
if(std::equal(infos.begin(), infos.end(), coldetBodyInfos.begin(),
[](ColdetBodyInfo& info1, ColdetBodyInfo& info2){
return (info1.bodyItem == info2.bodyItem &&
info1.isSelfCollisionDetectionEnabled == info2.isSelfCollisionDetectionEnabled); })){
return; // not changed
}
}
coldetBodyInfos = infos;
}
clearCollisionDetector();
for(auto& bodyInfo : coldetBodyInfos){
BodyItem* bodyItem = bodyInfo.bodyItem;
bodyCollisionDetector.addBody(
bodyItem->body(), bodyInfo.isSelfCollisionDetectionEnabled,
[&bodyInfo](Link* link, GeometryHandle geometry){
ColdetLinkInfo* linkInfo = new ColdetLinkInfo(bodyInfo.bodyItem, link, geometry);
bodyInfo.linkInfos.push_back(linkInfo);
return linkInfo;
});
ColdetBodyInfo* pBodyInfo = &bodyInfo;
sigKinematicStateChangedConnections.add(
bodyItem->sigKinematicStateChanged().connect(
[&, pBodyInfo](){
pBodyInfo->kinematicStateChanged = true;
updateCollisionsLater.setPriority(kinematicsBar->collisionDetectionPriority());
updateCollisionsLater();
}));
}
bodyCollisionDetector.makeReady();
updateCollisions(true);
}
/**
\todo reduce the number of calling this function when the project is loaded.
*/
void WorldItemImpl::updateCollisionDetector(bool forceUpdate)
{
if(TRACE_FUNCTIONS){
os << "WorldItemImpl::updateCollisionDetector()" << endl;
}
if(!isCollisionDetectionEnabled){
return;
}
if(!forceUpdate){
ItemList<BodyItem> prevBodyItems = collisionBodyItems;
boost::dynamic_bitset<> prevSelfCollisionFlags = collisionBodyItemsSelfCollisionFlags;
updateCollisionBodyItems();
if(collisionBodyItems == prevBodyItems &&
collisionBodyItemsSelfCollisionFlags == prevSelfCollisionFlags){
return;
}
} else {
updateCollisionBodyItems();
}
clearCollisionDetector();
for(size_t i=0; i < collisionBodyItems.size(); ++i){
BodyItem* bodyItem = collisionBodyItems.get(i);
const BodyPtr& body = bodyItem->body();
const int numLinks = body->numLinks();
pair<BodyItemInfoMap::iterator, bool> inserted =
bodyItemInfoMap.insert(make_pair(bodyItem, BodyItemInfo()));
BodyItemInfo& info = inserted.first->second;
info.geometryId = addBodyToCollisionDetector(
*body, *collisionDetector, bodyItem->isSelfCollisionDetectionEnabled());
geometryIdToBodyInfoMap.resize(collisionDetector->numGeometries(), inserted.first);
sigKinematicStateChangedConnections.add(
bodyItem->sigKinematicStateChanged().connect(
std::bind(&WorldItemImpl::onBodyKinematicStateChanged, this, bodyItem)));
}
collisionDetector->makeReady();
updateCollisions(true);
}