Merge branch 'improvedgjkepa' into 'master'

Improved iterative collision detection with GJK-EPA

See merge request walberla/walberla!62

src/pe/Types.h

@@ -65,6 +65,7 @@ class Box;
 class Capsule;
 class Contact;
 class Cylinder;
+class Ellipsoid;
 class CylindricalBoundary;
 class FixedJoint;
 class ForceGenerator;
@@ -111,6 +112,10 @@ typedef CylindricalBoundary        CylindricalBoundaryType;        //!< Type of
 typedef CylindricalBoundary*       CylindricalBoundaryID;          //!< Handle for a cylindrical boundary primitive.
 typedef const CylindricalBoundary* ConstCylindricalBoundaryID;     //!< Handle for a constant cylindrical boundary primitive.
 
+typedef Ellipsoid             EllipsoidType;       //!< Type of the ellipsoid geometric primitive.
+typedef Ellipsoid*            EllipsoidID;         //!< Handle for a ellipsoid primitive.
+typedef const Ellipsoid*      ConstEllipsoidID;    //!< Handle for a constant ellipsoid primitive.
+
 typedef Plane                 PlaneType;           //!< Type of the plane geometric primitive.
 typedef Plane*                PlaneID;             //!< Handle for a plane primitive.
 typedef const Plane*          ConstPlaneID;        //!< Handle for a constant plane primitive.

src/pe/basic.h

@@ -44,6 +44,7 @@
 #include "pe/rigidbody/PlaneFactory.h"
 #include "pe/rigidbody/SphereFactory.h"
 #include "pe/rigidbody/UnionFactory.h"
+#include "pe/rigidbody/EllipsoidFactory.h"
 
 #include "pe/synchronization/SyncNextNeighbors.h"
 #include "pe/synchronization/SyncShadowOwners.h"

src/pe/collision/GJK.h

+//======================================================================================================================
+//
+//  This file is part of waLBerla. waLBerla is free software: you can
+//  redistribute it and/or modify it under the terms of the GNU General Public
+//  License as published by the Free Software Foundation, either version 3 of
+//  the License, or (at your option) any later version.
+//
+//  waLBerla is distributed in the hope that it will be useful, but WITHOUT
+//  ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+//  FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License
+//  for more details.
+//
+//  You should have received a copy of the GNU General Public License along
+//  with waLBerla (see COPYING.txt). If not, see <http://www.gnu.org/licenses/>.
+//
+//! \file GJK.h
+//! \author Tobias Scharpff
+//! \author Tobias Leemann
+//
+//======================================================================================================================
+
+#pragma once
+
+//*************************************************************************************************
+// Includes
+//*************************************************************************************************
+
+#include <vector>
+
+#include <pe/rigidbody/GeomPrimitive.h>
+#include <pe/Thresholds.h>
+
+#include <core/Abort.h>
+#include <core/math/Limits.h>
+#include <core/math/Vector3.h>
+
+namespace walberla {
+namespace pe {
+namespace fcd {
+
+//=================================================================================================
+//
+//  CLASS DEFINITION
+//
+//=================================================================================================
+
+//*************************************************************************************************
+/*!\brief Impelementation of the Gilbert-Johnson-Keerthi Algorithm.
+ */
+class GJK
+{
+public:
+
+   //**Constructor*********************************************************************************
+   /*! \name Constructor */
+   //@{
+   explicit inline GJK();
+   //@}
+   //**********************************************************************************************
+
+   //**Query functions*****************************************************************************
+   /*! \name Query functions */
+   //@{
+   real_t doGJK( GeomPrimitive &geom1, GeomPrimitive &geom2, Vec3& normal, Vec3& contactPoint );
+
+   bool doGJKmargin( GeomPrimitive &geom1, GeomPrimitive &geom2, const real_t margin = contactThreshold);
+   //@}
+   //**********************************************************************************************
+
+   //**Get functions*******************************************************************************
+   /*! \name Get functions */
+   //@{
+   inline const std::vector<Vec3>& getSimplex()     const;
+   inline size_t                   getSimplexSize() const;
+   inline const std::vector<Vec3>& getSupportA()    const;
+   inline const std::vector<Vec3>& getSupportB()    const;
+   //@}
+   //**********************************************************************************************
+
+private:
+   //**Utility functions***************************************************************************
+   /*! \name Utility functions */
+   //@{
+   bool simplex2(Vec3& d);
+   bool simplex3(Vec3& d);
+   bool simplex4(Vec3& d);
+
+   inline bool sameDirection   ( const Vec3& vec1, const Vec3& vec2 ) const;
+   inline bool zeroLengthVector( const Vec3& vec )                     const;
+   real_t calcDistance    ( Vec3& normal, Vec3& contactPoint );
+   inline const Vec3 putSupport(const GeomPrimitive &geom1, const GeomPrimitive &geom2, const Vec3& dir, const real_t margin,
+                                std::vector<Vec3> &simplex, std::vector<Vec3> &supportA, std::vector<Vec3> &supportB, size_t index);
+   //@}
+   //**********************************************************************************************
+
+   //**Member variables****************************************************************************
+   /*! \name Member variables */
+   //@{
+   std::vector<Vec3> simplex_;   //<! Container to hold the simplex.
+   std::vector<Vec3> supportA_;  //<! Container to hold the support points generated in triangle mesh mA
+   std::vector<Vec3> supportB_;  //<! Container to hold the support points generated in triangle mesh mB
+   unsigned char     numPoints_; //<! Current number of points in the simplex.
+   Vec3              d_;         //<! The next search direction.
+   //@}
+   //**********************************************************************************************
+};
+//*************************************************************************************************
+
+//=================================================================================================
+//
+//  CONSTRUCTOR
+//
+//=================================================================================================
+
+//*************************************************************************************************
+inline GJK::GJK() : simplex_(4), supportA_(4), supportB_(4), numPoints_(0)
+{
+   d_ = Vec3(real_t(0.0),real_t(0.6),real_t(0.8)); // just start with any vector of length 1
+}
+//*************************************************************************************************
+
+
+//=================================================================================================
+//
+//  GET FUNCTIONS
+//
+//=================================================================================================
+
+//*************************************************************************************************
+inline const std::vector<Vec3>& GJK::getSimplex() const
+{
+   return simplex_;
+}
+//*************************************************************************************************
+
+
+//*************************************************************************************************
+inline size_t GJK::getSimplexSize() const
+{
+   return numPoints_;
+}
+//*************************************************************************************************
+
+
+//*************************************************************************************************
+inline const std::vector<Vec3>& GJK::getSupportA() const
+{
+   return supportA_;
+}
+//*************************************************************************************************
+
+
+//*************************************************************************************************
+inline const std::vector<Vec3>& GJK::getSupportB() const
+{
+   return supportB_;
+}
+//*************************************************************************************************
+
+
+//=================================================================================================
+//
+//  UTILITY FUNCTIONS
+//
+//=================================================================================================
+
+//*************************************************************************************************
+/*!\brief Checks if two vectors roughly point in the same directionTODO
+ */
+inline bool GJK::sameDirection(const Vec3& vec1, const Vec3& vec2) const
+{
+   return vec1 * vec2 > real_t(0.0);
+}
+//*************************************************************************************************
+
+
+//*************************************************************************************************
+/* Checks if the length of a vector is zero or as close to zero that it can not be distinguished form zero
+ */
+inline bool GJK::zeroLengthVector(const Vec3& vec) const
+{
+   return vec.sqrLength() < math::Limits<real_t>::fpuAccuracy();
+}
+//*************************************************************************************************
+
+
+//*************************************************************************************************
+/*!\brief Calucate a support point of a body extended by threshold.
+ * \param geom The body.
+ * \param dir The support point direction.
+ * \param threshold Extension of the Body.
+ */
+inline const Vec3 GJK::putSupport(const GeomPrimitive &geom1, const GeomPrimitive &geom2, const Vec3& dir, const real_t margin, 
+                                  std::vector<Vec3> &simplex, std::vector<Vec3> &supportA, std::vector<Vec3> &supportB, size_t index){
+   supportA[index] = geom1.support(dir);
+   supportB[index] = geom2.support(-dir);
+   Vec3 supp = supportA[index]- supportB[index] + (real_t(2.0) * dir * margin);
+   simplex[index] = supp;
+   return supp;
+}
+//*************************************************************************************************
+
+
+} // namespace fcd
+
+} // namespace pe
+
+} // namespace walberla

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;

src/pe/communication/DynamicMarshalling.h

@@ -33,6 +33,7 @@
 #include "pe/communication/rigidbody/Capsule.h"
 #include "pe/communication/rigidbody/Sphere.h"
 #include "pe/communication/rigidbody/Union.h"
+#include "pe/communication/rigidbody/Ellipsoid.h"
 #include "pe/utility/BodyCast.h"
 
 #include "core/Abort.h"

src/pe/communication/rigidbody/Ellipsoid.cpp

+//======================================================================================================================
+//
+//  This file is part of waLBerla. waLBerla is free software: you can
+//  redistribute it and/or modify it under the terms of the GNU General Public
+//  License as published by the Free Software Foundation, either version 3 of
+//  the License, or (at your option) any later version.
+//
+//  waLBerla is distributed in the hope that it will be useful, but WITHOUT
+//  ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+//  FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License
+//  for more details.
+//
+//  You should have received a copy of the GNU General Public License along
+//  with waLBerla (see COPYING.txt). If not, see <http://www.gnu.org/licenses/>.
+//
+//! \file Ellipsoid.cpp
+//! \author Tobias Preclik
+//! \author Sebastian Eibl <sebastian.eibl@fau.de>
+//! \brief Marshalling of objects for data transmission or storage.
+//
+//======================================================================================================================
+
+#include "Ellipsoid.h"
+
+namespace walberla {
+namespace pe {
+namespace communication {
+
+//*************************************************************************************************
+/*!\brief Marshalling a Ellipsoid primitive.
+ *
+ * \param buffer The buffer to be filled.
+ * \param obj The object to be marshalled.
+ * \return void
+ */
+void marshal( mpi::SendBuffer& buffer, const Ellipsoid& obj ) {
+   marshal( buffer, static_cast<const GeomPrimitive&>( obj ) );
+   buffer << obj.getSemiAxes();
+}
+//*************************************************************************************************
+
+
+//*************************************************************************************************
+/*!\brief Unmarshalling a Ellipsoid primitive.
+ *
+ * \param buffer The buffer from where to read.
+ * \param objparam The object to be reconstructed.
+ * \param hasSuperBody False if body is not part of a union. Passed on to rigid body unmarshalling.
+ * \return void
+ */
+void unmarshal( mpi::RecvBuffer& buffer, EllipsoidParameters& objparam ) {
+   unmarshal( buffer, static_cast<GeomPrimitiveParameters&>( objparam ) );
+   buffer >> objparam.semiAxes_;
+}
+//*************************************************************************************************
+
+}  // namespace communication
+}  // namespace pe
+}  // namespace walberla

