Added sort_angles function.

Former-commit-id: e51a3ea41e69b96e9f46571426cf347f46430dac

include/igl/sort_angles.cpp

@@ -0,0 +1,99 @@
+#include "sort_angles.h"
+#include <algorithm>
+
+template <typename DerivedM, typename DerivedR>
+IGL_INLINE void igl::sort_angles(
+        const Eigen::PlainObjectBase<DerivedM>& M,
+        Eigen::PlainObjectBase<DerivedR>& R) {
+    const size_t num_rows = M.rows();
+    const size_t num_cols = M.cols();
+    assert(num_cols >= 2);
+
+    R.resize(num_rows);
+    R.setLinSpaced(num_rows, 0, num_rows-1);
+
+    //              |
+    // (pi/2, pi)   | (0, pi/2)
+    //              |
+    // -------------+--------------
+    //              |
+    // (-pi, -pi/2) | (-pi/2, 0)
+    //              |
+    auto comp = [&](size_t i, size_t j) {
+        auto yi = M(i, 0);
+        auto xi = M(i, 1);
+        auto yj = M(j, 0);
+        auto xj = M(j, 1);
+
+        if (xi == xj && yi == yj) {
+            for (size_t idx=2; idx<num_cols; idx++) {
+                auto i_val = M(i, idx);
+                auto j_val = M(j, idx);
+                if (i_val != j_val) {
+                    return i_val < j_val;
+                }
+            }
+            // If the entire rows are equal, use the row index.
+            return i < j;
+        }
+
+        if (xi >= 0 && yi >= 0) {
+            if (xj >=0 && yj >= 0) {
+                if (xi != xj) {
+                    return xi > xj;
+                } else {
+                    return yi < yj;
+                }
+            } else if (xj < 0 && yj >= 0) {
+                return true;
+            } else if (xj < 0 && yj < 0) {
+                return false;
+            } else {
+                return false;
+            }
+        } else if (xi < 0 && yi >= 0) {
+            if (xj >= 0 && yj >= 0) {
+                return false;
+            } else if (xj < 0 && yj >= 0) {
+                if (xi != xj) {
+                    return xi > xj;
+                } else {
+                    return yi > yj;
+                }
+            } else if (xj < 0 && yj < 0) {
+                return false;
+            } else {
+                return false;
+            }
+        } else if (xi < 0 && yi < 0) {
+            if (xj >= 0 && yj >= 0) {
+                return true;
+            } else if (xj < 0 && yj >= 0) {
+                return true;
+            } else if (xj < 0 && yj < 0) {
+                if (xi != xj) {
+                    return xi < xj;
+                } else {
+                    return yi > yj;
+                }
+            } else {
+                return true;
+            }
+        } else {
+            if (xj >= 0 && yj >= 0) {
+                return true;
+            } else if (xj < 0 && yj >= 0) {
+                return true;
+            } else if (xj < 0 && yj < 0) {
+                return false;
+            } else {
+                if (xi != xj) {
+                    return xi < xj;
+                } else {
+                    return yi < yj;
+                }
+            }
+        }
+    };
+    std::sort(R.data(), R.data() + num_rows, comp);
+}

include/igl/sort_angles.h

@@ -0,0 +1,27 @@
+#ifndef SORT_ANGLES_H
+#define SORT_ANGLES_H
+
+#include "igl_inline.h"
+
+namespace igl {
+    // Sort angles in ascending order in a numerically robust way.
+    //
+    // Instead of computing angles using atan2(y, x), sort directly on (y, x).
+    //
+    // Inputs:
+    //   M: m by n matrix of scalars. (n >= 2).  Assuming the first column of M
+    //      contains values for y, and the second column is x.  Using the rest
+    //      of the columns as tie-breaker.
+    //   R: an array of m indices.  M.row(R[i]) contains the i-th smallest
+    //      angle.
+    template<typename DerivedM, typename DerivedR>
+    IGL_INLINE void sort_angles(
+            const Eigen::PlainObjectBase<DerivedM>& M,
+            Eigen::PlainObjectBase<DerivedR>& R);
+}
+
+#ifndef IGL_STATIC_LIBRARY
+#include "sort_angles.cpp"
+#endif
+
+#endif