feat: get the first closest point

Author: oktomus

src/core/closest_point_query.cpp

@@ -2,9 +2,10 @@
 
 #include "math.h"
 
+#include <glm/gtc/type_ptr.hpp>
+
 #include <chrono>
 #include <cstddef>
-#include <inttypes.h>
 #include <iostream>
 #include <limits>
 #include <vector>
@@ -14,14 +15,20 @@ namespace core
 
 ClosestPointQuery::ClosestPointQuery(const MeshPointCloud& mesh_point_cloud)
   : m_mesh_point_cloud(mesh_point_cloud)
+  , m_tree_index(
+    3,
+    mesh_point_cloud,
+    nanoflann::KDTreeSingleIndexAdaptorParams(10 /* tree leaf max size */))
 {
     // Start a timer to know how long it takes to build the query object.
     auto timer_start = std::chrono::high_resolution_clock::now();
 
+    m_tree_index.buildIndex();
+
     auto timer_stop = std::chrono::high_resolution_clock::now();
     auto process_time = std::chrono::duration_cast<std::chrono::milliseconds>(timer_stop - timer_start).count();
 
-    std::printf("Generated mesh query tree in %" PRId64 "ms.", process_time);
+    std::cout << "Generated mesh query tree in " << process_time << "ms.\n";
 }
 
 bool ClosestPointQuery::get_closest_point(
@@ -31,43 +38,38 @@ bool ClosestPointQuery::get_closest_point(
 {
     assert(max_distance > 0.0f);
 
-    const std::size_t triangle_count = m_mesh_point_cloud.kdtree_get_point_count() / 3;
-
-    //=> Naive implementation.
-    // 1. Go through each triangle
-    // 2. Compute the closest point on a particular mesh
-    // 3. Keep the closest
-    float closest_distance2 = std::numeric_limits<float>::max();
-    result = { 0, 0, 0 };
-    bool found = false;
-
-    for (std::size_t triangle_index = 0; triangle_index < triangle_count; ++triangle_index)
-    {
-        const glm::vec3& v1 = m_mesh_point_cloud.kdtree_get_pt(triangle_index * 3);
-        const glm::vec3& v2 = m_mesh_point_cloud.kdtree_get_pt(triangle_index * 3 + 1);
-        const glm::vec3& v3 = m_mesh_point_cloud.kdtree_get_pt(triangle_index * 3 + 2);
-
-        bool success = false;
-        const glm::vec3 p = closest_point_in_triangle(
-            query_point,
-            v1,
-            v2,
-            v3);
-
-        const float distance2_to_triangle = distance2(p, query_point);
-
-        if (distance2_to_triangle > max_distance)
-            continue;
-
-        if (distance2_to_triangle < closest_distance2)
-        {
-            closest_distance2 = distance2_to_triangle;
-            result = p;
-            found = true;
-        }
-    }
-
-    return found;
+    static nanoflann::SearchParams search_params(
+        32,    // ignored param
+        0.0f,  // epsilon
+        true); // true means we want results to be sorted by squared dist
+
+    std::vector<std::size_t> ret_index(1);
+    std::vector<float> out_dist_sqr(1);
+    const std::size_t num_results =
+        m_tree_index.knnSearch(
+            glm::value_ptr(query_point),
+            1, // We only want 1 result, the closest one.
+            &ret_index[0],
+            &out_dist_sqr[0]);
+
+    if (num_results == 0)
+        return false;
+
+    glm::vec3 v1, v2, v3;
+    m_mesh_point_cloud.get_triangle(ret_index[0], v1, v2, v3);
+
+    result = closest_point_in_triangle(
+        query_point,
+        v1,
+        v2,
+        v3);
+
+    const float distance2_to_triangle = distance2(result, query_point);
+
+    if (distance2_to_triangle > max_distance)
+        return false;;
+
+    return true;
 }
 
 } // namespace core

src/core/closest_point_query.h

@@ -34,6 +34,16 @@ class ClosestPointQuery
 
   private:
     const MeshPointCloud& m_mesh_point_cloud;
+
+    // nanoflann kdtree. Will be used to speed up look up time.
+    typedef nanoflann::KDTreeSingleIndexAdaptor<
+        nanoflann::L2_Simple_Adaptor<float, MeshPointCloud>,
+        MeshPointCloud,
+        3, /* Go 3D! */
+        size_t> TreeIndex;
+    TreeIndex m_tree_index;
+
+    typedef std::vector<std::pair<std::size_t, float>> TreeSearchVectorResult;
 };
 
 } // namespace core

src/core/mesh_point_cloud.h

@@ -21,6 +21,42 @@ class MeshPointCloud
   public:
     MeshPointCloud(const Mesh& mesh);
 
+    inline void get_triangle(
+        const std::size_t   idx,
+        glm::vec3&          v1,
+        glm::vec3&          v2,
+        glm::vec3&          v3) const
+    {
+        // The index we receive is a vertex index.
+        // Since each triangle is made of 3 vertex
+        // and we don't share vertices in the point cloud,
+        // we can deduce the v1, v2 and v3 easily.
+        // TODO: once more points are added in the cloud
+        // other than the vertices, this won't work anymore.
+        if (idx % 3 == 0)
+        {
+            v1 = m_points[idx];
+            v2 = m_points[idx + 1];
+            v3 = m_points[idx + 2];
+        }
+        else if ((idx - 1) % 3 == 0)
+        {
+            v1 = m_points[idx - 1];
+            v2 = m_points[idx];
+            v3 = m_points[idx + 1];
+        }
+        else if ((idx - 2) % 3 == 0)
+        {
+            v1 = m_points[idx - 2];
+            v2 = m_points[idx - 1];
+            v3 = m_points[idx];
+        }
+        else
+        {
+            assert(false);
+        }
+    }
+
     // nanoflann compatibility implementaiton.
     inline std::size_t kdtree_get_point_count() const
     {