m/mat4: Add Matrix 4x4 f64 functions

src/xrt/auxiliary/math/m_base.cpp

···
 	return copy(*q);
 }
 
+XRT_MAYBE_UNUSED static inline Eigen::Quaterniond
+copyd(const struct xrt_quat &q)
+{
+	// Eigen constructor order is different from XRT, OpenHMD and OpenXR!
+	//  Eigen: `float w, x, y, z`.
+	// OpenXR: `float x, y, z, w`.
+	return Eigen::Quaterniond(q.w, q.x, q.y, q.z);
+}
+
+XRT_MAYBE_UNUSED static inline Eigen::Quaterniond
+copyd(const struct xrt_quat *q)
+{
+	return copyd(*q);
+}
+
 static inline Eigen::Vector3f
 copy(const struct xrt_vec3 &v)
 {
···
 copy(const struct xrt_vec3 *v)
 {
 	return copy(*v);
+}
+
+XRT_MAYBE_UNUSED static inline Eigen::Vector3d
+copyd(const struct xrt_vec3 &v)
+{
+	return Eigen::Vector3d(v.x, v.y, v.z);
+}
+
+XRT_MAYBE_UNUSED static inline Eigen::Vector3d
+copyd(const struct xrt_vec3 *v)
+{
+	return copyd(*v);
 }
 
 static inline Eigen::Matrix3f
···
 	// clang-format on
 	return res;
 }
