Line data    Source code

       1              : #pragma once
       2              : 
       3              : #include "rbc/gjk/GJK.hpp"
       4              : #include "rbc/gjk/MinkowskiDiff.hpp"
       5              : 
       6              : namespace rbc
       7              : {
       8              : 
       9              :     struct EPAFace
      10              :     {
      11              :         m3d::vec3 n;          // Face normal (outward from polytope, away from origin)
      12              :         m3d::scalar d;        // Signed distance from origin to the face plane (>= 0 for valid faces)
      13              :         SimplexVertex *v[3];  // The three vertices making up the face (CCW when viewed from outside)
      14              :         EPAFace *adjacent[3]; // Adjacent faces
      15              :         int edge_adj[3];      // Which edge index on the adjacent face connects back to this one
      16              :         bool obsolete;        // Marked true when the face is removed during expansion
      17              :         unsigned int pass;    // Pass counter used during horizon traversal to detect already-visited faces
      18              :     };
      19              : 
      20              :     // Tracks the "horizon" ring of new faces being stitched around the new support point
      21              :     // during polytope expansion. The horizon is a closed loop of faces in CCW order.
      22              :     struct EPAHorizon
      23              :     {
      24              :         EPAFace *cf;  // The most recently added horizon face (tail)
      25              :         EPAFace *ff;  // The first horizon face added (head, used to close the ring)
      26              :         int nf;       // Number of faces added to the horizon so far
      27          262 :         EPAHorizon() : cf(nullptr), ff(nullptr), nf(0) {}
      28              :     };
      29              : 
      30              :     struct EPA
      31              :     {
      32              :         enum Status
      33              :         {
      34              :             Valid,
      35              :             Failed,
      36              :             OutOfFaces,
      37              :             OutOfVertices,
      38              :             NonLinear
      39              :         } status;
      40              : 
      41              :         // Configuration
      42              :         unsigned int max_faces     = 256;
      43              :         unsigned int max_vertices  = 128;
      44              :         unsigned int max_iterations = 128;
      45              :         m3d::scalar  tolerance     = 1e-6;
      46              : 
      47              :         // Results
      48              :         m3d::vec3   normal;
      49              :         m3d::scalar depth;
      50              :         m3d::vec3   contact_point;
      51              : 
      52              :         // Internal state
      53              :         EPAFace       **faces;
      54              :         SimplexVertex **vertices;
      55              :         unsigned int    num_faces;
      56              :         unsigned int    num_vertices;
      57              : 
      58              :         EPA();
      59              :         ~EPA();
      60              : 
      61              :         Status evaluate(const GJK &gjk_solver, const MinkowskiDiff &shape);
      62              : 
      63              :         void      initialize();
      64              :         EPAFace  *find_closest_face();
      65              :         void      bind_faces(EPAFace *f0, int e0, EPAFace *f1, int e1);
      66              : 
      67              :         // Recursively traverses the adjacency graph from face f (through edge e) to find
      68              :         // the horizon silhouette. Faces visible from w are marked obsolete; for each
      69              :         // horizon edge a new face is created and appended to the horizon ring.
      70              :         bool expand(unsigned int pass, SimplexVertex *w,
      71              :                     EPAFace *f, int e, EPAHorizon &horizon);
      72              : 
      73              :     private:
      74              :         // Allocates and initialises one face from the pre-allocated pool.
      75              :         // Returns nullptr if the face pool is exhausted or the face is degenerate.
      76              :         EPAFace *new_face(SimplexVertex *a, SimplexVertex *b, SimplexVertex *c);
      77              :     };
      78              : 
      79              : } // namespace rbc