mirror of
https://gitlab.freedesktop.org/monado/monado.git
synced 2025-02-13 17:20:09 +00:00
a/math: Add multiple isometry-related matrix functions
An isometry is a rigid body transform. In this context I'm using the term to refer to 4x4 homogeneous matrices in SE(3). I.e., matrices comprised of a 3x3 rotation, a 3x1 translation, and a [0,0,0,1] last row. This matrix represent both rigid body transforms.
This commit is contained in:
parent
8b2fa955b9
commit
05b8e320a5
src/xrt/auxiliary/math
|
@ -163,6 +163,15 @@ math_vec3_cross(const struct xrt_vec3 *l, const struct xrt_vec3 *r, struct xrt_v
|
|||
void
|
||||
math_vec3_f64_cross(const struct xrt_vec3_f64 *l, const struct xrt_vec3_f64 *r, struct xrt_vec3_f64 *result);
|
||||
|
||||
/*!
|
||||
* Get translation vector from isometry matrix (col-major).
|
||||
*
|
||||
* @relates xrt_vec3
|
||||
* @ingroup aux_math
|
||||
*/
|
||||
void
|
||||
math_vec3_translation_from_isometry(const struct xrt_matrix_4x4 *isometry, struct xrt_vec3 *result);
|
||||
|
||||
/*!
|
||||
* Normalize a vec3 in place.
|
||||
*
|
||||
|
@ -417,6 +426,15 @@ math_matrix_3x3_identity(struct xrt_matrix_3x3 *mat);
|
|||
void
|
||||
math_matrix_3x3_f64_identity(struct xrt_matrix_3x3_f64 *mat);
|
||||
|
||||
/*!
|
||||
* Print a 3x3 (col-major) matrix to stdout
|
||||
*
|
||||
* @see xrt_matrix_3x3
|
||||
* @ingroup aux_math
|
||||
*/
|
||||
void
|
||||
math_matrix_3x3_print(const struct xrt_matrix_3x3 *mat);
|
||||
|
||||
/*!
|
||||
* Transform a vec3 by a 3x3 matrix
|
||||
*
|
||||
|
@ -459,6 +477,15 @@ math_matrix_3x3_multiply(const struct xrt_matrix_3x3 *left,
|
|||
void
|
||||
math_matrix_3x3_inverse(const struct xrt_matrix_3x3 *in, struct xrt_matrix_3x3 *result);
|
||||
|
||||
/*!
|
||||
* Transpose Matrix3x3
|
||||
*
|
||||
* @relates xrt_matrix_3x3
|
||||
* @ingroup aux_math
|
||||
*/
|
||||
void
|
||||
math_matrix_3x3_transpose(const struct xrt_matrix_3x3 *in, struct xrt_matrix_3x3 *result);
|
||||
|
||||
/*!
|
||||
* Create a rotation from two vectors plus x and z, by
|
||||
* creating a rotation matrix by crossing z and x to
|
||||
|
@ -474,6 +501,15 @@ math_matrix_3x3_f64_from_plus_x_z(const struct xrt_vec3_f64 *plus_x,
|
|||
const struct xrt_vec3_f64 *plus_z,
|
||||
struct xrt_matrix_3x3_f64 *result);
|
||||
|
||||
/*!
|
||||
* Get the rotation matrix from an isomertry matrix (col-major).
|
||||
*
|
||||
* @relates xrt_matrix_4x4
|
||||
* @ingroup aux_math
|
||||
*/
|
||||
void
|
||||
math_matrix_3x3_rotation_from_isometry(const struct xrt_matrix_4x4 *isometry, struct xrt_matrix_3x3 *result);
|
||||
|
||||
/*!
|
||||
* Initialize Matrix4x4 with identity.
|
||||
*
|
||||
|
@ -483,6 +519,15 @@ math_matrix_3x3_f64_from_plus_x_z(const struct xrt_vec3_f64 *plus_x,
|
|||
void
|
||||
math_matrix_4x4_identity(struct xrt_matrix_4x4 *result);
|
||||
|
||||
/*!
|
||||
* Prints a Matrix4x4 (col-major).
|
||||
*
|
||||
* @relates xrt_matrix_4x4
|
||||
* @ingroup aux_math
|
||||
*/
|
||||
void
|
||||
math_matrix_4x4_print(const struct xrt_matrix_4x4 *mat);
|
||||
|
||||
/*!
|
||||
* Multiply Matrix4x4.
|
||||
*
|
||||
|
@ -494,6 +539,33 @@ math_matrix_4x4_multiply(const struct xrt_matrix_4x4 *left,
|
|||
const struct xrt_matrix_4x4 *right,
|
||||
struct xrt_matrix_4x4 *result);
|
||||
|
||||
/*!
|
||||
* Invert Matrix4x4.
|
||||
*
|
||||
* @relates xrt_matrix_4x4
|
||||
* @ingroup aux_math
|
||||
*/
|
||||
void
|
||||
math_matrix_4x4_inverse(const struct xrt_matrix_4x4 *in, struct xrt_matrix_4x4 *result);
|
||||
|
||||
/*!
|
||||
* Invert a homogeneous isometry 4x4 (col-major) matrix in SE(3).
|
||||
*
|
||||
* @relates xrt_matrix_4x4
|
||||
* @ingroup aux_math
|
||||
*/
|
||||
void
|
||||
math_matrix_4x4_isometry_inverse(const struct xrt_matrix_4x4 *in, struct xrt_matrix_4x4 *result);
|
||||
|
||||
/*!
|
||||
* Transpose Matrix4x4
|
||||
*
|
||||
* @relates xrt_matrix_4x4
|
||||
* @ingroup aux_math
|
||||
*/
|
||||
void
|
||||
math_matrix_4x4_transpose(const struct xrt_matrix_4x4 *in, struct xrt_matrix_4x4 *result);
|
||||
|
||||
/*!
|
||||
* Compute view matrix from xrt_pose.
|
||||
*
|
||||
|
@ -503,6 +575,18 @@ math_matrix_4x4_multiply(const struct xrt_matrix_4x4 *left,
|
|||
void
|
||||
math_matrix_4x4_view_from_pose(const struct xrt_pose *pose, struct xrt_matrix_4x4 *result);
|
||||
|
||||
/*!
|
||||
* Get an isometry matrix —in SE(3)— from a rotation matrix —SO(3)— and a
|
||||
* translation vector. All col-major matrices.
|
||||
*
|
||||
* @relates xrt_matrix_4x4
|
||||
* @ingroup aux_math
|
||||
*/
|
||||
void
|
||||
math_matrix_4x4_isometry_from_rt(const struct xrt_matrix_3x3 *rotation,
|
||||
const struct xrt_vec3 *translation,
|
||||
struct xrt_matrix_4x4 *result);
|
||||
|
||||
/*!
|
||||
* Compute quad layer model matrix from xrt_pose and xrt_vec2 size.
|
||||
*
|
||||
|
|
|
@ -12,6 +12,7 @@
|
|||
|
||||
#include "math/m_api.h"
|
||||
#include "math/m_eigen_interop.hpp"
|
||||
#include "util/u_logging.h"
|
||||
|
||||
#include <Eigen/Core>
|
||||
#include <Eigen/Geometry>
|
||||
|
@ -418,6 +419,17 @@ math_matrix_3x3_f64_identity(struct xrt_matrix_3x3_f64 *mat)
|
|||
map_matrix_3x3_f64(*mat) = Eigen::Matrix3d::Identity();
|
||||
}
|
||||
|
||||
extern "C" void
|
||||
math_matrix_3x3_print(const struct xrt_matrix_3x3 *mat)
|
||||
{
|
||||
const auto &m = mat->v;
|
||||
U_LOG_RAW("[\n");
|
||||
U_LOG_RAW("\t%f, %f, %f,\n", m[0], m[3], m[6]);
|
||||
U_LOG_RAW("\t%f, %f, %f,\n", m[1], m[4], m[7]);
|
||||
U_LOG_RAW("\t%f, %f, %f \n", m[2], m[5], m[8]);
|
||||
U_LOG_RAW("]\n");
|
||||
}
|
||||
|
||||
extern "C" void
|
||||
math_matrix_3x3_f64_transform_vec3_f64(const struct xrt_matrix_3x3_f64 *left,
|
||||
const struct xrt_vec3_f64 *right,
|
||||
|
@ -450,6 +462,13 @@ math_matrix_3x3_f64_from_plus_x_z(const struct xrt_vec3_f64 *plus_x,
|
|||
result->v[8] = plus_z->z;
|
||||
}
|
||||
|
||||
extern "C" void
|
||||
math_matrix_3x3_rotation_from_isometry(const struct xrt_matrix_4x4 *isometry, struct xrt_matrix_3x3 *result)
|
||||
{
|
||||
Eigen::Isometry3f transform{map_matrix_4x4(*isometry)};
|
||||
map_matrix_3x3(*result) = transform.linear();
|
||||
}
|
||||
|
||||
extern "C" void
|
||||
math_matrix_3x3_transform_vec3(const struct xrt_matrix_3x3 *left,
|
||||
const struct xrt_vec3 *right,
|
||||
|
@ -493,12 +512,31 @@ math_matrix_3x3_inverse(const struct xrt_matrix_3x3 *in, struct xrt_matrix_3x3 *
|
|||
map_matrix_3x3(*result) = m.inverse();
|
||||
}
|
||||
|
||||
extern "C" void
|
||||
math_matrix_3x3_transpose(const struct xrt_matrix_3x3 *in, struct xrt_matrix_3x3 *result)
|
||||
{
|
||||
Eigen::Matrix3f m = copy(in);
|
||||
map_matrix_3x3(*result) = m.transpose();
|
||||
}
|
||||
|
||||
extern "C" void
|
||||
math_matrix_4x4_identity(struct xrt_matrix_4x4 *result)
|
||||
{
|
||||
map_matrix_4x4(*result) = Eigen::Matrix4f::Identity();
|
||||
}
|
||||
|
||||
extern "C" void
|
||||
math_matrix_4x4_print(const struct xrt_matrix_4x4 *mat)
|
||||
{
|
||||
const auto &m = mat->v;
|
||||
U_LOG_RAW("[\n");
|
||||
U_LOG_RAW("\t%f, %f, %f, %f,\n", m[0], m[4], m[8], m[12]);
|
||||
U_LOG_RAW("\t%f, %f, %f, %f,\n", m[1], m[5], m[9], m[13]);
|
||||
U_LOG_RAW("\t%f, %f, %f, %f,\n", m[2], m[6], m[10], m[14]);
|
||||
U_LOG_RAW("\t%f, %f, %f, %f \n", m[3], m[7], m[11], m[15]);
|
||||
U_LOG_RAW("]\n");
|
||||
}
|
||||
|
||||
extern "C" void
|
||||
math_matrix_4x4_multiply(const struct xrt_matrix_4x4 *left,
|
||||
const struct xrt_matrix_4x4 *right,
|
||||
|
@ -507,6 +545,27 @@ math_matrix_4x4_multiply(const struct xrt_matrix_4x4 *left,
|
|||
map_matrix_4x4(*result) = copy(left) * copy(right);
|
||||
}
|
||||
|
||||
extern "C" void
|
||||
math_matrix_4x4_inverse(const struct xrt_matrix_4x4 *in, struct xrt_matrix_4x4 *result)
|
||||
{
|
||||
Eigen::Matrix4f m = copy(in);
|
||||
map_matrix_4x4(*result) = m.inverse();
|
||||
}
|
||||
|
||||
extern "C" void
|
||||
math_matrix_4x4_transpose(const struct xrt_matrix_4x4 *in, struct xrt_matrix_4x4 *result)
|
||||
{
|
||||
Eigen::Matrix4f m = copy(in);
|
||||
map_matrix_4x4(*result) = m.transpose();
|
||||
}
|
||||
|
||||
extern "C" void
|
||||
math_matrix_4x4_isometry_inverse(const struct xrt_matrix_4x4 *in, struct xrt_matrix_4x4 *result)
|
||||
{
|
||||
Eigen::Isometry3f m{copy(in)};
|
||||
map_matrix_4x4(*result) = m.inverse().matrix();
|
||||
}
|
||||
|
||||
extern "C" void
|
||||
math_matrix_4x4_view_from_pose(const struct xrt_pose *pose, struct xrt_matrix_4x4 *result)
|
||||
{
|
||||
|
@ -519,6 +578,17 @@ math_matrix_4x4_view_from_pose(const struct xrt_pose *pose, struct xrt_matrix_4x
|
|||
map_matrix_4x4(*result) = transformation.inverse().matrix();
|
||||
}
|
||||
|
||||
extern "C" void
|
||||
math_matrix_4x4_isometry_from_rt(const struct xrt_matrix_3x3 *rotation,
|
||||
const struct xrt_vec3 *translation,
|
||||
struct xrt_matrix_4x4 *result)
|
||||
{
|
||||
Eigen::Isometry3f transformation = Eigen::Isometry3f::Identity();
|
||||
transformation.linear() = map_matrix_3x3(*rotation);
|
||||
transformation.translation() = map_vec3(*translation);
|
||||
map_matrix_4x4(*result) = transformation.matrix();
|
||||
}
|
||||
|
||||
extern "C" void
|
||||
math_matrix_4x4_model(const struct xrt_pose *pose, const struct xrt_vec3 *size, struct xrt_matrix_4x4 *result)
|
||||
{
|
||||
|
@ -582,6 +652,13 @@ math_vec3_f64_cross(const struct xrt_vec3_f64 *l, const struct xrt_vec3_f64 *r,
|
|||
map_vec3_f64(*result) = map_vec3_f64(*l).cross(map_vec3_f64(*r));
|
||||
}
|
||||
|
||||
extern "C" void
|
||||
math_vec3_translation_from_isometry(const struct xrt_matrix_4x4 *transform, struct xrt_vec3 *result)
|
||||
{
|
||||
Eigen::Isometry3f isometry{map_matrix_4x4(*transform)};
|
||||
map_vec3(*result) = isometry.translation();
|
||||
}
|
||||
|
||||
extern "C" void
|
||||
m_mat4_f64_orientation(const struct xrt_quat *quat, struct xrt_matrix_4x4_f64 *result)
|
||||
{
|
||||
|
|
|
@ -98,6 +98,18 @@ map_vec3_f64(struct xrt_vec3_f64 &v)
|
|||
return Eigen::Map<Eigen::Vector3d>{&v.x};
|
||||
}
|
||||
|
||||
/*!
|
||||
* @brief Wrap an internal 3x3 matrix struct in an Eigen type, const overload.
|
||||
*
|
||||
* Permits zero-overhead manipulation of `xrt_matrix_3x3&` by Eigen routines as
|
||||
* if it were a `Eigen::Matrix3f&`.
|
||||
*/
|
||||
static inline Eigen::Map<const Eigen::Matrix3f>
|
||||
map_matrix_3x3(const struct xrt_matrix_3x3 &m)
|
||||
{
|
||||
return Eigen::Map<const Eigen::Matrix3f>(m.v);
|
||||
}
|
||||
|
||||
/*!
|
||||
* @brief Wrap an internal 3x3 matrix struct in an Eigen type, non-const
|
||||
* overload.
|
||||
|
@ -124,6 +136,19 @@ map_matrix_3x3_f64(struct xrt_matrix_3x3_f64 &m)
|
|||
return Eigen::Map<Eigen::Matrix3d>(m.v);
|
||||
}
|
||||
|
||||
/*!
|
||||
* @brief Wrap an internal 4x4 matrix struct in an Eigen type, const
|
||||
* overload.
|
||||
*
|
||||
* Permits zero-overhead manipulation of `xrt_matrix_4x4&` by Eigen routines as
|
||||
* if it were a `Eigen::Matrix4f&`.
|
||||
*/
|
||||
static inline Eigen::Map<const Eigen::Matrix4f>
|
||||
map_matrix_4x4(const struct xrt_matrix_4x4 &m)
|
||||
{
|
||||
return Eigen::Map<const Eigen::Matrix4f>(m.v);
|
||||
}
|
||||
|
||||
/*!
|
||||
* @brief Wrap an internal 4x4 matrix struct in an Eigen type, non-const
|
||||
* overload.
|
||||
|
|
Loading…
Reference in a new issue