1 #ifndef __GNUC__
2 #pragma once
3 #endif
4 #ifndef __XR_MATRIX_H__
5 #define __XR_MATRIX_H__
6
7 #include "xr_vector3.h"
8 #include "xr_limits.h"
9 #include "xr_math.h"
10
11 namespace xray_re {
12
13 template<typename T> struct _vector3;
14 template<typename T> struct _quaternion;
15
16 template<typename T> struct _matrix33 {
17 _matrix33<T>& set(const _matrix33<T>& a);
18 _matrix33<T>& set(const _vector3<T>& _i, const _vector3<T>& _j, const _vector3<T>& _k);
19 _matrix33<T>& identity();
20 T determinant() const;
21 union {
22 struct {
23 T _11, _12, _13;
24 T _21, _22, _23;
25 T _31, _32, _33;
26 };
27 struct {
28 _vector3<T> i;
29 _vector3<T> j;
30 _vector3<T> k;
31 };
32 T m[3][3];
33 };
34 };
35
36 typedef _matrix33<float> fmatrix33;
37 typedef _matrix33<double> dmatrix33;
38
39 template<typename T> inline
set(const _vector3<T> & _i,const _vector3<T> & _j,const _vector3<T> & _k)40 _matrix33<T>& _matrix33<T>::set(const _vector3<T>& _i, const _vector3<T>& _j, const _vector3<T>& _k)
41 {
42 i.set(_i);
43 j.set(_j);
44 k.set(_k);
45 return *this;
46 }
47
identity()48 template<typename T> inline _matrix33<T>& _matrix33<T>::identity()
49 {
50 i.set(1, 0, 0);
51 j.set(0, 1, 0);
52 k.set(0, 0, 1);
53 return *this;
54 }
55
determinant()56 template<typename T> inline T _matrix33<T>::determinant() const
57 {
58 T cf1 = _22*_33 - _23*_32;
59 T cf2 = _21*_33 - _23*_31;
60 T cf3 = _21*_32 - _22*_31;
61 return _11*cf1 - _12*cf2 + _13*cf3;
62 }
63
64 ////////////////////////////////////////////////////////////////////////////////
65
66 template<typename T> struct _matrix {
67 template<typename F> _matrix<T>& set(const _matrix<F>& a);
68
69 _matrix<T>& set(const _matrix33<T>& a);
70 _matrix<T>& identity();
71 bool is_identity() const;
72 _matrix<T>& mk_xform(const _quaternion<T>& q, const _vector3<T>& v);
73 _matrix<T>& rotation(const _quaternion<T>& q);
74 _matrix<T>& translation(T x, T y, T z);
75 _matrix<T>& translation(const _vector3<T>& v);
76 _matrix<T>& mul(const _matrix<T>& a, const _matrix<T>& b);
77 _matrix<T>& mul_43(const _matrix<T>& a, const _matrix<T>& b);
78 _matrix<T>& mul_a_43(const _matrix<T>& a);
79 _matrix<T>& mul_b_43(const _matrix<T>& b);
80 _matrix<T>& invert_43();
81 _matrix<T>& invert_43(const _matrix<T>& a);
82 _matrix<T>& set_hpb(T h, T p, T b); // not HPB (YXZ), but ZXY really?
83 _matrix<T>& set_hpb(const _vector3<T>& v);
84 _matrix<T>& set_xyz(T x, T y, T z); // not XYZ, but reordered HPB (
85 _matrix<T>& set_xyz(const _vector3<T>& v);
86 _matrix<T>& set_xyz_i(T x, T y, T z);
87 _matrix<T>& set_xyz_i(const _vector3<T>& v);
88 void get_hpb(T& h, T& p, T& b) const;
89 void get_hpb(_vector3<T>& v) const;
90 void get_xyz(T& x, T& y, T& z) const;
91 void get_xyz(_vector3<T>& v) const;
92 void get_xyz_i(T& x, T& y, T& z) const;
93 void get_xyz_i(_vector3<T>& v) const;
94
95 // gr1ph starts
96 bool can_invert_43(void) const;
97 //gr1ph ends
98
99 // these will operate with "canonical" euler angles
100 _matrix<T>& set_euler_xyz(T x, T y, T z);
101 _matrix<T>& set_euler_xyz(const _vector3<T>& v);
102 void get_euler_xyz(T& x, T& y, T& z) const;
103 void get_euler_xyz(_vector3<T>& v) const;
104
105 union {
106 struct {
107 T _11, _12, _13, _14;
108 T _21, _22, _23, _24;
109 T _31, _32, _33, _34;
110 T _41, _42, _43, _44;
111 };
112 struct {
113 _vector3<T> i;
114 T _14_dummy;
115 _vector3<T> j;
116 T _24_dummy;
117 _vector3<T> k;
118 T _34_dummy;
119 _vector3<T> c;
120 T _44_dummy;
121 };
122 T m[4][4];
123 T __array[16];
124 };
125 };
126
127 typedef _matrix<float> fmatrix;
128 typedef _matrix<double> dmatrix;
129
set(const _matrix<F> & a)130 template<typename T> template<typename F> inline _matrix<T>& _matrix<T>::set(const _matrix<F>& a)
131 {
132 for (uint_fast32_t i = 16; i != 0;) {
133 --i;
134 __array[i] = T(a.__array[i]);
135 }
136 return *this;
137 }
138
set(const _matrix33<T> & a)139 template<typename T> inline _matrix<T>& _matrix<T>::set(const _matrix33<T>& a)
140 {
141 i.set(a.i); _14 = 0;
142 j.set(a.j); _24 = 0;
143 k.set(a.k); _34 = 0;
144 c.set(0, 0, 0); _44 = 1;
145 return *this;
146 }
147
identity()148 template<typename T> inline _matrix<T>& _matrix<T>::identity()
149 {
150 i.set(1, 0, 0); _14 = 0;
151 j.set(0, 1, 0); _24 = 0;
152 k.set(0, 0, 1); _34 = 0;
153 c.set(0, 0, 0); _44 = 1;
154 return *this;
155 }
156
is_identity()157 template<typename T> inline bool _matrix<T>::is_identity() const
158 {
159 return _11 == 1 && _12 == 0 && _13 == 0 && _14 == 0 &&
160 _21 == 0 && _22 == 1 && _23 == 0 && _24 == 0 &&
161 _31 == 0 && _32 == 0 && _33 == 1 && _34 == 0 &&
162 _41 == 0 && _42 == 0 && _43 == 0 && _44 == 1;
163 }
164
mk_xform(const _quaternion<T> & q,const _vector3<T> & v)165 template<typename T> _matrix<T>& _matrix<T>::mk_xform(const _quaternion<T>& q, const _vector3<T>& v)
166 {
167 T wx = q.w*q.x;
168 T wy = q.w*q.y;
169 T wz = q.w*q.z;
170
171 T xx = q.x*q.x;
172 T xy = q.x*q.y;
173 T xz = q.x*q.z;
174
175 T yy = q.y*q.y;
176 T yz = q.y*q.z;
177 T zz = q.z*q.z;
178
179 #if 1
180 // can't assume the unit quaternion because of quantization errors.
181 T s = T(2)/(xx + yy + zz + q.w*q.w);
182 #else
183 xr_assert(equivalent<T>(q.magnitude(), 1, T(1e-3)));
184 const T s = 2;
185 #endif
186
187 i.set(T(1) - s*(yy + zz), s*(xy - wz), s*(xz + wy)); _14 = 0;
188 j.set(s*(xy + wz), T(1) - s*(xx + zz), s*(yz - wx)); _24 = 0;
189 k.set(s*(xz - wy), s*(yz + wx), T(1) - s*(xx + yy)); _34 = 0;
190 c.set(v); _44 = T(1);
191
192 return *this;
193 }
194
rotation(const _quaternion<T> & q)195 template<typename T> inline _matrix<T>& _matrix<T>::rotation(const _quaternion<T>& q)
196 {
197 return mk_xform(q, _vector3<T>().set());
198 }
199
translation(T x,T y,T z)200 template<typename T> inline _matrix<T>& _matrix<T>::translation(T x, T y, T z)
201 {
202 identity();
203 c.set(x, y, z);
204 return *this;
205 }
206
translation(const _vector3<T> & v)207 template<typename T> inline _matrix<T>& _matrix<T>::translation(const _vector3<T>& v)
208 {
209 return translation(v.x, v.y, v.z);
210 }
211
mul(const _matrix<T> & a,const _matrix<T> & b)212 template<typename T> _matrix<T>& _matrix<T>::mul(const _matrix<T>& a, const _matrix<T>& b)
213 {
214 assert(this != &a && this != &b);
215 _11 = a._11*b._11 + a._21*b._12 + a._31*b._13 + a._41*b._14;
216 _12 = a._12*b._11 + a._22*b._12 + a._32*b._13 + a._42*b._14;
217 _13 = a._13*b._11 + a._23*b._12 + a._33*b._13 + a._43*b._14;
218 _14 = a._14*b._11 + a._24*b._12 + a._34*b._13 + a._44*b._14;
219
220 _21 = a._11*b._21 + a._21*b._22 + a._31*b._23 + a._41*b._24;
221 _22 = a._12*b._21 + a._22*b._22 + a._32*b._23 + a._42*b._24;
222 _23 = a._13*b._21 + a._23*b._22 + a._33*b._23 + a._43*b._24;
223 _24 = a._14*b._21 + a._24*b._22 + a._34*b._23 + a._44*b._24;
224
225 _31 = a._11*b._31 + a._21*b._32 + a._31*b._33 + a._41*b._34;
226 _32 = a._12*b._31 + a._22*b._32 + a._32*b._33 + a._42*b._34;
227 _33 = a._13*b._31 + a._23*b._32 + a._33*b._33 + a._43*b._34;
228 _34 = a._14*b._31 + a._24*b._32 + a._34*b._33 + a._44*b._34;
229
230 _41 = a._11*b._41 + a._21*b._42 + a._31*b._43 + a._41*b._44;
231 _42 = a._12*b._41 + a._22*b._42 + a._32*b._43 + a._42*b._44;
232 _43 = a._13*b._41 + a._23*b._42 + a._33*b._43 + a._43*b._44;
233 _44 = a._14*b._41 + a._24*b._42 + a._34*b._43 + a._44*b._44;
234
235 return *this;
236 }
237
mul_43(const _matrix<T> & a,const _matrix<T> & b)238 template<typename T> _matrix<T>& _matrix<T>::mul_43(const _matrix<T>& a, const _matrix<T>& b)
239 {
240 assert(this != &a && this != &b);
241 _11 = a._11*b._11 + a._21*b._12 + a._31*b._13;
242 _12 = a._12*b._11 + a._22*b._12 + a._32*b._13;
243 _13 = a._13*b._11 + a._23*b._12 + a._33*b._13;
244 _14 = 0;
245
246 _21 = a._11*b._21 + a._21*b._22 + a._31*b._23;
247 _22 = a._12*b._21 + a._22*b._22 + a._32*b._23;
248 _23 = a._13*b._21 + a._23*b._22 + a._33*b._23;
249 _24 = 0;
250
251 _31 = a._11*b._31 + a._21*b._32 + a._31*b._33;
252 _32 = a._12*b._31 + a._22*b._32 + a._32*b._33;
253 _33 = a._13*b._31 + a._23*b._32 + a._33*b._33;
254 _34 = 0;
255
256 _41 = a._11*b._41 + a._21*b._42 + a._31*b._43 + a._41;
257 _42 = a._12*b._41 + a._22*b._42 + a._32*b._43 + a._42;
258 _43 = a._13*b._41 + a._23*b._42 + a._33*b._43 + a._43;
259 _44 = 1;
260
261 return *this;
262 }
263
mul_a_43(const _matrix<T> & a)264 template<typename T> inline _matrix<T>& _matrix<T>::mul_a_43(const _matrix<T>& a)
265 {
266 return mul_43(a, _matrix<T>().set(*this));
267 }
268
mul_b_43(const _matrix<T> & b)269 template<typename T> inline _matrix<T>& _matrix<T>::mul_b_43(const _matrix<T>& b)
270 {
271 return mul_43(_matrix<T>().set(*this), b);
272 }
273
can_invert_43(void)274 template<typename T> bool _matrix<T>::can_invert_43(void) const
275 {
276 T cf1 = _22*_33 - _23*_32;
277 T cf2 = _21*_33 - _23*_31;
278 T cf3 = _21*_32 - _22*_31;
279 T det = _11*cf1 - _12*cf2 + _13*cf3;
280
281 return (!equivalent<T>(det, 0, 0.0000001f));
282 }
283
invert_43(const _matrix<T> & a)284 template<typename T> _matrix<T>& _matrix<T>::invert_43(const _matrix<T>& a)
285 {
286 T cf1 = a._22*a._33 - a._23*a._32;
287 T cf2 = a._21*a._33 - a._23*a._31;
288 T cf3 = a._21*a._32 - a._22*a._31;
289
290 T det = a._11*cf1 - a._12*cf2 + a._13*cf3;
291
292 //xr_assert(!equivalent<T>(det, 0));
293 // 1850 xform asserted
294 xr_assert(!equivalent<T>(det, 0, 0.0000001f));
295
296 _11 = cf1/det;
297 _21 =-cf2/det;
298 _31 = cf3/det;
299
300 // _11 = (a._22*a._33 - a._23*a._32)/det;
301 _12 =-(a._12*a._33 - a._13*a._32)/det;
302 _13 = (a._12*a._23 - a._13*a._22)/det;
303 _14 = 0;
304
305 // _21 =-(a._21*a._33 - a._23*a._31)/det;
306 _22 = (a._11*a._33 - a._13*a._31)/det;
307 _23 =-(a._11*a._23 - a._13*a._21)/det;
308 _24 = 0;
309
310 // _31 = (a._21*a._32 - a._22*a._31)/det;
311 _32 =-(a._11*a._32 - a._12*a._31)/det;
312 _33 = (a._11*a._22 - a._12*a._21)/det;
313 _34 = 0;
314
315 _41 =-(a._41*_11 + a._42*_21 + a._43*_31);
316 _42 =-(a._41*_12 + a._42*_22 + a._43*_32);
317 _43 =-(a._41*_13 + a._42*_23 + a._43*_33);
318 _44 = 1;
319
320 return *this;
321 }
322
invert_43()323 template<typename T> inline _matrix<T>& _matrix<T>::invert_43()
324 {
325 return invert_43(_matrix<T>().set(*this));
326 }
327
set_hpb(T h,T p,T b)328 template<typename T> inline _matrix<T>& _matrix<T>::set_hpb(T h, T p, T b)
329 {
330 T sh = std::sin(h);
331 T ch = std::cos(h);
332 T sp = std::sin(p);
333 T cp = std::cos(p);
334 T sb = std::sin(b);
335 T cb = std::cos(b);
336
337 #if 1
338 _11 = ch*cb - sh*sp*sb;
339 _12 = -cp*sb;
340 _13 = ch*sb*sp + sh*cb;
341 _14 = 0;
342
343 _21 = sp*sh*cb + ch*sb;
344 _22 = cb*cp;
345 _23 = sh*sb - sp*ch*cb;
346 _24 = 0;
347
348 _31 = -cp*sh;
349 _32 = sp;
350 _33 = ch*cp;
351 _34 = 0;
352
353 _41 = 0;
354 _42 = 0;
355 _43 = 0;
356 _44 = T(1);
357 #else
358 i.set(ch*cb - sh*sp*sb, -cp*sb, ch*sb*sp + sh*cb); _14 = 0;
359 j.set(sp*sh*cb + ch*sb, cb*cp, sh*sb - sp*ch*cb); _24 = 0;
360 k.set(-cp*sh, sp, ch*cp); _34 = 0;
361 c.set(0, 0, 0); _44 = T(1);
362 #endif
363
364 return *this;
365 }
366
367 template<typename T> inline
set_hpb(const _vector3<T> & v)368 _matrix<T>& _matrix<T>::set_hpb(const _vector3<T>& v) { return set_hpb(v.x, v.y, v.z); }
369
370 template<typename T> inline
set_xyz(T x,T y,T z)371 _matrix<T>& _matrix<T>::set_xyz(T x, T y, T z) { return set_hpb(y, x, z); }
372
373 template<typename T> inline
set_xyz(const _vector3<T> & v)374 _matrix<T>& _matrix<T>::set_xyz(const _vector3<T>& v) { return set_hpb(v.y, v.x, v.z);}
375
376 template<typename T> inline
set_xyz_i(T x,T y,T z)377 _matrix<T>& _matrix<T>::set_xyz_i(T x, T y, T z) { return set_hpb(-y, -x, -z); }
378
379 template<typename T> inline
set_xyz_i(const _vector3<T> & v)380 _matrix<T>& _matrix<T>::set_xyz_i(const _vector3<T>& v) { return set_hpb(-v.y, -v.x, -v.z); }
381
get_hpb(T & h,T & p,T & b)382 template<typename T> void _matrix<T>::get_hpb(T& h, T& p, T& b) const
383 {
384 T cy = std::sqrt(_12*_12 + _22*_22);
385 if (cy > 16*xr_numeric_limits<T>::epsilon()) {
386 h = -std::atan2(_31, _33);
387 p = -std::atan2(-_32, cy);
388 b = -std::atan2(_12, _22);
389 } else {
390 h = -std::atan2(-_13, _11);
391 p = -std::atan2(-_32, cy);
392 b = 0;
393 }
394 }
395
396 template<typename T> inline
get_hpb(_vector3<T> & v)397 void _matrix<T>::get_hpb(_vector3<T>& v) const { get_hpb(v.x, v.y, v.z); }
398
399 template<typename T> inline
get_xyz(T & x,T & y,T & z)400 void _matrix<T>::get_xyz(T& x, T& y, T& z) const { get_hpb(y, x, z); }
401
402 template<typename T> inline
get_xyz(_vector3<T> & v)403 void _matrix<T>::get_xyz(_vector3<T>& v) const { get_hpb(v.y, v.x, v.z); }
404
get_xyz_i(T & x,T & y,T & z)405 template<typename T> inline void _matrix<T>::get_xyz_i(T& x, T& y, T& z) const
406 {
407 get_hpb(y, x, z);
408 x = -x;
409 y = -y;
410 z = -z;
411 }
412
get_xyz_i(_vector3<T> & v)413 template<typename T> inline void _matrix<T>::get_xyz_i(_vector3<T>& v) const
414 {
415 get_hpb(v.y, v.x, v.z);
416 v.invert();
417 }
418
419 // ZXY: i=2, j=1, k=3
420 // XYZ: i=3, j=2, k=1
421 // _11 -> _22
422 // _12 -> _23
423 // _13 -> _21
424 // _21 -> _32
425 // _22 -> _33
426 // _23 -> _31
427 // _31 -> _12
428 // _32 -> _13
429 // _33 -> _11
set_euler_xyz(T x,T y,T z)430 template<typename T> inline _matrix<T>& _matrix<T>::set_euler_xyz(T x, T y, T z)
431 {
432 x = -x;
433 y = -y;
434 z = -z;
435
436 T sx = std::sin(z);
437 T cx = std::cos(z);
438 T sy = std::sin(y);
439 T cy = std::cos(y);
440 T sz = std::sin(x);
441 T cz = std::cos(x);
442
443 _11 = cx*cy;
444 _12 = -cy*sx;
445 _13 = sy;
446 _14 = 0;
447
448 _21 = cx*sz*sy + sx*cz;
449 _22 = cx*cz - sx*sy*sz;
450 _23 = -cy*sz;
451 _24 = 0;
452
453 _31 = sx*sz - sy*cx*cz;
454 _32 = sy*sx*cz + cx*sz;
455 _33 = cz*cy;
456 _34 = 0;
457
458 _41 = 0;
459 _42 = 0;
460 _43 = 0;
461 _44 = T(1);
462
463 return *this;
464 }
465
466 template<typename T> inline
set_euler_xyz(const _vector3<T> & v)467 _matrix<T>& _matrix<T>::set_euler_xyz(const _vector3<T>& v) { return set_euler_xyz(v.x, v.y, v.z);}
468
469 // _11 -> _22
470 // _12 -> _23
471 // _13 -> _21
472 // _22 -> _33
473 // _31 -> _12
474 // _32 -> _13
475 // _33 -> _11
get_euler_xyz(T & x,T & y,T & z)476 template<typename T> inline void _matrix<T>::get_euler_xyz(T& x, T& y, T& z) const
477 {
478 T cy = std::sqrt(_23*_23 + _33*_33);
479 if (cy > 16*xr_numeric_limits<T>::epsilon()) {
480 z = std::atan2(_12, _11);
481 y = std::atan2(-_13, cy);
482 x = std::atan2(_23, _33);
483 } else {
484 z = std::atan2(-_21, _22);
485 y = std::atan2(-_13, cy);
486 x = 0;
487 }
488 }
489
490 template<typename T> inline
get_euler_xyz(_vector3<T> & v)491 void _matrix<T>::get_euler_xyz(_vector3<T>& v) const { get_euler_xyz(v.x, v.y, v.z); }
492
493 } // end of namespace xray_re
494
495 #endif
496