Add more utilities for using Matrix3x3

Source/Engine/Core/Math/Matrix.cpp

@@ -1,6 +1,7 @@
 // Copyright (c) 2012-2022 Wojciech Figat. All rights reserved.
 
 #include "Matrix.h"
+#include "Matrix3x3.h"
 #include "Vector2.h"
 #include "Quaternion.h"
 #include "Transform.h"
@@ -15,6 +16,17 @@ const Matrix Matrix::Identity(
     0.0f, 0.0f, 1.0f, 0.0f,
     0.0f, 0.0f, 0.0f, 1.0f);
 
+Matrix::Matrix(const Matrix3x3& matrix)
+{
+    Platform::MemoryCopy(&M11, &matrix.M11, sizeof(Vector3));
+    Platform::MemoryCopy(&M21, &matrix.M21, sizeof(Vector3));
+    Platform::MemoryCopy(&M31, &matrix.M31, sizeof(Vector3));
+    M14 = 0.0f;
+    M24 = 0.0f;
+    M34 = 0.0f;
+    M44 = 1.0f;
+}
+
 String Matrix::ToString() const
 {
     return String::Format(TEXT("{}"), *this);
@@ -284,8 +296,6 @@ void Matrix::LookAt(const Vector3& eye, const Vector3& target, const Vector3& up
     xaxis.Normalize();
     Vector3::Cross(zaxis, xaxis, yaxis);
 
-    result = Identity;
-
     result.M11 = xaxis.X;
     result.M21 = xaxis.Y;
     result.M31 = xaxis.Z;
@@ -298,9 +308,14 @@ void Matrix::LookAt(const Vector3& eye, const Vector3& target, const Vector3& up
     result.M23 = zaxis.Y;
     result.M33 = zaxis.Z;
 
+    result.M14 = 0.0f;
+    result.M24 = 0.0f;
+    result.M34 = 0.0f;
+
     result.M41 = -Vector3::Dot(xaxis, eye);
     result.M42 = -Vector3::Dot(yaxis, eye);
     result.M43 = -Vector3::Dot(zaxis, eye);
+    result.M44 = 1.0f;
 }
 
 void Matrix::OrthoOffCenter(float left, float right, float bottom, float top, float zNear, float zFar, Matrix& result)
@@ -587,33 +602,7 @@ void Matrix::Transformation2D(Vector2& scalingCenter, float scalingRotation, con
 Matrix Matrix::CreateWorld(const Vector3& position, const Vector3& forward, const Vector3& up)
 {
     Matrix result;
-    Vector3 vector3, vector31, vector32;
-
-    Vector3::Normalize(forward, vector3);
-    vector3.Negate();
-    Vector3::Normalize(Vector3::Cross(up, vector3), vector31);
-    Vector3::Cross(vector3, vector31, vector32);
-
-    result.M11 = vector31.X;
-    result.M12 = vector31.Y;
-    result.M13 = vector31.Z;
-    result.M14 = 0.0f;
-
-    result.M21 = vector32.X;
-    result.M22 = vector32.Y;
-    result.M23 = vector32.Z;
-    result.M24 = 0.0f;
-
-    result.M31 = vector3.X;
-    result.M32 = vector3.Y;
-    result.M33 = vector3.Z;
-    result.M34 = 0.0f;
-
-    result.M41 = position.X;
-    result.M42 = position.Y;
-    result.M43 = position.Z;
-    result.M44 = 1.0f;
-
+    CreateWorld(position, forward, up, result);
     return result;
 }
 
@@ -649,41 +638,9 @@ void Matrix::CreateWorld(const Vector3& position, const Vector3& forward, const
 
 Matrix Matrix::CreateFromAxisAngle(const Vector3& axis, float angle)
 {
-    Matrix matrix;
-
-    const float x = axis.X;
-    const float y = axis.Y;
-    const float z = axis.Z;
-    const float single = Math::Sin(angle);
-    const float single1 = Math::Cos(angle);
-    const float single2 = x * x;
-    const float single3 = y * y;
-    const float single4 = z * z;
-    const float single5 = x * y;
-    const float single6 = x * z;
-    const float single7 = y * z;
-
-    matrix.M11 = single2 + single1 * (1.0f - single2);
-    matrix.M12 = single5 - single1 * single5 + single * z;
-    matrix.M13 = single6 - single1 * single6 - single * y;
-    matrix.M14 = 0.0f;
-
-    matrix.M21 = single5 - single1 * single5 - single * z;
-    matrix.M22 = single3 + single1 * (1.0f - single3);
-    matrix.M23 = single7 - single1 * single7 + single * x;
-    matrix.M24 = 0.0f;
-
-    matrix.M31 = single6 - single1 * single6 + single * y;
-    matrix.M32 = single7 - single1 * single7 - single * x;
-    matrix.M33 = single4 + single1 * (1.0f - single4);
-    matrix.M34 = 0.0f;
-
-    matrix.M41 = 0.0f;
-    matrix.M42 = 0.0f;
-    matrix.M43 = 0.0f;
-    matrix.M44 = 1.0f;
-
-    return matrix;
+    Matrix result;
+    CreateFromAxisAngle(axis, angle, result);
+    return result;
 }
 
 void Matrix::CreateFromAxisAngle(const Vector3& axis, float angle, Matrix& result)

Source/Engine/Core/Math/Matrix.h

@@ -150,6 +150,8 @@ public:
         Platform::MemoryCopy(Raw, values, sizeof(float) * 16);
     }
 
+    explicit Matrix(const Matrix3x3& matrix);
+
 public:
 
     String ToString() const;

Source/Engine/Core/Math/Matrix3x3.cpp

@@ -1,8 +1,9 @@
 // Copyright (c) 2012-2022 Wojciech Figat. All rights reserved.
 
 #include "Matrix3x3.h"
-#include "../Types/String.h"
+#include "Matrix.h"
 #include "Quaternion.h"
+#include "../Types/String.h"
 
 const Matrix3x3 Matrix3x3::Zero(0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f);
 const Matrix3x3 Matrix3x3::Identity(
@@ -10,11 +11,37 @@ const Matrix3x3 Matrix3x3::Identity(
     0.0f, 1.0f, 0.0f,
     0.0f, 0.0f, 1.0f);
 
+Matrix3x3::Matrix3x3(const Matrix& matrix)
+{
+    Platform::MemoryCopy(&M11, &matrix.M11, sizeof(Vector3));
+    Platform::MemoryCopy(&M21, &matrix.M21, sizeof(Vector3));
+    Platform::MemoryCopy(&M31, &matrix.M31, sizeof(Vector3));
+}
+
 String Matrix3x3::ToString() const
 {
     return String::Format(TEXT("{}"), *this);
 }
 
+void Matrix3x3::NormalizeScale()
+{
+    const float scaleX = 1.0f / Vector3(M11, M21, M31).Length();
+    const float scaleY = 1.0f / Vector3(M12, M22, M32).Length();
+    const float scaleZ = 1.0f / Vector3(M13, M23, M33).Length();
+
+    M11 *= scaleX;
+    M21 *= scaleX;
+    M31 *= scaleX;
+
+    M12 *= scaleY;
+    M22 *= scaleY;
+    M32 *= scaleY;
+
+    M13 *= scaleZ;
+    M23 *= scaleZ;
+    M33 *= scaleZ;
+}
+
 void Matrix3x3::Invert(const Matrix3x3& value, Matrix3x3& result)
 {
     const float d11 = value.M22 * value.M33 + value.M23 * -value.M32;

Source/Engine/Core/Math/Matrix3x3.h

@@ -113,6 +113,12 @@ public:
         Platform::MemoryCopy(Raw, values, sizeof(float) * 9);
     }
 
+    /// <summary>
+    /// Initializes a new instance of the <see cref="Matrix3x3"/> struct.
+    /// </summary>
+    /// <param name="matrix">The 4 by 4 matrix to initialize from with rotation and scale (translation is skipped).</param>
+    explicit Matrix3x3(const Matrix& matrix);
+
 public:
 
     String ToString() const;
@@ -255,6 +261,11 @@ public:
         Transpose(*this, *this);
     }
 
+    /// <summary>
+    /// Removes any scaling from the matrix by performing the normalization (each row magnitude is 1).
+    /// </summary>
+    void NormalizeScale();
+
 public:
 
     /// <summary>

Source/Engine/Core/Math/OrientedBoundingBox.cpp

@@ -8,11 +8,18 @@
 
 OrientedBoundingBox::OrientedBoundingBox(const BoundingBox& bb)
 {
-    const Vector3 center = bb.Minimum + (bb.Maximum - bb.Minimum) / 2.0f;
+    const Vector3 center = bb.Minimum + (bb.Maximum - bb.Minimum) * 0.5f;
     Extents = bb.Maximum - center;
     Matrix::Translation(center, Transformation);
 }
 
+OrientedBoundingBox::OrientedBoundingBox(const Vector3& extents, const Matrix3x3& rotationScale, const Vector3& translation)
+    : Extents(extents)
+    , Transformation(rotationScale)
+{
+    Transformation.SetTranslation(translation);
+}
+
 OrientedBoundingBox::OrientedBoundingBox(Vector3 points[], int32 pointCount)
 {
     ASSERT(points && pointCount > 0);
@@ -104,6 +111,12 @@ void OrientedBoundingBox::GetBoundingBox(BoundingBox& result) const
     BoundingBox::FromPoints(corners, 8, result);
 }
 
+void OrientedBoundingBox::Transform(const Matrix& matrix)
+{
+    const Matrix tmp = Transformation;
+    Matrix::Multiply(tmp, matrix, Transformation);
+}
+
 ContainmentType OrientedBoundingBox::Contains(const Vector3& point, float* distance) const
 {
     // Transform the point into the obb coordinates

Source/Engine/Core/Math/OrientedBoundingBox.h

@@ -40,6 +40,8 @@ public:
         Transformation = transformation;
     }
 
+    OrientedBoundingBox(const Vector3& extents, const Matrix3x3& rotationScale, const Vector3& translation);
+
     // Init
     // @param minimum The minimum vertex of the bounding box.
     // @param maximum The maximum vertex of the bounding box.
@@ -99,10 +101,7 @@ public:
 
     // Transforms this box using a transformation matrix.
     // @param mat The transformation matrix.
-    void Transform(const Matrix& mat)
-    {
-        Transformation *= mat;
-    }
+    void Transform(const Matrix& matrix);
 
     // Scales the OBB by scaling its Extents without affecting the Transformation matrix.
     // By keeping Transformation matrix scaling-free, the collision detection methods will be more accurate.

Source/Engine/Core/Math/Vector3.cpp

@@ -9,6 +9,7 @@
 #include "Color.h"
 #include "Quaternion.h"
 #include "Matrix.h"
+#include "Matrix3x3.h"
 #include "Int2.h"
 #include "Int3.h"
 #include "Int4.h"
@@ -215,9 +216,15 @@ void Vector3::Transform(const Vector3& vector, const Matrix& transform, Vector3&
 void Vector3::Transform(const Vector3* vectors, const Matrix& transform, Vector3* results, int32 vectorsCount)
 {
     for (int32 i = 0; i < vectorsCount; i++)
-    {
         Transform(vectors[i], transform, results[i]);
-    }
+}
+
+void Vector3::Transform(const Vector3& vector, const Matrix3x3& transform, Vector3& result)
+{
+    result = Vector3(
+        vector.X * transform.M11 + vector.Y * transform.M21 + vector.Z * transform.M31,
+        vector.X * transform.M12 + vector.Y * transform.M22 + vector.Z * transform.M32,
+        vector.X * transform.M13 + vector.Y * transform.M23 + vector.Z * transform.M33);
 }
 
 Vector3 Vector3::Transform(const Vector3& vector, const Matrix& transform)

Source/Engine/Core/Math/Vector3.h

@@ -11,6 +11,7 @@ struct Double3;
 struct Double4;
 struct Quaternion;
 struct Matrix;
+struct Matrix3x3;
 struct Vector2;
 struct Vector4;
 struct Color;
@@ -767,6 +768,12 @@ public:
     // @param vectorsCount Amount of vectors to transform
     static void Transform(const Vector3* vectors, const Matrix& transform, Vector3* results, int32 vectorsCount);
 
+    // Transforms a 3D vector by the given matrix
+    // @param vector The source vector
+    // @param transform The transformation matrix
+    // @param result When the method completes, contains the transformed Vector3
+    static void Transform(const Vector3& vector, const Matrix3x3& transform, Vector3& result);
+
     // Transforms a 3D vector by the given matrix
     // @param vector The source vector
     // @param transform The transformation matrix