Implement argsort helper function

Author: Q-Minh

source/pbat/common/ArgSort.cpp

@@ -0,0 +1 @@
+#include "ArgSort.h"

source/pbat/common/ArgSort.h

@@ -0,0 +1,25 @@
+#ifndef PBAT_COMMON_ARG_SORT_H
+#define PBAT_COMMON_ARG_SORT_H
+
+#include <algorithm>
+#include <concepts>
+#include <iterator>
+#include <numeric>
+#include <vector>
+
+namespace pbat {
+namespace common {
+
+template <std::integral TIndex, class FLess>
+std::vector<TIndex> ArgSort(auto n, FLess less)
+{
+    std::vector<TIndex> inds(n);
+    std::iota(inds.begin(), inds.end(), TIndex(0));
+    std::sort(inds.begin(), inds.end(), less);
+    return inds;
+}
+
+} // namespace common
+} // namespace pbat
+
+#endif // PBAT_COMMON_ARG_SORT_H

source/pbat/common/CMakeLists.txt

@@ -2,6 +2,7 @@ target_sources(PhysicsBasedAnimationToolkit_PhysicsBasedAnimationToolkit
     PUBLIC
     FILE_SET api
     FILES 
+        "ArgSort.h"
         "Common.h"
         "Concepts.h"
         "ConstexprFor.h"
@@ -13,6 +14,7 @@ target_sources(PhysicsBasedAnimationToolkit_PhysicsBasedAnimationToolkit
 )
 target_sources(PhysicsBasedAnimationToolkit_PhysicsBasedAnimationToolkit
 PRIVATE
+    "ArgSort.cpp"
     "Eigen.cpp"
     "Indexing.cpp"
     "Queue.cpp"

source/pbat/common/Common.h

@@ -1,6 +1,7 @@
 #ifndef PBAT_COMMON_COMMON_H
 #define PBAT_COMMON_COMMON_H
 
+#include "ArgSort.h"
 #include "Concepts.h"
 #include "ConstexprFor.h"
 #include "Eigen.h"