diff --git a/src/pe/Types.h b/src/pe/Types.h
index 35d559f4f7c19d81a0ba36d04482a9d7639c7c43..764bf4bb70610b43ec54215b60c85a95219ae25f 100644
--- a/src/pe/Types.h
+++ b/src/pe/Types.h
@@ -65,6 +65,7 @@ class Box;
class Capsule;
class Contact;
class Cylinder;
+class Ellipsoid;
class CylindricalBoundary;
class FixedJoint;
class ForceGenerator;
@@ -111,6 +112,10 @@ typedef CylindricalBoundary CylindricalBoundaryType; //!< Type of
typedef CylindricalBoundary* CylindricalBoundaryID; //!< Handle for a cylindrical boundary primitive.
typedef const CylindricalBoundary* ConstCylindricalBoundaryID; //!< Handle for a constant cylindrical boundary primitive.
+typedef Ellipsoid EllipsoidType; //!< Type of the ellipsoid geometric primitive.
+typedef Ellipsoid* EllipsoidID; //!< Handle for a ellipsoid primitive.
+typedef const Ellipsoid* ConstEllipsoidID; //!< Handle for a constant ellipsoid primitive.
+
typedef Plane PlaneType; //!< Type of the plane geometric primitive.
typedef Plane* PlaneID; //!< Handle for a plane primitive.
typedef const Plane* ConstPlaneID; //!< Handle for a constant plane primitive.
diff --git a/src/pe/basic.h b/src/pe/basic.h
index 70bbdf4426bbaeb38d75c05fb89da6bea1c7bc9a..08051705953cf2e5641188942c29a11e0c0cf7ec 100644
--- a/src/pe/basic.h
+++ b/src/pe/basic.h
@@ -44,6 +44,7 @@
#include "pe/rigidbody/PlaneFactory.h"
#include "pe/rigidbody/SphereFactory.h"
#include "pe/rigidbody/UnionFactory.h"
+#include "pe/rigidbody/EllipsoidFactory.h"
#include "pe/synchronization/SyncNextNeighbors.h"
#include "pe/synchronization/SyncShadowOwners.h"
diff --git a/src/pe/collision/EPA.cpp b/src/pe/collision/EPA.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..b20d776dd6e1904a7c1f081b1a51b8db50bc14dc
--- /dev/null
+++ b/src/pe/collision/EPA.cpp
@@ -0,0 +1,883 @@
+//======================================================================================================================
+//
+// 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.
+//
+//=================================================================================================
+
+//*************************************************************************************************
+// Includes
+//*************************************************************************************************
+#include "EPA.h"
+
+#include <pe/Thresholds.h>
+#include <pe/Types.h>
+
+#include <core/math/Constants.h>
+#include <core/math/Limits.h>
+#include <core/math/Matrix3.h>
+#include <core/math/Quaternion.h>
+
+#include <vector>
+
+namespace walberla {
+namespace pe {
+namespace fcd {
+
+//=================================================================================================
+//
+// 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
+ */
+inline 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] = NULL;
+ 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( GeomPrimitive &geom1, GeomPrimitive &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( GeomPrimitive &geom1, GeomPrimitive &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( GeomPrimitive &geom1, GeomPrimitive &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( GeomPrimitive &geom1, GeomPrimitive &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 = NULL;
+
+ //EPA Main-Loop
+ do {
+ std::pop_heap(entryHeap.begin(), entryHeap.end(), EPA::EPA_TriangleComp());
+ current = entryHeap.back();
+ entryHeap.pop_back();
+ if(!current->isObsolete()) {
+ WALBERLA_ASSERT_GREATER(current->getSqrDist(), real_t(0.0), "EPA_Trianalge distance is negative.");
+ 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){
+ normal = current->getNormal().getNormalized();
+ }else{
+ normal = current->getClosest().getNormalized();
+ }
+ //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
+ Vec3 ctr;
+ real_t radius2 = calculateCircle(support, epaVolume[(*current)[0]],
+ epaVolume[(*current)[1]], epaVolume[(*current)[2]], ctr);
+ if(radius2 > real_t(0.0)){ //if a Sphere exists
+ //std::cerr << "Circle created with center at " << ctr << ". r2=" << radius2 << std::endl;
+ real_t center_len = ctr.length();
+ real_t circle_dist = (std::sqrt(radius2) - center_len); //Distance from center to the spheres surface
+ //Check if the circle matches the bounds given by EPA and limit max error to ca. 5%
+ if(circle_dist*circle_dist <= upperBoundSqr && circle_dist*circle_dist >= lowerBoundSqr &&
+ (circle_dist*circle_dist)/lowerBoundSqr < real_t(1.10) && !floatIsEqual(center_len, real_t(0.0))) {
+
+ ctr = -1*ctr.getNormalized();
+ //std::cerr << "New support direction: " << ctr << std::endl;
+ pushSupportMargin(geom1, geom2, ctr, margin, epaVolume, supportA, supportB);
+ support = epaVolume.back();
+ // Check if support is in expected direction
+
+ if(floatIsEqual((support % ctr).sqrLength()/support.sqrLength(), real_t(0.0))){ //Accept sphere
+
+ contactPoint = real_t(0.5) * (supportA.back() + supportB.back());
+ penetrationDepth = -support.length()+ real_t(2.0) * margin;
+ retNormal = -ctr;
+ //std::cerr << "Found penetration depth " << penetrationDepth << " with CurvedEPA!" << std::endl;
+ if(penetrationDepth < contactThreshold){
+ return true;
+ }else{
+ return false;
+ }
+ } else { //Reject sphere
+ removeSupportMargin(epaVolume, supportA, supportB);
+ support = epaVolume.back();
+ }
+ }
+ }
+
+ //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().getNormalized();
+
+ //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, GeomPrimitive &geom1, GeomPrimitive &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).getNormalized();
+ 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).getNormalized();
+ Vec3 direction3 = (rot*direction2).getNormalized();
+
+ //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).getNormalized(); //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 )
+{
+ 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(GeomPrimitive &geom1, GeomPrimitive &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.getNormalized();
+ /*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 != 0);
+ //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 ){
+ real_t l1, l2, l3, d1, d2, d3;
+ l1 = (A-B).length(); /* These three sqrt evaluations are necessary */
+ l2 = (A-C).length();
+ l3 = (A-D).length();
+
+ Vec3 n1, n2, n3;
+ n1 = (real_t(1.0)/l1)*(A-B);
+ n2 = (real_t(1.0)/l2)*(A-C);
+ n3 = (real_t(1.0)/l3)*(A-D);
+
+ // Here we already see if such circle exists.
+ real_t det = n1 * (n2 % n3);
+ if(std::fabs(det) < math::Limits<real_t>::fpuAccuracy()){
+ //no circle exists. Leave center untouched, and return -1.0
+ return real_t(-1.0);
+ }
+ real_t Alen = A.sqrLength();
+ d1 = (Alen - B.sqrLength())/(real_t(2.0)*l1);
+ d2 = (Alen - C.sqrLength())/(real_t(2.0)*l2);
+ d3 = (Alen - D.sqrLength())/(real_t(2.0)*l3);
+
+ //Apply solution formula
+ center = (real_t(1.0)/det)*(d1 * (n2 % n3) + d2 * (n3 % n1) + d3 * (n1 % n2));
+
+ return (A - center).sqrLength();
+}
+//*************************************************************************************************
+
+} //fcd
+} //pe
+} //walberla
diff --git a/src/pe/collision/EPA.h b/src/pe/collision/EPA.h
new file mode 100644
index 0000000000000000000000000000000000000000..da611ecaec9e01165899874d5769ea32511befd1
--- /dev/null
+++ b/src/pe/collision/EPA.h
@@ -0,0 +1,529 @@
+//======================================================================================================================
+//
+// 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
+
+//*************************************************************************************************
+// Includes
+//*************************************************************************************************
+
+#include "GJK.h"
+#include <pe/Thresholds.h>
+#include <pe/Types.h>
+
+#include <core/math/Constants.h>
+#include <core/math/Limits.h>
+#include <core/math/Matrix3.h>
+#include <core/math/Quaternion.h>
+
+#include <vector>
+
+namespace walberla {
+namespace pe {
+namespace fcd {
+
+//=================================================================================================
+//
+// CLASS DEFINITION
+//
+//=================================================================================================
+
+//*************************************************************************************************
+/*!\brief The Expanding-Polytope Algorithm.
+ * \ingroup fine_collision_detection
+ */
+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:
+ //**Query functions*****************************************************************************
+ /*!\name Query functions */
+ //@{
+ bool doEPAcontactThreshold( GeomPrimitive &geom1, GeomPrimitive &geom2, const GJK& gjk, Vec3& normal,
+ Vec3& contactPoint, real_t& penetrationDepth);
+
+
+ bool doEPAcontactThreshold( GeomPrimitive &geom1, GeomPrimitive &geom2, const GJK& gjk, Vec3& normal,
+ Vec3& contactPoint, real_t& penetrationDepth, real_t eps_rel);
+
+ bool doEPAmargin( GeomPrimitive &geom1, GeomPrimitive &geom2, const GJK& gjk, Vec3& normal,
+ Vec3& contactPoint, real_t& penetrationDepth, real_t margin);
+
+ bool doEPA( GeomPrimitive &geom1, GeomPrimitive &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_;}
+
+ //@}
+ //**********************************************************************************************
+
+private:
+ //**Utility functions***************************************************************************
+ /*!\name Utility functions */
+ //@{
+ inline void pushSupportMargin(const GeomPrimitive &geom1, const GeomPrimitive &geom2, const Vec3& dir, const real_t margin,
+ std::vector<Vec3>& epaVolume, std::vector<Vec3>& supportA, std::vector<Vec3>& supportB);
+
+ inline void replaceSupportMargin(const GeomPrimitive &geom1, const GeomPrimitive &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 (GeomPrimitive &geom1, GeomPrimitive &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, GeomPrimitive &geom1, GeomPrimitive &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;
+};
+//*************************************************************************************************
+
+
+
+
+//=================================================================================================
+//
+// 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 */
+ //@{
+ EPA_Triangle* getTriangle() const;
+ size_t getIndex() const;
+ size_t getStart() const;
+ size_t getEnd() const;
+ //@}
+ //**********************************************************************************************
+
+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_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 */
+ //@{
+ inline size_t operator[]( size_t i ) const;
+ inline const Vec3& getClosest() const;
+ inline const Vec3& getNormal() const;
+ inline Vec3 getClosestPoint(const std::vector<Vec3>& points) const;
+ inline real_t getSqrDist() const;
+ inline bool isObsolete() const;
+ 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_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 */
+ //@{
+ inline bool operator()( const EPA_Triangle *tria1, const EPA_Triangle *tria2 );
+ //@}
+ //**********************************************************************************************
+};
+//*************************************************************************************************
+
+
+//=================================================================================================
+//
+// 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_EDGE GET FUNCTIONS
+//
+//=================================================================================================
+
+//*************************************************************************************************
+/*! \brief Return the triangle this edge belongs to.
+ */
+inline EPA::EPA_Triangle* EPA::EPA_Edge::getTriangle() const
+{
+ return triangle_;
+}
+//*************************************************************************************************
+
+
+//*************************************************************************************************
+/*! \brief Get the Index of this edge in its triangle.
+ */
+inline size_t EPA::EPA_Edge::getIndex() const
+{
+ return startIdx_;
+}
+//*************************************************************************************************
+
+
+
+//*************************************************************************************************
+/*! \brief Return the start point index of an edge.
+ *
+ */
+inline size_t EPA::EPA_Edge::getStart() const
+{
+ return (*triangle_)[startIdx_];
+}
+//*************************************************************************************************
+
+
+//*************************************************************************************************
+/*! \brief Return the end point index of an edge.
+ */
+inline size_t EPA::EPA_Edge::getEnd() const
+{
+ return (*triangle_)[(startIdx_+1) % 3];
+}
+//*************************************************************************************************
+
+
+
+
+//=================================================================================================
+//
+// EPA::EPA_TRIANGLE GET FUNCTIONS
+//
+//=================================================================================================
+
+//*************************************************************************************************
+/*! \brief Returns the index of the internal vertex i(=0,1,2) within the EPA scope.
+ */
+inline size_t EPA::EPA_Triangle::operator[]( size_t i ) const
+{
+ return indices_[i];
+}
+//*************************************************************************************************
+
+
+//*************************************************************************************************
+/*! \brief Returns the point closest to the origin of the affine hull of the triangle, which is also the normal.
+ */
+inline const Vec3& EPA::EPA_Triangle::getClosest() const
+{
+ return closest_;
+}
+//*************************************************************************************************
+
+
+//*************************************************************************************************
+/*! \brief Returns the normal of the triangle. Normal is not normalized!
+ */
+inline const Vec3& EPA::EPA_Triangle::getNormal() const
+{
+ return normal_;
+}
+//*************************************************************************************************
+
+
+//*************************************************************************************************
+/*! \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]];
+
+}
+//*************************************************************************************************
+
+
+//*************************************************************************************************
+/*! \brief Returns the squared distance to the closest to the origin of the affine hull of the triangle.
+ */
+inline real_t EPA::EPA_Triangle::getSqrDist() const
+{
+ return sqrDist_;
+}
+//*************************************************************************************************
+
+
+//*************************************************************************************************
+/*! \brief Returns true if the triangle is no longer part of the EPA polygon.
+ */
+inline bool EPA::EPA_Triangle::isObsolete() const
+{
+ return obsolete_;
+}
+//*************************************************************************************************
+
+
+//*************************************************************************************************
+/*! 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::EPA_TRIANGLECOMP BINARY FUNCTION CALL OPERATOR
+//
+//=================================================================================================
+
+//*************************************************************************************************
+/*!\brief Compare two triangles by their distance.
+ */
+inline bool EPA::EPA_TriangleComp::operator()( const EPA_Triangle *tria1,
+ const EPA_Triangle *tria2 )
+{
+ return tria1->getSqrDist() > tria2->getSqrDist();
+}
+//*************************************************************************************************
+
+
+//=================================================================================================
+//
+// 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 GeomPrimitive &geom1, const GeomPrimitive &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.getNormalized();
+ }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 GeomPrimitive &geom1, const GeomPrimitive &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.getNormalized();
+ }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/pe/collision/GJK.cpp b/src/pe/collision/GJK.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..b681b780507d51e1b72bd684f6805b07f122bd62
--- /dev/null
+++ b/src/pe/collision/GJK.cpp
@@ -0,0 +1,1045 @@
+//======================================================================================================================
+//
+// 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"
+
+//*************************************************************************************************
+// Includes
+//*************************************************************************************************
+
+#include <vector>
+#include <pe/Types.h>
+#include <pe/Thresholds.h>
+#include <core/Abort.h>
+#include <core/math/Limits.h>
+#include <core/math/Vector3.h>
+
+
+namespace walberla {
+namespace pe {
+namespace fcd {
+
+//=================================================================================================
+//
+// 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(GeomPrimitive &geom1, GeomPrimitive &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 The first Body
+ * \param geom2 The second Body
+ * \param margin The margin by which the objects will be enlarged.
+ * \return true, if an itersection is found.
+ */
+bool GJK::doGJKmargin(GeomPrimitive &geom1, GeomPrimitive &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
+ 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;
+}
+//*************************************************************************************************
+
+} //fcd
+} //pe
+} //walberla
diff --git a/src/pe/collision/GJK.h b/src/pe/collision/GJK.h
new file mode 100644
index 0000000000000000000000000000000000000000..a562ccc0dca3e819b5e7fe1de0b02e1d1ecb8576
--- /dev/null
+++ b/src/pe/collision/GJK.h
@@ -0,0 +1,208 @@
+//======================================================================================================================
+//
+// 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
+
+//*************************************************************************************************
+// Includes
+//*************************************************************************************************
+
+#include <vector>
+
+#include <pe/rigidbody/GeomPrimitive.h>
+#include <pe/Thresholds.h>
+
+#include <core/Abort.h>
+#include <core/math/Limits.h>
+#include <core/math/Vector3.h>
+
+namespace walberla {
+namespace pe {
+namespace fcd {
+
+//=================================================================================================
+//
+// CLASS DEFINITION
+//
+//=================================================================================================
+
+//*************************************************************************************************
+/*!\brief Impelementation of the Gilbert-Johnson-Keerthi Algorithm.
+ */
+class GJK
+{
+public:
+
+ //**Constructor*********************************************************************************
+ /*! \name Constructor */
+ //@{
+ explicit inline GJK();
+ //@}
+ //**********************************************************************************************
+
+ //**Query functions*****************************************************************************
+ /*! \name Query functions */
+ //@{
+ real_t doGJK( GeomPrimitive &geom1, GeomPrimitive &geom2, Vec3& normal, Vec3& contactPoint );
+
+ bool doGJKmargin( GeomPrimitive &geom1, GeomPrimitive &geom2, const real_t margin = contactThreshold);
+ //@}
+ //**********************************************************************************************
+
+ //**Get functions*******************************************************************************
+ /*! \name Get functions */
+ //@{
+ inline const std::vector<Vec3>& getSimplex() const;
+ inline size_t getSimplexSize() const;
+ inline const std::vector<Vec3>& getSupportA() const;
+ inline const std::vector<Vec3>& getSupportB() const;
+ //@}
+ //**********************************************************************************************
+
+private:
+ //**Utility functions***************************************************************************
+ /*! \name Utility functions */
+ //@{
+ bool simplex2(Vec3& d);
+ bool simplex3(Vec3& d);
+ bool simplex4(Vec3& d);
+
+ inline bool sameDirection ( const Vec3& vec1, const Vec3& vec2 ) const;
+ inline bool zeroLengthVector( const Vec3& vec ) const;
+ real_t calcDistance ( Vec3& normal, Vec3& contactPoint );
+ inline const Vec3 putSupport(const GeomPrimitive &geom1, const GeomPrimitive &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.
+ //@}
+ //**********************************************************************************************
+};
+//*************************************************************************************************
+
+//=================================================================================================
+//
+// CONSTRUCTOR
+//
+//=================================================================================================
+
+//*************************************************************************************************
+inline 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
+}
+//*************************************************************************************************
+
+
+//=================================================================================================
+//
+// GET FUNCTIONS
+//
+//=================================================================================================
+
+//*************************************************************************************************
+inline const std::vector<Vec3>& GJK::getSimplex() const
+{
+ return simplex_;
+}
+//*************************************************************************************************
+
+
+//*************************************************************************************************
+inline size_t GJK::getSimplexSize() const
+{
+ return numPoints_;
+}
+//*************************************************************************************************
+
+
+//*************************************************************************************************
+inline const std::vector<Vec3>& GJK::getSupportA() const
+{
+ return supportA_;
+}
+//*************************************************************************************************
+
+
+//*************************************************************************************************
+inline const std::vector<Vec3>& GJK::getSupportB() const
+{
+ return supportB_;
+}
+//*************************************************************************************************
+
+
+//=================================================================================================
+//
+// UTILITY FUNCTIONS
+//
+//=================================================================================================
+
+//*************************************************************************************************
+/*!\brief Checks if two vectors roughly point in the same directionTODO
+ */
+inline bool GJK::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 GJK::zeroLengthVector(const Vec3& vec) const
+{
+ return vec.sqrLength() < math::Limits<real_t>::fpuAccuracy();
+}
+//*************************************************************************************************
+
+
+//*************************************************************************************************
+/*!\brief Calucate a support point of a body extended by threshold.
+ * \param geom The body.
+ * \param dir The support point direction.
+ * \param threshold Extension of the Body.
+ */
+inline const Vec3 GJK::putSupport(const GeomPrimitive &geom1, const GeomPrimitive &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;
+}
+//*************************************************************************************************
+
+
+} // namespace fcd
+
+} // namespace pe
+
+} // namespace walberla
diff --git a/src/pe/collision/GJKEPAHelper.cpp b/src/pe/collision/GJKEPAHelper.cpp
index d119ed751c55885eb09881aa5a3f67270dd2364b..d8316cd6db099983dc7b0772f6d506e9ac23cd3a 100644
--- a/src/pe/collision/GJKEPAHelper.cpp
+++ b/src/pe/collision/GJKEPAHelper.cpp
@@ -36,7 +36,7 @@ extern "C" {
namespace walberla {
namespace pe {
-Vec3 convertVec3(const ccd_vec3_t& vec) { return Vec3(vec.v[0], vec.v[1], vec.v[2]); }
+Vec3 convertVec3(const ccd_vec3_t& vec) { return Vec3(real_c(vec.v[0]), real_c(vec.v[1]), real_c(vec.v[2])); }
ccd_vec3_t convertVec3(const Vec3& vec) { ccd_vec3_t ret; ret.v[0] = vec[0]; ret.v[1] = vec[1]; ret.v[2] = vec[2]; return ret; }
void support(const void *obj, const ccd_vec3_t *dir, ccd_vec3_t *vec)
@@ -65,7 +65,9 @@ bool collideGJK( ConstGeomID bd1,
ccd.epa_tolerance = epaTol;
ccd_vec3_t dir, pos;
- int intersect = ccdGJKPenetration(reinterpret_cast<const void*> (bd1), reinterpret_cast<const void*> (bd2), &ccd, &penetrationDepth, &dir, &pos);
+ ccd_real_t penetrationDepthCCD;
+ int intersect = ccdGJKPenetration(reinterpret_cast<const void*> (bd1), reinterpret_cast<const void*> (bd2), &ccd, &penetrationDepthCCD, &dir, &pos);
+ penetrationDepth = real_c(penetrationDepthCCD);
contactPoint = convertVec3(pos);
contactNormal = -convertVec3(dir);
penetrationDepth *= -1;
diff --git a/src/pe/communication/DynamicMarshalling.h b/src/pe/communication/DynamicMarshalling.h
index 73f222c15c55051d33474390cc6df2512193f2f6..e2d2c4a6ceb2b4e35d1aad180c9b4700514bc03f 100644
--- a/src/pe/communication/DynamicMarshalling.h
+++ b/src/pe/communication/DynamicMarshalling.h
@@ -33,6 +33,7 @@
#include "pe/communication/rigidbody/Capsule.h"
#include "pe/communication/rigidbody/Sphere.h"
#include "pe/communication/rigidbody/Union.h"
+#include "pe/communication/rigidbody/Ellipsoid.h"
#include "pe/utility/BodyCast.h"
#include "core/Abort.h"
diff --git a/src/pe/communication/rigidbody/Ellipsoid.cpp b/src/pe/communication/rigidbody/Ellipsoid.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..d7e7199a4e4186b467734ac36b778f276eb223bb
--- /dev/null
+++ b/src/pe/communication/rigidbody/Ellipsoid.cpp
@@ -0,0 +1,59 @@
+//======================================================================================================================
+//
+// 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.cpp
+//! \author Tobias Preclik
+//! \author Sebastian Eibl <sebastian.eibl@fau.de>
+//! \brief Marshalling of objects for data transmission or storage.
+//
+//======================================================================================================================
+
+#include "Ellipsoid.h"
+
+namespace walberla {
+namespace pe {
+namespace communication {
+
+//*************************************************************************************************
+/*!\brief Marshalling a Ellipsoid primitive.
+ *
+ * \param buffer The buffer to be filled.
+ * \param obj The object to be marshalled.
+ * \return void
+ */
+void marshal( mpi::SendBuffer& buffer, const Ellipsoid& obj ) {
+ marshal( buffer, static_cast<const GeomPrimitive&>( obj ) );
+ buffer << obj.getSemiAxes();
+}
+//*************************************************************************************************
+
+
+//*************************************************************************************************
+/*!\brief Unmarshalling a Ellipsoid primitive.
+ *
+ * \param buffer The buffer from where to read.
+ * \param objparam The object to be reconstructed.
+ * \param hasSuperBody False if body is not part of a union. Passed on to rigid body unmarshalling.
+ * \return void
+ */
+void unmarshal( mpi::RecvBuffer& buffer, EllipsoidParameters& objparam ) {
+ unmarshal( buffer, static_cast<GeomPrimitiveParameters&>( objparam ) );
+ buffer >> objparam.semiAxes_;
+}
+//*************************************************************************************************
+
+} // namespace communication
+} // namespace pe
+} // namespace walberla
diff --git a/src/pe/communication/rigidbody/Ellipsoid.h b/src/pe/communication/rigidbody/Ellipsoid.h
new file mode 100644
index 0000000000000000000000000000000000000000..57916d268a30dbfea187071a18b4270ee63c83f3
--- /dev/null
+++ b/src/pe/communication/rigidbody/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 Tobias Preclik
+//! \author Sebastian Eibl <sebastian.eibl@fau.de>
+//! \brief Marshalling of objects for data transmission or storage.
+//
+//======================================================================================================================
+
+#pragma once
+
+
+//*************************************************************************************************
+// Includes
+//*************************************************************************************************
+
+#include "pe/communication/Instantiate.h"
+#include "pe/communication/Marshalling.h"
+#include "pe/rigidbody/Ellipsoid.h"
+
+namespace walberla {
+namespace pe {
+namespace communication {
+
+struct EllipsoidParameters : public GeomPrimitiveParameters {
+ Vec3 semiAxes_;
+};
+
+//*************************************************************************************************
+/*!\brief Marshalling a Ellipsoid primitive.
+ *
+ * \param buffer The buffer to be filled.
+ * \param obj The object to be marshalled.
+ * \return void
+ */
+void marshal( mpi::SendBuffer& buffer, const Ellipsoid& obj );
+//*************************************************************************************************
+
+//*************************************************************************************************
+/*!\brief Unmarshalling a Ellipsoid primitive.
+ *
+ * \param buffer The buffer from where to read.
+ * \param objparam The object to be reconstructed.
+ * \param hasSuperBody False if body is not part of a union. Passed on to rigid body unmarshalling.
+ * \return void
+ */
+void unmarshal( mpi::RecvBuffer& buffer, EllipsoidParameters& objparam );
+//*************************************************************************************************
+
+
+inline EllipsoidID instantiate( mpi::RecvBuffer& buffer, const math::AABB& domain, const math::AABB& block, EllipsoidID& newBody )
+{
+ EllipsoidParameters subobjparam;
+ unmarshal( buffer, subobjparam );
+ correctBodyPosition(domain, block.center(), subobjparam.gpos_);
+ newBody = new Ellipsoid( subobjparam.sid_, subobjparam.uid_, subobjparam.gpos_, subobjparam.rpos_, subobjparam.q_, subobjparam.semiAxes_, subobjparam.material_, false, subobjparam.communicating_, subobjparam.infiniteMass_ );
+ newBody->setLinearVel( subobjparam.v_ );
+ newBody->setAngularVel( subobjparam.w_ );
+ newBody->MPITrait.setOwner( subobjparam.mpiTrait_.owner_ );
+ return newBody;
+}
+
+} // namespace communication
+} // namespace pe
+} // namespace walberla
diff --git a/src/pe/fcd/GJKEPACollideFunctor.h b/src/pe/fcd/GJKEPACollideFunctor.h
new file mode 100644
index 0000000000000000000000000000000000000000..84b6d62e4bf58386aae3e2f4b94484647511854c
--- /dev/null
+++ b/src/pe/fcd/GJKEPACollideFunctor.h
@@ -0,0 +1,218 @@
+//======================================================================================================================
+//
+// 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 GJKEPACollideFunctor.h
+//! \author Tobias Leemann
+//! \author Sebastian Eibl <sebastian.eibl@fau.de>
+//
+//======================================================================================================================
+
+#pragma once
+
+
+#include "pe/Types.h"
+#include "pe/collision/EPA.h"
+#include "pe/collision/GJK.h"
+#include "pe/rigidbody/Plane.h"
+#include "pe/rigidbody/Union.h"
+#include <pe/Thresholds.h>
+#include <boost/tuple/tuple.hpp>
+
+namespace walberla{
+namespace pe{
+namespace fcd {
+namespace gjkepa{
+
+ //function for all single rigid bodies.
+ template<typename Container>
+ inline bool generateContacts(GeomPrimitive *a, GeomPrimitive *b, Container& contacts_);
+
+ //Planes
+ template<typename Container>
+ inline bool generateContacts(Plane *a, GeomPrimitive *b, Container& contacts_);
+
+ template<typename Container>
+ inline bool generateContacts(GeomPrimitive *a, Plane *b, Container& contacts_);
+
+ template< typename Container>
+ inline bool generateContacts(Plane *a, Plane *b, Container& contacts_);
+
+ //Unions
+ template<typename BodyTupleA, typename BodyB, typename Container>
+ inline bool generateContacts(Union<BodyTupleA> *a, BodyB *b, Container& contacts_);
+
+ template<typename BodyA, typename BodyTupleB, typename Container>
+ inline bool generateContacts(BodyA *a, Union<BodyTupleB> *b, Container& contacts_);
+
+ template<typename BodyTupleA, typename BodyTupleB, typename Container>
+ inline bool generateContacts(Union<BodyTupleA> *a, Union<BodyTupleB> *b, Container& contacts_);
+
+ //Union and Plane
+ template<typename BodyTupleA, typename Container>
+ inline bool generateContacts(Union<BodyTupleA> *a, Plane *b, Container& contacts_);
+
+ template<typename BodyTupleB, typename Container>
+ inline bool generateContacts(Plane *a, Union<BodyTupleB> *b, Container& contacts_);
+}
+
+/* Iterative Collide Functor for contact Generation with iterative collision detection (GJK and EPA algorithms).
+ * Usage: fcd::GenericFCD<BodyTuple, fcd::GJKEPACollideFunctor> testFCD;
+ * testFCD.generateContacts(...);
+ */
+template <typename Container>
+struct GJKEPACollideFunctor
+{
+ Container& contacts_;
+
+ GJKEPACollideFunctor(Container& contacts) : contacts_(contacts) {}
+
+ template< typename BodyType1, typename BodyType2 >
+ bool operator()( BodyType1* bd1, BodyType2* bd2) {
+ using namespace gjkepa;
+ return generateContacts(bd1, bd2, contacts_);
+ }
+};
+
+template <typename BodyType1, typename Container>
+struct GJKEPASingleCollideFunctor
+{
+ BodyType1* bd1_;
+ Container& contacts_;
+
+ GJKEPASingleCollideFunctor(BodyType1* bd1, Container& contacts) : bd1_(bd1), contacts_(contacts) {}
+
+ template< typename BodyType2 >
+ bool operator()( BodyType2* bd2) {
+ using namespace gjkepa;
+ return generateContacts( bd1_, bd2, contacts_);
+ }
+};
+
+
+namespace gjkepa{
+
+ //function for all single rigid bodies.
+ template<typename Container>
+ inline bool generateContacts(GeomPrimitive *a, GeomPrimitive *b, Container& contacts_){
+ Vec3 normal;
+ Vec3 contactPoint;
+ real_t penetrationDepth;
+
+ real_t margin = real_t(1e-4);
+ GJK gjk;
+ if(gjk.doGJKmargin(*a, *b, margin)){
+ //2. If collision is possible perform EPA.
+ EPA epa;
+ if(epa.doEPAmargin(*a, *b, gjk, normal, contactPoint, penetrationDepth, margin)){
+ contacts_.push_back( Contact(a, b, contactPoint, normal, penetrationDepth) );
+ return true;
+ }else{
+ return false;
+ }
+
+ }else{
+ return false;
+ }
+ }
+
+ //Planes
+ template<typename Container>
+ inline bool generateContacts(Plane *a, GeomPrimitive *b, Container& contacts_){
+ Vec3 normal;
+ Vec3 contactPoint;
+ real_t penetrationDepth;
+
+ Vec3 support_dir = -a->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 = b->support(support_dir);
+ real_t pdepth = contactp * a->getNormal() - a->getDisplacement();
+ if(pdepth < contactThreshold){ //We have a collision
+ normal = support_dir;
+ penetrationDepth = pdepth;
+ contactPoint = contactp + real_t(0.5) * penetrationDepth * normal;
+ contacts_.push_back( Contact(a, b, contactPoint, normal, penetrationDepth) );
+ return true;
+ }else{ //No collision
+ return false;
+ }
+ }
+
+ template<typename Container>
+ inline bool generateContacts(GeomPrimitive *a, Plane *b, Container& contacts_){
+ return generateContacts(b, a, contacts_);
+ }
+
+ //Planes cannot collide with each other
+ template< typename Container>
+ inline bool generateContacts(Plane*, Plane*, Container&){
+ return false;
+ }
+
+ //Unions
+ template<typename BodyTupleA, typename BodyB, typename Container>
+ inline bool generateContacts(Union<BodyTupleA> *a, BodyB *b, Container& contacts_){
+ GJKEPASingleCollideFunctor<BodyB, Container> func(b, contacts_);
+ bool collision = false;
+ for( auto it=a->begin(); it!=a->end(); ++it )
+ {
+ collision |= SingleCast<BodyTupleA, GJKEPASingleCollideFunctor<BodyB, Container>, bool>::execute(*it, func);
+ }
+ return collision;
+ }
+
+ template<typename BodyA, typename BodyTupleB, typename Container>
+ inline bool generateContacts(BodyA *a, Union<BodyTupleB> *b, Container& contacts_){
+ return generateContacts(b, a, contacts_);
+ }
+
+ template<typename BodyTupleA, typename BodyTupleB, typename Container>
+ inline bool generateContacts(Union<BodyTupleA> *a, Union<BodyTupleB> *b, Container& contacts_){
+ GJKEPACollideFunctor<Container> func(contacts_);
+ bool collision = false;
+ for( auto it1=a->begin(); it1!=a->end(); ++it1 )
+ {
+ for( auto it2=b->begin(); it2!=b->end(); ++it2 )
+ {
+ collision |= DoubleCast<BodyTupleA, BodyTupleB, GJKEPACollideFunctor<Container>, bool>::execute(*it1, *it2, func);
+ }
+ }
+ return collision;
+ }
+
+ //Union and Plane (these calls are ambigous if not implemented seperatly)
+ template<typename BodyTupleA, typename Container>
+ inline bool generateContacts(Union<BodyTupleA> *a, Plane *b, Container& contacts_){
+ GJKEPASingleCollideFunctor<Plane, Container> func(b, contacts_);
+ bool collision = false;
+ for( auto it=a->begin(); it!=a->end(); ++it )
+ {
+ collision |= SingleCast<BodyTupleA, GJKEPASingleCollideFunctor<Plane, Container>, bool>::execute(*it, func);
+ }
+ return collision;
+ }
+
+ template<typename BodyTupleB, typename Container>
+ inline bool generateContacts(Plane *a, Union<BodyTupleB> *b, Container& contacts_){
+ return generateContacts(b, a, contacts_);
+ }
+
+
+} //namespace gjkepa
+
+
+} //fcd
+} //pe
+} //walberla
diff --git a/src/pe/rigidbody/Box.h b/src/pe/rigidbody/Box.h
index 427c5ea2a9aa79e6c32866779b4315056171fd88..b76ab998319134bdb84faacaade1c192ea4543e0 100644
--- a/src/pe/rigidbody/Box.h
+++ b/src/pe/rigidbody/Box.h
@@ -83,6 +83,7 @@ public:
/*!\name Get functions */
//@{
inline const Vec3& getLengths() const;
+ virtual inline real_t getVolume() const;
//@}
//**********************************************************************************************
@@ -306,7 +307,16 @@ inline const Vec3& Box::getLengths() const
}
//*************************************************************************************************
-
+//*************************************************************************************************
+/*!\brief Returns the volume of the box.
+ *
+ * \return The volume of the box.
+ */
+inline real_t Box::getVolume() const
+{
+ return Box::calcVolume(getLengths());
+}
+//*************************************************************************************************
//=================================================================================================
diff --git a/src/pe/rigidbody/Ellipsoid.cpp b/src/pe/rigidbody/Ellipsoid.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..c3a0b6bb8c4c48b745b1f7db34511d96fb7af0eb
--- /dev/null
+++ b/src/pe/rigidbody/Ellipsoid.cpp
@@ -0,0 +1,264 @@
+//======================================================================================================================
+//
+// 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.cpp
+//! \author Klaus Iglberger
+//! \author Sebastian Eibl <sebastian.eibl@fau.de>
+//
+//======================================================================================================================
+
+#include "Ellipsoid.h"
+
+//*************************************************************************************************
+// Includes
+//*************************************************************************************************
+
+#include <iomanip>
+#include <iostream>
+#include <stdexcept>
+#include <cmath>
+#include <pe/Materials.h>
+#include <core/math/Matrix3.h>
+#include <core/debug/Debug.h>
+
+namespace walberla {
+namespace pe {
+
+//=================================================================================================
+//
+// CONSTRUCTOR
+//
+//=================================================================================================
+
+
+//*************************************************************************************************
+//*************************************************************************************************
+/*!\brief Constructor for the Ellipsoid class.
+ *
+ * \param sid Unique system-specific ID for the Ellipsoid.
+ * \param uid User-specific ID for the Ellipsoid.
+ * \param gpos Global geometric center of the Ellipsoid.
+ * \param rpos The relative position within the body frame of a superordinate body.
+ * \param q The orientation of the Ellipsoid's body frame in the global world frame.
+ * \param radius The radius of the Ellipsoid \f$ (0..\infty) \f$.
+ * \param material The material of the Ellipsoid.
+ * \param global specifies if the Ellipsoid should be created in the global storage
+ * \param communicating specifies if the Ellipsoid should take part in synchronization (syncNextNeighbour, syncShadowOwner)
+ * \param infiniteMass specifies if the Ellipsoid has infinite mass and will be treated as an obstacle
+ */
+Ellipsoid::Ellipsoid( id_t sid, id_t uid, const Vec3& gpos, const Vec3& rpos, const Quat& q,
+ const Vec3& semiAxes, MaterialID material,
+ const bool global, const bool communicating, const bool infiniteMass )
+ : Ellipsoid::Ellipsoid( getStaticTypeID(), sid, uid, gpos, rpos, q, semiAxes, material, global, communicating, infiniteMass )
+{}
+Ellipsoid::Ellipsoid( id_t const typeId, id_t sid, id_t uid, const Vec3& gpos, const Vec3& rpos, const Quat& q,
+ const Vec3& semiAxes, MaterialID material,
+ const bool global, const bool communicating, const bool infiniteMass )
+ : GeomPrimitive( typeId, sid, uid, material ) // Initialization of the parent class
+ , semiAxes_(semiAxes)
+{
+ // Checking the radius
+ // Since the Ellipsoid constructor is never directly called but only used in a small number
+ // of functions that already check the Ellipsoid arguments, only asserts are used here to
+ // double check the arguments.
+ WALBERLA_ASSERT( semiAxes_[0] > real_c(0), "Invalid Ellipsoid radius" );
+ WALBERLA_ASSERT( semiAxes_[1] > real_c(0), "Invalid Ellipsoid radius" );
+ WALBERLA_ASSERT( semiAxes_[2] > real_c(0), "Invalid Ellipsoid radius" );
+ // Setting the center of the Ellipsoid
+ gpos_ = gpos;
+
+ // Initializing the instantiated Ellipsoid
+ rpos_ = rpos; // Setting the relative position
+ q_ = q; // Setting the orientation
+ R_ = q_.toRotationMatrix(); // Setting the rotation matrix
+
+ // Calculating the Ellipsoid mass
+ mass_ = calcMass(semiAxes_, Material::getDensity( material ));
+ invMass_ = real_c(1) / mass_;
+
+ // Calculating the moment of inertia
+ calcInertia();
+
+ setGlobal( global );
+ setMass( infiniteMass );
+ setCommunicating( communicating );
+ setFinite( true );
+
+ // Setting the axis-aligned bounding box
+ Ellipsoid::calcBoundingBox();
+}
+//*************************************************************************************************
+
+
+
+
+//=================================================================================================
+//
+// DESTRUCTOR
+//
+//=================================================================================================
+
+//*************************************************************************************************
+/*!\brief Destructor for the Ellipsoid class.
+ */
+Ellipsoid::~Ellipsoid()
+{
+ // Logging the destruction of the Ellipsoid
+ WALBERLA_LOG_DETAIL( "Destroyed Ellipsoid " << sid_ );
+}
+//*************************************************************************************************
+
+
+
+
+//=================================================================================================
+//
+// UTILITY FUNCTIONS
+//
+//=================================================================================================
+
+//*************************************************************************************************
+/*!\brief Checks, whether a point in body relative coordinates lies inside the Ellipsoid.
+ *
+ * \param px The x-component of the relative coordinate.
+ * \param py The y-component of the relative coordinate.
+ * \param pz The z-component of the relative coordinate.
+ * \return \a true if the point lies inside the Ellipsoid, \a false if not.
+ */
+bool Ellipsoid::containsRelPointImpl( real_t px, real_t py, real_t pz ) const
+{
+return ( (px * px)/(semiAxes_[0] * semiAxes_[0]) + (py * py)/(semiAxes_[1] * semiAxes_[1])
+ + (pz * pz)/(semiAxes_[2] * semiAxes_[2]) <= real_t(1.0) );
+}
+//*************************************************************************************************
+
+
+//*************************************************************************************************
+/*!\brief Checks, whether a point in body relative coordinates lies on the surface of the Ellipsoid.
+ *
+ * \param px The x-component of the relative coordinate.
+ * \param py The y-component of the relative coordinate.
+ * \param pz The z-component of the relative coordinate.
+ * \return \a true if the point lies on the surface of the Ellipsoid, \a false if not.
+ *
+ * The (relative) tolerance level of the check is pe::surfaceThreshold.
+ */
+bool Ellipsoid::isSurfaceRelPointImpl( real_t px, real_t py, real_t pz ) const
+{
+ return floatIsEqual( (px * px)/(semiAxes_[0] * semiAxes_[0]) + (py * py)/(semiAxes_[1] * semiAxes_[1])
+ + (pz * pz)/(semiAxes_[2] * semiAxes_[2]), real_t(1.0), pe::surfaceThreshold);
+}
+//*************************************************************************************************
+
+
+
+
+//=================================================================================================
+//
+// OUTPUT FUNCTIONS
+//
+//=================================================================================================
+
+//*************************************************************************************************
+/*!\brief Output of the current state of a Ellipsoid.
+ *
+ * \param os Reference to the output stream.
+ * \param tab Indentation in front of every line of the Ellipsoid output.
+ * \return void
+ */
+void Ellipsoid::print( std::ostream& os, const char* tab ) const
+{
+ using std::setw;
+
+ os << tab << " Ellipsoid " << uid_ << " with semi-axis " << semiAxes_ << "\n";
+
+ os << tab << " Fixed: " << isFixed() << " , sleeping: " << !isAwake() << "\n";
+
+ os << tab << " System ID = " << getSystemID() << "\n"
+ << tab << " Total mass = " << getMass() << "\n"
+ << tab << " Material = " << Material::getName( material_ ) << "\n"
+ << tab << " Owner = " << MPITrait.getOwner() << "\n"
+ << tab << " Global position = " << getPosition() << "\n"
+ << tab << " Relative position = " << getRelPosition() << "\n"
+ << tab << " Linear velocity = " << getLinearVel() << "\n"
+ << tab << " Angular velocity = " << getAngularVel() << "\n";
+
+// if( verboseMode )
+// {
+ os << tab << " Bounding box = " << getAABB() << "\n"
+ << tab << " Quaternion = " << getQuaternion() << "\n"
+ << tab << " Rotation matrix = ( " << setw(9) << R_[0] << " , " << setw(9) << R_[1] << " , " << setw(9) << R_[2] << " )\n"
+ << tab << " ( " << setw(9) << R_[3] << " , " << setw(9) << R_[4] << " , " << setw(9) << R_[5] << " )\n"
+ << tab << " ( " << setw(9) << R_[6] << " , " << setw(9) << R_[7] << " , " << setw(9) << R_[8] << " )\n";
+
+ os << std::setiosflags(std::ios::right)
+ << tab << " Moment of inertia = ( " << setw(9) << I_[0] << " , " << setw(9) << I_[1] << " , " << setw(9) << I_[2] << " )\n"
+ << tab << " ( " << setw(9) << I_[3] << " , " << setw(9) << I_[4] << " , " << setw(9) << I_[5] << " )\n"
+ << tab << " ( " << setw(9) << I_[6] << " , " << setw(9) << I_[7] << " , " << setw(9) << I_[8] << " )\n"
+ << std::resetiosflags(std::ios::right);
+// }
+}
+//*************************************************************************************************
+
+
+
+
+//=================================================================================================
+//
+// GLOBAL OPERATORS
+//
+//=================================================================================================
+
+//*************************************************************************************************
+/*!\brief Global output operator for Ellipsoids.
+ *
+ * \param os Reference to the output stream.
+ * \param s Reference to a constant Ellipsoid object.
+ * \return Reference to the output stream.
+ */
+std::ostream& operator<<( std::ostream& os, const Ellipsoid& s )
+{
+ os << "--" << "Ellipsoid PARAMETERS"
+ << "-------------------------------------------------------------\n";
+ s.print( os, "" );
+ os << "--------------------------------------------------------------------------------\n"
+ << std::endl;
+ return os;
+}
+//*************************************************************************************************
+
+
+//*************************************************************************************************
+/*!\brief Global output operator for Ellipsoid handles.
+ *
+ * \param os Reference to the output stream.
+ * \param s Constant Ellipsoid handle.
+ * \return Reference to the output stream.
+ */
+std::ostream& operator<<( std::ostream& os, ConstEllipsoidID s )
+{
+ os << "--" << "Ellipsoid PARAMETERS"
+ << "-------------------------------------------------------------\n";
+ s->print( os, "" );
+ os << "--------------------------------------------------------------------------------\n"
+ << std::endl;
+ return os;
+}
+//*************************************************************************************************
+
+id_t Ellipsoid::staticTypeID_ = std::numeric_limits<id_t>::max();
+
+} // namespace pe
+} // namespace walberla
diff --git a/src/pe/rigidbody/Ellipsoid.h b/src/pe/rigidbody/Ellipsoid.h
new file mode 100644
index 0000000000000000000000000000000000000000..f2baad173f8f64029055721511af7c220a06e400
--- /dev/null
+++ b/src/pe/rigidbody/Ellipsoid.h
@@ -0,0 +1,348 @@
+//======================================================================================================================
+//
+// 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 Klaus Iglberger
+//! \author Sebastian Eibl <sebastian.eibl@fau.de
+//! \author Tobias Leemann <tobias.leemann@fau.de>
+//
+//======================================================================================================================
+
+#pragma once
+
+
+//*************************************************************************************************
+// Includes
+//*************************************************************************************************
+
+#include <pe/rigidbody/GeomPrimitive.h>
+#include <pe/Types.h>
+#include <core/math/Constants.h>
+#include <core/math/Matrix3.h>
+#include <core/math/Vector3.h>
+#include <core/DataTypes.h>
+#include <core/logging/Logging.h>
+#include <core/math/Constants.h>
+#include <core/math/Limits.h>
+#include <core/math/Utility.h>
+#include <pe/Config.h>
+
+
+namespace walberla {
+namespace pe {
+
+//=================================================================================================
+//
+// CLASS DEFINITION
+//
+//=================================================================================================
+
+//*************************************************************************************************
+/**
+ * \ingroup pe
+ * \brief Base class for the Ellipsoid geometry.
+ *
+ * The Ellipsoid class represents the base class for the Ellipsoid geometry. It provides
+ * the basic functionality of a Ellipsoid. An Ellipsoid is obtained from a sphere by deforming it by means
+ * of directional scalings. Its three semi-axes corresponding to the x, y, z direction are labeled
+ * a, b, c.
+ */
+class Ellipsoid : public GeomPrimitive
+{
+public:
+ //**Constructors********************************************************************************
+ /*!\name Constructors */
+ //@{
+ explicit Ellipsoid( id_t sid, id_t uid, const Vec3& gpos, const Vec3& rpos, const Quat& q,
+ const Vec3& semiAxes, MaterialID material,
+ const bool global, const bool communicating, const bool infiniteMass );
+ explicit Ellipsoid( id_t const typeID, id_t sid, id_t uid, const Vec3& gpos, const Vec3& rpos, const Quat& q,
+ const Vec3& semiAxes, MaterialID material,
+ const bool global, const bool communicating, const bool infiniteMass );
+ //@}
+ //**********************************************************************************************
+
+ //**Destructor**********************************************************************************
+ /*!\name Destructor */
+ //@{
+ virtual ~Ellipsoid();
+ //@}
+ //**********************************************************************************************
+ //**********************************************************************************************
+
+public:
+ //**Get functions*******************************************************************************
+ /*!\name Get functions */
+ //@{
+ inline const Vec3& getSemiAxes() const;
+ virtual inline real_t getVolume() const;
+ //@}
+ //**********************************************************************************************
+
+ //**Utility functions***************************************************************************
+ static inline id_t getStaticTypeID();
+ //@}
+ //**********************************************************************************************
+
+ //**Output functions****************************************************************************
+ /*!\name Output functions */
+ //@{
+ virtual void print( std::ostream& os, const char* tab ) const;
+ //@}
+ //**********************************************************************************************
+
+ //**Utility functions***************************************************************************
+ /*!\name Utility functions */
+ //@{
+ inline virtual Vec3 support( const Vec3& d ) const;
+ //@}
+ //**********************************************************************************************
+
+ //**Volume, mass and density functions**********************************************************
+ /*!\name Volume, mass and density functions */
+ //@{
+ static inline real_t calcVolume( const Vec3& semiAxes );
+ static inline real_t calcMass( const Vec3& semiAxes, real_t density );
+ static inline real_t calcDensity( const Vec3& semiAxes, real_t mass );
+ //@}
+ //**********************************************************************************************
+
+protected:
+ //**Utility functions***************************************************************************
+ /*!\name Utility functions */
+ //@{
+ virtual bool containsRelPointImpl ( real_t px, real_t py, real_t pz ) const;
+ virtual bool isSurfaceRelPointImpl( real_t px, real_t py, real_t pz ) const;
+ //@}
+ //**********************************************************************************************
+
+ //**Utility functions***************************************************************************
+ /*!\name Utility functions */
+ //@{
+ inline virtual void calcBoundingBox(); // Calculation of the axis-aligned bounding box
+ inline void calcInertia(); // Calculation of the moment of inertia
+ //@}
+ //**********************************************************************************************
+
+ //**Member variables****************************************************************************
+ /*!\name Member variables */
+ //@{
+ Vec3 semiAxes_; //!< Semi-axes of the Ellipsoid.
+ /*!< The radius is constrained to values larger than 0.0. */
+ //@}
+ //**********************************************************************************************
+private:
+ static id_t staticTypeID_; //< type id of Ellipsoid, will be set by SetBodyTypeIDs
+ static void setStaticTypeID(id_t typeID) {staticTypeID_ = typeID;}
+
+ //** friend declaration
+ /// needed to be able to set static type ids with setStaticTypeID
+ template <class T>
+ friend struct SetBodyTypeIDs;
+};
+//*************************************************************************************************
+
+
+
+
+//=================================================================================================
+//
+// GET FUNCTIONS
+//
+//=================================================================================================
+
+//*************************************************************************************************
+/*!\brief Returns the semi-axes of the Ellipsoid.
+ *
+ * \return The semi-axes of the Ellipsoid.
+ */
+inline const Vec3& Ellipsoid::getSemiAxes() const
+{
+ return semiAxes_;
+}
+//*************************************************************************************************
+
+
+//*************************************************************************************************
+/*!\brief Returns the volume of the Ellipsoid.
+ *
+ * \return The volume of the Ellipsoid.
+ */
+inline real_t Ellipsoid::getVolume() const
+{
+ return Ellipsoid::calcVolume(getSemiAxes());
+}
+//*************************************************************************************************
+
+
+
+
+//=================================================================================================
+//
+// VOLUME, MASS AND DENSITY FUNCTIONS
+//
+//=================================================================================================
+
+//*************************************************************************************************
+/*!\brief Calculates the volume of a Ellipsoid for a given vector of semi-axes.
+ *
+ * \param semiAxes The vector of semi-axes of the Ellipsoid.
+ * \return The volume of the Ellipsoid.
+ */
+inline real_t Ellipsoid::calcVolume(const Vec3& semiAxes )
+{
+ return real_c(4.0)/real_c(3.0) * math::M_PI * semiAxes[0] * semiAxes[1] * semiAxes[2];
+}
+//*************************************************************************************************
+
+
+//*************************************************************************************************
+/*!\brief Calculates the mass of a Ellipsoid for a given a given vector of semi-axes and density.
+ *
+ * \param semiAxes The vector of semi-axes of the Ellipsoid.
+ * \param density The density of the Ellipsoid.
+ * \return The total mass of the Ellipsoid.
+ */
+inline real_t Ellipsoid::calcMass(const Vec3& semiAxes, real_t density )
+{
+ return real_c(4.0)/real_c(3.0) * math::M_PI * semiAxes[0] * semiAxes[1] * semiAxes[2] * density;
+}
+//*************************************************************************************************
+
+
+//*************************************************************************************************
+/*!\brief Calculates the density of a Ellipsoid for a given vector of semi-axes and mass.
+ *
+ * \param semiAxes The vector of semi-axes of the Ellipsoid.
+ * \param mass The total mass of the Ellipsoid.
+ * \return The density of the Ellipsoid.
+ */
+inline real_t Ellipsoid::calcDensity(const Vec3& semiAxes, real_t mass )
+{
+ return real_c(0.75) * mass / ( math::M_PI * semiAxes[0] * semiAxes[1] * semiAxes[2] );
+}
+//*************************************************************************************************
+
+
+
+
+//=================================================================================================
+//
+// UTILITY FUNCTIONS
+//
+//=================================================================================================
+
+//*************************************************************************************************
+/*!\brief Calculation of the bounding box of the Ellipsoid.
+ *
+ * \return void
+ *
+ * This function updates the axis-aligned bounding box of the Ellipsoid primitive according to the
+ * current position and orientation of the Ellipsoid. Note that the bounding box is increased in
+ * all dimensions by pe::contactThreshold to guarantee that rigid bodies in close proximity of
+ * the Ellipsoid are also considered during the collision detection process.
+ * Algorithm: Use a non-axes-aligned bounding box of the ellipse (box
+ * with sides twice the semi-axes long) and calc its AABB.
+ */
+inline void Ellipsoid::calcBoundingBox()
+{
+ using std::fabs;
+
+ const real_t xlength( fabs(R_[0]*semiAxes_[0]) + fabs(R_[1]*semiAxes_[1]) + fabs(R_[2]*semiAxes_[2]) + contactThreshold );
+ const real_t ylength( fabs(R_[3]*semiAxes_[0]) + fabs(R_[4]*semiAxes_[1]) + fabs(R_[5]*semiAxes_[2]) + contactThreshold );
+ const real_t zlength( fabs(R_[6]*semiAxes_[0]) + fabs(R_[7]*semiAxes_[1]) + fabs(R_[8]*semiAxes_[2]) + contactThreshold );
+ aabb_ = math::AABB(
+ gpos_[0] - xlength,
+ gpos_[1] - ylength,
+ gpos_[2] - zlength,
+ gpos_[0] + xlength,
+ gpos_[1] + ylength,
+ gpos_[2] + zlength
+ );
+ // WALBERLA_ASSERT( aabb_.isValid() , "Invalid bounding box detected" );
+ WALBERLA_ASSERT( aabb_.contains( gpos_ ), "Invalid bounding box detected("<< getSystemID() <<")\n" << "pos: " << gpos_ << "\nlengths: " << xlength << "," << ylength << ", " << zlength<< "\nvel: " << getLinearVel() << "\nbox: " << aabb_ );
+}
+//*************************************************************************************************
+
+
+//*************************************************************************************************
+/*!\brief Calculation of the moment of inertia in reference to the body frame of the Ellipsoid.
+ *
+ * \return void
+ */
+inline void Ellipsoid::calcInertia()
+{
+ I_[0] = real_c(0.2) * mass_ * (semiAxes_[1] * semiAxes_[1] + semiAxes_[2] * semiAxes_[2]);
+ I_[4] = real_c(0.2) * mass_ * (semiAxes_[2] * semiAxes_[2] + semiAxes_[0] * semiAxes_[0]);
+ I_[8] = real_c(0.2) * mass_ * (semiAxes_[0] * semiAxes_[0] + semiAxes_[1] * semiAxes_[1]);
+ Iinv_ = I_.getInverse();
+}
+//*************************************************************************************************
+
+
+//*************************************************************************************************
+/*!\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.
+ */
+inline Vec3 Ellipsoid::support( const Vec3& d ) const
+{
+ auto len = d.sqrLength();
+ if (!math::equal(len, real_t(0)))
+ {
+ Vec3 d_loc = vectorFromWFtoBF(d);
+ 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 pointFromBFtoWF(local_support);
+ } else
+ {
+ return Vec3(0,0,0);
+ }
+}
+//*************************************************************************************************
+
+
+//*************************************************************************************************
+/*!\brief Returns unique type id of this type
+ *
+ * \return geometry specific type id
+ */
+inline id_t Ellipsoid::getStaticTypeID()
+{
+ return staticTypeID_;
+}
+//*************************************************************************************************
+
+
+//=================================================================================================
+//
+// GLOBAL OPERATORS
+//
+//=================================================================================================
+
+//*************************************************************************************************
+/*!\name Ellipsoid operators */
+//@{
+std::ostream& operator<<( std::ostream& os, const Ellipsoid& s );
+std::ostream& operator<<( std::ostream& os, ConstEllipsoidID s );
+//@}
+//*************************************************************************************************
+
+
+} // namespace pe
+} // namespace walberla
diff --git a/src/pe/rigidbody/EllipsoidFactory.cpp b/src/pe/rigidbody/EllipsoidFactory.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..e848dd28527d9c763c55bab2b7e02467c82b05a3
--- /dev/null
+++ b/src/pe/rigidbody/EllipsoidFactory.cpp
@@ -0,0 +1,83 @@
+//======================================================================================================================
+//
+// 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 EllipsoidFactory.cpp
+//! \author Tobias Leemann <tobias.leemann@fau.de>
+//! \author Sebastian Eibl <sebastian.eibl@fau.de>
+//
+//======================================================================================================================
+
+#include "EllipsoidFactory.h"
+
+#include "pe/Materials.h"
+#include "pe/rigidbody/BodyStorage.h"
+#include "pe/rigidbody/Ellipsoid.h"
+
+namespace walberla {
+namespace pe {
+
+EllipsoidID createEllipsoid( BodyStorage& globalStorage, BlockStorage& blocks, BlockDataID storageID,
+ id_t uid, const Vec3& gpos, const Vec3& semiAxes,
+ MaterialID material,
+ bool global, bool communicating, bool infiniteMass )
+{
+ WALBERLA_ASSERT_UNEQUAL( Ellipsoid::getStaticTypeID(), std::numeric_limits<id_t>::max(), "Ellipsoid TypeID not initalized!");
+ // Checking the semiAxes
+ if( semiAxes[0] <= real_c(0) || semiAxes[1] <= real_c(0) || semiAxes[2] <= real_c(0) )
+ throw std::invalid_argument( "Invalid Ellipsoid semi-axes" );
+
+ EllipsoidID ellipsoid = NULL;
+
+ if (global)
+ {
+ const id_t sid = UniqueID<RigidBody>::createGlobal();
+ WALBERLA_ASSERT_EQUAL(communicating, false);
+ WALBERLA_ASSERT_EQUAL(infiniteMass, true);
+ ellipsoid = new Ellipsoid(sid, uid, gpos, Vec3(0,0,0), Quat(), semiAxes, material, global, false, true);
+ globalStorage.add(ellipsoid);
+ } else
+ {
+ for (auto it = blocks.begin(); it != blocks.end(); ++it){
+ IBlock* block = (&(*it));
+ if (block->getAABB().contains(gpos))
+ {
+ const id_t sid( UniqueID<RigidBody>::create() );
+
+ Storage* bs = block->getData<Storage>(storageID);
+ ellipsoid = new Ellipsoid(sid, uid, gpos, Vec3(0,0,0), Quat(), semiAxes, material, global, communicating, infiniteMass);
+ ellipsoid->MPITrait.setOwner(Owner(MPIManager::instance()->rank(), block->getId().getID()));
+ (*bs)[0].add(ellipsoid);
+ }
+ }
+ }
+
+ if (ellipsoid != NULL)
+ {
+ // Logging the successful creation of the Ellipsoid
+ WALBERLA_LOG_DETAIL(
+ "Created Ellipsoid " << ellipsoid->getSystemID() << "\n"
+ << " User-ID = " << uid << "\n"
+ << " Global position = " << gpos << "\n"
+ << " Semi-axes = " << semiAxes << "\n"
+ << " LinVel = " << ellipsoid->getLinearVel() << "\n"
+ << " Material = " << Material::getName( material )
+ );
+ }
+
+ return ellipsoid;
+}
+
+} // namespace pe
+} // namespace walberla
diff --git a/src/pe/rigidbody/EllipsoidFactory.h b/src/pe/rigidbody/EllipsoidFactory.h
new file mode 100644
index 0000000000000000000000000000000000000000..78b335920d0eb92390fff93fdd35f6ea0e08c5f0
--- /dev/null
+++ b/src/pe/rigidbody/EllipsoidFactory.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 EllipsoidFactory.h
+//! \author Tobias Leemann
+//! \author Sebastian Eibl <sebastian.eibl@fau.de>
+//
+//======================================================================================================================
+
+#pragma once
+
+#include "pe/Materials.h"
+#include "pe/Types.h"
+
+#include "blockforest/BlockForest.h"
+#include "core/debug/Debug.h"
+
+namespace walberla {
+namespace pe {
+
+//=================================================================================================
+//
+// Ellipsoid SETUP FUNCTIONS
+//
+//=================================================================================================
+
+//*************************************************************************************************
+/**
+ * \ingroup pe
+ * \brief Setup of a new Ellipsoid.
+ *
+ * \param globalStorage process local global storage
+ * \param blocks storage of all the blocks on this process
+ * \param storageID BlockDataID of the BlockStorage block datum
+ * \param uid The user-specific ID of the Ellipsoid.
+ * \param gpos The global position of the center of the Ellipsoid.
+ * \param semiAxes The semiAxes of the Ellipsoid \f$ (0..\infty) \f$.
+ * \param material The material of the Ellipsoid.
+ * \param global specifies if the Ellipsoid should be created in the global storage
+ * \param communicating specifies if the Ellipsoid should take part in synchronization (syncNextNeighbour, syncShadowOwner)
+ * \param infiniteMass specifies if the Ellipsoid has infinite mass and will be treated as an obstacle
+ * \return Handle for the new Ellipsoid.
+ * \exception std::invalid_argument Invalid Ellipsoid semi-axes.
+ * \exception std::invalid_argument Invalid global Ellipsoid position.
+ *
+ * This function creates a Ellipsoid primitive in the \b pe simulation system. The Ellipsoid with
+ * user-specific ID \a uid is placed at the global position \a gpos, has the semi-axes \a semiAxes,
+ * and consists of the material \a material.
+ */
+EllipsoidID createEllipsoid( BodyStorage& globalStorage, BlockStorage& blocks, BlockDataID storageID,
+ id_t uid, const Vec3& gpos, const Vec3& semiAxes,
+ MaterialID material = Material::find("iron"),
+ bool global = false, bool communicating = true, bool infiniteMass = false );
+//*************************************************************************************************
+
+} // namespace pe
+} // namespace walberla
diff --git a/src/pe/rigidbody/Sphere.h b/src/pe/rigidbody/Sphere.h
index bc0e82566c5f171eee4aa354a2dbedb7e75033b7..536f7b7ce767e37ceac9cae875348dfce45b2ab8 100644
--- a/src/pe/rigidbody/Sphere.h
+++ b/src/pe/rigidbody/Sphere.h
@@ -306,7 +306,10 @@ inline Vec3 Sphere::support( const Vec3& d ) const
auto len = d.sqrLength();
if (!math::equal(len, real_t(0)))
{
- return getPosition() + getRadius() / sqrt(len) * d;
+ //WALBERLA_ASSERT_FLOAT_EQUAL( len, real_t(1), "search direction not normalized!");
+ const Vec3 s = getPosition() + (getRadius() / sqrt(len)) * d;
+ //std::cout << "Support in direction " << d << " with center " << getPosition() << " (r=" << getRadius() << ") is " << s << std::endl;
+ return s;
} else
{
return Vec3(0,0,0);
@@ -324,9 +327,17 @@ inline Vec3 Sphere::support( const Vec3& d ) const
*/
inline Vec3 Sphere::supportContactThreshold( const Vec3& d ) const
{
- WALBERLA_ASSERT( d.sqrLength() <= real_c(0.0), "Zero length search direction" );
- WALBERLA_ASSERT( 1.0-math::Limits<real_t>::fpuAccuracy() <= d.length() && d.length() <= 1.0+math::Limits<real_t>::fpuAccuracy(), "Search direction is not normalised" );
- return gpos_ + d*(radius_ + contactThreshold);
+ auto len = d.sqrLength();
+ if (!math::equal(len, real_t(0)))
+ {
+ //WALBERLA_ASSERT_FLOAT_EQUAL( len, real_t(1), "search direction not normalized!");
+ const Vec3 s = getPosition() + (getRadius() / sqrt(len) + contactThreshold) * d;
+ //std::cout << "Support in direction " << d << " with center " << getPosition() << " (r=" << getRadius() << ") is " << s << std::endl;
+ return s;
+ } else
+ {
+ return Vec3(0,0,0);
+ }
}
//*************************************************************************************************
diff --git a/tests/pe/CMakeLists.txt b/tests/pe/CMakeLists.txt
index 1427603f3758a076ba0f79507e38bf56e1a339b1..940099119c89027fd58d2107f946c0f5576505f2 100644
--- a/tests/pe/CMakeLists.txt
+++ b/tests/pe/CMakeLists.txt
@@ -22,6 +22,9 @@ waLBerla_execute_test( NAME PE_CHECKVITALPARAMETERS )
waLBerla_compile_test( NAME PE_COLLISION FILES Collision.cpp DEPENDS core )
waLBerla_execute_test( NAME PE_COLLISION )
+waLBerla_compile_test( NAME PE_COLLISIONTOBIASGJK FILES CollisionTobiasGJK.cpp DEPENDS core )
+waLBerla_execute_test( NAME PE_COLLISIONTOBIASGJK )
+
waLBerla_compile_test( NAME PE_DELETEBODY FILES DeleteBody.cpp DEPENDS core blockforest )
waLBerla_execute_test( NAME PE_DELETEBODY_NN COMMAND $<TARGET_FILE:PE_DELETEBODY> )
waLBerla_execute_test( NAME PE_DELETEBODY_SO COMMAND $<TARGET_FILE:PE_DELETEBODY> --syncShadowOwners )
diff --git a/tests/pe/CollisionTobiasGJK.cpp b/tests/pe/CollisionTobiasGJK.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..e800d11f80e76aed81568a148406c1e21578716b
--- /dev/null
+++ b/tests/pe/CollisionTobiasGJK.cpp
@@ -0,0 +1,440 @@
+//======================================================================================================================
+//
+// 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 CollisionTobiasGJK.cpp
+//! \author Sebastian Eibl <sebastian.eibl@fau.de>
+//
+//======================================================================================================================
+#include "pe/Types.h"
+
+
+#include "pe/contact/Contact.h"
+//#include "pe/fcd/IFCD.h"
+#include "pe/fcd/GenericFCD.h"
+#include "pe/fcd/AnalyticCollisionDetection.h"
+#include "pe/fcd/GJKEPACollideFunctor.h"
+#include "pe/Materials.h"
+
+#include "pe/rigidbody/Box.h"
+#include "pe/rigidbody/Capsule.h"
+#include "pe/rigidbody/Sphere.h"
+#include "pe/rigidbody/Plane.h"
+#include "pe/rigidbody/Union.h"
+#include "pe/rigidbody/Ellipsoid.h"
+
+#include "pe/rigidbody/SetBodyTypeIDs.h"
+
+
+#include "core/debug/TestSubsystem.h"
+#include "core/DataTypes.h"
+#include "core/math/Vector2.h"
+#include "core/math/Constants.h"
+
+#include "pe/collision/EPA.h"
+#include "pe/collision/GJK.h"
+
+using namespace walberla;
+using namespace walberla::pe;
+
+typedef boost::tuple<Box, Capsule, Plane, Sphere, Union<boost::tuple<Sphere, Union<boost::tuple<Sphere>>>>, Ellipsoid> BodyTuple ;
+
+bool gjkEPAcollideHybrid(GeomPrimitive &geom1, GeomPrimitive &geom2, Vec3& normal, Vec3& contactPoint, real_t& penetrationDepth)
+{
+ using namespace walberla::pe::fcd;
+ // 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-4);
+ GJK gjk;
+ if(gjk.doGJKmargin(geom1, geom2, margin)){
+ //2. If collision is possible perform EPA.
+ //std::cerr << "Peforming EPA.";
+ EPA epa;
+ 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 Contact& c1, const Contact& c2, const Vec3& planeNormal, const real_t accuracy = test_accuracy )
+{
+
+ WALBERLA_CHECK_EQUAL( c1.getBody1(), c2.getBody1() );
+ WALBERLA_CHECK_EQUAL( c1.getBody2(), c2.getBody2() );
+
+ WALBERLA_CHECK_LESS( fabs((c1.getNormal() - c2.getNormal()).sqrLength()), accuracy*accuracy );
+ WALBERLA_CHECK_LESS( fabs(c1.getDistance()- c2.getDistance()), accuracy );
+
+ //Unfortunately position accuracy is one-two orders of magnitude lower...
+ if(floatIsEqual(planeNormal.sqrLength(), real_t(0.0))){
+ WALBERLA_CHECK_LESS( fabs((c1.getPosition()- c2.getPosition()).sqrLength()), real_t(1e4)*accuracy*accuracy );
+ }else{
+ //check for containment in plane only.
+ WALBERLA_CHECK_LESS( fabs(c1.getPosition()*planeNormal-c2.getPosition()*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(GeomPrimitive &rb1, GeomPrimitive &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.getPosition(); //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.setPosition(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( Contact( &rb1, &rb2, pos1, normal1, comp_pen_depth1),
+ Contact( &rb1, &rb2, witnesspoint + depth[j] * witnessmove, real_axis, -depth[j] * penetration_factor ), planeNormal, accuracy );
+ checkContact( Contact( &rb2, &rb1, pos2, normal2, comp_pen_depth2),
+ Contact( &rb2, &rb1, 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()
+{
+ MaterialID iron = Material::find("iron");
+
+ // Original SPHERE <-> SPHERE
+ Sphere sp1(123, 1, Vec3(0,0,0), Vec3(0,0,0), Quat(), 1, iron, false, true, false);
+ Sphere sp2(124, 2, Vec3(real_t(1.5),0,0), Vec3(0,0,0), Quat(), 1, iron, false, true, false);
+ Sphere sp3(125, 3, Vec3(real_t(3.0),0,0), Vec3(0,0,0), Quat(), 1, iron, false, true, false);
+
+ 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( Contact( &sp1, &sp2, contactPoint, normal, penetrationDepth),
+ Contact( &sp1, &sp2, 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 box1_1(127, 5, Vec3(0, 0, 0), Vec3(0,0,0), Quat(), Vec3(10, 10, 10), iron, false, true, false);
+ Sphere sphere1_2(130, 8, Vec3(coordinate, coordinate, coordinate), Vec3(0,0,0), Quat(), 5, iron, false, true, false);
+ 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 box2_1(131, 9, Vec3(real_t(20.0),0,0), Vec3(0,0,0), Quat(), Vec3(real_t(30.0),1,1), iron, false, true, false);
+ 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");
+ Sphere sphere3_1(129, 7, Vec3(0,0,0), Vec3(0,0,0), Quat(), 5, iron, false, true, false);
+ Sphere sphere3_2(128, 6, Vec3(real_t(10.0),0,0), Vec3(0,0,0), Quat(), 5, iron, false, true, false);
+ 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));
+ Box box4_1(132, 10, Vec3(real_t(5.0)*(real_t(1.0)+sqr2), real_t(-5.0), 0), Vec3(0,0,0), q4, Vec3(10, 10, 10), iron, false, true, false);
+ Box box4_2(133, 11, Vec3(0, 0, 0), Vec3(0,0,0), Quat(), Vec3(10, 10, 10), iron, false, true, false);
+ 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)");
+ Box box5_1(133, 12, Vec3(0, 0, 0), Vec3(0,0,0), Quat(), Vec3(10, 10, 10), iron, false, true, false);
+ Box box5_2(134, 13, Vec3(real_t(15.0),real_t(5.5), 0), Vec3(0,0,0), Quat(), Vec3(real_t(30.0),1,1), iron, false, true, false);
+ 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)
+ Box box6_1(136, 14, Vec3(real_t(5.0)*(real_t(1.0)+sqr6_3), 0, 0), Vec3(0,0,0), q6, Vec3(10, 10, 10), iron, false, true, false);
+ Box box6_2(136, 15, Vec3(0, 0, 0), Vec3(0,0,0), Quat(), Vec3(10, 10, 10), iron, false, true, false);
+ 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");
+ Sphere sphere7_1(137, 16, Vec3(0,0,0), Vec3(0,0,0), Quat(), 5, iron, false, true, false);
+ Box box7_2(138, 17, Vec3(0, 0,real_t(7.5)), Vec3(0,0,0), Quat(), Vec3(5, 5, 5), iron, false, true, false);
+ 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 08:
+ // CAPSULE <-> CAPSULE
+ WALBERLA_LOG_INFO("Test 08: CAPSULE <-> CAPSULE");
+ Quat q8(Vec3(0,1,0), walberla::math::M_PI/real_t(2.0)); //creates a y-axis aligned capsule
+ Capsule cap8_1(139, 18, Vec3(0,0,0), Vec3(0,0,0), Quat(), real_t(4.0), real_t(10.0), iron, false, true, false);
+ Capsule cap8_2(140, 19, Vec3(0,0, real_t(13.0)), Vec3(0,0,0), q8, real_t(4.0), real_t(10.0), iron, false, true, false);
+ Vec3 wpm8(0, 0, real_t(-0.5));
+ Vec3 wp8(0, 0, real_t(4.0));
+ Vec3 axis8(0, 0, real_t(-1.0));
+ runCollisionDataTest(cap8_1, cap8_2, axis8, real_t(1.0), axis8, wp8, wpm8, Vec3(0,0,0));
+
+ //Testcase 09:
+ // ELLIPSOID <-> ELLIPSOID
+ WALBERLA_LOG_INFO("Test 09: ELLIPSOID <-> ELLIPSOID");
+ Ellipsoid ell9_1(141, 20, Vec3(0,0,0), Vec3(0,0,0), Quat(), Vec3(10,5,5), iron, false, true, false);
+ Ellipsoid ell9_2(142, 21, Vec3(15,0,0), Vec3(0,0,0), Quat(), Vec3(5,10,5), iron, false, true, false);
+ 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));
+
+}
+
+/** Test the GJK-EPA implementation for a collision
+ * of a plane and a body and test the interface calls. */
+void PlaneTest()
+{
+ WALBERLA_LOG_INFO("PLANE AND INTERFACE TEST");
+ MaterialID iron = Material::find("iron");
+ fcd::GenericFCD<BodyTuple, fcd::GJKEPACollideFunctor> testFCD;
+
+ Plane pl(1, 1, Vec3(0, 1, 0), Vec3(0, 1, 0), real_t(1.0), iron );
+ Sphere sphere(2, 2, Vec3(0, real_t(1.9), 0), Vec3(0,0,0), Quat(), 1, iron, false, true, false);
+ Sphere sphere2(3, 3, Vec3(0, real_t(0.1), 0), Vec3(0,0,0), Quat(), 1, iron, false, true, false);
+
+ PossibleContacts pcs;
+
+ pcs.push_back(std::pair<Sphere*, Sphere*>(&sphere, &sphere2));
+ Contacts& container = testFCD.generateContacts(pcs);
+ WALBERLA_CHECK(container.size() == 1);
+
+ Contact &c = container.back();
+ //
+ WALBERLA_LOG_DEVEL( c.getDistance() << " " << c.getNormal() << " " << c.getPosition() );
+ if(c.getBody1()->getID() == 2) {
+ checkContact( c, Contact(&sphere, &sphere2, Vec3(0, real_t(1), 0), Vec3(0, 1, 0), real_t(-0.2)), Vec3(0,0,0));
+ } else if (c.getBody1()->getID() == 3) {
+ checkContact( c, Contact(&sphere2, &sphere, Vec3(0, real_t(1), 0), Vec3(0, -1, 0), real_t(-0.2)), Vec3(0,0,0));
+ } else {
+ WALBERLA_ABORT("Unknown ID!");
+ }
+ pcs.clear();
+
+ pcs.push_back(std::pair<Plane*, Sphere*>(&pl, &sphere));
+ container = testFCD.generateContacts(pcs);
+ WALBERLA_CHECK(container.size() == 1);
+
+ c = container.back();
+ //
+ WALBERLA_LOG_DEVEL( c.getDistance() << " " << c.getNormal() << " " << c.getPosition() );
+ if(c.getBody1()->getID() == 1) {
+ checkContact( c, Contact(&pl, &sphere, Vec3(0, real_t(0.95), 0), Vec3(0, -1, 0), real_t(-0.1)), Vec3(0,0,0));
+ } else if (c.getBody1()->getID() == 2) {
+ checkContact( c, Contact(&sphere, &pl, Vec3(0, real_t(0.95), 0), Vec3(0, 1, 0), real_t(-0.1)), Vec3(0,0,0));
+ } else {
+ WALBERLA_ABORT("Unknown ID!");
+ }
+ pcs.clear();
+
+ pcs.push_back(std::pair<Sphere*, Plane*>(&sphere, &pl));
+
+ container = testFCD.generateContacts(pcs);
+ WALBERLA_CHECK(container.size() == 1);
+ c = container.back();
+
+ WALBERLA_LOG_DEVEL( c.getDistance() << " " << c.getNormal() << " " << c.getPosition() );
+ if(c.getBody1()->getID() == 1) {
+ checkContact( c, Contact(&pl, &sphere, Vec3(0, real_t(0.95), 0), Vec3(0, -1, 0), real_t(-0.1)), Vec3(0,0,0));
+ } else if (c.getBody1()->getID() == 2) {
+ checkContact( c, Contact(&sphere, &pl, Vec3(0, real_t(0.95), 0), Vec3(0, 1, 0), real_t(-0.1)), Vec3(0,0,0));
+ } else {
+ WALBERLA_ABORT("Unknown ID!");
+ }
+}
+
+/** Test the GJK-EPA implementation for a collision
+ * of a union and a body and the interface calls. */
+void UnionTest(){
+ WALBERLA_LOG_INFO("UNION AND INTERFACE TEST");
+ MaterialID iron = Material::find("iron");
+ fcd::GenericFCD<BodyTuple, fcd::GJKEPACollideFunctor> testFCD;
+
+ //A recursive union of three spheres is dropped on a box.
+ Box box(179, 179, Vec3(0,0,0), Vec3(0,0,0), Quat(), Vec3(real_t(10),real_t(2), real_t(10)), iron, false, true, false);
+
+
+ Union<boost::tuple<Sphere>> *unsub = new Union<boost::tuple<Sphere>>(192, 192, Vec3(0,real_t(3.8),0), Vec3(0,0,0), Quat(), false, true, false);
+
+ Sphere sp1( 180, 180, Vec3(-3,real_t(3.8),0), Vec3(0,0,0), Quat(), real_t(3.0) , iron, false, true, false );
+ Sphere sp2( 181, 181, Vec3(3,real_t(3.8),0), Vec3(0,0,0), Quat(), real_t(3.0), iron, false, true, false );
+
+ Sphere sp3( 182, 182, Vec3(0,real_t(6),0), Vec3(0,0,0), Quat(), real_t(3.0), iron, false, true, false );
+ unsub->add(&sp1);
+ unsub->add(&sp2);
+
+ //Create another union, and add sub union
+ Union<boost::tuple<Sphere, Union<boost::tuple<Sphere>>>> *un = new Union<boost::tuple<Sphere, Union<boost::tuple<Sphere>>>>(193, 193, Vec3(0, 0, 0), Vec3(0,0,0), Quat(), false, true, false);
+ un->add(&sp3);
+ un->add(unsub);
+
+
+ PossibleContacts pcs;
+ pcs.push_back(std::pair<Union<boost::tuple<Sphere,Union<boost::tuple<Sphere>>>>*, Box*>(un, &box));
+ Contacts& container = testFCD.generateContacts(pcs);
+ WALBERLA_CHECK(container.size() == 2);
+
+ Contact &c = container.back();
+ WALBERLA_LOG_DEVEL( c.getDistance() << " " << c.getNormal() << " " << c.getPosition() );
+ if(c.getBody1()->getID() == 181) {
+ checkContact( c, Contact(&sp2, &box, Vec3(real_t(3), real_t(0.9), 0), Vec3(0, 1, 0), real_t(-0.2)), Vec3(0,0,0));
+ } else if (c.getBody1()->getID() == 179) {
+ checkContact( c, Contact(&box, &sp2, Vec3(real_t(3), real_t(0.9), 0), Vec3(0, -1, 0), real_t(-0.2)), Vec3(0,0,0));
+ } else {
+ WALBERLA_ABORT("Unknown ID!");
+ }
+ container.pop_back();
+
+
+ c = container.back();
+ WALBERLA_LOG_DEVEL( c.getDistance() << " " << c.getNormal() << " " << c.getPosition() );
+ if(c.getBody1()->getID() == 180) {
+ checkContact( c, Contact(&sp1, &box, Vec3(real_t(-3), real_t(0.9), 0), Vec3(0, 1, 0), real_t(-0.2)), Vec3(0,0,0));
+ } else if (c.getBody1()->getID() == 179) {
+ checkContact( c, Contact(&box, &sp1, Vec3(real_t(-3), real_t(0.9), 0), Vec3(0, -1, 0), real_t(-0.2)), Vec3(0,0,0));
+ } else {
+ WALBERLA_ABORT("Unknown ID!");
+ }
+ pcs.clear();
+
+ //Vice Versa
+ pcs.push_back(std::pair<Box*, Union<boost::tuple<Sphere, Union<boost::tuple<Sphere>>>>* >(&box, un));
+ container = testFCD.generateContacts(pcs);
+ WALBERLA_CHECK(container.size() == 2);
+
+ c = container.back();
+ WALBERLA_LOG_DEVEL( c.getDistance() << " " << c.getNormal() << " " << c.getPosition() );
+ if(c.getBody1()->getID() == 181) {
+ checkContact( c, Contact(&sp2, &box, Vec3(real_t(3), real_t(0.9), 0), Vec3(0, 1, 0), real_t(-0.2)), Vec3(0,0,0));
+ } else if (c.getBody1()->getID() == 179) {
+ checkContact( c, Contact(&box, &sp2, Vec3(real_t(3), real_t(0.9), 0), Vec3(0, -1, 0), real_t(-0.2)), Vec3(0,0,0));
+ } else {
+ WALBERLA_ABORT("Unknown ID!");
+ }
+ container.pop_back();
+
+ c = container.back();
+ WALBERLA_LOG_DEVEL( c.getDistance() << " " << c.getNormal() << " " << c.getPosition() );
+ if(c.getBody1()->getID() == 180) {
+ checkContact( c, Contact(&sp1, &box, Vec3(real_t(-3), real_t(0.9), 0), Vec3(0, 1, 0), real_t(-0.2)), Vec3(0,0,0));
+ } else if (c.getBody1()->getID() == 179) {
+ checkContact( c, Contact(&box, &sp1, Vec3(real_t(-3), real_t(0.9), 0), Vec3(0, -1, 0), real_t(-0.2)), Vec3(0,0,0));
+ } else {
+ WALBERLA_ABORT("Unknown ID!");
+ }
+ pcs.clear();
+
+}
+
+int main( int argc, char** argv )
+{
+ walberla::debug::enterTestMode();
+
+ walberla::MPIManager::instance()->initializeMPI( &argc, &argv );
+
+ SetBodyTypeIDs<BodyTuple>::execute();
+ MainTest();
+ PlaneTest();
+ UnionTest();
+ return EXIT_SUCCESS;
+}
diff --git a/tests/pe/Marshalling.cpp b/tests/pe/Marshalling.cpp
index 57411b0d279cf2e5524170450d82688314fc6697..30da9d3be10fbad20a2e545dbfa915a85a988ac0 100644
--- a/tests/pe/Marshalling.cpp
+++ b/tests/pe/Marshalling.cpp
@@ -26,6 +26,7 @@
#include "pe/rigidbody/Squirmer.h"
#include "pe/rigidbody/UnionFactory.h"
#include "pe/rigidbody/Union.h"
+#include "pe/rigidbody/Ellipsoid.h"
#include "pe/communication/rigidbody/Squirmer.h"
#include "pe/communication/DynamicMarshalling.h"
#include "pe/rigidbody/SetBodyTypeIDs.h"
@@ -41,7 +42,7 @@ typedef boost::tuple<Sphere> UnionTypeTuple;
typedef Union< UnionTypeTuple > UnionT;
typedef UnionT* UnionID;
-typedef boost::tuple<Box, Capsule, Sphere, Squirmer, UnionT> BodyTuple ;
+typedef boost::tuple<Box, Capsule, Sphere, Squirmer, UnionT, Ellipsoid> BodyTuple ;
void testBox()
{
@@ -128,6 +129,28 @@ void testSquirmer()
WALBERLA_CHECK_FLOAT_EQUAL(s1.getSquirmerBeta(), s2->getSquirmerBeta());
}
+void testEllipsoid()
+{
+ MaterialID iron = Material::find("iron");
+
+ Ellipsoid e1(759847, 1234795, Vec3(real_c(1), real_c(2), real_c(3)), Vec3(0,0,0), Quat(), Vec3(3,1,5), iron, false, false, false);
+ e1.setLinearVel(Vec3(real_c(5.2), real_c(6.3), real_c(7.4)));
+ e1.setAngularVel(Vec3(real_c(1.2), real_c(2.3), real_c(3.4)));
+
+ mpi::SendBuffer sb;
+ MarshalDynamically<BodyTuple>::execute(sb, e1);
+ mpi::RecvBuffer rb(sb);
+
+ EllipsoidID e2 = static_cast<EllipsoidID> (UnmarshalDynamically<BodyTuple>::execute(rb, Ellipsoid::getStaticTypeID(), math::AABB(Vec3(-100,-100,-100), Vec3(100,100,100)), math::AABB(Vec3(-100,-100,-100), Vec3(100,100,100))));
+
+ WALBERLA_CHECK_FLOAT_EQUAL(e1.getPosition(), e2->getPosition());
+ WALBERLA_CHECK_FLOAT_EQUAL(e1.getLinearVel(), e2->getLinearVel());
+ WALBERLA_CHECK_FLOAT_EQUAL(e1.getAngularVel(), e2->getAngularVel());
+ WALBERLA_CHECK_FLOAT_EQUAL(e1.getSemiAxes(), e2->getSemiAxes());
+ WALBERLA_CHECK_EQUAL(e1.getID(), e2->getID());
+ WALBERLA_CHECK_EQUAL(e1.getSystemID(), e2->getSystemID());
+}
+
void testUnion()
{
UnionT u1(159, 423, Vec3(real_c(1), real_c(2), real_c(3)), Vec3(0,0,0), Quat(), false, false, false);
@@ -179,6 +202,7 @@ int main( int argc, char** argv )
testCapsule();
testUnion();
testSquirmer();
+ testEllipsoid();
return EXIT_SUCCESS;
}