summaryrefslogtreecommitdiffhomepage
path: root/compat/simple-mat.hpp
diff options
context:
space:
mode:
authorStanislaw Halik <sthalik@misaki.pl>2017-04-11 11:41:31 +0200
committerStanislaw Halik <sthalik@misaki.pl>2017-04-12 01:47:41 +0200
commit1d828d640e6d33a473e0a3e08e976bb9cb1b9346 (patch)
tree6ea166aa3c2367a0114e4da3212f51e3a5160a33 /compat/simple-mat.hpp
parentd3c55b0cf8b55d5d4b5c2108d3865de20e3137ea (diff)
compat/euler: add incomplete & untested quat code
Diffstat (limited to 'compat/simple-mat.hpp')
-rw-r--r--compat/simple-mat.hpp63
1 files changed, 63 insertions, 0 deletions
diff --git a/compat/simple-mat.hpp b/compat/simple-mat.hpp
index fbd0a75e..906c0969 100644
--- a/compat/simple-mat.hpp
+++ b/compat/simple-mat.hpp
@@ -273,3 +273,66 @@ Mat<num, h_, w_> operator*(const Mat<num, h_, w_>& self, num other)
ret(j, i) = self(j, i) * other;
return ret;
}
+
+template<typename num>
+class Quat : Mat<num, 4, 1>
+{
+ using quat = Quat<num>;
+
+ enum idx { qw, qx, qy, qz };
+
+ static quat _from_array(const num* data)
+ {
+ return quat(data[qw], data[qx], data[qy], data[qz]);
+ }
+
+ inline num elt(idx k) const { return operator()(k); }
+ inline num& elt(idx k) { return operator()(k); }
+public:
+ Quat(num w, num x, num y, num z) : Mat<num, 4, 1>(w, x, y, z)
+ {
+ }
+
+ Quat() : quat(1, 0, 0, 0) {}
+
+ static quat from_vector(const Mat<num, 4, 1>& data)
+ {
+ return _from_array(data);
+ }
+
+ static quat from_vector(const Mat<num, 1, 4>& data)
+ {
+ return _from_array(data);
+ }
+
+ quat normalized() const
+ {
+ const num x = elt(qx), y = elt(qy), z = elt(qz), w = elt(qw);
+ const num inv_n = 1./std::sqrt(x*x + y*y + z*z + w*w);
+
+ return Quat<num>(elt(qw) * inv_n,
+ elt(qx) * inv_n,
+ elt(qy) * inv_n,
+ elt(qz) * inv_n);
+ }
+
+ quat operator*(const quat& q2)
+ {
+ const quat& OTR_RESTRICT q1 = *this;
+ return quat(-q1.x() * q2.x() - q1.y() * q2.y() - q1.z() * q2.z() + q1.w() * q2.w(),
+ q1.x() * q2.w() + q1.y() * q2.z() - q1.z() * q2.y() + q1.w() * q2.x(),
+ -q1.x() * q2.z() + q1.y() * q2.w() + q1.z() * q2.x() + q1.w() * q2.y(),
+ q1.x() * q2.y() - q1.y() * q2.x() + q1.z() * q2.w() + q1.w() * q2.z());
+ }
+
+
+ inline num w() const { return elt(qw); }
+ inline num x() const { return elt(qx); }
+ inline num y() const { return elt(qy); }
+ inline num z() const { return elt(qz); }
+
+ inline num& w() { return elt(qw); }
+ inline num& x() { return elt(qx); }
+ inline num& y() { return elt(qy); }
+ inline num& z() { return elt(qz); }
+};