gjk: normalize guess

coal-library · Jun 24, 2022 · 331c6d6 · 331c6d6
1 parent 27373a2
commit 331c6d6
Showing 1 changed file with 1 addition and 0 deletions.
diff --git a/include/hpp/fcl/narrowphase/narrowphase.h b/include/hpp/fcl/narrowphase/narrowphase.h
@@ -77,6 +77,7 @@ struct HPP_FCL_DLLAPI GJKSolver {
         }
         guess.noalias() =
             s1.aabb_center - (shape.oR1 * s2.aabb_center + shape.ot1);
+        guess.normalize();
         support_hint.setZero();
         break;
       default: