clray

view src/matrix.cc @ 25:58642a8316b7

find changesets by author, revision, files, or words in the commit message
continuing with the kdtree
author John Tsiombikas <nuclear@member.fsf.org>
date Sat, 14 Aug 2010 07:00:04 +0100
parents
children
line source
1 #include <string.h>
2 #include "matrix.h"
3
4 #define M(x, y) ((y) * 4 + (x))
5
6 Matrix4x4::Matrix4x4()
7 {
8 memset(m, 0, sizeof m);
9 m[0] = m[5] = m[10] = m[15] = 1.0;
10 }
11
12 Matrix4x4::Matrix4x4(const float *mat)
13 {
14 memcpy(m, mat, sizeof m);
15 }
16
17 Matrix4x4::Matrix4x4(float m00, float m01, float m02, float m03,
18 float m10, float m11, float m12, float m13,
19 float m20, float m21, float m22, float m23,
20 float m30, float m31, float m32, float m33)
21 {
22 m[M(0, 0)] = m00; m[M(0, 1)] = m01; m[M(0, 2)] = m02; m[M(0, 3)] = m03;
23 m[M(1, 0)] = m10; m[M(1, 1)] = m11; m[M(1, 2)] = m12; m[M(1, 3)] = m13;
24 m[M(2, 0)] = m20; m[M(2, 1)] = m21; m[M(2, 2)] = m22; m[M(2, 3)] = m23;
25 m[M(3, 0)] = m30; m[M(3, 1)] = m31; m[M(3, 2)] = m32; m[M(3, 3)] = m33;
26 }
27
28 Matrix4x4::Matrix4x4(const Matrix4x4 &mat)
29 {
30 memcpy(m, mat.m, sizeof m);
31 }
32
33 Matrix4x4 &Matrix4x4::operator =(const Matrix4x4 &mat)
34 {
35 memcpy(m, mat.m, sizeof m);
36 return *this;
37 }
38
39 void Matrix4x4::identity()
40 {
41 memset(m, 0, sizeof m);
42 m[0] = m[5] = m[10] = m[15] = 1.0;
43 }
44
45 float Matrix4x4::determinant() const
46 {
47 float det11 = (m[M(1, 1)] * (m[M(2, 2)] * m[M(3, 3)] - m[M(3, 2)] * m[M(2, 3)])) -
48 (m[M(1, 2)] * (m[M(2, 1)] * m[M(3, 3)] - m[M(3, 1)] * m[M(2, 3)])) +
49 (m[M(1, 3)] * (m[M(2, 1)] * m[M(3, 2)] - m[M(3, 1)] * m[M(2, 2)]));
50
51 float det12 = (m[M(1, 0)] * (m[M(2, 2)] * m[M(3, 3)] - m[M(3, 2)] * m[M(2, 3)])) -
52 (m[M(1, 2)] * (m[M(2, 0)] * m[M(3, 3)] - m[M(3, 0)] * m[M(2, 3)])) +
53 (m[M(1, 3)] * (m[M(2, 0)] * m[M(3, 2)] - m[M(3, 0)] * m[M(2, 2)]));
54
55 float det13 = (m[M(1, 0)] * (m[M(2, 1)] * m[M(3, 3)] - m[M(3, 1)] * m[M(2, 3)])) -
56 (m[M(1, 1)] * (m[M(2, 0)] * m[M(3, 3)] - m[M(3, 0)] * m[M(2, 3)])) +
57 (m[M(1, 3)] * (m[M(2, 0)] * m[M(3, 1)] - m[M(3, 0)] * m[M(2, 1)]));
58
59 float det14 = (m[M(1, 0)] * (m[M(2, 1)] * m[M(3, 2)] - m[M(3, 1)] * m[M(2, 2)])) -
60 (m[M(1, 1)] * (m[M(2, 0)] * m[M(3, 2)] - m[M(3, 0)] * m[M(2, 2)])) +
61 (m[M(1, 2)] * (m[M(2, 0)] * m[M(3, 1)] - m[M(3, 0)] * m[M(2, 1)]));
62
63 return m[M(0, 0)] * det11 - m[M(0, 1)] * det12 + m[M(0, 2)] * det13 - m[M(0, 3)] * det14;
64 }
65
66 Matrix4x4 Matrix4x4::adjoint() const
67 {
68 Matrix4x4 coef;
69
70 coef.m[M(0, 0)] = (m[M(1, 1)] * (m[M(2, 2)] * m[M(3, 3)] - m[M(3, 2)] * m[M(2, 3)])) -
71 (m[M(1, 2)] * (m[M(2, 1)] * m[M(3, 3)] - m[M(3, 1)] * m[M(2, 3)])) +
72 (m[M(1, 3)] * (m[M(2, 1)] * m[M(3, 2)] - m[M(3, 1)] * m[M(2, 2)]));
73 coef.m[M(0, 1)] = (m[M(1, 0)] * (m[M(2, 2)] * m[M(3, 3)] - m[M(3, 2)] * m[M(2, 3)])) -
74 (m[M(1, 2)] * (m[M(2, 0)] * m[M(3, 3)] - m[M(3, 0)] * m[M(2, 3)])) +
75 (m[M(1, 3)] * (m[M(2, 0)] * m[M(3, 2)] - m[M(3, 0)] * m[M(2, 2)]));
76 coef.m[M(0, 2)] = (m[M(1, 0)] * (m[M(2, 1)] * m[M(3, 3)] - m[M(3, 1)] * m[M(2, 3)])) -
77 (m[M(1, 1)] * (m[M(2, 0)] * m[M(3, 3)] - m[M(3, 0)] * m[M(2, 3)])) +
78 (m[M(1, 3)] * (m[M(2, 0)] * m[M(3, 1)] - m[M(3, 0)] * m[M(2, 1)]));
79 coef.m[M(0, 3)] = (m[M(1, 0)] * (m[M(2, 1)] * m[M(3, 2)] - m[M(3, 1)] * m[M(2, 2)])) -
80 (m[M(1, 1)] * (m[M(2, 0)] * m[M(3, 2)] - m[M(3, 0)] * m[M(2, 2)])) +
81 (m[M(1, 2)] * (m[M(2, 0)] * m[M(3, 1)] - m[M(3, 0)] * m[M(2, 1)]));
82
83 coef.m[M(1, 0)] = (m[M(0, 1)] * (m[M(2, 2)] * m[M(3, 3)] - m[M(3, 2)] * m[M(2, 3)])) -
84 (m[M(0, 2)] * (m[M(2, 1)] * m[M(3, 3)] - m[M(3, 1)] * m[M(2, 3)])) +
85 (m[M(0, 3)] * (m[M(2, 1)] * m[M(3, 2)] - m[M(3, 1)] * m[M(2, 2)]));
86 coef.m[M(1, 1)] = (m[M(0, 0)] * (m[M(2, 2)] * m[M(3, 3)] - m[M(3, 2)] * m[M(2, 3)])) -
87 (m[M(0, 2)] * (m[M(2, 0)] * m[M(3, 3)] - m[M(3, 0)] * m[M(2, 3)])) +
88 (m[M(0, 3)] * (m[M(2, 0)] * m[M(3, 2)] - m[M(3, 0)] * m[M(2, 2)]));
89 coef.m[M(1, 2)] = (m[M(0, 0)] * (m[M(2, 1)] * m[M(3, 3)] - m[M(3, 1)] * m[M(2, 3)])) -
90 (m[M(0, 1)] * (m[M(2, 0)] * m[M(3, 3)] - m[M(3, 0)] * m[M(2, 3)])) +
91 (m[M(0, 3)] * (m[M(2, 0)] * m[M(3, 1)] - m[M(3, 0)] * m[M(2, 1)]));
92 coef.m[M(1, 3)] = (m[M(0, 0)] * (m[M(2, 1)] * m[M(3, 2)] - m[M(3, 1)] * m[M(2, 2)])) -
93 (m[M(0, 1)] * (m[M(2, 0)] * m[M(3, 2)] - m[M(3, 0)] * m[M(2, 2)])) +
94 (m[M(0, 2)] * (m[M(2, 0)] * m[M(3, 1)] - m[M(3, 0)] * m[M(2, 1)]));
95
96 coef.m[M(2, 0)] = (m[M(0, 1)] * (m[M(1, 2)] * m[M(3, 3)] - m[M(3, 2)] * m[M(1, 3)])) -
97 (m[M(0, 2)] * (m[M(1, 1)] * m[M(3, 3)] - m[M(3, 1)] * m[M(1, 3)])) +
98 (m[M(0, 3)] * (m[M(1, 1)] * m[M(3, 2)] - m[M(3, 1)] * m[M(1, 2)]));
99 coef.m[M(2, 1)] = (m[M(0, 0)] * (m[M(1, 2)] * m[M(3, 3)] - m[M(3, 2)] * m[M(1, 3)])) -
100 (m[M(0, 2)] * (m[M(1, 0)] * m[M(3, 3)] - m[M(3, 0)] * m[M(1, 3)])) +
101 (m[M(0, 3)] * (m[M(1, 0)] * m[M(3, 2)] - m[M(3, 0)] * m[M(1, 2)]));
102 coef.m[M(2, 2)] = (m[M(0, 0)] * (m[M(1, 1)] * m[M(3, 3)] - m[M(3, 1)] * m[M(1, 3)])) -
103 (m[M(0, 1)] * (m[M(1, 0)] * m[M(3, 3)] - m[M(3, 0)] * m[M(1, 3)])) +
104 (m[M(0, 3)] * (m[M(1, 0)] * m[M(3, 1)] - m[M(3, 0)] * m[M(1, 1)]));
105 coef.m[M(2, 3)] = (m[M(0, 0)] * (m[M(1, 1)] * m[M(3, 2)] - m[M(3, 1)] * m[M(1, 2)])) -
106 (m[M(0, 1)] * (m[M(1, 0)] * m[M(3, 2)] - m[M(3, 0)] * m[M(1, 2)])) +
107 (m[M(0, 2)] * (m[M(1, 0)] * m[M(3, 1)] - m[M(3, 0)] * m[M(1, 1)]));
108
109 coef.m[M(3, 0)] = (m[M(0, 1)] * (m[M(1, 2)] * m[M(2, 3)] - m[M(2, 2)] * m[M(1, 3)])) -
110 (m[M(0, 2)] * (m[M(1, 1)] * m[M(2, 3)] - m[M(2, 1)] * m[M(1, 3)])) +
111 (m[M(0, 3)] * (m[M(1, 1)] * m[M(2, 2)] - m[M(2, 1)] * m[M(1, 2)]));
112 coef.m[M(3, 1)] = (m[M(0, 0)] * (m[M(1, 2)] * m[M(2, 3)] - m[M(2, 2)] * m[M(1, 3)])) -
113 (m[M(0, 2)] * (m[M(1, 0)] * m[M(2, 3)] - m[M(2, 0)] * m[M(1, 3)])) +
114 (m[M(0, 3)] * (m[M(1, 0)] * m[M(2, 2)] - m[M(2, 0)] * m[M(1, 2)]));
115 coef.m[M(3, 2)] = (m[M(0, 0)] * (m[M(1, 1)] * m[M(2, 3)] - m[M(2, 1)] * m[M(1, 3)])) -
116 (m[M(0, 1)] * (m[M(1, 0)] * m[M(2, 3)] - m[M(2, 0)] * m[M(1, 3)])) +
117 (m[M(0, 3)] * (m[M(1, 0)] * m[M(2, 1)] - m[M(2, 0)] * m[M(1, 1)]));
118 coef.m[M(3, 3)] = (m[M(0, 0)] * (m[M(1, 1)] * m[M(2, 2)] - m[M(2, 1)] * m[M(1, 2)])) -
119 (m[M(0, 1)] * (m[M(1, 0)] * m[M(2, 2)] - m[M(2, 0)] * m[M(1, 2)])) +
120 (m[M(0, 2)] * (m[M(1, 0)] * m[M(2, 1)] - m[M(2, 0)] * m[M(1, 1)]));
121
122 coef.transpose();
123
124 for(int i=0; i<4; i++) {
125 for(int j=0; j<4; j++) {
126 coef.m[M(i, j)] = j % 2 ? -coef.m[M(i, j)] : coef.m[M(i, j)];
127 if(i % 2) coef.m[M(i, j)] = -coef.m[M(i, j)];
128 }
129 }
130
131 return coef;
132 }
133
134 void Matrix4x4::invert()
135 {
136 Matrix4x4 adj = adjoint();
137
138 float det = determinant();
139
140 for(int i=0; i<16; i++) {
141 m[i] = adj.m[i] / det;
142 }
143 }
144
145 void Matrix4x4::transpose()
146 {
147 float tmp[16];
148
149 memcpy(tmp, m, sizeof tmp);
150 for(int i=0; i<4; i++) {
151 for(int j=0; j<4; j++) {
152 m[M(i, j)] = tmp[M(j, i)];
153 }
154 }
155 }