src/pe/communication/rigidbody/Ellipsoid.h

+//======================================================================================================================
+//
+//  This file is part of waLBerla. waLBerla is free software: you can
+//  redistribute it and/or modify it under the terms of the GNU General Public
+//  License as published by the Free Software Foundation, either version 3 of
+//  the License, or (at your option) any later version.
+//
+//  waLBerla is distributed in the hope that it will be useful, but WITHOUT
+//  ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+//  FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License
+//  for more details.
+//
+//  You should have received a copy of the GNU General Public License along
+//  with waLBerla (see COPYING.txt). If not, see <http://www.gnu.org/licenses/>.
+//
+//! \file Ellipsoid.h
+//! \author Tobias Preclik
+//! \author Sebastian Eibl <sebastian.eibl@fau.de>
+//! \brief Marshalling of objects for data transmission or storage.
+//
+//======================================================================================================================
+
+#pragma once
+
+
+//*************************************************************************************************
+// Includes
+//*************************************************************************************************
+
+#include "pe/communication/Instantiate.h"
+#include "pe/communication/Marshalling.h"
+#include "pe/rigidbody/Ellipsoid.h"
+
+namespace walberla {
+namespace pe {
+namespace communication {
+
+struct EllipsoidParameters : public GeomPrimitiveParameters {
+   Vec3 semiAxes_;
+};
+
+//*************************************************************************************************
+/*!\brief Marshalling a Ellipsoid primitive.
+ *
+ * \param buffer The buffer to be filled.
+ * \param obj The object to be marshalled.
+ * \return void
+ */
+void marshal( mpi::SendBuffer& buffer, const Ellipsoid& obj );
+//*************************************************************************************************
+
+//*************************************************************************************************
+/*!\brief Unmarshalling a Ellipsoid primitive.
+ *
+ * \param buffer The buffer from where to read.
+ * \param objparam The object to be reconstructed.
+ * \param hasSuperBody False if body is not part of a union. Passed on to rigid body unmarshalling.
+ * \return void
+ */
+void unmarshal( mpi::RecvBuffer& buffer, EllipsoidParameters& objparam );
+//*************************************************************************************************
+
+
+inline EllipsoidID instantiate( mpi::RecvBuffer& buffer, const math::AABB& domain, const math::AABB& block, EllipsoidID& newBody )
+{
+   EllipsoidParameters subobjparam;
+   unmarshal( buffer, subobjparam );
+   correctBodyPosition(domain, block.center(), subobjparam.gpos_);
+   newBody = new Ellipsoid( subobjparam.sid_, subobjparam.uid_, subobjparam.gpos_, subobjparam.rpos_, subobjparam.q_, subobjparam.semiAxes_, subobjparam.material_, false, subobjparam.communicating_, subobjparam.infiniteMass_ );
+   newBody->setLinearVel( subobjparam.v_ );
+   newBody->setAngularVel( subobjparam.w_ );
+   newBody->MPITrait.setOwner( subobjparam.mpiTrait_.owner_ );
+   return newBody;
+}
+
+}  // namespace communication
+}  // namespace pe
+}  // namespace walberla

src/pe/fcd/GJKEPACollideFunctor.h

+//======================================================================================================================
+//
+//  This file is part of waLBerla. waLBerla is free software: you can
+//  redistribute it and/or modify it under the terms of the GNU General Public
+//  License as published by the Free Software Foundation, either version 3 of
+//  the License, or (at your option) any later version.
+//
+//  waLBerla is distributed in the hope that it will be useful, but WITHOUT
+//  ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+//  FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License
+//  for more details.
+//
+//  You should have received a copy of the GNU General Public License along
+//  with waLBerla (see COPYING.txt). If not, see <http://www.gnu.org/licenses/>.
+//
+//! \file GJKEPACollideFunctor.h
+//! \author Tobias Leemann
+//! \author Sebastian Eibl <sebastian.eibl@fau.de>
+//
+//======================================================================================================================
+
+#pragma once
+
+
+#include "pe/Types.h"
+#include "pe/collision/EPA.h"
+#include "pe/collision/GJK.h"
+#include "pe/rigidbody/Plane.h"
+#include "pe/rigidbody/Union.h"
+#include <pe/Thresholds.h>
+#include <boost/tuple/tuple.hpp>
+
+namespace walberla{
+namespace pe{
+namespace fcd {
+namespace gjkepa{
+
+   //function for all single rigid bodies.
+   template<typename Container>
+   inline bool generateContacts(GeomPrimitive *a, GeomPrimitive *b, Container& contacts_);
+
+   //Planes
+   template<typename Container>
+   inline bool generateContacts(Plane *a, GeomPrimitive *b, Container& contacts_);
+
+   template<typename Container>
+   inline bool generateContacts(GeomPrimitive *a, Plane *b, Container& contacts_);
+
+   template< typename Container>
+   inline bool generateContacts(Plane *a, Plane *b, Container& contacts_);
+
+   //Unions
+   template<typename BodyTupleA, typename BodyB, typename Container>
+   inline bool generateContacts(Union<BodyTupleA> *a, BodyB *b, Container& contacts_);
+
+   template<typename BodyA, typename BodyTupleB, typename Container>
+   inline bool generateContacts(BodyA *a, Union<BodyTupleB> *b, Container& contacts_);
+
+   template<typename BodyTupleA, typename BodyTupleB, typename Container>
+   inline bool generateContacts(Union<BodyTupleA> *a, Union<BodyTupleB>  *b, Container& contacts_);
+
+   //Union and Plane
+   template<typename BodyTupleA, typename Container>
+   inline bool generateContacts(Union<BodyTupleA> *a, Plane *b, Container& contacts_);
+
+   template<typename BodyTupleB, typename Container>
+   inline bool generateContacts(Plane *a, Union<BodyTupleB> *b, Container& contacts_);
+}
+
+/* Iterative Collide Functor for contact Generation with iterative collision detection (GJK and EPA algorithms).
+ * Usage: fcd::GenericFCD<BodyTuple, fcd::GJKEPACollideFunctor> testFCD;
+ * testFCD.generateContacts(...);
+ */
+template <typename Container>
+struct GJKEPACollideFunctor
+{
+   Container& contacts_;
+
+   GJKEPACollideFunctor(Container& contacts) : contacts_(contacts) {}
+
+   template< typename BodyType1, typename BodyType2 >
+   bool operator()( BodyType1* bd1, BodyType2* bd2) {
+      using namespace gjkepa;
+      return generateContacts(bd1, bd2, contacts_);
+   }
+};
+
+template <typename BodyType1, typename Container>
+struct GJKEPASingleCollideFunctor
+{
+   BodyType1* bd1_;
+   Container& contacts_;
+
+   GJKEPASingleCollideFunctor(BodyType1* bd1, Container& contacts) : bd1_(bd1), contacts_(contacts) {}
+
+   template< typename BodyType2 >
+   bool operator()( BodyType2* bd2) {
+      using namespace gjkepa;
+      return generateContacts( bd1_, bd2, contacts_);
+   }
+};
+
+
+namespace gjkepa{
+
+   //function for all single rigid bodies.
+   template<typename Container>
+   inline bool generateContacts(GeomPrimitive *a, GeomPrimitive *b, Container& contacts_){
+      Vec3 normal;
+      Vec3 contactPoint;
+      real_t penetrationDepth;
+
+      real_t margin = real_t(1e-4);
+      GJK gjk;
+      if(gjk.doGJKmargin(*a, *b, margin)){
+         //2. If collision is possible perform EPA.
+         EPA epa;
+         if(epa.doEPAmargin(*a, *b, gjk, normal, contactPoint, penetrationDepth, margin)){
+            contacts_.push_back( Contact(a, b, contactPoint, normal, penetrationDepth) );
+            return true;
+         }else{
+            return false;
+         }
+
+      }else{
+         return false;
+      }
+   }
+
+   //Planes
+   template<typename Container>
+   inline bool generateContacts(Plane *a, GeomPrimitive *b, Container& contacts_){
+      Vec3 normal;
+      Vec3 contactPoint;
+      real_t penetrationDepth;
+
+      Vec3 support_dir = -a->getNormal();
+      // We now have a direction facing to the "wall".
+      // Compute support point of body b in this direction. This will be the furthest point overlapping.
+      Vec3 contactp = b->support(support_dir);
+      real_t pdepth = contactp * a->getNormal() - a->getDisplacement();
+      if(pdepth < contactThreshold){ //We have a collision
+         normal = support_dir;
+         penetrationDepth = pdepth;
+         contactPoint = contactp + real_t(0.5) * penetrationDepth * normal;
+         contacts_.push_back( Contact(a, b, contactPoint, normal, penetrationDepth) );
+         return true;
+      }else{ //No collision
+         return false;
+      }
+   }
+
+   template<typename Container>
+   inline bool generateContacts(GeomPrimitive *a, Plane *b, Container& contacts_){
+      return generateContacts(b, a, contacts_);
+   }
+
+   //Planes cannot collide with each other
+   template< typename Container>
+   inline bool generateContacts(Plane*, Plane*, Container&){
+      return false;
+   }
+
+   //Unions
+   template<typename BodyTupleA, typename BodyB, typename Container>
+   inline bool generateContacts(Union<BodyTupleA> *a, BodyB *b, Container& contacts_){
+      GJKEPASingleCollideFunctor<BodyB, Container> func(b, contacts_);
+      bool collision = false;
+      for( auto it=a->begin(); it!=a->end(); ++it )
+      {
+         collision |= SingleCast<BodyTupleA, GJKEPASingleCollideFunctor<BodyB, Container>, bool>::execute(*it, func);
+      }
+      return collision;
+   }
+
+   template<typename BodyA, typename BodyTupleB, typename Container>
+   inline bool generateContacts(BodyA *a, Union<BodyTupleB> *b, Container& contacts_){
+      return generateContacts(b, a, contacts_);
+   }
+
+   template<typename BodyTupleA, typename BodyTupleB, typename Container>
+   inline bool generateContacts(Union<BodyTupleA> *a, Union<BodyTupleB>  *b, Container& contacts_){
+      GJKEPACollideFunctor<Container> func(contacts_);
+      bool collision = false;
+      for( auto it1=a->begin(); it1!=a->end(); ++it1 )
+      {
+         for( auto it2=b->begin(); it2!=b->end(); ++it2 )
+         {
+            collision |= DoubleCast<BodyTupleA, BodyTupleB, GJKEPACollideFunctor<Container>, bool>::execute(*it1, *it2, func);
+         }
+      }
+      return collision;
+   }
+
+   //Union and Plane (these calls are ambigous if not implemented seperatly)
+   template<typename BodyTupleA, typename Container>
+   inline bool generateContacts(Union<BodyTupleA> *a, Plane *b, Container& contacts_){
+      GJKEPASingleCollideFunctor<Plane, Container> func(b, contacts_);
+      bool collision = false;
+      for( auto it=a->begin(); it!=a->end(); ++it )
+      {
+         collision |= SingleCast<BodyTupleA, GJKEPASingleCollideFunctor<Plane, Container>, bool>::execute(*it, func);
+      }
+      return collision;
+   }
+
+   template<typename BodyTupleB, typename Container>
+   inline bool generateContacts(Plane *a, Union<BodyTupleB> *b, Container& contacts_){
+      return generateContacts(b, a, contacts_);
+   }
+
+
+} //namespace gjkepa
+
+
+} //fcd
+} //pe
+} //walberla

