From 7f88583d37ed3f91a483c3ed4cf1c9dd14f48cc6 Mon Sep 17 00:00:00 2001
From: Moses Turner <moses@collabora.com>
Date: Wed, 15 Sep 2021 01:30:50 -0500
Subject: [PATCH] d/survive: Use m_relation_history for HMD and controllers

---
 src/xrt/drivers/survive/survive_driver.c | 47 +++++++++++-------------
 1 file changed, 22 insertions(+), 25 deletions(-)

diff --git a/src/xrt/drivers/survive/survive_driver.c b/src/xrt/drivers/survive/survive_driver.c
index d5a77593f..9658d6e4b 100644
--- a/src/xrt/drivers/survive/survive_driver.c
+++ b/src/xrt/drivers/survive/survive_driver.c
@@ -41,6 +41,7 @@
 
 #include "util/u_hand_tracking.h"
 #include "util/u_logging.h"
+#include "math/m_relation_history.h"
 
 #include "math/m_predict.h"
 
@@ -126,8 +127,7 @@ struct survive_device
 	struct survive_system *sys;
 	const SurviveSimpleObject *survive_obj;
 
-	struct xrt_space_relation last_relation;
-	timepoint_ns last_relation_ts;
+	struct m_relation_history *relation_hist;
 
 	//! Number of inputs.
 	size_t num_last_inputs;
@@ -213,6 +213,7 @@ survive_device_destroy(struct xrt_device *xdev)
 		survive_simple_close(survive->sys->ctx);
 		free(survive->sys);
 	}
+	m_relation_history_destroy(&survive->relation_hist);
 
 	free(survive->last_inputs);
 	u_device_free(&survive->base);
@@ -299,20 +300,6 @@ pose_to_relation(const SurvivePose *pose, const SurviveVelocity *vel, struct xrt
 	}
 }
 
-static void
-_predict_pose(struct survive_device *survive, uint64_t at_timestamp_ns, struct xrt_space_relation *out_relation)
-{
-	timepoint_ns prediction_ns = at_timestamp_ns - survive->last_relation_ts;
-	double prediction_s = time_ns_to_s(prediction_ns);
-
-	timepoint_ns monotonic_now_ns = os_monotonic_get_ns();
-	timepoint_ns remaining_ns = at_timestamp_ns - monotonic_now_ns;
-	SURVIVE_TRACE(survive, "dev %s At %ldns: Pose requested for +%ldns (%ldns), predicting %ldns",
-	              survive->base.str, monotonic_now_ns, remaining_ns, at_timestamp_ns, prediction_ns);
-
-	m_predict_relation(&survive->last_relation, prediction_s, out_relation);
-}
-
 static bool
 verify_device_name(struct survive_device *survive, enum xrt_input_name name)
 {
@@ -344,9 +331,7 @@ survive_device_get_tracked_pose(struct xrt_device *xdev,
 		return;
 	}
 
-	os_mutex_lock(&survive->sys->lock);
-	_predict_pose(survive, at_timestamp_ns, out_relation);
-	os_mutex_unlock(&survive->sys->lock);
+	m_relation_history_get(survive->relation_hist, out_relation, at_timestamp_ns);
 
 	struct xrt_pose *p = &out_relation->pose;
 	SURVIVE_TRACE(survive, "GET_POSITION (%f %f %f) GET_ORIENTATION (%f, %f, %f, %f)", p->position.x, p->position.y,
@@ -445,10 +430,15 @@ survive_controller_get_hand_tracking(struct xrt_device *xdev,
 	struct xrt_pose hand_on_handle_pose;
 	u_hand_joints_offset_valve_index_controller(hand, &static_offset, &hand_on_handle_pose);
 
-	u_hand_joints_set_out_data(&survive->ctrl.hand_tracking, hand, &survive->last_relation, &hand_on_handle_pose,
-	                           out_value);
-	out_value->is_active = true; // Apparently libsurvive doesn't report controller tracked/untracked state, so just
-	                             // lie and say that the hand is being tracked
+	struct xrt_space_relation hand_relation;
+
+	m_relation_history_get(survive->relation_hist, &hand_relation, at_timestamp_ns);
+
+	u_hand_joints_set_out_data(&survive->ctrl.hand_tracking, hand, &hand_relation, &hand_on_handle_pose, out_value);
+
+	// This is a lie - apparently libsurvive doesn't report controller tracked/untracked state, so just say that the
+	// hand is being tracked
+	out_value->is_active = true;
 }
 
 static void
@@ -730,8 +720,12 @@ add_device(struct survive_system *ss, const struct SurviveSimpleConfigEvent *e);
 static void
 _process_pose_event(struct survive_device *survive, const struct SurviveSimplePoseUpdatedEvent *e)
 {
-	pose_to_relation(&e->pose, &e->velocity, &survive->last_relation);
-	survive->last_relation_ts = survive_timecode_to_monotonic(e->time);
+	struct xrt_space_relation rel;
+	timepoint_ns ts;
+	pose_to_relation(&e->pose, &e->velocity, &rel);
+	ts = survive_timecode_to_monotonic(e->time);
+	m_relation_history_push(survive->relation_hist, &rel, ts);
+
 	SURVIVE_TRACE(survive, "Process pose event for %s", survive->base.str);
 }
 
@@ -835,6 +829,8 @@ _create_hmd_device(struct survive_system *sys, const struct SurviveSimpleObject
 	survive->base.tracking_origin = &sys->base;
 
 	SURVIVE_INFO(survive, "survive HMD present");
+	m_relation_history_create(&survive->relation_hist);
+
 
 	size_t idx = 0;
 	survive->base.hmd->blend_modes[idx++] = XRT_BLEND_MODE_OPAQUE;
@@ -1037,6 +1033,7 @@ _create_controller_device(struct survive_system *sys,
 	int outputs = 1;
 	struct survive_device *survive = U_DEVICE_ALLOCATE(struct survive_device, flags, inputs, outputs);
 	survive->ctrl.config = *config;
+	m_relation_history_create(&survive->relation_hist);
 
 	sys->controllers[idx] = survive;
 	survive->sys = sys;