Sensor Scol plugin
Multi platform sensors for handled devices
Matrix4.h
1
2#ifndef __Matrix4__
3#define __Matrix4__
4
5#include <cassert>
6#include "tools/Vector3d.h"
7
8class Matrix4
9{
10protected:
12 union {
13 double m[4][4];
14 double _m[16];
15 };
16public:
21 inline Matrix4()
22 {
23 }
24
25 inline Matrix4(
26 double m00, double m01, double m02, double m03,
27 double m10, double m11, double m12, double m13,
28 double m20, double m21, double m22, double m23,
29 double m30, double m31, double m32, double m33 )
30 {
31 m[0][0] = m00;
32 m[0][1] = m01;
33 m[0][2] = m02;
34 m[0][3] = m03;
35 m[1][0] = m10;
36 m[1][1] = m11;
37 m[1][2] = m12;
38 m[1][3] = m13;
39 m[2][0] = m20;
40 m[2][1] = m21;
41 m[2][2] = m22;
42 m[2][3] = m23;
43 m[3][0] = m30;
44 m[3][1] = m31;
45 m[3][2] = m32;
46 m[3][3] = m33;
47 }
48
49 inline Matrix4(double* mat)
50 {
51 m[0][0] = mat[0];
52 m[0][1] = mat[1];
53 m[0][2] = mat[2];
54 m[0][3] = mat[3];
55 m[1][0] = mat[4];
56 m[1][1] = mat[5];
57 m[1][2] = mat[6];
58 m[1][3] = mat[7];
59 m[2][0] = mat[8];
60 m[2][1] = mat[9];
61 m[2][2] = mat[10];
62 m[2][3] = mat[11];
63 m[3][0] = mat[12];
64 m[3][1] = mat[13];
65 m[3][2] = mat[14];
66 m[3][3] = mat[15];
67 }
68
69 inline void convert(double mat[16])
70 {
71 mat[0] = m[0][0];
72 mat[1] = m[0][1];
73 mat[2] = m[0][2];
74 mat[3] = m[0][3];
75 mat[4] = m[1][0];
76 mat[5] = m[1][1];
77 mat[6] = m[1][2];
78 mat[7] = m[1][3];
79 mat[8] = m[2][0];
80 mat[9] = m[2][1];
81 mat[10] = m[2][2];
82 mat[11] = m[2][3];
83 mat[12] = m[3][0];
84 mat[13] = m[3][1];
85 mat[14] = m[3][2];
86 mat[15] = m[3][3];
87 }
88
89 inline double* operator [] ( unsigned int iRow )
90 {
91 assert( iRow < 4 );
92 return m[iRow];
93 }
94
95 inline const double *operator [] ( unsigned int iRow ) const
96 {
97 assert( iRow < 4 );
98 return m[iRow];
99 }
100
101 inline Matrix4 concatenate(const Matrix4 &m2) const
102 {
103 Matrix4 r;
104 r.m[0][0] = m[0][0] * m2.m[0][0] + m[0][1] * m2.m[1][0] + m[0][2] * m2.m[2][0] + m[0][3] * m2.m[3][0];
105 r.m[0][1] = m[0][0] * m2.m[0][1] + m[0][1] * m2.m[1][1] + m[0][2] * m2.m[2][1] + m[0][3] * m2.m[3][1];
106 r.m[0][2] = m[0][0] * m2.m[0][2] + m[0][1] * m2.m[1][2] + m[0][2] * m2.m[2][2] + m[0][3] * m2.m[3][2];
107 r.m[0][3] = m[0][0] * m2.m[0][3] + m[0][1] * m2.m[1][3] + m[0][2] * m2.m[2][3] + m[0][3] * m2.m[3][3];
108
109 r.m[1][0] = m[1][0] * m2.m[0][0] + m[1][1] * m2.m[1][0] + m[1][2] * m2.m[2][0] + m[1][3] * m2.m[3][0];
110 r.m[1][1] = m[1][0] * m2.m[0][1] + m[1][1] * m2.m[1][1] + m[1][2] * m2.m[2][1] + m[1][3] * m2.m[3][1];
111 r.m[1][2] = m[1][0] * m2.m[0][2] + m[1][1] * m2.m[1][2] + m[1][2] * m2.m[2][2] + m[1][3] * m2.m[3][2];
112 r.m[1][3] = m[1][0] * m2.m[0][3] + m[1][1] * m2.m[1][3] + m[1][2] * m2.m[2][3] + m[1][3] * m2.m[3][3];
113
114 r.m[2][0] = m[2][0] * m2.m[0][0] + m[2][1] * m2.m[1][0] + m[2][2] * m2.m[2][0] + m[2][3] * m2.m[3][0];
115 r.m[2][1] = m[2][0] * m2.m[0][1] + m[2][1] * m2.m[1][1] + m[2][2] * m2.m[2][1] + m[2][3] * m2.m[3][1];
116 r.m[2][2] = m[2][0] * m2.m[0][2] + m[2][1] * m2.m[1][2] + m[2][2] * m2.m[2][2] + m[2][3] * m2.m[3][2];
117 r.m[2][3] = m[2][0] * m2.m[0][3] + m[2][1] * m2.m[1][3] + m[2][2] * m2.m[2][3] + m[2][3] * m2.m[3][3];
118
119 r.m[3][0] = m[3][0] * m2.m[0][0] + m[3][1] * m2.m[1][0] + m[3][2] * m2.m[2][0] + m[3][3] * m2.m[3][0];
120 r.m[3][1] = m[3][0] * m2.m[0][1] + m[3][1] * m2.m[1][1] + m[3][2] * m2.m[2][1] + m[3][3] * m2.m[3][1];
121 r.m[3][2] = m[3][0] * m2.m[0][2] + m[3][1] * m2.m[1][2] + m[3][2] * m2.m[2][2] + m[3][3] * m2.m[3][2];
122 r.m[3][3] = m[3][0] * m2.m[0][3] + m[3][1] * m2.m[1][3] + m[3][2] * m2.m[2][3] + m[3][3] * m2.m[3][3];
123
124 return r;
125 }
126
129 inline Matrix4 operator * ( const Matrix4 &m2 ) const
130 {
131 return concatenate( m2 );
132 }
133
143 inline Vector3d operator * ( const Vector3d &v ) const
144 {
145 Vector3d r;
146
147 double fInvW = 1.0f / ( m[3][0] * v.x + m[3][1] * v.y + m[3][2] * v.z + m[3][3] );
148
149 r.x = ( m[0][0] * v.x + m[0][1] * v.y + m[0][2] * v.z + m[0][3] ) * fInvW;
150 r.y = ( m[1][0] * v.x + m[1][1] * v.y + m[1][2] * v.z + m[1][3] ) * fInvW;
151 r.z = ( m[2][0] * v.x + m[2][1] * v.y + m[2][2] * v.z + m[2][3] ) * fInvW;
152
153 return r;
154 }
155
156
159 inline Matrix4 operator + ( const Matrix4 &m2 ) const
160 {
161 Matrix4 r;
162
163 r.m[0][0] = m[0][0] + m2.m[0][0];
164 r.m[0][1] = m[0][1] + m2.m[0][1];
165 r.m[0][2] = m[0][2] + m2.m[0][2];
166 r.m[0][3] = m[0][3] + m2.m[0][3];
167
168 r.m[1][0] = m[1][0] + m2.m[1][0];
169 r.m[1][1] = m[1][1] + m2.m[1][1];
170 r.m[1][2] = m[1][2] + m2.m[1][2];
171 r.m[1][3] = m[1][3] + m2.m[1][3];
172
173 r.m[2][0] = m[2][0] + m2.m[2][0];
174 r.m[2][1] = m[2][1] + m2.m[2][1];
175 r.m[2][2] = m[2][2] + m2.m[2][2];
176 r.m[2][3] = m[2][3] + m2.m[2][3];
177
178 r.m[3][0] = m[3][0] + m2.m[3][0];
179 r.m[3][1] = m[3][1] + m2.m[3][1];
180 r.m[3][2] = m[3][2] + m2.m[3][2];
181 r.m[3][3] = m[3][3] + m2.m[3][3];
182
183 return r;
184 }
185
188 inline Matrix4 operator - ( const Matrix4 &m2 ) const
189 {
190 Matrix4 r;
191 r.m[0][0] = m[0][0] - m2.m[0][0];
192 r.m[0][1] = m[0][1] - m2.m[0][1];
193 r.m[0][2] = m[0][2] - m2.m[0][2];
194 r.m[0][3] = m[0][3] - m2.m[0][3];
195
196 r.m[1][0] = m[1][0] - m2.m[1][0];
197 r.m[1][1] = m[1][1] - m2.m[1][1];
198 r.m[1][2] = m[1][2] - m2.m[1][2];
199 r.m[1][3] = m[1][3] - m2.m[1][3];
200
201 r.m[2][0] = m[2][0] - m2.m[2][0];
202 r.m[2][1] = m[2][1] - m2.m[2][1];
203 r.m[2][2] = m[2][2] - m2.m[2][2];
204 r.m[2][3] = m[2][3] - m2.m[2][3];
205
206 r.m[3][0] = m[3][0] - m2.m[3][0];
207 r.m[3][1] = m[3][1] - m2.m[3][1];
208 r.m[3][2] = m[3][2] - m2.m[3][2];
209 r.m[3][3] = m[3][3] - m2.m[3][3];
210
211 return r;
212 }
213
216 inline bool operator == ( const Matrix4& m2 ) const
217 {
218 if(
219 m[0][0] != m2.m[0][0] || m[0][1] != m2.m[0][1] || m[0][2] != m2.m[0][2] || m[0][3] != m2.m[0][3] ||
220 m[1][0] != m2.m[1][0] || m[1][1] != m2.m[1][1] || m[1][2] != m2.m[1][2] || m[1][3] != m2.m[1][3] ||
221 m[2][0] != m2.m[2][0] || m[2][1] != m2.m[2][1] || m[2][2] != m2.m[2][2] || m[2][3] != m2.m[2][3] ||
222 m[3][0] != m2.m[3][0] || m[3][1] != m2.m[3][1] || m[3][2] != m2.m[3][2] || m[3][3] != m2.m[3][3] )
223 return false;
224 return true;
225 }
226
229 inline bool operator != ( const Matrix4& m2 ) const
230 {
231 if(
232 m[0][0] != m2.m[0][0] || m[0][1] != m2.m[0][1] || m[0][2] != m2.m[0][2] || m[0][3] != m2.m[0][3] ||
233 m[1][0] != m2.m[1][0] || m[1][1] != m2.m[1][1] || m[1][2] != m2.m[1][2] || m[1][3] != m2.m[1][3] ||
234 m[2][0] != m2.m[2][0] || m[2][1] != m2.m[2][1] || m[2][2] != m2.m[2][2] || m[2][3] != m2.m[2][3] ||
235 m[3][0] != m2.m[3][0] || m[3][1] != m2.m[3][1] || m[3][2] != m2.m[3][2] || m[3][3] != m2.m[3][3] )
236 return true;
237 return false;
238 }
239
240 inline Matrix4 transpose(void) const
241 {
242 return Matrix4(m[0][0], m[1][0], m[2][0], m[3][0],
243 m[0][1], m[1][1], m[2][1], m[3][1],
244 m[0][2], m[1][2], m[2][2], m[3][2],
245 m[0][3], m[1][3], m[2][3], m[3][3]);
246 }
247
248 inline void makeTrans( double tx, double ty, double tz )
249 {
250 m[0][0] = 1.0; m[0][1] = 0.0; m[0][2] = 0.0; m[0][3] = tx;
251 m[1][0] = 0.0; m[1][1] = 1.0; m[1][2] = 0.0; m[1][3] = ty;
252 m[2][0] = 0.0; m[2][1] = 0.0; m[2][2] = 1.0; m[2][3] = tz;
253 m[3][0] = 0.0; m[3][1] = 0.0; m[3][2] = 0.0; m[3][3] = 1.0;
254 }
255
256static const Matrix4 ZERO;
257static const Matrix4 IDENTITY;
258
259 inline Matrix4 operator*(double scalar) const
260 {
261 return Matrix4(
262 scalar*m[0][0], scalar*m[0][1], scalar*m[0][2], scalar*m[0][3],
263 scalar*m[1][0], scalar*m[1][1], scalar*m[1][2], scalar*m[1][3],
264 scalar*m[2][0], scalar*m[2][1], scalar*m[2][2], scalar*m[2][3],
265 scalar*m[3][0], scalar*m[3][1], scalar*m[3][2], scalar*m[3][3]);
266 }
267
268};
269#endif
Matrix4::operator!=
bool operator!=(const Matrix4 &m2) const
Definition Matrix4.h:229
Matrix4::operator-
Matrix4 operator-(const Matrix4 &m2) const
Definition Matrix4.h:188
Matrix4::operator==
bool operator==(const Matrix4 &m2) const
Definition Matrix4.h:216
Matrix4::Matrix4
Matrix4()
Definition Matrix4.h:21
Matrix4::operator*
Matrix4 operator*(const Matrix4 &m2) const
Definition Matrix4.h:129
Matrix4::operator+
Matrix4 operator+(const Matrix4 &m2) const
Definition Matrix4.h:159
Generated by doxygen 1.9.8