From e4a1285e2aa5437418d56f1cdd1a76bb66362f70 Mon Sep 17 00:00:00 2001
From: Ryan Pavlik <ryan.pavlik@collabora.com>
Date: Mon, 14 Oct 2019 21:40:53 +0200
Subject: [PATCH] wip

---
 src/xrt/auxiliary/math/m_api.h         | 59 ++++++++++++++++++++++++++
 src/xrt/auxiliary/math/m_base.cpp      | 48 +++++++++++++++++++++
 src/xrt/drivers/lgr100/lgr100_device.c | 38 ++++++++++++++++-
 3 files changed, 144 insertions(+), 1 deletion(-)

diff --git a/src/xrt/auxiliary/math/m_api.h b/src/xrt/auxiliary/math/m_api.h
index fbdb00ebe..301211300 100644
--- a/src/xrt/auxiliary/math/m_api.h
+++ b/src/xrt/auxiliary/math/m_api.h
@@ -96,6 +96,40 @@ void
 math_vec3_accum(const struct xrt_vec3 *additional, struct xrt_vec3 *inAndOut);
 
 
+/*!
+ * Get the length (L2 norm) of the vector.
+ *
+ * Involves a square-root, so if you can survive using the squared norm, please
+ * do.
+ *
+ * @see math_vec3_squared_norm
+ *
+ * @relates xrt_vec3
+ * @ingroup aux_math
+ */
+float
+math_vec3_norm(const struct xrt_vec3 *vec);
+
+/*!
+ * Get the square of the length (L2 norm) of the vector.
+ *
+ * @see math_vec3_norm
+ *
+ * @relates xrt_vec3
+ * @ingroup aux_math
+ */
+float
+math_vec3_squared_norm(const struct xrt_vec3 *vec);
+
+/*!
+ * Normalize the vector so the length is 1.
+ *
+ * @relates xrt_vec3
+ * @ingroup aux_math
+ */
+void
+math_vec3_normalize(struct xrt_vec3 *vec);
+
 /*
  *
  * Quat functions.
@@ -180,6 +214,31 @@ math_quat_finite_difference(const struct xrt_quat *quat0,
                             const float dt,
                             struct xrt_vec3 *out_ang_vel);
 
+
+/*!
+ * Create the rotation to rotate a to b.
+ *
+ * @relates xrt_quat
+ * @relatesalso xrt_vec3
+ * @ingroup aux_math
+ */
+void
+math_quat_from_two_vecs(const struct xrt_vec3 *a,
+                        const struct xrt_vec3 *b,
+                        struct xrt_quat *out);
+
+
+/*!
+ * Perform slerp (spherical linear interpolation) between two quats.
+ *
+ * @relates xrt_quat
+ * @ingroup aux_math
+ */
+void
+math_quat_slerp(const struct xrt_quat *a,
+                const struct xrt_quat *b,
+                const float alpha,
+                struct xrt_quat *out);
 /*
  *
  * Pose functions.
diff --git a/src/xrt/auxiliary/math/m_base.cpp b/src/xrt/auxiliary/math/m_base.cpp
index 2aff48f8b..c3c49cf5b 100644
--- a/src/xrt/auxiliary/math/m_base.cpp
+++ b/src/xrt/auxiliary/math/m_base.cpp
@@ -73,6 +73,27 @@ math_vec3_accum(const struct xrt_vec3* additional, struct xrt_vec3* inAndOut)
 	map_vec3(*inAndOut) += map_vec3(*additional);
 }
 
+extern "C" float
+math_vec3_norm(const struct xrt_vec3* vec)
+{
+	assert(vec != NULL);
+	return map_vec3(*vec).norm();
+}
+
+
+extern "C" float
+math_vec3_squared_norm(const struct xrt_vec3* vec)
+{
+	assert(vec != NULL);
+	return map_vec3(*vec).squaredNorm();
+}
+
+extern "C" void
+math_vec3_normalize(struct xrt_vec3* vec)
+{
+	assert(vec != NULL);
+	map_vec3(*vec).normalize();
+}
 
 /*
  *
@@ -144,6 +165,33 @@ math_quat_rotate_vec3(const struct xrt_quat* left,
 }
 
 
+extern "C" void
+math_quat_from_two_vecs(const struct xrt_vec3* a,
+                        const struct xrt_vec3* b,
+                        struct xrt_quat* out)
+{
+	assert(a != NULL);
+	assert(b != NULL);
+	assert(out != NULL);
+	Eigen::Quaternionf result =
+	    Eigen::Quaternionf::FromTwoVectors(map_vec3(*a), map_vec3(*b));
+	map_quat(*out) = result;
+}
+
+
+extern "C" void
+math_quat_slerp(const struct xrt_quat* a,
+                const struct xrt_quat* b,
+                const float t,
+                struct xrt_quat* out)
+{
+
+	assert(a != NULL);
+	assert(b != NULL);
+	assert(out != NULL);
+	Eigen::Quaternionf result = map_quat(*a).slerp(t, map_quat(*b));
+	map_quat(*out) = result;
+}
 /*
  *
  * Exported pose functions.
diff --git a/src/xrt/drivers/lgr100/lgr100_device.c b/src/xrt/drivers/lgr100/lgr100_device.c
index f1560b60c..b4e4faf7d 100644
--- a/src/xrt/drivers/lgr100/lgr100_device.c
+++ b/src/xrt/drivers/lgr100/lgr100_device.c
@@ -246,6 +246,9 @@ lgr100_parse_input(struct lgr100_device *lgd,
 	return LGR100_IRQ_NULL;
 }
 
+static const float G = 9.8f;
+static const float ACCEL_RESTORE_RATE = 0.9f;
+
 static void
 update_fusion(struct lgr100_device *lgd,
               struct lgr100_parsed_input *sample,
@@ -256,8 +259,40 @@ update_fusion(struct lgr100_device *lgd,
 
 	lgd->read.accel = sample->accel;
 	lgd->read.gyro = sample->gyro;
-	math_quat_integrate_velocity(&lgd->fusion.rot, &lgd->read.gyro, dt,
+	struct xrt_quat rot = lgd->fusion.rot;
+	struct xrt_quat inverse = {-rot.x, -rot.y, -rot.z, rot.w};
+	struct xrt_vec3 xformed_vel;
+	math_quat_rotate_vec3(&inverse, &(sample->gyro), &xformed_vel);
+
+	math_quat_integrate_velocity(&rot, &lgd->read.gyro, dt,
 	                             &lgd->fusion.rot);
+
+	float diff_of_squares =
+	    fabsf(G * G - math_vec3_squared_norm(&(sample->accel)));
+
+	// If gravity is exact, this is 1. it gets smaller the further away from
+	// gravity magnitude.
+	float goodness = 1.f - diff_of_squares;
+	if (goodness > 0.f) {
+		float scale = ACCEL_RESTORE_RATE * goodness;
+		struct xrt_vec3 normalized = sample->accel;
+		math_vec3_normalize(&normalized);
+		struct xrt_vec3 transformed_gravity;
+		math_quat_rotate_vec3(&inverse, &normalized,
+		                      &transformed_gravity);
+		struct xrt_vec3 zero = {0.f, 0.f, 0.f};
+
+
+		struct xrt_quat grav_adjust;
+		math_quat_from_two_vecs(&transformed_gravity, &zero,
+		                        &grav_adjust);
+		struct xrt_quat identity = {0.f, 0.f, 0.f, 1.f};
+
+
+		struct xrt_quat scaled_rotate;
+		math_quat_slerp(&identity, &grav_adjust, goodness * dt, &scaled_rotate);
+		math_quat_rotate(&scaled_rotate, &lgd->fusion.rot, &lgd->fusion.rot);
+	}
 }
 
 static void *
@@ -328,6 +363,7 @@ teardown(struct lgr100_device *lgd)
 		lgd->dev = NULL;
 	}
 }
+
 /*
  *
  * xrt_device functions.
-- 
GitLab