BLARG

src/xrt/auxiliary/math/m_api.h

@@ -350,26 +350,26 @@ math_quat_finite_difference(const struct xrt_quat *quat0,
                             float dt,
                             struct xrt_vec3 *out_ang_vel);
 
-/*!
- * Converts a rotation vector in axis-angle form to its corresponding unit quaternion.
- *
- * @relates xrt_quat
- * @see xrt_vec3
- * @ingroup aux_math
- */
-void
-math_quat_exp(const struct xrt_vec3 *axis_angle, struct xrt_quat *out_quat);
-
-
-/*!
- * Converts a unit quaternion into its corresponding axis-angle vector representation.
- *
- * @relates xrt_quat
- * @see xrt_vec3
- * @ingroup aux_math
- */
-void
-math_quat_ln(const struct xrt_quat *quat, struct xrt_vec3 *out_axis_angle);
+// /*!
+//  * Converts a rotation vector in axis-angle form to its corresponding unit quaternion.
+//  *
+//  * @relates xrt_quat
+//  * @see xrt_vec3
+//  * @ingroup aux_math
+//  */
+// void
+// math_quat_from_rodrigues(const struct xrt_vec3 *axis_angle, struct xrt_quat *out_quat);
+
+
+// /*!
+//  * Converts a unit quaternion into its corresponding axis-angle vector representation.
+//  *
+//  * @relates xrt_quat
+//  * @see xrt_vec3
+//  * @ingroup aux_math
+//  */
+// void
+// math_quat_to_rodrigues(const struct xrt_quat *quat, struct xrt_vec3 *out_axis_angle);
 
 /*!
  * Converts a rotation vector in axis-angle form to its corresponding unit quaternion.
@@ -379,7 +379,7 @@ math_quat_ln(const struct xrt_quat *quat, struct xrt_vec3 *out_axis_angle);
  * @ingroup aux_math
  */
 void
-math_quat_to_axis_angle(const struct xrt_quat *quat, struct xrt_vec3 *out_axis_angle);
+math_quat_to_rodrigues(const struct xrt_quat *quat, struct xrt_vec3 *out_axis_angle);
 
 
 /*!
@@ -390,7 +390,7 @@ math_quat_to_axis_angle(const struct xrt_quat *quat, struct xrt_vec3 *out_axis_a
  * @ingroup aux_math
  */
 void
-math_quat_from_axis_angle(const struct xrt_vec3 *axis_angle, struct xrt_quat *out_quat);
+math_quat_from_rodrigues(const struct xrt_vec3 *axis_angle, struct xrt_quat *out_quat);
 
 
 /*!

src/xrt/auxiliary/math/m_base.cpp

@@ -391,7 +391,7 @@ math_quat_slerp(const struct xrt_quat *left, const struct xrt_quat *right, float
  * @ingroup aux_math
  */
 extern "C" void
