From 856399e5f5c23b5cd5cdd4ff2a7c511cd28b8c6d Mon Sep 17 00:00:00 2001
From: Tobias Leemann <tobias.leemann@fau.de>
Date: Sat, 22 Dec 2018 17:36:54 +0100
Subject: [PATCH] Fix of Circle acceptance in EPA-Algorithm
---
src/pe/collision/EPA.cpp | 28 +++++++++++++++++-----------
1 file changed, 17 insertions(+), 11 deletions(-)
diff --git a/src/pe/collision/EPA.cpp b/src/pe/collision/EPA.cpp
index b2738d903..119e79373 100644
--- a/src/pe/collision/EPA.cpp
+++ b/src/pe/collision/EPA.cpp
@@ -324,34 +324,40 @@ bool EPA::doEPA( GeomPrimitive &geom1, GeomPrimitive &geom2, const GJK& gjk, Vec
epaVolume[(*current)[2]],
ctr);
if(radius2 > real_t(0.0)){ //if a Sphere exists
- //std::cerr << "Circle created with center at " << ctr << ". r2=" << radius2 << std::endl;
+ // std::cerr << "Circle created with center at " << ctr << ". r2=" << radius2 << std::endl;
real_t center_len = ctr.length();
real_t circle_dist = (std::sqrt(radius2) - center_len); //Distance from center to the spheres surface
- //Check if the circle matches the bounds given by EPA and limit max error to ca. 5%
+
+ // Check if the circle matches the bounds given by EPA and limit max error to ca. 5%
if (circle_dist*circle_dist <= upperBoundSqr &&
circle_dist*circle_dist >= lowerBoundSqr &&
(circle_dist*circle_dist) < real_t(1.10) * lowerBoundSqr &&
- !floatIsEqual(center_len, real_t(0.0)))
+ !floatIsEqual(center_len, real_t(0.0)) &&
+ circle_dist > real_t(0.0)) // In case of numerical errors, this can be the case
{
const auto ilen = real_t(1.0) / center_len;
ctr *= -ilen;
- //std::cerr << "New support direction: " << ctr << std::endl;
+ // 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
-
- if(floatIsEqual((support % ctr).sqrLength()/support.sqrLength(), real_t(0.0)))
- { //Accept sphere
+ 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)
+ {
+ //Accept sphere
contactPoint = real_t(0.5) * (supportA.back() + supportB.back());
- penetrationDepth = -support.length() + real_t(2.0) * margin;
+ penetrationDepth = -supp_dist + real_t(2.0) * margin;
retNormal = -ctr;
- //std::cerr << "Found penetration depth " << penetrationDepth << " with CurvedEPA!" << std::endl;
+ // std::cerr << "Found penetration depth " << penetrationDepth << " with CurvedEPA!" << std::endl;
if(penetrationDepth < contactThreshold){
return true;
}else{
return false;
}
- } else { //Reject sphere
+ } else {
+ //Reject sphere
removeSupportMargin(epaVolume, supportA, supportB);
support = epaVolume.back();
}
@@ -872,7 +878,7 @@ inline real_t EPA::calculateCircle(const Vec3& A, const Vec3& B, const Vec3& C,
// Here we already see if such circle exists.
const real_t det = n1 * (n2 % n3);
- if(std::fabs(det) < math::Limits<real_t>::fpuAccuracy()){
+ if(floatIsEqual(det, real_t(0.0))){
//no circle exists. Leave center untouched, and return -1.0
return real_t(-1.0);
}
--
GitLab