Commit 856399e5 authored by Tobias Leemann's avatar Tobias Leemann
Browse files

Fix of Circle acceptance in EPA-Algorithm

parent 4be3540c
......@@ -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);
}
......
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment