From 61bd97e7947b8600b1c27c85bbedbecd0312d467 Mon Sep 17 00:00:00 2001
From: Mikel Zhobro <zhobromikel@gmail.com>
Date: Fri, 22 Apr 2022 10:14:43 +0200
Subject: [PATCH] Capsule-sphere collides at closest point on segmnt.

tiny_vector3.h: change api according to Eigen

geometry.hpp: Capsule can compute position of tip & base given the pose
---
 src/contact_point.hpp        | 35 ++++++++++++++++++++++-------------
 src/geometry.hpp             | 11 +++++++++++
 src/math/tiny/tiny_vector3.h |  2 ++
 3 files changed, 35 insertions(+), 13 deletions(-)

diff --git a/src/contact_point.hpp b/src/contact_point.hpp
index 9edbd2ab..67efef51 100644
--- a/src/contact_point.hpp
+++ b/src/contact_point.hpp
@@ -402,6 +402,19 @@ int contact_sphere_box(const tds::Geometry<Algebra>* geomA,
   return 1;
 }
 
+template <typename Algebra>
+void get_closest_point_on_segment(const typename Algebra::Vector3& A,
+                                  const typename Algebra::Vector3& B,
+                                  const typename Algebra::Vector3& Point,
+                                  typename Algebra::Vector3& closestPoint) {
+  using Scalar = typename Algebra::Scalar;
+  using Vector3 = typename Algebra::Vector3;
+
+  Vector3 ab = B - A;
+  Scalar t = ab.dot(Point - A) / ab.squaredNorm();
+  closestPoint = A + ::tds::clamp(t, Algebra::zero(), Algebra::one()) * ab;
+}
+
 template <typename Algebra>
 int contact_capsule_sphere(const tds::Geometry<Algebra>* geomA,
                            const tds::Pose<Algebra>& poseA,
@@ -418,23 +431,19 @@ int contact_capsule_sphere(const tds::Geometry<Algebra>* geomA,
   assert(geomA->get_type() == TINY_CAPSULE_TYPE);
   assert(geomB->get_type() == TINY_SPHERE_TYPE);
   const Capsule* capsule = (const Capsule*)geomA;
-
   Sphere sphere(capsule->get_radius());
-  // shift the sphere to each end-point
+
   Pose offset;
   Algebra::set_identity(offset.orientation_);
-  offset.position_ = Vector3(Algebra::zero(), Algebra::zero(),
-                             Algebra::fraction(1, 2) * capsule->get_length());
-  Pose poseEndSphere = poseA * offset;
-  contact_sphere_sphere<Algebra>(&sphere, poseEndSphere, geomB, poseB,
-                                 contactsOut);
-  offset.position_ = Vector3(Algebra::zero(), Algebra::zero(),
-                             Algebra::fraction(-1, 2) * capsule->get_length());
-  poseEndSphere = poseA * offset;
-  contact_sphere_sphere<Algebra>(&sphere, poseEndSphere, geomB, poseB,
-                                 contactsOut);
 
-  return 2;
+  Pose poseTipSphere = capsule->get_tip_pose(poseA);
+  Pose poseBaseSphere = capsule->get_base_pose(poseA);
+  get_closest_point_on_segment<Algebra>(poseTipSphere.position_,
+                                        poseBaseSphere.position_,
+                                        poseB.position_, offset.position_);
+
+  contact_sphere_sphere<Algebra>(&sphere, offset, geomB, poseB, contactsOut);
+  return 1;
 }
 
 
diff --git a/src/geometry.hpp b/src/geometry.hpp
index 20c0d0f2..6de89573 100644
--- a/src/geometry.hpp
+++ b/src/geometry.hpp
@@ -105,6 +105,7 @@ template <typename Algebra>
 class Capsule : public Geometry<Algebra> {
   using Scalar = typename Algebra::Scalar;
   using Vector3 = typename Algebra::Vector3;
+  typedef tds::Pose<Algebra> Pose;
 
   Scalar radius;
   Scalar length;
@@ -149,6 +150,16 @@ class Capsule : public Geometry<Algebra> {
                                   this->length / Scalar(2)));
     return Algebra::norm(pt) - this->radius;
   }
+
+  Pose _get_t_b_pose(const Pose &pose, double tip) const {
+    Pose offset;
+    Algebra::set_identity(offset.orientation_);
+    offset.position_ = Vector3(Algebra::zero(), Algebra::zero(),
+                               Algebra::fraction(tip, 2) * get_length());
+    return pose * offset;
+  }
+  Pose get_tip_pose(const Pose &pose) const { return _get_t_b_pose(pose, 1); }
+  Pose get_base_pose(const Pose &pose) const { return _get_t_b_pose(pose, -1); }
 };
 
 template <typename Algebra>
diff --git a/src/math/tiny/tiny_vector3.h b/src/math/tiny/tiny_vector3.h
index d5d49320..0b599134 100644
--- a/src/math/tiny/tiny_vector3.h
+++ b/src/math/tiny/tiny_vector3.h
@@ -144,6 +144,7 @@ namespace TINY
             res = TinyConstants::sqrt1(res);
             return res;
         }
+        inline TinyScalar norm() const { return length(); }
 
         inline TinyScalar length_squared() const {
             TinyScalar res = (*this).dot(*this);
@@ -159,6 +160,7 @@ namespace TINY
         }
 
         inline TinyScalar sqnorm() const { return m_x * m_x + m_y * m_y + m_z * m_z; }
+        inline TinyScalar squaredNorm() const { return sqnorm(); }
 
         inline TinyVector3& operator+=(const TinyVector3& v) {
             m_x += v.m_x;