solver_gjk_pp.hpp

Go to the documentation of this file.

#pragma once
 
#include "collision_geometries.hpp"
#include "collision_solver_pp.hpp"
#include "gjk_simplex.hpp"
#include "particle.hpp"
#include "shape.hpp"
#include "wall.hpp"
 
namespace netdem {
 
class SolverGJKPP : public CollisionSolverPP {
public:
  double erosion_ratio_initial{0.01};
 
  double erosion_ratio_increment{0.01};
 
  bool use_erosion{false};
 
  bool enable_logging{false};
 
  SolverGJKPP();
 
  SolverGJKPP(Particle *const p1, Particle *const p2);
 
  CollisionSolverPP *Clone() const override;
 
  void Init(Particle *const p1, Particle *const p2) override;
 
  bool Detect() override;
 
  void ResolveInit(ContactPP *const cnt, double timestep) override;
 
  void ResolveUpdate(ContactPP *const cnt, double timestep) override;
 
  void ResolveInit(CollisionGeometries *const cnt_geoms, double timestep);
 
  void ResolveUpdate(CollisionGeometries *const cnt_geoms, double timestep);
 
protected:
  Shape *shape_1{nullptr}, *shape_2{nullptr};
  double bound_sphere_radius_1, bound_sphere_radius_2;
 
  Vec3d dpos_12, dpos_12_ref;
  Vec4d dquat_12, dquat_12_conj;
 
  Simplex simplex_after_gjk;
 
  bool GJK();
 
  std::tuple<double, Vec3d, Vec3d> GJK_EROSION();
 
  std::tuple<double, Vec3d, Vec3d> EPA();
 
  std::tuple<Vec3d, bool> GetContactPoint(Vec3d const &dir);
 
  std::tuple<Vec3d, bool>
  GetContactPoint_PlaneCase(Vec3d const &dir, const VecXT<Vec3d> &pos_vec_1,
                            const VecXT<Vec3d> &pos_vec_2);
  inline Vec3d MinkowskiDiff(Vec3d const &dir, double erosion_ratio = 0);
 
  void UpdateSimplex(Simplex *const s, Vec3d *const dir, double *const min_dist,
                     bool *const cnt_flag);
 
  void UpdateSimplexLine(Simplex *const s, Vec3d *const dir,
                         double *const min_dist, bool *const cnt_flag);
 
  void UpdateSimplexTriangle(Simplex *const s, Vec3d *const dir,
                             double *const min_dist, bool *const cnt_flag);
 
  void UpdateSimplexTetrahedron(Simplex *const s, Vec3d *const dir,
                                double *const min_dist, bool *const cnt_flag);
 
  inline std::tuple<Vec3d, double>
  GetFacetNormal(Vec3d const &a, Vec3d const &b, Vec3d const &c);
 
  void GetLooseEdges(VecXT<Vec2i> *const edges, Vec3i const &facet);
 
  void GetIntersections(VecXT<Vec3d> *const intersects, Vec3d const &dir_n,
                        Vec3d const &l1_p1, Vec3d const &l1_p2,
                        Vec3d const &l2_p1, Vec3d const &l2_p2);
 
  void GetIntersectionsAggresive(VecXT<Vec3d> *const intersects,
                                 Vec3d const &dir_n, Vec3d const &l1_p1,
                                 Vec3d const &l1_p2, Vec3d const &l2_p1,
                                 Vec3d const &l2_p2);
 
  void SortVertices(VecXT<Vec3d> *const pos_vec, Vec3d const &dir_n);
 
  bool IsInsidePolygon(VecXT<Vec3d> const &pos_vec, Vec3d const &dir_n,
                       Vec3d const &pos);
 
  Vec3d GetPolygonCentroid(VecXT<Vec3d> const &pos_vec, Vec3d const &dir_n);
};
 
} // namespace netdem