From b7d1b183b6a9b97afe8701c779b6592fdfd65e8b Mon Sep 17 00:00:00 2001
From: Sebastian Eibl <sebastian.eibl@fau.de>
Date: Fri, 10 Nov 2017 13:12:27 +0100
Subject: [PATCH] add gjk algorithm
---
src/pe/collision/EPA.h | 1416 +++++++++++++++++++++++++
src/pe/collision/GJK.h | 1214 +++++++++++++++++++++
src/pe/collision/GJKEPAHelper.cpp | 6 +-
src/pe/fcd/IterativeFCD.h | 195 ++++
src/pe/fcd/IterativeFCDDataHandling.h | 46 +
src/pe/rigidbody/Sphere.h | 19 +-
6 files changed, 2890 insertions(+), 6 deletions(-)
create mode 100644 src/pe/collision/EPA.h
create mode 100644 src/pe/collision/GJK.h
create mode 100644 src/pe/fcd/IterativeFCD.h
create mode 100644 src/pe/fcd/IterativeFCDDataHandling.h
diff --git a/src/pe/collision/EPA.h b/src/pe/collision/EPA.h
new file mode 100644
index 000000000..6abe12a8e
--- /dev/null
+++ b/src/pe/collision/EPA.h
@@ -0,0 +1,1416 @@
+//======================================================================================================================
+//
+// 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 */
+ //@{
+ inline bool doEPAcontactThreshold( GeomPrimitive &geom1, GeomPrimitive &geom2, const GJK& gjk, Vec3& normal,
+ Vec3& contactPoint, real_t& penetrationDepth);
+
+
+ inline bool doEPAcontactThreshold( GeomPrimitive &geom1, GeomPrimitive &geom2, const GJK& gjk, Vec3& normal,
+ Vec3& contactPoint, real_t& penetrationDepth, real_t eps_rel);
+
+
+ inline bool doEPAmargin( GeomPrimitive &geom1, GeomPrimitive &geom2, const GJK& gjk, Vec3& normal,
+ Vec3& contactPoint, real_t& penetrationDepth, real_t margin);
+
+ inline bool doEPA( GeomPrimitive &geom1, GeomPrimitive &geom2, const GJK& gjk, Vec3& normal,
+ Vec3& contactPoint, real_t& penetrationDepth, real_t margin, real_t eps_rel );
+
+ //@}
+ //**********************************************************************************************
+
+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 );
+ inline bool searchTetrahedron (GeomPrimitive &geom1, GeomPrimitive &geom2, std::vector<Vec3>& epaVolume,
+ std::vector<Vec3>& supportA, std::vector<Vec3>& supportB, EPA_EntryBuffer& entryBuffer, real_t margin );
+
+ inline void createInitialTetrahedron ( size_t top, size_t frontLeft, size_t frontRight,
+ size_t back, std::vector<Vec3>& epaVolume,
+ EPA_EntryBuffer& entryBuffer );
+
+ inline 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
+ static const size_t maxSupportPoints_ = 100;
+ static const size_t maxTriangles_ = 200;
+};
+//*************************************************************************************************
+
+
+
+
+//=================================================================================================
+//
+// EPA::EPA_EDGE CLASS DEFINITION
+//
+//=================================================================================================
+
+//*************************************************************************************************
+/*!\brief TODO
+ */
+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 TODO
+ *
+ * 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 );
+ //@}
+ //**********************************************************************************************
+ inline bool containsPoint(const std::vector<Vec3> &epaVolume);
+private:
+ //**Utility functions***************************************************************************
+ /*!\name Utility functions */
+ //@{
+ inline 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 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 )
+{
+ WALBERLA_ASSERT_UNEQUAL( a, b, "EPA_Triangle impoosible indices"); //TODO kein assert im constructor
+ WALBERLA_ASSERT_UNEQUAL( b, c, "EPA_Triangle impoosible indices"); //TODO kein assert im constructor
+ WALBERLA_ASSERT_UNEQUAL( c, a, "EPA_Triangle impoosible indices"); //TODO kein assert im constructor
+
+ 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 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_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;
+ /*
+ size_t v1 = indices_[edge0] ;
+ size_t v2 = tria->indices_[(edge1+1)%3];
+ size_t v3 = indices_[(edge0+1)%3];
+ size_t v4 = tria->indices_[edge1];
+
+ if( !(v1==v2 && v3==v4)) {
+ std::cout << "kaputt" << std::endl;
+ }
+ */
+
+ 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.
+ */
+inline 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::EPA_TRIANGLECOMP BINARY FUNCTION CALL OPERATOR
+//
+//=================================================================================================
+
+//*************************************************************************************************
+/*!\brief TODO
+ */
+inline bool EPA::EPA_TriangleComp::operator()( const EPA_Triangle *tria1,
+ const EPA_Triangle *tria2 )
+{
+ return tria1->getSqrDist() > tria2->getSqrDist();
+}
+//*************************************************************************************************
+
+
+/*inline void printHeap(std::vector<EPA::EPA_Triangle*> &entryHeap){
+ std::cerr << "Heap size: " << entryHeap.size();
+ for(int i = 0; i< entryHeap.size(); i++){
+ std::cerr << "[" << i << "]: " << entryHeap[i]->getSqrDist() <<std::endl;
+ }
+}*/
+
+inline bool EPA::EPA_Triangle::containsPoint(const std::vector<Vec3> &epaVolume){
+ Vec3 check_point(real_t(0.0265539),real_t(0.458987),real_t(0.888046));
+ for(int i = 0; i < 3; i++){
+ Vec3 A = epaVolume[indices_[i]];
+ Vec3 B = epaVolume[indices_[(i+1)%3]];
+ if(((A % B) * check_point )< real_t(0.0)){
+ return false;
+ }
+ }
+ return true;
+}
+
+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);
+
+//=================================================================================================
+//
+// EPA QUERY FUNCTIONS
+//
+//=================================================================================================
+
+
+
+//*************************************************************************************************
+/*! \brief Does an EPA computation with contactthreshold added. Use Default relative Error.
+ */
+inline 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.
+ */
+inline 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.
+ */
+inline 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.
+ */
+inline 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 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();
+}
+//*************************************************************************************************
+
+//*************************************************************************************************
+/*! \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 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
+ /*supportA.push_back(geom1.supportContactThreshold(direction1));
+ supportB.push_back(geom2.supportContactThreshold(-direction1));
+ Vec3 support1 = supportA.back() - supportB.back();
+ epaVolume.push_back(support1);*/ //epaVolume[2]
+
+ pushSupportMargin(geom1, geom2, direction1, margin, epaVolume, supportA, supportB);
+ //Vec3 support1 = epaVolume.back();
+
+ //std::cerr << "S1: " << support1 << std::endl;
+ //add point in negative normal direction2
+ /*supportA.push_back(geom1.supportContactThreshold(direction2));
+ supportB.push_back(geom2.supportContactThreshold(-direction2));
+ Vec3 support2 = supportA.back() - supportB.back();
+ epaVolume.push_back(support2); *///epaVolume[3]
+ pushSupportMargin(geom1, geom2, direction2, margin, epaVolume, supportA, supportB);
+ //Vec3 support2 = epaVolume.back();
+ //std::cerr << "S2: " << support2 << std::endl;
+
+ /* //add point in negative normal direction3
+ supportA.push_back(geom1.supportContactThreshold(direction3));
+ supportB.push_back(geom2.supportContactThreshold(-direction3));
+ Vec3 support3 = supportA.back() - supportB.back();
+ epaVolume.push_back(support3); //epaVolume[4]*/
+ pushSupportMargin(geom1, geom2, direction3, margin, epaVolume, supportA, supportB);
+ /*std::cerr << "S3: " << support3 << std::endl;
+ // TODO check what happens if two of the new support points are identical*/
+
+ //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);
+ //Vec3 support1 = epaVolume.back();
+ /*supportA.push_back(geom1.supportContactThreshold(ABC));
+ supportB.push_back(geom2.supportContactThreshold(-ABC));
+ Vec3 support1 = supportA.back() - supportB.back();
+ epaVolume.push_back(support1); //epaVolume[3]*/
+
+ //add point in negative normal direction
+ /*supportA.push_back(geom1.supportContactThreshold(-ABC));
+ supportB.push_back(geom2.supportContactThreshold(ABC));
+ Vec3 support2 = supportA.back() - supportB.back();
+ epaVolume.push_back(support2); //epaVolume[4]*/
+ 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 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();
+}
+
+//@}
+
+} // namespace fcd
+} // namespace pe
+} // namespace walberla
diff --git a/src/pe/collision/GJK.h b/src/pe/collision/GJK.h
new file mode 100644
index 000000000..49bde5a5f
--- /dev/null
+++ b/src/pe/collision/GJK.h
@@ -0,0 +1,1214 @@
+//======================================================================================================================
+//
+// 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/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 {
+
+//=================================================================================================
+//
+// CLASS DEFINITION
+//
+//=================================================================================================
+
+//*************************************************************************************************
+/*!\brief Impelementation of the Gilbert-Johnson-Keerthi Algorithm.
+ */
+class GJK
+{
+public:
+
+ //**Constructor*********************************************************************************
+ /*! \name Constructor */
+ //@{
+ explicit inline GJK();
+ //@}
+ //**********************************************************************************************
+
+ //**Query functions*****************************************************************************
+ /*! \name Query functions */
+ //@{
+ inline real_t doGJK( GeomPrimitive &geom1, GeomPrimitive &geom2, Vec3& normal, Vec3& contactPoint );
+
+ inline bool doGJKcontactThreshold( 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;
+ //@}
+ //**********************************************************************************************
+ std::vector<Vec3> simplex_; //<! Container to hold the simplex. (Public for debug reasons)
+private:
+ //**Utility functions***************************************************************************
+ /*! \name Utility functions */
+ //@{
+ inline bool simplex2(Vec3& d); //TODO rename 0-Simplex is a line not 1-Simplex
+ inline bool simplex3(Vec3& d);
+ inline bool simplex4(Vec3& d);
+
+ inline bool sameDirection ( const Vec3& vec1, const Vec3& vec2 ) const;
+ inline bool zeroLengthVector( const Vec3& vec ) const;
+ inline 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> 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
+}
+//*************************************************************************************************
+
+
+
+
+//=================================================================================================
+//
+// 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.
+ */
+inline 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 margin The margin by which the objects will be enlarged.
+ */
+inline bool GJK::doGJKcontactThreshold(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
+}
+//*************************************************************************************************
+
+
+
+//=================================================================================================
+//
+// 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 Process a simplex with two nodes.
+ */
+inline 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.
+ */
+inline 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.
+ */
+inline 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;
+}
+//*************************************************************************************************
+
+
+//*************************************************************************************************
+/*!\brief TODO
+ *
+ * Checks if tow vectors roughly point in the same direction
+ */
+inline bool GJK::sameDirection(const Vec3& vec1, const Vec3& vec2) const
+{
+ return vec1 * vec2 > real_t(0.0);
+}
+//*************************************************************************************************
+
+
+//*************************************************************************************************
+/*!\brief TODO
+ *
+ * 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;
+}
+//*************************************************************************************************
+
+
+//*************************************************************************************************
+/*!\brief TODO
+ */
+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;
+}
+//*************************************************************************************************
+
+
+} // namespace fcd
+
+} // namespace pe
+
+} // namespace walberla
diff --git a/src/pe/collision/GJKEPAHelper.cpp b/src/pe/collision/GJKEPAHelper.cpp
index d119ed751..d8316cd6d 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/fcd/IterativeFCD.h b/src/pe/fcd/IterativeFCD.h
new file mode 100644
index 000000000..995faf7ea
--- /dev/null
+++ b/src/pe/fcd/IterativeFCD.h
@@ -0,0 +1,195 @@
+//======================================================================================================================
+//
+// 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 IterativeFCD.h
+//! \author Tobias Leemann <tobias.leemann@fau.de>
+//
+//======================================================================================================================
+
+
+#pragma once
+
+#include "IFCD.h"
+#include "pe/Types.h"
+#include "pe/collision/EPA.h"
+#include "pe/collision/GJK.h"
+#include "pe/rigidbody/Plane.h"
+#include "pe/rigidbody/Plane.h"
+#include <pe/Thresholds.h>
+#include <boost/tuple/tuple.hpp>
+
+namespace walberla{
+namespace pe{
+namespace fcd {
+
+/**
+ * \ingroup pe
+ * \brief Implementation of the IFCD Interface with an iterative detection.
+ *
+ * This Class implements a collision detection compatible with the
+ * IFCD interface. It detects collisions using an iterative technique which employs
+ * the GJK and EPA algorithms.
+ */
+template <typename BodyTypeTuple>
+class IterativeFCD : public IFCD {
+ typedef Union< boost::tuple<BodyTypeTuple>> UnionGenericType;
+
+private:
+ /*!\brief This computes a collision between two rigid bodies.
+ *
+ * \param a The first body (must not be a Plane or Union).
+ * \param b The second body (must not be a Plane or Union).
+ * \param normal Returns the normal vector of the collision.
+ * \param contactPoint Returns a point of contact btw. the bodies.
+ * \param penetrationDepth Returns the depth.
+ * \return True, if the bodies collide.
+ *
+ * The Return parameters remain untouched if there is no collision.
+ * This function performes collision detection with bodies enlarged by a margin for numerical reasons.
+ * See "Collision detection in interactive 3D environments" by Gino van den Bergen
+ * for a detailed explanation of the algorithm used.
+ */
+ bool performIterativeDetection(GeomPrimitive &a, GeomPrimitive &b, Vec3& normal, Vec3& contactPoint, real_t& penetrationDepth){
+ real_t margin = real_t(1e-4);
+ GJK gjk;
+ if(gjk.doGJKcontactThreshold(a, b, margin)){
+ //2. If collision is possible perform EPA.
+ EPA epa;
+ return epa.doEPAmargin(a, b, gjk, normal, contactPoint, penetrationDepth, margin);
+ }else{
+ return false;
+ }
+ }
+
+ /*!\brief This computes a collision between a Plane and another body.
+ *
+ * \param pl The Plane.
+ * \param b The second body (must not be a Plane or Union).
+ * \param normal Returns the normal vector of the collision.
+ * \param contactPoint Returns a point of contact btw. the bodies.
+ * \param penetrationDepth Returns the depth.
+ * \return True, if the Plane and the body collide.
+ *
+ * The Return parameters remain untouched if there is no collision.
+ */
+ bool performPlaneDetection(Plane &pl, GeomPrimitive &b, Vec3& normal, Vec3& contactPoint, real_t& penetrationDepth){
+ Vec3 support_dir = -pl.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);
+ //std::cerr << contactp << std::endl;
+ //std::cerr << pl.getDisplacement() <<std::endl;
+ real_t pdepth = contactp * pl.getNormal() - pl.getDisplacement();
+ if(pdepth < contactThreshold){ //We have a collision
+ normal = support_dir;
+ penetrationDepth = pdepth;
+ contactPoint = contactp + real_t(0.5) * penetrationDepth * normal;
+ return true;
+ }else{ //No collision
+ return false;
+ }
+ }
+
+ /*!\brief This function adds contacts for single bodies (not unions)
+ *
+ * \param bd1 The first body.
+ * \param bd2 The second body.
+ *
+ * This function is called by generateContacts() on all normal
+ * body pairs and on each single bodys of the union.
+ * It checks if one of the bodies is a plane and calls the corresponding
+ * detection function. If collision is detected a Contact is generated and
+ * added to the vector.
+ */
+ void generateContactsSingle(BodyID bd1, BodyID bd2){
+ //Temporary Storage for data
+ Vec3 normal;
+ Vec3 contactPoint;
+ real_t penetrationDepth;
+
+ //Check if one primitive is a plane
+ if(bd1->getTypeID() == Plane::getStaticTypeID()){
+ if(!(bd2->getTypeID() == Plane::getStaticTypeID())){
+ //First object is a plane, second is not
+ Plane* pl = static_cast<Plane*>(bd1);
+ GeomPrimitive *gb = static_cast<GeomPrimitive*>(bd2);
+ if (performPlaneDetection(*pl, *gb, normal, contactPoint, penetrationDepth)){
+ contacts_.push_back( Contact(pl, gb, contactPoint, normal, penetrationDepth) );
+ }
+ }else{
+ return; //Plane plane collisions cannot be handled here.
+ }
+ }else if(bd2->getTypeID() == Plane::getStaticTypeID()){
+ if(!(bd1->getTypeID() == Plane::getStaticTypeID())){
+ //Second object is a plane, first is not
+ Plane* pl = static_cast<Plane*>(bd2);
+ GeomPrimitive *ga = static_cast<GeomPrimitive*>(bd1);
+ if (performPlaneDetection(*pl, *ga, normal, contactPoint, penetrationDepth)){
+ contacts_.push_back( Contact(ga, pl, contactPoint, -normal, penetrationDepth) );
+ }
+ }
+ }else{ // Both objects are geometries.
+ GeomPrimitive *ga = static_cast<GeomPrimitive*>(bd1);
+ GeomPrimitive *gb = static_cast<GeomPrimitive*>(bd2);
+
+ if (performIterativeDetection(*ga, *gb, normal, contactPoint, penetrationDepth)){
+ contacts_.push_back( Contact(ga, gb, contactPoint, normal, penetrationDepth) );
+ }
+ }
+ }
+
+public:
+ /*!\brief Checks each body pair in the given vector and generates
+ * a vector of contacts.
+ *
+ * \param possibleContacts Vector of body pairs thay may collide.
+ * \return Vector of contacts with information of all contacts generated.
+ */
+ virtual Contacts& generateContacts(PossibleContacts& possibleContacts)
+ {
+ contacts_.clear();
+ for (auto it = possibleContacts.begin(); it != possibleContacts.end(); ++it)
+ {
+ bool hasSubbodies1 = it->first->hasSubBodies();
+ UnionGenericType* u1 = nullptr;
+ if( hasSubbodies1 ){
+ u1= static_cast<UnionGenericType*>(it->first);
+ }
+ //Loop over possible subbodies of first rigid body
+ for(size_t bodynum1 = 0; bodynum1 < (hasSubbodies1 ? u1->size() : 1); bodynum1++){
+ BodyID bd1 = (hasSubbodies1 ? u1->getBody(bodynum1) : it->first);
+ bool hasSubbodies2 = it->second->hasSubBodies();
+ UnionGenericType* u2 = nullptr;
+ if( hasSubbodies2 ){
+ u2= static_cast<UnionGenericType*>(it->second);
+ }
+ //Loop over possible subbodies of second rigid body
+ for(size_t bodynum2 = 0; bodynum2 < (hasSubbodies2 ? u2->size() : 1); bodynum2++){
+ BodyID bd2 = (hasSubbodies2 ? u2->getBody(bodynum2) : it->second);
+ generateContactsSingle(bd1, bd2);
+ }
+ }
+ }
+ return contacts_;
+ }
+
+};
+
+
+}
+}
+}
diff --git a/src/pe/fcd/IterativeFCDDataHandling.h b/src/pe/fcd/IterativeFCDDataHandling.h
new file mode 100644
index 000000000..c4bb3e251
--- /dev/null
+++ b/src/pe/fcd/IterativeFCDDataHandling.h
@@ -0,0 +1,46 @@
+//======================================================================================================================
+//
+// 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 IterativeFCDDataHandling.h
+//! \author Sebastian Eibl <sebastian.eibl@fau.de>
+//! \author Tobias Leemann <tobias.leemann@fau.de>
+//
+//======================================================================================================================
+
+#pragma once
+
+#include "IterativeFCD.h"
+
+#include "blockforest/BlockDataHandling.h"
+
+namespace walberla{
+namespace pe{
+namespace fcd {
+
+template <typename BodyTypeTuple>
+class IterativeFCDDataHandling : public blockforest::AlwaysInitializeBlockDataHandling<IterativeFCD<BodyTypeTuple>>{
+public:
+ IterativeFCD<BodyTypeTuple> * initialize( IBlock * const /*block*/ ) {return new IterativeFCD<BodyTypeTuple>();}
+};
+
+template <typename BodyTypeTuple>
+shared_ptr<IterativeFCDDataHandling<BodyTypeTuple>> createIterativeFCDDataHandling()
+{
+ return make_shared<IterativeFCDDataHandling<BodyTypeTuple>>( );
+}
+
+}
+}
+}
diff --git a/src/pe/rigidbody/Sphere.h b/src/pe/rigidbody/Sphere.h
index bc0e82566..536f7b7ce 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);
+ }
}
//*************************************************************************************************
--
GitLab