From a9358f6f32a573fb7db1ce7e773b6e63ed6dc3d5 Mon Sep 17 00:00:00 2001
From: Pimpest <82343504+Pimpest@users.noreply.github.com>
Date: Sun, 27 Apr 2025 20:24:01 +0200
Subject: [PATCH] Change the way dwa_point behaves if goal is behind

---
 mg_navigation/include/handlers/dwa_point.hpp | 8 +++++---
 1 file changed, 5 insertions(+), 3 deletions(-)

diff --git a/mg_navigation/include/handlers/dwa_point.hpp b/mg_navigation/include/handlers/dwa_point.hpp
index 145af60..42024db 100644
--- a/mg_navigation/include/handlers/dwa_point.hpp
+++ b/mg_navigation/include/handlers/dwa_point.hpp
@@ -30,6 +30,7 @@ namespace mg {
         double       n_theta = NAN;
         double       score   = 0;
 
+        // The angle we will be facing after lookahead
         n_theta = w * delta;
 
         if (n_theta <= 1e-6) { //NOLINT
@@ -47,11 +48,12 @@ namespace mg {
         const glm::dvec2 n_face = glm::rotate(glm::dvec2(1, 0), n_theta);
         const glm::dvec2 face   = glm::rotate(glm::dvec2(1, 0), theta);
 
-        const double angl   = glm::angle(face, glm::normalize(target_pos - pos));
-        const double n_angl = glm::angle(n_face, glm::normalize(target_pos - n_pos));
+        // Calculate angle to goal post/pre movement
+        const double angl   = glm::asin(glm::determinant(glm::dmat2{ face, glm::normalize(target_pos - pos) }));
+        const double n_angl = glm::asin(glm::determinant(glm::dmat2{ n_face, glm::normalize(target_pos - n_pos) }));
 
         score += goal->pos_mult * (glm::distance(target_pos, pos) - glm::distance(target_pos, n_pos));
-        score += goal->ornt_mult * (angl - n_angl);
+        score += goal->ornt_mult * (glm::abs(angl) - glm::abs(n_angl));
 
         return score;
     }
-- 
2.49.0