diff --git a/Source/Engine/Core/Math/Matrix3x3.cpp b/Source/Engine/Core/Math/Matrix3x3.cpp index fd5673154..7296a36d1 100644 --- a/Source/Engine/Core/Math/Matrix3x3.cpp +++ b/Source/Engine/Core/Math/Matrix3x3.cpp @@ -201,6 +201,40 @@ void Matrix3x3::RotationQuaternion(const Quaternion& rotation, Matrix3x3& result result.M33 = 1.0f - 2.0f * (yy + xx); } +void Matrix3x3::Decompose(Float3& scale, Matrix3x3& rotation) const +{ + // Scaling is the length of the rows + scale = Float3( + Math::Sqrt(M11 * M11 + M12 * M12 + M13 * M13), + Math::Sqrt(M21 * M21 + M22 * M22 + M23 * M23), + Math::Sqrt(M31 * M31 + M32 * M32 + M33 * M33)); + + // If any of the scaling factors are zero, than the rotation matrix can not exist + rotation = Identity; + if (scale.IsAnyZero()) + return; + + // Calculate an perfect orthonormal matrix (no reflections) + const auto at = Float3(M31 / scale.Z, M32 / scale.Z, M33 / scale.Z); + const auto up = Float3::Cross(at, Float3(M11 / scale.X, M12 / scale.X, M13 / scale.X)); + const auto right = Float3::Cross(up, at); + rotation.SetRight(right); + rotation.SetUp(up); + rotation.SetBackward(at); + + // In case of reflexions + scale.X = Float3::Dot(right, GetRight()) > 0.0f ? scale.X : -scale.X; + scale.Y = Float3::Dot(up, GetUp()) > 0.0f ? scale.Y : -scale.Y; + scale.Z = Float3::Dot(at, GetBackward()) > 0.0f ? scale.Z : -scale.Z; +} + +void Matrix3x3::Decompose(Float3& scale, Quaternion& rotation) const +{ + Matrix3x3 rotationMatrix; + Decompose(scale, rotationMatrix); + Quaternion::RotationMatrix(rotationMatrix, rotation); +} + bool Matrix3x3::operator==(const Matrix3x3& other) const { return diff --git a/Source/Engine/Core/Math/Matrix3x3.h b/Source/Engine/Core/Math/Matrix3x3.h index 9094b313d..1d92c622c 100644 --- a/Source/Engine/Core/Math/Matrix3x3.h +++ b/Source/Engine/Core/Math/Matrix3x3.h @@ -120,6 +120,90 @@ public: String ToString() const; public: + // Gets the up Float3 of the matrix; that is M21, M22, and M23. + Float3 GetUp() const + { + return Float3(M21, M22, M23); + } + + // Sets Float3 of the matrix; that is M21, M22, and M23. + void SetUp(const Float3& value) + { + M21 = value.X; + M22 = value.Y; + M23 = value.Z; + } + + // Gets the down Float3 of the matrix; that is -M21, -M22, and -M23. + Float3 GetDown() const + { + return -Float3(M21, M22, M23); + } + + // Sets the down Float3 of the matrix; that is -M21, -M22, and -M23. + void SetDown(const Float3& value) + { + M21 = -value.X; + M22 = -value.Y; + M23 = -value.Z; + } + + // Gets the right Float3 of the matrix; that is M11, M12, and M13. + Float3 GetRight() const + { + return Float3(M11, M12, M13); + } + + // Sets the right Float3 of the matrix; that is M11, M12, and M13. + void SetRight(const Float3& value) + { + M11 = value.X; + M12 = value.Y; + M13 = value.Z; + } + + // Gets the left Float3 of the matrix; that is -M11, -M12, and -M13. + Float3 GetLeft() const + { + return -Float3(M11, M12, M13); + } + + // Sets the left Float3 of the matrix; that is -M11, -M12, and -M13. + void SetLeft(const Float3& value) + { + M11 = -value.X; + M12 = -value.Y; + M13 = -value.Z; + } + + // Gets the forward Float3 of the matrix; that is -M31, -M32, and -M33. + Float3 GetForward() const + { + return -Float3(M31, M32, M33); + } + + // Sets the forward Float3 of the matrix; that is -M31, -M32, and -M33. + void SetForward(const Float3& value) + { + M31 = -value.X; + M32 = -value.Y; + M33 = -value.Z; + } + + // Gets the backward Float3 of the matrix; that is M31, M32, and M33. + Float3 GetBackward() const + { + return Float3(M31, M32, M33); + } + + // Sets the backward Float3 of the matrix; that is M31, M32, and M33. + void SetBackward(const Float3& value) + { + M31 = value.X; + M32 = value.Y; + M33 = value.Z; + } + // Gets the first row in the matrix; that is M11, M12 and M13. Float3 GetRow1() const { @@ -497,6 +581,22 @@ public: /// The created rotation matrix. static void RotationQuaternion(const Quaternion& rotation, Matrix3x3& result); + /// + /// Decomposes a matrix into a scale and rotation. + /// + /// When the method completes, contains the scaling component of the decomposed matrix. + /// When the method completes, contains the rotation component of the decomposed matrix. + /// This method is designed to decompose an scale-rotation transformation matrix only. + void Decompose(Float3& scale, Matrix3x3& rotation) const; + + /// + /// Decomposes a matrix into a scale and rotation. + /// + /// When the method completes, contains the scaling component of the decomposed matrix. + /// When the method completes, contains the rotation component of the decomposed matrix. + /// This method is designed to decompose an scale-rotation transformation matrix only. + void Decompose(Float3& scale, Quaternion& rotation) const; + public: /// /// Tests for equality between two objects. diff --git a/Source/Engine/Core/Math/Transform.cpp b/Source/Engine/Core/Math/Transform.cpp index 2a5b6c038..faca8a211 100644 --- a/Source/Engine/Core/Math/Transform.cpp +++ b/Source/Engine/Core/Math/Transform.cpp @@ -2,11 +2,17 @@ #include "Transform.h" #include "Matrix.h" -#include "Vector2.h" +#include "Matrix3x3.h" #include "../Types/String.h" Transform Transform::Identity(Vector3(0, 0, 0)); +Transform::Transform(const Vector3& position, const Matrix3x3& rotationScale) + : Translation(position) +{ + rotationScale.Decompose(Scale, Orientation); +} + String Transform::ToString() const { return String::Format(TEXT("{}"), *this); @@ -122,11 +128,10 @@ void Transform::LocalToWorld(const Transform& other, Transform& result) const result.Translation = Vector3(tmp.X + Translation.X, tmp.Y + Translation.Y, tmp.Z + Translation.Z); } -Vector3 Transform::LocalToWorldVector(const Vector3& vector) const +void Transform::LocalToWorldVector(const Vector3& vector, Vector3& result) const { - Vector3 result = vector * Scale; - Vector3::Transform(result, Orientation, result); - return result; + Vector3 tmp = vector * Scale; + Vector3::Transform(tmp, Orientation, result); } void Transform::LocalToWorld(const Vector3& point, Vector3& result) const @@ -145,9 +150,7 @@ void Transform::WorldToLocal(const Transform& other, Transform& result) const invScale.Y = 1.0f / invScale.Y; if (invScale.Z != 0.0f) invScale.Z = 1.0f / invScale.Z; - const Quaternion invRotation = Orientation.Conjugated(); - Quaternion::Multiply(invRotation, other.Orientation, result.Orientation); result.Orientation.Normalize(); Float3::Multiply(other.Scale, invScale, result.Scale); @@ -171,7 +174,7 @@ void Transform::WorldToLocal(const Vector3& point, Vector3& result) const result *= invScale; } -Vector3 Transform::WorldToLocalVector(const Vector3& vector) const +void Transform::WorldToLocalVector(const Vector3& vector, Vector3& result) const { Float3 invScale = Scale; if (invScale.X != 0.0f) @@ -180,13 +183,9 @@ Vector3 Transform::WorldToLocalVector(const Vector3& vector) const invScale.Y = 1.0f / invScale.Y; if (invScale.Z != 0.0f) invScale.Z = 1.0f / invScale.Z; - const Quaternion invRotation = Orientation.Conjugated(); - - Vector3 result; Vector3::Transform(vector, invRotation, result); - - return result * invScale; + result *= invScale; } Transform Transform::Lerp(const Transform& t1, const Transform& t2, float amount) diff --git a/Source/Engine/Core/Math/Transform.cs b/Source/Engine/Core/Math/Transform.cs index f397f7a94..fdeabaa12 100644 --- a/Source/Engine/Core/Math/Transform.cs +++ b/Source/Engine/Core/Math/Transform.cs @@ -249,6 +249,41 @@ namespace FlaxEngine return vector; } + /// + /// Perform transformation of the given transform in local space + /// + /// Local space transform + /// World space transform + public void LocalToWorld(ref Transform other, out Transform result) + { + Quaternion.Multiply(ref Orientation, ref other.Orientation, out result.Orientation); + Float3.Multiply(ref Scale, ref other.Scale, out result.Scale); + result.Translation = LocalToWorld(other.Translation); + } + + /// + /// Perform transformation of the given point in local space + /// + /// Local space point + /// World space point + public void LocalToWorld(ref Vector3 point, out Vector3 result) + { + Vector3 tmp = point * Scale; + Vector3.Transform(ref tmp, ref Orientation, out result); + result += Translation; + } + + /// + /// Performs transformation of the given vector in local space to the world space of this transform. + /// + /// The local space vector. + /// World space vector + public void LocalToWorldVector(ref Vector3 vector, out Vector3 result) + { + Vector3 tmp = vector * Scale; + Vector3.Transform(ref tmp, ref Orientation, out result); + } + /// /// Perform transformation of the given points in local space /// @@ -276,14 +311,12 @@ namespace FlaxEngine invScale.Y = 1.0f / invScale.Y; if (invScale.Z != 0.0f) invScale.Z = 1.0f / invScale.Z; - Transform result; result.Orientation = Orientation; result.Orientation.Invert(); Quaternion.Multiply(ref result.Orientation, ref other.Orientation, out result.Orientation); Float3.Multiply(ref other.Scale, ref invScale, out result.Scale); result.Translation = WorldToLocal(other.Translation); - return result; } @@ -301,13 +334,9 @@ namespace FlaxEngine invScale.Y = 1.0f / invScale.Y; if (invScale.Z != 0.0f) invScale.Z = 1.0f / invScale.Z; - - Quaternion invRotation = Orientation; - invRotation.Invert(); - + Quaternion invRotation = Orientation.Conjugated(); Vector3 result = point - Translation; Vector3.Transform(ref result, ref invRotation, out result); - return result * invScale; } @@ -325,15 +354,51 @@ namespace FlaxEngine invScale.Y = 1.0f / invScale.Y; if (invScale.Z != 0.0f) invScale.Z = 1.0f / invScale.Z; - - Quaternion invRotation = Orientation; - invRotation.Invert(); - + Quaternion invRotation = Orientation.Conjugated(); Vector3.Transform(ref vector, ref invRotation, out var result); - return result * invScale; } + /// + /// Perform transformation of the given point in world space + /// + /// World space point + /// When the method completes, contains the local space point. + /// Local space point + public void WorldToLocal(ref Vector3 point, out Vector3 result) + { + var invScale = Scale; + if (invScale.X != 0.0f) + invScale.X = 1.0f / invScale.X; + if (invScale.Y != 0.0f) + invScale.Y = 1.0f / invScale.Y; + if (invScale.Z != 0.0f) + invScale.Z = 1.0f / invScale.Z; + Quaternion invRotation = Orientation.Conjugated(); + Vector3 tmp = point - Translation; + Vector3.Transform(ref tmp, ref invRotation, out result); + result *= invScale; + } + + /// + /// Perform transformation of the given vector in world space + /// + /// World space vector + /// Local space vector + public void WorldToLocalVector(ref Vector3 vector, out Vector3 result) + { + var invScale = Scale; + if (invScale.X != 0.0f) + invScale.X = 1.0f / invScale.X; + if (invScale.Y != 0.0f) + invScale.Y = 1.0f / invScale.Y; + if (invScale.Z != 0.0f) + invScale.Z = 1.0f / invScale.Z; + Quaternion invRotation = Orientation.Conjugated(); + Vector3.Transform(ref vector, ref invRotation, out result); + result *= invScale; + } + /// /// Perform transformation of the given points in world space /// @@ -348,10 +413,7 @@ namespace FlaxEngine invScale.Y = 1.0f / invScale.Y; if (invScale.Z != 0.0f) invScale.Z = 1.0f / invScale.Z; - - Quaternion invRotation = Orientation; - invRotation.Invert(); - + Quaternion invRotation = Orientation.Conjugated(); for (int i = 0; i < points.Length; i++) { result[i] = points[i] - Translation; diff --git a/Source/Engine/Core/Math/Transform.h b/Source/Engine/Core/Math/Transform.h index 33a031fb1..33cf83d57 100644 --- a/Source/Engine/Core/Math/Transform.h +++ b/Source/Engine/Core/Math/Transform.h @@ -80,6 +80,8 @@ public: { } + Transform(const Vector3& position, const Matrix3x3& rotationScale); + public: String ToString() const; @@ -192,12 +194,24 @@ public: return result; } + /// + /// Performs transformation of the given vector in local space to the world space of this transform. + /// + /// The local space vector. + /// The world space vector. + void LocalToWorldVector(const Vector3& vector, Vector3& result) const; + /// /// Performs transformation of the given vector in local space to the world space of this transform. /// /// The local space vector. /// The world space vector. - Vector3 LocalToWorldVector(const Vector3& vector) const; + Vector3 LocalToWorldVector(const Vector3& vector) const + { + Vector3 result; + LocalToWorldVector(vector, result); + return result; + } /// /// Performs transformation of the given point in local space to the world space of this transform. @@ -244,12 +258,24 @@ public: return result; } + /// + /// Performs transformation of the given vector in world space to the local space of this transform. + /// + /// The world space vector. + /// The local space vector. + void WorldToLocalVector(const Vector3& vector, Vector3& result) const; + /// /// Performs transformation of the given vector in world space to the local space of this transform. /// /// The world space vector. /// The local space vector. - Vector3 WorldToLocalVector(const Vector3& vector) const; + Vector3 WorldToLocalVector(const Vector3& vector) const + { + Vector3 result; + WorldToLocalVector(vector, result); + return result; + } public: FORCE_INLINE Transform operator*(const Transform& other) const