-math_quat_from_axis_angle(const struct xrt_vec3 *axis_angle, struct xrt_quat *out_quat)
+math_quat_from_rodrigues(const struct xrt_vec3 *axis_angle, struct xrt_quat *out_quat)
 {
 	float a0 = axis_angle->x;
 	float a1 = axis_angle->y;
@@ -429,7 +429,7 @@ math_quat_from_axis_angle(const struct xrt_vec3 *axis_angle, struct xrt_quat *ou
  * @ingroup aux_math
  */
 extern "C" void
-math_quat_to_axis_angle(const struct xrt_quat *quat, struct xrt_vec3 *out_axis_angle) {
+math_quat_to_rodrigues(const struct xrt_quat *quat, struct xrt_vec3 *out_axis_angle) {
 	float q1 = quat->x;
   float q2 = quat->y;
   float q3 = quat->z;
@@ -520,7 +520,7 @@ math_quat_from_swing_twist(const struct xrt_vec2 *swing, const float twist, stru
 
 	math_quat_from_swing(swing, &swing_quat);
 
-	math_quat_from_axis_angle(&aax_twist, &twist_quat);
+	math_quat_from_rodrigues(&aax_twist, &twist_quat);
 
 	math_quat_rotate(&swing_quat, &twist_quat, result);
 }
@@ -550,15 +550,15 @@ math_quat_to_swing_twist(const struct xrt_quat *in, struct xrt_vec2 *out_swing,
 	out_swing->y = swing_aax.axis().y() * swing_aax.angle();
 	assert(swing_aax.axis().z() < 0.001);
 
-	std::cout << "axis:" << swing_aax.axis()*swing_aax.angle() << "\n" ;
-	std::cout << "axis:" << twist_aax.axis() *twist_aax.angle()<< "\n" ;
+	// std::cout << "axis:" << swing_aax.axis()*swing_aax.angle() << "\n" ;
+	// std::cout << "axis:" << twist_aax.axis() *twist_aax.angle()<< "\n" ;
 
-	Eigen::Quaternionf result0 = swing_aax * twist_aax; // This one is right
-	// Eigen::Quaternionf result1 = twist_aax * swing_aax;
+	// Eigen::Quaternionf result0 = swing_aax * twist_aax; // This one is right
+	// // Eigen::Quaternionf result1 = twist_aax * swing_aax;
 
-	std::cout << result0 << std::endl << std::endl;
-	// std::cout << result1 << std::endl<< std::endl;
-	std::cout << map_quat(*in) << std::endl << std::endl;
+	// std::cout << result0 << std::endl << std::endl;
+	// // std::cout << result1 << std::endl<< std::endl;
+	// std::cout << map_quat(*in) << std::endl << std::endl;
 
 
 	*out_twist = twist_aax.axis().z() * twist_aax.angle();

src/xrt/auxiliary/math/m_filter_one_euro.c

@@ -276,15 +276,15 @@ m_filter_euro_quat_run(struct m_filter_euro_quat *f, uint64_t ts, const struct x
 
 	// Scale dy with dt through a conversion to angle_axis
 	struct xrt_vec3 dy_aa;
-	math_quat_ln(&dy, &dy_aa);
+	math_quat_to_rodrigues(&dy, &dy_aa);
 	dy_aa = m_vec3_div_scalar(dy_aa, dt);
-	math_quat_exp(&dy_aa, &dy);
+	math_quat_from_rodrigues(&dy_aa, &dy);
 
 	f->prev_dy = exp_smooth_quat(alpha_d, dy, f->prev_dy);
 
 	// The magnitud of the smoothed dy (f->prev_dy) is its rotation angle in radians
 	struct xrt_vec3 smooth_dy_aa;
-	math_quat_ln(&f->prev_dy, &smooth_dy_aa);
+	math_quat_to_rodrigues(&f->prev_dy, &smooth_dy_aa);
 	double smooth_dy_mag = m_vec3_len(smooth_dy_aa);
 
 	double alpha = filter_one_euro_compute_alpha(&f->base, dt, smooth_dy_mag);

src/xrt/auxiliary/math/m_quatexpmap.cpp

@@ -168,15 +168,15 @@ math_quat_finite_difference(const struct xrt_quat *quat0,
 	map_vec3(*out_ang_vel) = 2.f * quat_ln(inc_quat) / dt;
 }
 
-extern "C" void
-math_quat_exp(const struct xrt_vec3 *axis_angle, struct xrt_quat *out_quat)
-{
-	map_quat(*out_quat) = quat_exp(map_vec3(*axis_angle));
-}
-
-extern "C" void
-math_quat_ln(const struct xrt_quat *quat, struct xrt_vec3 *out_axis_angle)
-{
-	Eigen::Quaternionf eigen_quat = map_quat(*quat);
-	map_vec3(*out_axis_angle) = quat_ln(eigen_quat);
-}
+// extern "C" void
+// math_quat_from_rodrigues(const struct xrt_vec3 *axis_angle, struct xrt_quat *out_quat)
+// {
+// 	map_quat(*out_quat) = quat_exp(map_vec3(*axis_angle));
+// }
+
+// extern "C" void
+// math_quat_to_rodrigues(const struct xrt_quat *quat, struct xrt_vec3 *out_axis_angle)
+// {
+// 	Eigen::Quaternionf eigen_quat = map_quat(*quat);
+// 	map_vec3(*out_axis_angle) = quat_ln(eigen_quat);
+// }

tests/tests_quatexpmap.cpp

@@ -67,10 +67,10 @@ TEST_CASE("m_quatexpmap")
 
 		for (xrt_vec3 aa : aas) {
 			xrt_quat quat{};
-			math_quat_from_axis_angle(&aa, &quat);
+			math_quat_from_rodrigues(&aa, &quat);
 
 			xrt_vec3 expected_aa{};
-			math_quat_to_axis_angle(&quat, &expected_aa);
+			math_quat_to_rodrigues(&quat, &expected_aa);
 
 			CHECK(m_vec3_len(expected_aa - aa) <= 0.001);
 		}
@@ -84,7 +84,7 @@ TEST_CASE("m_quatexpmap")
 		xrt_vec3 axis = axis4;
 		xrt_vec3 aa = axis * angle;
 		xrt_quat q{};
-		math_quat_from_axis_angle(&aa, &q);
+		math_quat_from_rodrigues(&aa, &q);
 
 		CHECK(q.x == Approx(axis.x * sin(angle / 2)));
 		CHECK(q.y == Approx(axis.y * sin(angle / 2)));