From 3f2b71a9d5c75206532646bd26de75cbf8e1f9af Mon Sep 17 00:00:00 2001
From: Moshi Turner <moses@collabora.com>
Date: Mon, 6 Mar 2023 20:05:01 -0600
Subject: [PATCH] h/mercury: Use post-rotation residual that'll handle big
 rotations

Fixes a long-standing bug
---
 .../hand/mercury/kine_lm/lm_defines.hpp       | 40 +++++++++++++++----
 .../tracking/hand/mercury/kine_lm/lm_main.cpp | 25 ++++++++++--
 2 files changed, 53 insertions(+), 12 deletions(-)

diff --git a/src/xrt/tracking/hand/mercury/kine_lm/lm_defines.hpp b/src/xrt/tracking/hand/mercury/kine_lm/lm_defines.hpp
index 896116c21..a5c66ce13 100644
--- a/src/xrt/tracking/hand/mercury/kine_lm/lm_defines.hpp
+++ b/src/xrt/tracking/hand/mercury/kine_lm/lm_defines.hpp
@@ -289,18 +289,42 @@ template <typename Scalar> struct Vec3
 		return Vec3(0.f, 0.f, 0.f);
 	}
 
-	// Norm, vector length, whatever.
 	Scalar
-	norm()
+	norm_sqrd() const
 	{
-		Scalar len = (Scalar)(0);
+		Scalar len_sqrd = (Scalar)(0);
 
-		len += this->x * this->x;
-		len += this->y * this->y;
-		len += this->z * this->z;
+		len_sqrd += this->x * this->x;
+		len_sqrd += this->y * this->y;
+		len_sqrd += this->z * this->z;
+		return len_sqrd;
+	}
 
-		len = sqrt(len);
-		return len;
+	// Norm, vector length, whatever.
+	// WARNING: Can return NaNs in the derivative part of Jets if magnitude is 0, because d/dx(sqrt(x)) at x=0 is
+	// undefined.
+	// There's no norm_safe because generally you need to add zero-checks somewhere *before* calling
+	// this, and it's not possible to produce correct derivatives from here.
+	Scalar
+	norm() const
+	{
+		Scalar len_sqrd = this->norm_sqrd();
+
+		return sqrt(len_sqrd);
+	}
+
+	// WARNING: Will return NaNs if vector magnitude is zero due to zero division.
+	// Do not call this on vectors with zero norm.
+	Vec3
+	normalized() const
+	{
+		Scalar norm = this->norm();
+
+		Vec3<Scalar> retval;
+		retval.x = this->x / norm;
+		retval.y = this->y / norm;
+		retval.z = this->z / norm;
+		return retval;
 	}
 };
 
diff --git a/src/xrt/tracking/hand/mercury/kine_lm/lm_main.cpp b/src/xrt/tracking/hand/mercury/kine_lm/lm_main.cpp
index 675b549b7..5ce36e47f 100644
--- a/src/xrt/tracking/hand/mercury/kine_lm/lm_main.cpp
+++ b/src/xrt/tracking/hand/mercury/kine_lm/lm_main.cpp
@@ -328,9 +328,26 @@ computeResidualStability(const OptimizerHand<T> &hand,
 	helper.AddValue((hand.wrist_post_location.z) * stab.stabilityRootPosition);
 
 
-	helper.AddValue((hand.wrist_post_orientation_aax.x) * (T)(stab.stabilityHandOrientationXY));
-	helper.AddValue((hand.wrist_post_orientation_aax.y) * (T)(stab.stabilityHandOrientationXY));
-	helper.AddValue((hand.wrist_post_orientation_aax.z) * (T)(stab.stabilityHandOrientationZ));
+	// Needed because d/dx(sqrt(x)) at x=0 is undefined, and the first iteration *always* starts at 0.
+	// x-2sin(0.5x) at x=0.001 is 4.16e-11 - this is a reasonable epsilon to pick.
+	const float epsilon = 0.001;
+	if (hand.wrist_post_orientation_aax.x < epsilon && //
+	    hand.wrist_post_orientation_aax.y < epsilon && //
+	    hand.wrist_post_orientation_aax.z < epsilon) {
+		helper.AddValue((hand.wrist_post_orientation_aax.x) * (T)(stab.stabilityHandOrientationXY));
+		helper.AddValue((hand.wrist_post_orientation_aax.y) * (T)(stab.stabilityHandOrientationXY));
+		helper.AddValue((hand.wrist_post_orientation_aax.z) * (T)(stab.stabilityHandOrientationZ));
+	} else {
+		T rotation_magnitude = hand.wrist_post_orientation_aax.norm();
+		T magnitude_sin = T(2) * sin(T(0.5) * rotation_magnitude);
+
+		Vec3<T> rotation_axis = hand.wrist_post_orientation_aax.normalized();
+
+
+		helper.AddValue((magnitude_sin * rotation_axis.x) * (T)(stab.stabilityHandOrientationXY));
+		helper.AddValue((magnitude_sin * rotation_axis.y) * (T)(stab.stabilityHandOrientationXY));
+		helper.AddValue((magnitude_sin * rotation_axis.z) * (T)(stab.stabilityHandOrientationZ));
+	}
 
 
 
@@ -1065,7 +1082,7 @@ optimizer_run(KinematicHandLM *hand,
 	state.this_frame_pre_position = state.last_frame.wrist_final_location;
 	// Repack - brings the curl values back into original domain. Look at ModelToLM/LMToModel, we're
 	// using sin/asin.
-	
+
 	state.last_frame.wrist_post_location.x = 0.0f;
 	state.last_frame.wrist_post_location.y = 0.0f;
 	state.last_frame.wrist_post_location.z = 0.0f;