From 64faac97454161a229233b3e35e9867fa9953e8f Mon Sep 17 00:00:00 2001
From: Sebastian Eibl <sebastian.eibl@fau.de>
Date: Mon, 12 Mar 2018 15:13:21 +0100
Subject: [PATCH] made sphere optimization for EPA optional
---
.../pe/rigid_body/ConvexPolyhedronFactory.cpp | 2 -
src/pe/collision/EPA.cpp | 99 ++++++++++---------
src/pe/collision/EPA.h | 10 ++
src/pe/fcd/GJKEPACollideFunctor.h | 1 +
tests/pe/CMakeLists.txt | 2 +-
tests/pe/CollisionTobiasGJK.cpp | 1 +
6 files changed, 66 insertions(+), 49 deletions(-)
diff --git a/src/mesh/pe/rigid_body/ConvexPolyhedronFactory.cpp b/src/mesh/pe/rigid_body/ConvexPolyhedronFactory.cpp
index 959215e94..9919e2f6a 100644
--- a/src/mesh/pe/rigid_body/ConvexPolyhedronFactory.cpp
+++ b/src/mesh/pe/rigid_body/ConvexPolyhedronFactory.cpp
@@ -103,8 +103,6 @@ ConvexPolyhedronID createConvexPolyhedron( BodyStorage& globalStorage, BlockStor
"Created ConvexPolyhedron " << poly->getSystemID() << "\n"
<< " User-ID = " << uid << "\n"
<< " Global position = " << gpos << "\n"
- << " side length = " << lengths << "\n"
- << " LinVel = " << box->getLinearVel() << "\n"
<< " Material = " << Material::getName( material )
);
}
diff --git a/src/pe/collision/EPA.cpp b/src/pe/collision/EPA.cpp
index 2e0d0e889..92d78ff85 100644
--- a/src/pe/collision/EPA.cpp
+++ b/src/pe/collision/EPA.cpp
@@ -119,7 +119,7 @@ inline bool EPA::EPA_Triangle::link( size_t edge0, EPA_Triangle* tria, size_t ed
tria->adjEdges_[edge1] = edge0;
bool b = indices_[edge0] == tria->indices_[(edge1+1)%3] &&
- indices_[(edge0+1)%3] == tria->indices_[edge1];
+ indices_[(edge0+1)%3] == tria->indices_[edge1];
return b;
}
//*************************************************************************************************
@@ -145,7 +145,7 @@ inline void EPA::EPA_Triangle::silhouette( const Vec3& w, EPA_EdgeBuffer& edgeBu
/*! \brief Recursive silhuette finding method.
*/
void EPA::EPA_Triangle::silhouette( size_t index, const Vec3& w,
- EPA_EdgeBuffer& edgeBuffer )
+ EPA_EdgeBuffer& edgeBuffer )
{
if (!obsolete_) {
real_t test = (closest_ * w);
@@ -184,7 +184,7 @@ const long double EpsilonRelEPA< long double >::value = static_cast< long double
/*! \brief Does an EPA computation with contactthreshold added. Use Default relative Error.
*/
bool EPA::doEPAcontactThreshold( GeomPrimitive &geom1, GeomPrimitive &geom2, const GJK& gjk, Vec3& retNormal,
- Vec3& contactPoint, real_t& penetrationDepth){
+ Vec3& contactPoint, real_t& penetrationDepth){
//Default relative epsilon
return doEPA(geom1, geom2, gjk, retNormal, contactPoint, penetrationDepth, contactThreshold, EpsilonRelEPA<real_t>::value);
@@ -196,7 +196,7 @@ bool EPA::doEPAcontactThreshold( GeomPrimitive &geom1, GeomPrimitive &geom2, con
/*! \brief Does an EPA computation with contactThreshold added. Relative Error can be specified.
*/
bool EPA::doEPAcontactThreshold( GeomPrimitive &geom1, GeomPrimitive &geom2, const GJK& gjk, Vec3& retNormal,
- Vec3& contactPoint, real_t& penetrationDepth, real_t eps_rel){
+ Vec3& contactPoint, real_t& penetrationDepth, real_t eps_rel){
return doEPA(geom1, geom2, gjk, retNormal, contactPoint, penetrationDepth, contactThreshold, eps_rel);
}
@@ -207,7 +207,7 @@ bool EPA::doEPAcontactThreshold( GeomPrimitive &geom1, GeomPrimitive &geom2, con
/*! \brief Does an EPA computation with margin added. Use Default relative Error.
*/
bool EPA::doEPAmargin( GeomPrimitive &geom1, GeomPrimitive &geom2, const GJK& gjk, Vec3& retNormal,
- Vec3& contactPoint, real_t& penetrationDepth, real_t margin){
+ Vec3& contactPoint, real_t& penetrationDepth, real_t margin){
//Default relative epsilon
return doEPA(geom1, geom2, gjk, retNormal, contactPoint, penetrationDepth, margin, EpsilonRelEPA<real_t>::value);
}
@@ -218,7 +218,7 @@ bool EPA::doEPAmargin( GeomPrimitive &geom1, GeomPrimitive &geom2, const GJK& gj
/*! \brief Does an epa computation with contact margin added and specified realtive error.
*/
bool EPA::doEPA( GeomPrimitive &geom1, GeomPrimitive &geom2, const GJK& gjk, Vec3& retNormal,
- Vec3& contactPoint, real_t& penetrationDepth, real_t margin, real_t eps_rel )
+ Vec3& contactPoint, real_t& penetrationDepth, real_t margin, real_t eps_rel )
{
//have in mind that we use a support mapping which blows up the objects a wee bit so
//zero penetraion aka toching contact means that the original bodies have a distance of 2*margin between them
@@ -271,8 +271,10 @@ bool EPA::doEPA( GeomPrimitive &geom1, GeomPrimitive &geom2, const GJK& gjk, Vec
std::make_heap(entryHeap.begin(), entryHeap.end(), EPA::EPA_TriangleComp());
EPA_Triangle* current = NULL;
+ numIterations_ = 0;
//EPA Main-Loop
do {
+ ++numIterations_;
std::pop_heap(entryHeap.begin(), entryHeap.end(), EPA::EPA_TriangleComp());
current = entryHeap.back();
entryHeap.pop_back();
@@ -310,38 +312,43 @@ bool EPA::doEPA( GeomPrimitive &geom1, GeomPrimitive &geom2, const GJK& gjk, Vec
//std::cerr << "New upper bound: " << farDist*farDist << std::endl;
upperBoundSqr = std::min(upperBoundSqr, farDist*farDist);
+
+
//Try to approximate the new surface with a sphere
- Vec3 ctr;
- real_t radius2 = calculateCircle(support, epaVolume[(*current)[0]],
- epaVolume[(*current)[1]], 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;
- 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%
- if(circle_dist*circle_dist <= upperBoundSqr && circle_dist*circle_dist >= lowerBoundSqr &&
- (circle_dist*circle_dist)/lowerBoundSqr < real_t(1.10) && !floatIsEqual(center_len, real_t(0.0))) {
-
- ctr = -1*ctr.getNormalized();
- //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
-
- contactPoint = real_t(0.5) * (supportA.back() + supportB.back());
- penetrationDepth = -support.length()+ real_t(2.0) * margin;
- retNormal = -ctr;
- //std::cerr << "Found penetration depth " << penetrationDepth << " with CurvedEPA!" << std::endl;
- if(penetrationDepth < contactThreshold){
- return true;
- }else{
- return false;
- }
- } else { //Reject sphere
- removeSupportMargin(epaVolume, supportA, supportB);
+ if (bUseSphereOptimization_)
+ {
+ Vec3 ctr;
+ real_t radius2 = calculateCircle(support, epaVolume[(*current)[0]],
+ epaVolume[(*current)[1]], 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;
+ 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%
+ if(circle_dist*circle_dist <= upperBoundSqr && circle_dist*circle_dist >= lowerBoundSqr &&
+ (circle_dist*circle_dist)/lowerBoundSqr < real_t(1.10) && !floatIsEqual(center_len, real_t(0.0))) {
+
+ ctr = -1*ctr.getNormalized();
+ //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
+
+ contactPoint = real_t(0.5) * (supportA.back() + supportB.back());
+ penetrationDepth = -support.length()+ real_t(2.0) * margin;
+ retNormal = -ctr;
+ //std::cerr << "Found penetration depth " << penetrationDepth << " with CurvedEPA!" << std::endl;
+ if(penetrationDepth < contactThreshold){
+ return true;
+ }else{
+ return false;
+ }
+ } else { //Reject sphere
+ removeSupportMargin(epaVolume, supportA, supportB);
+ support = epaVolume.back();
+ }
}
}
}
@@ -853,15 +860,15 @@ inline bool EPA::searchTetrahedron(GeomPrimitive &geom1, GeomPrimitive &geom2, s
*/
inline real_t EPA::calculateCircle(const Vec3& A, const Vec3& B, const Vec3& C,
const Vec3& D, Vec3& center ){
- real_t l1, l2, l3, d1, d2, d3;
- l1 = (A-B).length(); /* These three sqrt evaluations are necessary */
- l2 = (A-C).length();
- l3 = (A-D).length();
+ real_t il1, il2, il3, d1, d2, d3;
+ il1 = real_t(1.0)/(A-B).length(); /* inverse These three sqrt evaluations are necessary */
+ il2 = real_t(1.0)/(A-C).length();
+ il3 = real_t(1.0)/(A-D).length();
Vec3 n1, n2, n3;
- n1 = (real_t(1.0)/l1)*(A-B);
- n2 = (real_t(1.0)/l2)*(A-C);
- n3 = (real_t(1.0)/l3)*(A-D);
+ n1 = il1*(A-B);
+ n2 = il2*(A-C);
+ n3 = il3*(A-D);
// Here we already see if such circle exists.
real_t det = n1 * (n2 % n3);
@@ -870,9 +877,9 @@ inline real_t EPA::calculateCircle(const Vec3& A, const Vec3& B, const Vec3& C,
return real_t(-1.0);
}
real_t Alen = A.sqrLength();
- d1 = (Alen - B.sqrLength())/(real_t(2.0)*l1);
- d2 = (Alen - C.sqrLength())/(real_t(2.0)*l2);
- d3 = (Alen - D.sqrLength())/(real_t(2.0)*l3);
+ d1 = (Alen - B.sqrLength())*real_t(0.5)*il1;
+ d2 = (Alen - C.sqrLength())*real_t(0.5)*il2;
+ d3 = (Alen - D.sqrLength())*real_t(0.5)*il3;
//Apply solution formula
center = (real_t(1.0)/det)*(d1 * (n2 % n3) + d2 * (n3 % n1) + d3 * (n1 % n2));
diff --git a/src/pe/collision/EPA.h b/src/pe/collision/EPA.h
index da611ecae..a318e7e57 100644
--- a/src/pe/collision/EPA.h
+++ b/src/pe/collision/EPA.h
@@ -86,6 +86,8 @@ public:
bool doEPA( GeomPrimitive &geom1, GeomPrimitive &geom2, const GJK& gjk, Vec3& normal,
Vec3& contactPoint, real_t& penetrationDepth, real_t margin, real_t eps_rel );
+
+
//@}
//**********************************************************************************************
@@ -100,6 +102,11 @@ public:
inline size_t getMaxTriangles() {return maxTriangles_;}
+ inline int getNumIterations() const {return numIterations_; }
+
+ inline bool useSphereOptimization() const {return bUseSphereOptimization_; }
+ inline void useSphereOptimization(const bool useIt) {bUseSphereOptimization_ = useIt;}
+
//@}
//**********************************************************************************************
@@ -143,6 +150,9 @@ private:
//EPA constants
size_t maxSupportPoints_ = 100;
size_t maxTriangles_ = 200;
+
+ int numIterations_ = 0;
+ bool bUseSphereOptimization_ = false;
};
//*************************************************************************************************
diff --git a/src/pe/fcd/GJKEPACollideFunctor.h b/src/pe/fcd/GJKEPACollideFunctor.h
index 84b6d62e4..35a72fcac 100644
--- a/src/pe/fcd/GJKEPACollideFunctor.h
+++ b/src/pe/fcd/GJKEPACollideFunctor.h
@@ -115,6 +115,7 @@ namespace gjkepa{
if(gjk.doGJKmargin(*a, *b, margin)){
//2. If collision is possible perform EPA.
EPA epa;
+ epa.useSphereOptimization(true);
if(epa.doEPAmargin(*a, *b, gjk, normal, contactPoint, penetrationDepth, margin)){
contacts_.push_back( Contact(a, b, contactPoint, normal, penetrationDepth) );
return true;
diff --git a/tests/pe/CMakeLists.txt b/tests/pe/CMakeLists.txt
index ce10e244c..b29ec6940 100644
--- a/tests/pe/CMakeLists.txt
+++ b/tests/pe/CMakeLists.txt
@@ -22,7 +22,7 @@ waLBerla_execute_test( NAME PE_CHECKVITALPARAMETERS )
waLBerla_compile_test( NAME PE_COLLISION FILES Collision.cpp DEPENDS core )
waLBerla_execute_test( NAME PE_COLLISION )
-waLBerla_compile_test( NAME PE_COLLISIONTOBIASGJK FILES CollisionTobiasGJK.cpp DEPENDS core )
+waLBerla_compile_test( NAME PE_COLLISIONTOBIASGJK FILES CollisionTobiasGJK.cpp DEPENDS core )
waLBerla_execute_test( NAME PE_COLLISIONTOBIASGJK )
waLBerla_compile_test( NAME PE_CREATEWORLD FILES CreateWorld.cpp DEPENDS core )
diff --git a/tests/pe/CollisionTobiasGJK.cpp b/tests/pe/CollisionTobiasGJK.cpp
index e800d11f8..bed8bc4ce 100644
--- a/tests/pe/CollisionTobiasGJK.cpp
+++ b/tests/pe/CollisionTobiasGJK.cpp
@@ -63,6 +63,7 @@ bool gjkEPAcollideHybrid(GeomPrimitive &geom1, GeomPrimitive &geom2, Vec3& norma
//2. If collision is possible perform EPA.
//std::cerr << "Peforming EPA.";
EPA epa;
+ epa.useSphereOptimization( true );
return epa.doEPAmargin(geom1, geom2, gjk, normal, contactPoint, penetrationDepth, margin);
}else{
return false;
--
GitLab