From 4b2621b41cc52ab973e56ae540678333d86189e1 Mon Sep 17 00:00:00 2001
From: Tobias Leemann <tobias.leemann@fau.de>
Date: Tue, 8 Jan 2019 11:21:19 +0100
Subject: [PATCH] Fix of circle Acceptance in EPA
---
src/pe/collision/EPA.cpp | 5 ++---
1 file changed, 2 insertions(+), 3 deletions(-)
diff --git a/src/pe/collision/EPA.cpp b/src/pe/collision/EPA.cpp
index 119e79373..8e93e89a5 100644
--- a/src/pe/collision/EPA.cpp
+++ b/src/pe/collision/EPA.cpp
@@ -337,11 +337,10 @@ bool EPA::doEPA( GeomPrimitive &geom1, GeomPrimitive &geom2, const GJK& gjk, Vec
{
const auto ilen = real_t(1.0) / center_len;
ctr *= -ilen;
- // std::cerr << "New support direction: " << ctr << std::endl;
pushSupportMargin(geom1, geom2, ctr, margin, epaVolume, supportA, supportB);
support = epaVolume.back();
// Check if support is in expected direction
- real_t supp_dist = support.length()
+ real_t supp_dist = support.length();
if(floatIsEqual((support % ctr).sqrLength()/support.sqrLength(), real_t(0.0)) &&
supp_dist*supp_dist <= upperBoundSqr &&
supp_dist*supp_dist >= lowerBoundSqr)
@@ -350,7 +349,7 @@ bool EPA::doEPA( GeomPrimitive &geom1, GeomPrimitive &geom2, const GJK& gjk, Vec
contactPoint = real_t(0.5) * (supportA.back() + supportB.back());
penetrationDepth = -supp_dist + real_t(2.0) * margin;
retNormal = -ctr;
- // std::cerr << "Found penetration depth " << penetrationDepth << " with CurvedEPA!" << std::endl;
+
if(penetrationDepth < contactThreshold){
return true;
}else{
--
GitLab