src/pe/rigidbody/Box.h

@@ -83,6 +83,7 @@ public:
    /*!\name Get functions */
    //@{
    inline const Vec3& getLengths() const;
+   virtual inline real_t getVolume() const;
    //@}
    //**********************************************************************************************
 
@@ -306,7 +307,16 @@ inline const Vec3& Box::getLengths() const
 }
 //*************************************************************************************************
 
-
+//*************************************************************************************************
+/*!\brief Returns the volume of the box.
+ *
+ * \return The volume of the box.
+ */
+inline real_t Box::getVolume() const
+{
+   return Box::calcVolume(getLengths());
+}
+//*************************************************************************************************
 
 
 //=================================================================================================

src/pe/rigidbody/Ellipsoid.cpp

+//======================================================================================================================
+//
+//  This file is part of waLBerla. waLBerla is free software: you can
+//  redistribute it and/or modify it under the terms of the GNU General Public
+//  License as published by the Free Software Foundation, either version 3 of
+//  the License, or (at your option) any later version.
+//
+//  waLBerla is distributed in the hope that it will be useful, but WITHOUT
+//  ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+//  FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License
+//  for more details.
+//
+//  You should have received a copy of the GNU General Public License along
+//  with waLBerla (see COPYING.txt). If not, see <http://www.gnu.org/licenses/>.
+//
+//! \file Ellipsoid.cpp
+//! \author Klaus Iglberger
+//! \author Sebastian Eibl <sebastian.eibl@fau.de>
+//
+//======================================================================================================================
+
+#include "Ellipsoid.h"
+
+//*************************************************************************************************
+// Includes
+//*************************************************************************************************
+
+#include <iomanip>
+#include <iostream>
+#include <stdexcept>
+#include <cmath>
+#include <pe/Materials.h>
+#include <core/math/Matrix3.h>
+#include <core/debug/Debug.h>
+
+namespace walberla {
+namespace pe {
+
+//=================================================================================================
+//
+//  CONSTRUCTOR
+//
+//=================================================================================================
+
+
+//*************************************************************************************************
+//*************************************************************************************************
+/*!\brief Constructor for the Ellipsoid class.
+ *
+ * \param sid Unique system-specific ID for the Ellipsoid.
+ * \param uid User-specific ID for the Ellipsoid.
+ * \param gpos Global geometric center of the Ellipsoid.
+ * \param rpos The relative position within the body frame of a superordinate body.
+ * \param q The orientation of the Ellipsoid's body frame in the global world frame.
+ * \param radius The radius of the Ellipsoid \f$ (0..\infty) \f$.
+ * \param material The material of the Ellipsoid.
+ * \param global specifies if the Ellipsoid should be created in the global storage
+ * \param communicating specifies if the Ellipsoid should take part in synchronization (syncNextNeighbour, syncShadowOwner)
+ * \param infiniteMass specifies if the Ellipsoid has infinite mass and will be treated as an obstacle
+ */
+Ellipsoid::Ellipsoid( id_t sid, id_t uid, const Vec3& gpos, const Vec3& rpos, const Quat& q,
+                const Vec3& semiAxes, MaterialID material,
+                const bool global, const bool communicating, const bool infiniteMass )
+   : Ellipsoid::Ellipsoid( getStaticTypeID(), sid, uid, gpos, rpos, q, semiAxes, material, global, communicating, infiniteMass )
+{}
+Ellipsoid::Ellipsoid( id_t const typeId, id_t sid, id_t uid, const Vec3& gpos, const Vec3& rpos, const Quat& q,
+                const Vec3& semiAxes, MaterialID material,
+                const bool global, const bool communicating, const bool infiniteMass )
+   : GeomPrimitive( typeId, sid, uid, material )  // Initialization of the parent class
+   , semiAxes_(semiAxes)
+{
+   // Checking the radius
+   // Since the Ellipsoid constructor is never directly called but only used in a small number
+   // of functions that already check the Ellipsoid arguments, only asserts are used here to
+   // double check the arguments.
+   WALBERLA_ASSERT( semiAxes_[0] > real_c(0), "Invalid Ellipsoid radius" );
+   WALBERLA_ASSERT( semiAxes_[1] > real_c(0), "Invalid Ellipsoid radius" );
+   WALBERLA_ASSERT( semiAxes_[2] > real_c(0), "Invalid Ellipsoid radius" );
+   // Setting the center of the Ellipsoid
+   gpos_ = gpos;
+
+   // Initializing the instantiated Ellipsoid
+   rpos_   = rpos;                   // Setting the relative position
+   q_      = q;                      // Setting the orientation
+   R_      = q_.toRotationMatrix();  // Setting the rotation matrix
+
+   // Calculating the Ellipsoid mass
+   mass_ = calcMass(semiAxes_, Material::getDensity( material ));
+   invMass_ = real_c(1) / mass_;
+
+   // Calculating the moment of inertia
+   calcInertia();
+
+   setGlobal( global );
+   setMass( infiniteMass );
+   setCommunicating( communicating );
+   setFinite( true );
+
+   // Setting the axis-aligned bounding box
+   Ellipsoid::calcBoundingBox();
+}
+//*************************************************************************************************
+
+
+
+
+//=================================================================================================
+//
+//  DESTRUCTOR
+//
+//=================================================================================================
+
+//*************************************************************************************************
+/*!\brief Destructor for the Ellipsoid class.
+ */
+Ellipsoid::~Ellipsoid()
+{
+   // Logging the destruction of the Ellipsoid
+   WALBERLA_LOG_DETAIL( "Destroyed Ellipsoid " << sid_ );
+}
+//*************************************************************************************************
+
+
+
+
+//=================================================================================================
+//
+//  UTILITY FUNCTIONS
+//
+//=================================================================================================
+
+//*************************************************************************************************
+/*!\brief Checks, whether a point in body relative coordinates lies inside the Ellipsoid.
+ *
+ * \param px The x-component of the relative coordinate.
+ * \param py The y-component of the relative coordinate.
+ * \param pz The z-component of the relative coordinate.
+ * \return \a true if the point lies inside the Ellipsoid, \a false if not.
+ */
+bool Ellipsoid::containsRelPointImpl( real_t px, real_t py, real_t pz ) const
+{
+return ( (px * px)/(semiAxes_[0] * semiAxes_[0]) + (py * py)/(semiAxes_[1] * semiAxes_[1]) 
+		+ (pz * pz)/(semiAxes_[2] * semiAxes_[2]) <= real_t(1.0) );
+}
+//*************************************************************************************************
+
+
+//*************************************************************************************************
+/*!\brief Checks, whether a point in body relative coordinates lies on the surface of the Ellipsoid.
+ *
+ * \param px The x-component of the relative coordinate.
+ * \param py The y-component of the relative coordinate.
+ * \param pz The z-component of the relative coordinate.
+ * \return \a true if the point lies on the surface of the Ellipsoid, \a false if not.
+ *
+ * The (relative) tolerance level of the check is pe::surfaceThreshold.
+ */
+bool Ellipsoid::isSurfaceRelPointImpl( real_t px, real_t py, real_t pz ) const
+{
+   return floatIsEqual( (px * px)/(semiAxes_[0] * semiAxes_[0]) + (py * py)/(semiAxes_[1] * semiAxes_[1]) 
+		+ (pz * pz)/(semiAxes_[2] * semiAxes_[2]), real_t(1.0), pe::surfaceThreshold);
+}
+//*************************************************************************************************
+
+
+
+
+//=================================================================================================
+//
+//  OUTPUT FUNCTIONS
+//
+//=================================================================================================
+
+//*************************************************************************************************
+/*!\brief Output of the current state of a Ellipsoid.
+ *
+ * \param os Reference to the output stream.
+ * \param tab Indentation in front of every line of the Ellipsoid output.
+ * \return void
+ */
+void Ellipsoid::print( std::ostream& os, const char* tab ) const
+{
+   using std::setw;
+
+   os << tab << " Ellipsoid " << uid_ << " with semi-axis " << semiAxes_ << "\n";
+
+   os << tab << "   Fixed: " << isFixed() << " , sleeping: " << !isAwake() << "\n";
+
+   os << tab << "   System ID         = " << getSystemID() << "\n"
+      << tab << "   Total mass        = " << getMass() << "\n"
+      << tab << "   Material          = " << Material::getName( material_ ) << "\n"
+      << tab << "   Owner             = " << MPITrait.getOwner() << "\n"
+      << tab << "   Global position   = " << getPosition() << "\n"
+      << tab << "   Relative position = " << getRelPosition() << "\n"
+      << tab << "   Linear velocity   = " << getLinearVel() << "\n"
+      << tab << "   Angular velocity  = " << getAngularVel() << "\n";
+
+//   if( verboseMode )
+//   {
+      os << tab << "   Bounding box      = " << getAABB() << "\n"
+         << tab << "   Quaternion        = " << getQuaternion() << "\n"
+         << tab << "   Rotation matrix   = ( " << setw(9) << R_[0] << " , " << setw(9) << R_[1] << " , " << setw(9) << R_[2] << " )\n"
+         << tab << "                       ( " << setw(9) << R_[3] << " , " << setw(9) << R_[4] << " , " << setw(9) << R_[5] << " )\n"
+         << tab << "                       ( " << setw(9) << R_[6] << " , " << setw(9) << R_[7] << " , " << setw(9) << R_[8] << " )\n";
+
+      os << std::setiosflags(std::ios::right)
+         << tab << "   Moment of inertia = ( " << setw(9) << I_[0] << " , " << setw(9) << I_[1] << " , " << setw(9) << I_[2] << " )\n"
+         << tab << "                       ( " << setw(9) << I_[3] << " , " << setw(9) << I_[4] << " , " << setw(9) << I_[5] << " )\n"
+         << tab << "                       ( " << setw(9) << I_[6] << " , " << setw(9) << I_[7] << " , " << setw(9) << I_[8] << " )\n"
+         << std::resetiosflags(std::ios::right);
+//   }
+}
+//*************************************************************************************************
+
+
+
+
+//=================================================================================================
+//
+//  GLOBAL OPERATORS
+//
+//=================================================================================================
+
+//*************************************************************************************************
+/*!\brief Global output operator for Ellipsoids.
+ *
+ * \param os Reference to the output stream.
+ * \param s Reference to a constant Ellipsoid object.
+ * \return Reference to the output stream.
+ */
+std::ostream& operator<<( std::ostream& os, const Ellipsoid& s )
+{
+   os << "--" << "Ellipsoid PARAMETERS"
+      << "-------------------------------------------------------------\n";
+   s.print( os, "" );
+   os << "--------------------------------------------------------------------------------\n"
+      << std::endl;
+   return os;
+}
+//*************************************************************************************************
+
+
+//*************************************************************************************************
+/*!\brief Global output operator for Ellipsoid handles.
+ *
+ * \param os Reference to the output stream.
+ * \param s Constant Ellipsoid handle.
+ * \return Reference to the output stream.
+ */
+std::ostream& operator<<( std::ostream& os, ConstEllipsoidID s )
+{
+   os << "--" << "Ellipsoid PARAMETERS"
+      << "-------------------------------------------------------------\n";
+   s->print( os, "" );
+   os << "--------------------------------------------------------------------------------\n"
+      << std::endl;
+   return os;
+}
+//*************************************************************************************************
+
+id_t Ellipsoid::staticTypeID_ = std::numeric_limits<id_t>::max();
+
+} // namespace pe
+}  // namespace walberla

