Fix Matrix3x3::Matrix3x3(Quaternion)

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 <typename Matrix>
+    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. */