diff --git a/apps/benchmarks/GranularGas/Accessor.h b/apps/benchmarks/GranularGas/Accessor.h
index 13667332c7553c48142c776ef6fc00be7e4ac065..1d34ce30eb3530257c916014fb46edc915737a1f 100644
--- a/apps/benchmarks/GranularGas/Accessor.h
+++ b/apps/benchmarks/GranularGas/Accessor.h
@@ -33,13 +33,8 @@ public:
, ss_(ss)
{}
- const walberla::real_t& getInvMass(const size_t p_idx) const {return ss_->shapes[ps_->getShapeIDRef(p_idx)]->getInvMass();}
- walberla::real_t& getInvMassRef(const size_t p_idx) {return ss_->shapes[ps_->getShapeIDRef(p_idx)]->getInvMass();}
- void setInvMass(const size_t p_idx, const walberla::real_t& v) { ss_->shapes[ps_->getShapeIDRef(p_idx)]->getInvMass() = v;}
-
+ const auto& getInvMass(const size_t p_idx) const {return ss_->shapes[ps_->getShapeIDRef(p_idx)]->getInvMass();}
const auto& getInvInertiaBF(const size_t p_idx) const {return ss_->shapes[ps_->getShapeIDRef(p_idx)]->getInvInertiaBF();}
- auto& getInvInertiaBFRef(const size_t p_idx) {return ss_->shapes[ps_->getShapeIDRef(p_idx)]->getInvInertiaBF();}
- void setInvInertiaBF(const size_t p_idx, const Mat3& v) { ss_->shapes[ps_->getShapeIDRef(p_idx)]->getInvInertiaBF() = v;}
data::BaseShape* getShape(const size_t p_idx) const {return ss_->shapes[ps_->getShapeIDRef(p_idx)].get();}
private:
diff --git a/python/mesa_pd.py b/python/mesa_pd.py
index d356d40abd4a2b125008f67fbd1b4e248ae97a4d..401fc3bc7b00f0870367758a68db435e5afa727c 100755
--- a/python/mesa_pd.py
+++ b/python/mesa_pd.py
@@ -27,7 +27,7 @@ if __name__ == '__main__':
os.makedirs(args.path + "/src/mesa_pd/mpi/notifications", exist_ok = True)
os.makedirs(args.path + "/src/mesa_pd/vtk", exist_ok = True)
- shapes = ["Sphere", "HalfSpace", "CylindricalBoundary", "Box"]
+ shapes = ["Sphere", "HalfSpace", "CylindricalBoundary", "Box", "Ellipsoid"]
ps = data.ParticleStorage()
ch = data.ContactHistory()
diff --git a/src/core/debug/CheckFunctions.impl.h b/src/core/debug/CheckFunctions.impl.h
index d8f7e384cf0539a2533fcb563c7ab049afa2a705..023d5f7d3fa48d2920d7ba26444453931502572c 100644
--- a/src/core/debug/CheckFunctions.impl.h
+++ b/src/core/debug/CheckFunctions.impl.h
@@ -351,7 +351,7 @@ void check_float_equal( const T & lhs, const U & rhs,
<< "File: " << filename << ":" << line << '\n'
<< "Expression: " << lhsExpression << " == " << rhsExpression << '\n'
//<< "ULP: " << distance << '\n'
- << "Values: " << std::setw(length) << std::setfill(' ') << lhsExpression << " = ";
+ << "Values: " << std::setw(length) << std::setfill(' ') << lhsExpression << " = ";
printValue( ss, lhs ) << '\n';
diff --git a/src/core/math/Random.h b/src/core/math/Random.h
index d63d2e7ede6338606219e743d7fc44444ca23010..3ae4dc747b5f01f810e7a44cb6e7c5e9b79e37de 100644
--- a/src/core/math/Random.h
+++ b/src/core/math/Random.h
@@ -158,7 +158,7 @@ private:
* \brief Returns a random floating point number of type REAL in the range [min,max) (max excluded!)
*/
//**********************************************************************************************************************
-template< typename REAL >
+template< typename REAL = real_t >
REAL realRandom( const REAL min = REAL(0), const REAL max = REAL(1), std::mt19937 & generator = internal::getGenerator() )
{
static_assert( std::numeric_limits<REAL>::is_specialized && !std::numeric_limits<REAL>::is_integer, "Floating point type required/expected!" );
diff --git a/src/core/math/Utility.h b/src/core/math/Utility.h
index de5f00f75c5caf9e0d975926db3584625f712124..b71ea90e07522396b798cef116fc6af8e76d5efb 100644
--- a/src/core/math/Utility.h
+++ b/src/core/math/Utility.h
@@ -84,6 +84,8 @@ inline real_t round( real_t a );
//
// The sign function only works for signed built-in data types. The attempt to use unsigned data
// types or user-defined class types will result in a compile time error.
+//
+// http://stackoverflow.com/questions/1903954/is-there-a-standard-sign-function-signum-sgn-in-c-c
*/
template< typename T >
inline const T sign( T a )
diff --git a/src/mesa_pd/collision_detection/AnalyticContactDetection.h b/src/mesa_pd/collision_detection/AnalyticContactDetection.h
index cbc5b34d4168d4368b5fb232cb6b2dccef4251ba..bb3f8f1f21ba9356d0a4385c8de871efe0ec5dc6 100644
--- a/src/mesa_pd/collision_detection/AnalyticContactDetection.h
+++ b/src/mesa_pd/collision_detection/AnalyticContactDetection.h
@@ -46,6 +46,8 @@ namespace collision_detection {
class AnalyticContactDetection
{
public:
+ virtual ~AnalyticContactDetection() {}
+
size_t& getIdx1() {return idx1_;}
size_t& getIdx2() {return idx2_;}
Vec3& getContactPoint() {return contactPoint_;}
@@ -116,7 +118,35 @@ public:
const data::Sphere& s,
Accessor& ac);
-private:
+ template <typename Accessor>
+ bool operator()( const size_t idx1,
+ const size_t idx2,
+ const data::HalfSpace& p0,
+ const data::HalfSpace& p1,
+ Accessor& ac);
+
+ template <typename Accessor>
+ bool operator()( const size_t idx1,
+ const size_t idx2,
+ const data::CylindricalBoundary& p0,
+ const data::HalfSpace& p1,
+ Accessor& ac);
+
+ template <typename Accessor>
+ bool operator()( const size_t idx1,
+ const size_t idx2,
+ const data::HalfSpace& p0,
+ const data::CylindricalBoundary& p1,
+ Accessor& ac);
+
+ template <typename Accessor>
+ bool operator()( const size_t idx1,
+ const size_t idx2,
+ const data::CylindricalBoundary& p0,
+ const data::CylindricalBoundary& p1,
+ Accessor& ac);
+
+protected:
size_t idx1_;
size_t idx2_;
Vec3 contactPoint_;
@@ -254,6 +284,46 @@ inline bool AnalyticContactDetection::operator()( const size_t idx1,
return operator()(idx2, idx1, s, p, ac);
}
+template <typename Accessor>
+inline bool AnalyticContactDetection::operator()( const size_t /*idx1*/,
+ const size_t /*idx2*/,
+ const data::HalfSpace& /*geo1*/,
+ const data::HalfSpace& /*geo2*/,
+ Accessor& /*ac*/)
+{
+ WALBERLA_ABORT("Collision between two half spaces is not defined!")
+}
+
+template <typename Accessor>
+inline bool AnalyticContactDetection::operator()( const size_t /*idx1*/,
+ const size_t /*idx2*/,
+ const data::HalfSpace& /*geo1*/,
+ const data::CylindricalBoundary& /*geo2*/,
+ Accessor& /*ac*/)
+{
+ WALBERLA_ABORT("Collision between half spaces and cylindrical boundary is not defined!")
+}
+
+template <typename Accessor>
+inline bool AnalyticContactDetection::operator()( const size_t /*idx1*/,
+ const size_t /*idx2*/,
+ const data::CylindricalBoundary& /*geo1*/,
+ const data::HalfSpace& /*geo2*/,
+ Accessor& /*ac*/)
+{
+ WALBERLA_ABORT("Collision between half spaces and cylindrical boundary is not defined!")
+}
+
+template <typename Accessor>
+inline bool AnalyticContactDetection::operator()( const size_t /*idx1*/,
+ const size_t /*idx2*/,
+ const data::CylindricalBoundary& /*geo1*/,
+ const data::CylindricalBoundary& /*geo2*/,
+ Accessor& /*ac*/)
+{
+ WALBERLA_ABORT("Collision between two cylindrical boundaries is not defined!")
+}
+
inline
std::ostream& operator<<( std::ostream& os, const AnalyticContactDetection& ac )
{
diff --git a/src/mesa_pd/collision_detection/EPA.cpp b/src/mesa_pd/collision_detection/EPA.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..2076f680629d6095f21c248cc01dc05fde62851c
--- /dev/null
+++ b/src/mesa_pd/collision_detection/EPA.cpp
@@ -0,0 +1,923 @@
+//======================================================================================================================
+//
+// This file is part of waLBerla. waLBerla is free software: you can
+// redistribute it and/or modify it under the terms of the GNU General Public
+// License as published by the Free Software Foundation, either version 3 of
+// the License, or (at your option) any later version.
+//
+// waLBerla is distributed in the hope that it will be useful, but WITHOUT
+// ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+// FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
+// for more details.
+//
+// You should have received a copy of the GNU General Public License along
+// with waLBerla (see COPYING.txt). If not, see <http://www.gnu.org/licenses/>.
+//
+//! \file EPA.cpp
+//! \author Tobias Scharpff
+//! \author Tobias Leemann
+//
+// DISCLAIMER: The following source file contains modified code from the SOLID-3.5 library for
+// interference detection as it is published in the book "Collision Detection in Interactive
+// 3D Environments" by Gino van den Bergen <info@dtecta.com>. Even though the original source
+// was published under the GPL version 2 not allowing later versions, the original author of the
+// source code permitted the relicensing of the SOLID-3.5 library under the GPL version 3 license.
+//
+//=================================================================================================
+
+#include "EPA.h"
+#include <core/math/Constants.h>
+
+#include <vector>
+
+namespace walberla {
+namespace mesa_pd {
+namespace collision_detection {
+
+//=================================================================================================
+//
+// EPA::EPA_TRIANGLE CONSTRUCTOR
+//
+//=================================================================================================
+
+//*************************************************************************************************
+/*! \brief Construct a new EPA_Triangle.
+ * \param a First point index
+ * \param b Second point index
+ * \param c Third point index
+ * \param points Vector with all points
+ */
+EPA::EPA_Triangle::EPA_Triangle( size_t a,
+ size_t b,
+ size_t c,
+ const std::vector<Vec3>& points )
+{
+ const Vec3& A = points[a];
+ const Vec3& B = points[b];
+ const Vec3& C = points[c];
+
+ indices_[0] = a;
+ indices_[1] = b;
+ indices_[2] = c;
+
+ //calculate the closest point to the origin
+ //Real-Time Collsion Buch Seite 137
+ Vec3 ab = B-A;
+ Vec3 ac = C-A;
+ //Vec3 bc = C-B;
+
+ normal_ = ab % ac;
+ Vec3 nT = normal_;
+
+ //
+ real_t vc = nT * (A % B);
+ real_t va = nT * (B % C);
+ real_t vb = nT * (C % A);
+ real_t denom = real_t(1.0) / (va + vb + vc);
+
+ bar_[0] = va * denom;
+ bar_[1] = vb * denom;
+ bar_[2] = real_t(1.0) - bar_[0] - bar_[1];
+
+ closest_ = bar_[0] * A + bar_[1] * B + bar_[2] * C;
+
+ //sqrDist=key is square distance of v to origin
+ sqrDist_ = closest_.sqrLength();
+
+ //adjoined triangles not set yet
+ adjTriangle_[0] = adjTriangle_[1] = adjTriangle_[2] = nullptr;
+ adjEdges_[0] = adjEdges_[1] = adjEdges_[2] = 4;
+
+ obsolete_ = false;
+}
+
+//=================================================================================================
+//
+// EPA::EPA_TRIANGLE UTILITY FUNCTIONS
+//
+//=================================================================================================
+
+//*************************************************************************************************
+/*! \brief Sets the link of this triangles edge0 neighbor to tria and vice versa.
+ */
+inline bool EPA::EPA_Triangle::link( size_t edge0, EPA_Triangle* tria, size_t edge1 )
+{
+ WALBERLA_ASSERT_LESS(edge0, 3, "link: invalid edge index");
+ WALBERLA_ASSERT_LESS(edge1, 3, "link: invalid edge index");
+
+ adjTriangle_[edge0] = tria;
+ adjEdges_[edge0] = edge1;
+ tria->adjTriangle_[edge1] = this;
+ tria->adjEdges_[edge1] = edge0;
+
+ bool b = indices_[edge0] == tria->indices_[(edge1+1)%3] &&
+ indices_[(edge0+1)%3] == tria->indices_[edge1];
+ return b;
+}
+//*************************************************************************************************
+
+
+//*************************************************************************************************
+/*! \brief Fills edgeBuffer with the CCW contour of triangles not seen from point w which is in normal direction of the triangle.
+ */
+inline void EPA::EPA_Triangle::silhouette( const Vec3& w, EPA_EdgeBuffer& edgeBuffer )
+{
+ //std::cerr << "Starting Silhoutette search on Triangle {" << indices_[0] << "," << indices_[1] << "," << indices_[2] << "}" << std::endl;
+ edgeBuffer.clear();
+ obsolete_ = true;
+
+ adjTriangle_[0]->silhouette(adjEdges_[0], w, edgeBuffer);
+ adjTriangle_[1]->silhouette(adjEdges_[1], w, edgeBuffer);
+ adjTriangle_[2]->silhouette(adjEdges_[2], w, edgeBuffer);
+}
+//*************************************************************************************************
+
+
+//*************************************************************************************************
+/*! \brief Recursive silhuette finding method.
+ */
+void EPA::EPA_Triangle::silhouette( size_t index, const Vec3& w,
+ EPA_EdgeBuffer& edgeBuffer )
+{
+ if (!obsolete_) {
+ real_t test = (closest_ * w);
+ if (test < sqrDist_) {
+ edgeBuffer.push_back(EPA_Edge(this, index));
+ }
+ else {
+ obsolete_ = true; // Facet is visible
+ size_t next = (index+1) % 3;
+ adjTriangle_[next]->silhouette(adjEdges_[next], w, edgeBuffer);
+ next = (next+1) % 3;
+ adjTriangle_[next]->silhouette(adjEdges_[next], w, edgeBuffer);
+ }
+ }
+}
+//*************************************************************************************************
+
+//=================================================================================================
+//
+// EPA QUERY FUNCTIONS
+//
+//=================================================================================================
+
+//*************************************************************************************************
+//EPA Precision default values for different data types
+template< class T > struct EpsilonRelEPA;
+template<> struct EpsilonRelEPA< float > { static const float value; };
+template<> struct EpsilonRelEPA< double > { static const double value; };
+template<> struct EpsilonRelEPA< long double > { static const long double value; };
+
+const float EpsilonRelEPA< float >::value = static_cast< float >(1e-4);
+const double EpsilonRelEPA< double >::value = static_cast< double >(1e-6);
+const long double EpsilonRelEPA< long double >::value = static_cast< long double >(1e-6);
+
+//*************************************************************************************************
+/*! \brief Does an EPA computation with contactthreshold added. Use Default relative Error.
+ */
+bool EPA::doEPAcontactThreshold( Support &geom1,
+ Support &geom2,
+ const GJK& gjk,
+ Vec3& retNormal,
+ Vec3& contactPoint,
+ real_t& penetrationDepth){
+
+ //Default relative epsilon
+ return doEPA(geom1, geom2, gjk, retNormal, contactPoint, penetrationDepth, contactThreshold, EpsilonRelEPA<real_t>::value);
+}
+//*************************************************************************************************
+
+
+//*************************************************************************************************
+/*! \brief Does an EPA computation with contactThreshold added. Relative Error can be specified.
+ */
+bool EPA::doEPAcontactThreshold( Support &geom1,
+ Support &geom2,
+ const GJK& gjk,
+ Vec3& retNormal,
+ Vec3& contactPoint,
+ real_t& penetrationDepth,
+ real_t eps_rel){
+
+ return doEPA(geom1, geom2, gjk, retNormal, contactPoint, penetrationDepth, contactThreshold, eps_rel);
+}
+//*************************************************************************************************
+
+
+//*************************************************************************************************
+/*! \brief Does an EPA computation with margin added. Use Default relative Error.
+ */
+bool EPA::doEPAmargin( Support &geom1,
+ Support &geom2,
+ const GJK& gjk,
+ Vec3& retNormal,
+ Vec3& contactPoint,
+ real_t& penetrationDepth,
+ real_t margin){
+ //Default relative epsilon
+ return doEPA(geom1, geom2, gjk, retNormal, contactPoint, penetrationDepth, margin, EpsilonRelEPA<real_t>::value);
+}
+//*************************************************************************************************
+
+
+//*************************************************************************************************
+/*! \brief Does an epa computation with contact margin added and specified realtive error.
+ */
+bool EPA::doEPA( Support &geom1,
+ Support &geom2,
+ const GJK& gjk,
+ Vec3& retNormal,
+ 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
+
+ //Set references to the results of GJK
+ size_t numPoints( static_cast<size_t>( gjk.getSimplexSize() ) );
+ std::vector<Vec3> epaVolume( gjk.getSimplex() );
+ std::vector<Vec3> supportA ( gjk.getSupportA() );
+ std::vector<Vec3> supportB ( gjk.getSupportB() );
+
+ Vec3 support;
+
+ epaVolume.reserve( maxSupportPoints_ );
+ supportA.reserve ( maxSupportPoints_ );
+ supportB.reserve ( maxSupportPoints_ );
+
+ EPA_EntryBuffer entryBuffer;
+ entryBuffer.reserve(maxTriangles_);
+
+ EPA_EntryHeap entryHeap;
+ entryHeap.reserve(maxTriangles_);
+
+ EPA_EdgeBuffer edgeBuffer;
+ edgeBuffer.reserve(20);
+
+ real_t lowerBoundSqr = math::Limits<real_t>::inf();
+ real_t upperBoundSqr = math::Limits<real_t>::inf();
+
+ //create an Initial simplex
+ if(numPoints == 1) {
+ //If the GJK-Simplex contains only one point, it must be the origin and it must be on the boundary of the CSO.
+ //This means the enlarged bodies are in touching contact and the original bodies do not intersect.
+ return false;
+ }
+ else {
+ createInitialSimplex(numPoints, geom1, geom2, supportA, supportB, epaVolume, entryBuffer, margin);
+ }
+
+ for(EPA_EntryBuffer::iterator it=entryBuffer.begin(); it != entryBuffer.end(); ++it) {
+ if(it->isClosestInternal()) {
+ entryHeap.push_back(&(*it));
+ }
+ }
+
+ if(entryHeap.size() == 0) {
+ //unrecoverable error.
+ return false;
+ }
+
+ std::make_heap(entryHeap.begin(), entryHeap.end(), EPA::EPA_TriangleComp());
+ EPA_Triangle* current = nullptr;
+
+ numIterations_ = 0;
+ //EPA Main-Loop
+ do {
+ ++numIterations_;
+ std::pop_heap(entryHeap.begin(), entryHeap.end(), EPA::EPA_TriangleComp());
+ current = entryHeap.back();
+ entryHeap.pop_back();
+ if(!current->isObsolete()) {
+ lowerBoundSqr = current->getSqrDist();
+
+ if(epaVolume.size() == maxSupportPoints_) {
+ WALBERLA_ASSERT(false, "Support point limit reached.");
+ break;
+ }
+
+ // Compute new support direction
+ // if origin is contained in plane, use out-facing normal.
+ Vec3 normal;
+ if(current->getSqrDist() < real_comparison::Epsilon<real_t>::value*real_comparison::Epsilon<real_t>::value){
+ if(current->getNormal().sqrLength() < real_comparison::Epsilon<real_t>::value*real_comparison::Epsilon<real_t>::value){
+ break;
+ }
+
+ normal = current->getNormal().getNormalizedOrZero();
+ }else{
+ normal = current->getClosest().getNormalizedOrZero();
+ }
+ //std::cerr << "Current Closest: " << current->getClosest();
+ //std::cerr << "New support direction: " << normal << std::endl;
+
+ pushSupportMargin(geom1, geom2, normal, margin, epaVolume, supportA, supportB);
+ support = epaVolume.back();
+
+ numPoints++;
+
+ real_t farDist = support * normal; //not yet squared
+
+ WALBERLA_ASSERT_GREATER(farDist, real_t(0.0), "EPA support mapping gave invalid point in expansion direction");
+ //std::cerr << "New upper bound: " << farDist*farDist << std::endl;
+ upperBoundSqr = std::min(upperBoundSqr, farDist*farDist);
+
+
+
+ //Try to approximate the new surface with a sphere
+ 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) < real_t(1.10) * lowerBoundSqr &&
+ !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;
+ pushSupportMargin(geom1, geom2, ctr, margin, epaVolume, supportA, supportB);
+ support = epaVolume.back();
+ // Check if support is in expected direction
+ 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 = -supp_dist + real_t(2.0) * margin;
+ retNormal = -ctr;
+
+ if(penetrationDepth < contactThreshold){
+ return true;
+ }else{
+ return false;
+ }
+ } else {
+ //Reject sphere
+ removeSupportMargin(epaVolume, supportA, supportB);
+ support = epaVolume.back();
+ }
+ }
+ }
+ }
+
+ //terminating criteria's
+ //- we found that the two bounds are close enough
+ //- the added support point was already in the epaVolume
+ if(upperBoundSqr <= (real_t(1.0)+eps_rel)*(real_t(1.0)+eps_rel)*lowerBoundSqr
+ || support == epaVolume[(*current)[0]]
+ || support == epaVolume[(*current)[1]]
+ || support == epaVolume[(*current)[2]])
+ {
+ //std::cerr << "Tolerance reached." << std::endl;
+ break;
+ }
+
+ // Compute the silhouette cast by the new vertex
+ // Note that the new vertex is on the positive side
+ // of the current triangle, so the current triangle
+ // will not be in the convex hull. Start local search
+ // from this facet.
+
+ current->silhouette(support, edgeBuffer);
+ if(edgeBuffer.size() < 3 ) {
+ return false;
+ }
+
+ if(entryBuffer.size() == maxSupportPoints_) {
+ //"out of memory" so stop here
+ //std::cerr << "Memory Limit reached." << std::endl;
+ break;
+ }
+
+ EPA_EdgeBuffer::const_iterator it = edgeBuffer.begin();
+ entryBuffer.push_back(EPA_Triangle(it->getEnd(), it->getStart(), epaVolume.size()-1, epaVolume));
+
+ EPA_Triangle* firstTriangle = &(entryBuffer.back());
+ //if it is expanding candidate add to heap
+ //std::cerr << "Considering Triangle (" << firstTriangle->getSqrDist() << ") {" << (*firstTriangle)[0] << "," << (*firstTriangle)[1] << ","<< (*firstTriangle)[2] << "} ("<< epaVolume[(*firstTriangle)[0]] * firstTriangle->getNormal()<< ")" << std::endl;
+ if(epaVolume[(*firstTriangle)[0]] * firstTriangle->getNormal() < real_t(0.0)){
+ //the whole triangle is on the wrong side of the origin.
+ //This is a numerical error and will produce wrong results, if the search is continued. Stop here.
+ break;
+ }
+ if(firstTriangle->isClosestInternal()
+ && firstTriangle->getSqrDist() > lowerBoundSqr
+ && firstTriangle->getSqrDist() < upperBoundSqr)
+ {
+ entryHeap.push_back(firstTriangle);
+ std::push_heap(entryHeap.begin(), entryHeap.end(), EPA::EPA_TriangleComp());
+ }
+
+ firstTriangle->link(0, it->getTriangle(), it->getIndex());
+
+ EPA_Triangle* lastTriangle = firstTriangle;
+
+ ++it;
+ for(; it != edgeBuffer.end(); ++it){
+ if(entryBuffer.size() == maxSupportPoints_) {
+ //"out of memory" so stop here
+ break;
+ }
+
+ entryBuffer.push_back(EPA_Triangle(it->getEnd(), it->getStart(), epaVolume.size()-1, epaVolume));
+ EPA_Triangle* newTriangle = &(entryBuffer.back());
+
+ //std::cerr << "Considering Triangle (" << newTriangle->getSqrDist() << ") {" << (*newTriangle)[0] << "," << (*newTriangle)[1] << ","<< (*newTriangle)[2] << "} ("<< epaVolume[(*newTriangle)[0]] * newTriangle->getNormal() << ")" << std::endl;
+
+ if(epaVolume[(*newTriangle)[0]] * newTriangle->getNormal() < real_t(0.0)){
+ //the whole triangle is on the wrong side of the origin.
+ //This is an error.
+ break;
+ }
+ //if it is expanding candidate add to heap
+ if(newTriangle->isClosestInternal()
+ && newTriangle->getSqrDist() > lowerBoundSqr
+ && newTriangle->getSqrDist() < upperBoundSqr)
+ {
+ entryHeap.push_back(newTriangle);
+ std::push_heap(entryHeap.begin(), entryHeap.end(), EPA::EPA_TriangleComp());
+ }
+
+ if(!newTriangle->link(0, it->getTriangle(), it->getIndex())) {
+ break;
+ }
+
+ if(!newTriangle->link(2, lastTriangle, 1)) {
+ break;
+ }
+
+ lastTriangle = newTriangle;
+ }
+
+ if(it != edgeBuffer.end()) {
+ //For some reason the silhouette couldn't be processed completely
+ //so we stop here and take the last result
+ break;
+ }
+
+ firstTriangle->link(2, lastTriangle, 1);
+ }
+ } while (entryHeap.size() > 0 && entryHeap[0]->getSqrDist() <= upperBoundSqr);
+
+ //Normal must be inverted
+ retNormal = -current->getClosest().getNormalizedOrZero();
+
+ //Calculate Witness points
+ const Vec3 wittnessA = current->getClosestPoint(supportA);
+ const Vec3 wittnessB = current->getClosestPoint(supportB);
+ contactPoint = real_t(0.5) * (wittnessA + wittnessB);
+
+ //Penetration Depth
+ penetrationDepth = -(current->getClosest().length() - real_t(2.0) * margin);
+
+ /*std::cerr << "normal=" << retNormal <<std::endl;
+ std::cerr << "close =" << current->getClosest() << std::endl;
+ std::cerr << "diff =" << wittnesA - wittnesB <<std::endl;
+ std::cerr << "wittnesA =" << wittnesA <<std::endl;
+ std::cerr << "wittnesB =" << wittnesB <<std::endl;
+ std::cerr << "contactPoint=" << contactPoint << std::endl;
+ std::cerr << "penDepth=" << penetrationDepth <<std::endl;
+ std::cerr << "lowerBound=" << sqrt(lowerBoundSqr) <<std::endl;
+ std::cerr << "curreBound=" << current->getClosest().length() << std::endl;
+ std::cerr << "upperBound=" << sqrt(upperBoundSqr) <<std::endl;
+ std::cerr << "Heap Size=" << entryHeap.size() << std::endl;
+ std::cerr << "entryHeap[0]->getSqrDist()=" << entryHeap[0]->getSqrDist() << std::endl;*/
+ //std::cout << "EPA penetration depth: " << penetrationDepth << std::endl;
+
+ if(penetrationDepth < contactThreshold) {
+ return true;
+ }
+
+ //no intersection found!
+ return false;
+}
+//*************************************************************************************************
+
+//=================================================================================================
+//
+// EPA UTILITY FUNCTIONS
+//
+//=================================================================================================
+
+//*************************************************************************************************
+/*! \brief Create a starting tetrahedron for EPA, from the GJK Simplex.
+ */
+inline void EPA::createInitialSimplex( size_t numPoints,
+ Support &geom1,
+ Support &geom2,
+ std::vector<Vec3>& supportA,
+ std::vector<Vec3>& supportB,
+ std::vector<Vec3>& epaVolume,
+ EPA_EntryBuffer& entryBuffer,
+ real_t margin )
+{
+ switch(numPoints) {
+ case 2:
+ {
+ //simplex is a line segement
+ //add 3 points around the this segment
+ //the COS is konvex so the resulting hexaheadron should be konvex too
+
+ Vec3 d = epaVolume[1] - epaVolume[0];
+ //find coordinate axis e_i which is furthest from paralell to d
+ //and therefore d has the smallest abs(d[i])
+ real_t abs0 = std::abs(d[0]);
+ real_t abs1 = std::abs(d[1]);
+ real_t abs2 = std::abs(d[2]);
+
+ Vec3 axis;
+ if( abs0 < abs1 && abs0 < abs2) {
+ axis = Vec3(real_t(1.0), real_t(0.0), real_t(0.0));
+ }
+ else if( abs1 < abs0 && abs1 < abs2) {
+ axis = Vec3(real_t(0.0), real_t(1.0), real_t(0.0));
+ }
+ else {
+ axis = Vec3(real_t(0.0), real_t(0.0), real_t(1.0));
+ }
+
+ Vec3 direction1 = (d % axis).getNormalizedOrZero();
+ Quat q(d, (real_t(2.0)/real_t(3.0)) * real_t(walberla::math::M_PI));
+ Mat3 rot = q.toRotationMatrix();
+ Vec3 direction2 = (rot*direction1).getNormalizedOrZero();
+ Vec3 direction3 = (rot*direction2).getNormalizedOrZero();
+
+ //add point in positive normal direction1
+ pushSupportMargin(geom1, geom2, direction1, margin, epaVolume, supportA, supportB);
+ //std::cerr << "S1: " << support1 << std::endl;
+
+ //add point in negative normal direction2
+ pushSupportMargin(geom1, geom2, direction2, margin, epaVolume, supportA, supportB);
+ //std::cerr << "S2: " << support2 << std::endl;
+
+ //add point in negative normal direction
+ pushSupportMargin(geom1, geom2, direction3, margin, epaVolume, supportA, supportB);
+ //std::cerr << "S3: " << support3 << std::endl;
+
+ //Build the hexahedron as it is convex
+ //epaVolume[1] = up
+ //epaVolume[0] = down
+ //epaVolume[2] = ccw1
+ //epaVolume[3] = ccw2
+ //epaVolume[4] = ccw3
+
+
+ //check for containment inside
+ if(originInTetrahedron(epaVolume[0], epaVolume[2], epaVolume[3], epaVolume[4]) || originInTetrahedron(epaVolume[1], epaVolume[2], epaVolume[3], epaVolume[4]) ){
+ //insert triangle 1
+ entryBuffer.push_back(EPA_Triangle(1, 2, 3, epaVolume)); //[0] up->ccw1->ccw2
+ //insert triangle 2
+ entryBuffer.push_back(EPA_Triangle(1, 3, 4, epaVolume)); //[1] up->ccw2->ccw3
+ //insert triangle 3
+ entryBuffer.push_back(EPA_Triangle(1, 4, 2, epaVolume)); //[2] up->ccw3->ccw1
+
+ //link these 3 triangles
+ entryBuffer[0].link(2, &(entryBuffer[1]), 0); //edge up->ccw1
+ entryBuffer[1].link(2, &(entryBuffer[2]), 0); //edge up->ccw2
+ entryBuffer[2].link(2, &(entryBuffer[0]), 0); //edge up->ccw3
+
+
+ //insert triangle 4
+ entryBuffer.push_back(EPA_Triangle(0, 2, 4, epaVolume)); //[3] down->ccw1->ccw3
+ //insert triangle 5
+ entryBuffer.push_back(EPA_Triangle(0, 4, 3, epaVolume)); //[4] down->ccw3->ccw2
+ //insert triangle 6
+ entryBuffer.push_back(EPA_Triangle(0, 3, 2, epaVolume)); //[5] down->ccw2->ccw1
+
+ //link these 3 triangles
+ entryBuffer[3].link(2, &(entryBuffer[4]), 0); //edge down->ccw3
+ entryBuffer[4].link(2, &(entryBuffer[5]), 0); //edge down->ccw1
+ entryBuffer[5].link(2, &(entryBuffer[3]), 0); //edge down->ccw1
+
+ //link the two pyramids
+ entryBuffer[0].link(1, &(entryBuffer[5]), 1); //edge ccw1->ccw2
+ entryBuffer[1].link(1, &(entryBuffer[4]), 1); //edge ccw2->ccw3
+ entryBuffer[2].link(1, &(entryBuffer[3]), 1); //edge ccw3->ccw1
+ }else{
+ //Apply iterative search
+ removeSupportMargin(epaVolume, supportA, supportB); //remove 5th point.
+ //Search starts from the remaining 4 points
+ searchTetrahedron(geom1, geom2, epaVolume, supportA, supportB, entryBuffer, margin);
+ }
+
+ break;
+ }
+ case 3:
+ {
+ //simplex is a triangle, add tow points in positive and negative normal direction
+
+ const Vec3& A = epaVolume[2]; //The Point last added to the simplex
+ const Vec3& B = epaVolume[1]; //One Point that was already in the simplex
+ const Vec3& C = epaVolume[0]; //One Point that was already in the simplex
+ //ABC is a conterclockwise triangle
+
+ const Vec3 AB = B-A; //The vector A->B
+ const Vec3 AC = C-A; //The vector A->C
+ const Vec3 ABC = (AB%AC).getNormalizedOrZero(); //The the normal pointing towards the viewer if he sees a CCW triangle ABC
+
+ //add point in positive normal direction
+ pushSupportMargin(geom1, geom2, ABC, margin, epaVolume, supportA, supportB);
+
+ //add point in negative normal direction
+ pushSupportMargin(geom1, geom2, -ABC, margin, epaVolume, supportA, supportB);
+ //Vec3 support2 = epaVolume.back();
+
+ //check if the hexahedron is convex aka check if a partial tetrahedron contains the last point
+ if(pointInTetrahedron(epaVolume[3], epaVolume[4], epaVolume[0], epaVolume[2], epaVolume[1])) {
+ //epaVolumne[1] is whithin the tetraheadron 3-4-0-2 so this is the epaVolume to take
+ createInitialTetrahedron(3,4,0,2, epaVolume, entryBuffer);
+ }
+ else if(pointInTetrahedron(epaVolume[3], epaVolume[4], epaVolume[1], epaVolume[0], epaVolume[2])) {
+ createInitialTetrahedron(3,4,1,0, epaVolume, entryBuffer);
+ }
+ else if(pointInTetrahedron(epaVolume[3], epaVolume[4], epaVolume[2], epaVolume[1], epaVolume[0])) {
+ createInitialTetrahedron(3,4,2,1, epaVolume, entryBuffer);
+ }
+ else {
+ //Build the hexahedron as it is convex
+ //insert triangle 1
+ entryBuffer.push_back(EPA_Triangle(3, 2, 1, epaVolume)); //[0] support1->A->B
+ //insert triangle 2
+ entryBuffer.push_back(EPA_Triangle(3, 1, 0, epaVolume)); //[1] support1->B->C
+ //insert triangle 3
+ entryBuffer.push_back(EPA_Triangle(3, 0, 2, epaVolume)); //[2] support1->C->A
+
+ //link these 3 triangles
+ entryBuffer[0].link(2, &(entryBuffer[1]), 0); //edge support1->A
+ entryBuffer[1].link(2, &(entryBuffer[2]), 0); //edge support1->B
+ entryBuffer[2].link(2, &(entryBuffer[0]), 0); //edge support1->C
+
+
+ //insert triangle 4
+ entryBuffer.push_back(EPA_Triangle(4, 2, 0, epaVolume)); //[3] support2->A->C
+ //insert triangle 5
+ entryBuffer.push_back(EPA_Triangle(4, 0, 1, epaVolume)); //[4] support2->C->B
+ //insert triangle 6
+ entryBuffer.push_back(EPA_Triangle(4, 1, 2, epaVolume)); //[5] support2->B->A
+
+ //link these 3 triangles
+ entryBuffer[3].link(2, &(entryBuffer[4]), 0); //edge support2->C
+ entryBuffer[4].link(2, &(entryBuffer[5]), 0); //edge support2->B
+ entryBuffer[5].link(2, &(entryBuffer[3]), 0); //edge support2->A
+
+ //link the two pyramids
+ entryBuffer[0].link(1, &(entryBuffer[5]), 1); //edge A->B
+ entryBuffer[1].link(1, &(entryBuffer[4]), 1); //edge B->C
+ entryBuffer[2].link(1, &(entryBuffer[3]), 1); //edge C->A
+
+ }
+
+ break;
+ }
+ case 4:
+ {
+ createInitialTetrahedron(3,2,1,0, epaVolume, entryBuffer);
+ break;
+ }
+ default:
+ {
+ WALBERLA_ASSERT( false, "invalid number of simplex points in EPA" );
+ break;
+ }
+ }
+}
+
+//*************************************************************************************************
+
+//*************************************************************************************************
+/*! \brief TODO
+ *
+ * see Book "collision detection in interactive 3D environments" page161
+ * ATTENTION seems to have no consistent behavior on the surface and vertices
+ */
+inline bool EPA::originInTetrahedron( const Vec3& p0, const Vec3& p1, const Vec3& p2,
+ const Vec3& p3 )
+{
+ Vec3 normal0T = (p1 -p0) % (p2-p0);
+ if( (normal0T*p0 > real_t(0.0)) == (normal0T*p3 > real_t(0.0)) ) {
+ return false;
+ }
+ Vec3 normal1T = (p2 -p1) % (p3-p1);
+ if( (normal1T*p1 > real_t(0.0)) == (normal1T*p0 > real_t(0.0)) ) {
+ return false;
+ }
+ Vec3 normal2T = (p3 -p2) % (p0-p2);
+ if( (normal2T*p2 > real_t(0.0)) == (normal2T*p1 > real_t(0.0)) ) {
+ return false;
+ }
+ Vec3 normal3T = (p0 -p3) % (p1-p3);
+ if( (normal3T*p3 > real_t(0.0)) == (normal3T*p2 > real_t(0.0)) ) {
+ return false;
+ }
+
+ return true;
+}
+//*************************************************************************************************
+
+
+//*************************************************************************************************
+/*! \brief Retrurns true, if the origin lies in the tetrahedron ABCD.
+ */
+inline bool EPA::originInTetrahedronVolumeMethod( const Vec3& A, const Vec3& B, const Vec3& C,
+ const Vec3& D )
+{
+ const Vec3& aoT = A;
+ if((aoT * (B % C)) <= real_t(0.0)) {
+ //if volume of ABC and Origin <0.0 than the origin is on the wrong side of ABC
+ //http://mathworld.wolfram.com/Tetrahedron.html volume formula
+ return false;
+ }
+ if((aoT * (C % D)) <= real_t(0.0)) {
+ return false;
+ }
+ if((aoT * (D % B)) <= real_t(0.0)) {
+ return false;
+ }
+ if((B * (D % C)) <= real_t(0.0)) {
+ return false;
+ }
+ return true;
+}
+//*************************************************************************************************
+
+
+//*************************************************************************************************
+/*! \brief Retrurns true, if a point lies in the tetrahedron ABCD.
+ * \param point The point to be checked for containment.
+ */
+inline bool EPA::pointInTetrahedron( const Vec3& A, const Vec3& B, const Vec3& C, const Vec3& D,
+ const Vec3& point )
+{
+ return originInTetrahedronVolumeMethod( A-point, B-point, C-point, D-point );
+}
+//*************************************************************************************************
+
+
+//*************************************************************************************************
+/*!\brief TODO
+ * top, frontLeft ... are indices
+ */
+inline void EPA::createInitialTetrahedron( size_t top,
+ size_t frontLeft,
+ size_t frontRight,
+ size_t back,
+ std::vector<Vec3>& epaVolume,
+ EPA_EntryBuffer& entryBuffer )
+{
+ //insert triangle 1
+ entryBuffer.push_back(EPA_Triangle(top, frontLeft, frontRight, epaVolume)); //[0] vorne
+ //insert triangle 2
+ entryBuffer.push_back(EPA_Triangle(top, frontRight, back, epaVolume)); //[1] rechts hinten
+ //insert triangle 3
+ entryBuffer.push_back(EPA_Triangle(top, back, frontLeft, epaVolume)); //[2] links hinten
+ //insert triangle 4
+ entryBuffer.push_back(EPA_Triangle(back, frontRight, frontLeft, epaVolume)); //[3] unten
+
+ //make links between the triangles
+ entryBuffer[0].link(0, &(entryBuffer[2]), 2); //Kante vorne links
+ entryBuffer[0].link(2, &(entryBuffer[1]), 0); //Kante vorne rechts
+ entryBuffer[0].link(1, &(entryBuffer[3]), 1); //kante vorne unten
+
+ entryBuffer[1].link(2, &(entryBuffer[2]), 0); //Kante hinten
+ entryBuffer[1].link(1, &(entryBuffer[3]), 0); //kante rechts unten
+
+ entryBuffer[2].link(1, &(entryBuffer[3]), 2); //kante links unten
+
+}
+//*************************************************************************************************
+
+/*! \brief Search a tetrahedron that contains the origin.
+ * Start with four arbitrary support points in epaVolume that form a
+ * tetrahedron. (This needn't contain the origin.)
+ * This algorithm will search and return an altered tetrahedron
+ * containing the origin. Do only use this function if the object/body
+ * certainly contains the origin!
+ * \return True, if a tetrahedron was found. False if search has been aborted.
+ */
+inline bool EPA::searchTetrahedron(Support &geom1,
+ Support &geom2,
+ std::vector<Vec3>& epaVolume,
+ std::vector<Vec3>& supportA,
+ std::vector<Vec3>& supportB,
+ EPA_EntryBuffer& entryBuffer,
+ real_t margin )
+{
+ //Store the point no longer needed (0 if all points are needed, and origin is contained.)
+ int loopCount = 0;
+ int pointIndexToRemove = -1;
+ Vec3 newSearchDirection;
+ do{
+ loopCount++;
+ pointIndexToRemove = -1;
+ //Check if opposite tetrahedron point and orign are on the same side
+ //of the face. (for all faces)
+ Vec3 normal0T = (epaVolume[1] -epaVolume[0]) % (epaVolume[2]-epaVolume[0]);
+ real_t dot_val = normal0T*epaVolume[0];
+ if( (normal0T*epaVolume[3] < dot_val) == (dot_val < real_t(0.0)) ) {
+ pointIndexToRemove = 3;
+ newSearchDirection = (normal0T*epaVolume[3] < dot_val) ? normal0T : -normal0T;
+ }
+
+ Vec3 normal1T = (epaVolume[2] -epaVolume[1]) % (epaVolume[3]-epaVolume[1]);
+ dot_val = normal1T*epaVolume[1];
+ if( (normal1T*epaVolume[0] < dot_val) == (dot_val < real_t(0.0)) ) {
+ pointIndexToRemove = 0;
+ newSearchDirection = (normal1T*epaVolume[0] < dot_val) ? normal1T : -normal1T;
+ }
+
+ Vec3 normal2T = (epaVolume[3] -epaVolume[2]) % (epaVolume[0]-epaVolume[2]);
+ dot_val = normal2T*epaVolume[2];
+ if( (normal2T*epaVolume[1] < dot_val) == (dot_val < real_t(0.0)) ) {
+ pointIndexToRemove = 1;
+ newSearchDirection = (normal2T*epaVolume[1] < dot_val) ? normal2T : -normal2T;
+ }
+
+ Vec3 normal3T = (epaVolume[0] -epaVolume[3]) % (epaVolume[1]-epaVolume[3]);
+ dot_val = normal3T*epaVolume[3];
+ if( (normal3T*epaVolume[2] < dot_val) == (dot_val < real_t(0.0)) ) {
+ pointIndexToRemove = 2;
+ newSearchDirection = (normal3T*epaVolume[2] < dot_val) ? normal3T : -normal3T;
+ }
+ //Origin not contained in tetrahedron.
+ if(pointIndexToRemove != -1){
+ if(loopCount > 50){
+ return false;
+ }
+ //Get new support point and replace old.
+ /*std::cerr << "Search Direction is: "<< newSearchDirection << std::endl;
+ std::cerr << "Projection of unnecc. point " << pointIndexToRemove << ": " << epaVolume[pointIndexToRemove] * newSearchDirection << std::endl;
+ std::cerr << "Projection of other points: " << epaVolume[(pointIndexToRemove+1)%4] * newSearchDirection << std::endl;*/
+ newSearchDirection = newSearchDirection.getNormalizedOrZero();
+ /*supportA[pointIndexToRemove] = geom1.supportContactThreshold(newSearchDirection);
+ supportB[pointIndexToRemove] = geom2.supportContactThreshold(-newSearchDirection);
+ epaVolume[pointIndexToRemove] = supportA[pointIndexToRemove] - supportB[pointIndexToRemove];*/
+ replaceSupportMargin(geom1, geom2, newSearchDirection, margin, epaVolume, supportA, supportB, (size_t)pointIndexToRemove);
+ //std::cerr << "Projection of new support point " << epaVolume[pointIndexToRemove] << ": " << epaVolume[pointIndexToRemove] * newSearchDirection << std::endl;
+
+ }
+ }
+ while(pointIndexToRemove != -1);
+ //std::cerr << "Found Tet after " << loopCount << " searches." << std::endl;
+
+ //Build final tetrahedron
+ Vec3 check_normal = (epaVolume[1] -epaVolume[0]) % (epaVolume[2]-epaVolume[0]);
+ if(check_normal*epaVolume[3] > check_normal*epaVolume[0]){
+ //p3 is behind.
+ createInitialTetrahedron(1, 0, 2, 3, epaVolume, entryBuffer);
+ }else{
+ //p3 is in front
+ createInitialTetrahedron(1, 3, 2, 0, epaVolume, entryBuffer);
+ }
+ return true;
+}
+//*************************************************************************************************
+
+
+//*************************************************************************************************
+/*! \brief Calculate a Circle through the for Points A, B, C, D.
+ * \param center Contains the center point of the circle after the call
+ * \return The squared radius of the circle or a negative value if no such circle exists.
+ */
+inline real_t EPA::calculateCircle(const Vec3& A, const Vec3& B, const Vec3& C,
+ const Vec3& D, Vec3& center ){
+ const Vec3 n1(A-B);
+ const Vec3 n2(A-C);
+ const Vec3 n3(A-D);
+
+ // Here we already see if such circle exists.
+ const real_t det = n1 * (n2 % n3);
+ if(floatIsEqual(det, real_t(0.0))){
+ //no circle exists. Leave center untouched, and return -1.0
+ return real_t(-1.0);
+ }
+ const real_t Alen = A.sqrLength();
+ const real_t d1 = (Alen - B.sqrLength())*real_t(0.5);
+ const real_t d2 = (Alen - C.sqrLength())*real_t(0.5);
+ const real_t d3 = (Alen - D.sqrLength())*real_t(0.5);
+
+ //Apply solution formula
+ center = (real_t(1.0)/det)*(d1 * (n2 % n3) + d2 * (n3 % n1) + d3 * (n1 % n2));
+
+ return (A - center).sqrLength();
+}
+//*************************************************************************************************
+
+} //collision_detection
+} //mesa_pd
+} //walberla
diff --git a/src/mesa_pd/collision_detection/EPA.h b/src/mesa_pd/collision_detection/EPA.h
new file mode 100644
index 0000000000000000000000000000000000000000..46d59a08ad592056d3e93246a0ba435bf57c3ca0
--- /dev/null
+++ b/src/mesa_pd/collision_detection/EPA.h
@@ -0,0 +1,476 @@
+//======================================================================================================================
+//
+// This file is part of waLBerla. waLBerla is free software: you can
+// redistribute it and/or modify it under the terms of the GNU General Public
+// License as published by the Free Software Foundation, either version 3 of
+// the License, or (at your option) any later version.
+//
+// waLBerla is distributed in the hope that it will be useful, but WITHOUT
+// ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+// FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
+// for more details.
+//
+// You should have received a copy of the GNU General Public License along
+// with waLBerla (see COPYING.txt). If not, see <http://www.gnu.org/licenses/>.
+//
+//! \file EPA.h
+//! \author Tobias Scharpff
+//! \author Tobias Leemann
+//
+// DISCLAIMER: The following source file contains modified code from the SOLID-3.5 library for
+// interference detection as it is published in the book "Collision Detection in Interactive
+// 3D Environments" by Gino van den Bergen <info@dtecta.com>. Even though the original source
+// was published under the GPL version 2 not allowing later versions, the original author of the
+// source code permitted the relicensing of the SOLID-3.5 library under the GPL version 3 license.
+//
+//=================================================================================================
+
+#pragma once
+
+#include <mesa_pd/data/DataTypes.h>
+#include <mesa_pd/collision_detection/GJK.h>
+#include <mesa_pd/collision_detection/Support.h>
+
+#include <vector>
+
+namespace walberla {
+namespace mesa_pd {
+namespace collision_detection {
+
+/*!
+ * \brief The Expanding-Polytope Algorithm.
+ */
+class EPA
+{
+private :
+ //**Type definitions****************************************************************************
+ class EPA_Edge;
+ class EPA_Triangle;
+ class EPA_TriangleComp;
+
+ typedef std::vector<EPA_Triangle> EPA_EntryBuffer;
+ typedef std::vector<EPA_Triangle*> EPA_EntryHeap;
+ typedef std::vector<EPA_Edge> EPA_EdgeBuffer;
+ //**********************************************************************************************
+
+public:
+ static constexpr real_t contactThreshold = real_t(1e-8);
+
+ //**Query functions*****************************************************************************
+ /*!\name Query functions */
+ //@{
+ bool doEPAcontactThreshold( Support &geom1,
+ Support &geom2,
+ const GJK& gjk,
+ Vec3& normal,
+ Vec3& contactPoint,
+ real_t& penetrationDepth);
+
+
+ bool doEPAcontactThreshold( Support &geom1,
+ Support &geom2,
+ const GJK& gjk,
+ Vec3& normal,
+ Vec3& contactPoint,
+ real_t& penetrationDepth,
+ real_t eps_rel);
+
+ bool doEPAmargin( Support &geom1,
+ Support &geom2,
+ const GJK& gjk,
+ Vec3& normal,
+ Vec3& contactPoint,
+ real_t& penetrationDepth,
+ real_t margin);
+
+ bool doEPA( Support &geom1,
+ Support &geom2,
+ const GJK& gjk,
+ Vec3& normal,
+ Vec3& contactPoint,
+ real_t& penetrationDepth,
+ real_t margin,
+ real_t eps_rel );
+
+
+
+ //@}
+ //**********************************************************************************************
+
+ //**Getter/Setter functions*****************************************************************************
+ /*!\name Getter and Setter functions */
+ //@{
+ inline void setMaxSupportPoints( size_t maxSupportPoints) {maxSupportPoints_ = maxSupportPoints;}
+
+ inline size_t getMaxSupportPoints() {return maxSupportPoints_;}
+
+ inline void setMaxTriangles( size_t maxTriangles) {maxTriangles_ = maxTriangles;}
+
+ 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;}
+
+ //@}
+ //**********************************************************************************************
+
+private:
+ //**Utility functions***************************************************************************
+ /*!\name Utility functions */
+ //@{
+ inline void pushSupportMargin(const Support &geom1,
+ const Support &geom2,
+ const Vec3& dir,
+ const real_t margin,
+ std::vector<Vec3>& epaVolume,
+ std::vector<Vec3>& supportA,
+ std::vector<Vec3>& supportB);
+
+ inline void replaceSupportMargin(const Support &geom1,
+ const Support &geom2,
+ const Vec3& dir,
+ const real_t margin,
+ std::vector<Vec3>& epaVolume,
+ std::vector<Vec3>& supportA,
+ std::vector<Vec3>& supportB,
+ size_t indexToReplace);
+
+ inline void removeSupportMargin(std::vector<Vec3>& epaVolume, std::vector<Vec3>& supportA, std::vector<Vec3>& supportB);
+
+ inline bool originInTetrahedron ( const Vec3& A, const Vec3& B, const Vec3& C,
+ const Vec3& D );
+ inline bool originInTetrahedronVolumeMethod( const Vec3& A, const Vec3& B, const Vec3& C,
+ const Vec3& D );
+ inline bool pointInTetrahedron ( const Vec3& A, const Vec3& B, const Vec3& C,
+ const Vec3& D, const Vec3& point );
+ bool searchTetrahedron (Support &geom1,
+ Support &geom2,
+ std::vector<Vec3>& epaVolume,
+ std::vector<Vec3>& supportA,
+ std::vector<Vec3>& supportB,
+ EPA_EntryBuffer& entryBuffer,
+ real_t margin );
+
+ void createInitialTetrahedron ( size_t top,
+ size_t frontLeft,
+ size_t frontRight,
+ size_t back,
+ std::vector<Vec3>& epaVolume,
+ EPA_EntryBuffer& entryBuffer );
+
+ void createInitialSimplex ( size_t numPoints,
+ Support &geom1,
+ Support &geom2,
+ std::vector<Vec3>& supportA,
+ std::vector<Vec3>& supportB,
+ std::vector<Vec3>& epaVolume,
+ EPA_EntryBuffer& entryBuffer,
+ real_t margin );
+ inline real_t calculateCircle ( const Vec3& A,
+ const Vec3& B,
+ const Vec3& C,
+ const Vec3& D,
+ Vec3& center );
+ //@}
+ //**********************************************************************************************
+
+
+private:
+ //EPA constants
+ size_t maxSupportPoints_ = 100;
+ size_t maxTriangles_ = 200;
+
+ int numIterations_ = 0;
+ bool bUseSphereOptimization_ = false;
+};
+//*************************************************************************************************
+
+
+
+
+//=================================================================================================
+//
+// EPA::EPA_TRIANGLE CLASS DEFINITION
+//
+//=================================================================================================
+
+//*************************************************************************************************
+/*!\brief Class storing Information about a triangular facette (Triangle) of the EPA-Polytope
+ *
+ * see Collision detction in interactiv 3D environments; Gino van den bergen page 155
+ */
+class EPA::EPA_Triangle {
+public:
+ //**Constructor*********************************************************************************
+ /*!\name Constructor */
+ //@{
+ explicit inline EPA_Triangle( size_t a, size_t b, size_t c, const std::vector<Vec3>& points );
+ //@}
+ //**********************************************************************************************
+
+ //**Get functions*******************************************************************************
+ /*!\name Get functions */
+ //@{
+ ///Returns the index of the internal vertex i(=0,1,2) within the EPA scope.
+ inline size_t operator[]( size_t i ) const {return indices_[i];}
+ ///Returns the point closest to the origin of the affine hull of the triangle, which is also the normal.
+ inline const Vec3& getClosest() const {return closest_;}
+ ///Returns the normal of the triangle. Normal is not normalized!
+ inline const Vec3& getNormal() const {return normal_;}
+ inline Vec3 getClosestPoint(const std::vector<Vec3>& points) const;
+ ///Returns the squared distance to the closest to the origin of the affine hull of the triangle.
+ inline real_t getSqrDist() const {return sqrDist_;}
+ ///Returns true if the triangle is no longer part of the EPA polygon.
+ inline bool isObsolete() const {return obsolete_;}
+ inline bool isClosestInternal() const;
+ //@}
+ //**********************************************************************************************
+
+ //**Utility functions***************************************************************************
+ /*!\name Utility functions */
+ //@{
+ inline bool link( size_t edge0, EPA_Triangle* tria, size_t edge1 );
+ inline void silhouette( const Vec3& w, EPA_EdgeBuffer& edgeBuffer );
+ //@}
+ //**********************************************************************************************
+
+private:
+ //**Utility functions***************************************************************************
+ /*!\name Utility functions */
+ //@{
+ void silhouette( size_t index, const Vec3& w, EPA_EdgeBuffer& edgeBuffer );
+ //@}
+ //**********************************************************************************************
+
+ //**Member variables****************************************************************************
+ /*!\name Member variables */
+ //@{
+ size_t indices_[3]; //!< indices of the vertices of the triangle
+ bool obsolete_; //!< flag to denote whether die triangle is visible from the new support point
+
+ Vec3 closest_; //!< the point closest to the origin of the affine hull of the triangle
+ Vec3 normal_; //!< normal pointing away from the origin
+ real_t bar_[3]; //!< the barycentric coordinate of closest_
+ real_t sqrDist_; //!< =key; square distance of closest_ to the origin
+
+ EPA_Triangle* adjTriangle_[3]; //!< pointer to the triangle adjacent to edge i(=0,1,2)
+ size_t adjEdges_[3]; //!< for each adjoining triangle adjTriangle_[i], the index of the adjoining edge
+ //@}
+ //**********************************************************************************************
+};
+//*************************************************************************************************
+
+
+//=================================================================================================
+//
+// EPA::EPA_EDGE CLASS DEFINITION
+//
+//=================================================================================================
+
+//*************************************************************************************************
+/*!\brief Class storing Information about an Edge of the EPA-Polytope
+ */
+class EPA::EPA_Edge {
+public:
+ //**Constructor*********************************************************************************
+ /*!\name Constructor */
+ //@{
+ EPA_Edge( EPA_Triangle* triangle, size_t index );
+ //@}
+ //**********************************************************************************************
+
+ //**Get functions*******************************************************************************
+ /*!\name Get functions */
+ //@{
+ ///Return the triangle this edge belongs to.
+ EPA_Triangle* getTriangle() const {return triangle_;}
+ ///Get the Index of this edge in its triangle.
+ size_t getIndex() const {return startIdx_;}
+ ///Return the start point index of an edge.
+ size_t getStart() const {return (*triangle_)[startIdx_];}
+ ///Return the end point index of an edge.
+ size_t getEnd() const {return (*triangle_)[(startIdx_+1) % 3];}
+ //@}
+ //**********************************************************************************************
+
+private:
+ //**Member variables****************************************************************************
+ /*!\name Member variables */
+ //@{
+ EPA_Triangle* triangle_; //!< the EPA triangle the edge is contained in
+ size_t startIdx_; //!< the index of the point the edge starts at (0, 1, 2)
+ //@}
+ //**********************************************************************************************
+};
+//*************************************************************************************************
+
+
+
+//=================================================================================================
+//
+// EPA::EPA_TRIANGLECOMP CLASS DEFINITION
+//
+//=================================================================================================
+
+//*************************************************************************************************
+/*!\brief
+ * Compare Triangles by their closest points to sort the triangle heap.
+ */
+class EPA::EPA_TriangleComp {
+public:
+ //**Binary function call operator***************************************************************
+ /*!\name Binary function call operator */
+ //@{
+ ///Compare two triangles by their distance.
+ inline bool operator()( const EPA_Triangle *tria1,
+ const EPA_Triangle *tria2 )
+ { return tria1->getSqrDist() > tria2->getSqrDist(); }
+ //@}
+ //**********************************************************************************************
+};
+//*************************************************************************************************
+
+
+//=================================================================================================
+//
+// EPA_EDGE CONSTRUCTOR
+//
+//=================================================================================================
+
+//*************************************************************************************************
+/*!\brief
+ * Construct a new Triangle Edge.
+ */
+inline EPA::EPA_Edge::EPA_Edge( EPA_Triangle* triangle,
+ size_t index )
+ : triangle_(triangle)
+ , startIdx_(index)
+{
+}
+//*************************************************************************************************
+
+
+//=================================================================================================
+//
+// EPA::EPA_TRIANGLE GET FUNCTIONS
+//
+//=================================================================================================
+
+
+//*************************************************************************************************
+/*! \brief Calculates the corresponding closest point from the given points, using barycentric coordinates.
+ */
+inline Vec3 EPA::EPA_Triangle::getClosestPoint(const std::vector<Vec3>& points) const
+{
+ return bar_[0] * points[indices_[0]]
+ + bar_[1] * points[indices_[1]]
+ + bar_[2] * points[indices_[2]];
+
+}
+//*************************************************************************************************
+
+
+//*************************************************************************************************
+/*! Returns true if the point closest to the origin of the affine hull of the triangle, lies inside the triangle.
+ */
+inline bool EPA::EPA_Triangle::isClosestInternal() const
+{
+ real_t tol = real_t(0.0);
+ return bar_[0] >= tol
+ && bar_[1] >= tol
+ && bar_[2] >= tol;
+}
+//*************************************************************************************************
+
+
+//=================================================================================================
+//
+// EPA UTILITY FUNCTIONS
+//
+//=================================================================================================
+
+//*************************************************************************************************
+/*! \brief Calucates a support point of a body extended by threshold.
+ * Adds this support and the base points at bodies a and b to the vector.
+ * \param geom The body.
+ * \param dir The support point direction.
+ * \param margin Extension of the Body.
+ */
+inline void EPA::pushSupportMargin(const Support &geom1,
+ const Support &geom2,
+ const Vec3& dir,
+ const real_t margin,
+ std::vector<Vec3>& epaVolume,
+ std::vector<Vec3>& supportA,
+ std::vector<Vec3>& supportB)
+{
+ Vec3 ndir;
+ if(floatIsEqual(dir.sqrLength(), real_t(1.0))){
+ ndir = dir.getNormalizedOrZero();
+ }else{
+ ndir = dir;
+ }
+ Vec3 sA = geom1.support(ndir);
+ Vec3 sB = geom2.support(-ndir);
+ supportA.push_back(sA);
+ supportB.push_back(sB);
+
+ Vec3 support = sA -sB + real_t(2.0) * ndir * margin;
+ epaVolume.push_back(support);
+}
+//*************************************************************************************************
+
+
+//*************************************************************************************************
+/*! \brief Calucates a support point of a body extended by threshold.
+ * Replaces the old value in the vectors at "IndexToReplace" with this support and the base points at bodies a and b .
+ * \param geom The body.
+ * \param dir The support point direction.
+ * \param margin Extension of the Body.
+ */
+inline void EPA::replaceSupportMargin(const Support &geom1,
+ const Support &geom2,
+ const Vec3& dir,
+ const real_t margin,
+ std::vector<Vec3>& epaVolume,
+ std::vector<Vec3>& supportA,
+ std::vector<Vec3>& supportB,
+ size_t indexToReplace)
+{
+ Vec3 ndir;
+ if(floatIsEqual(dir.sqrLength(), real_t(1.0))){
+ ndir = dir.getNormalizedOrZero();
+ }else{
+ ndir = dir;
+ }
+ Vec3 sA = geom1.support(ndir);
+ Vec3 sB = geom2.support(-ndir);
+ Vec3 support = sA -sB + real_t(2.0) * ndir * margin;
+
+ supportA[indexToReplace] = sA;
+ supportB[indexToReplace] = sB;
+ epaVolume[indexToReplace] = support;
+}
+//*************************************************************************************************
+
+//*************************************************************************************************
+/*! \brief Removes a support point from the volume.
+ */
+inline void EPA::removeSupportMargin(std::vector<Vec3>& epaVolume,
+ std::vector<Vec3>& supportA,
+ std::vector<Vec3>& supportB)
+{
+ supportA.pop_back();
+ supportB.pop_back();
+ epaVolume.pop_back();
+}
+//*************************************************************************************************
+
+
+//@}
+
+} // namespace fcd
+} // namespace pe
+} // namespace walberla
diff --git a/src/mesa_pd/collision_detection/GJK.cpp b/src/mesa_pd/collision_detection/GJK.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..edd2a963ff1cf5678e2363af7aa28e3e202c23e7
--- /dev/null
+++ b/src/mesa_pd/collision_detection/GJK.cpp
@@ -0,0 +1,1072 @@
+//======================================================================================================================
+//
+// This file is part of waLBerla. waLBerla is free software: you can
+// redistribute it and/or modify it under the terms of the GNU General Public
+// License as published by the Free Software Foundation, either version 3 of
+// the License, or (at your option) any later version.
+//
+// waLBerla is distributed in the hope that it will be useful, but WITHOUT
+// ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+// FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
+// for more details.
+//
+// You should have received a copy of the GNU General Public License along
+// with waLBerla (see COPYING.txt). If not, see <http://www.gnu.org/licenses/>.
+//
+//! \file GJK.cpp
+//! \author Tobias Scharpff
+//! \author Tobias Leemann
+//
+//======================================================================================================================
+
+#include "GJK.h"
+
+#include <vector>
+
+#include <core/Abort.h>
+#include <core/math/Limits.h>
+#include <core/math/Vector3.h>
+
+
+namespace walberla {
+namespace mesa_pd {
+namespace collision_detection {
+
+GJK::GJK()
+ : simplex_(4)
+ , supportA_(4)
+ , supportB_(4)
+ , numPoints_(0)
+{
+ d_ = Vec3(real_t(0.0),real_t(0.6),real_t(0.8)); // just start with any vector of length 1
+}
+
+/**
+ * \brief Calucate a support point of a particle extended by threshold.
+ * \param geom support functions for particle 1 and 2.
+ * \param dir The support point direction.
+ * \param threshold Extension of the particle.
+ */
+const Vec3 GJK::putSupport(const Support &geom1,
+ const Support &geom2,
+ const Vec3& dir,
+ const real_t margin,
+ std::vector<Vec3> &simplex,
+ std::vector<Vec3> &supportA,
+ std::vector<Vec3> &supportB,
+ size_t index)
+{
+ supportA[index] = geom1.support(dir);
+ supportB[index] = geom2.support(-dir);
+ Vec3 supp = supportA[index]- supportB[index] + (real_t(2.0) * dir * margin);
+ simplex[index] = supp;
+ return supp;
+}
+//*************************************************************************************************
+
+//=================================================================================================
+//
+// QUERY FUNCTIONS
+//
+//=================================================================================================
+
+//*************************************************************************************************
+/*! \brief Calculate an upper bound for the distance of two Geometries.
+ * \return Distance between geom1 and geom2 or 0.0 if they are intersecting.
+ */
+real_t GJK::doGJK(const Support &geom1, const Support &geom2, Vec3& normal, Vec3& contactPoint)
+{
+ //Variables
+ Vec3 support; //the current support point
+ real_t ret; //return value aka distance between geom1 and geom2
+
+
+ ////////////////////////////////////////////////////////////////////////
+ //Initial initialisation step
+ ret = 0.0;
+ supportA_.resize(4);
+ supportB_.resize(4);
+ simplex_.resize(4);
+
+ //get any first support point
+
+ supportA_[0] = geom1.support(d_);
+ supportB_[0] = geom2.support(-d_);
+ support = supportA_[0] - supportB_[0];
+
+ //add this point to the simplex_
+ simplex_[0] = support;
+ numPoints_ = 1;
+
+ if(support * d_ < real_t(0.0)){
+ //we went as far as we could in direction 'd' but not passed the origin
+ //this means the bodies don't overlap
+ ret = calcDistance(normal, contactPoint);
+ return ret;
+ }
+
+ //first real search direction is in the opposite direction of the first support po
+ d_ = -support;
+
+ ////////////////////////////////////////////////////////////////////////
+ //GJK main loop
+ while (true) {
+ //get the support point in the current search direction
+ normalize(d_);
+ supportA_[numPoints_] = geom1.support(d_);
+ supportB_[numPoints_] = geom2.support(-d_);
+ support = supportA_[numPoints_] - supportB_[numPoints_];
+ //std::cerr << "[GJK] Support Direction: " << d_ << std::endl;
+ //std::cerr << "[GJK] Got Support: " << support << std::endl;
+
+ //check if "support" is passed the origin in search direction
+ if(support * d_ < real_t(0.0)){
+ //we went as far as we could in direction 'd' but not passed the origin
+ //this means the bodies don't overlap
+ //calc distance simplex to Origin
+ ret = calcDistance(normal, contactPoint);
+
+ return ret;
+ }
+
+ //add the new support point into the simplex
+ simplex_[numPoints_] = support;
+ numPoints_++;
+
+ ////////////////////////////////////////////////////////////////
+ //check if the origin is in the simplex
+ //if it is the triangle mashes are overlapping
+ switch(numPoints_)
+ {
+ case 2:
+ {
+ if(simplex2(d_)) {
+ simplex_.pop_back();
+ simplex_.pop_back();
+ supportA_.pop_back();
+ supportA_.pop_back();
+ supportB_.pop_back();
+ supportB_.pop_back();
+ return ret;
+ }
+ }
+ break;
+
+ case 3:
+ {
+ if(simplex3(d_)) {
+ simplex_.pop_back();
+ supportA_.pop_back();
+ supportB_.pop_back();
+ return ret;
+ }
+ }
+ break;
+
+ case 4:
+ {
+ if(simplex4(d_)) {
+
+ return ret;
+ }
+ }
+ break;
+ default:
+ {
+ WALBERLA_ABORT( "Number of points in the simplex is not 1<=n<=4" );
+ }
+ break;
+ }
+ }
+
+ return ret; //never reach this point
+}
+//*************************************************************************************************
+
+//*************************************************************************************************
+/**
+ * \brief Compute if two geometries intersect. Both can be enlarged by a specified margin.
+ * \param geom1 support function for the first particle
+ * \param geom2 support function for the second particle
+ * \param margin The margin by which the objects will be enlarged.
+ * \return true, if an itersection is found.
+ */
+bool GJK::doGJKmargin(const Support &geom1, const Support &geom2, real_t margin)
+{
+ //Variables
+ Vec3 support; //the current support point
+
+ ////////////////////////////////////////////////////////////////////////
+ //Initial initialisation step
+ supportA_.resize(4);
+ supportB_.resize(4);
+ simplex_.resize(4);
+
+ //get any first support point
+ if(numPoints_ != 0) {
+ normalize(d_);
+ }
+ support = putSupport(geom1, geom2, d_, margin, simplex_, supportA_, supportB_, 0);
+
+ //std::cerr << "Support 1: " << support << std::endl;
+ //add this point to the simplex_
+ numPoints_ = 1;
+
+ //first real_t search direction is in the opposite direction of the first support point
+ d_ = -support;
+
+ /*
+ if(support * d_ < 0.0){
+ //we went as far as we could in direction 'd' but not passed the origin
+ //this means the triangle mashes don't overlap
+ //and as the support()-function extends the support point by contactThreshold
+ //the mashes are not even close enough to be considered in contact.
+ return false;
+ }
+ */
+ ////////////////////////////////////////////////////////////////////////
+ //GJK main loop
+ while (true) {
+ //get the support point in the current search direction
+ normalize(d_);
+ support = putSupport(geom1, geom2, d_, margin, simplex_, supportA_, supportB_, numPoints_);
+
+ //std::cerr << "GJK: Got support storing at " << (int)numPoints_ << ": "<< support << std::endl;
+ //check if "support" is passed the origin in search direction
+ if(support * d_ < 0.0){
+ // std::cerr << support * d_ << ": Returning false." << std::endl;
+ //we went as far as we could in direction 'd' but not passed the origin
+ //this means the triangle meshes don't overlap
+ //and as the support()-function extends the support point by contactThreshold
+ //the meshes are not even close enough to be considered in contact.
+ return false;
+ }
+
+ //add the new support point into the simplex
+ numPoints_++;
+
+ //std::cerr << "Num points " << (int)numPoints_ << std::endl;
+ ////////////////////////////////////////////////////////////////
+ //check if the origin is in the simplex
+ //if it is the triangle mashes are overlapping
+ switch(numPoints_)
+ {
+ case 2:
+ {
+ if(simplex2(d_)) {
+
+ //std::cerr << "Simplex2 success." << std::endl;
+ while(simplex_.size() > numPoints_){
+ simplex_.pop_back();
+ supportA_.pop_back();
+ supportB_.pop_back();
+ }
+ return true;
+ }
+ }
+ break;
+
+ case 3:
+ {
+ if(simplex3(d_)) {
+ //std::cerr << "Simplex3 success." << std::endl;
+ while(simplex_.size() > numPoints_){
+ simplex_.pop_back();
+ supportA_.pop_back();
+ supportB_.pop_back();
+ }
+ return true;
+ }
+ }
+ break;
+
+ case 4:
+ {
+ if(simplex4(d_)) {
+ //std::cerr << "Simplex4 success." << std::endl;
+ return true;
+ }
+ }
+ break;
+
+ default:
+ {
+ //std::cerr << "numPoints_="<< numPoints_ <<std::endl;
+ WALBERLA_ABORT( "Number of points in the simplex is not 1<=n<=4" );
+ }
+ break;
+ }
+ }
+
+ return false; //never reach this point
+}
+//*************************************************************************************************
+
+
+//*************************************************************************************************
+/**
+ * \brief Calculate clostes Point in the simplex and its distance to the origin.
+ */
+inline real_t GJK::calcDistance( Vec3& normal, Vec3& contactPoint )
+{
+ //find the point in the simplex closest to the origin#
+ //its distance to the origin is the distance of the two objects
+ real_t dist= 0.0;
+
+ real_t barCoords[3] = { 0.0, 0.0, 0.0};
+ real_t& u = barCoords[0];
+ real_t& v = barCoords[1];
+ real_t& w = barCoords[2];
+
+ Vec3& A = simplex_[0];
+ Vec3& B = simplex_[1];
+ Vec3& C = simplex_[2];
+ //std::cerr << (int) numPoints_ << " " << A << B << C << std::endl;
+ switch(numPoints_){
+ case 1:
+ {
+ //the only point in simplex is closest to Origin
+ dist = std::sqrt(A.sqrLength());
+ u = real_t(1.0);
+ break;
+ }
+ case 2:
+ {
+ //calc distance Origin to line segment
+ //it is definitively closest do the segment not to one of the end points
+ //as the voronoi regions of the points also consist of the border borderline between
+ //point region and segment region
+ // compare "Real-Time Collision Detection" by Christer Ericson page 129ff
+ Vec3 ab = B - A;
+ //Vec3 ac = -A;
+ //Vec3 bc = -simplex[1];
+
+ //calc baryzenctric coordinats
+ // compare "Real-Time Collision Detection" by Christer Ericson page 129
+ //double t = ac*ab;
+ real_t t = real_t(-1.0) * (A * ab);
+ real_t denom = std::sqrt(ab.sqrLength());
+ u = t / denom;
+ v = real_t(1.0) - u;
+ Vec3 closestPoint = u*A + v*B;
+ dist = std::sqrt(closestPoint.sqrLength());
+ // compare "Real-Time Collision Detection" by Christer Ericson page 130
+ //double& e = t;
+ //double& f = denom;
+ //dist = ac.sqrLength() - e*e/f;
+ break;
+ }
+ case 3:
+ {
+ //origin is surly in the voronoi region of the face itself
+ //not the bordering lines or one of the 3 points
+ //to be more precise it is also in normal direction.
+ // compare "Real-Time Collision Detection" by Christer Ericson page 139
+ //TODO: evlt kann man das berechnen ohne den projektionspunkt zu bestimmen
+
+ //Vec3 ab= B - A;
+ //Vec3 ac= C - A;
+ //Vec3 bc= C - B;
+ Vec3& n = d_; //we already know the normal
+ const Vec3& nT = n;
+
+ real_t vc = nT * (A % B);
+ real_t va = nT * (B % C);
+ real_t vb = nT * (C % A);
+ real_t denom = real_t(1.0) / (va + vb + vc);
+ u = va * denom;
+ v = vb * denom;
+ w = real_t(1.0) - u - v;
+ //std::cerr << u << " " << v << " " << w << std::endl;
+ Vec3 closestPoint = u*A + v*B + w*C;
+ dist = std::sqrt(closestPoint.sqrLength());
+
+ break;
+ }
+ default:
+ {
+ std::cout << "falsche anzahl an Punkten im simplex" <<std::endl;
+ break;
+ }
+ }
+
+ Vec3 pointOnA = u * supportA_[0];
+ Vec3 pointOnB = u * supportB_[0];
+ for( size_t i = 1; i < numPoints_; ++i) {
+ pointOnA += barCoords[i] * supportA_[i];
+ pointOnB += barCoords[i] * supportB_[i];
+ }
+
+ normal = (pointOnA - pointOnB).getNormalized();
+ contactPoint = (pointOnA + pointOnB) * real_t(0.5);
+
+
+ return dist;
+}
+//*************************************************************************************************
+
+
+
+//=================================================================================================
+//
+// UTILITY FUNCTIONS
+//
+//=================================================================================================
+
+//*************************************************************************************************
+/*! \brief Process a simplex with two nodes.
+ */
+bool GJK::simplex2(Vec3& d)
+{
+ //the simplex is a line
+ const Vec3& A = simplex_[1]; //The Point last added to the simplex
+ const Vec3& B = simplex_[0]; //The Point that was already in the simplex
+ const Vec3 AO = -A; //The vector A->O with 0 the origin
+ const Vec3& AOt = AO; //The transposed vector A->O with O the origin
+ const Vec3 AB = B-A; //The vector A->B
+
+ if( sameDirection(AOt, AB) ) {
+ //The origin O is in the same direction as B is so the line AB is closest to the origin
+ //=> keep A and B in the simplex
+ d = AB % AO % AB;
+ }
+ else {
+ //The origin is not in the direction of B seen from A.
+ //So O lies in the voronoi region of A
+ //=> simplex is just A
+ simplex_[0] = A; //aka simplex_[1]
+ supportA_[0] = supportA_[1];
+ supportB_[0] = supportB_[1];
+ numPoints_ = 1;
+ d = AO;
+ }
+
+ //if the new search direction has zero length
+ //than the origin is on the simplex
+ if(zeroLengthVector(d)) {
+ d_ = Vec3(real_t(0.0),real_t(0.6),real_t(0.8)); // give the GJK a chance to rerun
+ return true;
+ }
+ return false;
+}
+//*************************************************************************************************
+
+
+//*************************************************************************************************
+/*! \brief Process a simplex with three nodes.
+ */
+bool GJK::simplex3(Vec3& d)
+{
+ //the simplex is a triangle
+ const Vec3& A = simplex_[2]; //The Point last added to the simplex
+ const Vec3& B = simplex_[1]; //One Point that was already in the simplex
+ const Vec3& C = simplex_[0]; //One Point that was already in the simplex
+ //ABC is a conterclockwise triangle
+
+ const Vec3 AO = -A; //The vector A->O with 0 the origin
+ const Vec3& AOt = AO; //The transposed vector A->O with O the origin
+ const Vec3 AB = B-A; //The vector A->B
+ const Vec3 AC = C-A; //The vector A->C
+ const Vec3 ABC = AB%AC; //The the normal pointing towards the viewer if he sees a CCW triangle ABC
+
+ if( sameDirection(AOt, (AB % ABC)) ) {
+ //Origin is on the outside of the triangle of the line AB
+ if( AOt * AB > 0.0) {
+ //Origin in the voronoi region of AB outside the triangle
+ //=> AB is the new simplex
+ simplex_[0] = B; //aka simplex_[1]
+ simplex_[1] = A; //aka simplex_[2]
+ supportA_[0] = supportA_[1];
+ supportA_[1] = supportA_[2];
+ supportB_[0] = supportB_[1];
+ supportB_[1] = supportB_[2];
+ numPoints_ = 2;
+ d = AB % AO % AB;
+
+
+ }
+ else {
+ //STAR
+ if( sameDirection(AOt,AC) ) {
+ //Origin is on a subspace of the voronio region of AC
+ //=> AC is the new simplex
+ //simplex_[0] = C; //aka simplex_[0] already there
+ simplex_[1] = A; //aka simplex_[2]
+ supportA_[1] = supportA_[2];
+ supportB_[1] = supportB_[2];
+ numPoints_ = 2;
+ d = AC % AO % AC;
+ }
+ else {
+ //Origin is in the voronio region of A
+ //=> A is the new simplex
+ simplex_[0] = A; //aka simplex_[2]
+ supportA_[0] = supportA_[2];
+ supportB_[0] = supportB_[2];
+ numPoints_ = 1;
+ d = AO;
+ }
+ }
+ }
+ else {
+ if( sameDirection(AOt, (ABC % AC)) ) {
+ //Origin is on the outside of the triangle of the line AC
+ //STAR
+ if( AOt * AC > 0.0) {
+ //Origin is on a subspace of the voronio region of AC
+ //=> AC is the new simplex
+ //simplex_[0] = C; //aka simplex_[0] already there
+ simplex_[1] = A; //aka simplex_[2]
+ supportA_[1] = supportA_[2];
+ supportB_[1] = supportB_[2];
+ numPoints_ = 2;
+ d = AC % AO % AC;
+ }
+ else {
+ //Origin is in the voronio region of A
+ //=> A is the new simplex
+ simplex_[0] = A; //aka simplex_[2]
+ supportA_[0] = supportA_[2];
+ supportB_[0] = supportB_[2];
+ numPoints_ = 1;
+ d = AO;
+ }
+ }
+ else {
+ //origin is above or below the triangle ABC but its mapping on the plane ABC lies within ABC
+ if( sameDirection(AOt, ABC) ) {
+ //Origin is above the triangle
+ //=>Keep triangle as simplex seen from the origin it is already CCW
+ d = ABC;
+ }
+ else {
+ if( sameDirection(AOt, -ABC) ) {
+ //Origin is below the triangle
+ //=>Keep triangle as simplex.
+ //seen from the origin ABC is CW so change the winding
+ Vec3 temp = B; //aka simplex_[1]
+ simplex_[1] = C; //aka simplex_[0]
+ simplex_[0] = temp;
+ //simplex_[2] = A; //aka simplex_[2] already there
+ //old simplex 2:A 1:B 0:C
+ //simplex now contains 2:A 1:C 0:B
+
+ temp = supportA_[1];
+ supportA_[1] = supportA_[0];
+ supportA_[0] = temp;
+ temp = supportB_[1];
+ supportB_[1] = supportB_[0];
+ supportB_[0] = temp;
+
+ d = -ABC;
+ }
+ else{
+ //Origin lies in the triangle
+ return true;
+ }
+ }
+ }
+ }
+
+ //if the new search direction has zero length
+ //than the origin is on the boundary of the simplex
+ if(zeroLengthVector(d)) {
+ d_ = Vec3(real_t(0.0),real_t(0.6),real_t(0.8)); // give the GJK a chance to rerun
+ return true;
+ }
+ return false;
+}
+//*************************************************************************************************
+
+
+//*************************************************************************************************
+/*! \brief Process a simplex with four nodes.
+ */
+bool GJK::simplex4(Vec3& d)
+{
+ //the simplex is a tetrahedron
+ const Vec3& A = simplex_[3]; //The Point last added to the tetrahedron
+ //t in front mens just a temp varialble
+ const Vec3& B = simplex_[2]; //One Point that was already in the simplex
+ const Vec3& C = simplex_[1]; //One Point that was already in the simplex
+ const Vec3& D = simplex_[0];
+ //BCD is a clockwise triangle wenn seen from A
+
+ const Vec3 AO = -A; //The vector A->O with 0 the origin
+ const Vec3& AOt = AO; //The transposed vector A->O with O the origin
+ const Vec3 AB = B-A; //The vector A->B
+ const Vec3 AC = C-A; //The vector A->C
+ const Vec3 AD = D-A; //The vector A-D
+
+ //https://mollyrocket.com/forums/viewtopic.php?p=1829#1829
+ unsigned char testWhere = 0;
+
+ const Vec3 ABC = AB % AC; //The the normal pointing out of the tetrahedron towards the viewer if he sees a CCW triangle ABC
+ const Vec3 ACD = AC % AD; //The the normal pointing out of the tetrahedron towards the viewer if he sees a CCW triangle ACD
+ const Vec3 ADB = AD % AB; //The the normal pointing out of the tetrahedron towards the viewer if he sees a CCW triangle ADB
+
+ if(sameDirection(AOt, ABC)) {
+ testWhere |= 0x1;
+ }
+
+ if(sameDirection(AOt, ACD)) {
+ testWhere |= 0x2;
+ }
+
+ if(sameDirection(AOt, ADB)) {
+ testWhere |= 0x4;
+ }
+
+ switch(testWhere)
+ {
+ case 0:
+ {
+ //origin is in the tetrahedro
+ //=> the two triangle mashes overlap
+ //std::cout << "Origin is within the tetrahedron\nA=" << A << " B=" << B << " C="<< C << " D="<< D << std::endl;
+ return true;
+ } break;
+
+ case 1:
+ {
+ // In front of ABC only
+ //Origin is outside the tetrahedron above ABC
+ //=> rearrange simplex to use the triangle case
+ simplex_[0] = C; //aka simplex_[1] 0:C
+ simplex_[1] = B; //aka simplex_[2] 1:B
+ simplex_[2] = A; //aka simplex_[3] 2:A
+
+ supportA_[0] = supportA_[1];
+ supportA_[1] = supportA_[2];
+ supportA_[2] = supportA_[3];
+ supportB_[0] = supportB_[1];
+ supportB_[1] = supportB_[2];
+ supportB_[2] = supportB_[3];
+
+ numPoints_ = 3;
+
+ return simplex3(d);
+ } break;
+
+ case 2:
+ {
+ // In front of ACD only
+ //Origin is outside the tetrahedron above ACD
+ //=> rearrange simplex to use the triangle case
+ //simplex_[0] = D; //aka simplex_[0] 0:D already the case
+ //simplex_[1] = C; //aka simplex_[1] 1:C already the case
+ simplex_[2] = A; //aka simplex_[3] 2:A
+
+ supportA_[2] = supportA_[3];
+ supportB_[2] = supportB_[3];
+
+ numPoints_ = 3;
+
+ return simplex3(d);
+ } break;
+
+ case 4:
+ {
+ // In front of ADB only
+ //Origin is outside the tetrahedron above ADB
+ //=> rearrange simplex to use the triangle case
+ simplex_[1] = D; //aka simplex_[0] 1:D
+ simplex_[0] = B; //aka simplex_[2] 0:B already there
+ simplex_[2] = A; //aka simplex_[3] 2:A
+
+ supportA_[1] = supportA_[0];
+ supportA_[0] = supportA_[2];
+ supportA_[2] = supportA_[3];
+ supportB_[1] = supportB_[0];
+ supportB_[0] = supportB_[2];
+ supportB_[2] = supportB_[3];
+
+ numPoints_ = 3;
+
+ return simplex3(d);
+ } break;
+
+ case 3:
+ {
+ // In front of ABC and ACD
+ if(sameDirection(AOt, ABC%AC)) {
+ if(sameDirection(AOt, AC%ACD)) {
+ if(sameDirection(AOt, AC)) {
+ //AddEdgeSimplex(A, C);
+ simplex_[0] = C; //aka simplex_[1] 0:C
+ simplex_[1] = A; //aka simplex_[3] 1:A
+
+ supportA_[0] = supportA_[1];
+ supportA_[1] = supportA_[3];
+ supportB_[0] = supportB_[1];
+ supportB_[1] = supportB_[3];
+
+ numPoints_ = 2;
+ d = AC % AO % AC;
+ }
+ else {
+ //AddPointSimplex;
+ simplex_[0] = A; //aka simplex_[3] 0:A
+
+ supportA_[0] = supportA_[3];
+ supportB_[0] = supportB_[3];
+
+ numPoints_ = 1;
+ d = AO;
+ }
+ }
+ else
+ {
+ if(sameDirection(AOt, ACD%AD)) {
+ //AddEdgeSimplex(A, D);
+ //simplex_[0] = D; //aka simplex_[0] 0:D already there
+ simplex_[1] = A; //aka simplex_[3] 1:A
+
+ supportA_[1] = supportA_[3];
+ supportB_[1] = supportB_[3];
+
+ numPoints_ = 2;
+ d = AD % AO % AD;
+ }
+ else {
+ //AddTriangleSimplex(A, C, D);
+ //simplex_[0] = D; //aka simplex_[0] 0:D already there
+ //simplex_[1] = C; //aka simplex_[1] 1:C already there
+ simplex_[2] = A; //aka simplex_[3] 2:A
+
+ supportA_[2] = supportA_[3];
+ supportB_[2] = supportB_[3];
+
+ numPoints_ = 3;
+ d = ACD;
+ }
+ }
+ }
+ else
+ {
+ if(sameDirection(AOt, AB%ABC)) {
+ if(sameDirection(AOt, AB)) {
+ //AddEdgeSimplex(A, B);
+ simplex_[0] = B; //aka simplex_[2] 0:B
+ simplex_[1] = A; //aka simplex_[3] 1:A
+
+ supportA_[0] = supportA_[2];
+ supportA_[1] = supportA_[3];
+ supportB_[0] = supportB_[2];
+ supportB_[1] = supportB_[3];
+
+ numPoints_ = 2;
+ d = AB % AO % AB;
+ }
+ else {
+ //AddPointSimplex;
+ simplex_[0] = A; //aka simplex_[3] 0:A
+
+ supportA_[0] = supportA_[3];
+ supportB_[0] = supportB_[3];
+
+ numPoints_ = 1;
+ d = AO;
+ }
+ }
+ else {
+ //AddTriangleSimplex(A, B, C);
+ simplex_[0] = C; //aka simplex_[1] 0:C
+ simplex_[1] = B; //aka simplex_[2] 1:B
+ simplex_[2] = A; //aka simplex_[3] 2:A
+
+ supportA_[0] = supportA_[1];
+ supportA_[1] = supportA_[2];
+ supportA_[2] = supportA_[3];
+ supportB_[0] = supportB_[1];
+ supportB_[1] = supportB_[2];
+ supportB_[2] = supportB_[3];
+
+ numPoints_ = 3;
+ d = ABC;
+ }
+ }
+ } break;
+
+
+ case 5:
+ {
+ // In front of ADB and ABC
+ if(sameDirection(AOt, ADB%AB)) {
+ if(sameDirection(AOt, AB%ABC)) {
+ if(sameDirection(AOt, AB)) {
+ //AddEdgeSimplex(A, B);
+ simplex_[0] = B; //aka simplex_[2] 0:B
+ simplex_[1] = A; //aka simplex_[3] 1:A
+
+ supportA_[0] = supportA_[2];
+ supportA_[1] = supportA_[3];
+ supportB_[0] = supportB_[2];
+ supportB_[1] = supportB_[3];
+
+ numPoints_ = 2;
+ d = AB % AO % AB;
+ }
+ else {
+ //AddPointSimplex;
+ simplex_[0] = A; //aka simplex_[3] 0:A
+
+ supportA_[0] = supportA_[3];
+ supportB_[0] = supportB_[3];
+
+ numPoints_ = 1;
+ d = AO;
+ }
+ }
+ else
+ {
+ if(sameDirection(AOt, ABC%AC)) {
+ //AddEdgeSimplex(A, C);
+ simplex_[0] = C; //aka simplex_[1] 0:C
+ simplex_[1] = A; //aka simplex_[3] 1:A
+
+ supportA_[0] = supportA_[1];
+ supportA_[1] = supportA_[3];
+ supportB_[0] = supportB_[1];
+ supportB_[1] = supportB_[3];
+
+ numPoints_ = 2;
+ d = AC % AO % AC;
+ }
+ else {
+ //AddTriangleSimplex(A, B, C);
+ simplex_[0] = C; //aka simplex_[1] 0:C
+ simplex_[1] = B; //aka simplex_[2] 1:B
+ simplex_[2] = A; //aka simplex_[3] 2:A
+
+ supportA_[0] = supportA_[1];
+ supportA_[1] = supportA_[2];
+ supportA_[2] = supportA_[3];
+ supportB_[0] = supportB_[1];
+ supportB_[1] = supportB_[2];
+ supportB_[2] = supportB_[3];
+
+ numPoints_ = 3;
+ d = ABC;
+ }
+ }
+ }
+ else
+ {
+ if(sameDirection(AOt, AD%ADB)) {
+ if(sameDirection(AOt, AD)) {
+ //AddEdgeSimplex(A, D);
+ //simplex_[0] = D; //aka simplex_[0] 0:D already there
+ simplex_[1] = A; //aka simplex_[3] 1:A
+
+ supportA_[1] = supportA_[3];
+ supportB_[1] = supportB_[3];
+
+ numPoints_ = 2;
+ d = AD % AO % AD;
+ }
+ else {
+ //AddPointSimplex;
+ simplex_[0] = A; //aka simplex_[3] 0:A
+
+ supportA_[0] = supportA_[3];
+ supportB_[0] = supportB_[3];
+
+ numPoints_ = 1;
+ d = AO;
+ }
+ }
+ else {
+ //AddTriangleSimplex(A, D, B);
+ simplex_[1] = D; //aka simplex[0] 1:D
+ simplex_[0] = B; //aka simplex[2] 0:B
+ simplex_[2] = A; //aka simplex[3] 2:A
+
+ supportA_[1] = supportA_[0];
+ supportA_[0] = supportA_[2];
+ supportA_[2] = supportA_[3];
+ supportB_[1] = supportB_[0];
+ supportB_[0] = supportB_[2];
+ supportB_[2] = supportB_[3];
+
+ numPoints_ = 3;
+
+ numPoints_ = 3;
+ d = ADB;
+ }
+ }
+ } break;
+
+ case 6:
+ {
+ // In front of ACD and ADB
+ if(sameDirection(AOt, ACD%AD)) {
+ if(sameDirection(AOt, AD%ADB)) {
+ if(sameDirection(AOt, AD)) {
+ //AddEdgeSimplex(A, D);
+ //simplex_[0] = D; //aka simplex_[0] 0:D already there
+ simplex_[1] = A; //aka simplex_[3] 1:A
+
+ supportA_[1] = supportA_[3];
+ supportB_[1] = supportB_[3];
+
+ numPoints_ = 2;
+ d = AD % AO % AD;
+ }
+ else {
+ //AddPointSimplex;
+ simplex_[0] = A; //aka simplex_[3] 0:A
+
+ supportA_[0] = supportA_[3];
+ supportB_[0] = supportB_[3];
+
+ numPoints_ = 1;
+ d = AO;
+ }
+ }
+ else
+ {
+ if(sameDirection(AOt, ADB%AB)) {
+ //AddEdgeSimplex(A, B);
+ simplex_[0] = B; //aka simplex_[2] 0:B
+ simplex_[1] = A; //aka simplex_[3] 1:A
+
+ supportA_[0] = supportA_[2];
+ supportA_[1] = supportA_[3];
+ supportB_[0] = supportB_[2];
+ supportB_[1] = supportB_[3];
+
+ numPoints_ = 2;
+ d = AB % AO % AB;
+ }
+ else {
+ //AddTriangleSimplex(A, D, B);
+ simplex_[1] = D; //aka simplex[0] 1:D
+ simplex_[0] = B; //aka simplex[2] 0:B
+ simplex_[2] = A; //aka simplex[3] 2:A
+
+ supportA_[1] = supportA_[0];
+ supportA_[0] = supportA_[2];
+ supportA_[2] = supportA_[3];
+ supportB_[1] = supportB_[0];
+ supportB_[0] = supportB_[2];
+ supportB_[2] = supportB_[3];
+
+ numPoints_ = 3;
+
+ numPoints_ = 3;
+ d = ADB;
+ }
+ }
+ }
+ else
+ {
+ if(sameDirection(AOt, AC%ACD)) {
+ if(sameDirection(AOt, AC)) {
+ //AddEdgeSimplex(A, C);
+ simplex_[0] = C; //aka simplex_[1] 0:C
+ simplex_[1] = A; //aka simplex_[3] 1:A
+
+ supportA_[0] = supportA_[1];
+ supportA_[1] = supportA_[3];
+ supportB_[0] = supportB_[1];
+ supportB_[1] = supportB_[3];
+
+ numPoints_ = 2;
+ d = AC % AO % AC;
+ }
+ else
+ {
+ //AddPointSimplex;
+ simplex_[0] = A; //aka simplex_[3] 0:A
+
+ supportA_[0] = supportA_[3];
+ supportB_[0] = supportB_[3];
+
+ numPoints_ = 1;
+ d = AO;
+ }
+ }
+ else
+ {
+ //AddTriangleSimplex(A, C, D);
+ //simplex_[0] = D; //aka simplex_[0] 0:D already there
+ //simplex_[1] = C; //aka simplex_[1] 1:C already there
+ simplex_[2] = A; //aka simplex_[3] 2:A
+
+ supportA_[2] = supportA_[3];
+ supportB_[2] = supportB_[3];
+
+ numPoints_ = 3;
+ d = ACD;
+ }
+ }
+ } break;
+
+ case 7:
+ {
+ // In front of ABC, ACD and ADB
+ if(sameDirection(AOt, AB)) {
+ simplex_[0] = B; //aka simplex_[2] 0:B
+ simplex_[1] = A; //aka simplex_[3] 1:A
+
+ supportA_[0] = supportA_[2];
+ supportA_[1] = supportA_[3];
+ supportB_[0] = supportB_[2];
+ supportB_[1] = supportB_[3];
+
+ numPoints_ = 2;
+ d = AB % AO % AB;
+ }
+ else
+ {
+ if(sameDirection(AOt, AC)) {
+ simplex_[0] = C; //aka simplex_[1] 0:C
+ simplex_[1] = A; //aka simplex_[3] 1:A
+
+ supportA_[0] = supportA_[1];
+ supportA_[1] = supportA_[3];
+ supportB_[0] = supportB_[1];
+ supportB_[1] = supportB_[3];
+
+ numPoints_ = 2;
+ d = AC % AO % AC;
+
+ }
+ else
+ {
+ if(sameDirection(AOt, AD)) {
+ //simplex_[0] = D; //aka simplex_[1] 0:D already there
+ simplex_[1] = A; //aka simplex_[3] 1:A
+
+ supportA_[1] = supportA_[3];
+ supportB_[1] = supportB_[3];
+
+ numPoints_ = 2;
+ d = AD % AO % AD;
+ }
+ else {
+ simplex_[0] = A; //aka simplex_[3] 0:A
+
+ supportA_[0] = supportA_[3];
+ supportB_[0] = supportB_[3];
+
+ numPoints_ = 1;
+ d = AO;
+ }
+ }
+ }
+ } break;
+ default:
+ {
+ //all 8 cases 0-7 are covered
+ } break;
+ }
+
+ return false;
+}
+//*************************************************************************************************
+
+} //collision_detection
+} //mesa_pd
+} //walberla
diff --git a/src/mesa_pd/collision_detection/GJK.h b/src/mesa_pd/collision_detection/GJK.h
new file mode 100644
index 0000000000000000000000000000000000000000..96d6d5be8220dd16ef93e73a4951a5a65af67277
--- /dev/null
+++ b/src/mesa_pd/collision_detection/GJK.h
@@ -0,0 +1,110 @@
+//======================================================================================================================
+//
+// This file is part of waLBerla. waLBerla is free software: you can
+// redistribute it and/or modify it under the terms of the GNU General Public
+// License as published by the Free Software Foundation, either version 3 of
+// the License, or (at your option) any later version.
+//
+// waLBerla is distributed in the hope that it will be useful, but WITHOUT
+// ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+// FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
+// for more details.
+//
+// You should have received a copy of the GNU General Public License along
+// with waLBerla (see COPYING.txt). If not, see <http://www.gnu.org/licenses/>.
+//
+//! \file GJK.h
+//! \author Tobias Scharpff
+//! \author Tobias Leemann
+//
+//======================================================================================================================
+
+#pragma once
+
+#include <mesa_pd/collision_detection/Support.h>
+#include <mesa_pd/data/DataTypes.h>
+
+#include <vector>
+
+#include <core/Abort.h>
+#include <core/math/Limits.h>
+#include <core/math/Vector3.h>
+
+namespace walberla {
+namespace mesa_pd {
+namespace collision_detection {
+
+//*************************************************************************************************
+/*!\brief Impelementation of the Gilbert-Johnson-Keerthi Algorithm.
+ */
+class GJK
+{
+public:
+
+ //**Constructor*********************************************************************************
+ /*! \name Constructor */
+ //@{
+ explicit GJK();
+ //@}
+ //**********************************************************************************************
+
+ //**Query functions*****************************************************************************
+ /*! \name Query functions */
+ //@{
+ real_t doGJK( const Support &geom1, const Support &geom2, Vec3& normal, Vec3& contactPoint );
+
+ bool doGJKmargin( const Support &geom1, const Support &geom2, const real_t margin = real_t(0));
+ //@}
+ //**********************************************************************************************
+
+ //**Get functions*******************************************************************************
+ /*! \name Get functions */
+ //@{
+ inline const std::vector<Vec3>& getSimplex() const { return simplex_; }
+ inline size_t getSimplexSize() const { return numPoints_; }
+ inline const std::vector<Vec3>& getSupportA() const { return supportA_; }
+ inline const std::vector<Vec3>& getSupportB() const { return supportB_; }
+ //@}
+ //**********************************************************************************************
+
+private:
+ //**Utility functions***************************************************************************
+ /*! \name Utility functions */
+ //@{
+ bool simplex2(Vec3& d);
+ bool simplex3(Vec3& d);
+ bool simplex4(Vec3& d);
+
+ /// Checks if two vectors roughly point in the same direction
+ inline bool sameDirection ( const Vec3& vec1, const Vec3& vec2 ) const { return vec1 * vec2 > real_t(0.0); }
+ /// Checks if the length of a vector is zero or as close to zero that it can not be distinguished form zero
+ inline bool zeroLengthVector( const Vec3& vec ) const { return vec.sqrLength() < math::Limits<real_t>::fpuAccuracy(); }
+ real_t calcDistance ( Vec3& normal, Vec3& contactPoint );
+
+ inline const Vec3 putSupport(const Support &geom1,
+ const Support &geom2,
+ const Vec3& dir,
+ const real_t margin,
+ std::vector<Vec3> &simplex,
+ std::vector<Vec3> &supportA,
+ std::vector<Vec3> &supportB,
+ size_t index);
+ //@}
+ //**********************************************************************************************
+
+ //**Member variables****************************************************************************
+ /*! \name Member variables */
+ //@{
+ std::vector<Vec3> simplex_; ///< Container to hold the simplex.
+ std::vector<Vec3> supportA_; ///< Container to hold the support points generated in triangle mesh mA
+ std::vector<Vec3> supportB_; ///< Container to hold the support points generated in triangle mesh mB
+ unsigned char numPoints_; ///< Current number of points in the simplex.
+ Vec3 d_; ///< The next search direction.
+ //@}
+ //**********************************************************************************************
+};
+//*************************************************************************************************
+
+} // namespace collision_detection
+} // namespace mesa_pd
+} // namespace walberla
diff --git a/src/mesa_pd/collision_detection/GeneralContactDetection.h b/src/mesa_pd/collision_detection/GeneralContactDetection.h
new file mode 100644
index 0000000000000000000000000000000000000000..bf6d3fad69ca37ecd89bc56ed296a4ef66184a2b
--- /dev/null
+++ b/src/mesa_pd/collision_detection/GeneralContactDetection.h
@@ -0,0 +1,217 @@
+//======================================================================================================================
+//
+// This file is part of waLBerla. waLBerla is free software: you can
+// redistribute it and/or modify it under the terms of the GNU General Public
+// License as published by the Free Software Foundation, either version 3 of
+// the License, or (at your option) any later version.
+//
+// waLBerla is distributed in the hope that it will be useful, but WITHOUT
+// ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+// FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
+// for more details.
+//
+// You should have received a copy of the GNU General Public License along
+// with waLBerla (see COPYING.txt). If not, see <http://www.gnu.org/licenses/>.
+//
+//! \file GeneralContactDetection.h
+//! \author Sebastian Eibl <sebastian.eibl@fau.de>
+//
+//======================================================================================================================
+
+#pragma once
+
+#include <mesa_pd/collision_detection/AnalyticContactDetection.h>
+#include <mesa_pd/collision_detection/EPA.h>
+#include <mesa_pd/collision_detection/GJK.h>
+#include <mesa_pd/collision_detection/Support.h>
+#include <mesa_pd/data/DataTypes.h>
+#include <mesa_pd/data/shape/BaseShape.h>
+#include <mesa_pd/data/shape/Box.h>
+#include <mesa_pd/data/shape/CylindricalBoundary.h>
+#include <mesa_pd/data/shape/Ellipsoid.h>
+#include <mesa_pd/data/shape/HalfSpace.h>
+#include <mesa_pd/data/shape/Sphere.h>
+
+#include <core/logging/Logging.h>
+
+namespace walberla {
+namespace mesa_pd {
+namespace collision_detection {
+
+/**
+ * Collision detection functor which uses recommended collision functions.
+ *
+ * Calculates and stores contact information between two particles.
+ * If a collision was successfully detected by the operator()
+ * contactPoint, contactNormal and penetrationDepth contain the
+ * contact information. If no collision was detected the values of
+ * these variables is undefined!
+ */
+class GeneralContactDetection : public AnalyticContactDetection
+{
+public:
+ using AnalyticContactDetection::operator();
+
+ template <typename GEO1_T, typename GEO2_T, typename Accessor>
+ bool operator()( const size_t idx1,
+ const size_t idx2,
+ const GEO1_T& geo1,
+ const GEO2_T& geo2,
+ Accessor& ac);
+
+ template <typename GEO2_T, typename Accessor>
+ bool operator()(const size_t idx1,
+ const size_t idx2,
+ const data::HalfSpace& geo1,
+ const GEO2_T& geo2,
+ Accessor& ac);
+
+ template <typename GEO1_T, typename Accessor>
+ bool operator()(const size_t idx1,
+ const size_t idx2,
+ const GEO1_T& geo1,
+ const data::HalfSpace& geo2,
+ Accessor& ac);
+
+ template <typename GEO2_T, typename Accessor>
+ bool operator()(const size_t idx1,
+ const size_t idx2,
+ const data::CylindricalBoundary& geo1,
+ const GEO2_T& geo2,
+ Accessor& ac);
+
+ template <typename GEO1_T, typename Accessor>
+ bool operator()(const size_t idx1,
+ const size_t idx2,
+ const GEO1_T& geo1,
+ const data::CylindricalBoundary& geo2,
+ Accessor& ac);
+private:
+ bool collideGJKEPA(Support& geom0, Support& geom1);
+};
+
+template <typename GEO1_T, typename GEO2_T, typename Accessor>
+bool GeneralContactDetection::operator()( const size_t idx1,
+ const size_t idx2,
+ const GEO1_T& geo1,
+ const GEO2_T& geo2,
+ Accessor& ac)
+{
+ WALBERLA_ASSERT_UNEQUAL(idx1, idx2, "colliding with itself!");
+
+ //ensure collision order idx2 has to be larger than idx1
+ if (ac.getUid(idx2) < ac.getUid(idx1))
+ return operator()(idx2, idx1, geo2, geo1, ac);
+
+ getIdx1() = idx1;
+ getIdx2() = idx2;
+
+ Support e1(ac.getPosition(getIdx1()), ac.getRotation(getIdx1()), geo1);
+ Support e2(ac.getPosition(getIdx2()), ac.getRotation(getIdx2()), geo2);
+ return collideGJKEPA(e1, e2);
+}
+
+template <typename GEO2_T, typename Accessor>
+bool GeneralContactDetection::operator()(const size_t idx1,
+ const size_t idx2,
+ const data::HalfSpace& geo1,
+ const GEO2_T& geo2,
+ Accessor& ac)
+{
+ getIdx1() = idx1;
+ getIdx2() = idx2;
+
+ Support sup(ac.getPosition(idx2), ac.getRotation(idx2), geo2);
+ Vec3 support_dir = -geo1.getNormal();
+ // We now have a direction facing to the "wall".
+ // Compute support point of body b in this direction. This will be the furthest point overlapping.
+ Vec3 contactp = sup.support(support_dir);
+ real_t pdepth = contactp * geo1.getNormal() - ac.getPosition(idx1) * geo1.getNormal();
+ if(pdepth < contactThreshold_)
+ { //We have a collision
+ contactNormal_ = support_dir;
+ penetrationDepth_ = pdepth;
+ contactPoint_ = contactp + real_t(0.5) * penetrationDepth_ * contactNormal_;
+ return true;
+ } else
+ { //No collision
+ return false;
+ }
+}
+
+template <typename GEO1_T, typename Accessor>
+bool GeneralContactDetection::operator()(const size_t idx1,
+ const size_t idx2,
+ const GEO1_T& geo1,
+ const data::HalfSpace& geo2,
+ Accessor& ac)
+{
+ return operator()(idx2, idx1, geo2, geo1, ac);
+}
+
+template <typename GEO2_T, typename Accessor>
+bool GeneralContactDetection::operator()(const size_t idx1,
+ const size_t idx2,
+ const data::CylindricalBoundary& geo1,
+ const GEO2_T& geo2,
+ Accessor& ac)
+{
+ getIdx1() = idx1;
+ getIdx2() = idx2;
+
+ Support sup(ac.getPosition(idx2), ac.getRotation(idx2), geo2);
+
+ WALBERLA_CHECK_FLOAT_EQUAL(geo1.getAxis().sqrLength(), real_t(1));
+
+ auto d = ac.getPosition(idx2) - (dot((ac.getPosition(idx2) - ac.getPosition(idx1)), geo1.getAxis()) * geo1.getAxis() + ac.getPosition(idx1));
+ Vec3 farestP = sup.support(d);
+ auto d2 = farestP - (dot((farestP - ac.getPosition(idx1)), geo1.getAxis()) * geo1.getAxis() + ac.getPosition(idx1));
+ real_t dist = d2.sqrLength();
+
+ if(dist > geo1.getRadius() * geo1.getRadius())
+ { //We have a collision
+ penetrationDepth_ = geo1.getRadius() - std::sqrt(dist);
+ normalize(d2);
+ contactNormal_ = d2;
+ contactPoint_ = farestP + d2 * penetrationDepth_ * real_t(0.5);
+ return true;
+ } else
+ { //No collision
+ return false;
+ }
+}
+
+template <typename GEO1_T, typename Accessor>
+bool GeneralContactDetection::operator()(const size_t idx1,
+ const size_t idx2,
+ const GEO1_T& geo1,
+ const data::CylindricalBoundary& geo2,
+ Accessor& ac)
+{
+ return operator()(idx2, idx1, geo2, geo1, ac);
+}
+
+bool GeneralContactDetection::collideGJKEPA(Support& geom0, Support& geom1)
+{
+ real_t margin = real_t(1e-4);
+ GJK gjk;
+ if(gjk.doGJKmargin(geom0, geom1, margin))
+ {
+ EPA epa;
+ epa.useSphereOptimization(false);
+ if (epa.doEPAmargin(geom0, geom1, gjk, contactNormal_, contactPoint_, penetrationDepth_, margin))
+ {
+ return true;
+ } else
+ {
+ return false;
+ }
+ } else
+ {
+ return false;
+ }
+}
+
+} //namespace collision_detection
+} //namespace mesa_pd
+} //namespace walberla
diff --git a/src/mesa_pd/collision_detection/Support.h b/src/mesa_pd/collision_detection/Support.h
new file mode 100644
index 0000000000000000000000000000000000000000..10e64af5e770d067ca496471fa39580eb29269d2
--- /dev/null
+++ b/src/mesa_pd/collision_detection/Support.h
@@ -0,0 +1,69 @@
+//======================================================================================================================
+//
+// This file is part of waLBerla. waLBerla is free software: you can
+// redistribute it and/or modify it under the terms of the GNU General Public
+// License as published by the Free Software Foundation, either version 3 of
+// the License, or (at your option) any later version.
+//
+// waLBerla is distributed in the hope that it will be useful, but WITHOUT
+// ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+// FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
+// for more details.
+//
+// You should have received a copy of the GNU General Public License along
+// with waLBerla (see COPYING.txt). If not, see <http://www.gnu.org/licenses/>.
+//
+//! \file Support.h
+//! \author Tobias Scharpff
+//! \author Tobias Leemann
+//
+//======================================================================================================================
+
+#pragma once
+
+#include <mesa_pd/data/DataTypes.h>
+#include <mesa_pd/data/shape/BaseShape.h>
+
+namespace walberla {
+namespace mesa_pd {
+namespace collision_detection {
+
+class Support
+{
+public:
+ /// \attention Lifetime of shape has to surpass lifetime of Support!!!
+ /// No copy is stored.
+ Support(const Vec3& pos, const Rot3& rot, const data::BaseShape& shape)
+ : pos_(pos)
+ , rot_(rot)
+ , shape_(shape)
+ {}
+
+ /**
+ * \brief Estimates the point which is farthest in direction \a d.
+ *
+ * \param d The normalized search direction in world-frame coordinates.
+ * \return The support point in world-frame coordinates in direction a\ d.
+ */
+ Vec3 support( Vec3 d ) const;
+public:
+ Vec3 pos_;
+ Rot3 rot_;
+ const data::BaseShape& shape_;
+};
+
+inline
+Vec3 Support::support( Vec3 d ) const
+{
+ auto len = d.sqrLength();
+ if (math::equal(len, real_t(0)))
+ return Vec3(0,0,0);
+
+ d = rot_.getMatrix().getTranspose() * d.getNormalized(); //vectorFromWFtoBF
+ d = shape_.support(d);
+ return pos_ + rot_.getMatrix() * d; //vectorFromBFtoWF
+}
+
+} // namespace collision_detection
+} // namespace mesa_pd
+} // namespace walberla
diff --git a/src/mesa_pd/data/ParticleStorage.h b/src/mesa_pd/data/ParticleStorage.h
index aba34e3fe6f19f5cb4034398737abb07751c10b3..b4b41decb6835558ce00002945d2f70d67d88d83 100644
--- a/src/mesa_pd/data/ParticleStorage.h
+++ b/src/mesa_pd/data/ParticleStorage.h
@@ -528,6 +528,8 @@ void swap(ParticleStorage::Particle lhs, ParticleStorage::Particle rhs)
std::swap(lhs.getNewContactHistoryRef(), rhs.getNewContactHistoryRef());
std::swap(lhs.getTemperatureRef(), rhs.getTemperatureRef());
std::swap(lhs.getHeatFluxRef(), rhs.getHeatFluxRef());
+ std::swap(lhs.getDvRef(), rhs.getDvRef());
+ std::swap(lhs.getDwRef(), rhs.getDwRef());
}
inline
diff --git a/src/mesa_pd/data/ShapeStorage.h b/src/mesa_pd/data/ShapeStorage.h
index 4ab850ff7ac86d53bb05d0ff21082c2f34465466..f7e158d0e40e9bc9c6f16e678b74001b7b27925c 100644
--- a/src/mesa_pd/data/ShapeStorage.h
+++ b/src/mesa_pd/data/ShapeStorage.h
@@ -33,6 +33,7 @@
#include <mesa_pd/data/shape/HalfSpace.h>
#include <mesa_pd/data/shape/CylindricalBoundary.h>
#include <mesa_pd/data/shape/Box.h>
+#include <mesa_pd/data/shape/Ellipsoid.h>
#include <core/Abort.h>
#include <core/debug/Debug.h>
@@ -62,6 +63,7 @@ struct ShapeStorage
static_assert( Sphere::SHAPE_TYPE != HalfSpace::SHAPE_TYPE, "Shape types have to be different!" );
static_assert( Sphere::SHAPE_TYPE != CylindricalBoundary::SHAPE_TYPE, "Shape types have to be different!" );
static_assert( Sphere::SHAPE_TYPE != Box::SHAPE_TYPE, "Shape types have to be different!" );
+static_assert( Sphere::SHAPE_TYPE != Ellipsoid::SHAPE_TYPE, "Shape types have to be different!" );
template <typename ShapeT, typename... Args>
size_t ShapeStorage::create(Args&&... args)
@@ -81,6 +83,7 @@ ReturnType ShapeStorage::singleDispatch( ParticleStorage& ps, size_t idx, func&
case HalfSpace::SHAPE_TYPE : return f(ps, idx, *static_cast<HalfSpace*>(shapes[ps.getShapeID(idx)].get()));
case CylindricalBoundary::SHAPE_TYPE : return f(ps, idx, *static_cast<CylindricalBoundary*>(shapes[ps.getShapeID(idx)].get()));
case Box::SHAPE_TYPE : return f(ps, idx, *static_cast<Box*>(shapes[ps.getShapeID(idx)].get()));
+ case Ellipsoid::SHAPE_TYPE : return f(ps, idx, *static_cast<Ellipsoid*>(shapes[ps.getShapeID(idx)].get()));
default : WALBERLA_ABORT("Shape type (" << shapes[ps.getShapeID(idx)]->getShapeType() << ") could not be determined!");
}
}
@@ -116,6 +119,11 @@ ReturnType ShapeStorage::doubleDispatch( ParticleStorage& ps, size_t idx, size_t
*static_cast<Sphere*>(shapes[ps.getShapeID(idx)].get()),
idy,
*static_cast<Box*>(shapes[ps.getShapeID(idy)].get()));
+ case Ellipsoid::SHAPE_TYPE : return f(ps,
+ idx,
+ *static_cast<Sphere*>(shapes[ps.getShapeID(idx)].get()),
+ idy,
+ *static_cast<Ellipsoid*>(shapes[ps.getShapeID(idy)].get()));
default : WALBERLA_ABORT("Shape type (" << shapes[ps.getShapeID(idy)]->getShapeType() << ") could not be determined!");
}
case HalfSpace::SHAPE_TYPE :
@@ -141,6 +149,11 @@ ReturnType ShapeStorage::doubleDispatch( ParticleStorage& ps, size_t idx, size_t
*static_cast<HalfSpace*>(shapes[ps.getShapeID(idx)].get()),
idy,
*static_cast<Box*>(shapes[ps.getShapeID(idy)].get()));
+ case Ellipsoid::SHAPE_TYPE : return f(ps,
+ idx,
+ *static_cast<HalfSpace*>(shapes[ps.getShapeID(idx)].get()),
+ idy,
+ *static_cast<Ellipsoid*>(shapes[ps.getShapeID(idy)].get()));
default : WALBERLA_ABORT("Shape type (" << shapes[ps.getShapeID(idy)]->getShapeType() << ") could not be determined!");
}
case CylindricalBoundary::SHAPE_TYPE :
@@ -166,6 +179,11 @@ ReturnType ShapeStorage::doubleDispatch( ParticleStorage& ps, size_t idx, size_t
*static_cast<CylindricalBoundary*>(shapes[ps.getShapeID(idx)].get()),
idy,
*static_cast<Box*>(shapes[ps.getShapeID(idy)].get()));
+ case Ellipsoid::SHAPE_TYPE : return f(ps,
+ idx,
+ *static_cast<CylindricalBoundary*>(shapes[ps.getShapeID(idx)].get()),
+ idy,
+ *static_cast<Ellipsoid*>(shapes[ps.getShapeID(idy)].get()));
default : WALBERLA_ABORT("Shape type (" << shapes[ps.getShapeID(idy)]->getShapeType() << ") could not be determined!");
}
case Box::SHAPE_TYPE :
@@ -191,6 +209,41 @@ ReturnType ShapeStorage::doubleDispatch( ParticleStorage& ps, size_t idx, size_t
*static_cast<Box*>(shapes[ps.getShapeID(idx)].get()),
idy,
*static_cast<Box*>(shapes[ps.getShapeID(idy)].get()));
+ case Ellipsoid::SHAPE_TYPE : return f(ps,
+ idx,
+ *static_cast<Box*>(shapes[ps.getShapeID(idx)].get()),
+ idy,
+ *static_cast<Ellipsoid*>(shapes[ps.getShapeID(idy)].get()));
+ default : WALBERLA_ABORT("Shape type (" << shapes[ps.getShapeID(idy)]->getShapeType() << ") could not be determined!");
+ }
+ case Ellipsoid::SHAPE_TYPE :
+ switch (shapes[ps.getShapeID(idy)]->getShapeType())
+ {
+ case Sphere::SHAPE_TYPE : return f(ps,
+ idx,
+ *static_cast<Ellipsoid*>(shapes[ps.getShapeID(idx)].get()),
+ idy,
+ *static_cast<Sphere*>(shapes[ps.getShapeID(idy)].get()));
+ case HalfSpace::SHAPE_TYPE : return f(ps,
+ idx,
+ *static_cast<Ellipsoid*>(shapes[ps.getShapeID(idx)].get()),
+ idy,
+ *static_cast<HalfSpace*>(shapes[ps.getShapeID(idy)].get()));
+ case CylindricalBoundary::SHAPE_TYPE : return f(ps,
+ idx,
+ *static_cast<Ellipsoid*>(shapes[ps.getShapeID(idx)].get()),
+ idy,
+ *static_cast<CylindricalBoundary*>(shapes[ps.getShapeID(idy)].get()));
+ case Box::SHAPE_TYPE : return f(ps,
+ idx,
+ *static_cast<Ellipsoid*>(shapes[ps.getShapeID(idx)].get()),
+ idy,
+ *static_cast<Box*>(shapes[ps.getShapeID(idy)].get()));
+ case Ellipsoid::SHAPE_TYPE : return f(ps,
+ idx,
+ *static_cast<Ellipsoid*>(shapes[ps.getShapeID(idx)].get()),
+ idy,
+ *static_cast<Ellipsoid*>(shapes[ps.getShapeID(idy)].get()));
default : WALBERLA_ABORT("Shape type (" << shapes[ps.getShapeID(idy)]->getShapeType() << ") could not be determined!");
}
default : WALBERLA_ABORT("Shape type (" << shapes[ps.getShapeID(idx)]->getShapeType() << ") could not be determined!");
diff --git a/src/mesa_pd/data/shape/BaseShape.h b/src/mesa_pd/data/shape/BaseShape.h
index 24936a75b6074d15b909ee5982c155801678b58b..d00b0adeeaf2f87361380cdb89598710044b374b 100644
--- a/src/mesa_pd/data/shape/BaseShape.h
+++ b/src/mesa_pd/data/shape/BaseShape.h
@@ -51,6 +51,8 @@ public:
const int& getShapeType() const {return shapeType_;}
+ virtual Vec3 support( const Vec3& /*d*/ ) const {WALBERLA_ABORT("Not implemented!");}
+
static const int INVALID_SHAPE = -1; ///< Unique *invalid* shape type identifier.\ingroup mesa_pd_shape
protected:
real_t mass_ = real_t(0); ///< mass
diff --git a/src/mesa_pd/data/shape/Box.h b/src/mesa_pd/data/shape/Box.h
index 7a3de3a9a761fffe61cc479dd47fa24df7239125..95fcd9d0e027c8684255f42721e3c5287f53c19c 100644
--- a/src/mesa_pd/data/shape/Box.h
+++ b/src/mesa_pd/data/shape/Box.h
@@ -41,7 +41,9 @@ public:
real_t getVolume() const override { return edgeLength_[0] * edgeLength_[1] * edgeLength_[2]; };
void updateMassAndInertia(const real_t density) override;
- static const int SHAPE_TYPE = 3; ///< Unique shape type identifier for boxes.\ingroup mesa_pd_shape
+ Vec3 support( const Vec3& bfD ) const override;
+
+ constexpr static int SHAPE_TYPE = 3; ///< Unique shape type identifier for boxes.\ingroup mesa_pd_shape
private:
Vec3 edgeLength_; ///< edge length of the box
@@ -56,8 +58,19 @@ void Box::updateMassAndInertia(const real_t density)
edgeLength_[0]*edgeLength_[0] + edgeLength_[2]*edgeLength_[2] ,
edgeLength_[0]*edgeLength_[0] + edgeLength_[1]*edgeLength_[1] ) * (m / static_cast<real_t>( 12 ));
- getInvMass() = real_t(1.0) / m;
- getInvInertiaBF() = I.getInverse();
+ invMass_ = real_t(1.0) / m;
+ invInertiaBF_ = I.getInverse();
+}
+
+inline
+Vec3 Box::support( const Vec3& bfD ) const
+{
+ //As it is save to say we have atleast one component of the d-vector != 0 we can use
+ Vec3 relativSupport = Vec3( math::sign(bfD[0])*edgeLength_[0]*real_t(0.5),
+ math::sign(bfD[1])*edgeLength_[1]*real_t(0.5),
+ math::sign(bfD[2])*edgeLength_[2]*real_t(0.5) );
+
+ return relativSupport;
}
} //namespace data
diff --git a/src/mesa_pd/data/shape/CylindricalBoundary.h b/src/mesa_pd/data/shape/CylindricalBoundary.h
index dc2f3e67cc816b4ac275e2df7befcf80ec8ac624..86380cfa14e3aa672519e46665d0f4367a1be3c8 100644
--- a/src/mesa_pd/data/shape/CylindricalBoundary.h
+++ b/src/mesa_pd/data/shape/CylindricalBoundary.h
@@ -45,7 +45,7 @@ public:
real_t getVolume() const override { return std::numeric_limits<real_t>::infinity(); };
void updateMassAndInertia(const real_t density) override;
- static const int SHAPE_TYPE = 2; ///< Unique shape type identifier for cylindrical boundaries.\ingroup mesa_pd_shape
+ constexpr static int SHAPE_TYPE = 2; ///< Unique shape type identifier for cylindrical boundaries.\ingroup mesa_pd_shape
private:
real_t radius_; ///< radius of the cylinder
@@ -55,8 +55,8 @@ private:
inline
void CylindricalBoundary::updateMassAndInertia(const real_t /*density*/)
{
- getInvMass() = real_t(0.0);
- getInvInertiaBF() = Mat3(real_t(0.0));
+ invMass_ = real_t(0.0);
+ invInertiaBF_ = Mat3(real_t(0.0));
}
} //namespace data
diff --git a/src/mesa_pd/data/shape/Ellipsoid.h b/src/mesa_pd/data/shape/Ellipsoid.h
new file mode 100644
index 0000000000000000000000000000000000000000..6ab1d673a0c6fc02271ed05100e25a1ec0cc2af6
--- /dev/null
+++ b/src/mesa_pd/data/shape/Ellipsoid.h
@@ -0,0 +1,78 @@
+//======================================================================================================================
+//
+// This file is part of waLBerla. waLBerla is free software: you can
+// redistribute it and/or modify it under the terms of the GNU General Public
+// License as published by the Free Software Foundation, either version 3 of
+// the License, or (at your option) any later version.
+//
+// waLBerla is distributed in the hope that it will be useful, but WITHOUT
+// ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+// FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
+// for more details.
+//
+// You should have received a copy of the GNU General Public License along
+// with waLBerla (see COPYING.txt). If not, see <http://www.gnu.org/licenses/>.
+//
+//! \file Ellipsoid.h
+//! \author Sebastian Eibl <sebastian.eibl@fau.de>
+//
+//======================================================================================================================
+
+#pragma once
+
+#include <mesa_pd/data/shape/BaseShape.h>
+
+#include <core/math/Constants.h>
+
+namespace walberla {
+namespace mesa_pd {
+namespace data {
+
+class Ellipsoid : public BaseShape
+{
+public:
+ explicit Ellipsoid(const Vec3& semiAxes)
+ : BaseShape(Ellipsoid::SHAPE_TYPE)
+ , semiAxes_(semiAxes)
+ {}
+
+ const Vec3& getSemiAxes() const { return semiAxes_; }
+
+ real_t getVolume() const override { return real_c(4.0/3.0) * math::M_PI * semiAxes_[0] * semiAxes_[1] * semiAxes_[2]; }
+ void updateMassAndInertia(const real_t density) override;
+
+ Vec3 support( const Vec3& d_loc ) const override;
+
+ constexpr static int SHAPE_TYPE = 4; ///< Unique shape type identifier for boxes.\ingroup mesa_pd_shape
+
+private:
+ Vec3 semiAxes_; ///< edge length of the box
+};
+
+inline
+void Ellipsoid::updateMassAndInertia(const real_t density)
+{
+ const real_t m = getVolume() * density;
+ const Mat3 I = Mat3::makeDiagonalMatrix(
+ real_c(0.2) * m * (semiAxes_[1] * semiAxes_[1] + semiAxes_[2] * semiAxes_[2]),
+ real_c(0.2) * m * (semiAxes_[2] * semiAxes_[2] + semiAxes_[0] * semiAxes_[0]),
+ real_c(0.2) * m * (semiAxes_[0] * semiAxes_[0] + semiAxes_[1] * semiAxes_[1]));
+
+ invMass_ = real_t(1.0) / m;
+ invInertiaBF_ = I.getInverse();
+}
+
+inline
+Vec3 Ellipsoid::support( const Vec3& d_loc ) const
+{
+ Vec3 norm_vec(d_loc[0] * semiAxes_[0], d_loc[1] * semiAxes_[1], d_loc[2] * semiAxes_[2]);
+ real_t norm_length = norm_vec.length();
+ Vec3 local_support = (real_t(1.0) / norm_length) * Vec3(semiAxes_[0] * semiAxes_[0] * d_loc[0],
+ semiAxes_[1] * semiAxes_[1] * d_loc[1],
+ semiAxes_[2] * semiAxes_[2] * d_loc[2]);
+ return local_support;
+}
+
+} //namespace data
+} //namespace mesa_pd
+} //namespace walberla
diff --git a/src/mesa_pd/data/shape/HalfSpace.h b/src/mesa_pd/data/shape/HalfSpace.h
index 9088900df7ac9302f97d2b21f20b9dbc9a0fac37..d24db7ad1b5fbd359aba714c8e485f035ad623b1 100644
--- a/src/mesa_pd/data/shape/HalfSpace.h
+++ b/src/mesa_pd/data/shape/HalfSpace.h
@@ -43,7 +43,7 @@ public:
const Vec3& getNormal() const { return normal_; }
- static const int SHAPE_TYPE = 0; ///< Unique shape type identifier for planes.\ingroup mesa_pd_shape
+ constexpr static int SHAPE_TYPE = 0; ///< Unique shape type identifier for planes.\ingroup mesa_pd_shape
private:
/**
* Normal of the plane in reference to the global world frame.
diff --git a/src/mesa_pd/data/shape/ShapeTypes.cpp b/src/mesa_pd/data/shape/ShapeTypes.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..d33273b37a30d7fcfb940bffa4e6f21b1cb4fe79
--- /dev/null
+++ b/src/mesa_pd/data/shape/ShapeTypes.cpp
@@ -0,0 +1,39 @@
+//======================================================================================================================
+//
+// This file is part of waLBerla. waLBerla is free software: you can
+// redistribute it and/or modify it under the terms of the GNU General Public
+// License as published by the Free Software Foundation, either version 3 of
+// the License, or (at your option) any later version.
+//
+// waLBerla is distributed in the hope that it will be useful, but WITHOUT
+// ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+// FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
+// for more details.
+//
+// You should have received a copy of the GNU General Public License along
+// with waLBerla (see COPYING.txt). If not, see <http://www.gnu.org/licenses/>.
+//
+//! \file ShapeTypes.cpp
+//! \author Sebastian Eibl <sebastian.eibl@fau.de>
+//
+//======================================================================================================================
+
+#include <mesa_pd/data/shape/Box.h>
+#include <mesa_pd/data/shape/CylindricalBoundary.h>
+#include <mesa_pd/data/shape/HalfSpace.h>
+#include <mesa_pd/data/shape/Ellipsoid.h>
+#include <mesa_pd/data/shape/Sphere.h>
+
+namespace walberla {
+namespace mesa_pd {
+namespace data {
+
+const int Box::SHAPE_TYPE ;
+const int CylindricalBoundary::SHAPE_TYPE;
+const int HalfSpace::SHAPE_TYPE ;
+const int Ellipsoid::SHAPE_TYPE ;
+const int Sphere::SHAPE_TYPE ;
+
+} //namespace data
+} //namespace mesa_pd
+} //namespace walberla
diff --git a/src/mesa_pd/data/shape/Sphere.h b/src/mesa_pd/data/shape/Sphere.h
index 79137ea18b38369439263f616bd7d0f439c417f9..3c24aff0cc87635fcc7b5a7de70c9b957d9d07c7 100644
--- a/src/mesa_pd/data/shape/Sphere.h
+++ b/src/mesa_pd/data/shape/Sphere.h
@@ -39,7 +39,9 @@ public:
real_t getVolume() const override { return (real_t(4) / real_t(3)) * math::pi * getRadius() * getRadius() * getRadius(); }
- static const int SHAPE_TYPE = 1; ///< Unique shape type identifier for spheres.\ingroup mesa_pd_shape
+ Vec3 support( const Vec3& d ) const override;
+
+ constexpr static int SHAPE_TYPE = 1; ///< Unique shape type identifier for spheres.\ingroup mesa_pd_shape
private:
real_t radius_; ///< radius of the sphere
@@ -58,6 +60,12 @@ void Sphere::updateMassAndInertia(const real_t density)
invInertiaBF_ = I.getInverse();
}
+inline
+Vec3 Sphere::support( const Vec3& d ) const
+{
+ return radius_ * d;
+}
+
} //namespace data
} //namespace mesa_pd
} //namespace walberla
diff --git a/src/mesa_pd/kernel/DoubleCast.h b/src/mesa_pd/kernel/DoubleCast.h
index 5b4cee0e212f0cdb259be97fea78b6b1ffc2312d..46e917cc7adf7f2360763256b6c53c51b4a762ac 100644
--- a/src/mesa_pd/kernel/DoubleCast.h
+++ b/src/mesa_pd/kernel/DoubleCast.h
@@ -33,6 +33,7 @@
#include <mesa_pd/data/shape/HalfSpace.h>
#include <mesa_pd/data/shape/CylindricalBoundary.h>
#include <mesa_pd/data/shape/Box.h>
+#include <mesa_pd/data/shape/Ellipsoid.h>
#include <core/Abort.h>
#include <core/debug/Debug.h>
@@ -88,6 +89,11 @@ auto DoubleCast::operator()( size_t idx, size_t idy, Accessor& ac, func& f, Args
*static_cast<Sphere*>(ac.getShape(idx)),
*static_cast<Box*>(ac.getShape(idy)),
std::forward<Args>(args)...);
+ case Ellipsoid::SHAPE_TYPE : return f(idx,
+ idy,
+ *static_cast<Sphere*>(ac.getShape(idx)),
+ *static_cast<Ellipsoid*>(ac.getShape(idy)),
+ std::forward<Args>(args)...);
default : WALBERLA_ABORT("Shape type (" << ac.getShape(idy)->getShapeType() << ") could not be determined!");
}
case HalfSpace::SHAPE_TYPE :
@@ -113,6 +119,11 @@ auto DoubleCast::operator()( size_t idx, size_t idy, Accessor& ac, func& f, Args
*static_cast<HalfSpace*>(ac.getShape(idx)),
*static_cast<Box*>(ac.getShape(idy)),
std::forward<Args>(args)...);
+ case Ellipsoid::SHAPE_TYPE : return f(idx,
+ idy,
+ *static_cast<HalfSpace*>(ac.getShape(idx)),
+ *static_cast<Ellipsoid*>(ac.getShape(idy)),
+ std::forward<Args>(args)...);
default : WALBERLA_ABORT("Shape type (" << ac.getShape(idy)->getShapeType() << ") could not be determined!");
}
case CylindricalBoundary::SHAPE_TYPE :
@@ -138,6 +149,11 @@ auto DoubleCast::operator()( size_t idx, size_t idy, Accessor& ac, func& f, Args
*static_cast<CylindricalBoundary*>(ac.getShape(idx)),
*static_cast<Box*>(ac.getShape(idy)),
std::forward<Args>(args)...);
+ case Ellipsoid::SHAPE_TYPE : return f(idx,
+ idy,
+ *static_cast<CylindricalBoundary*>(ac.getShape(idx)),
+ *static_cast<Ellipsoid*>(ac.getShape(idy)),
+ std::forward<Args>(args)...);
default : WALBERLA_ABORT("Shape type (" << ac.getShape(idy)->getShapeType() << ") could not be determined!");
}
case Box::SHAPE_TYPE :
@@ -163,6 +179,41 @@ auto DoubleCast::operator()( size_t idx, size_t idy, Accessor& ac, func& f, Args
*static_cast<Box*>(ac.getShape(idx)),
*static_cast<Box*>(ac.getShape(idy)),
std::forward<Args>(args)...);
+ case Ellipsoid::SHAPE_TYPE : return f(idx,
+ idy,
+ *static_cast<Box*>(ac.getShape(idx)),
+ *static_cast<Ellipsoid*>(ac.getShape(idy)),
+ std::forward<Args>(args)...);
+ default : WALBERLA_ABORT("Shape type (" << ac.getShape(idy)->getShapeType() << ") could not be determined!");
+ }
+ case Ellipsoid::SHAPE_TYPE :
+ switch (ac.getShape(idy)->getShapeType())
+ {
+ case Sphere::SHAPE_TYPE : return f(idx,
+ idy,
+ *static_cast<Ellipsoid*>(ac.getShape(idx)),
+ *static_cast<Sphere*>(ac.getShape(idy)),
+ std::forward<Args>(args)...);
+ case HalfSpace::SHAPE_TYPE : return f(idx,
+ idy,
+ *static_cast<Ellipsoid*>(ac.getShape(idx)),
+ *static_cast<HalfSpace*>(ac.getShape(idy)),
+ std::forward<Args>(args)...);
+ case CylindricalBoundary::SHAPE_TYPE : return f(idx,
+ idy,
+ *static_cast<Ellipsoid*>(ac.getShape(idx)),
+ *static_cast<CylindricalBoundary*>(ac.getShape(idy)),
+ std::forward<Args>(args)...);
+ case Box::SHAPE_TYPE : return f(idx,
+ idy,
+ *static_cast<Ellipsoid*>(ac.getShape(idx)),
+ *static_cast<Box*>(ac.getShape(idy)),
+ std::forward<Args>(args)...);
+ case Ellipsoid::SHAPE_TYPE : return f(idx,
+ idy,
+ *static_cast<Ellipsoid*>(ac.getShape(idx)),
+ *static_cast<Ellipsoid*>(ac.getShape(idy)),
+ std::forward<Args>(args)...);
default : WALBERLA_ABORT("Shape type (" << ac.getShape(idy)->getShapeType() << ") could not be determined!");
}
default : WALBERLA_ABORT("Shape type (" << ac.getShape(idx)->getShapeType() << ") could not be determined!");
diff --git a/src/mesa_pd/kernel/SingleCast.h b/src/mesa_pd/kernel/SingleCast.h
index 316fbb62a847fb4fce90b181c263d8e53ff90cf1..12f85b93009ddba097dd3b8f5cf05b0a01ee0354 100644
--- a/src/mesa_pd/kernel/SingleCast.h
+++ b/src/mesa_pd/kernel/SingleCast.h
@@ -33,6 +33,7 @@
#include <mesa_pd/data/shape/HalfSpace.h>
#include <mesa_pd/data/shape/CylindricalBoundary.h>
#include <mesa_pd/data/shape/Box.h>
+#include <mesa_pd/data/shape/Ellipsoid.h>
#include <core/Abort.h>
#include <core/debug/Debug.h>
@@ -68,6 +69,7 @@ auto SingleCast::operator()( size_t idx, Accessor& ac, func& f, Args&&... args )
case HalfSpace::SHAPE_TYPE : return f(idx, *static_cast<HalfSpace*>(ac.getShape(idx)), std::forward<Args>(args)...);
case CylindricalBoundary::SHAPE_TYPE : return f(idx, *static_cast<CylindricalBoundary*>(ac.getShape(idx)), std::forward<Args>(args)...);
case Box::SHAPE_TYPE : return f(idx, *static_cast<Box*>(ac.getShape(idx)), std::forward<Args>(args)...);
+ case Ellipsoid::SHAPE_TYPE : return f(idx, *static_cast<Ellipsoid*>(ac.getShape(idx)), std::forward<Args>(args)...);
default : WALBERLA_ABORT("Shape type (" << ac.getShape(idx)->getShapeType() << ") could not be determined!");
}
}
diff --git a/src/mesa_pd/vtk/OutputSelector.h b/src/mesa_pd/vtk/OutputSelector.h
index 85a5436800e5ec06a6504d98e6235445485dceaa..177b38f8b4c49c25a422e53f3e2ccfd75797a9cd 100644
--- a/src/mesa_pd/vtk/OutputSelector.h
+++ b/src/mesa_pd/vtk/OutputSelector.h
@@ -67,6 +67,6 @@ private:
};
} // namespace vtk
-} // namespace pe
+} // namespace mesa_pd
} // namespace walberla
diff --git a/src/mesa_pd/vtk/TensorGlyph.cpp b/src/mesa_pd/vtk/TensorGlyph.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..4f058f6677ac05e473bc470dbffcc0b626924b89
--- /dev/null
+++ b/src/mesa_pd/vtk/TensorGlyph.cpp
@@ -0,0 +1,44 @@
+//======================================================================================================================
+//
+// This file is part of waLBerla. waLBerla is free software: you can
+// redistribute it and/or modify it under the terms of the GNU General Public
+// License as published by the Free Software Foundation, either version 3 of
+// the License, or (at your option) any later version.
+//
+// waLBerla is distributed in the hope that it will be useful, but WITHOUT
+// ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+// FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
+// for more details.
+//
+// You should have received a copy of the GNU General Public License along
+// with waLBerla (see COPYING.txt). If not, see <http://www.gnu.org/licenses/>.
+//
+//! \file TensorGlyph.cpp
+//! \author Sebastian Eibl <sebastian.eibl@fau.de>
+//
+//======================================================================================================================
+
+#include <mesa_pd/vtk/TensorGlyph.h>
+
+namespace walberla {
+namespace mesa_pd {
+namespace vtk {
+
+TensorGlyph createTensorGlyph(const Vec3& semiAxes, const Rot3& rot)
+{
+ // compute tensor glyph for visualization with ParaView (tensorGlyph)
+ const Mat3& rotMat = rot.getMatrix();
+ Vector3<real_t> directionVectorX(rotMat[0], rotMat[3], rotMat[6]);
+ Vector3<real_t> directionVectorY(rotMat[1], rotMat[4], rotMat[7]);
+ Vector3<real_t> directionVectorZ(rotMat[2], rotMat[5], rotMat[8]);
+ Mat3 axa = math::dyadicProduct(directionVectorX, directionVectorX);
+ Mat3 bxb = math::dyadicProduct(directionVectorY, directionVectorY);
+ Mat3 cxc = math::dyadicProduct(directionVectorZ, directionVectorZ);
+ Mat3 tensor = axa * semiAxes[0] + bxb * semiAxes[1] + cxc * semiAxes[2];
+ // use symmetry to only write 6 of the 9 elements: XX YY ZZ XY YZ XZ
+ return {{tensor(0,0), tensor(1,1), tensor(2,2), tensor(0,1), tensor(1,2), tensor(0,2)}};
+}
+
+} // namespace vtk
+} // namespace pe
+} // namespace walberla
diff --git a/src/mesa_pd/vtk/TensorGlyph.h b/src/mesa_pd/vtk/TensorGlyph.h
new file mode 100644
index 0000000000000000000000000000000000000000..d739f8b0070297ca702cddab2046bce93284e935
--- /dev/null
+++ b/src/mesa_pd/vtk/TensorGlyph.h
@@ -0,0 +1,37 @@
+//======================================================================================================================
+//
+// This file is part of waLBerla. waLBerla is free software: you can
+// redistribute it and/or modify it under the terms of the GNU General Public
+// License as published by the Free Software Foundation, either version 3 of
+// the License, or (at your option) any later version.
+//
+// waLBerla is distributed in the hope that it will be useful, but WITHOUT
+// ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+// FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
+// for more details.
+//
+// You should have received a copy of the GNU General Public License along
+// with waLBerla (see COPYING.txt). If not, see <http://www.gnu.org/licenses/>.
+//
+//! \file TensorGlyph.h
+//! \author Sebastian Eibl <sebastian.eibl@fau.de>
+//
+//======================================================================================================================
+
+#pragma once
+
+#include <mesa_pd/data/DataTypes.h>
+
+#include <array>
+
+namespace walberla {
+namespace mesa_pd {
+namespace vtk {
+
+using TensorGlyph = std::array<real_t, 6>;
+
+TensorGlyph createTensorGlyph(const Vec3& semiAxes, const Rot3& rot);
+
+} // namespace vtk
+} // namespace pe
+} // namespace walberla
diff --git a/src/mesa_pd/vtk/WriteOutput.h b/src/mesa_pd/vtk/WriteOutput.h
index ab22c75132d8bfe4dad8628aaea8c8dc23e832e6..b6016ea796622d6b87baad35a1b01bc1d02762e5 100644
--- a/src/mesa_pd/vtk/WriteOutput.h
+++ b/src/mesa_pd/vtk/WriteOutput.h
@@ -21,6 +21,7 @@
#pragma once
#include <mesa_pd/data/DataTypes.h>
+#include <mesa_pd/vtk/TensorGlyph.h>
#include <core/debug/CheckFunctions.h>
#include <vtk/Base64Writer.h>
@@ -42,6 +43,14 @@ void writeOutput(std::ostream& os, const T& data, const uint_t component)
walberla::vtk::toStream(os, data);
}
+template <>
+inline
+void writeOutput(std::ostream& os, const TensorGlyph& data, const uint_t component)
+{
+ WALBERLA_ASSERT_LESS(component, 6);
+ walberla::vtk::toStream(os, data[component]);
+}
+
template <>
inline
void writeOutput(std::ostream& os, const Vec3& data, const uint_t component)
@@ -60,6 +69,14 @@ void writeOutput(walberla::vtk::Base64Writer& b64, const T& data, const uint_t c
b64 << data;
}
+template <>
+inline
+void writeOutput(walberla::vtk::Base64Writer& b64, const TensorGlyph& data, const uint_t component)
+{
+ WALBERLA_ASSERT_LESS(component, 6);
+ b64 << data[component];
+}
+
template <>
inline
void writeOutput(walberla::vtk::Base64Writer& b64, const Vec3& data, const uint_t component)
diff --git a/tests/mesa_pd/CMakeLists.txt b/tests/mesa_pd/CMakeLists.txt
index 9abfc2b20472f2decf915335dc799689b9d5ecfd..07083c0e89ac832200caa1c0e5e304910c4b3bb2 100644
--- a/tests/mesa_pd/CMakeLists.txt
+++ b/tests/mesa_pd/CMakeLists.txt
@@ -10,6 +10,27 @@ waLBerla_execute_test( NAME MESA_PD_COLLISIONDETECTION_AnalyticCollisionFuncti
waLBerla_compile_test( NAME MESA_PD_COLLISIONDETECTION_AnalyticContactDetection FILES collision_detection/AnalyticContactDetection.cpp DEPENDS core )
waLBerla_execute_test( NAME MESA_PD_COLLISIONDETECTION_AnalyticContactDetection )
+waLBerla_compile_test( NAME MESA_PD_COLLISIONDETECTION_BoxSupport FILES collision_detection/BoxSupport.cpp DEPENDS core )
+waLBerla_execute_test( NAME MESA_PD_COLLISIONDETECTION_BoxSupport )
+
+waLBerla_compile_test( NAME MESA_PD_COLLISIONDETECTION_EllipsoidSupport FILES collision_detection/EllipsoidSupport.cpp DEPENDS core )
+waLBerla_execute_test( NAME MESA_PD_COLLISIONDETECTION_EllipsoidSupport )
+
+waLBerla_compile_test( NAME MESA_PD_COLLISIONDETECTION_EPA FILES collision_detection/EPA.cpp DEPENDS core )
+waLBerla_execute_test( NAME MESA_PD_COLLISIONDETECTION_EPA )
+
+waLBerla_compile_test( NAME MESA_PD_COLLISIONDETECTION_GeneralContactDetection FILES collision_detection/GeneralContactDetection.cpp DEPENDS blockforest core pe)
+waLBerla_execute_test( NAME MESA_PD_COLLISIONDETECTION_GeneralContactDetection )
+
+waLBerla_compile_test( NAME MESA_PD_COLLISIONDETECTION_GJK FILES collision_detection/GJK.cpp DEPENDS core )
+waLBerla_execute_test( NAME MESA_PD_COLLISIONDETECTION_GJK )
+
+waLBerla_compile_test( NAME MESA_PD_COLLISIONDETECTION_GJKEPA FILES collision_detection/GJK_EPA.cpp DEPENDS core )
+waLBerla_execute_test( NAME MESA_PD_COLLISIONDETECTION_GJKEPA )
+
+waLBerla_compile_test( NAME MESA_PD_COLLISIONDETECTION_SphereSupport FILES collision_detection/SphereSupport.cpp DEPENDS core )
+waLBerla_execute_test( NAME MESA_PD_COLLISIONDETECTION_SphereSupport )
+
waLBerla_compile_test( NAME MESA_PD_COMMON_IntersectionRatio FILES common/IntersectionRatio.cpp DEPENDS core )
waLBerla_execute_test( NAME MESA_PD_COMMON_IntersectionRatio )
@@ -42,6 +63,9 @@ waLBerla_execute_test( NAME MESA_PD_Domain_DynamicRefinement PROCESSES 8)
waLBerla_compile_test( NAME MESA_PD_Domain_SerializeDeserialize FILES domain/SerializeDeserialize.cpp DEPENDS blockforest core pe)
waLBerla_execute_test( NAME MESA_PD_Domain_SerializeDeserialize PROCESSES 8 )
+waLBerla_compile_test( NAME MESA_PD_DropTest FILES DropTest.cpp DEPENDS core )
+waLBerla_execute_test( NAME MESA_PD_DropTest )
+
waLBerla_compile_test( NAME MESA_PD_Kernel_ClearNextNeighborSync FILES kernel/ClearNextNeighborSync.cpp DEPENDS core )
waLBerla_execute_test( NAME MESA_PD_Kernel_ClearNextNeighborSync PROCESSES 2 )
diff --git a/tests/mesa_pd/ContactDetection.cpp b/tests/mesa_pd/ContactDetection.cpp
index b60126d3d1b6043b2eb5465aab0f6d3d4454005c..3b63a9a7e07ad57d9faa97096519b4c7915735d9 100644
--- a/tests/mesa_pd/ContactDetection.cpp
+++ b/tests/mesa_pd/ContactDetection.cpp
@@ -21,6 +21,7 @@
#include <mesa_pd/vtk/ParticleVtkOutput.h>
#include <mesa_pd/collision_detection/AnalyticContactDetection.h>
+#include <mesa_pd/collision_detection/GeneralContactDetection.h>
#include <mesa_pd/data/LinkedCells.h>
#include <mesa_pd/data/ParticleAccessor.h>
#include <mesa_pd/data/ParticleStorage.h>
diff --git a/tests/mesa_pd/DropTest.cpp b/tests/mesa_pd/DropTest.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..d9d249e99e126cdfd40e223594e00d7973971750
--- /dev/null
+++ b/tests/mesa_pd/DropTest.cpp
@@ -0,0 +1,175 @@
+//======================================================================================================================
+//
+// This file is part of waLBerla. waLBerla is free software: you can
+// redistribute it and/or modify it under the terms of the GNU General Public
+// License as published by the Free Software Foundation, either version 3 of
+// the License, or (at your option) any later version.
+//
+// waLBerla is distributed in the hope that it will be useful, but WITHOUT
+// ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+// FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
+// for more details.
+//
+// You should have received a copy of the GNU General Public License along
+// with waLBerla (see COPYING.txt). If not, see <http://www.gnu.org/licenses/>.
+//
+//! \file DropTest.cpp
+//! \author Sebastian Eibl <sebastian.eibl@fau.de>
+//
+//======================================================================================================================
+
+#include <mesa_pd/collision_detection/GeneralContactDetection.h>
+
+#include <mesa_pd/data/ParticleAccessor.h>
+#include <mesa_pd/data/ParticleStorage.h>
+#include <mesa_pd/data/ShapeStorage.h>
+
+#include <mesa_pd/kernel/DoubleCast.h>
+#include <mesa_pd/kernel/ExplicitEulerWithShape.h>
+#include <mesa_pd/kernel/ParticleSelector.h>
+#include <mesa_pd/kernel/SpringDashpot.h>
+
+#include <core/Abort.h>
+#include <core/Environment.h>
+#include <core/logging/Logging.h>
+#include <core/waLBerlaBuildInfo.h>
+
+#include <functional>
+#include <memory>
+#include <string>
+#include <type_traits>
+
+namespace walberla {
+namespace mesa_pd {
+
+class ParticleAccessorWithShape : public data::ParticleAccessor
+{
+public:
+ ParticleAccessorWithShape(std::shared_ptr<data::ParticleStorage>& ps, std::shared_ptr<data::ShapeStorage>& ss)
+ : ParticleAccessor(ps)
+ , ss_(ss)
+ {}
+
+ const auto& getInvMass(const size_t p_idx) const {return ss_->shapes[ps_->getShapeID(p_idx)]->getInvMass();}
+
+ const auto& getInvInertiaBF(const size_t p_idx) const {return ss_->shapes[ps_->getShapeID(p_idx)]->getInvInertiaBF();}
+
+ data::BaseShape* getShape(const size_t p_idx) const {return ss_->shapes[ps_->getShapeID(p_idx)].get();}
+private:
+ std::shared_ptr<data::ShapeStorage> ss_;
+};
+
+int main( int argc, char ** argv )
+{
+ using namespace walberla::timing;
+
+ Environment env(argc, argv);
+ walberla::mpi::MPIManager::instance()->useWorldComm();
+
+ if (std::is_same<walberla::real_t, float>::value)
+ {
+ WALBERLA_LOG_WARNING("waLBerla build in sp mode: skipping test due to low precision");
+ return EXIT_SUCCESS;
+ }
+
+ //init data structures
+ auto ps = std::make_shared<data::ParticleStorage>(100);
+ auto ss = std::make_shared<data::ShapeStorage>();
+ ParticleAccessorWithShape accessor(ps, ss);
+
+ auto p = ps->create();
+ p->getPositionRef() = Vec3(real_t(0), real_t(0), real_t(0));
+ p->getInteractionRadiusRef() = std::numeric_limits<real_t>::infinity();
+ p->getShapeIDRef() = ss->create<data::HalfSpace>( Vec3(real_t(0), real_t(0), real_t(1)) );
+ p->getOwnerRef() = walberla::mpi::MPIManager::instance()->rank();
+ p->getTypeRef() = 0;
+
+ auto sp = ps->create();
+ sp->getPositionRef() = Vec3(real_t(0), real_t(0), real_t(0.01));
+ sp->getInteractionRadiusRef() = real_t(1);
+ sp->getShapeIDRef() = ss->create<data::Sphere>( real_t(0.004) );
+ ss->shapes[1]->updateMassAndInertia(real_t(2707));
+ WALBERLA_LOG_DEVEL_VAR(ss->shapes[1]->getInvMass());
+ WALBERLA_LOG_DEVEL_VAR(ss->shapes[1]->getInvInertiaBF());
+ sp->getOwnerRef() = walberla::mpi::MPIManager::instance()->rank();
+ sp->getTypeRef() = 0;
+
+ auto bx = ps->create();
+ bx->getPositionRef() = Vec3(real_t(1), real_t(0), real_t(0.01));
+ bx->getInteractionRadiusRef() = real_t(1);
+ bx->getShapeIDRef() = ss->create<data::Box>( Vec3(real_t(0.008*0.8)) );
+ ss->shapes[2]->updateMassAndInertia(real_t(2707));
+ WALBERLA_LOG_DEVEL_VAR(ss->shapes[2]->getInvMass());
+ WALBERLA_LOG_DEVEL_VAR(ss->shapes[2]->getInvInertiaBF());
+ bx->getOwnerRef() = walberla::mpi::MPIManager::instance()->rank();
+ bx->getTypeRef() = 0;
+
+ auto el = ps->create();
+ el->getPositionRef() = Vec3(real_t(2), real_t(0), real_t(0.01));
+ el->getInteractionRadiusRef() = real_t(1);
+ el->getShapeIDRef() = ss->create<data::Ellipsoid>( Vec3(real_t(0.004)) );
+ ss->shapes[3]->updateMassAndInertia(real_t(2707));
+ WALBERLA_LOG_DEVEL_VAR(ss->shapes[3]->getInvMass());
+ WALBERLA_LOG_DEVEL_VAR(ss->shapes[3]->getInvInertiaBF());
+ el->getOwnerRef() = walberla::mpi::MPIManager::instance()->rank();
+ el->getTypeRef() = 0;
+
+ int64_t simulationSteps = 2000;
+ real_t dt = real_t(0.000001);
+
+ // Init kernels
+ kernel::ExplicitEulerWithShape explicitEulerWithShape( dt );
+ kernel::SpringDashpot dem(1);
+ dem.setStiffness(0, 0, real_t(8.11e6));
+ dem.setDampingN (0, 0, real_t(6.86e1));
+ dem.setDampingT (0, 0, real_t(6.86e1));
+ dem.setFriction (0, 0, real_t(1.2));
+
+ collision_detection::GeneralContactDetection gcd;
+ kernel::DoubleCast double_cast;
+
+
+ for (int64_t i=0; i < simulationSteps; ++i)
+ {
+ ps->forEachParticle(false,
+ kernel::SelectLocal(),
+ accessor,
+ [&](const size_t idx, auto& ac){ac.setForce(idx, Vec3(real_t(0), real_t(0), real_t(-9.81)) );},
+ accessor);
+
+ ps->forEachParticlePairHalf(true,
+ kernel::SelectAll(),
+ accessor,
+ [&](const size_t idx1, const size_t idx2, auto& ac)
+ {
+ if (double_cast(idx1, idx2, ac, gcd, ac ))
+ {
+ dem(gcd.getIdx1(), gcd.getIdx2(), ac, gcd.getContactPoint(), gcd.getContactNormal(), gcd.getPenetrationDepth());
+ }
+ },
+ accessor );
+
+ ps->forEachParticle(true,
+ kernel::SelectLocal(),
+ accessor,
+ explicitEulerWithShape,
+ accessor);
+
+// if(i%1 == 0)
+// WALBERLA_LOG_DEVEL(bx->getPosition()[2]);
+ }
+
+ WALBERLA_CHECK_FLOAT_EQUAL_EPSILON(sp->getPosition()[2], real_t(0.004), real_t(1e-3));
+ WALBERLA_CHECK_FLOAT_EQUAL_EPSILON(bx->getPosition()[2], real_t(0.004*0.8), real_t(1e-3));
+ WALBERLA_CHECK_FLOAT_EQUAL_EPSILON(el->getPosition()[2], real_t(0.004), real_t(1e-3));
+
+ return EXIT_SUCCESS;
+}
+
+} // namespace mesa_pd
+} // namespace walberla
+
+int main( int argc, char* argv[] )
+{
+ return walberla::mesa_pd::main( argc, argv );
+}
diff --git a/tests/mesa_pd/collision_detection/BoxSupport.cpp b/tests/mesa_pd/collision_detection/BoxSupport.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..08ae7bcc0e5110a463735eee663a5d9347652f77
--- /dev/null
+++ b/tests/mesa_pd/collision_detection/BoxSupport.cpp
@@ -0,0 +1,82 @@
+//======================================================================================================================
+//
+// This file is part of waLBerla. waLBerla is free software: you can
+// redistribute it and/or modify it under the terms of the GNU General Public
+// License as published by the Free Software Foundation, either version 3 of
+// the License, or (at your option) any later version.
+//
+// waLBerla is distributed in the hope that it will be useful, but WITHOUT
+// ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+// FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
+// for more details.
+//
+// You should have received a copy of the GNU General Public License along
+// with waLBerla (see COPYING.txt). If not, see <http://www.gnu.org/licenses/>.
+//
+//! \file BoxSupport.cpp
+//! \author Sebastian Eibl <sebastian.eibl@fau.de>
+//
+//======================================================================================================================
+
+#include <mesa_pd/collision_detection/Support.h>
+#include <mesa_pd/data/DataTypes.h>
+#include <mesa_pd/data/shape/Box.h>
+
+#include <core/Environment.h>
+#include <core/logging/Logging.h>
+
+#include <cmath>
+#include <iostream>
+
+namespace walberla {
+namespace mesa_pd {
+
+void checkContour(const Vec3& pt, const Vec3& edgeLength )
+{
+ WALBERLA_CHECK_FLOAT_EQUAL(std::abs(pt[0]), edgeLength[0] * real_t(0.5), pt);
+ WALBERLA_CHECK_FLOAT_EQUAL(std::abs(pt[1]), edgeLength[1] * real_t(0.5), pt);
+ WALBERLA_CHECK_FLOAT_EQUAL(std::abs(pt[2]), edgeLength[2] * real_t(0.5), pt);
+}
+
+void check( )
+{
+ using namespace walberla::mesa_pd::collision_detection;
+
+ Vec3 pos = Vec3(real_t(1),real_t(2),real_t(3));
+ Vec3 edgeLength = Vec3(real_t(2),real_t(3),real_t(1));
+
+ auto bx = data::Box(edgeLength);
+ Support b0(pos, Rot3(), bx);
+ WALBERLA_CHECK_FLOAT_EQUAL(b0.support(Vec3(real_t(+1),real_t(+1),real_t(+1))), Vec3(real_t(2),real_t(3.5),real_t(3.5)));
+ WALBERLA_CHECK_FLOAT_EQUAL(b0.support(Vec3(real_t(-1),real_t(+1),real_t(+1))), Vec3(real_t(0),real_t(3.5),real_t(3.5)));
+ WALBERLA_CHECK_FLOAT_EQUAL(b0.support(Vec3(real_t(+1),real_t(-1),real_t(+1))), Vec3(real_t(2),real_t(0.5),real_t(3.5)));
+ WALBERLA_CHECK_FLOAT_EQUAL(b0.support(Vec3(real_t(-1),real_t(-1),real_t(+1))), Vec3(real_t(0),real_t(0.5),real_t(3.5)));
+ WALBERLA_CHECK_FLOAT_EQUAL(b0.support(Vec3(real_t(+1),real_t(+1),real_t(-1))), Vec3(real_t(2),real_t(3.5),real_t(2.5)));
+ WALBERLA_CHECK_FLOAT_EQUAL(b0.support(Vec3(real_t(-1),real_t(+1),real_t(-1))), Vec3(real_t(0),real_t(3.5),real_t(2.5)));
+ WALBERLA_CHECK_FLOAT_EQUAL(b0.support(Vec3(real_t(+1),real_t(-1),real_t(-1))), Vec3(real_t(2),real_t(0.5),real_t(2.5)));
+ WALBERLA_CHECK_FLOAT_EQUAL(b0.support(Vec3(real_t(-1),real_t(-1),real_t(-1))), Vec3(real_t(0),real_t(0.5),real_t(2.5)));
+
+ checkContour(b0.support(Vec3(real_t(-1),real_t(-2),real_t(-3)).getNormalized()) - pos, edgeLength );
+ checkContour(b0.support(Vec3(real_t(-2),real_t(-3),real_t(-1)).getNormalized()) - pos, edgeLength );
+ checkContour(b0.support(Vec3(real_t(-3),real_t(-1),real_t(-2)).getNormalized()) - pos, edgeLength );
+
+ Rot3 rot = Rot3(Vec3(1,3,2).getNormalized(), real_t(1.56));
+ Support b1(pos, rot, bx);
+ checkContour(rot.getMatrix().getTranspose() * (b1.support(Vec3(real_t(-1),real_t(-2),real_t(-3)).getNormalized()) - pos), edgeLength );
+ checkContour(rot.getMatrix().getTranspose() * (b1.support(Vec3(real_t(-2),real_t(-3),real_t(-1)).getNormalized()) - pos), edgeLength );
+ checkContour(rot.getMatrix().getTranspose() * (b1.support(Vec3(real_t(-3),real_t(-1),real_t(-2)).getNormalized()) - pos), edgeLength );
+}
+
+} //namespace mesa_pd
+} //namespace walberla
+
+int main( int argc, char ** argv )
+{
+ walberla::Environment env(argc, argv);
+ WALBERLA_UNUSED(env);
+ walberla::mpi::MPIManager::instance()->useWorldComm();
+
+ walberla::mesa_pd::check();
+
+ return EXIT_SUCCESS;
+}
diff --git a/tests/mesa_pd/collision_detection/EPA.cpp b/tests/mesa_pd/collision_detection/EPA.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..fa171a209012b7c496d58b4217ae84477126a805
--- /dev/null
+++ b/tests/mesa_pd/collision_detection/EPA.cpp
@@ -0,0 +1,90 @@
+//======================================================================================================================
+//
+// This file is part of waLBerla. waLBerla is free software: you can
+// redistribute it and/or modify it under the terms of the GNU General Public
+// License as published by the Free Software Foundation, either version 3 of
+// the License, or (at your option) any later version.
+//
+// waLBerla is distributed in the hope that it will be useful, but WITHOUT
+// ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+// FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
+// for more details.
+//
+// You should have received a copy of the GNU General Public License along
+// with waLBerla (see COPYING.txt). If not, see <http://www.gnu.org/licenses/>.
+//
+//! \file EPA.cpp
+//! \author Sebastian Eibl <sebastian.eibl@fau.de>
+//
+//======================================================================================================================
+
+#include <mesa_pd/collision_detection/AnalyticCollisionFunctions.h>
+#include <mesa_pd/collision_detection/EPA.h>
+#include <mesa_pd/collision_detection/GJK.h>
+#include <mesa_pd/collision_detection/Support.h>
+#include <mesa_pd/data/shape/Sphere.h>
+
+#include <core/Environment.h>
+#include <core/logging/Logging.h>
+
+#include <cmath>
+#include <iostream>
+#include <memory>
+
+namespace walberla {
+namespace mesa_pd {
+
+void check( )
+{
+ using namespace walberla::mesa_pd::collision_detection;
+
+ auto pos0 = Vec3(real_t(1),real_t(2),real_t(3));
+ auto radius0 = real_t(0.6);
+ auto pos1 = Vec3(real_t(2),real_t(2),real_t(3));
+ auto radius1 = real_t(0.6);
+
+ auto sp0 = data::Sphere(real_t(0.6));
+ auto geom0 = Support(pos0, Rot3(), sp0);
+ auto geom1 = Support(pos1, Rot3(), sp0);
+
+ Vec3 epa_normal;
+ Vec3 epa_contactPoint;
+ real_t epa_penetrationDepth;
+
+ real_t margin = real_t(1e-4);
+ GJK gjk;
+ WALBERLA_CHECK(gjk.doGJKmargin(geom0, geom1, margin));
+ EPA epa;
+ epa.useSphereOptimization(true);
+ WALBERLA_CHECK(epa.doEPAmargin(geom0, geom1, gjk, epa_normal, epa_contactPoint, epa_penetrationDepth, margin));
+
+ Vec3 analytic_normal;
+ Vec3 analytic_contactPoint;
+ real_t analytic_penetrationDepth;
+ WALBERLA_CHECK(analytic::detectSphereSphereCollision(pos0,
+ radius0,
+ pos1,
+ radius1,
+ analytic_contactPoint,
+ analytic_normal,
+ analytic_penetrationDepth,
+ margin));
+
+ WALBERLA_CHECK_FLOAT_EQUAL(epa_contactPoint, analytic_contactPoint);
+ WALBERLA_CHECK_FLOAT_EQUAL(epa_normal, analytic_normal);
+ WALBERLA_CHECK_FLOAT_EQUAL(epa_penetrationDepth, analytic_penetrationDepth);
+}
+
+} //namespace mesa_pd
+} //namespace walberla
+
+int main( int argc, char ** argv )
+{
+ walberla::Environment env(argc, argv);
+ WALBERLA_UNUSED(env);
+ walberla::mpi::MPIManager::instance()->useWorldComm();
+
+ walberla::mesa_pd::check();
+
+ return EXIT_SUCCESS;
+}
diff --git a/tests/mesa_pd/collision_detection/EllipsoidSupport.cpp b/tests/mesa_pd/collision_detection/EllipsoidSupport.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..820d34005c376b98c6e9b3c17eb8b1980223c12c
--- /dev/null
+++ b/tests/mesa_pd/collision_detection/EllipsoidSupport.cpp
@@ -0,0 +1,81 @@
+//======================================================================================================================
+//
+// This file is part of waLBerla. waLBerla is free software: you can
+// redistribute it and/or modify it under the terms of the GNU General Public
+// License as published by the Free Software Foundation, either version 3 of
+// the License, or (at your option) any later version.
+//
+// waLBerla is distributed in the hope that it will be useful, but WITHOUT
+// ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+// FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
+// for more details.
+//
+// You should have received a copy of the GNU General Public License along
+// with waLBerla (see COPYING.txt). If not, see <http://www.gnu.org/licenses/>.
+//
+//! \file EllipsoidSupport.cpp
+//! \author Sebastian Eibl <sebastian.eibl@fau.de>
+//
+//======================================================================================================================
+
+#include <mesa_pd/collision_detection/Support.h>
+#include <mesa_pd/data/DataTypes.h>
+#include <mesa_pd/data/shape/Ellipsoid.h>
+
+#include <core/Environment.h>
+#include <core/logging/Logging.h>
+
+#include <cmath>
+#include <iostream>
+
+namespace walberla {
+namespace mesa_pd {
+
+inline
+real_t contour(const Vec3& point, const Vec3& semiAxes)
+{
+ return (point[0] * point[0]) / (semiAxes[0] * semiAxes[0]) +
+ (point[1] * point[1]) / (semiAxes[1] * semiAxes[1]) +
+ (point[2] * point[2]) / (semiAxes[2] * semiAxes[2]);
+}
+
+void check( )
+{
+ using namespace walberla::mesa_pd::collision_detection;
+
+ Vec3 pos = Vec3(real_t(1),real_t(2),real_t(3));
+ Vec3 semiAxes = Vec3(real_t(2),real_t(3),real_t(1));
+
+ auto el = data::Ellipsoid(semiAxes);
+ Support e0(pos, Rot3(), el);
+ WALBERLA_CHECK_FLOAT_EQUAL(e0.support(Vec3(real_t(1),real_t(0),real_t(0))), Vec3(real_t(3),real_t(2),real_t(3)));
+ WALBERLA_CHECK_FLOAT_EQUAL(e0.support(Vec3(real_t(0),real_t(1),real_t(0))), Vec3(real_t(1),real_t(5),real_t(3)));
+ WALBERLA_CHECK_FLOAT_EQUAL(e0.support(Vec3(real_t(0),real_t(0),real_t(1))), Vec3(real_t(1),real_t(2),real_t(4)));
+ WALBERLA_CHECK_FLOAT_EQUAL(e0.support(Vec3(real_t(-1),real_t(0),real_t(0))), Vec3(real_t(-1),real_t(2),real_t(3)));
+ WALBERLA_CHECK_FLOAT_EQUAL(e0.support(Vec3(real_t(0),real_t(-1),real_t(0))), Vec3(real_t(1),real_t(-1),real_t(3)));
+ WALBERLA_CHECK_FLOAT_EQUAL(e0.support(Vec3(real_t(0),real_t(0),real_t(-1))), Vec3(real_t(1),real_t(2),real_t(2)));
+
+ WALBERLA_CHECK_FLOAT_EQUAL( contour(e0.support(Vec3(real_t(-1),real_t(-2),real_t(-3)).getNormalized()) - pos, semiAxes), real_t(1) );
+ WALBERLA_CHECK_FLOAT_EQUAL( contour(e0.support(Vec3(real_t(-2),real_t(-3),real_t(-1)).getNormalized()) - pos, semiAxes), real_t(1) );
+ WALBERLA_CHECK_FLOAT_EQUAL( contour(e0.support(Vec3(real_t(-3),real_t(-1),real_t(-2)).getNormalized()) - pos, semiAxes), real_t(1) );
+
+ Rot3 rot = Rot3(Vec3(1,3,2).getNormalized(), real_t(1.56));
+ Support e1(pos, rot, el);
+ WALBERLA_CHECK_FLOAT_EQUAL( contour( rot.getMatrix().getTranspose() * (e1.support(Vec3(real_t(-1),real_t(-2),real_t(-3)).getNormalized()) - pos), semiAxes), real_t(1) );
+ WALBERLA_CHECK_FLOAT_EQUAL( contour( rot.getMatrix().getTranspose() * (e1.support(Vec3(real_t(-2),real_t(-3),real_t(-1)).getNormalized()) - pos), semiAxes), real_t(1) );
+ WALBERLA_CHECK_FLOAT_EQUAL( contour( rot.getMatrix().getTranspose() * (e1.support(Vec3(real_t(-3),real_t(-1),real_t(-2)).getNormalized()) - pos), semiAxes), real_t(1) );
+}
+
+} //namespace mesa_pd
+} //namespace walberla
+
+int main( int argc, char ** argv )
+{
+ walberla::Environment env(argc, argv);
+ WALBERLA_UNUSED(env);
+ walberla::mpi::MPIManager::instance()->useWorldComm();
+
+ walberla::mesa_pd::check();
+
+ return EXIT_SUCCESS;
+}
diff --git a/tests/mesa_pd/collision_detection/GJK.cpp b/tests/mesa_pd/collision_detection/GJK.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..43f044c5de663f11d9221662409a503c059f7c98
--- /dev/null
+++ b/tests/mesa_pd/collision_detection/GJK.cpp
@@ -0,0 +1,61 @@
+//======================================================================================================================
+//
+// This file is part of waLBerla. waLBerla is free software: you can
+// redistribute it and/or modify it under the terms of the GNU General Public
+// License as published by the Free Software Foundation, either version 3 of
+// the License, or (at your option) any later version.
+//
+// waLBerla is distributed in the hope that it will be useful, but WITHOUT
+// ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+// FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
+// for more details.
+//
+// You should have received a copy of the GNU General Public License along
+// with waLBerla (see COPYING.txt). If not, see <http://www.gnu.org/licenses/>.
+//
+//! \file GJK.cpp
+//! \author Sebastian Eibl <sebastian.eibl@fau.de>
+//
+//======================================================================================================================
+
+#include <mesa_pd/collision_detection/GJK.h>
+#include <mesa_pd/collision_detection/Support.h>
+#include <mesa_pd/data/shape/Sphere.h>
+
+#include <core/Environment.h>
+#include <core/logging/Logging.h>
+
+#include <cmath>
+#include <iostream>
+#include <memory>
+
+namespace walberla {
+namespace mesa_pd {
+
+void check( )
+{
+ using namespace walberla::mesa_pd::collision_detection;
+
+ auto sp0 = data::Sphere(real_t(0.6));
+ auto sp1 = data::Sphere(real_t(0.4));
+
+ GJK gjk;
+ WALBERLA_CHECK( gjk.doGJKmargin( Support(Vec3(real_t(1),real_t(2),real_t(3)), Rot3(), sp0),
+ Support(Vec3(real_t(2),real_t(2),real_t(3)), Rot3(), sp0) ));
+ WALBERLA_CHECK( !gjk.doGJKmargin( Support(Vec3(real_t(1),real_t(2),real_t(3)), Rot3(), sp1),
+ Support(Vec3(real_t(2),real_t(2),real_t(3)), Rot3(), sp1) ));
+}
+
+} //namespace mesa_pd
+} //namespace walberla
+
+int main( int argc, char ** argv )
+{
+ walberla::Environment env(argc, argv);
+ WALBERLA_UNUSED(env);
+ walberla::mpi::MPIManager::instance()->useWorldComm();
+
+ walberla::mesa_pd::check();
+
+ return EXIT_SUCCESS;
+}
diff --git a/tests/mesa_pd/collision_detection/GJK_EPA.cpp b/tests/mesa_pd/collision_detection/GJK_EPA.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..c9eb7921d03c5e477a04a0a7e34b4f5736bff1de
--- /dev/null
+++ b/tests/mesa_pd/collision_detection/GJK_EPA.cpp
@@ -0,0 +1,365 @@
+//======================================================================================================================
+//
+// This file is part of waLBerla. waLBerla is free software: you can
+// redistribute it and/or modify it under the terms of the GNU General Public
+// License as published by the Free Software Foundation, either version 3 of
+// the License, or (at your option) any later version.
+//
+// waLBerla is distributed in the hope that it will be useful, but WITHOUT
+// ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+// FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
+// for more details.
+//
+// You should have received a copy of the GNU General Public License along
+// with waLBerla (see COPYING.txt). If not, see <http://www.gnu.org/licenses/>.
+//
+//! \file GJK_EPA.cpp
+//! \author Sebastian Eibl <sebastian.eibl@fau.de>
+//
+//======================================================================================================================
+
+#include <mesa_pd/collision_detection/AnalyticCollisionFunctions.h>
+#include <mesa_pd/collision_detection/EPA.h>
+#include <mesa_pd/collision_detection/GJK.h>
+#include <mesa_pd/collision_detection/Support.h>
+#include <mesa_pd/data/DataTypes.h>
+#include <mesa_pd/data/shape/Box.h>
+#include <mesa_pd/data/shape/Ellipsoid.h>
+#include <mesa_pd/data/shape/Sphere.h>
+
+#include "core/debug/TestSubsystem.h"
+#include "core/DataTypes.h"
+#include "core/math/Constants.h"
+#include "core/math/Random.h"
+#include "core/math/Vector2.h"
+
+namespace walberla {
+namespace mesa_pd {
+
+using namespace walberla::mesa_pd::collision_detection;
+using namespace walberla::mesa_pd::collision_detection::analytic;
+
+bool gjkEPAcollideHybrid(Support &geom1, Support &geom2, Vec3& normal, Vec3& contactPoint, real_t& penetrationDepth)
+{
+ // For more information on hybrid GJK/EPA see page 166 in "Collision Detecton in Interactive 3D
+ // Environments" by Gino van den Bergen.
+
+ //1. Run GJK with considerably enlarged objects.
+ real_t margin = real_t(1e-6);
+ GJK gjk;
+ if(gjk.doGJKmargin(geom1, geom2, margin)){
+ //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;
+ }
+}
+
+//Define Test values for different precision levels
+#ifdef WALBERLA_DOUBLE_ACCURACY
+static const int distancecount = 6;
+static const real_t depth[distancecount] = {real_t(-1e-5), real_t(1e-5), real_t(1e-4), real_t(1e-2), real_t(0.1), real_t(1.0)};
+static const real_t test_accuracy = real_t(1e-3);
+#else
+static const int distancecount = 3;
+static const real_t depth[distancecount] = {real_t(1e-2), real_t(0.1), real_t(1.0)};
+static const real_t test_accuracy = real_t(1e-2); //Single Precision is v. bad!
+#endif
+
+
+/** Compares Computed Contact c1 to analytical Contact c2,
+ * and tests for equivalence.
+ * The computed position must only be in the same plane, if planeNormal has not length 0. */
+void checkContact(const Vec3& contactPoint1,
+ const Vec3& normal1,
+ const real_t penetrationDepth1,
+ const Vec3& contactPoint2,
+ const Vec3& normal2,
+ const real_t penetrationDepth2,
+ const Vec3& planeNormal,
+ const real_t accuracy = test_accuracy )
+{
+ WALBERLA_CHECK_LESS( fabs((normal1 - normal2).sqrLength()), accuracy*accuracy );
+ WALBERLA_CHECK_LESS( fabs(penetrationDepth1- penetrationDepth2), accuracy );
+
+ //Unfortunately position accuracy is one-two orders of magnitude lower...
+ if(floatIsEqual(planeNormal.sqrLength(), real_t(0.0))){
+ WALBERLA_CHECK_LESS( fabs((contactPoint1- contactPoint2).sqrLength()), real_t(1e4)*accuracy*accuracy );
+ }else{
+ //check for containment in plane only.
+ WALBERLA_CHECK_LESS( fabs(contactPoint1*planeNormal-contactPoint2*planeNormal), real_t(1e2)*accuracy );
+ }
+
+}
+
+/** \brief Executes a test setup for collision data collection.
+ * \param rb1 first rigid body
+ * \param rb2 second rigid body
+ * \param dir1 direction of rb2 moving towards rb1 (unit vector)
+ * \param penetration_factor Increment of the penetration if rb2 is moved by dir1 (=1.0 in most cases)
+ * \param real_axis Analytical collision normal (unit vector)
+ * \param witnesspoint Analytical touching point of rb1 and rb2
+ * \param witnessmove Movement of the touching point, if rb2 is moved by dir1
+ * \param planeNormal The normal of the touching plane (if the touching point is unique,
+ * a Vector of length 0.0 shall be passed)
+ * \param accuracy Acceptance threshold
+ * Before the test, rb1 and rb2 shall be in touching contact.
+ * This function checks the collision data returned for different penetration depths and argument orders.
+ */
+void runCollisionDataTest(Support &rb1, Support &rb2, const Vec3& dir1, const real_t penetration_factor,
+ const Vec3& real_axis, const Vec3& witnesspoint, const Vec3& witnessmove, const Vec3& planeNormal, const real_t accuracy = test_accuracy){
+
+ Vec3 org_pos = rb2.pos_; //Safe position
+
+ Vec3 normal1, normal2;
+ Vec3 pos1, pos2;
+ real_t comp_pen_depth1, comp_pen_depth2;
+
+ for(int j = 0; j < distancecount; j++){
+ //move rb1.
+ rb2.pos_ = (org_pos + depth[j]*dir1);
+// WALBERLA_LOG_INFO("Using depth: "+ std::to_string(depth[j]));
+ //Compute collision between rb1 and rb2 and vice versa
+ bool result1 = gjkEPAcollideHybrid(rb1, rb2, normal1, pos1, comp_pen_depth1);
+// WALBERLA_LOG_DEVEL( normal1 << " " << pos1 << " " << comp_pen_depth1);
+ bool result2 = gjkEPAcollideHybrid(rb2, rb1, normal2, pos2, comp_pen_depth2);
+// WALBERLA_LOG_DEVEL( normal2 << " " << pos2 << " " << comp_pen_depth2);
+ if(depth[j] > real_t(0.0)){
+ WALBERLA_CHECK(result1);
+ WALBERLA_CHECK(result2);
+ //Check contact information
+ checkContact( pos1, normal1, comp_pen_depth1,
+ witnesspoint + depth[j] * witnessmove, real_axis, -depth[j] * penetration_factor, planeNormal, accuracy );
+ checkContact( pos2, normal2, comp_pen_depth2,
+ witnesspoint + depth[j] * witnessmove, real_t(-1.0)*real_axis, -depth[j] * penetration_factor, planeNormal, accuracy );
+ }
+ if(depth[j] < real_t(0.0)){
+ WALBERLA_CHECK(!result1);
+ WALBERLA_CHECK(!result2);
+ }
+ }
+}
+
+/** Test the GJK-EPA implementation on a variety of configuations
+ * and penetation depths */
+void MainTest()
+{
+ using namespace walberla::mesa_pd::data;
+ // Original SPHERE <-> SPHERE
+ auto sp = data::Sphere(real_t(1));
+ auto sp1 = Support( Vec3(0,0,0), Rot3(), sp);
+ auto sp2 = Support( Vec3(1.5,0,0), Rot3(), sp);
+ auto sp3 = Support( Vec3(3.0,0,0), Rot3(), sp);
+
+ Vec3 normal;
+ Vec3 contactPoint;
+ real_t penetrationDepth;
+
+
+ WALBERLA_LOG_INFO("Original: SPHERE <-> SPHERE");
+ WALBERLA_CHECK( !gjkEPAcollideHybrid(sp1, sp3, normal, contactPoint, penetrationDepth) );
+ WALBERLA_CHECK( gjkEPAcollideHybrid(sp1, sp2, normal, contactPoint, penetrationDepth) );
+ checkContact( contactPoint, normal, penetrationDepth,
+ Vec3(real_t(0.75), 0, 0), Vec3(real_t(-1.0), 0, 0), real_t(-0.5), Vec3(0,0,0) );
+
+ //Testcase 01 Box Sphere
+ WALBERLA_LOG_INFO("Test 01: BOX <-> SPHERE");
+ real_t sqr3_inv = real_t(1.0)/std::sqrt(real_t(3.0));
+ real_t coordinate= real_t(5.0)* sqr3_inv + real_t(5.0); // 5*(1+ (1/sqrt(3)))
+ Box bx_101010(Vec3(10, 10, 10));
+ Support box1_1(Vec3(0, 0, 0), Rot3(), bx_101010);
+ Sphere s_5(real_t(5));
+ Support sphere1_2(Vec3(coordinate, coordinate, coordinate), Rot3(), s_5);
+ Vec3 wp1(real_t(5.0), real_t(5.0), real_t(5.0));
+ Vec3 wpm1(sqr3_inv*real_t(-0.5), sqr3_inv*real_t(-0.5), sqr3_inv*real_t(-0.5));
+ Vec3 axis1(-sqr3_inv, -sqr3_inv, -sqr3_inv);
+ runCollisionDataTest(box1_1, sphere1_2, axis1, real_t(1.0), axis1, wp1, wpm1, Vec3(0,0,0));
+
+ //Testcase 02 Box LongBox (touching plane)
+ //Reuse box1_1
+ WALBERLA_LOG_INFO("Test 02: BOX <-> LONG BOX");
+ Box bx_3011(Vec3(real_t(30.0),1,1));
+ Support box2_1(Vec3(real_t(20.0),0,0), Rot3(), bx_3011);
+ Vec3 wp2(5, 0, 0);
+ Vec3 wpm2(real_t(-0.5),0,0);
+ Vec3 axis2(-1,0,0);
+ runCollisionDataTest(box1_1, box2_1, axis2, real_t(1.0), axis2, wp2, wpm2, axis2);
+
+ //Testcase 03 Sphere Sphere
+ WALBERLA_LOG_INFO("Test 03: SPHERE <-> SPHERE");
+ Support sphere3_1(Vec3(0,0,0), Rot3(), s_5);
+ Support sphere3_2(Vec3(real_t(10.0),0,0), Rot3(), s_5);
+ Vec3 wp3(5, 0, 0);
+ Vec3 wpm3(real_t(-0.5),0,0);
+ Vec3 axis3(-1,0,0);
+ runCollisionDataTest(sphere3_1, sphere3_2, axis3, real_t(1.0), axis3, wp3, wpm3, Vec3(0,0,0));
+
+ //Testcase 04 Cube with turned Cube
+ WALBERLA_LOG_INFO("Test 04: CUBE <-> TURNED CUBE");
+ //compute rotation.
+ real_t angle = walberla::math::M_PI/real_t(4.0);
+ Vec3 zaxis(0, 0, 1);
+ Quat q4(zaxis, angle);
+
+ //create turned box
+ real_t sqr2 = std::sqrt(real_t(2.0));
+ Support box4_1( Vec3(real_t(5.0)*(real_t(1.0)+sqr2), real_t(-5.0), 0), Rot3(q4), bx_101010);
+ Support box4_2( Vec3(0, 0, 0), Rot3(), bx_101010);
+ Vec3 wp4(5, -5, 0);
+ Vec3 wpm4(real_t(-0.25),real_t(+0.25),0);
+ Vec3 collision_axis4(-sqr2/real_t(2.0),+sqr2/real_t(2.0),0);
+ Vec3 axis4(-1, 0, 0);
+
+ runCollisionDataTest(box4_2, box4_1, axis4, sqr2/real_t(2.0), collision_axis4, wp4, wpm4, Vec3(0,real_t(1.0),0));
+
+ //Testcase 05 Cube and Long Box non-centric (touching plane)
+ WALBERLA_LOG_INFO("Test 05: CUBE <-> LONG BOX (NON_CENTRIC)");
+ Support box5_1(Vec3(0, 0, 0), Rot3(), bx_101010);
+ Support box5_2(Vec3(real_t(15.0),real_t(5.5), 0), Rot3(), bx_3011);
+ Vec3 wp5(real_t(3.75), 5, 0);
+ Vec3 wpm5(0, real_t(-0.5), 0);
+ Vec3 axis5(0, -1, 0);
+ runCollisionDataTest(box5_1, box5_2, axis5, real_t(1.0), axis5, wp5, wpm5, axis5); //check only for containment in plane.
+
+
+ //Testcase 06:
+ WALBERLA_LOG_INFO("Test 06: CUBE <-> TURNED CUBE 2");
+ //compute rotation.
+
+ real_t sqr6_2 = std::sqrt(real_t(2.0));
+ real_t sqr6_3 = std::sqrt(real_t(3.0));
+ real_t angle6 = std::acos(real_t(1.0)/sqr6_3); //acos(1/sqrt(3))
+ Vec3 rot_axis6(0, real_t(1.0)/sqr6_2, -real_t(1.0)/sqr6_2);
+ Quat q6(rot_axis6, angle6);
+
+ //create turned box with pos = (5*(1+sqrt(3)), 0, 0)
+ Support box6_1(Vec3(real_t(5.0)*(real_t(1.0)+sqr6_3), 0, 0), Rot3(q6), bx_101010);
+ Support box6_2(Vec3(0, 0, 0), Rot3(), bx_101010);
+ Vec3 wp6(5, 0, 0);
+ Vec3 wpm6(real_t(-0.5), 0, 0);
+ Vec3 axis6(-1, 0, 0);
+ runCollisionDataTest(box6_2, box6_1, axis6, real_t(1.0), axis6, wp6, wpm6, Vec3(0,0,0));
+
+ //Testcase 07:
+ // BOX <-> SPHERE
+ WALBERLA_LOG_INFO("Test 07: BOX <-> SPHERE");
+ Support sphere7_1(Vec3(0,0,0), Rot3(), s_5);
+ Box bx_555( Vec3(5, 5, 5));
+ Support box7_2(Vec3(0, 0,real_t(7.5)), Rot3(), bx_555);
+ Vec3 wpm7(0, 0, real_t(-0.5));
+ Vec3 wp7(0, 0, real_t(5.0));
+ Vec3 axis7(0, 0, real_t(-1.0));
+ runCollisionDataTest(sphere7_1, box7_2, axis7, real_t(1.0), axis7, wp7, wpm7, Vec3(0,0,0));
+
+ //Testcase 09:
+ // ELLIPSOID <-> ELLIPSOID
+ WALBERLA_LOG_INFO("Test 09: ELLIPSOID <-> ELLIPSOID");
+ Ellipsoid ell9_1_(Vec3(10,5,5));
+ Support ell9_1(Vec3(0,0,0), Rot3(), ell9_1_);
+ Ellipsoid ell9_2_(Vec3(5,10,5));
+ Support ell9_2(Vec3(15,0,0), Rot3(), ell9_2_);
+ Vec3 wpm9(real_t(-0.5), 0, 0);
+ Vec3 wp9(real_t(10), 0, 0);
+ Vec3 axis9(real_t(-1.0), 0, 0);
+ runCollisionDataTest(ell9_1, ell9_2, axis9, real_t(1.0), axis9, wp9, wpm9, Vec3(0,0,0));
+
+}
+
+void FuzzyEllipsoidEllipsoid(uint_t iterations)
+{
+ using namespace walberla::mesa_pd::data;
+
+ Vec3 normal;
+ Vec3 contactPoint;
+ real_t penetrationDepth;
+
+ Ellipsoid el(Vec3(2,2,2));
+ Support e1(Vec3(0,0,0), Rot3(), el);
+ Support e2(Vec3(real_t(3.999),0,0), Rot3(), el);
+
+ for (uint_t i = 0; i < iterations; ++i)
+ {
+ e1.rot_.rotate( Vec3(math::realRandom(), math::realRandom(), math::realRandom()) );
+ e2.rot_.rotate( Vec3(math::realRandom(), math::realRandom(), math::realRandom()) );
+ WALBERLA_CHECK(gjkEPAcollideHybrid(e1, e2, normal, contactPoint, penetrationDepth));
+ WALBERLA_CHECK_FLOAT_EQUAL(normal, Vec3(real_t(-1),0,0));
+ WALBERLA_CHECK_FLOAT_EQUAL(contactPoint, Vec3(real_t(3.999) * real_t(0.5), 0, 0));
+ WALBERLA_CHECK_FLOAT_EQUAL(penetrationDepth, real_t(3.999) - real_t(4));
+ }
+}
+
+void FuzzyEllipsoidBox(uint_t iterations)
+{
+ using namespace walberla::mesa_pd::data;
+
+ Vec3 normal;
+ Vec3 contactPoint;
+ real_t penetrationDepth;
+
+ Vec3 gjk_normal;
+ Vec3 gjk_contactPoint;
+ real_t gjk_penetrationDepth;
+
+ Box bx(Vec3(2,2,2));
+ Ellipsoid el(Vec3(2,2,2));
+ Support p1(Vec3(0,0,0), Rot3(), el);
+ Support p2(Vec3(real_t(3.999),0,0), Rot3(), bx);
+
+ for (uint_t i = 0; i < iterations; ++i)
+ {
+ //if(i%100==0) WALBERLA_LOG_INFO(i);
+ p1.rot_.rotate( Vec3(math::realRandom(), math::realRandom(), math::realRandom()) );
+ p2.rot_.rotate( Vec3(math::realRandom(), math::realRandom(), math::realRandom()) );
+ p2.pos_ = Vec3(4,0,0);
+ bool bCollision = false;
+ while (!bCollision)
+ {
+ p2.pos_ -= Vec3(real_t(0.1),0,0);
+ if (p2.pos_[0]<real_t(0)) WALBERLA_ABORT("ERROR");
+ bCollision = detectSphereBoxCollision(p1.pos_,
+ el.getSemiAxes()[0],
+ p2.pos_,
+ bx.getEdgeLength(),
+ p2.rot_,
+ contactPoint,
+ normal,
+ penetrationDepth,
+ real_t(1e-4));
+ }
+
+ WALBERLA_CHECK(gjkEPAcollideHybrid(p1, p2, gjk_normal, gjk_contactPoint, gjk_penetrationDepth), penetrationDepth);
+ //WALBERLA_CHECK_FLOAT_EQUAL_EPSILON(gjk_normal, normal, real_t(1e-2));
+ //WALBERLA_CHECK_FLOAT_EQUAL_EPSILON(gjk_contactPoint, contactPoint, real_t(1e-2));
+ //WALBERLA_CHECK_FLOAT_EQUAL_EPSILON(gjk_penetrationDepth, penetrationDepth, real_t(1e-2));
+ }
+}
+
+int main( int argc, char** argv )
+{
+ walberla::debug::enterTestMode();
+
+ walberla::MPIManager::instance()->initializeMPI( &argc, &argv );
+
+ if (std::is_same<walberla::real_t, float>::value)
+ {
+ WALBERLA_LOG_WARNING("waLBerla build in sp mode: skipping test due to low precision");
+ return EXIT_SUCCESS;
+ }
+
+ MainTest();
+ FuzzyEllipsoidEllipsoid(1000);
+ FuzzyEllipsoidBox(1000);
+
+ return EXIT_SUCCESS;
+}
+
+} // namespace mesa_pd
+} // namespace walberla
+
+int main( int argc, char* argv[] )
+{
+ return walberla::mesa_pd::main( argc, argv );
+}
diff --git a/tests/mesa_pd/collision_detection/GeneralContactDetection.cpp b/tests/mesa_pd/collision_detection/GeneralContactDetection.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..9f3be3feb720aced306b7042bbd9ca47f193afcb
--- /dev/null
+++ b/tests/mesa_pd/collision_detection/GeneralContactDetection.cpp
@@ -0,0 +1,105 @@
+//======================================================================================================================
+//
+// This file is part of waLBerla. waLBerla is free software: you can
+// redistribute it and/or modify it under the terms of the GNU General Public
+// License as published by the Free Software Foundation, either version 3 of
+// the License, or (at your option) any later version.
+//
+// waLBerla is distributed in the hope that it will be useful, but WITHOUT
+// ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+// FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
+// for more details.
+//
+// You should have received a copy of the GNU General Public License along
+// with waLBerla (see COPYING.txt). If not, see <http://www.gnu.org/licenses/>.
+//
+//! \file GeneralContactDetection.cpp
+//! \author Sebastian Eibl <sebastian.eibl@fau.de>
+//
+//======================================================================================================================
+
+#include <mesa_pd/collision_detection/GeneralContactDetection.h>
+#include <mesa_pd/data/ParticleAccessor.h>
+#include <mesa_pd/data/ParticleStorage.h>
+#include <mesa_pd/data/ShapeStorage.h>
+#include <mesa_pd/kernel/DoubleCast.h>
+
+#include <core/Abort.h>
+#include <core/Environment.h>
+#include <core/logging/Logging.h>
+#include <core/waLBerlaBuildInfo.h>
+
+#include <memory>
+
+namespace walberla {
+namespace mesa_pd {
+
+class ParticleAccessorWithShape : public data::ParticleAccessor
+{
+public:
+ ParticleAccessorWithShape(std::shared_ptr<data::ParticleStorage>& ps, std::shared_ptr<data::ShapeStorage>& ss)
+ : ParticleAccessor(ps)
+ , ss_(ss)
+ {}
+
+ data::BaseShape* getShape(const size_t p_idx) const {return ss_->shapes[ps_->getShapeIDRef(p_idx)].get();}
+private:
+ std::shared_ptr<data::ShapeStorage> ss_;
+};
+
+void generalContactDetection()
+{
+ //init data structures
+ auto ps = std::make_shared<data::ParticleStorage>(100);
+ auto ss = std::make_shared<data::ShapeStorage>();
+ ParticleAccessorWithShape accessor(ps, ss);
+
+ auto e0 = ps->create();
+ e0->setPosition(Vec3(real_t(0),real_t(0),real_t(0)));
+ e0->setShapeID(ss->create<data::Ellipsoid>(Vec3(real_t(1),real_t(2),real_t(3))));
+
+ auto e1 = ps->create();
+ e1->setPosition(Vec3(real_t(1.9),real_t(0),real_t(0)));
+ e1->setShapeID(ss->create<data::Ellipsoid>(Vec3(real_t(1),real_t(2),real_t(3))));
+
+ auto p1 = ps->create();
+ p1->setPosition(Vec3(real_t(-0.9),real_t(0),real_t(0)));
+ p1->setShapeID(ss->create<data::HalfSpace>(Vec3(real_t(1),real_t(0),real_t(0))));
+
+ auto cb1 = ps->create();
+ cb1->setPosition(Vec3(real_t(0),real_t(0),real_t(0)));
+ cb1->setShapeID(ss->create<data::CylindricalBoundary>(real_t(3), Vec3(real_t(0),real_t(0),real_t(1))));
+
+ collision_detection::GeneralContactDetection gcd;
+ kernel::DoubleCast double_cast;
+
+ WALBERLA_CHECK(double_cast(0, 1, accessor, gcd, accessor));
+ WALBERLA_CHECK_FLOAT_EQUAL( gcd.getContactPoint(), Vec3(real_t(0.95),real_t(0),real_t(0)) );
+ WALBERLA_CHECK_FLOAT_EQUAL_EPSILON( gcd.getContactNormal(), Vec3(real_t(-1),real_t(0),real_t(0)), real_t(1e-3) );
+ WALBERLA_CHECK_FLOAT_EQUAL_EPSILON( gcd.getPenetrationDepth(), real_t(-0.1), real_t(1e-3) );
+ e1->setPosition(Vec3(real_t(2.1),real_t(0),real_t(0)));
+ WALBERLA_CHECK(!double_cast(0, 1, accessor, gcd, accessor));
+
+ WALBERLA_CHECK(double_cast(0, 2, accessor, gcd, accessor));
+ WALBERLA_CHECK_FLOAT_EQUAL( gcd.getContactPoint(), Vec3(real_t(-0.95),real_t(0),real_t(0)) );
+ WALBERLA_CHECK_FLOAT_EQUAL_EPSILON( gcd.getContactNormal(), Vec3(real_t(-1),real_t(0),real_t(0)), real_t(1e-3) );
+ WALBERLA_CHECK_FLOAT_EQUAL_EPSILON( gcd.getPenetrationDepth(), real_t(-0.1), real_t(1e-3) );
+ WALBERLA_CHECK(!double_cast(1, 2, accessor, gcd, accessor));
+
+ WALBERLA_CHECK(double_cast(1, 3, accessor, gcd, accessor));
+ WALBERLA_CHECK_FLOAT_EQUAL( gcd.getContactPoint(), Vec3(real_t(3.05),real_t(0),real_t(0)) );
+ WALBERLA_CHECK_FLOAT_EQUAL_EPSILON( gcd.getContactNormal(), Vec3(real_t(+1),real_t(0),real_t(0)), real_t(1e-3) );
+ WALBERLA_CHECK_FLOAT_EQUAL_EPSILON( gcd.getPenetrationDepth(), real_t(-0.1), real_t(1e-3) );
+ WALBERLA_CHECK(!double_cast(0, 3, accessor, gcd, accessor));
+}
+
+} // namespace mesa_pd
+} // namespace walberla
+
+int main( int argc, char* argv[] )
+{
+ walberla::Environment env(argc, argv);
+ WALBERLA_UNUSED(env);
+ walberla::mesa_pd::generalContactDetection();
+ return EXIT_SUCCESS;
+}
diff --git a/tests/mesa_pd/collision_detection/SphereSupport.cpp b/tests/mesa_pd/collision_detection/SphereSupport.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..2c94e0fc5878ffad2525d4d70787bbdfaa31484f
--- /dev/null
+++ b/tests/mesa_pd/collision_detection/SphereSupport.cpp
@@ -0,0 +1,73 @@
+//======================================================================================================================
+//
+// This file is part of waLBerla. waLBerla is free software: you can
+// redistribute it and/or modify it under the terms of the GNU General Public
+// License as published by the Free Software Foundation, either version 3 of
+// the License, or (at your option) any later version.
+//
+// waLBerla is distributed in the hope that it will be useful, but WITHOUT
+// ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+// FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
+// for more details.
+//
+// You should have received a copy of the GNU General Public License along
+// with waLBerla (see COPYING.txt). If not, see <http://www.gnu.org/licenses/>.
+//
+//! \file SphereSupport.cpp
+//! \author Sebastian Eibl <sebastian.eibl@fau.de>
+//
+//======================================================================================================================
+
+#include <mesa_pd/collision_detection/Support.h>
+#include <mesa_pd/data/DataTypes.h>
+#include <mesa_pd/data/shape/Sphere.h>
+
+#include <core/Environment.h>
+#include <core/logging/Logging.h>
+
+#include <cmath>
+#include <iostream>
+
+namespace walberla {
+namespace mesa_pd {
+
+inline
+real_t contour(const Vec3& point, const real_t& radius)
+{
+ return ((point[0] * point[0]) + (point[1] * point[1]) + (point[2] * point[2])) / (radius * radius);
+}
+
+void check( )
+{
+ using namespace walberla::mesa_pd::collision_detection;
+
+ Vec3 pos = Vec3(real_t(1),real_t(2),real_t(3));
+ real_t radius = real_t(2.356);
+
+ auto sp = data::Sphere(radius);
+ Support e0(pos, Rot3(), sp);
+ WALBERLA_CHECK_FLOAT_EQUAL(e0.support(Vec3(real_t(1),real_t(0),real_t(0))), Vec3(real_t(1+2.356),real_t(2),real_t(3)));
+ WALBERLA_CHECK_FLOAT_EQUAL(e0.support(Vec3(real_t(0),real_t(1),real_t(0))), Vec3(real_t(1),real_t(2+2.356),real_t(3)));
+ WALBERLA_CHECK_FLOAT_EQUAL(e0.support(Vec3(real_t(0),real_t(0),real_t(1))), Vec3(real_t(1),real_t(2),real_t(3+2.356)));
+ WALBERLA_CHECK_FLOAT_EQUAL(e0.support(Vec3(real_t(-1),real_t(0),real_t(0))), Vec3(real_t(1-2.356),real_t(2),real_t(3)));
+ WALBERLA_CHECK_FLOAT_EQUAL(e0.support(Vec3(real_t(0),real_t(-1),real_t(0))), Vec3(real_t(1),real_t(2-2.356),real_t(3)));
+ WALBERLA_CHECK_FLOAT_EQUAL(e0.support(Vec3(real_t(0),real_t(0),real_t(-1))), Vec3(real_t(1),real_t(2),real_t(3-2.356)));
+
+ WALBERLA_CHECK_FLOAT_EQUAL( contour(e0.support(Vec3(real_t(-1),real_t(-2),real_t(-3)).getNormalized()) - pos, radius), real_t(1) );
+ WALBERLA_CHECK_FLOAT_EQUAL( contour(e0.support(Vec3(real_t(-2),real_t(-3),real_t(-1)).getNormalized()) - pos, radius), real_t(1) );
+ WALBERLA_CHECK_FLOAT_EQUAL( contour(e0.support(Vec3(real_t(-3),real_t(-1),real_t(-2)).getNormalized()) - pos, radius), real_t(1) );
+}
+
+} //namespace mesa_pd
+} //namespace walberla
+
+int main( int argc, char ** argv )
+{
+ walberla::Environment env(argc, argv);
+ WALBERLA_UNUSED(env);
+ walberla::mpi::MPIManager::instance()->useWorldComm();
+
+ walberla::mesa_pd::check();
+
+ return EXIT_SUCCESS;
+}