From 6c3754a3fa40d32485503183292f6fb165f42bd9 Mon Sep 17 00:00:00 2001
From: David Allemang <david.allemang@kitware.com>
Date: Sat, 4 Feb 2023 13:35:52 -0500
Subject: [PATCH] ENH: Introduce rotor() helper

---
 vis/include/mirror.hpp | 18 ++++++++++++++++++
 1 file changed, 18 insertions(+)

diff --git a/vis/include/mirror.hpp b/vis/include/mirror.hpp
index d624717..8bd2194 100644
--- a/vis/include/mirror.hpp
+++ b/vis/include/mirror.hpp
@@ -100,3 +100,21 @@ mat<N> rot(int u, int v, float theta) {
     res(v, v) = std::cos(theta);
     return res;
 }
+
+template<class U, class V>
+auto rotor(U const &u, V const &v) {
+    using Rotor = Eigen::Matrix<
+        typename U::Scalar,
+        std::min(U::RowsAtCompileTime, V::RowsAtCompileTime),
+        std::min(U::RowsAtCompileTime, V::RowsAtCompileTime),
+        U::Options,
+        std::max(U::MaxRowsAtCompileTime, V::MaxRowsAtCompileTime),
+        std::max(U::MaxRowsAtCompileTime, V::MaxRowsAtCompileTime)
+    >;
+
+    const auto &ident = Rotor::Identity(u.rows(), v.rows());
+    const auto &inner = (u + v) / (1 + u.dot(v)) * (u + v).transpose();
+    const auto &outer = v * u.transpose();
+
+    return ident - inner + 2 * outer;
+}