collision checker clean up: collision model id was not used

VirtualRobot/CollisionDetection/CollisionModel.cpp

@@ -11,10 +11,9 @@
 namespace VirtualRobot
 {
 
-    CollisionModel::CollisionModel(VisualizationNodePtr visu, const std::string& name, CollisionCheckerPtr colChecker, int id, float margin)
+    CollisionModel::CollisionModel(VisualizationNodePtr visu, const std::string& name, CollisionCheckerPtr colChecker, float margin)
     {
         globalPose = Eigen::Matrix4f::Identity();
-        this->id = id;
 
         this->name = name;
         this->margin = margin;
@@ -34,11 +33,11 @@ namespace VirtualRobot
         setVisualization(visu);
     }
 
-    CollisionModel::CollisionModel(VisualizationNodePtr visu, const std::string &name, CollisionCheckerPtr colChecker, int id, InternalCollisionModelPtr collisionModel)
+    CollisionModel::CollisionModel(VisualizationNodePtr visu, const std::string &name, CollisionCheckerPtr colChecker, InternalCollisionModelPtr collisionModel)
     {
         margin = 0.0;
         globalPose = Eigen::Matrix4f::Identity();
-        this->id = id;
+//        this->id = id;
 
         this->name = name;
 
@@ -93,7 +92,7 @@ namespace VirtualRobot
 
 
 #if defined(VR_COLLISION_DETECTION_PQP)
-            collisionModelImplementation.reset(new CollisionModelPQP(model, colChecker, id));
+            collisionModelImplementation.reset(new CollisionModelPQP(model, colChecker));
 #else
             collisionModelImplementation.reset(new CollisionModelDummy(colChecker));
 #endif
@@ -131,14 +130,14 @@ namespace VirtualRobot
             visuOrigNew = origVisualization->clone(deepVisuMesh, scaling);
 
         std::string nameNew = name;
-        int idNew = id;
+
 
         CollisionModelPtr p;
         if(deepVisuMesh || !this->collisionModelImplementation)
-            p.reset(new CollisionModel(visuOrigNew, nameNew, colChecker, idNew, margin));
+            p.reset(new CollisionModel(visuOrigNew, nameNew, colChecker, margin));
         else
         {
-            p.reset(new CollisionModel(visuOrigNew, nameNew, colChecker, idNew, this->collisionModelImplementation));
+            p.reset(new CollisionModel(visuOrigNew, nameNew, colChecker, this->collisionModelImplementation));
             if(visualization)
             {
                 p->visualization = visualization->clone(false, scaling);
@@ -168,7 +167,7 @@ namespace VirtualRobot
 
     int CollisionModel::getId()
     {
-        return id;
+        return collisionModelImplementation->getId();
     }
 
     void CollisionModel::setUpdateVisualization(bool enable)
@@ -395,7 +394,7 @@ namespace VirtualRobot
             TriMeshModelPtr modelScaled = model->clone(scaleFactor);
             bbox = modelScaled->boundingBox;
 #if defined(VR_COLLISION_DETECTION_PQP)
-            collisionModelImplementation.reset(new CollisionModelPQP(modelScaled, colChecker, id));
+            collisionModelImplementation.reset(new CollisionModelPQP(modelScaled, colChecker));
 #else
             collisionModelImplementation.reset(new CollisionModelDummy(colChecker));
 #endif

VirtualRobot/CollisionDetection/CollisionModel.h

@@ -64,7 +64,7 @@ namespace VirtualRobot
             \param colChecker If not specified, the global singleton instance is used. Only useful, when parallel collision checks should be performed.
             \param id A user id.
         */
-        CollisionModel(const VisualizationNodePtr visu, const std::string& name = "", CollisionCheckerPtr colChecker = CollisionCheckerPtr(), int id = 666, float margin = 0.0f);
+        CollisionModel(const VisualizationNodePtr visu, const std::string& name = "", CollisionCheckerPtr colChecker = CollisionCheckerPtr(), float margin = 0.0f);
         /*!Standard Destructor
         */
         virtual ~CollisionModel();
@@ -171,7 +171,7 @@ namespace VirtualRobot
 
     protected:
         // internal constructor needed for flat copy of internal collision model
-        CollisionModel(VisualizationNodePtr visu, const std::string& name, CollisionCheckerPtr colChecker, int id, InternalCollisionModelPtr collisionModel);
+        CollisionModel(VisualizationNodePtr visu, const std::string& name, CollisionCheckerPtr colChecker, InternalCollisionModelPtr collisionModel);
 
         //! delete all data
         void destroyData();
@@ -186,7 +186,7 @@ namespace VirtualRobot
         //! the name
         std::string name;
 
-        int id;
+//        int id;
 
         CollisionCheckerPtr colChecker;
 

VirtualRobot/CollisionDetection/CollisionModelImplementation.h

@@ -27,6 +27,7 @@
 #include <string>
 #include <vector>
 #include <map>
+#include <atomic>
 
 #include <Eigen/Core>
 #include <Eigen/Geometry>
@@ -47,12 +48,19 @@ namespace VirtualRobot
         /*!Standard Constructor
         If collision checks should be done in parallel, different CollisionCheckers can be specified.
         */
-        CollisionModelImplementation(TriMeshModelPtr modelData, CollisionCheckerPtr /*pColChecker*/, int id)
+        CollisionModelImplementation(TriMeshModelPtr modelData, CollisionCheckerPtr /*pColChecker*/)
         {
             this->modelData = modelData;
-            this->id = id;
-        };
 
+            this->id = NextId();
+        }
+
+        static int NextId()
+        {
+            // use globally unique id
+            static std::atomic<int> idCounter{0};
+            return idCounter++;
+        }
         /*!Standard Destructor
         */
         virtual ~CollisionModelImplementation() {}
@@ -74,14 +82,13 @@ namespace VirtualRobot
         virtual void print()
         {
             cout << "Dummy Collision Model Implementation..." << endl;
-        };
+        }
 
         TriMeshModelPtr getTriMeshModel()
         {
             return modelData;
         }
-
-
+        inline int getId() const { return id;}
         virtual boost::shared_ptr<CollisionModelImplementation> clone(bool deepCopy = false) const = 0;
     protected:
 

VirtualRobot/CollisionDetection/PQP/CollisionModelPQP.cpp

@@ -8,28 +8,28 @@
 namespace VirtualRobot
 {
 
-    CollisionModelPQP::CollisionModelPQP(TriMeshModelPtr modelData, CollisionCheckerPtr colChecker, int id)
-        : CollisionModelImplementation(modelData, colChecker, id)
+    CollisionModelPQP::CollisionModelPQP(TriMeshModelPtr modelData, CollisionCheckerPtr colChecker)
+        : CollisionModelImplementation(modelData, colChecker)
     {
-        if (!colChecker)
-        {
-            colChecker = CollisionChecker::getGlobalCollisionChecker();
-        }
-
-        if (!colChecker)
-        {
-            VR_WARNING << "no col checker..." << endl;
-        }
-        else
-        {
-            colCheckerPQP = colChecker->getCollisionCheckerImplementation();
-        }
+//        if (!colChecker)
+//        {
+//            colChecker = CollisionChecker::getGlobalCollisionChecker();
+//        }
+
+//        if (!colChecker)
+//        {
+//            VR_WARNING << "no col checker..." << endl;
+//        }
+//        else
+//        {
+//            colCheckerPQP = colChecker->getCollisionCheckerImplementation();
+//        }
 
         createPQPModel();
     }
 
     CollisionModelPQP::CollisionModelPQP(const CollisionModelPQP &orig) :
-        CollisionModelImplementation(orig.modelData, NULL, orig.id)
+        CollisionModelImplementation(orig.modelData, NULL)
     {
         pqpModel = orig.pqpModel;
     }

VirtualRobot/CollisionDetection/PQP/CollisionModelPQP.h

@@ -51,7 +51,7 @@ namespace VirtualRobot
         /*!Standard Constructor
         Ptr If collision checks should be done in parallel, different CollisionCheckers can be specified.
         */
-        CollisionModelPQP(TriMeshModelPtr modelData, CollisionCheckerPtr colChecker, int id);
+        CollisionModelPQP(TriMeshModelPtr modelData, CollisionCheckerPtr colChecker);
         /*!Standard Destructor
         */
         ~CollisionModelPQP() override;
@@ -72,7 +72,7 @@ namespace VirtualRobot
 
         boost::shared_ptr<PQP::PQP_Model> pqpModel;
 
-        boost::shared_ptr<CollisionCheckerPQP> colCheckerPQP;
+//        boost::shared_ptr<CollisionCheckerPQP> colCheckerPQP;
     };
 
 } // namespace