From 6572885724797b9bf8787a2b20a96cce692fffa9 Mon Sep 17 00:00:00 2001
From: Justin Carpentier <justin.carpentier@inria.fr>
Date: Fri, 24 Jun 2022 17:07:15 +0200
Subject: [PATCH] debug

---
 include/hpp/fcl/narrowphase/narrowphase.h | 8 +++++---
 1 file changed, 5 insertions(+), 3 deletions(-)

diff --git a/include/hpp/fcl/narrowphase/narrowphase.h b/include/hpp/fcl/narrowphase/narrowphase.h
index 5f1a11442..66b11d755 100644
--- a/include/hpp/fcl/narrowphase/narrowphase.h
+++ b/include/hpp/fcl/narrowphase/narrowphase.h
@@ -41,6 +41,7 @@
 #define HPP_FCL_NARROWPHASE_H
 
 #include <limits>
+#include <iostream>
 
 #include <hpp/fcl/narrowphase/gjk.h>
 #include <hpp/fcl/collision_data.h>
@@ -75,9 +76,10 @@ struct HPP_FCL_DLLAPI GJKSolver {
               "GJKInitialGuess::BoundingVolumeGuess.",
               std::logic_error);
         }
-        guess.noalias() =
-            s1.aabb_center - (shape.oR1 * s2.aabb_center + shape.ot1);
-        guess.normalize();
+        std::cout << "guess: " << guess.transpose() << std::endl;
+        guess.noalias() = s1.aabb_local.center() -
+                          (shape.oR1 * s2.aabb_local.center() + shape.ot1);
+        //        guess.normalize();
         support_hint.setZero();
         break;
       default: