aux/math: Add math_matrix_3x3_from_quat()

Add a function to get a 3x3 rotation matrix
from a quaternion.
This commit is contained in:
Jan Schmidt 2022-05-23 02:32:18 +10:00 committed by Jakob Bornecrantz
parent e5466553a2
commit ce13de9484
2 changed files with 29 additions and 0 deletions
src/xrt/auxiliary/math

View file

@ -448,6 +448,15 @@ math_matrix_2x2_transform_vec2(const struct xrt_matrix_2x2 *left,
void
math_matrix_3x3_identity(struct xrt_matrix_3x3 *mat);
/*!
* Initialize a 3x3 matrix from a quaternion
*
* @see xrt_matrix_3x3
* @ingroup aux_math
*/
void
math_matrix_3x3_from_quat(const struct xrt_quat *q, struct xrt_matrix_3x3 *result_out);
/*!
* Initialize a double 3x3 matrix to the identity matrix
*

View file

@ -473,6 +473,26 @@ math_matrix_3x3_identity(struct xrt_matrix_3x3 *mat)
map_matrix_3x3(*mat) = Eigen::Matrix3f::Identity();
}
extern "C" void
math_matrix_3x3_from_quat(const struct xrt_quat *q, struct xrt_matrix_3x3 *result_out)
{
struct xrt_matrix_3x3 result = {{
1 - 2 * q->y * q->y - 2 * q->z * q->z,
2 * q->x * q->y - 2 * q->w * q->z,
2 * q->x * q->z + 2 * q->w * q->y,
2 * q->x * q->y + 2 * q->w * q->z,
1 - 2 * q->x * q->x - 2 * q->z * q->z,
2 * q->y * q->z - 2 * q->w * q->x,
2 * q->x * q->z - 2 * q->w * q->y,
2 * q->y * q->z + 2 * q->w * q->x,
1 - 2 * q->x * q->x - 2 * q->y * q->y,
}};
*result_out = result;
}
extern "C" void
math_matrix_3x3_f64_identity(struct xrt_matrix_3x3_f64 *mat)
{