src/pe/rigidbody/Ellipsoid.h

+//======================================================================================================================
+//
+//  This file is part of waLBerla. waLBerla is free software: you can
+//  redistribute it and/or modify it under the terms of the GNU General Public
+//  License as published by the Free Software Foundation, either version 3 of
+//  the License, or (at your option) any later version.
+//
+//  waLBerla is distributed in the hope that it will be useful, but WITHOUT
+//  ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+//  FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License
+//  for more details.
+//
+//  You should have received a copy of the GNU General Public License along
+//  with waLBerla (see COPYING.txt). If not, see <http://www.gnu.org/licenses/>.
+//
+//! \file Ellipsoid.h
+//! \author Klaus Iglberger
+//! \author Sebastian Eibl <sebastian.eibl@fau.de
+//! \author Tobias Leemann <tobias.leemann@fau.de>
+//
+//======================================================================================================================
+
+#pragma once
+
+
+//*************************************************************************************************
+// Includes
+//*************************************************************************************************
+
+#include <pe/rigidbody/GeomPrimitive.h>
+#include <pe/Types.h>
+#include <core/math/Constants.h>
+#include <core/math/Matrix3.h>
+#include <core/math/Vector3.h>
+#include <core/DataTypes.h>
+#include <core/logging/Logging.h>
+#include <core/math/Constants.h>
+#include <core/math/Limits.h>
+#include <core/math/Utility.h>
+#include <pe/Config.h>
+
+
+namespace walberla {
+namespace pe {
+
+//=================================================================================================
+//
+//  CLASS DEFINITION
+//
+//=================================================================================================
+
+//*************************************************************************************************
+/**
+ * \ingroup pe
+ * \brief Base class for the Ellipsoid geometry.
+ *
+ * The Ellipsoid class represents the base class for the Ellipsoid geometry. It provides
+ * the basic functionality of a Ellipsoid. An Ellipsoid is obtained from a sphere by deforming it by means 
+ * of directional scalings. Its three semi-axes corresponding to the x, y, z direction are labeled 
+ * a, b, c.
+ */
+class Ellipsoid : public GeomPrimitive
+{
+public:
+   //**Constructors********************************************************************************
+   /*!\name Constructors */
+   //@{
+   explicit Ellipsoid( id_t sid, id_t uid, const Vec3& gpos, const Vec3& rpos, const Quat& q,
+                    const Vec3& semiAxes, MaterialID material,
+                    const bool global, const bool communicating, const bool infiniteMass );
+   explicit Ellipsoid( id_t const typeID, id_t sid, id_t uid, const Vec3& gpos, const Vec3& rpos, const Quat& q,
+                    const Vec3& semiAxes, MaterialID material,
+                    const bool global, const bool communicating, const bool infiniteMass );
+   //@}
+   //**********************************************************************************************
+
+   //**Destructor**********************************************************************************
+   /*!\name Destructor */
+   //@{
+   virtual ~Ellipsoid();
+   //@}
+   //**********************************************************************************************
+   //**********************************************************************************************
+
+public:
+   //**Get functions*******************************************************************************
+   /*!\name Get functions */
+   //@{
+   inline const Vec3& getSemiAxes() const;
+   virtual inline real_t getVolume()         const;
+   //@}
+   //**********************************************************************************************
+
+   //**Utility functions***************************************************************************
+   static inline id_t getStaticTypeID();
+   //@}
+   //**********************************************************************************************
+
+   //**Output functions****************************************************************************
+   /*!\name Output functions */
+   //@{
+   virtual void print( std::ostream& os, const char* tab ) const;
+   //@}
+   //**********************************************************************************************
+
+   //**Utility functions***************************************************************************
+   /*!\name Utility functions */
+   //@{
+   inline virtual Vec3 support( const Vec3& d ) const;
+   //@}
+   //**********************************************************************************************
+
+   //**Volume, mass and density functions**********************************************************
+   /*!\name Volume, mass and density functions */
+   //@{
+   static inline real_t calcVolume( const Vec3& semiAxes );
+   static inline real_t calcMass( const Vec3& semiAxes, real_t density );
+   static inline real_t calcDensity( const Vec3& semiAxes, real_t mass );
+   //@}
+   //**********************************************************************************************
+
+protected:
+   //**Utility functions***************************************************************************
+   /*!\name Utility functions */
+   //@{
+   virtual bool containsRelPointImpl ( real_t px, real_t py, real_t pz ) const;
+   virtual bool isSurfaceRelPointImpl( real_t px, real_t py, real_t pz ) const;
+   //@}
+   //**********************************************************************************************
+
+   //**Utility functions***************************************************************************
+   /*!\name Utility functions */
+   //@{
+   inline virtual void calcBoundingBox();  // Calculation of the axis-aligned bounding box
+   inline         void calcInertia();      // Calculation of the moment of inertia
+   //@}
+   //**********************************************************************************************
+
+   //**Member variables****************************************************************************
+   /*!\name Member variables */
+   //@{
+   Vec3 semiAxes_;  //!< Semi-axes of the Ellipsoid.
+                  /*!< The radius is constrained to values larger than 0.0. */
+   //@}
+   //**********************************************************************************************
+private:
+   static id_t staticTypeID_;  //< type id of Ellipsoid, will be set by SetBodyTypeIDs
+   static void setStaticTypeID(id_t typeID) {staticTypeID_ = typeID;}
+
+   //** friend declaration
+   /// needed to be able to set static type ids with setStaticTypeID
+   template <class T>
+   friend struct SetBodyTypeIDs;
+};
+//*************************************************************************************************
+
+
+
+
+//=================================================================================================
+//
+//  GET FUNCTIONS
+//
+//=================================================================================================
+
+//*************************************************************************************************
+/*!\brief Returns the semi-axes of the Ellipsoid.
+ *
+ * \return The semi-axes of the Ellipsoid.
+ */
+inline const Vec3& Ellipsoid::getSemiAxes() const
+{
+   return semiAxes_;
+}
+//*************************************************************************************************
+
+
+//*************************************************************************************************
+/*!\brief Returns the volume of the Ellipsoid.
+ *
+ * \return The volume of the Ellipsoid.
+ */
+inline real_t Ellipsoid::getVolume() const
+{
+   return Ellipsoid::calcVolume(getSemiAxes());
+}
+//*************************************************************************************************
+
+
+
+
+//=================================================================================================
+//
+//  VOLUME, MASS AND DENSITY FUNCTIONS
+//
+//=================================================================================================
+
+//*************************************************************************************************
+/*!\brief Calculates the volume of a Ellipsoid for a given vector of semi-axes.
+ *
+ * \param semiAxes The vector of semi-axes of the Ellipsoid.
+ * \return The volume of the Ellipsoid.
+ */
+inline real_t Ellipsoid::calcVolume(const Vec3& semiAxes ) 
+{
+   return real_c(4.0)/real_c(3.0) * math::M_PI * semiAxes[0] * semiAxes[1] * semiAxes[2];
+}
+//*************************************************************************************************
+
+
+//*************************************************************************************************
+/*!\brief Calculates the mass of a Ellipsoid for a given a given vector of semi-axes and density.
+ *
+ * \param semiAxes The vector of semi-axes of the Ellipsoid.
+ * \param density The density of the Ellipsoid.
+ * \return The total mass of the Ellipsoid.
+ */
+inline real_t Ellipsoid::calcMass(const Vec3& semiAxes, real_t density )
+{
+   return real_c(4.0)/real_c(3.0) * math::M_PI * semiAxes[0] * semiAxes[1] * semiAxes[2] * density;
+}
+//*************************************************************************************************
+
+
+//*************************************************************************************************
+/*!\brief Calculates the density of a Ellipsoid for a given vector of semi-axes and mass.
+ *
+ * \param semiAxes The vector of semi-axes of the Ellipsoid.
+ * \param mass The total mass of the Ellipsoid.
+ * \return The density of the Ellipsoid.
+ */
+inline real_t Ellipsoid::calcDensity(const Vec3& semiAxes, real_t mass )
+{
+   return real_c(0.75) * mass / ( math::M_PI * semiAxes[0] * semiAxes[1] * semiAxes[2] );
+}
+//*************************************************************************************************
+
+
+
+
+//=================================================================================================
+//
+//  UTILITY FUNCTIONS
+//
+//=================================================================================================
+
+//*************************************************************************************************
+/*!\brief Calculation of the bounding box of the Ellipsoid.
+ *
+ * \return void
+ *
+ * This function updates the axis-aligned bounding box of the Ellipsoid primitive according to the
+ * current position and orientation of the Ellipsoid. Note that the bounding box is increased in
+ * all dimensions by pe::contactThreshold to guarantee that rigid bodies in close proximity of
+ * the Ellipsoid are also considered during the collision detection process.
+ * Algorithm: Use a non-axes-aligned bounding box of the ellipse (box
+ * with sides twice the semi-axes long) and calc its AABB.
+ */
+inline void Ellipsoid::calcBoundingBox()
+{	
+   using std::fabs;
+
+   const real_t xlength( fabs(R_[0]*semiAxes_[0]) + fabs(R_[1]*semiAxes_[1]) + fabs(R_[2]*semiAxes_[2])  + contactThreshold );
+   const real_t ylength( fabs(R_[3]*semiAxes_[0]) + fabs(R_[4]*semiAxes_[1]) + fabs(R_[5]*semiAxes_[2])  + contactThreshold );
+   const real_t zlength( fabs(R_[6]*semiAxes_[0]) + fabs(R_[7]*semiAxes_[1]) + fabs(R_[8]*semiAxes_[2])  + contactThreshold );
+   aabb_ = math::AABB(
+         gpos_[0] - xlength,
+         gpos_[1] - ylength,
+         gpos_[2] - zlength,
+         gpos_[0] + xlength,
+         gpos_[1] + ylength,
+         gpos_[2] + zlength
+         );
+	//   WALBERLA_ASSERT( aabb_.isValid()        , "Invalid bounding box detected" );
+   WALBERLA_ASSERT( aabb_.contains( gpos_ ), "Invalid bounding box detected("<< getSystemID() <<")\n" << "pos: " << gpos_ << "\nlengths: " << xlength << "," << ylength << ", " << zlength<< "\nvel: " << getLinearVel() << "\nbox: " << aabb_ );
+}
+//*************************************************************************************************
+
+
+//*************************************************************************************************
+/*!\brief Calculation of the moment of inertia in reference to the body frame of the Ellipsoid.
+ *
+ * \return void
+ */
+inline void Ellipsoid::calcInertia()
+{
+   I_[0] = real_c(0.2) * mass_ * (semiAxes_[1] * semiAxes_[1] + semiAxes_[2] * semiAxes_[2]);
+   I_[4] = real_c(0.2) * mass_ * (semiAxes_[2] * semiAxes_[2] + semiAxes_[0] * semiAxes_[0]);
+   I_[8] = real_c(0.2) * mass_ * (semiAxes_[0] * semiAxes_[0] + semiAxes_[1] * semiAxes_[1]);
+   Iinv_ = I_.getInverse();
+}
+//*************************************************************************************************
+
+
+//*************************************************************************************************
+/*!\brief Estimates the point which is farthest in direction \a d.
+ *
+ * \param d The normalized search direction in world-frame coordinates.
+ * \return The support point in world-frame coordinates in direction a\ d.
+ */
+inline Vec3 Ellipsoid::support( const Vec3& d ) const
+{
+   auto len = d.sqrLength();
+   if (!math::equal(len, real_t(0)))
+   {
+	  Vec3 d_loc = vectorFromWFtoBF(d);
+	  Vec3 norm_vec(d_loc[0] * semiAxes_[0], d_loc[1] * semiAxes_[1], d_loc[2] * semiAxes_[2]);
+	  real_t norm_length = norm_vec.length();
+	  Vec3 local_support = (real_t(1.0) / norm_length) * Vec3(semiAxes_[0] * semiAxes_[0] * d_loc[0], 
+			semiAxes_[1] * semiAxes_[1] * d_loc[1], semiAxes_[2] * semiAxes_[2] * d_loc[2]);
+      return pointFromBFtoWF(local_support);
+   } else
+   {
+      return Vec3(0,0,0);
+   }
+}
+//*************************************************************************************************
+
+
+//*************************************************************************************************
+/*!\brief Returns unique type id of this type
+ *
+ * \return geometry specific type id
+ */
+inline id_t Ellipsoid::getStaticTypeID()
+{
+   return staticTypeID_;
+}
+//*************************************************************************************************
+
+
+//=================================================================================================
+//
+//  GLOBAL OPERATORS
+//
+//=================================================================================================
+
+//*************************************************************************************************
+/*!\name Ellipsoid operators */
+//@{
+std::ostream& operator<<( std::ostream& os, const Ellipsoid& s );
+std::ostream& operator<<( std::ostream& os, ConstEllipsoidID s );
+//@}
+//*************************************************************************************************
+
+
+} // namespace pe
+}  // namespace walberla

