summaryrefslogtreecommitdiffhomepage
diff options
context:
space:
mode:
authorStanislaw Halik <sthalik@misaki.pl>2015-06-01 17:44:07 +0200
committerStanislaw Halik <sthalik@misaki.pl>2015-06-01 17:44:07 +0200
commit277266b9970d0cdd19734d98b953d87ad585c749 (patch)
treeff54495638a22b0227502cdab1f518661c634eef
parentfc03209f0819e2d247f0b24ed02842fc5505ef73 (diff)
simple-mat: implement dot and cross product
-rw-r--r--opentrack/simple-mat.hpp37
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;