cpp: libraries: fix ObstacleCircle nearest_point() method

As the bounding box margin is now an offset instead of a coefficient,
the nearest point on the circle is computed very far, even outside the
table.
Ensure the projected point remains on the circle boundary.
Also handle the special case where the input point is at the circle
center.

Signed-off-by: Gilles DOFFE <[email protected]>

cogip/cpp/libraries/avoidance/Avoidance.cpp

@@ -58,6 +58,7 @@ bool Avoidance::avoidance(const models::Coords& start,
         }
         if (current_obstacle.is_point_inside(start_pose_)) {
             start_pose_ = current_obstacle.nearest_point(start_pose_);
+            logger_.debug() << "start pose inside obstacle, updated: " << start_pose_ << std::endl;
         }
     }
 

cogip/cpp/libraries/obstacles/ObstacleCircle.cpp

@@ -83,11 +83,26 @@ bool ObstacleCircle::is_segment_crossing(const models::Coords& a, const models::
 
 models::Coords ObstacleCircle::nearest_point(const models::Coords& p)
 {
+    // Vector from the circle center to the given point
     models::Coords vect(p.x() - data_->center.x, p.y() - data_->center.y);
     double vect_norm = std::hypot(vect.x(), vect.y());
 
-    double scale = (data_->radius * (1 + data_->bounding_box_margin)) / vect_norm;
+    // Effective radius including the bounding box margin
+    double effective_radius = data_->radius + data_->bounding_box_margin;
 
+    // Special case: if the point is exactly at the center, return a point on the circle
+    // This case should never
+    if (vect_norm == 0) {
+        return models::Coords(
+            data_->center.x + effective_radius,
+            data_->center.y
+        );
+    }
+
+    // Scale the vector to project the point onto the circle perimeter
+    double scale = effective_radius / vect_norm;
+
+    // Return the projected point on the circle
     return models::Coords(
         data_->center.x + vect.x() * scale,
         data_->center.y + vect.y() * scale