src/pe/rigidbody/EllipsoidFactory.cpp

+//======================================================================================================================
+//
+//  This file is part of waLBerla. waLBerla is free software: you can
+//  redistribute it and/or modify it under the terms of the GNU General Public
+//  License as published by the Free Software Foundation, either version 3 of
+//  the License, or (at your option) any later version.
+//
+//  waLBerla is distributed in the hope that it will be useful, but WITHOUT
+//  ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+//  FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License
+//  for more details.
+//
+//  You should have received a copy of the GNU General Public License along
+//  with waLBerla (see COPYING.txt). If not, see <http://www.gnu.org/licenses/>.
+//
+//! \file EllipsoidFactory.cpp
+//! \author Tobias Leemann <tobias.leemann@fau.de>
+//! \author Sebastian Eibl <sebastian.eibl@fau.de>
+//
+//======================================================================================================================
+
+#include "EllipsoidFactory.h"
+
+#include "pe/Materials.h"
+#include "pe/rigidbody/BodyStorage.h"
+#include "pe/rigidbody/Ellipsoid.h"
+
+namespace walberla {
+namespace pe {
+
+EllipsoidID createEllipsoid( BodyStorage& globalStorage, BlockStorage& blocks, BlockDataID storageID,
+                       id_t uid, const Vec3& gpos, const Vec3& semiAxes,
+                       MaterialID material,
+                       bool global, bool communicating, bool infiniteMass )
+{
+   WALBERLA_ASSERT_UNEQUAL( Ellipsoid::getStaticTypeID(), std::numeric_limits<id_t>::max(), "Ellipsoid TypeID not initalized!");
+   // Checking the semiAxes
+   if( semiAxes[0] <= real_c(0) || semiAxes[1] <= real_c(0) || semiAxes[2] <= real_c(0) )
+      throw std::invalid_argument( "Invalid Ellipsoid semi-axes" );
+
+   EllipsoidID ellipsoid = NULL;
+
+   if (global)
+   {
+      const id_t sid = UniqueID<RigidBody>::createGlobal();
+      WALBERLA_ASSERT_EQUAL(communicating, false);
+      WALBERLA_ASSERT_EQUAL(infiniteMass, true);
+      ellipsoid = new Ellipsoid(sid, uid, gpos, Vec3(0,0,0), Quat(), semiAxes, material, global, false, true);
+      globalStorage.add(ellipsoid);
+   } else
+   {
+      for (auto it = blocks.begin(); it != blocks.end(); ++it){
+         IBlock* block = (&(*it));
+         if (block->getAABB().contains(gpos))
+         {
+            const id_t sid( UniqueID<RigidBody>::create() );
+
+            Storage* bs = block->getData<Storage>(storageID);
+            ellipsoid = new Ellipsoid(sid, uid, gpos, Vec3(0,0,0), Quat(), semiAxes, material, global, communicating, infiniteMass);
+            ellipsoid->MPITrait.setOwner(Owner(MPIManager::instance()->rank(), block->getId().getID()));
+            (*bs)[0].add(ellipsoid);
+         }
+      }
+   }
+
+   if (ellipsoid != NULL)
+   {
+      // Logging the successful creation of the Ellipsoid
+      WALBERLA_LOG_DETAIL(
+                "Created Ellipsoid " << ellipsoid->getSystemID() << "\n"
+             << "   User-ID         = " << uid << "\n"
+             << "   Global position = " << gpos << "\n"
+             << "   Semi-axes       = " << semiAxes << "\n"
+             << "   LinVel          = " << ellipsoid->getLinearVel() << "\n"
+             << "   Material        = " << Material::getName( material )
+               );
+   }
+
+   return ellipsoid;
+}
+
+}  // namespace pe
+}  // namespace walberla

src/pe/rigidbody/EllipsoidFactory.h

+//======================================================================================================================
+//
+//  This file is part of waLBerla. waLBerla is free software: you can
+//  redistribute it and/or modify it under the terms of the GNU General Public
+//  License as published by the Free Software Foundation, either version 3 of
+//  the License, or (at your option) any later version.
+//
+//  waLBerla is distributed in the hope that it will be useful, but WITHOUT
+//  ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+//  FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License
+//  for more details.
+//
+//  You should have received a copy of the GNU General Public License along
+//  with waLBerla (see COPYING.txt). If not, see <http://www.gnu.org/licenses/>.
+//
+//! \file EllipsoidFactory.h
+//! \author Tobias Leemann
+//! \author Sebastian Eibl <sebastian.eibl@fau.de>
+//
+//======================================================================================================================
+
+#pragma once
+
+#include "pe/Materials.h"
+#include "pe/Types.h"
+
+#include "blockforest/BlockForest.h"
+#include "core/debug/Debug.h"
+
+namespace walberla {
+namespace pe {
+
+//=================================================================================================
+//
+//  Ellipsoid SETUP FUNCTIONS
+//
+//=================================================================================================
+
+//*************************************************************************************************
+/**
+ * \ingroup pe
+ * \brief Setup of a new Ellipsoid.
+ *
+ * \param globalStorage process local global storage
+ * \param blocks storage of all the blocks on this process
+ * \param storageID BlockDataID of the BlockStorage block datum
+ * \param uid The user-specific ID of the Ellipsoid.
+ * \param gpos The global position of the center of the Ellipsoid.
+ * \param semiAxes The semiAxes of the Ellipsoid \f$ (0..\infty) \f$.
+ * \param material The material of the Ellipsoid.
+ * \param global specifies if the Ellipsoid should be created in the global storage
+ * \param communicating specifies if the Ellipsoid should take part in synchronization (syncNextNeighbour, syncShadowOwner)
+ * \param infiniteMass specifies if the Ellipsoid has infinite mass and will be treated as an obstacle
+ * \return Handle for the new Ellipsoid.
+ * \exception std::invalid_argument Invalid Ellipsoid semi-axes.
+ * \exception std::invalid_argument Invalid global Ellipsoid position.
+ *
+ * This function creates a Ellipsoid primitive in the \b pe simulation system. The Ellipsoid with
+ * user-specific ID \a uid is placed at the global position \a gpos, has the semi-axes \a semiAxes,
+ * and consists of the material \a material.
+ */
+EllipsoidID createEllipsoid( BodyStorage& globalStorage, BlockStorage& blocks, BlockDataID storageID,
+                       id_t uid, const Vec3& gpos, const Vec3& semiAxes,
+                       MaterialID material = Material::find("iron"),
+                       bool global = false, bool communicating = true, bool infiniteMass = false );
+//*************************************************************************************************
+
+}  // namespace pe
+}  // namespace walberla

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);
+   }
 }
 //*************************************************************************************************
 

