From 278454ae17d01a8878dc0d3c7012fc0adabf4c7a Mon Sep 17 00:00:00 2001 From: erwincoumans Date: Fri, 21 Sep 2018 17:33:13 -0700 Subject: [PATCH] allow to disable self-collision for a link-pair --- .../collisionFilterPlugin.cpp | 21 +++++++++++++++++++ 1 file changed, 21 insertions(+) diff --git a/examples/SharedMemory/plugins/collisionFilterPlugin/collisionFilterPlugin.cpp b/examples/SharedMemory/plugins/collisionFilterPlugin/collisionFilterPlugin.cpp index 4e65cfcbc..3ec2a99ca 100644 --- a/examples/SharedMemory/plugins/collisionFilterPlugin/collisionFilterPlugin.cpp +++ b/examples/SharedMemory/plugins/collisionFilterPlugin/collisionFilterPlugin.cpp @@ -62,6 +62,13 @@ struct DefaultPluginCollisionInterface : public b3PluginCollisionInterface b3Swap(keyValue.m_objectUniqueIdA,keyValue.m_objectUniqueIdB); b3Swap(keyValue.m_linkIndexA,keyValue.m_linkIndexB); } + if (objectUniqueIdA==objectUniqueIdB) + { + if (keyValue.m_linkIndexA>keyValue.m_linkIndexB) + { + b3Swap(keyValue.m_linkIndexA,keyValue.m_linkIndexB); + } + } m_customCollisionFilters.insert(keyValue,keyValue); @@ -83,6 +90,13 @@ struct DefaultPluginCollisionInterface : public b3PluginCollisionInterface b3Swap(keyValue.m_objectUniqueIdA,keyValue.m_objectUniqueIdB); b3Swap(keyValue.m_linkIndexA,keyValue.m_linkIndexB); } + if (objectUniqueIdA==objectUniqueIdB) + { + if (keyValue.m_linkIndexA>keyValue.m_linkIndexB) + { + b3Swap(keyValue.m_linkIndexA,keyValue.m_linkIndexB); + } + } m_customCollisionFilters.remove(keyValue); } @@ -116,6 +130,13 @@ struct DefaultPluginCollisionInterface : public b3PluginCollisionInterface b3Swap(keyValue.m_objectUniqueIdA,keyValue.m_objectUniqueIdB); b3Swap(keyValue.m_linkIndexA,keyValue.m_linkIndexB); } + if (objectUniqueIdA==objectUniqueIdB) + { + if (keyValue.m_linkIndexA>keyValue.m_linkIndexB) + { + b3Swap(keyValue.m_linkIndexA,keyValue.m_linkIndexB); + } + } b3CustomCollisionFilter* filter = m_customCollisionFilters.find(keyValue); if (filter)