From 5f1eeeef7b9c151686fb2230c8a9ccf01c72a096 Mon Sep 17 00:00:00 2001
From: artzha <arthurz@cs.utexas.edu>
Date: Tue, 14 Jan 2025 08:28:01 -0600
Subject: [PATCH] Added vector angle formula

---
 math/geometry.h | 9 +++++++++
 1 file changed, 9 insertions(+)

diff --git a/math/geometry.h b/math/geometry.h
index f7f1d65..7d356b1 100644
--- 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>();