tests/pe/CMakeLists.txt

@@ -22,6 +22,9 @@ waLBerla_execute_test( NAME   PE_CHECKVITALPARAMETERS )
 waLBerla_compile_test( NAME   PE_COLLISION FILES Collision.cpp DEPENDS core  )
 waLBerla_execute_test( NAME   PE_COLLISION )
 
+waLBerla_compile_test( NAME   PE_COLLISIONTOBIASGJK FILES CollisionTobiasGJK.cpp DEPENDS core  )
+waLBerla_execute_test( NAME   PE_COLLISIONTOBIASGJK )
+
 waLBerla_compile_test( NAME   PE_DELETEBODY FILES DeleteBody.cpp DEPENDS core blockforest  )
 waLBerla_execute_test( NAME   PE_DELETEBODY_NN COMMAND $<TARGET_FILE:PE_DELETEBODY> )
 waLBerla_execute_test( NAME   PE_DELETEBODY_SO COMMAND $<TARGET_FILE:PE_DELETEBODY> --syncShadowOwners )

tests/pe/CollisionTobiasGJK.cpp

+//======================================================================================================================
+//
+//  This file is part of waLBerla. waLBerla is free software: you can 
+//  redistribute it and/or modify it under the terms of the GNU General Public
+//  License as published by the Free Software Foundation, either version 3 of 
+//  the License, or (at your option) any later version.
+//  
+//  waLBerla is distributed in the hope that it will be useful, but WITHOUT 
+//  ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or 
+//  FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License 
+//  for more details.
+//  
+//  You should have received a copy of the GNU General Public License along
+//  with waLBerla (see COPYING.txt). If not, see <http://www.gnu.org/licenses/>.
+//
+//! \file CollisionTobiasGJK.cpp
+//! \author Sebastian Eibl <sebastian.eibl@fau.de>
+//
+//======================================================================================================================
+#include "pe/Types.h"
+
+
+#include "pe/contact/Contact.h"
+//#include "pe/fcd/IFCD.h"
+#include "pe/fcd/GenericFCD.h"
+#include "pe/fcd/AnalyticCollisionDetection.h"
+#include "pe/fcd/GJKEPACollideFunctor.h"
+#include "pe/Materials.h"
+
+#include "pe/rigidbody/Box.h"
+#include "pe/rigidbody/Capsule.h"
+#include "pe/rigidbody/Sphere.h"
+#include "pe/rigidbody/Plane.h"
+#include "pe/rigidbody/Union.h"
+#include "pe/rigidbody/Ellipsoid.h"
+
+#include "pe/rigidbody/SetBodyTypeIDs.h"
+
+
+#include "core/debug/TestSubsystem.h"
+#include "core/DataTypes.h"
+#include "core/math/Vector2.h"
+#include "core/math/Constants.h"
+
+#include "pe/collision/EPA.h"
+#include "pe/collision/GJK.h"
+
+using namespace walberla;
+using namespace walberla::pe;
+
+typedef boost::tuple<Box, Capsule, Plane, Sphere, Union<boost::tuple<Sphere, Union<boost::tuple<Sphere>>>>, Ellipsoid> BodyTuple ;
+
+bool gjkEPAcollideHybrid(GeomPrimitive &geom1, GeomPrimitive &geom2, Vec3& normal, Vec3& contactPoint, real_t& penetrationDepth)
+{
+   using namespace walberla::pe::fcd;
+   // For more information on hybrid GJK/EPA see page 166 in "Collision Detecton in Interactive 3D
+   // Environments" by Gino van den Bergen.
+
+   //1. Run GJK with considerably enlarged objects.
+   real_t margin = real_t(1e-4);
+   GJK gjk;
+   if(gjk.doGJKmargin(geom1, geom2, margin)){
+      //2. If collision is possible perform EPA.
+      //std::cerr << "Peforming EPA.";
+      EPA epa;
+      return epa.doEPAmargin(geom1, geom2, gjk, normal, contactPoint, penetrationDepth, margin);
+   }else{
+      return false;
+   }
+}
+
+//Define Test values for different precision levels
+#ifdef WALBERLA_DOUBLE_ACCURACY
+static const int distancecount = 6;
+static const real_t depth[distancecount] = {real_t(-1e-5), real_t(1e-5), real_t(1e-4), real_t(1e-2), real_t(0.1), real_t(1.0)};
+static const real_t test_accuracy = real_t(1e-3);
+#else
+static const int distancecount = 3;
+static const real_t depth[distancecount] = {real_t(1e-2), real_t(0.1), real_t(1.0)};
+static const real_t test_accuracy = real_t(1e-2); //Single Precision is v. bad!
+#endif
+
+
+/** Compares Computed Contact c1 to analytical Contact c2,
+ * and tests for equivalence.
+ * The computed position must only be in the same plane, if planeNormal has not length 0. */
+void checkContact(const Contact& c1, const Contact& c2, const Vec3& planeNormal, const real_t accuracy = test_accuracy )
+{
+
+   WALBERLA_CHECK_EQUAL( c1.getBody1(), c2.getBody1() );
+   WALBERLA_CHECK_EQUAL( c1.getBody2(), c2.getBody2() );
+
+   WALBERLA_CHECK_LESS( fabs((c1.getNormal() - c2.getNormal()).sqrLength()), accuracy*accuracy );
+   WALBERLA_CHECK_LESS( fabs(c1.getDistance()- c2.getDistance()), accuracy );
+   
+   //Unfortunately position accuracy is one-two orders of magnitude lower...
+   if(floatIsEqual(planeNormal.sqrLength(), real_t(0.0))){
+      WALBERLA_CHECK_LESS( fabs((c1.getPosition()- c2.getPosition()).sqrLength()), real_t(1e4)*accuracy*accuracy  );
+   }else{
+      //check for containment in plane only.
+      WALBERLA_CHECK_LESS( fabs(c1.getPosition()*planeNormal-c2.getPosition()*planeNormal), real_t(1e2)*accuracy );
+   }
+   
+}
+
+/** \brief Executes a test setup for collision data collection.
+    * \param rb1 first rigid body
+    * \param rb2 second rigid body
+    * \param dir1 direction of rb2 moving towards rb1 (unit vector)
+    * \param penetration_factor Increment of the penetration if rb2 is moved by dir1 (=1.0 in most cases)
+    * \param real_axis Analytical collision normal (unit vector)
+    * \param witnesspoint Analytical touching point of rb1 and rb2
+    * \param witnessmove Movement of the touching point, if rb2 is moved by dir1
+    * \param planeNormal The normal of the touching plane (if the touching point is unique,
+    * a Vector of length 0.0 shall be passed)
+    * \param accuracy Acceptance threshold
+    * Before the test, rb1 and rb2 shall be in touching contact.
+    * This function checks the collision data returned for different penetration depths and argument orders.
+    */
+void runCollisionDataTest(GeomPrimitive &rb1, GeomPrimitive &rb2, const Vec3& dir1, const real_t penetration_factor,
+                          const Vec3& real_axis, const Vec3& witnesspoint, const Vec3& witnessmove, const Vec3& planeNormal, const real_t accuracy = test_accuracy){
+
+   Vec3 org_pos = rb2.getPosition(); //Safe position
+
+   Vec3 normal1, normal2;
+   Vec3 pos1, pos2;
+   real_t comp_pen_depth1, comp_pen_depth2;
+
+   for(int j = 0; j < distancecount; j++){
+      //move rb1.
+      rb2.setPosition(org_pos + depth[j]*dir1);
+      WALBERLA_LOG_INFO("Using depth: "+ std::to_string(depth[j]));
+      //Compute collision between rb1 and rb2 and vice versa
+      bool result1 = gjkEPAcollideHybrid(rb1, rb2, normal1, pos1, comp_pen_depth1);
+      WALBERLA_LOG_DEVEL( normal1 << " " << pos1 << " " <<  comp_pen_depth1);
+      bool result2 = gjkEPAcollideHybrid(rb2, rb1, normal2, pos2, comp_pen_depth2);
+      WALBERLA_LOG_DEVEL( normal2 << " " << pos2 << " " <<  comp_pen_depth2);
+      if(depth[j] > real_t(0.0)){
+         WALBERLA_CHECK(result1);
+         WALBERLA_CHECK(result2);
+         //Check contact information
+         checkContact( Contact( &rb1, &rb2, pos1, normal1, comp_pen_depth1),
+                       Contact( &rb1, &rb2, witnesspoint + depth[j] * witnessmove, real_axis, -depth[j] * penetration_factor ), planeNormal, accuracy );
+         checkContact( Contact( &rb2, &rb1, pos2, normal2, comp_pen_depth2),
+                       Contact( &rb2, &rb1, witnesspoint + depth[j] * witnessmove, real_t(-1.0)*real_axis, -depth[j] * penetration_factor ), planeNormal, accuracy );
+      }
+      if(depth[j] < real_t(0.0)){
+         WALBERLA_CHECK(!result1);
+         WALBERLA_CHECK(!result2);
+      }
+   }
+}
+
+/** Test the GJK-EPA implementation on a variety of configuations 
+ * and penetation depths */
+void MainTest()
+{
+   MaterialID iron = Material::find("iron");
+
+   // Original SPHERE <-> SPHERE
+   Sphere sp1(123, 1, Vec3(0,0,0), Vec3(0,0,0), Quat(), 1, iron, false, true, false);
+   Sphere sp2(124, 2, Vec3(real_t(1.5),0,0), Vec3(0,0,0), Quat(), 1, iron, false, true, false);
+   Sphere sp3(125, 3, Vec3(real_t(3.0),0,0), Vec3(0,0,0), Quat(), 1, iron, false, true, false);
+
+   Vec3     normal;
+   Vec3     contactPoint;
+   real_t   penetrationDepth;
+
+
+   WALBERLA_LOG_INFO("Original: SPHERE <-> SPHERE");
+   WALBERLA_CHECK( !gjkEPAcollideHybrid(sp1, sp3, normal, contactPoint, penetrationDepth) );
+   WALBERLA_CHECK(  gjkEPAcollideHybrid(sp1, sp2, normal, contactPoint, penetrationDepth) );
+   checkContact( Contact( &sp1, &sp2, contactPoint,  normal, penetrationDepth),
+                 Contact( &sp1, &sp2, Vec3(real_t(0.75), 0, 0), Vec3(real_t(-1.0), 0, 0), real_t(-0.5)), Vec3(0,0,0) );
+
+   //Testcase 01 Box Sphere
+   WALBERLA_LOG_INFO("Test 01: BOX <-> SPHERE");
+   real_t sqr3_inv = real_t(1.0)/std::sqrt(real_t(3.0));
+   real_t coordinate= real_t(5.0)* sqr3_inv + real_t(5.0); // 5*(1+ (1/sqrt(3)))
+   Box box1_1(127, 5, Vec3(0, 0, 0), Vec3(0,0,0), Quat(), Vec3(10, 10, 10), iron, false, true, false);
+   Sphere sphere1_2(130, 8, Vec3(coordinate, coordinate, coordinate), Vec3(0,0,0), Quat(), 5, iron, false, true, false);
+   Vec3 wp1(real_t(5.0), real_t(5.0), real_t(5.0));
+   Vec3 wpm1(sqr3_inv*real_t(-0.5), sqr3_inv*real_t(-0.5), sqr3_inv*real_t(-0.5));
+   Vec3 axis1(-sqr3_inv, -sqr3_inv, -sqr3_inv);
+   runCollisionDataTest(box1_1, sphere1_2, axis1, real_t(1.0), axis1, wp1, wpm1, Vec3(0,0,0));
+
+   //Testcase 02 Box LongBox (touching plane)
+   //Reuse box1_1
+   WALBERLA_LOG_INFO("Test 02: BOX <-> LONG BOX");
+   Box box2_1(131, 9, Vec3(real_t(20.0),0,0), Vec3(0,0,0), Quat(), Vec3(real_t(30.0),1,1), iron, false, true, false);
+   Vec3 wp2(5, 0, 0);
+   Vec3 wpm2(real_t(-0.5),0,0);
+   Vec3 axis2(-1,0,0);
+   runCollisionDataTest(box1_1, box2_1, axis2, real_t(1.0), axis2, wp2, wpm2, axis2);
+
+   //Testcase 03 Sphere Sphere
+   WALBERLA_LOG_INFO("Test 03: SPHERE <-> SPHERE");
+   Sphere sphere3_1(129, 7, Vec3(0,0,0), Vec3(0,0,0), Quat(), 5, iron, false, true, false);
+   Sphere sphere3_2(128, 6, Vec3(real_t(10.0),0,0), Vec3(0,0,0), Quat(), 5, iron, false, true, false);
+   Vec3 wp3(5, 0, 0);
+   Vec3 wpm3(real_t(-0.5),0,0);
+   Vec3 axis3(-1,0,0);
+   runCollisionDataTest(sphere3_1, sphere3_2, axis3, real_t(1.0), axis3, wp3, wpm3, Vec3(0,0,0));
+
+   //Testcase 04 Cube with turned Cube
+   WALBERLA_LOG_INFO("Test 04: CUBE <-> TURNED CUBE");
+   //compute rotation.
+   real_t angle = walberla::math::M_PI/real_t(4.0);
+   Vec3 zaxis(0, 0, 1);
+   Quat q4(zaxis, angle);
+
+   //create turned box
+   real_t sqr2 = std::sqrt(real_t(2.0));
+   Box box4_1(132, 10, Vec3(real_t(5.0)*(real_t(1.0)+sqr2), real_t(-5.0), 0), Vec3(0,0,0), q4, Vec3(10, 10, 10), iron, false, true, false);
+   Box box4_2(133, 11, Vec3(0, 0, 0), Vec3(0,0,0), Quat(), Vec3(10, 10, 10), iron, false, true, false);
+   Vec3 wp4(5, -5, 0);
+   Vec3 wpm4(real_t(-0.25),real_t(+0.25),0);
+   Vec3 collision_axis4(-sqr2/real_t(2.0),+sqr2/real_t(2.0),0);
+   Vec3 axis4(-1, 0, 0);
+
+   runCollisionDataTest(box4_2, box4_1, axis4, sqr2/real_t(2.0), collision_axis4, wp4, wpm4, Vec3(0,real_t(1.0),0));
+
+   //Testcase 05 Cube and Long Box non-centric (touching plane)
+   WALBERLA_LOG_INFO("Test 05: CUBE <-> LONG BOX (NON_CENTRIC)");
+   Box box5_1(133, 12, Vec3(0, 0, 0), Vec3(0,0,0), Quat(), Vec3(10, 10, 10), iron, false, true, false);
+   Box box5_2(134, 13, Vec3(real_t(15.0),real_t(5.5), 0), Vec3(0,0,0), Quat(), Vec3(real_t(30.0),1,1), iron, false, true, false);
+   Vec3 wp5(real_t(3.75), 5, 0);
+   Vec3 wpm5(0, real_t(-0.5), 0);
+   Vec3 axis5(0, -1, 0);
+   runCollisionDataTest(box5_1, box5_2, axis5, real_t(1.0), axis5, wp5, wpm5, axis5);  //check only for containment in plane.
+
+
+   //Testcase 06:
+   WALBERLA_LOG_INFO("Test 06: CUBE <-> TURNED CUBE 2");
+   //compute rotation.
+
+   real_t sqr6_2 = std::sqrt(real_t(2.0));
+   real_t sqr6_3 = std::sqrt(real_t(3.0));
+   real_t angle6 = std::acos(real_t(1.0)/sqr6_3); //acos(1/sqrt(3))
+   Vec3 rot_axis6(0, real_t(1.0)/sqr6_2, -real_t(1.0)/sqr6_2);
+   Quat q6(rot_axis6, angle6);
+
+   //create turned box with pos = (5*(1+sqrt(3)), 0, 0)
+   Box box6_1(136, 14, Vec3(real_t(5.0)*(real_t(1.0)+sqr6_3), 0, 0), Vec3(0,0,0), q6, Vec3(10, 10, 10), iron, false, true, false);
+   Box box6_2(136, 15, Vec3(0, 0, 0), Vec3(0,0,0), Quat(), Vec3(10, 10, 10), iron, false, true, false);
+   Vec3 wp6(5, 0, 0);
+   Vec3 wpm6(real_t(-0.5), 0, 0);
+   Vec3 axis6(-1, 0, 0);
+   runCollisionDataTest(box6_2, box6_1, axis6, real_t(1.0), axis6, wp6, wpm6, Vec3(0,0,0));
+
+   //Testcase 07:
+   // BOX <-> SPHERE
+   WALBERLA_LOG_INFO("Test 07: BOX <-> SPHERE");
+   Sphere sphere7_1(137, 16, Vec3(0,0,0), Vec3(0,0,0), Quat(), 5, iron, false, true, false);
+   Box box7_2(138, 17, Vec3(0, 0,real_t(7.5)), Vec3(0,0,0), Quat(), Vec3(5, 5, 5), iron, false, true, false);
+   Vec3 wpm7(0, 0, real_t(-0.5));
+   Vec3 wp7(0, 0, real_t(5.0));
+   Vec3 axis7(0, 0,  real_t(-1.0));
+   runCollisionDataTest(sphere7_1, box7_2, axis7, real_t(1.0), axis7, wp7, wpm7, Vec3(0,0,0));
+
+   //Testcase 08:
+   // CAPSULE <-> CAPSULE
+   WALBERLA_LOG_INFO("Test 08: CAPSULE <-> CAPSULE");
+   Quat q8(Vec3(0,1,0), walberla::math::M_PI/real_t(2.0)); //creates a y-axis aligned capsule
+   Capsule cap8_1(139, 18, Vec3(0,0,0), Vec3(0,0,0), Quat(), real_t(4.0), real_t(10.0), iron, false, true, false);
+   Capsule cap8_2(140, 19, Vec3(0,0, real_t(13.0)), Vec3(0,0,0), q8, real_t(4.0), real_t(10.0), iron, false, true, false);
+   Vec3 wpm8(0, 0, real_t(-0.5));
+   Vec3 wp8(0, 0, real_t(4.0));
+   Vec3 axis8(0, 0,  real_t(-1.0));
+   runCollisionDataTest(cap8_1, cap8_2, axis8, real_t(1.0), axis8, wp8, wpm8, Vec3(0,0,0));
+
+   //Testcase 09:
+   // ELLIPSOID <-> ELLIPSOID
+   WALBERLA_LOG_INFO("Test 09: ELLIPSOID <-> ELLIPSOID");
+   Ellipsoid ell9_1(141, 20, Vec3(0,0,0), Vec3(0,0,0), Quat(), Vec3(10,5,5), iron, false, true, false);
+   Ellipsoid ell9_2(142, 21, Vec3(15,0,0), Vec3(0,0,0), Quat(), Vec3(5,10,5), iron, false, true, false);
+   Vec3 wpm9(real_t(-0.5), 0, 0);
+   Vec3 wp9(real_t(10), 0, 0);
+   Vec3 axis9(real_t(-1.0), 0, 0);
+   runCollisionDataTest(ell9_1, ell9_2, axis9, real_t(1.0), axis9, wp9, wpm9, Vec3(0,0,0));
+
+}
+
+/** Test the GJK-EPA implementation for a collision 
+ *	of a plane and a body and test the interface calls. */
+void PlaneTest()
+{
+   WALBERLA_LOG_INFO("PLANE AND INTERFACE TEST");
+   MaterialID iron = Material::find("iron");
+   fcd::GenericFCD<BodyTuple, fcd::GJKEPACollideFunctor> testFCD;
+
+   Plane pl(1, 1, Vec3(0, 1, 0), Vec3(0, 1, 0), real_t(1.0), iron );
+   Sphere sphere(2, 2, Vec3(0, real_t(1.9), 0), Vec3(0,0,0), Quat(), 1, iron, false, true, false);
+   Sphere sphere2(3, 3, Vec3(0, real_t(0.1), 0), Vec3(0,0,0), Quat(), 1, iron, false, true, false);
+
+   PossibleContacts pcs;
+
+   pcs.push_back(std::pair<Sphere*, Sphere*>(&sphere, &sphere2));
+   Contacts& container = testFCD.generateContacts(pcs);
+   WALBERLA_CHECK(container.size() == 1);
+
+   Contact &c = container.back();
+   //
+   WALBERLA_LOG_DEVEL( c.getDistance() << " " << c.getNormal() << " " << c.getPosition() );
+   if(c.getBody1()->getID() == 2) {
+      checkContact( c, Contact(&sphere, &sphere2,  Vec3(0, real_t(1), 0), Vec3(0, 1, 0), real_t(-0.2)), Vec3(0,0,0));
+   } else if (c.getBody1()->getID() == 3) {
+      checkContact( c, Contact(&sphere2, &sphere, Vec3(0, real_t(1), 0), Vec3(0, -1, 0), real_t(-0.2)), Vec3(0,0,0));
+   } else {
+      WALBERLA_ABORT("Unknown ID!");
+   }
+   pcs.clear();
+
+   pcs.push_back(std::pair<Plane*, Sphere*>(&pl, &sphere));
+   container = testFCD.generateContacts(pcs);
+   WALBERLA_CHECK(container.size() == 1);
+
+   c = container.back();
+   //
+   WALBERLA_LOG_DEVEL( c.getDistance() << " " << c.getNormal() << " " << c.getPosition() );
+   if(c.getBody1()->getID() == 1) {
+      checkContact( c, Contact(&pl, &sphere,  Vec3(0, real_t(0.95), 0), Vec3(0, -1, 0), real_t(-0.1)), Vec3(0,0,0));
+   } else if (c.getBody1()->getID() == 2) {
+      checkContact( c, Contact(&sphere, &pl, Vec3(0, real_t(0.95), 0), Vec3(0, 1, 0), real_t(-0.1)), Vec3(0,0,0));
+   } else {
+      WALBERLA_ABORT("Unknown ID!");
+   }
+   pcs.clear();
+
+   pcs.push_back(std::pair<Sphere*, Plane*>(&sphere, &pl));
+
+   container = testFCD.generateContacts(pcs);
+   WALBERLA_CHECK(container.size() == 1);
+   c = container.back();
+
+   WALBERLA_LOG_DEVEL( c.getDistance() << " " << c.getNormal() << " " << c.getPosition() );
+   if(c.getBody1()->getID() == 1) {
+      checkContact( c, Contact(&pl, &sphere,  Vec3(0, real_t(0.95), 0), Vec3(0, -1, 0), real_t(-0.1)), Vec3(0,0,0));
+   } else if (c.getBody1()->getID() == 2) {
+      checkContact( c, Contact(&sphere, &pl, Vec3(0, real_t(0.95), 0), Vec3(0, 1, 0), real_t(-0.1)), Vec3(0,0,0));
+   } else {
+      WALBERLA_ABORT("Unknown ID!");
+   }
+}
+
+/** Test the GJK-EPA implementation for a collision 
+ *	of a union and a body and the interface calls. */
+void UnionTest(){
+   WALBERLA_LOG_INFO("UNION AND INTERFACE TEST");
+   MaterialID iron = Material::find("iron");
+   fcd::GenericFCD<BodyTuple, fcd::GJKEPACollideFunctor> testFCD;
+
+   //A recursive union of three spheres is dropped on a box.
+   Box box(179, 179, Vec3(0,0,0), Vec3(0,0,0), Quat(), Vec3(real_t(10),real_t(2), real_t(10)), iron, false, true, false);
+
+
+   Union<boost::tuple<Sphere>> *unsub = new Union<boost::tuple<Sphere>>(192, 192, Vec3(0,real_t(3.8),0), Vec3(0,0,0), Quat(), false, true, false);
+
+   Sphere sp1( 180, 180, Vec3(-3,real_t(3.8),0), Vec3(0,0,0), Quat(), real_t(3.0)  , iron, false, true, false );
+   Sphere sp2( 181, 181, Vec3(3,real_t(3.8),0), Vec3(0,0,0), Quat(), real_t(3.0), iron, false, true, false );
+
+   Sphere sp3( 182, 182, Vec3(0,real_t(6),0), Vec3(0,0,0), Quat(), real_t(3.0), iron, false, true, false );
+   unsub->add(&sp1);
+   unsub->add(&sp2);
+
+   //Create another union, and add sub union
+   Union<boost::tuple<Sphere, Union<boost::tuple<Sphere>>>> *un = new Union<boost::tuple<Sphere, Union<boost::tuple<Sphere>>>>(193, 193, Vec3(0, 0, 0), Vec3(0,0,0), Quat(), false, true, false);
+   un->add(&sp3);
+   un->add(unsub);
+
+
+   PossibleContacts pcs;
+   pcs.push_back(std::pair<Union<boost::tuple<Sphere,Union<boost::tuple<Sphere>>>>*, Box*>(un, &box));
+   Contacts& container = testFCD.generateContacts(pcs);
+   WALBERLA_CHECK(container.size() == 2);
+
+   Contact &c = container.back();
+   WALBERLA_LOG_DEVEL( c.getDistance() << " " << c.getNormal() << " " << c.getPosition() );
+   if(c.getBody1()->getID() == 181) {
+      checkContact( c, Contact(&sp2, &box,  Vec3(real_t(3), real_t(0.9), 0), Vec3(0, 1, 0), real_t(-0.2)), Vec3(0,0,0));
+   } else if (c.getBody1()->getID() == 179) {
+      checkContact( c, Contact(&box, &sp2,  Vec3(real_t(3), real_t(0.9), 0), Vec3(0, -1, 0), real_t(-0.2)), Vec3(0,0,0));
+   } else {
+      WALBERLA_ABORT("Unknown ID!");
+   }
+   container.pop_back();
+
+
+   c = container.back();
+   WALBERLA_LOG_DEVEL( c.getDistance() << " " << c.getNormal() << " " << c.getPosition() );
+   if(c.getBody1()->getID() == 180) {
+      checkContact( c, Contact(&sp1, &box,  Vec3(real_t(-3), real_t(0.9), 0), Vec3(0, 1, 0), real_t(-0.2)), Vec3(0,0,0));
+   } else if (c.getBody1()->getID() == 179) {
+      checkContact( c, Contact(&box, &sp1,  Vec3(real_t(-3), real_t(0.9), 0), Vec3(0, -1, 0), real_t(-0.2)), Vec3(0,0,0));
+   } else {
+      WALBERLA_ABORT("Unknown ID!");
+   }
+   pcs.clear();
+
+   //Vice Versa
+   pcs.push_back(std::pair<Box*, Union<boost::tuple<Sphere, Union<boost::tuple<Sphere>>>>* >(&box, un));
+   container = testFCD.generateContacts(pcs);
+   WALBERLA_CHECK(container.size() == 2);
+
+   c = container.back();
+   WALBERLA_LOG_DEVEL( c.getDistance() << " " << c.getNormal() << " " << c.getPosition() );
+   if(c.getBody1()->getID() == 181) {
+      checkContact( c, Contact(&sp2, &box,  Vec3(real_t(3), real_t(0.9), 0), Vec3(0, 1, 0), real_t(-0.2)), Vec3(0,0,0));
+   } else if (c.getBody1()->getID() == 179) {
+      checkContact( c, Contact(&box, &sp2,  Vec3(real_t(3), real_t(0.9), 0), Vec3(0, -1, 0), real_t(-0.2)), Vec3(0,0,0));
+   } else {
+      WALBERLA_ABORT("Unknown ID!");
+   }
+   container.pop_back();
+
+   c = container.back();
+   WALBERLA_LOG_DEVEL( c.getDistance() << " " << c.getNormal() << " " << c.getPosition() );
+   if(c.getBody1()->getID() == 180) {
+      checkContact( c, Contact(&sp1, &box,  Vec3(real_t(-3), real_t(0.9), 0), Vec3(0, 1, 0), real_t(-0.2)), Vec3(0,0,0));
+   } else if (c.getBody1()->getID() == 179) {
+      checkContact( c, Contact(&box, &sp1,  Vec3(real_t(-3), real_t(0.9), 0), Vec3(0, -1, 0), real_t(-0.2)), Vec3(0,0,0));
+   } else {
+      WALBERLA_ABORT("Unknown ID!");
+   }
+   pcs.clear();
+
+}
+
+int main( int argc, char** argv )
+{
+   walberla::debug::enterTestMode();
+
+   walberla::MPIManager::instance()->initializeMPI( &argc, &argv );
+
+   SetBodyTypeIDs<BodyTuple>::execute();
+   MainTest();
+   PlaneTest();
+   UnionTest();
+   return EXIT_SUCCESS;
+}

