Merge pull request #172 from gcherchi/master

point_segment_sqrd_dist added to segment_utils
mlivesu · Oct 26, 2023 · 8c30883 · 8c30883
2 parents 650c817 + b2c16e2
commit 8c30883
Show file tree

Hide file tree

Showing 2 changed files with 42 additions and 0 deletions.
diff --git a/include/cinolib/geometry/segment_utils.cpp b/include/cinolib/geometry/segment_utils.cpp
@@ -137,4 +137,33 @@ mat<3,1,T> segment_intersection(const mat<3,1,T> & s00,
     return p;
 }
 
+//::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::
+
+/* Given a segments S(s00,s01) and a point p, returns the squared distance between point p and segment S
+ * Ref: Real Time Collision Detection, Section 5.1.2.1
+ */
+
+template<class T>
+CINO_INLINE
+T point_segment_sqrd_dist(const mat<3,1,T> & s00, //a
+                          const mat<3,1,T> & s01, //b
+                          const mat<3,1,T> & p)
+{
+    mat<3,1,T> ab = s01 - s00;
+    mat<3,1,T> ac = p - s00;
+    mat<3,1,T> bc = p - s01;
+    T e = ac.dot(ab);
+
+    // Handle cases where p projects outside S
+    if (e <= 0.0f)
+        return ac.dot(ac);
+
+    T f = ab.dot(ab);
+    if (e >= f)
+        return bc.dot(bc);
+
+    // Handle cases where p projects onto S
+    return ac.dot(ac) - (e * e) / f;
+}
+
 }
diff --git a/include/cinolib/geometry/segment_utils.h b/include/cinolib/geometry/segment_utils.h
@@ -73,8 +73,21 @@ mat<3,1,T> segment_intersection(const mat<3,1,T> & s00,
                                 const mat<3,1,T> & s01,
                                 const mat<3,1,T> & s10,
                                 const mat<3,1,T> & s11);
+
+//::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::
+
+/* Given a segments S(s00,s01) and a point p, returns the squared distance between point p and segment S
+ * Ref: Real Time Collision Detection, Section 5.1.2.1
+ */
+
+    template<class T>
+    CINO_INLINE
+    T point_segment_sqrd_dist(const mat<3,1,T> & s00,
+                              const mat<3,1,T> & s01,
+                              const mat<3,1,T> & p);
 }
 
+
 #ifndef  CINO_STATIC_LIB
 #include "segment_utils.cpp"
 #endif