From b2c16e223e41ebad6c3963b9b08be3f3c92bde6e Mon Sep 17 00:00:00 2001
From: Gianmarco Cherchi <gianmarcher@gmail.com>
Date: Thu, 26 Oct 2023 17:08:02 +0200
Subject: [PATCH] point_segment_sqrd_dist added to segment_utils
---
include/cinolib/geometry/segment_utils.cpp | 29 ++++++++++++++++++++++
include/cinolib/geometry/segment_utils.h | 13 ++++++++++
2 files changed, 42 insertions(+)
diff --git a/include/cinolib/geometry/segment_utils.cpp b/include/cinolib/geometry/segment_utils.cpp
index 026adac4..7e85e0d5 100644
--- 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
index 76024dfb..4ad5b324 100644
--- 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