tests/pe/Marshalling.cpp

@@ -26,6 +26,7 @@
 #include "pe/rigidbody/Squirmer.h"
 #include "pe/rigidbody/UnionFactory.h"
 #include "pe/rigidbody/Union.h"
+#include "pe/rigidbody/Ellipsoid.h"
 #include "pe/communication/rigidbody/Squirmer.h"
 #include "pe/communication/DynamicMarshalling.h"
 #include "pe/rigidbody/SetBodyTypeIDs.h"
@@ -41,7 +42,7 @@ typedef boost::tuple<Sphere>       UnionTypeTuple;
 typedef Union< UnionTypeTuple >    UnionT;
 typedef UnionT*                    UnionID;
 
-typedef boost::tuple<Box, Capsule, Sphere, Squirmer, UnionT> BodyTuple ;
+typedef boost::tuple<Box, Capsule, Sphere, Squirmer, UnionT, Ellipsoid> BodyTuple ;
 
 void testBox()
 {
@@ -128,6 +129,28 @@ void testSquirmer()
    WALBERLA_CHECK_FLOAT_EQUAL(s1.getSquirmerBeta(), s2->getSquirmerBeta());
 }
 
+void testEllipsoid()
+{
+   MaterialID iron = Material::find("iron");
+
+   Ellipsoid e1(759847, 1234795, Vec3(real_c(1), real_c(2), real_c(3)), Vec3(0,0,0), Quat(), Vec3(3,1,5), iron, false, false, false);
+   e1.setLinearVel(Vec3(real_c(5.2), real_c(6.3), real_c(7.4)));
+   e1.setAngularVel(Vec3(real_c(1.2), real_c(2.3), real_c(3.4)));
+
+   mpi::SendBuffer sb;
+   MarshalDynamically<BodyTuple>::execute(sb, e1);
+   mpi::RecvBuffer rb(sb);
+
+   EllipsoidID e2 = static_cast<EllipsoidID> (UnmarshalDynamically<BodyTuple>::execute(rb, Ellipsoid::getStaticTypeID(), math::AABB(Vec3(-100,-100,-100), Vec3(100,100,100)), math::AABB(Vec3(-100,-100,-100), Vec3(100,100,100))));
+
+   WALBERLA_CHECK_FLOAT_EQUAL(e1.getPosition(), e2->getPosition());
+   WALBERLA_CHECK_FLOAT_EQUAL(e1.getLinearVel(), e2->getLinearVel());
+   WALBERLA_CHECK_FLOAT_EQUAL(e1.getAngularVel(), e2->getAngularVel());
+   WALBERLA_CHECK_FLOAT_EQUAL(e1.getSemiAxes(), e2->getSemiAxes());
+   WALBERLA_CHECK_EQUAL(e1.getID(), e2->getID());
+   WALBERLA_CHECK_EQUAL(e1.getSystemID(), e2->getSystemID());
+}
+
 void testUnion()
 {
    UnionT u1(159, 423, Vec3(real_c(1), real_c(2), real_c(3)), Vec3(0,0,0), Quat(), false, false, false);
@@ -179,6 +202,7 @@ int main( int argc, char** argv )
    testCapsule();
    testUnion();
    testSquirmer();
+   testEllipsoid();
 
    return EXIT_SUCCESS;
 }