Revert "collision checker clean up: collision model id was not used"

This reverts commit 443e4ed5.

VirtualRobot/CollisionDetection/CollisionModel.cpp

@@ -11,9 +11,10 @@
 namespace VirtualRobot
 {
 
-    CollisionModel::CollisionModel(VisualizationNodePtr visu, const std::string& name, CollisionCheckerPtr colChecker, float margin)
+    CollisionModel::CollisionModel(VisualizationNodePtr visu, const std::string& name, CollisionCheckerPtr colChecker, int id, float margin)
     {
         globalPose = Eigen::Matrix4f::Identity();
+        this->id = id;
 
         this->name = name;
         this->margin = margin;
@@ -33,11 +34,11 @@ namespace VirtualRobot
         setVisualization(visu);
     }
 
-    CollisionModel::CollisionModel(VisualizationNodePtr visu, const std::string &name, CollisionCheckerPtr colChecker, InternalCollisionModelPtr collisionModel)
+    CollisionModel::CollisionModel(VisualizationNodePtr visu, const std::string &name, CollisionCheckerPtr colChecker, int id, InternalCollisionModelPtr collisionModel)
     {
         margin = 0.0;
         globalPose = Eigen::Matrix4f::Identity();
-//        this->id = id;
+        this->id = id;
 
         this->name = name;
 
@@ -92,7 +93,7 @@ namespace VirtualRobot
 
 
 #if defined(VR_COLLISION_DETECTION_PQP)
-            collisionModelImplementation.reset(new CollisionModelPQP(model, colChecker));
+            collisionModelImplementation.reset(new CollisionModelPQP(model, colChecker, id));
 #else
             collisionModelImplementation.reset(new CollisionModelDummy(colChecker));
 #endif
@@ -130,14 +131,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, margin));
+            p.reset(new CollisionModel(visuOrigNew, nameNew, colChecker, idNew, margin));
         else
         {
-            p.reset(new CollisionModel(visuOrigNew, nameNew, colChecker, this->collisionModelImplementation));
+            p.reset(new CollisionModel(visuOrigNew, nameNew, colChecker, idNew, this->collisionModelImplementation));
             if(visualization)
             {
                 p->visualization = visualization->clone(false, scaling);
@@ -166,7 +167,7 @@ namespace VirtualRobot
 
     int CollisionModel::getId()
     {
-        return collisionModelImplementation->getId();
+        return id;
     }
 
     void CollisionModel::setUpdateVisualization(bool enable)
@@ -393,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));
+            collisionModelImplementation.reset(new CollisionModelPQP(modelScaled, colChecker, id));
 #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(), float margin = 0.0f);
+        CollisionModel(const VisualizationNodePtr visu, const std::string& name = "", CollisionCheckerPtr colChecker = CollisionCheckerPtr(), int id = 666, 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, InternalCollisionModelPtr collisionModel);
+        CollisionModel(VisualizationNodePtr visu, const std::string& name, CollisionCheckerPtr colChecker, int id, 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,7 +27,6 @@
 #include <string>
 #include <vector>
 #include <map>
-#include <atomic>
 
 #include <Eigen/Core>
 #include <Eigen/Geometry>
@@ -48,19 +47,12 @@ namespace VirtualRobot
         /*!Standard Constructor
         If collision checks should be done in parallel, different CollisionCheckers can be specified.
         */
-        CollisionModelImplementation(TriMeshModelPtr modelData, CollisionCheckerPtr /*pColChecker*/)
+        CollisionModelImplementation(TriMeshModelPtr modelData, CollisionCheckerPtr /*pColChecker*/, int id)
         {
             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() {}
@@ -82,13 +74,14 @@ 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)
-        : CollisionModelImplementation(modelData, colChecker)
+    CollisionModelPQP::CollisionModelPQP(TriMeshModelPtr modelData, CollisionCheckerPtr colChecker, int id)
+        : CollisionModelImplementation(modelData, colChecker, id)
     {
-//        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)
+        CollisionModelImplementation(orig.modelData, NULL, orig.id)
     {
         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);
+        CollisionModelPQP(TriMeshModelPtr modelData, CollisionCheckerPtr colChecker, int id);
         /*!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