diff --git a/include/hpp/fcl/narrowphase/gjk.h b/include/hpp/fcl/narrowphase/gjk.h index 43a7b072b93cf73c6ac5d857b9e189ab0e36d654..1925a3815a8bc53f75a5978ff8548fba5ae7fb36 100644 --- a/include/hpp/fcl/narrowphase/gjk.h +++ b/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. diff --git a/src/narrowphase/gjk.cpp b/src/narrowphase/gjk.cpp index 4e042c755f0012f9421301d8db1322d68e923812..4c668f18beada7ce162bf92f73293d60c0a44fb0 100644 --- a/src/narrowphase/gjk.cpp +++ b/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;