flush and push

1 files changed, 66 insertions, 0 deletions

diff --git a/facetracknoir/pose.hpp b/facetracknoir/pose.hpp
new file mode 100644
index 00000000..ec9faaa3
--- /dev/null
+++ b/facetracknoir/pose.hpp
@@ -0,0 +1,66 @@
+#pragma once
+
+#include <utility>
+#include <algorithm>
+#include "./quat.hpp"
+#include "./plugin-api.hpp"
+
+class Pose {
+private:
+    static constexpr double pi = 3.141592653;
+    static constexpr double d2r = pi/180.0;
+    static constexpr double r2d = 180./pi;
+
+    double axes[6];
+public:
+    Pose() : axes {0,0,0, 0,0,0 } {}
+
+    inline operator double*() { return axes; }
+    inline operator const double*() const { return axes; }
+
+    inline double& operator()(int i) { return axes[i]; }
+    inline double operator()(int i) const { return axes[i]; }
+
+    Quat quat() const
+    {
+        return Quat(axes[Yaw]*d2r, axes[Pitch]*d2r, axes[Roll]*d2r);
+    }
+
+    static Pose fromQuat(const Quat& q)
+    {
+        Pose ret;
+        q.to_euler_degrees(ret(Yaw), ret(Pitch), ret(Roll));
+        return ret;
+    }
+
+    Pose operator-(const Pose& B) const
+    {
+        const Quat q = (quat() * B.quat().inv());
+        Pose ret = fromQuat(q);
+        for (int i = TX; i < Yaw; i++)
+            ret(i) = B(i);
+        return ret;
+    }
+
+    Pose operator+(const Pose& B) const
+    {
+        const Quat q = (quat() * B.quat().inv());
+        Pose ret = fromQuat(q);
+        for (int i = TX; i < Yaw; i++)
+            ret(i) = B(i);
+        return ret;
+    }
+
+    Pose operator|(const Pose& replacement) const
+    {
+        Pose ret = *this;
+        for (int i = 0; i < 6; i++)
+        {
+            static constexpr double eps = 1e-5;
+            // NB replace zero-valued elements with argument's
+            if (std::abs(ret(i)) < eps)
+                ret(i) = replacement(i);
+        }
+        return ret;
+    }
+};