fix: Fix errors in safety projected circle calculation

VirtualRobot/Safety.h

@@ -19,7 +19,7 @@ namespace VirtualRobot
     inline Circle projectedBoundingCircle(const Robot& robot)
     {
         // MathTools::createConvexHull2D(const std::vector<Eigen::Vector2f> &points)
-        std::vector<Eigen::Vector2f> nodePositions;
+        std::vector<Eigen::Vector3f> nodePositions;
 
         const auto nodes = robot.getRobotNodes();
 
@@ -27,19 +27,28 @@ namespace VirtualRobot
                        nodes.end(),
                        std::back_inserter(nodePositions),
                        [](const RobotNodePtr& node)
-                       { return node->getPositionInRootFrame().head<2>(); });
+                       { return node->getPositionInRootFrame(); });
 
-        const MathTools::ConvexHull2DPtr hull =
-            MathTools::createConvexHull2D(nodePositions);
+        //const MathTools::ConvexHull2DPtr hull =
+        //    MathTools::createConvexHull2D(nodePositions);
 
         std::vector<float> distances;
 
-        std::transform(hull->vertices.begin(),
-                       hull->vertices.end(),
+        std::transform(nodePositions.begin(),
+                       nodePositions.end(),
                        std::back_inserter(distances),
-                       [](const Eigen::Vector2f& pos) { return pos.norm(); });
-
-        const float maxDistance = *std::max(distances.begin(), distances.end());
+                       [](const Eigen::Vector3f& pos) { return Eigen::Vector2f(pos.x(), pos.y()).norm(); });
+
+        float maxDistance = -std::numeric_limits<float>::infinity();
+        for (const auto distance : distances)
+        {
+            if (distance > maxDistance)
+            {
+                maxDistance = distance;
+            }
+        }
+
+        //const float maxDistance = *std::max(distances.begin(), distances.end());
         return Circle{.center = Eigen::Vector2f::Zero(), .radius = maxDistance};
     }
-} // namespace VirtualRobot
\ No newline at end of file
+} // namespace VirtualRobot