summaryrefslogtreecommitdiff
path: root/Runtime/Math/Matrix4x4.cpp
diff options
context:
space:
mode:
Diffstat (limited to 'Runtime/Math/Matrix4x4.cpp')
-rw-r--r--Runtime/Math/Matrix4x4.cpp805
1 files changed, 805 insertions, 0 deletions
diff --git a/Runtime/Math/Matrix4x4.cpp b/Runtime/Math/Matrix4x4.cpp
new file mode 100644
index 0000000..660a6bf
--- /dev/null
+++ b/Runtime/Math/Matrix4x4.cpp
@@ -0,0 +1,805 @@
+#include "UnityPrefix.h"
+#include "Matrix4x4.h"
+#include "Matrix3x3.h"
+#include "Quaternion.h"
+#include "Runtime/Utilities/Utility.h"
+
+using namespace std;
+
+namespace
+{
+ Matrix4x4f CreateIdentityMatrix4x4f ()
+ {
+ Matrix4x4f temp;
+ temp.SetIdentity ();
+ return temp;
+ }
+}
+
+const Matrix4x4f Matrix4x4f::identity = CreateIdentityMatrix4x4f ();
+
+Matrix4x4f::Matrix4x4f (const float data[16])
+{
+ for (int i=0; i<16; i++)
+ m_Data[i] = data[i];
+}
+
+Matrix4x4f::Matrix4x4f (const Matrix3x3f &other)
+{
+ m_Data[0] = other.m_Data[0];
+ m_Data[1] = other.m_Data[1];
+ m_Data[2] = other.m_Data[2];
+ m_Data[3] = 0.0F;
+
+ m_Data[4] = other.m_Data[3];
+ m_Data[5] = other.m_Data[4];
+ m_Data[6] = other.m_Data[5];
+ m_Data[7] = 0.0F;
+
+ m_Data[8] = other.m_Data[6];
+ m_Data[9] = other.m_Data[7];
+ m_Data[10] = other.m_Data[8];
+ m_Data[11] = 0.0F;
+
+ m_Data[12] = 0.0F;
+ m_Data[13] = 0.0F;
+ m_Data[14] = 0.0F;
+ m_Data[15] = 1.0F;
+}
+
+Matrix4x4f& Matrix4x4f::operator = (const Matrix3x3f& other)
+{
+ m_Data[0] = other.m_Data[0];
+ m_Data[1] = other.m_Data[1];
+ m_Data[2] = other.m_Data[2];
+ m_Data[3] = 0.0F;
+
+ m_Data[4] = other.m_Data[3];
+ m_Data[5] = other.m_Data[4];
+ m_Data[6] = other.m_Data[5];
+ m_Data[7] = 0.0F;
+
+ m_Data[8] = other.m_Data[6];
+ m_Data[9] = other.m_Data[7];
+ m_Data[10] = other.m_Data[8];
+ m_Data[11] = 0.0F;
+
+ m_Data[12] = 0.0F;
+ m_Data[13] = 0.0F;
+ m_Data[14] = 0.0F;
+ m_Data[15] = 1.0F;
+ return *this;
+}
+
+bool Matrix4x4f::IsIdentity (float threshold) const
+{
+ if (CompareApproximately (Get (0,0),1.0f, threshold) && CompareApproximately (Get (0,1),0.0f, threshold) && CompareApproximately (Get (0,2),0.0f, threshold) && CompareApproximately (Get (0,3),0.0f, threshold) &&
+ CompareApproximately (Get (1,0),0.0f, threshold) && CompareApproximately (Get (1,1),1.0f, threshold) && CompareApproximately (Get (1,2),0.0f, threshold) && CompareApproximately (Get (1,3),0.0f, threshold) &&
+ CompareApproximately (Get (2,0),0.0f, threshold) && CompareApproximately (Get (2,1),0.0f, threshold) && CompareApproximately (Get (2,2),1.0f, threshold) && CompareApproximately (Get (2,3),0.0f, threshold) &&
+ CompareApproximately (Get (3,0),0.0f, threshold) && CompareApproximately (Get (3,1),0.0f, threshold) && CompareApproximately (Get (3,2),0.0f, threshold) && CompareApproximately (Get (3,3),1.0f, threshold))
+ return true;
+ return false;
+}
+
+double Matrix4x4f::GetDeterminant () const
+{
+ double m00 = Get(0, 0); double m01 = Get(0, 1); double m02 = Get(0, 2); double m03 = Get(0, 3);
+ double m10 = Get(1, 0); double m11 = Get(1, 1); double m12 = Get(1, 2); double m13 = Get(1, 3);
+ double m20 = Get(2, 0); double m21 = Get(2, 1); double m22 = Get(2, 2); double m23 = Get(2, 3);
+ double m30 = Get(3, 0); double m31 = Get(3, 1); double m32 = Get(3, 2); double m33 = Get(3, 3);
+
+ double result =
+ m03 * m12 * m21 * m30 - m02 * m13 * m21 * m30 - m03 * m11 * m22 * m30 + m01 * m13 * m22 * m30 +
+ m02 * m11 * m23 * m30 - m01 * m12 * m23 * m30 - m03 * m12 * m20 * m31 + m02 * m13 * m20 * m31 +
+ m03 * m10 * m22 * m31 - m00 * m13 * m22 * m31 - m02 * m10 * m23 * m31 + m00 * m12 * m23 * m31 +
+ m03 * m11 * m20 * m32 - m01 * m13 * m20 * m32 - m03 * m10 * m21 * m32 + m00 * m13 * m21 * m32 +
+ m01 * m10 * m23 * m32 - m00 * m11 * m23 * m32 - m02 * m11 * m20 * m33 + m01 * m12 * m20 * m33 +
+ m02 * m10 * m21 * m33 - m00 * m12 * m21 * m33 - m01 * m10 * m22 * m33 + m00 * m11 * m22 * m33;
+ return result;
+}
+
+Matrix4x4f& Matrix4x4f::operator *= (const Matrix4x4f& inM1)
+{
+ Assert(&inM1 != this);
+ Matrix4x4f tmp;
+ MultiplyMatrices4x4(this, &inM1, &tmp);
+ *this = tmp;
+ return *this;
+}
+
+void MultiplyMatrices3x4( const Matrix4x4f& lhs, const Matrix4x4f& rhs, Matrix4x4f& res)
+{
+ for (int i=0;i<3;i++)
+ {
+ res.m_Data[i] = lhs.m_Data[i] * rhs.m_Data[0] + lhs.m_Data[i+4] * rhs.m_Data[1] + lhs.m_Data[i+8] * rhs.m_Data[2];// + lhs.m_Data[i+12] * rhs.m_Data[3];
+ res.m_Data[i+4] = lhs.m_Data[i] * rhs.m_Data[4] + lhs.m_Data[i+4] * rhs.m_Data[5] + lhs.m_Data[i+8] * rhs.m_Data[6];// + lhs.m_Data[i+12] * rhs.m_Data[7];
+ res.m_Data[i+8] = lhs.m_Data[i] * rhs.m_Data[8] + lhs.m_Data[i+4] * rhs.m_Data[9] + lhs.m_Data[i+8] * rhs.m_Data[10];// + lhs.m_Data[i+12] * rhs.m_Data[11];
+ res.m_Data[i+12] = lhs.m_Data[i] * rhs.m_Data[12] + lhs.m_Data[i+4] * rhs.m_Data[13] + lhs.m_Data[i+8] * rhs.m_Data[14] + lhs.m_Data[i+12];// * rhs.m_Data[15];
+ }
+
+ res.m_Data[3] = 0.0f;
+ res.m_Data[7] = 0.0f;
+ res.m_Data[11] = 0.0f;
+ res.m_Data[15] = 1.0f;
+}
+
+
+Matrix4x4f& Matrix4x4f::SetIdentity ()
+{
+ Get (0, 0) = 1.0; Get (0, 1) = 0.0; Get (0, 2) = 0.0; Get (0, 3) = 0.0;
+ Get (1, 0) = 0.0; Get (1, 1) = 1.0; Get (1, 2) = 0.0; Get (1, 3) = 0.0;
+ Get (2, 0) = 0.0; Get (2, 1) = 0.0; Get (2, 2) = 1.0; Get (2, 3) = 0.0;
+ Get (3, 0) = 0.0; Get (3, 1) = 0.0; Get (3, 2) = 0.0; Get (3, 3) = 1.0;
+ return *this;
+}
+
+Matrix4x4f& Matrix4x4f::SetOrthoNormalBasis (const Vector3f& inX, const Vector3f& inY, const Vector3f& inZ)
+{
+ Get (0, 0) = inX[0]; Get (0, 1) = inY[0]; Get (0, 2) = inZ[0]; Get (0, 3) = 0.0;
+ Get (1, 0) = inX[1]; Get (1, 1) = inY[1]; Get (1, 2) = inZ[1]; Get (1, 3) = 0.0;
+ Get (2, 0) = inX[2]; Get (2, 1) = inY[2]; Get (2, 2) = inZ[2]; Get (2, 3) = 0.0;
+ Get (3, 0) = 0.0; Get (3, 1) = 0.0; Get (3, 2) = 0.0; Get (3, 3) = 1.0;
+ return *this;
+}
+
+Matrix4x4f& Matrix4x4f::SetOrthoNormalBasisInverse (const Vector3f& inX, const Vector3f& inY, const Vector3f& inZ)
+{
+ Get (0, 0) = inX[0]; Get (1, 0) = inY[0]; Get (2, 0) = inZ[0]; Get (3, 0) = 0.0;
+ Get (0, 1) = inX[1]; Get (1, 1) = inY[1]; Get (2, 1) = inZ[1]; Get (3, 1) = 0.0;
+ Get (0, 2) = inX[2]; Get (1, 2) = inY[2]; Get (2, 2) = inZ[2]; Get (3, 2) = 0.0;
+ Get (0, 3) = 0.0; Get (1, 3) = 0.0; Get (2, 3) = 0.0; Get (3, 3) = 1.0;
+ return *this;
+}
+
+Matrix4x4f& Matrix4x4f::SetPositionAndOrthoNormalBasis (const Vector3f& inPosition, const Vector3f& inX, const Vector3f& inY, const Vector3f& inZ)
+{
+ Get (0, 0) = inX[0]; Get (0, 1) = inY[0]; Get (0, 2) = inZ[0]; Get (0, 3) = inPosition[0];
+ Get (1, 0) = inX[1]; Get (1, 1) = inY[1]; Get (1, 2) = inZ[1]; Get (1, 3) = inPosition[1];
+ Get (2, 0) = inX[2]; Get (2, 1) = inY[2]; Get (2, 2) = inZ[2]; Get (2, 3) = inPosition[2];
+ Get (3, 0) = 0.0; Get (3, 1) = 0.0; Get (3, 2) = 0.0; Get (3, 3) = 1.0;
+ return *this;
+}
+
+Matrix4x4f& Matrix4x4f::SetScale (const Vector3f& inScale)
+{
+ Get (0, 0) = inScale[0]; Get (0, 1) = 0.0; Get (0, 2) = 0.0; Get (0, 3) = 0.0;
+ Get (1, 0) = 0.0; Get (1, 1) = inScale[1]; Get (1, 2) = 0.0; Get (1, 3) = 0.0;
+ Get (2, 0) = 0.0; Get (2, 1) = 0.0; Get (2, 2) = inScale[2]; Get (2, 3) = 0.0;
+ Get (3, 0) = 0.0; Get (3, 1) = 0.0; Get (3, 2) = 0.0; Get (3, 3) = 1.0;
+ return *this;
+}
+
+Matrix4x4f& Matrix4x4f::Scale (const Vector3f& inScale)
+{
+ Get (0, 0) *= inScale[0];
+ Get (1, 0) *= inScale[0];
+ Get (2, 0) *= inScale[0];
+ Get (3, 0) *= inScale[0];
+
+ Get (0, 1) *= inScale[1];
+ Get (1, 1) *= inScale[1];
+ Get (2, 1) *= inScale[1];
+ Get (3, 1) *= inScale[1];
+
+ Get (0, 2) *= inScale[2];
+ Get (1, 2) *= inScale[2];
+ Get (2, 2) *= inScale[2];
+ Get (3, 2) *= inScale[2];
+ return *this;
+}
+
+Matrix4x4f& Matrix4x4f::Translate (const Vector3f& inTrans)
+{
+ Get (0, 3) = Get (0, 0) * inTrans[0] + Get (0, 1) * inTrans[1] + Get (0, 2) * inTrans[2] + Get (0, 3);
+ Get (1, 3) = Get (1, 0) * inTrans[0] + Get (1, 1) * inTrans[1] + Get (1, 2) * inTrans[2] + Get (1, 3);
+ Get (2, 3) = Get (2, 0) * inTrans[0] + Get (2, 1) * inTrans[1] + Get (2, 2) * inTrans[2] + Get (2, 3);
+ Get (3, 3) = Get (3, 0) * inTrans[0] + Get (3, 1) * inTrans[1] + Get (3, 2) * inTrans[2] + Get (3, 3);
+ return *this;
+}
+
+Matrix4x4f& Matrix4x4f::SetTranslate (const Vector3f& inTrans)
+{
+ Get (0, 0) = 1.0; Get (0, 1) = 0.0; Get (0, 2) = 0.0; Get (0, 3) = inTrans[0];
+ Get (1, 0) = 0.0; Get (1, 1) = 1.0; Get (1, 2) = 0.0; Get (1, 3) = inTrans[1];
+ Get (2, 0) = 0.0; Get (2, 1) = 0.0; Get (2, 2) = 1.0; Get (2, 3) = inTrans[2];
+ Get (3, 0) = 0.0; Get (3, 1) = 0.0; Get (3, 2) = 0.0; Get (3, 3) = 1.0;
+ return *this;
+}
+
+Matrix4x4f& Matrix4x4f::SetPerspective(
+ float fovy,
+ float aspect,
+ float zNear,
+ float zFar )
+{
+ float cotangent, deltaZ;
+ float radians = Deg2Rad (fovy / 2.0f);
+ cotangent = cos (radians) / sin (radians);
+ deltaZ = zNear - zFar;
+
+ Get (0,0) = cotangent / aspect; Get (0,1) = 0.0F; Get (0,2) = 0.0F; Get (0,3) = 0.0F;
+ Get (1,0) = 0.0F; Get (1,1) = cotangent; Get (1,2) = 0.0F; Get (1,3) = 0.0F;
+ Get (2,0) = 0.0F; Get (2,1) = 0.0F; Get (2,2) = (zFar + zNear) / deltaZ; Get (2,3) = 2.0F * zNear * zFar / deltaZ;
+ Get (3,0) = 0.0F; Get (3,1) = 0.0F; Get (3,2) = -1.0F; Get (3,3) = 0.0F;
+
+ return *this;
+}
+
+Matrix4x4f& Matrix4x4f::SetPerspectiveCotan(
+ float cotangent,
+ float zNear,
+ float zFar )
+{
+ float deltaZ = zNear - zFar;
+
+ Get (0,0) = cotangent; Get (0,1) = 0.0F; Get (0,2) = 0.0F; Get (0,3) = 0.0F;
+ Get (1,0) = 0.0F; Get (1,1) = cotangent; Get (1,2) = 0.0F; Get (1,3) = 0.0F;
+ Get (2,0) = 0.0F; Get (2,1) = 0.0F; Get (2,2) = (zFar + zNear) / deltaZ; Get (2,3) = 2.0F * zNear * zFar / deltaZ;
+ Get (3,0) = 0.0F; Get (3,1) = 0.0F; Get (3,2) = -1.0F; Get (3,3) = 0.0F;
+
+ return *this;
+}
+
+Matrix4x4f& Matrix4x4f::SetOrtho (
+ float left,
+ float right,
+ float bottom,
+ float top,
+ float zNear,
+ float zFar )
+{
+ SetIdentity ();
+
+ float deltax = right - left;
+ float deltay = top - bottom;
+ float deltaz = zFar - zNear;
+
+ Get(0,0) = 2.0F / deltax;
+ Get(0,3) = -(right + left) / deltax;
+ Get(1,1) = 2.0F / deltay;
+ Get(1,3) = -(top + bottom) / deltay;
+ Get(2,2) = -2.0F / deltaz;
+ Get(2,3) = -(zFar + zNear) / deltaz;
+ return *this;
+}
+
+Matrix4x4f& Matrix4x4f::SetFrustum (
+ float left,
+ float right,
+ float bottom,
+ float top,
+ float nearval,
+ float farval )
+{
+ float x, y, a, b, c, d, e;
+
+ x = (2.0F * nearval) / (right - left);
+ y = (2.0F * nearval) / (top - bottom);
+ a = (right + left) / (right - left);
+ b = (top + bottom) / (top - bottom);
+ c = -(farval + nearval) / (farval - nearval);
+ d = -(2.0f * farval * nearval) / (farval - nearval);
+ e = -1.0f;
+
+ Get (0,0) = x; Get (0,1) = 0.0; Get (0,2) = a; Get (0,3) = 0.0;
+ Get (1,0) = 0.0; Get (1,1) = y; Get (1,2) = b; Get (1,3) = 0.0;
+ Get (2,0) = 0.0; Get (2,1) = 0.0; Get (2,2) = c; Get (2,3) = d;
+ Get (3,0) = 0.0; Get (3,1) = 0.0; Get (3,2) = e; Get (3,3) = 0.0;
+ return *this;
+}
+
+
+
+TransformType ComputeTransformType (const Matrix4x4f& matrix, float& outUniformScale, float epsilon)
+{
+ float lengthX = Magnitude(matrix.GetAxisX());
+ float lengthY = Magnitude(matrix.GetAxisY());
+ float lengthZ = Magnitude(matrix.GetAxisZ());
+ float minAxis = std::min(std::min(lengthX, lengthY), lengthZ);
+ float maxAxis = std::max(std::max(lengthX, lengthY), lengthZ);
+ TransformType transType = kNoScaleTransform;
+ outUniformScale = 1.0f;
+ if (minAxis < 1.0 - epsilon || maxAxis > 1.0 + epsilon)
+ {
+ if (minAxis != 0.0f && maxAxis / minAxis < 1.0 + epsilon)
+ {
+ transType = kUniformScaleTransform;
+ outUniformScale = minAxis;
+ }
+ else
+ transType = kNonUniformScaleTransform;
+ }
+ return transType;
+}
+
+
+#define MAT(m,r,c) (m)[(c)*4+(r)]
+
+#define RETURN_ZERO \
+{ \
+ for (int i=0;i<16;i++) \
+ out[i] = 0.0F; \
+ return false; \
+}
+
+// 4x4 matrix inversion by Gaussian reduction with partial pivoting followed by back/substitution;
+// with loops manually unrolled.
+
+#define SWAP_ROWS(a, b) { float *_tmp = a; (a)=(b); (b)=_tmp; }
+bool InvertMatrix4x4_Full(const float* m, float* out)
+{
+ float wtmp[4][8];
+ float m0, m1, m2, m3, s;
+ float *r0, *r1, *r2, *r3;
+
+ r0 = wtmp[0], r1 = wtmp[1], r2 = wtmp[2], r3 = wtmp[3];
+
+ r0[0] = MAT(m,0,0), r0[1] = MAT(m,0,1),
+ r0[2] = MAT(m,0,2), r0[3] = MAT(m,0,3),
+ r0[4] = 1.0, r0[5] = r0[6] = r0[7] = 0.0,
+
+ r1[0] = MAT(m,1,0), r1[1] = MAT(m,1,1),
+ r1[2] = MAT(m,1,2), r1[3] = MAT(m,1,3),
+ r1[5] = 1.0, r1[4] = r1[6] = r1[7] = 0.0,
+
+ r2[0] = MAT(m,2,0), r2[1] = MAT(m,2,1),
+ r2[2] = MAT(m,2,2), r2[3] = MAT(m,2,3),
+ r2[6] = 1.0, r2[4] = r2[5] = r2[7] = 0.0,
+
+ r3[0] = MAT(m,3,0), r3[1] = MAT(m,3,1),
+ r3[2] = MAT(m,3,2), r3[3] = MAT(m,3,3),
+ r3[7] = 1.0, r3[4] = r3[5] = r3[6] = 0.0;
+
+ /* choose pivot - or die */
+ if (Abs(r3[0])>Abs(r2[0])) SWAP_ROWS(r3, r2);
+ if (Abs(r2[0])>Abs(r1[0])) SWAP_ROWS(r2, r1);
+ if (Abs(r1[0])>Abs(r0[0])) SWAP_ROWS(r1, r0);
+ if (0.0F == r0[0]) RETURN_ZERO
+
+ /* eliminate first variable */
+ m1 = r1[0]/r0[0]; m2 = r2[0]/r0[0]; m3 = r3[0]/r0[0];
+ s = r0[1]; r1[1] -= m1 * s; r2[1] -= m2 * s; r3[1] -= m3 * s;
+ s = r0[2]; r1[2] -= m1 * s; r2[2] -= m2 * s; r3[2] -= m3 * s;
+ s = r0[3]; r1[3] -= m1 * s; r2[3] -= m2 * s; r3[3] -= m3 * s;
+ s = r0[4];
+ if (s != 0.0F) { r1[4] -= m1 * s; r2[4] -= m2 * s; r3[4] -= m3 * s; }
+ s = r0[5];
+ if (s != 0.0F) { r1[5] -= m1 * s; r2[5] -= m2 * s; r3[5] -= m3 * s; }
+ s = r0[6];
+ if (s != 0.0F) { r1[6] -= m1 * s; r2[6] -= m2 * s; r3[6] -= m3 * s; }
+ s = r0[7];
+ if (s != 0.0F) { r1[7] -= m1 * s; r2[7] -= m2 * s; r3[7] -= m3 * s; }
+
+ /* choose pivot - or die */
+ if (Abs(r3[1])>Abs(r2[1])) SWAP_ROWS(r3, r2);
+ if (Abs(r2[1])>Abs(r1[1])) SWAP_ROWS(r2, r1);
+ if (0.0F == r1[1]) RETURN_ZERO;
+
+ /* eliminate second variable */
+ m2 = r2[1]/r1[1]; m3 = r3[1]/r1[1];
+ r2[2] -= m2 * r1[2]; r3[2] -= m3 * r1[2];
+ r2[3] -= m2 * r1[3]; r3[3] -= m3 * r1[3];
+ s = r1[4]; if (0.0F != s) { r2[4] -= m2 * s; r3[4] -= m3 * s; }
+ s = r1[5]; if (0.0F != s) { r2[5] -= m2 * s; r3[5] -= m3 * s; }
+ s = r1[6]; if (0.0F != s) { r2[6] -= m2 * s; r3[6] -= m3 * s; }
+ s = r1[7]; if (0.0F != s) { r2[7] -= m2 * s; r3[7] -= m3 * s; }
+
+ /* choose pivot - or die */
+ if (Abs(r3[2])>Abs(r2[2])) SWAP_ROWS(r3, r2);
+ if (0.0F == r2[2]) RETURN_ZERO;
+
+ /* eliminate third variable */
+ m3 = r3[2]/r2[2];
+ r3[3] -= m3 * r2[3], r3[4] -= m3 * r2[4],
+ r3[5] -= m3 * r2[5], r3[6] -= m3 * r2[6],
+ r3[7] -= m3 * r2[7];
+
+ /* last check */
+ if (0.0F == r3[3]) RETURN_ZERO;
+
+ s = 1.0F/r3[3]; /* now back substitute row 3 */
+ r3[4] *= s; r3[5] *= s; r3[6] *= s; r3[7] *= s;
+
+ m2 = r2[3]; /* now back substitute row 2 */
+ s = 1.0F/r2[2];
+ r2[4] = s * (r2[4] - r3[4] * m2), r2[5] = s * (r2[5] - r3[5] * m2),
+ r2[6] = s * (r2[6] - r3[6] * m2), r2[7] = s * (r2[7] - r3[7] * m2);
+ m1 = r1[3];
+ r1[4] -= r3[4] * m1, r1[5] -= r3[5] * m1,
+ r1[6] -= r3[6] * m1, r1[7] -= r3[7] * m1;
+ m0 = r0[3];
+ r0[4] -= r3[4] * m0, r0[5] -= r3[5] * m0,
+ r0[6] -= r3[6] * m0, r0[7] -= r3[7] * m0;
+
+ m1 = r1[2]; /* now back substitute row 1 */
+ s = 1.0F/r1[1];
+ r1[4] = s * (r1[4] - r2[4] * m1), r1[5] = s * (r1[5] - r2[5] * m1),
+ r1[6] = s * (r1[6] - r2[6] * m1), r1[7] = s * (r1[7] - r2[7] * m1);
+ m0 = r0[2];
+ r0[4] -= r2[4] * m0, r0[5] -= r2[5] * m0,
+ r0[6] -= r2[6] * m0, r0[7] -= r2[7] * m0;
+
+ m0 = r0[1]; /* now back substitute row 0 */
+ s = 1.0F/r0[0];
+ r0[4] = s * (r0[4] - r1[4] * m0), r0[5] = s * (r0[5] - r1[5] * m0),
+ r0[6] = s * (r0[6] - r1[6] * m0), r0[7] = s * (r0[7] - r1[7] * m0);
+
+ MAT(out,0,0) = r0[4]; MAT(out,0,1) = r0[5], MAT(out,0,2) = r0[6]; MAT(out,0,3) = r0[7],
+ MAT(out,1,0) = r1[4]; MAT(out,1,1) = r1[5], MAT(out,1,2) = r1[6]; MAT(out,1,3) = r1[7],
+ MAT(out,2,0) = r2[4]; MAT(out,2,1) = r2[5], MAT(out,2,2) = r2[6]; MAT(out,2,3) = r2[7],
+ MAT(out,3,0) = r3[4]; MAT(out,3,1) = r3[5], MAT(out,3,2) = r3[6]; MAT(out,3,3) = r3[7];
+
+ return true;
+}
+
+#undef SWAP_ROWS
+
+// Invert 3D transformation matrix (not perspective). Adapted from graphics gems 2.
+// Inverts upper left by calculating its determinant and multiplying it to the symmetric
+// adjust matrix of each element. Finally deals with the translation by transforming the
+// original translation using by the calculated inverse.
+bool InvertMatrix4x4_General3D( const float* in, float* out )
+{
+ float pos, neg, t;
+ float det;
+
+ // Calculate the determinant of upper left 3x3 sub-matrix and
+ // determine if the matrix is singular.
+ pos = neg = 0.0;
+ t = MAT(in,0,0) * MAT(in,1,1) * MAT(in,2,2);
+ if (t >= 0.0) pos += t; else neg += t;
+
+ t = MAT(in,1,0) * MAT(in,2,1) * MAT(in,0,2);
+ if (t >= 0.0) pos += t; else neg += t;
+
+ t = MAT(in,2,0) * MAT(in,0,1) * MAT(in,1,2);
+ if (t >= 0.0) pos += t; else neg += t;
+
+ t = -MAT(in,2,0) * MAT(in,1,1) * MAT(in,0,2);
+ if (t >= 0.0) pos += t; else neg += t;
+
+ t = -MAT(in,1,0) * MAT(in,0,1) * MAT(in,2,2);
+ if (t >= 0.0) pos += t; else neg += t;
+
+ t = -MAT(in,0,0) * MAT(in,2,1) * MAT(in,1,2);
+ if (t >= 0.0) pos += t; else neg += t;
+
+ det = pos + neg;
+
+ if (det*det < 1e-25)
+ RETURN_ZERO;
+
+ det = 1.0F / det;
+ MAT(out,0,0) = ( (MAT(in,1,1)*MAT(in,2,2) - MAT(in,2,1)*MAT(in,1,2) )*det);
+ MAT(out,0,1) = (- (MAT(in,0,1)*MAT(in,2,2) - MAT(in,2,1)*MAT(in,0,2) )*det);
+ MAT(out,0,2) = ( (MAT(in,0,1)*MAT(in,1,2) - MAT(in,1,1)*MAT(in,0,2) )*det);
+ MAT(out,1,0) = (- (MAT(in,1,0)*MAT(in,2,2) - MAT(in,2,0)*MAT(in,1,2) )*det);
+ MAT(out,1,1) = ( (MAT(in,0,0)*MAT(in,2,2) - MAT(in,2,0)*MAT(in,0,2) )*det);
+ MAT(out,1,2) = (- (MAT(in,0,0)*MAT(in,1,2) - MAT(in,1,0)*MAT(in,0,2) )*det);
+ MAT(out,2,0) = ( (MAT(in,1,0)*MAT(in,2,1) - MAT(in,2,0)*MAT(in,1,1) )*det);
+ MAT(out,2,1) = (- (MAT(in,0,0)*MAT(in,2,1) - MAT(in,2,0)*MAT(in,0,1) )*det);
+ MAT(out,2,2) = ( (MAT(in,0,0)*MAT(in,1,1) - MAT(in,1,0)*MAT(in,0,1) )*det);
+
+ // Do the translation part
+ MAT(out,0,3) = - (MAT(in,0,3) * MAT(out,0,0) +
+ MAT(in,1,3) * MAT(out,0,1) +
+ MAT(in,2,3) * MAT(out,0,2) );
+ MAT(out,1,3) = - (MAT(in,0,3) * MAT(out,1,0) +
+ MAT(in,1,3) * MAT(out,1,1) +
+ MAT(in,2,3) * MAT(out,1,2) );
+ MAT(out,2,3) = - (MAT(in,0,3) * MAT(out,2,0) +
+ MAT(in,1,3) * MAT(out,2,1) +
+ MAT(in,2,3) * MAT(out,2,2) );
+
+ MAT(out,3,0) = 0.0f;
+ MAT(out,3,1) = 0.0f;
+ MAT(out,3,2) = 0.0f;
+ MAT(out,3,3) = 1.0f;
+
+ return true;
+}
+
+#undef MAT
+#undef RETURN_ZERO
+
+
+/*
+4x4 matrix inverse based on Cramer's rule. From Intel's "Streaming SIMD Extensions - Inverse of 4x4 Matrix" paper.
+Seems to be about the same speed as our current one, maybe slightly faster. Less numerically robust though,
+at very small numbers.
+
+bool InvertMatrix4x4_Cramer (const float* mat, float* dst)
+{
+ float tmp[12]; // temp array for pairs
+ float src[16]; // array of transpose source matrix
+ float det; // determinant
+ // transpose matrix
+ for (int i = 0; i < 4; i++) {
+ src[i] = mat[i*4];
+ src[i + 4] = mat[i*4 + 1];
+ src[i + 8] = mat[i*4 + 2];
+ src[i + 12] = mat[i*4 + 3];
+ }
+ // calculate pairs for first 8 elements (cofactors)
+ tmp[0] = src[10] * src[15];
+ tmp[1] = src[11] * src[14];
+ tmp[2] = src[9] * src[15];
+ tmp[3] = src[11] * src[13];
+ tmp[4] = src[9] * src[14];
+ tmp[5] = src[10] * src[13];
+ tmp[6] = src[8] * src[15];
+ tmp[7] = src[11] * src[12];
+ tmp[8] = src[8] * src[14];
+ tmp[9] = src[10] * src[12];
+ tmp[10] = src[8] * src[13];
+ tmp[11] = src[9] * src[12];
+ // calculate first 8 elements (cofactors)
+ dst[0] = tmp[0]*src[5] + tmp[3]*src[6] + tmp[4]*src[7];
+ dst[0] -= tmp[1]*src[5] + tmp[2]*src[6] + tmp[5]*src[7];
+ dst[1] = tmp[1]*src[4] + tmp[6]*src[6] + tmp[9]*src[7];
+ dst[1] -= tmp[0]*src[4] + tmp[7]*src[6] + tmp[8]*src[7];
+ dst[2] = tmp[2]*src[4] + tmp[7]*src[5] + tmp[10]*src[7];
+ dst[2] -= tmp[3]*src[4] + tmp[6]*src[5] + tmp[11]*src[7];
+ dst[3] = tmp[5]*src[4] + tmp[8]*src[5] + tmp[11]*src[6];
+ dst[3] -= tmp[4]*src[4] + tmp[9]*src[5] + tmp[10]*src[6];
+ dst[4] = tmp[1]*src[1] + tmp[2]*src[2] + tmp[5]*src[3];
+ dst[4] -= tmp[0]*src[1] + tmp[3]*src[2] + tmp[4]*src[3];
+ dst[5] = tmp[0]*src[0] + tmp[7]*src[2] + tmp[8]*src[3];
+ dst[5] -= tmp[1]*src[0] + tmp[6]*src[2] + tmp[9]*src[3];
+ dst[6] = tmp[3]*src[0] + tmp[6]*src[1] + tmp[11]*src[3];
+ dst[6] -= tmp[2]*src[0] + tmp[7]*src[1] + tmp[10]*src[3];
+ dst[7] = tmp[4]*src[0] + tmp[9]*src[1] + tmp[10]*src[2];
+ dst[7] -= tmp[5]*src[0] + tmp[8]*src[1] + tmp[11]*src[2];
+ // calculate pairs for second 8 elements (cofactors)
+ tmp[0] = src[2]*src[7];
+ tmp[1] = src[3]*src[6];
+ tmp[2] = src[1]*src[7];
+ tmp[3] = src[3]*src[5];
+ tmp[4] = src[1]*src[6];
+ tmp[5] = src[2]*src[5];
+ tmp[6] = src[0]*src[7];
+ tmp[7] = src[3]*src[4];
+ tmp[8] = src[0]*src[6];
+ tmp[9] = src[2]*src[4];
+ tmp[10] = src[0]*src[5];
+ tmp[11] = src[1]*src[4];
+ // calculate second 8 elements (cofactors)
+ dst[8] = tmp[0]*src[13] + tmp[3]*src[14] + tmp[4]*src[15];
+ dst[8] -= tmp[1]*src[13] + tmp[2]*src[14] + tmp[5]*src[15];
+ dst[9] = tmp[1]*src[12] + tmp[6]*src[14] + tmp[9]*src[15];
+ dst[9] -= tmp[0]*src[12] + tmp[7]*src[14] + tmp[8]*src[15];
+ dst[10] = tmp[2]*src[12] + tmp[7]*src[13] + tmp[10]*src[15];
+ dst[10]-= tmp[3]*src[12] + tmp[6]*src[13] + tmp[11]*src[15];
+ dst[11] = tmp[5]*src[12] + tmp[8]*src[13] + tmp[11]*src[14];
+ dst[11]-= tmp[4]*src[12] + tmp[9]*src[13] + tmp[10]*src[14];
+ dst[12] = tmp[2]*src[10] + tmp[5]*src[11] + tmp[1]*src[9];
+ dst[12]-= tmp[4]*src[11] + tmp[0]*src[9] + tmp[3]*src[10];
+ dst[13] = tmp[8]*src[11] + tmp[0]*src[8] + tmp[7]*src[10];
+ dst[13]-= tmp[6]*src[10] + tmp[9]*src[11] + tmp[1]*src[8];
+ dst[14] = tmp[6]*src[9] + tmp[11]*src[11] + tmp[3]*src[8];
+ dst[14]-= tmp[10]*src[11] + tmp[2]*src[8] + tmp[7]*src[9];
+ dst[15] = tmp[10]*src[10] + tmp[4]*src[8] + tmp[9]*src[9];
+ dst[15]-= tmp[8]*src[9] + tmp[11]*src[10] + tmp[5]*src[8];
+ // calculate determinant
+ det=src[0]*dst[0]+src[1]*dst[1]+src[2]*dst[2]+src[3]*dst[3];
+ // calculate matrix inverse
+ if( CompareApproximately(det,0.0f) )
+ {
+ for (int i=0;i<16;i++)
+ dst[i] = 0.0F;
+ return false;
+ }
+
+ det = 1.0f/det;
+ for (int j = 0; j < 16; j++)
+ dst[j] *= det;
+
+ return true;
+}
+*/
+
+
+/*
+// SSE based matrix inverse from Intel's "Streaming SIMD Extensions - Inverse of 4x4 Matrix" paper.
+// Does not seem to be much faster on Core 2 Duo. Keeping it here just in case.
+
+#include <emmintrin.h>
+
+bool InvertMatrix4x4( const float* src, float* dst )
+{
+ __m128 minor0, minor1, minor2, minor3;
+ __m128 row0, row1, row2, row3;
+ __m128 det, tmp1;
+ tmp1 = _mm_loadh_pi(_mm_loadl_pi(tmp1, (__m64*)(src)), (__m64*)(src+ 4));
+ row1 = _mm_loadh_pi(_mm_loadl_pi(row1, (__m64*)(src+8)), (__m64*)(src+12));
+ row0 = _mm_shuffle_ps(tmp1, row1, 0x88);
+ row1 = _mm_shuffle_ps(row1, tmp1, 0xDD);
+ tmp1 = _mm_loadh_pi(_mm_loadl_pi(tmp1, (__m64*)(src+ 2)), (__m64*)(src+ 6));
+ row3 = _mm_loadh_pi(_mm_loadl_pi(row3, (__m64*)(src+10)), (__m64*)(src+14));
+ row2 = _mm_shuffle_ps(tmp1, row3, 0x88);
+ row3 = _mm_shuffle_ps(row3, tmp1, 0xDD);
+ // -----------------------------------------------
+ tmp1 = _mm_mul_ps(row2, row3);
+ tmp1 = _mm_shuffle_ps(tmp1, tmp1, 0xB1);
+ minor0 = _mm_mul_ps(row1, tmp1);
+ minor1 = _mm_mul_ps(row0, tmp1);
+ tmp1 = _mm_shuffle_ps(tmp1, tmp1, 0x4E);
+ minor0 = _mm_sub_ps(_mm_mul_ps(row1, tmp1), minor0);
+ minor1 = _mm_sub_ps(_mm_mul_ps(row0, tmp1), minor1);
+ minor1 = _mm_shuffle_ps(minor1, minor1, 0x4E);
+ // -----------------------------------------------
+ tmp1 = _mm_mul_ps(row1, row2);
+ tmp1 = _mm_shuffle_ps(tmp1, tmp1, 0xB1);
+ minor0 = _mm_add_ps(_mm_mul_ps(row3, tmp1), minor0);
+ minor3 = _mm_mul_ps(row0, tmp1);
+ tmp1 = _mm_shuffle_ps(tmp1, tmp1, 0x4E);
+ minor0 = _mm_sub_ps(minor0, _mm_mul_ps(row3, tmp1));
+ minor3 = _mm_sub_ps(_mm_mul_ps(row0, tmp1), minor3);
+ minor3 = _mm_shuffle_ps(minor3, minor3, 0x4E);
+ // -----------------------------------------------
+ tmp1 = _mm_mul_ps(_mm_shuffle_ps(row1, row1, 0x4E), row3);
+ tmp1 = _mm_shuffle_ps(tmp1, tmp1, 0xB1);
+ row2 = _mm_shuffle_ps(row2, row2, 0x4E);
+ minor0 = _mm_add_ps(_mm_mul_ps(row2, tmp1), minor0);
+ minor2 = _mm_mul_ps(row0, tmp1);
+ tmp1 = _mm_shuffle_ps(tmp1, tmp1, 0x4E);
+ minor0 = _mm_sub_ps(minor0, _mm_mul_ps(row2, tmp1));
+ minor2 = _mm_sub_ps(_mm_mul_ps(row0, tmp1), minor2);
+ minor2 = _mm_shuffle_ps(minor2, minor2, 0x4E);
+ // -----------------------------------------------
+ tmp1 = _mm_mul_ps(row0, row1);
+ tmp1 = _mm_shuffle_ps(tmp1, tmp1, 0xB1);
+ minor2 = _mm_add_ps(_mm_mul_ps(row3, tmp1), minor2);
+ minor3 = _mm_sub_ps(_mm_mul_ps(row2, tmp1), minor3);
+ tmp1 = _mm_shuffle_ps(tmp1, tmp1, 0x4E);
+ minor2 = _mm_sub_ps(_mm_mul_ps(row3, tmp1), minor2);
+ minor3 = _mm_sub_ps(minor3, _mm_mul_ps(row2, tmp1));
+ // -----------------------------------------------
+ tmp1 = _mm_mul_ps(row0, row3);
+ tmp1 = _mm_shuffle_ps(tmp1, tmp1, 0xB1);
+ minor1 = _mm_sub_ps(minor1, _mm_mul_ps(row2, tmp1));
+ minor2 = _mm_add_ps(_mm_mul_ps(row1, tmp1), minor2);
+ tmp1 = _mm_shuffle_ps(tmp1, tmp1, 0x4E);
+ minor1 = _mm_add_ps(_mm_mul_ps(row2, tmp1), minor1);
+ minor2 = _mm_sub_ps(minor2, _mm_mul_ps(row1, tmp1));
+ // -----------------------------------------------
+ tmp1 = _mm_mul_ps(row0, row2);
+ tmp1 = _mm_shuffle_ps(tmp1, tmp1, 0xB1);
+ minor1 = _mm_add_ps(_mm_mul_ps(row3, tmp1), minor1);
+ minor3 = _mm_sub_ps(minor3, _mm_mul_ps(row1, tmp1));
+ tmp1 = _mm_shuffle_ps(tmp1, tmp1, 0x4E);
+ minor1 = _mm_sub_ps(minor1, _mm_mul_ps(row3, tmp1));
+ minor3 = _mm_add_ps(_mm_mul_ps(row1, tmp1), minor3);
+ // -----------------------------------------------
+ det = _mm_mul_ps(row0, minor0);
+ det = _mm_add_ps(_mm_shuffle_ps(det, det, 0x4E), det);
+ det = _mm_add_ss(_mm_shuffle_ps(det, det, 0xB1), det);
+ // TODO: detect zero determinant
+ tmp1 = _mm_rcp_ss(det);
+ det = _mm_sub_ss(_mm_add_ss(tmp1, tmp1), _mm_mul_ss(det, _mm_mul_ss(tmp1, tmp1)));
+ det = _mm_shuffle_ps(det, det, 0x00);
+ minor0 = _mm_mul_ps(det, minor0);
+ _mm_storel_pi((__m64*)(dst), minor0);
+ _mm_storeh_pi((__m64*)(dst+2), minor0);
+ minor1 = _mm_mul_ps(det, minor1);
+ _mm_storel_pi((__m64*)(dst+4), minor1);
+ _mm_storeh_pi((__m64*)(dst+6), minor1);
+ minor2 = _mm_mul_ps(det, minor2);
+ _mm_storel_pi((__m64*)(dst+ 8), minor2);
+ _mm_storeh_pi((__m64*)(dst+10), minor2);
+ minor3 = _mm_mul_ps(det, minor3);
+ _mm_storel_pi((__m64*)(dst+12), minor3);
+ _mm_storeh_pi((__m64*)(dst+14), minor3);
+
+ return true;
+}
+*/
+
+
+Matrix4x4f& Matrix4x4f::Transpose ()
+{
+ swap(Get (0,1),Get (1,0));
+ swap(Get (0,2),Get (2,0));
+ swap(Get (0,3),Get (3,0));
+ swap(Get (1,2),Get (2,1));
+ swap(Get (1,3),Get (3,1));
+ swap(Get (2,3),Get (3,2));
+ return *this;
+}
+
+Matrix4x4f& Matrix4x4f::Copy (const Matrix4x4f& inM)
+{
+ CopyMatrix(inM.m_Data, m_Data);
+ return *this;
+}
+
+void fromToRotation(const float from[3], const float to[3],float mtx[3][3]);
+
+Matrix4x4f& Matrix4x4f::SetFromToRotation (const Vector3f& from, const Vector3f& to)
+{
+ float mtx[3][3];
+ fromToRotation (from.GetPtr (), to.GetPtr (), mtx);
+ Get (0, 0) = mtx[0][0]; Get (0, 1) = mtx[0][1]; Get (0, 2) = mtx[0][2]; Get (0, 3) = 0.0;
+ Get (1, 0) = mtx[1][0]; Get (1, 1) = mtx[1][1]; Get (1, 2) = mtx[1][2]; Get (1, 3) = 0.0;
+ Get (2, 0) = mtx[2][0]; Get (2, 1) = mtx[2][1]; Get (2, 2) = mtx[2][2]; Get (2, 3) = 0.0;
+ Get (3, 0) = 0.0; Get (3, 1) = 0.0; Get (3, 2) = 0.0; Get (3, 3) = 1.0;
+ return *this;
+}
+
+bool CompareApproximately (const Matrix4x4f& lhs, const Matrix4x4f& rhs, float dist)
+{
+ for (int i=0;i<16;i++)
+ {
+ if (!CompareApproximately (lhs[i], rhs[i], dist))
+ return false;
+ }
+ return true;
+}
+
+void Matrix4x4f::SetTR (const Vector3f& pos, const Quaternionf& q)
+{
+ QuaternionToMatrix (q, *this);
+ m_Data[12] = pos[0];
+ m_Data[13] = pos[1];
+ m_Data[14] = pos[2];
+}
+
+void Matrix4x4f::SetTRS (const Vector3f& pos, const Quaternionf& q, const Vector3f& s)
+{
+ QuaternionToMatrix (q, *this);
+
+ m_Data[0] *= s[0];
+ m_Data[1] *= s[0];
+ m_Data[2] *= s[0];
+
+ m_Data[4] *= s[1];
+ m_Data[5] *= s[1];
+ m_Data[6] *= s[1];
+
+ m_Data[8] *= s[2];
+ m_Data[9] *= s[2];
+ m_Data[10] *= s[2];
+
+ m_Data[12] = pos[0];
+ m_Data[13] = pos[1];
+ m_Data[14] = pos[2];
+}
+
+void Matrix4x4f::SetTRInverse (const Vector3f& pos, const Quaternionf& q)
+{
+ QuaternionToMatrix (::Inverse (q), *this);
+ Translate (Vector3f (-pos[0], -pos[1], -pos[2]));
+}
+
+void TransformPoints3x3 (const Matrix4x4f& matrix, const Vector3f* in, Vector3f* out, int count)
+{
+ Matrix3x3f m = Matrix3x3f(matrix);
+ for (int i=0;i<count;i++)
+ out[i] = m.MultiplyPoint3 (in[i]);
+}
+
+void TransformPoints3x4 (const Matrix4x4f& matrix, const Vector3f* in, Vector3f* out, int count)
+{
+ for (int i=0;i<count;i++)
+ out[i] = matrix.MultiplyPoint3 (in[i]);
+}
+
+void TransformPoints3x3 (const Matrix4x4f& matrix, const Vector3f* in, size_t inStride, Vector3f* out, size_t outStride, int count)
+{
+ Matrix3x3f m = Matrix3x3f(matrix);
+ for (int i=0;i<count; ++i, in = Stride (in, inStride), out = Stride (out, outStride))
+ {
+ *out = m.MultiplyPoint3 (*in);
+ }
+}
+
+void TransformPoints3x4 (const Matrix4x4f& matrix, const Vector3f* in, size_t inStride, Vector3f* out, size_t outStride, int count)
+{
+ for (int i=0;i<count; ++i, in = Stride (in, inStride), out = Stride (out, outStride))
+ {
+ *out = matrix.MultiplyPoint3 (*in);
+ }
+}
+
+
+#include "Matrix4x4_REF.cpp"
generated by cgit v1.1-26-g67d0 at 2025-05-18 19:22:06 +0000