diff --git a/src/user/user_mesh.cc b/src/user/user_mesh.cc index d06905b3..cb1d5c0a 100644 --- a/src/user/user_mesh.cc +++ b/src/user/user_mesh.cc @@ -55,7 +55,6 @@ #include #include "engine/engine_crossplatform.h" #include "engine/engine_plugin.h" -#include "engine/engine_sort.h" #include "engine/engine_util_errmem.h" #include "user/user_cache.h" #include "user/user_model.h" @@ -843,33 +842,31 @@ void mjCMesh::FitGeom(mjCGeom* geom, double* meshpos) { // comparison function for vertex sorting -quicksortfunc(vertcompare, context, el1, el2) { - float* vert = (float*) context; - float x1 = vert[3*(*(int*)el1)] + 1e-2*vert[1+3*(*(int*)el1)] + 1e-4*vert[2+3*(*(int*)el1)]; - float x2 = vert[3*(*(int*)el2)] + 1e-2*vert[1+3*(*(int*)el2)] + 1e-4*vert[2+3*(*(int*)el2)]; - - if (x1 < x2) { - return -1; - } else if (x1 == x2) { - return 0; - } else { - return 1; +bool vertcompare(int index1, int index2, const std::vector& vert) { + for (int i = 0; i < 3; i++) { + if (vert[3*index1 + i] < vert[3*index2 + i]) { + return true; + } + if (vert[3*index1 + i] > vert[3*index2 + i]) { + return false; + } } + return false; } // remove repeated vertices void mjCMesh::RemoveRepeated() { int repeated = 0; - // allocate sort and redirection indices, set to identity - auto index = std::unique_ptr(new int[nvert()]); - auto redirect = std::unique_ptr(new int[nvert()]); + std::vector index(nvert()); + std::vector redirect(nvert()); for (int i=0; i < nvert(); i++) { index[i] = redirect[i] = i; } - // sort vertices - mjQUICKSORT(index.get(), nvert(), sizeof(int), vertcompare, vert_.data()); + std::stable_sort(index.begin(), index.end(), [&vert = vert_](int a, int b) { + return vertcompare(a, b, vert); + }); // find repeated vertices, set redirect for (int i=1; i < nvert(); i++) {