From 2a0633eaac8ab0470e176340b776f6ccd7ae20a7 Mon Sep 17 00:00:00 2001
From: Joseph Mirabel <jmirabel@laas.fr>
Date: Thu, 12 Sep 2019 15:03:23 +0200
Subject: [PATCH] [GJK] Add checks for ill-conditionned directions in support
function.
---
src/narrowphase/gjk.cpp | 8 ++++++++
1 file changed, 8 insertions(+)
diff --git a/src/narrowphase/gjk.cpp b/src/narrowphase/gjk.cpp
index fa370558..9db577c4 100644
--- a/src/narrowphase/gjk.cpp
+++ b/src/narrowphase/gjk.cpp
@@ -248,6 +248,14 @@ void getSupportFuncTpl (const MinkowskiDiff& md,
bool ( (bool)shape_traits<Shape0>::NeedNormalizedDir
|| (bool)shape_traits<Shape1>::NeedNormalizedDir)
};
+#ifndef NDEBUG
+ // Need normalized direction and direction is normalized
+ assert(!NeedNormalizedDir || !dirIsNormalized || fabs(dir.squaredNorm() - 1) < 1e-6);
+ // Need normalized direction but direction is not normalized.
+ assert(!NeedNormalizedDir || dirIsNormalized || fabs(dir.normalized().squaredNorm() - 1) < 1e-6);
+ // Don't need normalized direction. Check that dir is not zero.
+ assert( NeedNormalizedDir || dir.cwiseAbs().maxCoeff() >= 1e-6);
+#endif
getSupportTpl<Shape0, Shape1, TransformIsIdentity> (
static_cast <const Shape0*>(md.shapes[0]),
static_cast <const Shape1*>(md.shapes[1]),
--
GitLab