Check valid orientation when adding agents

TODO: Check for invalid orientations from user during simulation
PedestrianDynamics · Jan 21, 2025 · dc21dfe · dc21dfe
1 parent 99b7d9b
commit dc21dfe
Show file tree

Hide file tree

Showing 3 changed files with 22 additions and 20 deletions.
diff --git a/libsimulator/src/Point.cpp b/libsimulator/src/Point.cpp
@@ -6,6 +6,15 @@
 #include <Logger.hpp>
 #include <limits>
 
+bool Point::isInvalidOrientation() const
+{
+    if((std::abs(x) < 1e-6 && std::abs(y) < 1e-6) || (std::abs(NormSquare() - 1.0) > 1e-6)) {
+        LOG_WARNING("Orientation vector ({}, {}) is not normalized, correcting it.", x, y);
+        return true;
+    }
+    return false;
+}
+
 double Point::Norm() const
 {
     return sqrt(NormSquare());
@@ -40,14 +49,6 @@ std::tuple<double, Point> Point::NormAndNormalized() const
  */
 Point Point::TransformToEllipseCoordinates(const Point& center, double cphi, double sphi) const
 {
-    // if((std::abs(cphi) < 1e-6 && std::abs(sphi) < 1e-6) ||
-    //    std::abs(cphi * cphi + sphi * sphi - 1.0) > 1e-6) {
-    //     throw std::invalid_argument(
-    //         "Invalid rotation inputs: vector is zero or not normalized (cphi^2 + sphi^2 != 1). "
-    //         "Values: cphi=" +
-    //         std::to_string(cphi) + ", sphi=" + std::to_string(sphi));
-    // }
-
     Point p = Point(x, y);
     return (p - center).Rotate(cphi, -sphi);
 }
@@ -67,13 +68,6 @@ Point Point::TransformToEllipseCoordinates(const Point& center, double cphi, dou
  */
 Point Point::TransformToCartesianCoordinates(const Point& center, double cphi, double sphi) const
 {
-    // if((std::abs(cphi) < 1e-6 && std::abs(sphi) < 1e-6) ||
-    //    std::abs(cphi * cphi + sphi * sphi - 1.0) > 1e-6) {
-    //     throw std::invalid_argument(
-    //         "Invalid rotation inputs: vector is zero or not normalized (cphi^2 + sphi^2 != 1). "
-    //         "Values: cphi=" +
-    //         std::to_string(cphi) + ", sphi=" + std::to_string(sphi));
-    // }
 
     Point p = Point(x, y);
     return (p.Rotate(cphi, sphi) + center);

diff --git a/libsimulator/src/Point.hpp b/libsimulator/src/Point.hpp
@@ -17,6 +17,8 @@ class Point
 public:
     Point(double x = 0, double y = 0) : x(x), y(y) {};
 
+    bool isInvalidOrientation() const;
+
     /// Norm
     double Norm() const;
 

diff --git a/libsimulator/src/Simulation.cpp b/libsimulator/src/Simulation.cpp
@@ -8,6 +8,7 @@
 #include "OperationalModel.hpp"
 #include "Stage.hpp"
 #include "Visitor.hpp"
+#include <Logger.hpp>
 
 #include <memory>
 #include <variant>
@@ -177,6 +178,7 @@ BaseStage::ID Simulation::AddStage(const StageDescription stageDescription)
 
 GenericAgent::ID Simulation::AddAgent(GenericAgent&& agent)
 {
+
     if(!_geometry->InsideGeometry(agent.pos)) {
         throw SimulationError("Agent {} not inside walkable area", agent.pos);
     }
@@ -186,13 +188,17 @@ GenericAgent::ID Simulation::AddAgent(GenericAgent&& agent)
 
     Point targetPoint = std::get<0>(_journeys.at(agent.journeyId)->Target(agent));
     if((targetPoint - agent.pos).Norm() < 1e-6) {
-        std::cerr << "Warning: Agent's position " << agent.pos.x << ", " << agent.pos.y
-                  << " is the same as the target " << targetPoint.x << ", " << targetPoint.y
-                  << ". Setting default direction to (1, 0)." << std::endl;
-
+        LOG_WARNING(
+            "Agent's position ({}, {})  is the same as the target ({}, {}). Set defaul orientation "
+            "to (1,0)",
+            agent.pos.x,
+            agent.pos.y,
+            targetPoint.x,
+            targetPoint.y);
         agent.orientation = Point(1.0, 0.0);
     }
-    if(std::abs(agent.orientation.x) < 1e-6 && std::abs(agent.orientation.y) < 1e-6) {
+
+    if(agent.orientation.isInvalidOrientation()) {
         Point direction = (targetPoint - agent.pos).Normalized();
         agent.orientation = direction;
     }