diff options
| -rw-r--r-- | opentrack/simple-mat.hpp | 148 |
1 files changed, 113 insertions, 35 deletions
diff --git a/opentrack/simple-mat.hpp b/opentrack/simple-mat.hpp index 158cb30d..649d7086 100644 --- a/opentrack/simple-mat.hpp +++ b/opentrack/simple-mat.hpp @@ -1,39 +1,117 @@ #pragma once #include <initializer_list> +#include <type_traits> -template<typename num, int h, int w> +namespace { + // last param to fool SFINAE into overloading + template<int i, int j, int ignored> + struct equals + { + enum { value = i == j }; + }; + template<int i, int j, int min> + struct maybe_add_swizzle + { + enum { value = (i == 1 || j == 1) && (i >= min || j >= min) }; + }; +} + +template<typename num, int h_, int w_> struct Mat { - num data[h][w]; + num data[h_][w_]; + + template<int Q = w_> typename std::enable_if<equals<Q, 1, 0>::value, num>::type + __inline operator()(int i) const { return data[i][0]; } + + template<int P = h_> typename std::enable_if<equals<P, 1, 1>::value, num>::type + __inline operator()(int i) const { return data[0][i]; } + + template<int Q = w_> typename std::enable_if<equals<Q, 1, 2>::value, num&>::type + __inline operator()(int i) { return data[i][0]; } + + template<int P = h_> typename std::enable_if<equals<P, 1, 3>::value, num&>::type + __inline operator()(int i) { return data[0][i]; } + + template<int P = h_, int Q = w_> typename std::enable_if<maybe_add_swizzle<P, Q, 1>::value, num>::type + __inline x() const { return operator()(0); } + + template<int P = h_, int Q = w_> typename std::enable_if<maybe_add_swizzle<P, Q, 2>::value, num>::type + __inline y() const { return operator()(1); } - Mat<num, h, w> operator+(const Mat<num, h, w>& other) const + template<int P = h_, int Q = w_> typename std::enable_if<maybe_add_swizzle<P, Q, 3>::value, num>::type + __inline z() const { return operator()(2); } + + template<int P = h_, int Q = w_> typename std::enable_if<maybe_add_swizzle<P, Q, 4>::value, num>::type + __inline w() const { return operator()(3); } + + template<int P = h_, int Q = w_> typename std::enable_if<maybe_add_swizzle<P, Q, 1>::value, num&>::type + __inline x() { return operator()(0); } + + template<int P = h_, int Q = w_> typename std::enable_if<maybe_add_swizzle<P, Q, 2>::value, num&>::type + __inline y() { return operator()(1); } + + template<int P = h_, int Q = w_> typename std::enable_if<maybe_add_swizzle<P, Q, 3>::value, num&>::type + __inline z() { return operator()(2); } + + 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); } + + Mat<num, h_, w_> operator+(const Mat<num, h_, w_>& other) const { - Mat<num, h, w> ret; - for (int j = 0; j < h; j++) + Mat<num, h_, w_> ret; + for (int j = 0; j < h_; j++) for (int i = 0; i < w; i++) ret(j, i) = this->operator ()(j, i) + other(j, i); return ret; } - Mat<num, h, w> operator-(const Mat<num, h, w>& other) const + Mat<num, h_, w_> operator-(const Mat<num, h_, w_>& other) const { - Mat<num, h, w> ret; - for (int j = 0; j < h; j++) - for (int i = 0; i < w; i++) + Mat<num, h_, w_> ret; + for (int j = 0; j < h_; j++) + for (int i = 0; i < w_; i++) ret(j, i) = this->operator ()(j, i) - other(j, i); return ret; } - + + Mat<num, h_, w_> operator+(const num& other) const + { + Mat<num, h_, w_> ret; + for (int j = 0; j < h_; j++) + for (int i = 0; i < w; i++) + ret(j, i) = this->operator ()(j, i) + other; + return ret; + } + + Mat<num, h_, w_> operator-(const num& other) const + { + Mat<num, h_, w_> ret; + for (int j = 0; j < h_; j++) + for (int i = 0; i < w_; i++) + ret(j, i) = this->operator ()(j, i) - other; + return ret; + } + + Mat<num, w_, h_> operator*(const num& other) const + { + Mat<num, h_, w_> ret; + for (int j = 0; j < h_; j++) + for (int i = 0; i < w_; i++) + ret(j, i) = operator()(j, i) * other; + return ret; + } + template<int p> - Mat<num, w, p> operator*(const Mat<num, w, p>& other) const + Mat<num, w_, p> operator*(const Mat<num, w_, p>& other) const { - Mat<num, w, p> ret; - for (int j = 0; j < w; j++) + Mat<num, w_, p> ret; + for (int j = 0; j < w_; j++) for (int i = 0; i < p; i++) { num sum = num(0); - for (int k = 0; k < h; k++) + for (int k = 0; k < h_; k++) sum += data[j][k]*other.data[k][i]; ret.data[j][i] = sum; @@ -42,59 +120,59 @@ struct Mat return ret; } - num operator()(int j, int i) const { return data[j][i]; } - num& operator()(int j, int i) { return data[j][i]; } + __inline num operator()(int j, int i) const { return data[j][i]; } + __inline num& operator()(int j, int i) { return data[j][i]; } Mat(std::initializer_list<num>&& list) { auto iter = list.begin(); - for (int i = 0; i < h; i++) - for (int j = 0; j < w; j++) + for (int i = 0; i < h_; i++) + for (int j = 0; j < w_; j++) data[i][j] = *iter++; } Mat() { - for (int j = 0; j < h; j++) - for (int i = 0; i < w; i++) + for (int j = 0; j < h_; j++) + for (int i = 0; i < w_; i++) data[j][i] = 0; } Mat(const num* mem) { - for (int j = 0; j < h; j++) - for (int i = 0; i < w; i++) - data[j][i] = mem[i*h+j]; + for (int j = 0; j < h_; j++) + for (int i = 0; i < w_; i++) + data[j][i] = mem[i*h_+j]; } // XXX add more operators as needed, third-party dependencies mostly // not needed merely for matrix algebra -sh 20141030 - static Mat<num, h, h> eye() + static Mat<num, h_, h_> eye() { - Mat<num, h, h> ret; - for (int j = 0; j < h; j++) - for (int i = 0; i < w; i++) + Mat<num, h_, h_> ret; + for (int j = 0; j < h_; j++) + for (int i = 0; i < w_; i++) ret.data[j][i] = 0; - for (int i = 0; i < h; i++) + for (int i = 0; i < h_; i++) ret.data[i][i] = 1; return ret; } - Mat<num, w, h> t() const + Mat<num, w_, h_> t() const { - Mat<num, w, h> ret; + Mat<num, w_, h_> ret; - for (int j = 0; j < h; j++) - for (int i = 0; i < w; i++) + for (int j = 0; j < h_; j++) + for (int i = 0; i < w_; i++) ret.data[i][j] = data[j][i]; return ret; } - template<int h_, int w_> using dmat = Mat<double, h_, w_>; + template<int h__, int w__> using dmat = Mat<double, h__, w__>; // http://stackoverflow.com/a/18436193 static dmat<3, 1> rmat_to_euler(const dmat<3, 3>& R) @@ -151,5 +229,5 @@ struct Mat return dmat<3, 3>(foo); } }; - -template<int h, int w> using dmat = Mat<double, h, w>; + +template<int h_, int w_> using dmat = Mat<double, h_, w_>; |