a/math: Cleanup motion estimation API and gracefully handle zero dt

Part-of: <https://gitlab.freedesktop.org/monado/monado/-/merge_requests/2613>

src/xrt/auxiliary/math/m_relation_history.cpp

···
 	}
 }
 
-bool
-m_relation_history_estimate_motion(struct m_relation_history *rh,
-                                   const struct xrt_space_relation *in_relation,
-                                   int64_t timestamp,
-                                   struct xrt_space_relation *out_relation)
+static void
+m_relation_history_estimate_motion(struct xrt_space_relation const &old_relation,
+                                   struct xrt_space_relation const &new_relation,
+                                   float dt,
+                                   struct xrt_vec3 &out_linear_velocity,
+                                   struct xrt_vec3 &out_angular_velocity,
+                                   enum xrt_space_relation_flags &out_flags)
 {
+	assert(dt != 0.0f);
 
-	int64_t last_time_ns;
-	struct xrt_space_relation last_relation;
-	if (!m_relation_history_get_latest(rh, &last_time_ns, &last_relation)) {
-		return false;
-	};
+	enum xrt_space_relation_flags shared_flags =
+	    (enum xrt_space_relation_flags)(old_relation.relation_flags & new_relation.relation_flags);
 
-	float dt = (float)time_ns_to_s(timestamp - last_time_ns);
+	// If both relations have position data, estimate linear velocity
+	if (shared_flags & XRT_SPACE_RELATION_POSITION_VALID_BIT) {
+		out_flags = (enum xrt_space_relation_flags)(out_flags | XRT_SPACE_RELATION_LINEAR_VELOCITY_VALID_BIT);
 
-	// 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);
+		out_linear_velocity = (new_relation.pose.position - old_relation.pose.position) / dt;
+	}
 
-		outf = (enum xrt_space_relation_flags)(outf | XRT_SPACE_RELATION_LINEAR_VELOCITY_VALID_BIT);
+	// If both relations have orientation data, estimate angular velocity
+	if (shared_flags & XRT_SPACE_RELATION_ORIENTATION_VALID_BIT) {
+		out_flags = (enum xrt_space_relation_flags)(out_flags | XRT_SPACE_RELATION_ANGULAR_VELOCITY_VALID_BIT);
 
-		out_relation->linear_velocity = (in_relation->pose.position - last_relation.pose.position) / dt;
+		math_quat_finite_difference(&old_relation.pose.orientation, &new_relation.pose.orientation, dt,
+		                            &out_angular_velocity);
 	}
+}
+
+bool
+m_relation_history_push_with_motion_estimation(struct m_relation_history *rh,
+                                               struct xrt_space_relation const *in_relation,
+                                               int64_t timestamp)
+{
+	assert((in_relation->relation_flags & XRT_SPACE_RELATION_LINEAR_VELOCITY_VALID_BIT) == 0);
+	assert((in_relation->relation_flags & XRT_SPACE_RELATION_ANGULAR_VELOCITY_VALID_BIT) == 0);
 
-	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);
+	struct xrt_space_relation final_relation = *in_relation;
 
-		outf = (enum xrt_space_relation_flags)(outf | XRT_SPACE_RELATION_ANGULAR_VELOCITY_VALID_BIT);
+	int64_t last_time_ns;
+	struct xrt_space_relation last_relation;
+	if (m_relation_history_get_latest(rh, &last_time_ns, &last_relation) && timestamp > last_time_ns) {
+		float dt = (float)time_ns_to_s(timestamp - last_time_ns);
 
-		math_quat_finite_difference(&last_relation.pose.orientation, &in_relation->pose.orientation, dt,
-		                            &out_relation->angular_velocity);
+		m_relation_history_estimate_motion(last_relation, *in_relation, dt, final_relation.linear_velocity,
+		                                   final_relation.angular_velocity, final_relation.relation_flags);
 	}
 
-	out_relation->pose = in_relation->pose;
-
-	return true;
+	return m_relation_history_push(rh, &final_relation, timestamp);
 }
 
 bool

src/xrt/auxiliary/math/m_relation_history.h

···
 m_relation_history_push(struct m_relation_history *rh, struct xrt_space_relation const *in_relation, int64_t timestamp);
 
 /*!
- * Interpolates or extrapolates to the desired timestamp.
+ * Pushes a new pose to the history, estimating linear and angular velocity based on the previous entry.
  *
- * Read-only operation - doesn't remove anything from the buffer or anything like that - you can call this as often as
- * you want.
+ * If the history is full, it will also pop a pose out of the other side of the buffer.
+ *
+ * @return false if the timestamp is earlier than the most recent timestamp already recorded
  *
  * @public @memberof m_relation_history
  */
-enum m_relation_history_result
-m_relation_history_get(const struct m_relation_history *rh,
-                       int64_t at_timestamp_ns,
-                       struct xrt_space_relation *out_relation);
+bool
+m_relation_history_push_with_motion_estimation(struct m_relation_history *rh,
+                                               struct xrt_space_relation const *in_relation,
+                                               int64_t timestamp);
 
 /*!
- * 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).
+ * Interpolates or extrapolates to the desired timestamp.
  *
- * 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.
+ * Read-only operation - doesn't remove anything from the buffer or anything like that - you can call this as often
+ * as you want.
  *
  * @public @memberof m_relation_history
  */
-bool
-m_relation_history_estimate_motion(struct m_relation_history *rh,
-                                   const struct xrt_space_relation *in_relation,
-                                   int64_t timestamp,
-                                   struct xrt_space_relation *out_relation);
+enum m_relation_history_result
+m_relation_history_get(const struct m_relation_history *rh,
+                       int64_t at_timestamp_ns,
+                       struct xrt_space_relation *out_relation);
 
 /*!
  * Get the latest report in the buffer, if any.

src/xrt/drivers/hydra/hydra_driver.c

···
 	    (XRT_SPACE_RELATION_ORIENTATION_TRACKED_BIT | XRT_SPACE_RELATION_ORIENTATION_VALID_BIT) |
 	    (XRT_SPACE_RELATION_POSITION_TRACKED_BIT | XRT_SPACE_RELATION_POSITION_VALID_BIT);
 
-	m_relation_history_estimate_motion(state->relation_history, &space_relation, now, &space_relation);
-
-	m_relation_history_push(state->relation_history, &space_relation, now);
+	m_relation_history_push_with_motion_estimation(state->relation_history, &space_relation, now);
 
 	state->buttons = hydra_read_uint8(&buf);
 

src/xrt/tracking/hand/t_hand_tracking_async.c

···
 			struct xrt_space_relation wrist_rel =
 			    hta->working.hands[i].values.hand_joint_set_default[XRT_HAND_JOINT_WRIST].relation;
 
-			m_relation_history_estimate_motion( //
-			    hta->present.relation_hist[i],  //
-			    &wrist_rel,                     //
-			    hta->working.timestamp,         //
-			    &wrist_rel);                    //
-
-			m_relation_history_push(           //
-			    hta->present.relation_hist[i], //
-			    &wrist_rel,                    //
-			    hta->working.timestamp);       //
+			m_relation_history_push_with_motion_estimation( //
+			    hta->present.relation_hist[i],              //
+			    &wrist_rel,                                 //
+			    hta->working.timestamp);                    //
 		}
 
 		hta->hand_tracking_work_active = false;