GJK.hpp

Go to the documentation of this file.

// This file is part of GranOO, a workbench for DEM simulation.
//
// Author(s)    : - Damien Andre       IRCER/UNILIM, Limoges France
//                  <damien.andre@unilim.fr>
//                - Jean-luc Charles   Arts et Metiers ParisTech, CNRS, I2M, Bordeaux France
//                  <jean-luc.charles@ensam.eu>
//                - Jeremie Girardot   Arts et Metiers ParisTech, CNRS, I2M, Bordeaux France
//                  <jeremie.girardot@ensam.eu>
//                - Cedric Hubert      LAMIH/UPHF, Valenciennes France
//                  <cedric.hubert@uphf.fr>
//                - Ivan Iordanoff     Arts et Metiers ParisTech, CNRS, I2M, Bordeaux France
//                  <ivan.iordanoff@ensam.eu>
//
// Copyright (C) 2008-2019 D. Andre, JL. Charles, J. Girardot, C. Hubert, I. Iordanoff
//
// This program 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.
//
// This program 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 this program.  If not, see <http://www.gnu.org/licenses/>.
 
 
 
 
#ifndef _LibCollision_GJK_H
#define _LibCollision_GJK_H
 
#include <array>
 
#include "GranOO3/Geom/Vector.hpp"
#include "GranOO3/Collision/Data.hpp"
#include "GranOO3/Math/UniRandom.hpp"
 
namespace GranOO3
{
 
  namespace Shape {class Volume;}
 
  namespace Collision
  {
 
    namespace GJK
    {
 
 
      // The SupportPoint struct help us to store points
      // in the minkowsky space and world space
      struct SupportPoint
      {
        SupportPoint(const Geom::Vector& v1, const Geom::Vector& v2);
        SupportPoint();
        bool operator==(const SupportPoint&) const;
 
        //public data
        Geom::Vector v; // the point in the minkowsky space
        std::array<Geom::Vector, 2> local; // local points shape1 and shape2 in world space
 
      };
 
      // inlined definitions
      inline
      SupportPoint::SupportPoint(const Geom::Vector& v1, const Geom::Vector& v2)
        : v(v1-v2), local({{v1,v2}}) {
      }
 
      inline
      SupportPoint::SupportPoint()
        : v(), local() {
      }
 
      inline
      bool SupportPoint::operator==(const SupportPoint &r) const {
        return v == r.v;
      }
 
      struct Simplex
      {
      public:
        Simplex();
        void clear();
        void set(const SupportPoint& a, const SupportPoint& b, const SupportPoint& c, const SupportPoint& d);
        void set(const SupportPoint& a, const SupportPoint& b, const SupportPoint& c);
        void set(const SupportPoint& a, const SupportPoint& b);
        void set(const SupportPoint& a);
 
        void Push(const SupportPoint& p);
 
        //public data
        int num;
        SupportPoint &a;
        SupportPoint &b;
        SupportPoint &c;
        SupportPoint &d;
 
      private:
        SupportPoint point[4];
 
      };
 
      inline
      Simplex::Simplex() :
        a(point[0]), b(point[1]), c(point[2]), d(point[3]) {
        clear();
      }
 
      inline void
      Simplex::clear() {
        num = 0;
      }
 
      inline void
      Simplex::set(const SupportPoint& a, const SupportPoint& b, const SupportPoint& c, const SupportPoint& d) {
        num = 4;
        this->a = a;
        this->b = b;
        this->c = c;
        this->d = d;
      }
 
      inline void
      Simplex::set(const SupportPoint& a, const SupportPoint& b, const SupportPoint& c) {
        num = 3;
        this->a = a;
        this->b = b;
        this->c = c;
      }
 
      inline void
      Simplex::set(const SupportPoint& a, const SupportPoint& b) {
        num = 2;
        this->a = a;
        this->b = b;
      }
 
      inline void
      Simplex::set(const SupportPoint& a) {
        num = 1;
        this->a = a;
      }
 
      inline void
      Simplex::Push(const SupportPoint& p) {
        num = (std::min)(num+1,4);
        for (int i = num-1; i > 0; i--)
          point[i] = point[i-1];
 
        point[0] = p;
      }
 
    } // End of EPA namespace
 
 
    // The GJK algoritm : it allows to detect if two convex shapes are colliding
    class GJK_Algorithm
    {
    public:
      static unsigned int max_iter;
      static Math::UniRandom rand_dir_x;
      static Math::UniRandom rand_dir_y;
      static Math::UniRandom rand_dir_z;
 
    public:
      static void random_direction(Geom::Vector&);
 
    public:
      GJK_Algorithm(const Shape::Volume& v1, const Shape::Volume& v2);
      ~GJK_Algorithm();
 
      bool is_shape_colliding();
 
      GJK::SupportPoint support(const Geom::Vector&) const;
 
    private:
      bool is_support_direction_valid(const GJK::SupportPoint& s, const Geom::Vector& dir) const;
 
    public:
      const Shape::Volume& shape1;
      const Shape::Volume& shape2;
      GJK::Simplex sim;
    };
 
  }
}
 
 
#endif