add gjk algorithm

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/fcd/IterativeFCD.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 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_;
+   }
+
+};
+
+
+}
+}
+}

src/pe/fcd/IterativeFCDDataHandling.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 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>>( );
+}
+
+}
+}
+}

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