251 lines
7.8 KiB
C++
251 lines
7.8 KiB
C++
// Copyright (c) 2012-2022 Wojciech Figat. All rights reserved.
|
|
|
|
#include "Matrix3x3.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(
|
|
1.0f, 0.0f, 0.0f,
|
|
0.0f, 1.0f, 0.0f,
|
|
0.0f, 0.0f, 1.0f);
|
|
|
|
Matrix3x3::Matrix3x3(const Matrix& matrix)
|
|
{
|
|
Platform::MemoryCopy(&M11, &matrix.M11, sizeof(Float3));
|
|
Platform::MemoryCopy(&M21, &matrix.M21, sizeof(Float3));
|
|
Platform::MemoryCopy(&M31, &matrix.M31, sizeof(Float3));
|
|
}
|
|
|
|
String Matrix3x3::ToString() const
|
|
{
|
|
return String::Format(TEXT("{}"), *this);
|
|
}
|
|
|
|
void Matrix3x3::NormalizeScale()
|
|
{
|
|
const float scaleX = 1.0f / Float3(M11, M21, M31).Length();
|
|
const float scaleY = 1.0f / Float3(M12, M22, M32).Length();
|
|
const float scaleZ = 1.0f / Float3(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;
|
|
const float d12 = value.M21 * value.M33 + value.M23 * -value.M31;
|
|
const float d13 = value.M21 * value.M32 + value.M22 * -value.M31;
|
|
|
|
float det = value.M11 * d11 - value.M12 * d12 + value.M13 * d13;
|
|
if (Math::Abs(det) < ZeroTolerance)
|
|
{
|
|
result = Zero;
|
|
return;
|
|
}
|
|
|
|
det = 1.0f / det;
|
|
|
|
const float d21 = value.M12 * value.M33 + value.M13 * -value.M32;
|
|
const float d22 = value.M11 * value.M33 + value.M13 * -value.M31;
|
|
const float d23 = value.M11 * value.M32 + value.M12 * -value.M31;
|
|
|
|
const float d31 = value.M12 * value.M23 - value.M13 * value.M22;
|
|
const float d32 = value.M11 * value.M23 - value.M13 * value.M21;
|
|
const float d33 = value.M11 * value.M22 - value.M12 * value.M21;
|
|
|
|
result = Matrix3x3(
|
|
+d11 * det,
|
|
-d21 * det,
|
|
+d31 * det,
|
|
-d12 * det,
|
|
+d22 * det,
|
|
-d32 * det,
|
|
+d13 * det,
|
|
-d23 * det,
|
|
+d33 * det
|
|
);
|
|
}
|
|
|
|
void Matrix3x3::Transpose(const Matrix3x3& value, Matrix3x3& result)
|
|
{
|
|
result = Matrix3x3(
|
|
value.M11,
|
|
value.M21,
|
|
value.M31,
|
|
value.M12,
|
|
value.M22,
|
|
value.M32,
|
|
value.M13,
|
|
value.M23,
|
|
value.M33
|
|
);
|
|
}
|
|
|
|
void Matrix3x3::Add(const Matrix3x3& left, const Matrix3x3& right, Matrix3x3& result)
|
|
{
|
|
result.M11 = left.M11 + right.M11;
|
|
result.M12 = left.M12 + right.M12;
|
|
result.M13 = left.M13 + right.M13;
|
|
result.M21 = left.M21 + right.M21;
|
|
result.M22 = left.M22 + right.M22;
|
|
result.M23 = left.M23 + right.M23;
|
|
result.M31 = left.M31 + right.M31;
|
|
result.M32 = left.M32 + right.M32;
|
|
result.M33 = left.M33 + right.M33;
|
|
}
|
|
|
|
void Matrix3x3::Subtract(const Matrix3x3& left, const Matrix3x3& right, Matrix3x3& result)
|
|
{
|
|
result.M11 = left.M11 - right.M11;
|
|
result.M12 = left.M12 - right.M12;
|
|
result.M13 = left.M13 - right.M13;
|
|
result.M21 = left.M21 - right.M21;
|
|
result.M22 = left.M22 - right.M22;
|
|
result.M23 = left.M23 - right.M23;
|
|
result.M31 = left.M31 - right.M31;
|
|
result.M32 = left.M32 - right.M32;
|
|
result.M33 = left.M33 - right.M33;
|
|
}
|
|
|
|
void Matrix3x3::Multiply(const Matrix3x3& left, float right, Matrix3x3& result)
|
|
{
|
|
result.M11 = left.M11 * right;
|
|
result.M12 = left.M12 * right;
|
|
result.M13 = left.M13 * right;
|
|
result.M21 = left.M21 * right;
|
|
result.M22 = left.M22 * right;
|
|
result.M23 = left.M23 * right;
|
|
result.M31 = left.M31 * right;
|
|
result.M32 = left.M32 * right;
|
|
result.M33 = left.M33 * right;
|
|
}
|
|
|
|
void Matrix3x3::Multiply(const Matrix3x3& left, const Matrix3x3& right, Matrix3x3& result)
|
|
{
|
|
result = Matrix3x3(
|
|
left.M11 * right.M11 + left.M12 * right.M21 + left.M13 * right.M31,
|
|
left.M11 * right.M12 + left.M12 * right.M22 + left.M13 * right.M32,
|
|
left.M11 * right.M13 + left.M12 * right.M23 + left.M13 * right.M33,
|
|
left.M21 * right.M11 + left.M22 * right.M21 + left.M23 * right.M31,
|
|
left.M21 * right.M12 + left.M22 * right.M22 + left.M23 * right.M32,
|
|
left.M21 * right.M13 + left.M22 * right.M23 + left.M23 * right.M33,
|
|
left.M31 * right.M11 + left.M32 * right.M21 + left.M33 * right.M31,
|
|
left.M31 * right.M12 + left.M32 * right.M22 + left.M33 * right.M32,
|
|
left.M31 * right.M13 + left.M32 * right.M23 + left.M33 * right.M33
|
|
);
|
|
}
|
|
|
|
void Matrix3x3::Divide(const Matrix3x3& left, float right, Matrix3x3& result)
|
|
{
|
|
ASSERT(!Math::IsZero(right));
|
|
const float inv = 1.0f / right;
|
|
|
|
result.M11 = left.M11 * inv;
|
|
result.M12 = left.M12 * inv;
|
|
result.M13 = left.M13 * inv;
|
|
result.M21 = left.M21 * inv;
|
|
result.M22 = left.M22 * inv;
|
|
result.M23 = left.M23 * inv;
|
|
result.M31 = left.M31 * inv;
|
|
result.M32 = left.M32 * inv;
|
|
result.M33 = left.M33 * inv;
|
|
}
|
|
|
|
void Matrix3x3::Divide(const Matrix3x3& left, const Matrix3x3& right, Matrix3x3& result)
|
|
{
|
|
result.M11 = left.M11 / right.M11;
|
|
result.M12 = left.M12 / right.M12;
|
|
result.M13 = left.M13 / right.M13;
|
|
result.M21 = left.M21 / right.M21;
|
|
result.M22 = left.M22 / right.M22;
|
|
result.M23 = left.M23 / right.M23;
|
|
result.M31 = left.M31 / right.M31;
|
|
result.M32 = left.M32 / right.M32;
|
|
result.M33 = left.M33 / right.M33;
|
|
}
|
|
|
|
void Matrix3x3::RotationQuaternion(const Quaternion& rotation, Matrix3x3& result)
|
|
{
|
|
const float xx = rotation.X * rotation.X;
|
|
const float yy = rotation.Y * rotation.Y;
|
|
const float zz = rotation.Z * rotation.Z;
|
|
const float xy = rotation.X * rotation.Y;
|
|
const float zw = rotation.Z * rotation.W;
|
|
const float zx = rotation.Z * rotation.X;
|
|
const float yw = rotation.Y * rotation.W;
|
|
const float yz = rotation.Y * rotation.Z;
|
|
const float xw = rotation.X * rotation.W;
|
|
|
|
result.M11 = 1.0f - 2.0f * (yy + zz);
|
|
result.M12 = 2.0f * (xy + zw);
|
|
result.M13 = 2.0f * (zx - yw);
|
|
|
|
result.M21 = 2.0f * (xy - zw);
|
|
result.M22 = 1.0f - 2.0f * (zz + xx);
|
|
result.M23 = 2.0f * (yz + xw);
|
|
|
|
result.M31 = 2.0f * (zx + yw);
|
|
result.M32 = 2.0f * (yz - xw);
|
|
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
|
|
Math::NearEqual(M11, other.M11) &&
|
|
Math::NearEqual(M12, other.M12) &&
|
|
Math::NearEqual(M13, other.M13) &&
|
|
Math::NearEqual(M21, other.M21) &&
|
|
Math::NearEqual(M22, other.M22) &&
|
|
Math::NearEqual(M23, other.M23) &&
|
|
Math::NearEqual(M31, other.M31) &&
|
|
Math::NearEqual(M32, other.M32) &&
|
|
Math::NearEqual(M33, other.M33);
|
|
}
|