From cf5574a4e9bde4e77f00257b4d3dcdd88a5d7343 Mon Sep 17 00:00:00 2001
From: Mateo de Mayo <mateo.demayo@collabora.com>
Date: Thu, 30 Dec 2021 17:30:37 -0300
Subject: [PATCH] m/space: Add m_space_relation_interpolate

---
 src/xrt/auxiliary/math/m_space.cpp | 23 +++++++++++++++++++++++
 src/xrt/auxiliary/math/m_space.h   | 11 +++++++++++
 2 files changed, 34 insertions(+)

diff --git a/src/xrt/auxiliary/math/m_space.cpp b/src/xrt/auxiliary/math/m_space.cpp
index 05007334c..02f531124 100644
--- a/src/xrt/auxiliary/math/m_space.cpp
+++ b/src/xrt/auxiliary/math/m_space.cpp
@@ -29,6 +29,29 @@ m_space_relation_invert(struct xrt_space_relation *relation, struct xrt_space_re
 	out_relation->angular_velocity = m_vec3_mul_scalar(relation->angular_velocity, -1);
 }
 
+extern "C" void
+m_space_relation_interpolate(struct xrt_space_relation *a,
+                             struct xrt_space_relation *b,
+                             float t,
+                             enum xrt_space_relation_flags flags,
+                             struct xrt_space_relation *out_relation)
+{
+	out_relation->relation_flags = flags;
+
+	if (flags & XRT_SPACE_RELATION_ORIENTATION_VALID_BIT) {
+		math_quat_slerp(&a->pose.orientation, &b->pose.orientation, t, &out_relation->pose.orientation);
+	}
+	if (flags & XRT_SPACE_RELATION_POSITION_VALID_BIT) {
+		out_relation->pose.position = m_vec3_lerp(a->pose.position, b->pose.position, t);
+	}
+	if (flags & XRT_SPACE_RELATION_LINEAR_VELOCITY_VALID_BIT) {
+		out_relation->linear_velocity = m_vec3_lerp(a->linear_velocity, b->linear_velocity, t);
+	}
+	if (flags & XRT_SPACE_RELATION_ANGULAR_VELOCITY_VALID_BIT) {
+		out_relation->angular_velocity = m_vec3_lerp(a->angular_velocity, b->angular_velocity, t);
+	}
+}
+
 /*
  *
  * Dump functions.
diff --git a/src/xrt/auxiliary/math/m_space.h b/src/xrt/auxiliary/math/m_space.h
index 04f1d09cc..b7c98e345 100644
--- a/src/xrt/auxiliary/math/m_space.h
+++ b/src/xrt/auxiliary/math/m_space.h
@@ -83,6 +83,17 @@ m_space_relation_ident(struct xrt_space_relation *out_relation)
 void
 m_space_relation_invert(struct xrt_space_relation *relation, struct xrt_space_relation *out_relation);
 
+/*!
+ * Linearly interpolate between two relations @p a and @p b. Uses slerp for
+ * their orientations. Sets @p flags in @p out_relation.
+ */
+void
+m_space_relation_interpolate(struct xrt_space_relation *a,
+                             struct xrt_space_relation *b,
+                             float t,
+                             enum xrt_space_relation_flags flags,
+                             struct xrt_space_relation *out_relation);
+
 /*
  *
  * Relation chain functions.