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)