EPA.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_EPA_H
#define _LibCollision_EPA_H
 
#include <array>
#include <list>
 
#include "GranOO3/Geom/Vector.hpp"
#include "GranOO3/Collision/Data.hpp"
#include "GranOO3/Collision/GJK.hpp"
 
namespace GranOO3
{
  namespace Collision
  {
    namespace EPA
    {
 
 
      // Triangle struct that compose the polyhedron
      struct Triangle
      {
 
        Triangle(const GJK::SupportPoint &a,const GJK::SupportPoint &b,const GJK::SupportPoint &c);
        // public data
        std::array<GJK::SupportPoint, 3> point;
        Geom::Vector normal;
      };
 
 
 
 
      // Edge struct that compose the polyhedron
      struct Edge
      {
        Edge(const GJK::SupportPoint &a,const GJK::SupportPoint &b);
        // public data
        std::array<GJK::SupportPoint, 2> point;
 
      };
 
 
 
 
      // Polyhedron struct needed for the EPA algorithm
      struct Polyhedron
      {
        void AddEdge(const GJK::SupportPoint& a, const GJK::SupportPoint& b);
        // public data
 
        std::list<Triangle> triangle;
        std::list<Edge> edge;
      };
 
 
      // Some inlined definitions
      inline
      Triangle::Triangle(const GJK::SupportPoint &a,const GJK::SupportPoint &b,const GJK::SupportPoint &c)
        : point({{a,b,c}}), normal(((b.v-a.v)^(c.v-a.v)).unit()) {
      }
 
      inline
      Edge::Edge(const GJK::SupportPoint &a,const GJK::SupportPoint &b)
        : point({{a,b}}) {
      }
 
    }// End of EPA namespace
 
 
 
    // The EPA algorithm : it allows to compute the contact information (depth, normal and point),
    // this algo work together with the GJK
    class EPA_Algorithm
    {
    public:
      static unsigned int max_iter;
      static double exit_treshold;
 
    public:
      EPA_Algorithm(GJK_Algorithm&);
      ~EPA_Algorithm();
 
      bool compute_data(Data&);
      bool compute_data(const EPA::Triangle& triangle, double penetration, Data&);
      void barycentric(const EPA::Triangle& triangle, const Geom::Vector &p,
                       double& u, double& v, double& w);
 
    public:
      GJK_Algorithm&  gjk;
      EPA::Polyhedron polyhedron;
    };
 
 
 
  }
}
 
 
#endif