gjk_simplex.hpp

Go to the documentation of this file.

#pragma once
 
#include "utils_math.hpp"
 
namespace netdem {
 
class Simplex {
public:
  VecNT<Vec3d, 4> points;
 
private:
  int size{0};
 
public:
  Simplex() {}
 
  Simplex(Vec3d const &a) {
    size = 1;
 
    points[0] = a;
  }
 
  Simplex(Vec3d const &a, Vec3d const &b) {
    size = 2;
 
    points[0] = a;
    points[1] = b;
  }
 
  Simplex(Vec3d const &a, Vec3d const &b, Vec3d const &c) {
    size = 3;
 
    points[0] = a;
    points[1] = b;
    points[2] = c;
  }
 
  Simplex(Vec3d const &a, Vec3d const &b, Vec3d const &c, Vec3d const &d) {
    size = 4;
 
    points[0] = a;
    points[1] = b;
    points[2] = c;
    points[3] = d;
  }
 
  int Size() { return size; };
 
  void PushBack(Vec3d const &p) {
    points[size] = p;
    size += 1;
  }
 
  void PushFront(Vec3d const &p) {
    for (int i = size; i > 0; i--) {
      points[size] = points[size - 1];
    }
    points[0] = p;
    size += 1;
  }
};
 
} // namespace netdem