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 +#include #include #include @@ -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: