From 4574686c1db0e4cad579bfcec34ec490965ac269 Mon Sep 17 00:00:00 2001
From: Benau <Benau@users.noreply.github.com>
Date: Tue, 13 Sep 2016 12:49:30 +0800
Subject: [PATCH] Make angle computation work in 3D

---
 src/karts/controller/ai_base_controller.cpp | 16 +++++++---------
 src/karts/controller/skidding_ai.cpp        | 16 ++++++++++------
 src/karts/controller/test_ai.cpp            | 16 ++++++++++------
 3 files changed, 27 insertions(+), 21 deletions(-)

diff --git a/src/karts/controller/ai_base_controller.cpp b/src/karts/controller/ai_base_controller.cpp
index ec93e1599..b44c91453 100644
--- a/src/karts/controller/ai_base_controller.cpp
+++ b/src/karts/controller/ai_base_controller.cpp
@@ -86,9 +86,7 @@ float AIBaseController::steerToPoint(const Vec3 &point)
 
     // First translate and rotate the point the AI is aiming
     // at into the kart's local coordinate system.
-    Vec3 lc;
-    btTransform trans = m_kart->getTrans().inverse();
-    lc = trans(point);
+    Vec3 lc = m_kart->getTrans().inverse()(point);
 
     // The point the kart is aiming at can be reached 'incorrectly' if the
     // point is below the y=x line: Instead of aiming at that point directly
@@ -287,10 +285,10 @@ void AIBaseController::checkPosition(const Vec3 &point, posData *pos_data,
                                      Vec3 *lc, bool use_front_xyz) const
 {
     // Convert to local coordinates from the point of view of current kart
-    btQuaternion q(btVector3(0, 1, 0), -m_kart->getHeading());
-    Vec3 p = point -
-        (use_front_xyz ? m_kart->getFrontXYZ() : m_kart->getXYZ());
-    Vec3 local_coordinates = quatRotate(q, p);
+    btTransform t;
+    t.setBasis(m_kart->getTrans().getBasis());
+    t.setOrigin(use_front_xyz ? m_kart->getFrontXYZ() : m_kart->getXYZ());
+    Vec3 local_coordinates = t.inverse()(point);
 
     // Save local coordinates for later use if needed
     if (lc) *lc = local_coordinates;
@@ -308,8 +306,8 @@ void AIBaseController::checkPosition(const Vec3 &point, posData *pos_data,
     else
         pos_data->behind = false;
 
-    pos_data->angle = atan2(fabsf(local_coordinates.getX()),
+    pos_data->angle = atan2f(fabsf(local_coordinates.getX()),
         fabsf(local_coordinates.getZ()));
-    pos_data->distance = p.length();
+    pos_data->distance = local_coordinates.length();
 
 }   //  checkPosition
diff --git a/src/karts/controller/skidding_ai.cpp b/src/karts/controller/skidding_ai.cpp
index 703995587..1e443a56e 100644
--- a/src/karts/controller/skidding_ai.cpp
+++ b/src/karts/controller/skidding_ai.cpp
@@ -1330,15 +1330,19 @@ void SkiddingAI::handleItems(const float dt)
             bool straight_ahead = false;
             if (m_kart_behind)
             {
-                posData behind_pos = {0};
-                checkPosition(m_kart_behind->getXYZ(), &behind_pos);
-                if (behind_pos.angle < 0.2f) straight_behind = true;
+                Vec3 behind_lc = m_kart->getTrans().inverse()
+                    (m_kart_behind->getXYZ());
+                const float abs_angle =
+                    atan2f(fabsf(behind_lc.x()), fabsf(behind_lc.z()));
+                if (abs_angle < 0.2f) straight_behind = true;
             }
             if (m_kart_ahead)
             {
-                posData ahead_pos = {0};
-                checkPosition(m_kart_ahead->getXYZ(), &ahead_pos);
-                if (ahead_pos.angle < 0.2f) straight_ahead = true;
+                Vec3 ahead_lc = m_kart->getTrans().inverse()
+                    (m_kart_ahead->getXYZ());
+                const float abs_angle =
+                    atan2f(fabsf(ahead_lc.x()), fabsf(ahead_lc.z()));
+                if (abs_angle < 0.2f) straight_ahead = true;
             }
 
             // Bowling balls are slower, so only fire on closer karts - but when
diff --git a/src/karts/controller/test_ai.cpp b/src/karts/controller/test_ai.cpp
index 6c8334188..e2004a484 100644
--- a/src/karts/controller/test_ai.cpp
+++ b/src/karts/controller/test_ai.cpp
@@ -1336,15 +1336,19 @@ void SkiddingAI::handleItems(const float dt)
             bool straight_ahead = false;
             if (m_kart_behind)
             {
-                posData behind_pos = {0};
-                checkPosition(m_kart_behind->getXYZ(), &behind_pos);
-                if (behind_pos.angle < 0.2f) straight_behind = true;
+                Vec3 behind_lc = m_kart->getTrans().inverse()
+                    (m_kart_behind->getXYZ());
+                const float abs_angle =
+                    atan2f(fabsf(behind_lc.x()), fabsf(behind_lc.z()));
+                if (abs_angle < 0.2f) straight_behind = true;
             }
             if (m_kart_ahead)
             {
-                posData ahead_pos = {0};
-                checkPosition(m_kart_ahead->getXYZ(), &ahead_pos);
-                if (ahead_pos.angle < 0.2f) straight_ahead = true;
+                Vec3 ahead_lc = m_kart->getTrans().inverse()
+                    (m_kart_ahead->getXYZ());
+                const float abs_angle =
+                    atan2f(fabsf(ahead_lc.x()), fabsf(ahead_lc.z()));
+                if (abs_angle < 0.2f) straight_ahead = true;
             }
 
             // Bowling balls are slower, so only fire on closer karts - but when