+
 
 /*
  *
···
 	Eigen::Matrix4f vp = copy(projection) * v3;
 	map_matrix_4x4(*result) = vp.inverse();
 }
+
+
+/*
+ *
+ * Exported Matrix 4x4 functions.
+ *
+ */
+
+extern "C" void
+m_mat4_f64_identity(struct xrt_matrix_4x4_f64 *result)
+{
+	map_matrix_4x4_f64(*result) = Eigen::Matrix4d::Identity();
+}
+
+extern "C" void
+m_mat4_f64_invert(const struct xrt_matrix_4x4_f64 *matrix, struct xrt_matrix_4x4_f64 *result)
+{
+	Eigen::Matrix4d m = map_matrix_4x4_f64(*matrix);
+	map_matrix_4x4_f64(*result) = m.inverse();
+}
+
+extern "C" void
+m_mat4_f64_multiply(const struct xrt_matrix_4x4_f64 *left,
+                    const struct xrt_matrix_4x4_f64 *right,
+                    struct xrt_matrix_4x4_f64 *result)
+{
+	Eigen::Matrix4d l = map_matrix_4x4_f64(*left);
+	Eigen::Matrix4d r = map_matrix_4x4_f64(*right);
+
+	map_matrix_4x4_f64(*result) = l * r;
+}
+
+extern "C" void
+m_mat4_f64_orientation(const struct xrt_quat *quat, struct xrt_matrix_4x4_f64 *result)
+{
+	map_matrix_4x4_f64(*result) = Eigen::Affine3d(copyd(*quat)).matrix();
+}
+
+extern "C" void
+m_mat4_f64_model(const struct xrt_pose *pose, const struct xrt_vec3 *size, struct xrt_matrix_4x4_f64 *result)
+{
+	Eigen::Vector3d position = copyd(pose->position);
+	Eigen::Quaterniond orientation = copyd(pose->orientation);
+
+	auto scale = Eigen::Scaling(copyd(size));
+
+	Eigen::Translation3d translation(position);
+	Eigen::Affine3d transformation = translation * orientation * scale;
+
+	map_matrix_4x4_f64(*result) = transformation.matrix();
+}
+
+extern "C" void
+m_mat4_f64_view(const struct xrt_pose *pose, struct xrt_matrix_4x4_f64 *result)
+{
+	Eigen::Vector3d position = copyd(pose->position);
+	Eigen::Quaterniond orientation = copyd(pose->orientation);
+
+	Eigen::Translation3d translation(position);
+	Eigen::Affine3d transformation = translation * orientation;
+
+	map_matrix_4x4_f64(*result) = transformation.matrix().inverse();
+}
+
 
 /*
  *

src/xrt/auxiliary/math/m_eigen_interop.hpp

···
 	return Eigen::Map<Eigen::Matrix4f>(m.v);
 }
 
+/*!
+ * @brief Wrap an internal 4x4 matrix f64 struct in an Eigen type, const overload.
+ *
+ * Permits zero-overhead manipulation of `const xrt_matrix_4x4_f64&` by Eigen routines as if it were a
+ * `const Eigen::Matrix4d&`.
+ */
+static inline Eigen::Map<const Eigen::Matrix4d>
+map_matrix_4x4_f64(const struct xrt_matrix_4x4_f64 &m)
+{
+	return Eigen::Map<const Eigen::Matrix4d>(m.v);
+}
+
+/*!
+ * @brief Wrap an internal 4x4 matrix struct in an Eigen type, non-const overload.
+ *
+ * Permits zero-overhead manipulation of `xrt_matrix_4x4_f64&` by Eigen routines as if it were a `Eigen::Matrix4d&`.
+ */
+static inline Eigen::Map<Eigen::Matrix4d>
+map_matrix_4x4_f64(struct xrt_matrix_4x4_f64 &m)
+{
+	return Eigen::Map<Eigen::Matrix4d>(m.v);
+}
+
+
 /*
  *
  * Pose deconstruction helpers.

src/xrt/auxiliary/math/m_matrix_4x4_f64.h

···
+// Copyright 2019-2021, Collabora, Ltd.
+// SPDX-License-Identifier: BSL-1.0
+/*!
+ * @file
+ * @brief  C matrix 4x4 f64 math library.
+ * @author Jakob Bornecrantz <[e-mail]>
+ *
+ * @see xrt_matrix_4x4_f64
+ * @ingroup aux_math
+ */
+
+#pragma once
+
+#include "xrt/xrt_defines.h"
+
+#include "m_mathinclude.h"
+
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+
+/*!
+ * Initialize Matrix4x4 F64 with identity.
+ *
+ * @relates xrt_matrix_4x4_f64
+ * @ingroup aux_math
+ */
+void
+m_mat4_f64_identity(struct xrt_matrix_4x4_f64 *result);
+
+/*!
+ * Invert a Matrix4x4 F64.
+ *
+ * @relates xrt_matrix_4x4_f64
+ * @ingroup aux_math
+ */
+void
+m_mat4_f64_invert(const struct xrt_matrix_4x4_f64 *matrix, struct xrt_matrix_4x4_f64 *result);
+
+/*!
+ * Multiply Matrix4x4 F64.
+ *
+ * @relates xrt_matrix_4x4_f64
+ * @ingroup aux_math
+ */
+void
+m_mat4_f64_multiply(const struct xrt_matrix_4x4_f64 *left,
+                    const struct xrt_matrix_4x4_f64 *right,
+                    struct xrt_matrix_4x4_f64 *result);
+
+/*!
+ * Initialize Matrix4x4 F64 with a orientation.
+ *
+ * @relates xrt_matrix_4x4_f64
+ * @ingroup aux_math
+ */
+void
+m_mat4_f64_orientation(const struct xrt_quat *quat, struct xrt_matrix_4x4_f64 *result);
+
+/*!
+ * Initialize Matrix4x4 F64 with a pose and size that can be used as a model matrix.
+ *
+ * @relates xrt_matrix_4x4_f64
+ * @ingroup aux_math
+ */
+void
+m_mat4_f64_model(const struct xrt_pose *pose, const struct xrt_vec3 *size, struct xrt_matrix_4x4_f64 *result);
+
+/*!
+ * Initialize Matrix4x4 F64 with a pose that can be used as a view martix.
+ *
+ * @relates xrt_matrix_4x4_f64
+ * @ingroup aux_math
+ */
+void
+m_mat4_f64_view(const struct xrt_pose *pose, const struct xrt_vec3 *size, struct xrt_matrix_4x4_f64 *result);
+
+
+#ifdef __cplusplus
+}
+
+
+static inline struct xrt_matrix_4x4_f64 // Until clang-format-11 is on the CI.
+operator*(const struct xrt_matrix_4x4_f64 &a, const struct xrt_matrix_4x4_f64 &b)
+{
+	struct xrt_matrix_4x4_f64 ret = {{0}};
+	m_mat4_f64_multiply(&l, &r, &ret);
+	return ret;
+}
+
+static inline void
+operator*=(struct xrt_matrix_4x4_f64 &a, const struct xrt_matrix_4x4_f64 &b)
+{
+	a = a * b;
+}
+
+
+#endif