forked from AtomicGameEngine/AtomicGameEngine
-
Notifications
You must be signed in to change notification settings - Fork 0
Expand file tree
/
Copy pathMatrix3x4.cs
More file actions
95 lines (83 loc) · 3.4 KB
/
Matrix3x4.cs
File metadata and controls
95 lines (83 loc) · 3.4 KB
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
using System.Runtime.InteropServices;
namespace AtomicEngine
{
[StructLayout(LayoutKind.Sequential)]
public struct Matrix3x4
{
public float m00;
public float m01;
public float m02;
public float m03;
public float m10;
public float m11;
public float m12;
public float m13;
public float m20;
public float m21;
public float m22;
public float m23;
static public readonly Matrix3x4 IDENTITY = CreateIdentity();
static Matrix3x4 CreateIdentity()
{
var m = new Matrix3x4();
m.m00 = 1.0f;
m.m01 = 0.0f;
m.m02 = 0.0f;
m.m03 = 0.0f;
m.m10 = 0.0f;
m.m11 = 1.0f;
m.m12 = 0.0f;
m.m13 = 0.0f;
m.m20 = 0.0f;
m.m21 = 0.0f;
m.m22 = 1.0f;
m.m23 = 0.0f;
return m;
}
public Matrix3x4 Inverse()
{
float det = m00 * m11 * m22 +
m10 * m21 * m02 +
m20 * m01 * m12 -
m20 * m11 * m02 -
m10 * m01 * m22 -
m00 * m21 * m12;
float invDet = 1.0f / det;
Matrix3x4 ret;
ret.m00 = (m11 * m22 - m21 * m12) * invDet;
ret.m01 = -(m01 * m22 - m21 * m02) * invDet;
ret.m02 = (m01 * m12 - m11 * m02) * invDet;
ret.m03 = -(m03 * ret.m00 + m13 * ret.m01 + m23 * ret.m02);
ret.m10 = -(m10 * m22 - m20 * m12) * invDet;
ret.m11 = (m00 * m22 - m20 * m02) * invDet;
ret.m12 = -(m00 * m12 - m10 * m02) * invDet;
ret.m13 = -(m03 * ret.m10 + m13 * ret.m11 + m23 * ret.m12);
ret.m20 = (m10 * m21 - m20 * m11) * invDet;
ret.m21 = -(m00 * m21 - m20 * m01) * invDet;
ret.m22 = (m00 * m11 - m10 * m01) * invDet;
ret.m23 = -(m03 * ret.m20 + m13 * ret.m21 + m23 * ret.m22);
return ret;
}
public static Matrix4 operator *(Matrix4 left, Matrix3x4 rhs)
{
return new Matrix4(
left.M11 * rhs.m00 + left.M12 * rhs.m10 + left.M13 * rhs.m20,
left.M11 * rhs.m01 + left.M12 * rhs.m11 + left.M13 * rhs.m21,
left.M11 * rhs.m02 + left.M12 * rhs.m12 + left.M13 * rhs.m22,
left.M11 * rhs.m03 + left.M12 * rhs.m13 + left.M13 * rhs.m23 + left.M14,
left.M21 * rhs.m00 + left.M22 * rhs.m10 + left.M23 * rhs.m20,
left.M21 * rhs.m01 + left.M22 * rhs.m11 + left.M23 * rhs.m21,
left.M21 * rhs.m02 + left.M22 * rhs.m12 + left.M23 * rhs.m22,
left.M21 * rhs.m03 + left.M22 * rhs.m13 + left.M23 * rhs.m23 + left.M24,
left.M31 * rhs.m00 + left.M32 * rhs.m10 + left.M33 * rhs.m20,
left.M31 * rhs.m01 + left.M32 * rhs.m11 + left.M33 * rhs.m21,
left.M31 * rhs.m02 + left.M32 * rhs.m12 + left.M33 * rhs.m22,
left.M31 * rhs.m03 + left.M32 * rhs.m13 + left.M33 * rhs.m23 + left.M34,
left.M41 * rhs.m00 + left.M42 * rhs.m10 + left.M43 * rhs.m20,
left.M41 * rhs.m01 + left.M42 * rhs.m11 + left.M43 * rhs.m21,
left.M41 * rhs.m02 + left.M42 * rhs.m12 + left.M43 * rhs.m22,
left.M41 * rhs.m03 + left.M42 * rhs.m13 + left.M43 * rhs.m23 + left.M44
);
}
}
}