From 7a1dbbe8a18c606658e85bfef1eaacc53c4448e4 Mon Sep 17 00:00:00 2001
From: Christoph Haag <christoph.haag@collabora.com>
Date: Tue, 16 Jun 2020 15:39:16 +0200
Subject: [PATCH] st/oxr: Ensure quaternion is normalized in xrLocateViews()

In rare cases the state tracker's pose transformations resulted in a quaternion
that accumulated float precision errors such that the norm of the quaternion was
not within float precision of 1.0 anymore.

Introduce a function math_quat_ensure_normalized() that can be used after multiple
operations have been performed on a quaternion.
---
 src/xrt/auxiliary/math/m_api.h           | 12 ++++++++++++
 src/xrt/auxiliary/math/m_base.cpp        | 22 ++++++++++++++++++++++
 src/xrt/state_trackers/oxr/oxr_session.c |  7 +++++++
 3 files changed, 41 insertions(+)

diff --git a/src/xrt/auxiliary/math/m_api.h b/src/xrt/auxiliary/math/m_api.h
index 03032efb4..d06023ba9 100644
--- a/src/xrt/auxiliary/math/m_api.h
+++ b/src/xrt/auxiliary/math/m_api.h
@@ -166,6 +166,18 @@ math_quat_validate(const struct xrt_quat *quat);
 void
 math_quat_normalize(struct xrt_quat *inout);
 
+/*!
+ * Normalizes a quaternion if it has accumulated float precision errors.
+ * Returns true if the quaternion was already normalized or was normalized after
+ * being found within a small float precision tolerance.
+ * Returns false if the quaternion was not at all normalized.
+ *
+ * @relates xrt_quat
+ * @ingroup aux_math
+ */
+bool
+math_quat_ensure_normalized(struct xrt_quat *inout);
+
 /*!
  * Rotate a vector.
  *
diff --git a/src/xrt/auxiliary/math/m_base.cpp b/src/xrt/auxiliary/math/m_base.cpp
index 11916d14b..dc696f041 100644
--- a/src/xrt/auxiliary/math/m_base.cpp
+++ b/src/xrt/auxiliary/math/m_base.cpp
@@ -183,6 +183,28 @@ math_quat_normalize(struct xrt_quat *inout)
 	map_quat(*inout).normalize();
 }
 
+extern "C" bool
+math_quat_ensure_normalized(struct xrt_quat *inout)
+{
+	assert(inout != NULL);
+
+	if (math_quat_validate(inout))
+		return true;
+
+	const float FLOAT_EPSILON = Eigen::NumTraits<float>::epsilon();
+	const float TOLERANCE = FLOAT_EPSILON * 5;
+
+	auto rot = copy(*inout);
+	auto norm = rot.norm();
+	if (norm > 1.0f + TOLERANCE || norm < 1.0f - TOLERANCE) {
+		return false;
+	}
+
+	map_quat(*inout).normalize();
+	return true;
+}
+
+
 extern "C" void
 math_quat_rotate(const struct xrt_quat *left,
                  const struct xrt_quat *right,
diff --git a/src/xrt/state_trackers/oxr/oxr_session.c b/src/xrt/state_trackers/oxr/oxr_session.c
index a8022377a..f0b8ec554 100644
--- a/src/xrt/state_trackers/oxr/oxr_session.c
+++ b/src/xrt/state_trackers/oxr/oxr_session.c
@@ -389,6 +389,13 @@ oxr_session_views(struct oxr_logger *log,
 		safe_copy.xrt = xdev->hmd->views[i].fov;
 		views[i].fov = safe_copy.oxr;
 
+		if (!math_quat_ensure_normalized(
+		        &((struct xrt_pose *)&views[i].pose)->orientation)) {
+			return oxr_error(
+			    log, XR_ERROR_RUNTIME_FAILURE,
+			    "Quaternion in xrLocateViews was invalid");
+		}
+
 		print_view_fov(sess, i, (struct xrt_fov *)&views[i].fov);
 		print_view_pose(sess, i, (struct xrt_pose *)&views[i].pose);
 	}