From 543a436c9963d00619a2754b87a189c18d1ddf27 Mon Sep 17 00:00:00 2001
From: Moses Turner <moses@collabora.com>
Date: Sat, 18 Jun 2022 21:49:56 +0100
Subject: [PATCH] m/relation_history: Add motion estimation

---
 src/xrt/auxiliary/math/m_relation_history.cpp | 47 +++++++++++++++++++
 src/xrt/auxiliary/math/m_relation_history.h   | 16 +++++++
 2 files changed, 63 insertions(+)

diff --git a/src/xrt/auxiliary/math/m_relation_history.cpp b/src/xrt/auxiliary/math/m_relation_history.cpp
index 7d5892bf1..e10737fb5 100644
--- a/src/xrt/auxiliary/math/m_relation_history.cpp
+++ b/src/xrt/auxiliary/math/m_relation_history.cpp
@@ -174,6 +174,53 @@ m_relation_history_get(struct m_relation_history *rh, uint64_t at_timestamp_ns,
 	}
 }
 
+bool
+m_relation_history_estimate_motion(struct m_relation_history *rh,
+                                   const struct xrt_space_relation *in_relation,
+                                   uint64_t timestamp,
+                                   struct xrt_space_relation *out_relation)
+{
+
+	uint64_t last_time_ns;
+	struct xrt_space_relation last_relation;
+	if (!m_relation_history_get_latest(rh, &last_time_ns, &last_relation)) {
+		return false;
+	};
+
+	float dt = time_ns_to_s(timestamp - last_time_ns);
+
+	// Used to find out what values are valid in both the old relation and the new relation
+	enum xrt_space_relation_flags tmp_flags =
+	    (enum xrt_space_relation_flags)(last_relation.relation_flags & in_relation->relation_flags);
+
+	// Brevity
+	enum xrt_space_relation_flags &outf = out_relation->relation_flags;
+
+
+	if (tmp_flags & XRT_SPACE_RELATION_POSITION_VALID_BIT) {
+		outf = (enum xrt_space_relation_flags)(outf | XRT_SPACE_RELATION_POSITION_VALID_BIT);
+		outf = (enum xrt_space_relation_flags)(outf | XRT_SPACE_RELATION_POSITION_TRACKED_BIT);
+
+		outf = (enum xrt_space_relation_flags)(outf | XRT_SPACE_RELATION_LINEAR_VELOCITY_VALID_BIT);
+
+		out_relation->linear_velocity = (in_relation->pose.position - last_relation.pose.position) / dt;
+	}
+
+	if (tmp_flags & XRT_SPACE_RELATION_ORIENTATION_VALID_BIT) {
+		outf = (enum xrt_space_relation_flags)(outf | XRT_SPACE_RELATION_ORIENTATION_VALID_BIT);
+		outf = (enum xrt_space_relation_flags)(outf | XRT_SPACE_RELATION_ORIENTATION_TRACKED_BIT);
+
+		outf = (enum xrt_space_relation_flags)(outf | XRT_SPACE_RELATION_ANGULAR_VELOCITY_VALID_BIT);
+
+		math_quat_finite_difference(&last_relation.pose.orientation, &in_relation->pose.orientation, dt,
+		                            &out_relation->angular_velocity);
+	}
+
+	out_relation->pose = in_relation->pose;
+
+	return true;
+}
+
 bool
 m_relation_history_get_latest(struct m_relation_history *rh,
                               uint64_t *out_time_ns,
diff --git a/src/xrt/auxiliary/math/m_relation_history.h b/src/xrt/auxiliary/math/m_relation_history.h
index 6eabcb5a8..3e6e84def 100644
--- a/src/xrt/auxiliary/math/m_relation_history.h
+++ b/src/xrt/auxiliary/math/m_relation_history.h
@@ -71,6 +71,22 @@ m_relation_history_get(struct m_relation_history *rh,
                        uint64_t at_timestamp_ns,
                        struct xrt_space_relation *out_relation);
 
+/*!
+ * Estimates the movement (velocity and angular velocity) of a new relation based on
+ * the latest relation found in the buffer (as returned by m_relation_history_get_latest).
+ *
+ * Read-only on m_relation_history and in_relation.
+ * Copies in_relation->pose to out_relation->pose, and writes new flags and linear/angular velocities to
+ * out_relation->pose. OK to alias in_relation and out_relation.
+ *
+ * @public @memberof m_relation_history
+ */
+bool
+m_relation_history_estimate_motion(struct m_relation_history *rh,
+                                   const struct xrt_space_relation *in_relation,
+                                   uint64_t timestamp,
+                                   struct xrt_space_relation *out_relation);
+
 /*!
  * Get the latest report in the buffer, if any.
  *