diff --git a/SimoxUtility/math/distance/angle_between.h b/SimoxUtility/math/distance/angle_between.h
index 3bab0ca7c07418aedd5057c653fc4495d04eeca9..bdcc376bd2e57c79f97a3dcbb38fba117c7b4f7d 100644
--- a/SimoxUtility/math/distance/angle_between.h
+++ b/SimoxUtility/math/distance/angle_between.h
@@ -8,18 +8,39 @@
 
 namespace simox::math
 {
-//    template<class D1, class D2> inline
-//    meta::enable_if_vec3_vec3<D1, D2, float>
-//    angle_between_vec3f_vec3f(const Eigen::MatrixBase<D1>& l, const Eigen::MatrixBase<D2>& r)
-//    {
-//        return std::acos(l.normalized().dot(r.normalized()));
-//    }
+    /**
+     * @brief Angle between l and r on the plane defined by normal n
+     * @param l vector one
+     * @param r vector two
+     * @param n normal vector
+     */
+    template<class D1, class D2, class D3> inline
+    std::enable_if_t <
+    simox::meta::is_vec3_v<D1>&&
+    simox::meta::is_vec3_v<D2>&&
+    simox::meta::is_vec3_v<D3>, float >
+    angle_between_vec3f_vec3f(const Eigen::MatrixBase<D1>& l,
+                              const Eigen::MatrixBase<D2>& r,
+                              const Eigen::MatrixBase<D3>& n)
+    {
+        const Eigen::Vector3f ln = l.normalized();
+        const Eigen::Vector3f rn = r.normalized();
+        const Eigen::Vector3f nn = n.normalized();
+        return -std::atan2(rn.cross(ln).dot(nn), ln.dot(rn));
+    }
+
+    template<class D1, class D2> inline
+    meta::enable_if_vec3_vec3<D1, D2, float>
+    cos_angle_between_vec3f_vec3f(const Eigen::MatrixBase<D1>& l, const Eigen::MatrixBase<D2>& r)
+    {
+        return std::max(-1.f, std::min(1.f, l.normalized().dot(r.normalized())));
+    }
 
     template<class D1, class D2> inline
     meta::enable_if_vec3_vec3<D1, D2, float>
     angle_between_vec3f_vec3f_abs(const Eigen::MatrixBase<D1>& l, const Eigen::MatrixBase<D2>& r)
     {
-        return std::acos(l.normalized().dot(r.normalized()));
+        return std::acos(cos_angle_between_vec3f_vec3f(l, r));
     }
 }