EPA: use std::vector to manage EPA's storage

Note: resizing a std::vector with a size smaller than its current size
will not reallocate the memory.
So there is no downside to always using resize in the reset method
compared to the previous implementation.

include/hpp/fcl/narrowphase/gjk.h

@@ -412,8 +412,8 @@ struct HPP_FCL_DLLAPI EPA {
   FCL_REAL depth;
 
  private:
-  SimplexV* sv_store;
-  SimplexF* fc_store;
+  std::vector<SimplexV> sv_store;
+  std::vector<SimplexF> fc_store;
   size_t nextsv;
   SimplexList hull, stock;
 
@@ -433,15 +433,10 @@ struct HPP_FCL_DLLAPI EPA {
       : max_face_num(other.max_face_num),
         max_vertex_num(other.max_vertex_num),
         max_iterations(other.max_iterations),
-        tolerance(other.tolerance) {
+        tolerance(other.tolerance),
+        sv_store(other.sv_store),
+        fc_store(other.fc_store) {
     initialize();
-    memcpy(sv_store, other.sv_store, max_vertex_num * sizeof(SimplexV));
-    memcpy(fc_store, other.fc_store, max_face_num * sizeof(SimplexF));
-  }
-
-  ~EPA() {
-    delete[] sv_store;
-    delete[] fc_store;
   }
 
   /// @brief resets the EPA algorithm, preparing it for a new run.

src/narrowphase/gjk.cpp

@@ -1467,31 +1467,13 @@ bool GJK::projectTetrahedraOrigin(const Simplex& current, Simplex& next) {
   return false;
 }
 
-void EPA::initialize() {
-  if (max_vertex_num > 0) {
-    sv_store = new SimplexV[max_vertex_num];
-  } else {
-    sv_store = nullptr;
-  }
-  if (max_face_num > 0) {
-    fc_store = new SimplexF[max_face_num];
-  } else {
-    fc_store = nullptr;
-  }
-  reset(max_vertex_num, max_face_num);
-}
+void EPA::initialize() { reset(max_vertex_num, max_face_num); }
 
 void EPA::reset(size_t max_vertex_num_, size_t max_face_num_) {
-  if (max_vertex_num_ > max_vertex_num) {
-    if (sv_store != nullptr) delete[] sv_store;
-    sv_store = new SimplexV[max_vertex_num_];
-    max_vertex_num = max_vertex_num_;
-  }
-  if (max_face_num_ > max_face_num) {
-    if (fc_store != nullptr) delete[] fc_store;
-    fc_store = new SimplexF[max_face_num_];
-    max_face_num = max_face_num_;
-  }
+  sv_store.resize(max_vertex_num_);
+  max_vertex_num = max_vertex_num_;
+  fc_store.resize(max_face_num_);
+  max_face_num = max_face_num_;
   status = DidNotRun;
   normal = Vec3f(0, 0, 0);
   depth = 0;