Added vector angle formula

ut-amrl · Jan 14, 2025 · 5f1eeee · 5f1eeee
1 parent 1966da7
commit 5f1eeee
Showing 1 changed file with 9 additions and 0 deletions.
diff --git a/math/geometry.h b/math/geometry.h
@@ -58,6 +58,15 @@ T Cross(const Eigen::Matrix<T, 2, 1>& v1, const Eigen::Matrix<T, 2, 1>& v2) {
   return (v1.x() * v2.y() - v2.x() * v1.y());
 }
 
+// Return the angle between two vectors in radians
+template <typename T>
+float VectorAngle(const Eigen::Matrix<T, 2, 1>& v1, const Eigen::Matrix<T, 2, 1>& v2) {
+  if (v1.norm() < kEpsilon || v2.norm() < kEpsilon) {
+    return 0; // Degenerate case
+  }
+  return acos(v1.dot(v2) / (v1.norm() * v2.norm()));
+}
+
 template <typename T, int N>
 Eigen::Matrix<T, N, 1> GetNormalizedOrZero(const Eigen::Matrix<T, N, 1>& vec) {
   const auto norm = vec.template lpNorm<1>();