From b06b5966b2bfef150b26bfc93c778314ba3dccf5 Mon Sep 17 00:00:00 2001
From: Joseph Mirabel <jmirabel@laas.fr>
Date: Tue, 3 Sep 2019 14:59:13 +0200
Subject: [PATCH] [GJK] Avoid normalization for Cylinder, Convex and Cone.
---
src/narrowphase/gjk.cpp | 35 +++++++++++++++++++++++++----------
1 file changed, 25 insertions(+), 10 deletions(-)
diff --git a/src/narrowphase/gjk.cpp b/src/narrowphase/gjk.cpp
index 4cfbf0fb..fa370558 100644
--- a/src/narrowphase/gjk.cpp
+++ b/src/narrowphase/gjk.cpp
@@ -67,6 +67,24 @@ template <> struct shape_traits<Box> : shape_traits_base
};
};
+template <> struct shape_traits<Cone> : shape_traits_base
+{
+ enum { NeedNormalizedDir = false
+ };
+};
+
+template <> struct shape_traits<Cylinder> : shape_traits_base
+{
+ enum { NeedNormalizedDir = false
+ };
+};
+
+template <> struct shape_traits<Convex> : shape_traits_base
+{
+ enum { NeedNormalizedDir = false
+ };
+};
+
void getShapeSupport(const TriangleP* triangle, const Vec3f& dir, Vec3f& support)
{
FCL_REAL dota = dir.dot(triangle->a);
@@ -138,17 +156,14 @@ void getShapeSupport(const Cone* cone, const Vec3f& dir, Vec3f& support)
void getShapeSupport(const Cylinder* cylinder, const Vec3f& dir, Vec3f& support)
{
static const FCL_REAL eps (sqrt(std::numeric_limits<FCL_REAL>::epsilon()));
- FCL_REAL zdist = std::sqrt(dir[0] * dir[0] + dir[1] * dir[1]);
FCL_REAL half_h = cylinder->lz * 0.5;
- if(zdist == 0.0)
- support = Vec3f(0, 0, (dir[2]>0)? half_h:-half_h);
- else {
- FCL_REAL d = cylinder->radius / zdist;
- FCL_REAL z (0.);
- if (dir [2] > eps) z = half_h;
- else if (dir [2] < -eps) z = -half_h;
- support << d * dir.head<2>(), z;
- }
+ if (dir [2] > eps) support[2] = half_h;
+ else if (dir [2] < -eps) support[2] = -half_h;
+ else support[2] = 0;
+ if (dir.head<2>().isZero())
+ support.head<2>().setZero();
+ else
+ support.head<2>() = dir.head<2>().normalized() * cylinder->radius;
assert (fabs (support [0] * dir [1] - support [1] * dir [0]) < eps);
}
--
GitLab