From 704a11cbc01865b4e2c0a424a78278991db701da Mon Sep 17 00:00:00 2001 From: josh Date: Wed, 22 May 2024 20:27:08 -0400 Subject: [PATCH] Fix Matrix3x3::Matrix3x3(Quaternion) --- include/J3ML/LinearAlgebra/Matrix3x3.h | 18 +++++++++++++++++- 1 file changed, 17 insertions(+), 1 deletion(-) diff --git a/include/J3ML/LinearAlgebra/Matrix3x3.h b/include/J3ML/LinearAlgebra/Matrix3x3.h index 814608a..d394ab2 100644 --- a/include/J3ML/LinearAlgebra/Matrix3x3.h +++ b/include/J3ML/LinearAlgebra/Matrix3x3.h @@ -81,6 +81,19 @@ namespace J3ML::LinearAlgebra { m[2][0] = -cx*cz*sy + sx*sz; m[2][1] = cz*sx + cx*sy*sz; m[2][2] = cx*cy; } + + template + void SetMatrixRotatePart(Matrix &m, const Quaternion &q) + { + // See https://www.geometrictools.com/Documentation/LinearAlgebraicQuaternions.pdf . + + assert(q.IsNormalized(1e-3f)); + const float x = q.x; const float y = q.y; const float z = q.z; const float w = q.w; + m[0][0] = 1 - 2*(y*y + z*z); m[0][1] = 2*(x*y - z*w); m[0][2] = 2*(x*y + y*w); + m[1][0] = 2*(x*y + z*w); m[1][1] = 1 - 2*(x*x + z*z); m[1][2] = 2*(y*z - x*w); + m[2][0] = 2*(x*z - y*w); m[2][1] = 2*(y*z + x*w); m[2][2] = 1 - 2*(x*x + y*y); + } + class Quaternion; /// A 3-by-3 matrix for linear transformations of 3D geometry. @@ -207,7 +220,10 @@ namespace J3ML::LinearAlgebra { void SetRotatePart(const Vector3& a, float angle); void SetRotatePart(const AxisAngle& axisAngle); /// Sets this matrix to perform the rotation expressed by the given quaternion. - void SetRotatePart(const Quaternion& quat); + void SetRotatePart(const Quaternion& quat) + { + SetMatrixRotatePart(*this, quat); + } /// Returns the given row. /** @param row The zero-based index [0, 2] of the row to get. */