simple-mat: implement dot and cross product

1 files changed, 37 insertions, 0 deletions

diff --git a/opentrack/simple-mat.hpp b/opentrack/simple-mat.hpp
index 649d7086..9518af90 100644
--- a/opentrack/simple-mat.hpp
+++ b/opentrack/simple-mat.hpp
@@ -1,4 +1,5 @@
 #pragma once
+#include <algorithm>
 #include <initializer_list>
 #include <type_traits>
 
@@ -14,6 +15,23 @@ namespace {
     {
         enum { value = (i == 1 || j == 1) && (i >= min || j >= min) };
     };
+    template<int i1, int j1, int i2, int j2>
+    struct is_vector_pair
+    {
+        enum { value = i1 == i2 && j1 == 1 && j2 == 1 || j1 == j2 && i1 == 1 && i2 == 1 };
+    };
+    template<int i, int j>
+    struct vector_len
+    {
+        enum { value = i > j ? i : j };
+    };
+    template<int a, int b, int c, int d>
+    struct is_dim3
+    {
+        enum { value = a == 1 && c == 1 && b == 3 && d == 3 || a == 3 && c == 3 && b == 1 && d == 1 };
+        enum { P = a == 1 ? 1 : 3 };
+        enum { Q = a == 1 ? 3 : 1 };
+    };
 }
 
 template<typename num, int h_, int w_>
@@ -57,6 +75,25 @@ struct Mat
     template<int P = h_, int Q = w_> typename std::enable_if<maybe_add_swizzle<P, Q, 4>::value, num&>::type
     __inline w() { return operator()(3); }
     
+    template<int R, int S, int P = h_, int Q = w_>
+    typename std::enable_if<is_vector_pair<R, S, P, Q>::value, num>::type
+    __inline dot(const Mat<num, R, S>& p2) const {
+        num ret = 0;
+        constexpr int len = vector_len<R, S>::value;
+        for (int i = 0; i < len; i++)
+            ret += operator()(i) * p2.operator ()(i);
+        return ret;
+    }
+    
+    template<int R, int S, int P = h_, int Q = w_>
+    typename std::enable_if<is_dim3<P, Q, R, S>::value, Mat<num, is_dim3<P, Q, R, S>::P, is_dim3<P, Q, R, S>::Q>>::type
+    __inline cross(const Mat<num, R, S>& p2) const
+    {
+        return Mat<num, R, S>({y() * p2.z() - p2.y() * z(),
+                               p2.x() * z() - x() * p2.z(),
+                               x() * p2.y() - y() * p2.x()});
+    }
+    
     Mat<num, h_, w_> operator+(const Mat<num, h_, w_>& other) const
     {
         Mat<num, h_, w_> ret;