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

@@ -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.
  *

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,

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);
 	}