Properly sort intersection points for complex shapes

[libs/math.git] / source / geometry / surfacepoint.h

diff --git a/source/geometry/surfacepoint.h b/source/geometry/surfacepoint.h
index a5bd794f90052bc96fcc307dfdb67b74244707c8..57a66d1e95f3898bf1f050ed65afa77bf8d89cae 100644 (file)
--- a/source/geometry/surfacepoint.h
+++ b/source/geometry/surfacepoint.h
@@ -1,6 +1,7 @@
 #ifndef MSP_GEOMETRY_SURFACEPOINT_H_
 #define MSP_GEOMETRY_SURFACEPOINT_H_
 
+#include <algorithm>
 #include <msp/linal/vector.h>
 
 namespace Msp {
@@ -17,6 +18,20 @@ struct SurfacePoint
        T distance;
 };
 
+template<typename T, unsigned N>
+void sort_points(SurfacePoint<T, N> *points, unsigned size)
+{
+       for(unsigned i=0; i<size; ++i)
+       {
+               unsigned n = i;
+               for(unsigned j=i+1; j<size; ++j)
+                       if(points[j].distance<points[n].distance)
+                               n = j;
+               if(n!=i)
+                       std::swap(points[i], points[n]);
+       }
+}
+
 } // namespace Geometry
 } // namespace Msp