1 /*
2 * vec_mat.h - vector and matrix defination & calculation
3 *
4 * Copyright (c) 2017 Intel Corporation
5 *
6 * Licensed under the Apache License, Version 2.0 (the "License");
7 * you may not use this file except in compliance with the License.
8 * You may obtain a copy of the License at
9 *
10 * http://www.apache.org/licenses/LICENSE-2.0
11 *
12 * Unless required by applicable law or agreed to in writing, software
13 * distributed under the License is distributed on an "AS IS" BASIS,
14 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
15 * See the License for the specific language governing permissions and
16 * limitations under the License.
17 *
18 * Author: Zong Wei <wei.zong@intel.com>
19 */
20
21 #ifndef XCAM_VECTOR_MATRIX_H
22 #define XCAM_VECTOR_MATRIX_H
23
24 #include <xcam_std.h>
25 #include <cmath>
26
27
28 namespace XCam {
29
30 #ifndef PI
31 #define PI 3.14159265358979323846
32 #endif
33
34 #ifndef FLT_EPSILON
35 #define FLT_EPSILON 1.19209290e-07F // float
36 #endif
37
38 #ifndef DBL_EPSILON
39 #define DBL_EPSILON 2.2204460492503131e-16 // double
40 #endif
41
42 #ifndef DEGREE_2_RADIANS
43 #define DEGREE_2_RADIANS(x) (((x) * PI) / 180.0)
44 #endif
45
46 #ifndef RADIANS_2_DEGREE
47 #define RADIANS_2_DEGREE(x) (((x) * 180.0) / PI)
48 #endif
49
50 #define XCAM_VECT2_OPERATOR_VECT2(op) \
51 Vector2<T> operator op (const Vector2<T>& b) const { \
52 return Vector2<T>(x op b.x, y op b.y); \
53 } \
54 Vector2<T> &operator op##= (const Vector2<T>& b) { \
55 x op##= b.x; y op##= b.y; return *this; \
56 }
57
58 #define XCAM_VECT2_OPERATOR_SCALER(op) \
59 Vector2<T> operator op (const T& b) const { \
60 return Vector2<T>(x op b, y op b); \
61 } \
62 Vector2<T> &operator op##= (const T& b) { \
63 x op##= b; y op##= b; return *this; \
64 }
65
66 template<class T>
67 class Vector2
68 {
69 public:
70
71 T x;
72 T y;
73
Vector2()74 Vector2 () : x(0), y(0) {};
Vector2(T _x,T _y)75 Vector2 (T _x, T _y) : x(_x), y(_y) {};
76
77 template <typename New>
convert_to()78 Vector2<New> convert_to () const {
79 Vector2<New> ret((New)(this->x), (New)(this->y));
80 return ret;
81 }
82
83 Vector2<T>& operator = (const Vector2<T>& rhs)
84 {
85 x = rhs.x;
86 y = rhs.y;
87 return *this;
88 }
89
90 template <typename Other>
91 Vector2<T>& operator = (const Vector2<Other>& rhs)
92 {
93 x = rhs.x;
94 y = rhs.y;
95 return *this;
96 }
97
98 Vector2<T> operator - () const {
99 return Vector2<T>(-x, -y);
100 }
101
102 XCAM_VECT2_OPERATOR_VECT2 (+)
103 XCAM_VECT2_OPERATOR_VECT2 (-)
104 XCAM_VECT2_OPERATOR_VECT2 (*)
105 XCAM_VECT2_OPERATOR_VECT2 ( / )
106 XCAM_VECT2_OPERATOR_SCALER (+)
107 XCAM_VECT2_OPERATOR_SCALER (-)
108 XCAM_VECT2_OPERATOR_SCALER (*)
109 XCAM_VECT2_OPERATOR_SCALER ( / )
110
111 bool operator == (const Vector2<T>& rhs) const {
112 return (x == rhs.x) && (y == rhs.y);
113 }
114
reset()115 void reset () {
116 this->x = (T) 0;
117 this->y = (T) 0;
118 }
119
set(T _x,T _y)120 void set (T _x, T _y) {
121 this->x = _x;
122 this->y = _y;
123 }
124
magnitude()125 T magnitude () const {
126 return (T) sqrtf(x * x + y * y);
127 }
128
distance(const Vector2<T> & vec)129 float distance (const Vector2<T>& vec) const {
130 return sqrtf((vec.x - x) * (vec.x - x) + (vec.y - y) * (vec.y - y));
131 }
132
dot(const Vector2<T> & vec)133 T dot (const Vector2<T>& vec) const {
134 return (x * vec.x + y * vec.y);
135 }
136
lerp(T weight,const Vector2<T> & vec)137 inline Vector2<T> lerp (T weight, const Vector2<T>& vec) const {
138 return (*this) + (vec - (*this)) * weight;
139 }
140
141 };
142
143 template<class T, uint32_t N>
144 class VectorN
145 {
146 public:
147
148 VectorN ();
149 VectorN (T x);
150 VectorN (T x, T y);
151 VectorN (T x, T y, T z);
152 VectorN (T x, T y, T z, T w);
153 VectorN (VectorN<T, 3> vec3, T w);
154
155 inline VectorN<T, N>& operator = (const VectorN<T, N>& rhs);
156 inline VectorN<T, N> operator - () const;
157 inline bool operator == (const VectorN<T, N>& rhs) const;
158
159 inline T& operator [] (uint32_t index) {
160 XCAM_ASSERT(index >= 0 && index < N);
161 return data[index];
162 }
163 inline const T& operator [] (uint32_t index) const {
164 XCAM_ASSERT(index >= 0 && index < N);
165 return data[index];
166 }
167
168 inline VectorN<T, N> operator + (const T rhs) const;
169 inline VectorN<T, N> operator - (const T rhs) const;
170 inline VectorN<T, N> operator * (const T rhs) const;
171 inline VectorN<T, N> operator / (const T rhs) const;
172 inline VectorN<T, N> operator += (const T rhs);
173 inline VectorN<T, N> operator -= (const T rhs);
174 inline VectorN<T, N> operator *= (const T rhs);
175 inline VectorN<T, N> operator /= (const T rhs);
176
177 inline VectorN<T, N> operator + (const VectorN<T, N>& rhs) const;
178 inline VectorN<T, N> operator - (const VectorN<T, N>& rhs) const;
179 inline VectorN<T, N> operator * (const VectorN<T, N>& rhs) const;
180 inline VectorN<T, N> operator / (const VectorN<T, N>& rhs) const;
181 inline VectorN<T, N> operator += (const VectorN<T, N>& rhs);
182 inline VectorN<T, N> operator -= (const VectorN<T, N>& rhs);
183 inline VectorN<T, N> operator *= (const VectorN<T, N>& rhs);
184 inline VectorN<T, N> operator /= (const VectorN<T, N>& rhs);
185
186 template <typename NEW> inline
187 VectorN<NEW, N> convert_to () const;
188
189 inline void zeros ();
190 inline void set (T x, T y);
191 inline void set (T x, T y, T z);
192 inline void set (T x, T y, T z, T w);
193 inline T magnitude () const;
194 inline float distance (const VectorN<T, N>& vec) const;
195 inline T dot (const VectorN<T, N>& vec) const;
196 inline VectorN<T, N> lerp (T weight, const VectorN<T, N>& vec) const;
197
198 private:
199 T data[N];
200
201 };
202
203
204 template<class T, uint32_t N> inline
VectorN()205 VectorN<T, N>::VectorN ()
206 {
207 for (uint32_t i = 0; i < N; i++) {
208 data[i] = 0;
209 }
210 }
211
212 template<class T, uint32_t N> inline
VectorN(T x)213 VectorN<T, N>::VectorN (T x) {
214 data[0] = x;
215 }
216
217 template<class T, uint32_t N> inline
VectorN(T x,T y)218 VectorN<T, N>::VectorN (T x, T y) {
219 if (N >= 2) {
220 data[0] = x;
221 data[1] = y;
222 }
223 }
224
225 template<class T, uint32_t N> inline
VectorN(T x,T y,T z)226 VectorN<T, N>::VectorN (T x, T y, T z) {
227 if (N >= 3) {
228 data[0] = x;
229 data[1] = y;
230 data[2] = z;
231 }
232 }
233
234 template<class T, uint32_t N> inline
VectorN(T x,T y,T z,T w)235 VectorN<T, N>::VectorN (T x, T y, T z, T w) {
236 if (N >= 4) {
237 data[0] = x;
238 data[1] = y;
239 data[2] = z;
240 data[3] = w;
241 }
242 }
243
244 template<class T, uint32_t N> inline
VectorN(VectorN<T,3> vec3,T w)245 VectorN<T, N>::VectorN (VectorN<T, 3> vec3, T w) {
246 if (N >= 4) {
247 data[0] = vec3.data[0];
248 data[1] = vec3.data[1];
249 data[2] = vec3.data[2];
250 data[3] = w;
251 }
252 }
253
254 template<class T, uint32_t N> inline
255 VectorN<T, N>& VectorN<T, N>::operator = (const VectorN<T, N>& rhs) {
256 for (uint32_t i = 0; i < N; i++) {
257 data[i] = rhs.data[i];
258 }
259
260 return *this;
261 }
262
263 template<class T, uint32_t N> inline
264 VectorN<T, N> VectorN<T, N>::operator - () const {
265 for (uint32_t i = 0; i < N; i++) {
266 data[i] = -data[i];
267 }
268
269 return *this;
270 }
271
272 template<class T, uint32_t N> inline
273 VectorN<T, N> VectorN<T, N>::operator + (const T rhs) const {
274 VectorN<T, N> result;
275
276 for (uint32_t i = 0; i < N; i++) {
277 result.data[i] = data[i] + rhs;
278 }
279 return result;
280 }
281
282 template<class T, uint32_t N> inline
283 VectorN<T, N> VectorN<T, N>::operator - (const T rhs) const {
284 VectorN<T, N> result;
285
286 for (uint32_t i = 0; i < N; i++) {
287 result.data[i] = data[i] - rhs;
288 }
289 return result;
290 }
291
292 template<class T, uint32_t N> inline
293 VectorN<T, N> VectorN<T, N>::operator * (const T rhs) const {
294 VectorN<T, N> result;
295
296 for (uint32_t i = 0; i < N; i++) {
297 result.data[i] = data[i] * rhs;
298 }
299 return result;
300 }
301
302 template<class T, uint32_t N> inline
303 VectorN<T, N> VectorN<T, N>::operator / (const T rhs) const {
304 VectorN<T, N> result;
305
306 for (uint32_t i = 0; i < N; i++) {
307 result.data[i] = data[i] / rhs;
308 }
309 return result;
310 }
311
312 template<class T, uint32_t N> inline
313 VectorN<T, N> VectorN<T, N>::operator += (const T rhs) {
314 for (uint32_t i = 0; i < N; i++) {
315 data[i] += rhs;
316 }
317 return *this;
318 }
319
320 template<class T, uint32_t N> inline
321 VectorN<T, N> VectorN<T, N>::operator -= (const T rhs) {
322 for (uint32_t i = 0; i < N; i++) {
323 data[i] -= rhs;
324 }
325 return *this;
326 }
327
328 template<class T, uint32_t N> inline
329 VectorN<T, N> VectorN<T, N>::operator *= (const T rhs) {
330 for (uint32_t i = 0; i < N; i++) {
331 data[i] *= rhs;
332 }
333 return *this;
334 }
335
336 template<class T, uint32_t N> inline
337 VectorN<T, N> VectorN<T, N>::operator /= (const T rhs) {
338 for (uint32_t i = 0; i < N; i++) {
339 data[i] /= rhs;
340 }
341 return *this;
342 }
343
344 template<class T, uint32_t N> inline
345 VectorN<T, N> VectorN<T, N>::operator + (const VectorN<T, N>& rhs) const {
346 VectorN<T, N> result;
347
348 for (uint32_t i = 0; i < N; i++) {
349 result.data[i] = data[i] + rhs.data[i];
350 }
351 return result;
352 }
353
354 template<class T, uint32_t N> inline
355 VectorN<T, N> VectorN<T, N>::operator - (const VectorN<T, N>& rhs) const {
356 VectorN<T, N> result;
357
358 for (uint32_t i = 0; i < N; i++) {
359 result.data[i] = data[i] - rhs.data[i];
360 }
361 return result;
362 }
363
364 template<class T, uint32_t N> inline
365 VectorN<T, N> VectorN<T, N>::operator * (const VectorN<T, N>& rhs) const {
366 VectorN<T, N> result;
367
368 for (uint32_t i = 0; i < N; i++) {
369 result.data[i] = data[i] * rhs.data[i];
370 }
371 return result;
372 }
373
374 template<class T, uint32_t N> inline
375 VectorN<T, N> VectorN<T, N>::operator / (const VectorN<T, N>& rhs) const {
376 VectorN<T, N> result;
377
378 for (uint32_t i = 0; i < N; i++) {
379 result.data[i] = data[i] / rhs.data[i];
380 }
381 return result;
382 }
383
384 template<class T, uint32_t N> inline
385 VectorN<T, N> VectorN<T, N>::operator += (const VectorN<T, N>& rhs) {
386
387 for (uint32_t i = 0; i < N; i++) {
388 data[i] += rhs.data[i];
389 }
390 return *this;
391 }
392
393 template<class T, uint32_t N> inline
394 VectorN<T, N> VectorN<T, N>::operator -= (const VectorN<T, N>& rhs) {
395
396 for (uint32_t i = 0; i < N; i++) {
397 data[i] -= rhs.data[i];
398 }
399 return *this;
400 }
401
402 template<class T, uint32_t N> inline
403 VectorN<T, N> VectorN<T, N>::operator *= (const VectorN<T, N>& rhs) {
404
405 for (uint32_t i = 0; i < N; i++) {
406 data[i] *= rhs.data[i];
407 }
408 return *this;
409 }
410
411 template<class T, uint32_t N> inline
412 VectorN<T, N> VectorN<T, N>::operator /= (const VectorN<T, N>& rhs) {
413
414 for (uint32_t i = 0; i < N; i++) {
415 data[i] /= rhs.data[i];
416 }
417 return *this;
418 }
419
420 template<class T, uint32_t N> inline
421 bool VectorN<T, N>::operator == (const VectorN<T, N>& rhs) const {
422 for (uint32_t i = 0; i < N; i++) {
423 if (data[i] != rhs[i]) {
424 return false;
425 }
426 }
427 return true;
428 }
429
430 template <class T, uint32_t N>
431 template <typename NEW>
convert_to()432 VectorN<NEW, N> VectorN<T, N>::convert_to () const {
433 VectorN<NEW, N> result;
434
435 for (uint32_t i = 0; i < N; i++) {
436 result[i] = (NEW)(this->data[i]);
437 }
438 return result;
439 }
440
441 template <class T, uint32_t N> inline
zeros()442 void VectorN<T, N>::zeros () {
443 for (uint32_t i = 0; i < N; i++) {
444 data[i] = (T)(0);
445 }
446 }
447
448 template<class T, uint32_t N> inline
set(T x,T y)449 void VectorN<T, N>::set (T x, T y) {
450 if (N >= 2) {
451 data[0] = x;
452 data[1] = y;
453 }
454 }
455
456 template<class T, uint32_t N> inline
set(T x,T y,T z)457 void VectorN<T, N>::set (T x, T y, T z) {
458 if (N >= 3) {
459 data[0] = x;
460 data[1] = y;
461 data[2] = z;
462 }
463 }
464
465 template<class T, uint32_t N> inline
set(T x,T y,T z,T w)466 void VectorN<T, N>::set (T x, T y, T z, T w) {
467 if (N >= 4) {
468 data[0] - x;
469 data[1] = y;
470 data[2] = z;
471 data[3] = w;
472 }
473 }
474
475 template<class T, uint32_t N> inline
magnitude()476 T VectorN<T, N>::magnitude () const {
477 T result = 0;
478
479 for (uint32_t i = 0; i < N; i++) {
480 result += (data[i] * data[i]);
481 }
482 return (T) sqrtf(result);
483 }
484
485 template<class T, uint32_t N> inline
distance(const VectorN<T,N> & vec)486 float VectorN<T, N>::distance (const VectorN<T, N>& vec) const {
487 T result = 0;
488
489 for (uint32_t i = 0; i < N; i++) {
490 result += (vec.data[i] - data[i]) * (vec.data[i] - data[i]);
491 }
492 return sqrtf(result);
493 }
494
495 template<class T, uint32_t N> inline
dot(const VectorN<T,N> & vec)496 T VectorN<T, N>::dot (const VectorN<T, N>& vec) const {
497 T result = 0;
498
499 for (uint32_t i = 0; i < N; i++) {
500 result += (vec.data[i] * data[i]);
501 }
502 return result;
503 }
504
505 template<class T, uint32_t N> inline
lerp(T weight,const VectorN<T,N> & vec)506 VectorN<T, N> VectorN<T, N>::lerp (T weight, const VectorN<T, N>& vec) const {
507 return (*this) + (vec - (*this)) * weight;
508 }
509
510 // NxN matrix in row major order
511 template<class T, uint32_t N>
512 class MatrixN
513 {
514 public:
515 MatrixN ();
516 MatrixN (VectorN<T, 2> a, VectorN<T, 2> b);
517 MatrixN (VectorN<T, 3> a, VectorN<T, 3> b, VectorN<T, 3> c);
518 MatrixN (VectorN<T, 4> a, VectorN<T, 4> b, VectorN<T, 4> c, VectorN<T, 4> d);
519
520 inline void zeros ();
521 inline void eye ();
522
at(uint32_t row,uint32_t col)523 inline T& at (uint32_t row, uint32_t col) {
524 XCAM_ASSERT(row >= 0 && row < N);
525 XCAM_ASSERT(col >= 0 && col < N);
526
527 return data[row * N + col];
528 };
at(uint32_t row,uint32_t col)529 inline const T& at (uint32_t row, uint32_t col) const {
530 XCAM_ASSERT(row >= 0 && row < N);
531 XCAM_ASSERT(col >= 0 && col < N);
532
533 return data[row * N + col];
534 };
535
operator()536 inline T& operator () (uint32_t row, uint32_t col) {
537 return at (row, col);
538 };
operator()539 inline const T& operator () (uint32_t row, uint32_t col) const {
540 return at (row, col);
541 };
542
543 inline MatrixN<T, N>& operator = (const MatrixN<T, N>& rhs);
544 inline MatrixN<T, N> operator - () const;
545 inline MatrixN<T, N> operator + (const MatrixN<T, N>& rhs) const;
546 inline MatrixN<T, N> operator - (const MatrixN<T, N>& rhs) const;
547 inline MatrixN<T, N> operator * (const T a) const;
548 inline MatrixN<T, N> operator / (const T a) const;
549 inline VectorN<T, N> operator * (const VectorN<T, N>& rhs) const;
550 inline MatrixN<T, N> operator * (const MatrixN<T, N>& rhs) const;
551 inline MatrixN<T, N> transpose ();
552 inline MatrixN<T, N> inverse ();
553 inline T trace ();
554
555 private:
556 inline MatrixN<T, 2> inverse (const MatrixN<T, 2>& mat);
557 inline MatrixN<T, 3> inverse (const MatrixN<T, 3>& mat);
558 inline MatrixN<T, 4> inverse (const MatrixN<T, 4>& mat);
559
560 private:
561 T data[N * N];
562
563 };
564
565 // NxN matrix in row major order
566 template<class T, uint32_t N>
MatrixN()567 MatrixN<T, N>::MatrixN () {
568 eye ();
569 }
570
571 template<class T, uint32_t N>
MatrixN(VectorN<T,2> a,VectorN<T,2> b)572 MatrixN<T, N>::MatrixN (VectorN<T, 2> a, VectorN<T, 2> b) {
573 if (N == 2) {
574 data[0] = a[0];
575 data[1] = a[1];
576 data[2] = b[0];
577 data[3] = b[1];
578 } else {
579 eye ();
580 }
581 }
582
583 template<class T, uint32_t N>
MatrixN(VectorN<T,3> a,VectorN<T,3> b,VectorN<T,3> c)584 MatrixN<T, N>::MatrixN (VectorN<T, 3> a, VectorN<T, 3> b, VectorN<T, 3> c) {
585 if (N == 3) {
586 data[0] = a[0];
587 data[1] = a[1];
588 data[2] = a[2];
589 data[3] = b[0];
590 data[4] = b[1];
591 data[5] = b[2];
592 data[6] = c[0];
593 data[7] = c[1];
594 data[8] = c[2];
595 } else {
596 eye ();
597 }
598 }
599
600 template<class T, uint32_t N>
MatrixN(VectorN<T,4> a,VectorN<T,4> b,VectorN<T,4> c,VectorN<T,4> d)601 MatrixN<T, N>::MatrixN (VectorN<T, 4> a, VectorN<T, 4> b, VectorN<T, 4> c, VectorN<T, 4> d) {
602 if (N == 4) {
603 data[0] = a[0];
604 data[1] = a[1];
605 data[2] = a[2];
606 data[3] = a[3];
607 data[4] = b[0];
608 data[5] = b[1];
609 data[6] = b[2];
610 data[7] = b[3];
611 data[8] = c[0];
612 data[9] = c[1];
613 data[10] = c[2];
614 data[11] = c[3];
615 data[12] = d[0];
616 data[13] = d[1];
617 data[14] = d[2];
618 data[15] = d[3];
619 } else {
620 eye ();
621 }
622 }
623
624 template<class T, uint32_t N> inline
zeros()625 void MatrixN<T, N>::zeros () {
626 for (uint32_t i = 0; i < N * N; i++) {
627 data[i] = 0;
628 }
629 }
630
631 template<class T, uint32_t N> inline
eye()632 void MatrixN<T, N>::eye () {
633 zeros ();
634 for (uint32_t i = 0; i < N; i++) {
635 data[i * N + i] = 1;
636 }
637 }
638
639 template<class T, uint32_t N> inline
640 MatrixN<T, N>& MatrixN<T, N>::operator = (const MatrixN<T, N>& rhs) {
641 for (uint32_t i = 0; i < N * N; i++) {
642 data[i] = rhs.data[i];
643 }
644 return *this;
645 }
646
647 template<class T, uint32_t N> inline
648 MatrixN<T, N> MatrixN<T, N>::operator - () const {
649 MatrixN<T, N> result;
650 for (uint32_t i = 0; i < N * N; i++) {
651 result.data[i] = -data[i];
652 }
653 return result;
654 }
655
656 template<class T, uint32_t N> inline
657 MatrixN<T, N> MatrixN<T, N>::operator + (const MatrixN<T, N>& rhs) const {
658 MatrixN<T, N> result;
659 for (uint32_t i = 0; i < N * N; i++) {
660 result.data[i] = data[i] + rhs.data[i];
661 }
662 return result;
663 }
664
665 template<class T, uint32_t N> inline
666 MatrixN<T, N> MatrixN<T, N>::operator - (const MatrixN<T, N>& rhs) const {
667 MatrixN<T, N> result;
668 for (uint32_t i = 0; i < N * N; i++) {
669 result.data[i] = data[i] - rhs.data[i];
670 }
671 return result;
672 }
673
674 template<class T, uint32_t N> inline
675 MatrixN<T, N> MatrixN<T, N>::operator * (const T a) const {
676 MatrixN<T, N> result;
677 for (uint32_t i = 0; i < N * N; i++) {
678 result.data[i] = data[i] * a;
679 }
680 return result;
681 }
682
683 template<class T, uint32_t N> inline
684 MatrixN<T, N> MatrixN<T, N>::operator / (const T a) const {
685 MatrixN<T, N> result;
686 for (uint32_t i = 0; i < N * N; i++) {
687 result.data[i] = data[i] / a;
688 }
689 return result;
690 }
691
692 template<class T, uint32_t N> inline
693 MatrixN<T, N> MatrixN<T, N>::operator * (const MatrixN<T, N>& rhs) const {
694 MatrixN<T, N> result;
695 result.zeros ();
696
697 for (uint32_t i = 0; i < N; i++) {
698 for (uint32_t j = 0; j < N; j++) {
699 T element = 0;
700 for (uint32_t k = 0; k < N; k++) {
701 element += at(i, k) * rhs(k, j);
702 }
703 result(i, j) = element;
704 }
705 }
706 return result;
707 }
708
709 template<class T, uint32_t N> inline
710 VectorN<T, N> MatrixN<T, N>::operator * (const VectorN<T, N>& rhs) const {
711 VectorN<T, N> result;
712 for (uint32_t i = 0; i < N; i++) { // row
713 for (uint32_t j = 0; j < N; j++) { // col
714 result.data[i] = data[i * N + j] * rhs.data[j];
715 }
716 }
717 return result;
718 }
719
720 template<class T, uint32_t N> inline
transpose()721 MatrixN<T, N> MatrixN<T, N>::transpose () {
722 MatrixN<T, N> result;
723 for (uint32_t i = 0; i < N; i++) {
724 for (uint32_t j = 0; j <= N; j++) {
725 result.data[i * N + j] = data[j * N + i];
726 }
727 }
728 return result;
729 }
730
731 // if the matrix is non-invertible, return identity matrix
732 template<class T, uint32_t N> inline
inverse()733 MatrixN<T, N> MatrixN<T, N>::inverse () {
734 MatrixN<T, N> result;
735
736 result = inverse (*this);
737 return result;
738 }
739
740 template<class T, uint32_t N> inline
trace()741 T MatrixN<T, N>::trace () {
742 T t = 0;
743 for ( uint32_t i = 0; i < N; i++ ) {
744 t += data(i, i);
745 }
746 return t;
747 }
748
749 template<class T, uint32_t N> inline
inverse(const MatrixN<T,2> & mat)750 MatrixN<T, 2> MatrixN<T, N>::inverse (const MatrixN<T, 2>& mat)
751 {
752 MatrixN<T, 2> result;
753
754 T det = mat(0, 0) * mat(1, 1) - mat(0, 1) * mat(1, 0);
755
756 if (det == (T)0) {
757 return result;
758 }
759
760 result(0, 0) = mat(1, 1);
761 result(0, 1) = -mat(0, 1);
762 result(1, 0) = -mat(1, 0);
763 result(1, 1) = mat(0, 0);
764
765 return result * (1.0f / det);
766 }
767
768 template<class T, uint32_t N> inline
inverse(const MatrixN<T,3> & mat)769 MatrixN<T, 3> MatrixN<T, N>::inverse (const MatrixN<T, 3>& mat)
770 {
771 MatrixN<T, 3> result;
772
773 T det = mat(0, 0) * mat(1, 1) * mat(2, 2) +
774 mat(1, 0) * mat(2, 1) * mat(0, 2) +
775 mat(2, 0) * mat(0, 1) * mat(1, 2) -
776 mat(0, 0) * mat(2, 1) * mat(1, 2) -
777 mat(1, 0) * mat(0, 1) * mat(2, 2) -
778 mat(2, 0) * mat(1, 1) * mat(0, 2);
779
780 if (det == (T)0) {
781 return result;
782 }
783
784 result(0, 0) = mat(1, 1) * mat(2, 2) - mat(1, 2) * mat(2, 1);
785 result(1, 0) = mat(1, 2) * mat(2, 0) - mat(1, 0) * mat(2, 2);
786 result(2, 0) = mat(1, 0) * mat(2, 1) - mat(1, 1) * mat(2, 0);
787 result(0, 1) = mat(0, 2) * mat(2, 1) - mat(0, 1) * mat(2, 2);
788 result(1, 1) = mat(0, 0) * mat(2, 2) - mat(0, 2) * mat(2, 0);
789 result(2, 1) = mat(0, 1) * mat(2, 0) - mat(0, 0) * mat(2, 1);
790 result(0, 2) = mat(0, 1) * mat(1, 2) - mat(0, 2) * mat(1, 1);
791 result(1, 2) = mat(0, 2) * mat(1, 0) - mat(0, 0) * mat(1, 2);
792 result(2, 2) = mat(0, 0) * mat(1, 1) - mat(0, 1) * mat(1, 0);
793
794 return result * (1.0f / det);
795 }
796
797 template<class T, uint32_t N> inline
inverse(const MatrixN<T,4> & mat)798 MatrixN<T, 4> MatrixN<T, N>::inverse (const MatrixN<T, 4>& mat)
799 {
800 MatrixN<T, 4> result;
801
802 T det = mat(0, 3) * mat(1, 2) * mat(2, 1) * mat(3, 1) -
803 mat(0, 2) * mat(1, 3) * mat(2, 1) * mat(3, 1) -
804 mat(0, 3) * mat(1, 1) * mat(2, 2) * mat(3, 1) +
805 mat(0, 1) * mat(1, 3) * mat(2, 2) * mat(3, 1) +
806 mat(0, 2) * mat(1, 1) * mat(2, 3) * mat(3, 1) -
807 mat(0, 1) * mat(1, 2) * mat(2, 3) * mat(3, 1) -
808 mat(0, 3) * mat(1, 2) * mat(2, 0) * mat(3, 1) +
809 mat(0, 2) * mat(1, 3) * mat(2, 0) * mat(3, 1) +
810 mat(0, 3) * mat(1, 0) * mat(2, 2) * mat(3, 1) -
811 mat(0, 0) * mat(1, 3) * mat(2, 2) * mat(3, 1) -
812 mat(0, 2) * mat(1, 0) * mat(2, 3) * mat(3, 1) +
813 mat(0, 0) * mat(1, 2) * mat(2, 3) * mat(3, 1) +
814 mat(0, 3) * mat(1, 1) * mat(2, 0) * mat(3, 2) -
815 mat(0, 1) * mat(1, 3) * mat(2, 0) * mat(3, 2) -
816 mat(0, 3) * mat(1, 0) * mat(2, 1) * mat(3, 2) +
817 mat(0, 0) * mat(1, 3) * mat(2, 1) * mat(3, 2) +
818 mat(0, 1) * mat(1, 0) * mat(2, 3) * mat(3, 2) -
819 mat(0, 0) * mat(1, 1) * mat(2, 3) * mat(3, 2) -
820 mat(0, 2) * mat(1, 1) * mat(2, 0) * mat(3, 3) +
821 mat(0, 1) * mat(1, 2) * mat(2, 0) * mat(3, 3) +
822 mat(0, 2) * mat(1, 0) * mat(2, 1) * mat(3, 3) -
823 mat(0, 0) * mat(1, 2) * mat(2, 1) * mat(3, 3) -
824 mat(0, 1) * mat(1, 0) * mat(2, 2) * mat(3, 3) +
825 mat(0, 0) * mat(1, 1) * mat(2, 2) * mat(3, 3);
826
827 if (det == (T)0) {
828 return result;
829 }
830
831 result(0, 0) = mat(1, 2) * mat(2, 3) * mat(3, 1) -
832 mat(1, 3) * mat(2, 2) * mat(3, 1) +
833 mat(1, 3) * mat(2, 1) * mat(3, 2) -
834 mat(1, 1) * mat(2, 3) * mat(3, 2) -
835 mat(1, 2) * mat(2, 1) * mat(3, 3) +
836 mat(1, 1) * mat(2, 2) * mat(3, 3);
837
838 result(0, 1) = mat(0, 3) * mat(2, 2) * mat(3, 1) -
839 mat(0, 2) * mat(2, 3) * mat(3, 1) -
840 mat(0, 3) * mat(2, 1) * mat(3, 2) +
841 mat(0, 1) * mat(2, 3) * mat(3, 2) +
842 mat(0, 2) * mat(2, 1) * mat(3, 3) -
843 mat(0, 1) * mat(2, 2) * mat(3, 3);
844
845 result(0, 2) = mat(0, 2) * mat(1, 3) * mat(3, 1) -
846 mat(0, 3) * mat(1, 2) * mat(3, 1) +
847 mat(0, 3) * mat(1, 1) * mat(3, 2) -
848 mat(0, 1) * mat(1, 3) * mat(3, 2) -
849 mat(0, 2) * mat(1, 1) * mat(3, 3) +
850 mat(0, 1) * mat(1, 2) * mat(3, 3);
851
852 result(0, 3) = mat(0, 3) * mat(1, 2) * mat(2, 1) -
853 mat(0, 2) * mat(1, 3) * mat(2, 1) -
854 mat(0, 3) * mat(1, 1) * mat(2, 2) +
855 mat(0, 1) * mat(1, 3) * mat(2, 2) +
856 mat(0, 2) * mat(1, 1) * mat(2, 3) -
857 mat(0, 1) * mat(1, 2) * mat(2, 3);
858
859 result(1, 0) = mat(1, 3) * mat(2, 2) * mat(3, 0) -
860 mat(1, 2) * mat(2, 3) * mat(3, 0) -
861 mat(1, 3) * mat(2, 0) * mat(3, 2) +
862 mat(1, 0) * mat(2, 3) * mat(3, 2) +
863 mat(1, 2) * mat(2, 0) * mat(3, 3) -
864 mat(1, 0) * mat(2, 2) * mat(3, 3);
865
866 result(1, 1) = mat(0, 2) * mat(2, 3) * mat(3, 0) -
867 mat(0, 3) * mat(2, 2) * mat(3, 0) +
868 mat(0, 3) * mat(2, 0) * mat(3, 2) -
869 mat(0, 0) * mat(2, 3) * mat(3, 2) -
870 mat(0, 2) * mat(2, 0) * mat(3, 3) +
871 mat(0, 0) * mat(2, 2) * mat(3, 3);
872
873 result(1, 2) = mat(0, 3) * mat(1, 2) * mat(3, 0) -
874 mat(0, 2) * mat(1, 3) * mat(3, 0) -
875 mat(0, 3) * mat(1, 0) * mat(3, 2) +
876 mat(0, 0) * mat(1, 3) * mat(3, 2) +
877 mat(0, 2) * mat(1, 0) * mat(3, 3) -
878 mat(0, 0) * mat(1, 2) * mat(3, 3);
879
880 result(1, 3) = mat(0, 2) * mat(1, 3) * mat(2, 0) -
881 mat(0, 3) * mat(1, 2) * mat(2, 0) +
882 mat(0, 3) * mat(1, 0) * mat(2, 2) -
883 mat(0, 0) * mat(1, 3) * mat(2, 2) -
884 mat(0, 2) * mat(1, 0) * mat(2, 3) +
885 mat(0, 0) * mat(1, 2) * mat(2, 3);
886
887 result(2, 0) = mat(1, 1) * mat(2, 3) * mat(3, 0) -
888 mat(1, 3) * mat(2, 1) * mat(3, 0) +
889 mat(1, 3) * mat(2, 0) * mat(3, 1) -
890 mat(1, 0) * mat(2, 3) * mat(3, 1) -
891 mat(1, 1) * mat(2, 0) * mat(3, 3) +
892 mat(1, 0) * mat(2, 1) * mat(3, 3);
893
894 result(2, 1) = mat(0, 3) * mat(2, 1) * mat(3, 0) -
895 mat(0, 1) * mat(2, 3) * mat(3, 0) -
896 mat(0, 3) * mat(2, 0) * mat(3, 1) +
897 mat(0, 0) * mat(2, 3) * mat(3, 1) +
898 mat(0, 1) * mat(2, 0) * mat(3, 3) -
899 mat(0, 0) * mat(2, 1) * mat(3, 3);
900
901 result(2, 2) = mat(0, 1) * mat(1, 3) * mat(3, 0) -
902 mat(0, 3) * mat(1, 1) * mat(3, 0) +
903 mat(0, 3) * mat(1, 0) * mat(3, 1) -
904 mat(0, 0) * mat(1, 3) * mat(3, 1) -
905 mat(0, 1) * mat(1, 0) * mat(3, 3) +
906 mat(0, 0) * mat(1, 1) * mat(3, 3);
907
908 result(2, 3) = mat(0, 3) * mat(1, 1) * mat(2, 0) -
909 mat(0, 1) * mat(1, 3) * mat(2, 0) -
910 mat(0, 3) * mat(1, 0) * mat(2, 1) +
911 mat(0, 0) * mat(1, 3) * mat(2, 1) +
912 mat(0, 1) * mat(1, 0) * mat(2, 3) -
913 mat(0, 0) * mat(1, 1) * mat(2, 3);
914
915 result(3, 0) = mat(1, 2) * mat(2, 1) * mat(3, 0) -
916 mat(1, 1) * mat(2, 2) * mat(3, 0) -
917 mat(1, 2) * mat(2, 0) * mat(3, 1) +
918 mat(1, 0) * mat(2, 2) * mat(3, 1) +
919 mat(1, 1) * mat(2, 0) * mat(3, 2) -
920 mat(1, 0) * mat(2, 1) * mat(3, 2);
921
922 result(3, 1) = mat(1, 1) * mat(2, 2) * mat(3, 0) -
923 mat(1, 2) * mat(2, 1) * mat(3, 0) +
924 mat(1, 2) * mat(2, 0) * mat(3, 1) -
925 mat(1, 0) * mat(2, 2) * mat(3, 1) -
926 mat(1, 1) * mat(2, 0) * mat(3, 2) +
927 mat(1, 0) * mat(2, 1) * mat(3, 2);
928
929 result(3, 2) = mat(0, 2) * mat(1, 1) * mat(3, 0) -
930 mat(0, 1) * mat(1, 2) * mat(3, 0) -
931 mat(0, 2) * mat(1, 0) * mat(3, 1) +
932 mat(0, 0) * mat(1, 2) * mat(3, 1) +
933 mat(0, 1) * mat(1, 0) * mat(3, 2) -
934 mat(0, 0) * mat(1, 1) * mat(3, 2);
935
936 result(3, 3) = mat(0, 1) * mat(1, 2) * mat(2, 0) -
937 mat(0, 2) * mat(1, 1) * mat(2, 0) +
938 mat(0, 2) * mat(1, 0) * mat(2, 1) -
939 mat(0, 0) * mat(1, 2) * mat(2, 1) -
940 mat(0, 1) * mat(1, 0) * mat(2, 2) +
941 mat(0, 0) * mat(1, 1) * mat(2, 2);
942
943 return result * (1.0f / det);
944 }
945
946 typedef VectorN<double, 2> Vec2d;
947 typedef VectorN<double, 3> Vec3d;
948 typedef VectorN<double, 4> Vec4d;
949 typedef MatrixN<double, 2> Mat2d;
950 typedef MatrixN<double, 3> Mat3d;
951 typedef MatrixN<double, 4> Mat4d;
952
953 typedef VectorN<float, 2> Vec2f;
954 typedef VectorN<float, 3> Vec3f;
955 typedef VectorN<float, 4> Vec4f;
956 typedef MatrixN<float, 3> Mat3f;
957 typedef MatrixN<float, 4> Mat4f;
958
959 template<class T>
960 class Quaternion
961 {
962 public:
963
964 Vec3d v;
965 T w;
966
Quaternion()967 Quaternion () : v(0, 0, 0), w(0) {};
Quaternion(const Quaternion<T> & q)968 Quaternion (const Quaternion<T>& q) : v(q.v), w(q.w) {};
969
Quaternion(const Vec3d & vec,T _w)970 Quaternion (const Vec3d& vec, T _w) : v(vec), w(_w) {};
Quaternion(const Vec4d & vec)971 Quaternion (const Vec4d& vec) : v(vec[0], vec[1], vec[2]), w(vec[3]) {};
Quaternion(T _x,T _y,T _z,T _w)972 Quaternion (T _x, T _y, T _z, T _w) : v(_x, _y, _z), w(_w) {};
973
reset()974 inline void reset () {
975 v.zeros();
976 w = (T) 0;
977 }
978
979 inline Quaternion<T>& operator = (const Quaternion<T>& rhs) {
980 v = rhs.v;
981 w = rhs.w;
982 return *this;
983 }
984
985 inline Quaternion<T> operator + (const Quaternion<T>& rhs) const {
986 const Quaternion<T>& lhs = *this;
987 return Quaternion<T>(lhs.v + rhs.v, lhs.w + rhs.w);
988 }
989
990 inline Quaternion<T> operator - (const Quaternion<T>& rhs) const {
991 const Quaternion<T>& lhs = *this;
992 return Quaternion<T>(lhs.v - rhs.v, lhs.w - rhs.w);
993 }
994
995 inline Quaternion<T> operator * (T rhs) const {
996 return Quaternion<T>(v * rhs, w * rhs);
997 }
998
999 inline Quaternion<T> operator * (const Quaternion<T>& rhs) const {
1000 const Quaternion<T>& lhs = *this;
1001 return Quaternion<T>(lhs.w * rhs.v[0] + lhs.v[0] * rhs.w + lhs.v[1] * rhs.v[2] - lhs.v[2] * rhs.v[1],
1002 lhs.w * rhs.v[1] - lhs.v[0] * rhs.v[2] + lhs.v[1] * rhs.w + lhs.v[2] * rhs.v[0],
1003 lhs.w * rhs.v[2] + lhs.v[0] * rhs.v[1] - lhs.v[1] * rhs.v[0] + lhs.v[2] * rhs.w,
1004 lhs.w * rhs.w - lhs.v[0] * rhs.v[0] - lhs.v[1] * rhs.v[1] - lhs.v[2] * rhs.v[2]);
1005 }
1006
1007 /*
1008 --------
1009 / --
1010 |Qr| = \/ Qr.Qr
1011 */
magnitude()1012 inline T magnitude () const {
1013 return (T) sqrtf(w * w + v[0] * v[0] + v[1] * v[1] + v[2] * v[2]);
1014 }
1015
normalize()1016 inline void normalize ()
1017 {
1018 T length = magnitude ();
1019 w = w / length;
1020 v = v / length;
1021 }
1022
conjugate(const Quaternion<T> & quat)1023 inline Quaternion<T> conjugate (const Quaternion<T>& quat) const {
1024 return Quaternion<T>(-quat.v, quat.w);
1025 }
1026
inverse(const Quaternion<T> & quat)1027 inline Quaternion<T> inverse (const Quaternion<T>& quat) const {
1028 return conjugate(quat) * ( 1.0f / magnitude(quat));
1029 }
1030
lerp(T weight,const Quaternion<T> & quat)1031 inline Quaternion<T> lerp (T weight, const Quaternion<T>& quat) const {
1032 return Quaternion<T>(v.lerp(weight, quat.v), (1 - weight) * w + weight * quat.w);
1033 }
1034
slerp(T r,const Quaternion<T> & quat)1035 inline Quaternion<T> slerp(T r, const Quaternion<T>& quat) const {
1036 Quaternion<T> ret;
1037 T cos_theta = w * quat.w + v[0] * quat.v[0] + v[1] * quat.v[1] + v[2] * quat.v[2];
1038 T theta = (T) acos(cos_theta);
1039 if (fabs(theta) < FLT_EPSILON)
1040 {
1041 ret = *this;
1042 }
1043 else
1044 {
1045 T sin_theta = (T) sqrt(1.0 - cos_theta * cos_theta);
1046 if (fabs(sin_theta) < FLT_EPSILON)
1047 {
1048 ret.w = 0.5 * w + 0.5 * quat.w;
1049 ret.v = v.lerp(0.5, quat.v);
1050 }
1051 else
1052 {
1053 T r0 = (T) sin((1.0 - r) * theta) / sin_theta;
1054 T r1 = (T) sin(r * theta) / sin_theta;
1055
1056 ret.w = w * r0 + quat.w * r1;
1057 ret.v[0] = v[0] * r0 + quat.v[0] * r1;
1058 ret.v[1] = v[1] * r0 + quat.v[1] * r1;
1059 ret.v[2] = v[2] * r0 + quat.v[2] * r1;
1060 }
1061 }
1062 return ret;
1063 }
1064
create_quaternion(Vec3d axis,T angle_rad)1065 static Quaternion<T> create_quaternion (Vec3d axis, T angle_rad) {
1066 T theta_over_two = angle_rad / (T) 2.0;
1067 T sin_theta_over_two = std::sin(theta_over_two);
1068 T cos_theta_over_two = std::cos(theta_over_two);
1069 return Quaternion<T>(axis * sin_theta_over_two, cos_theta_over_two);
1070 }
1071
create_quaternion(Vec3d euler)1072 static Quaternion<T> create_quaternion (Vec3d euler) {
1073 return create_quaternion(Vec3d(1, 0, 0), euler[0]) *
1074 create_quaternion(Vec3d(0, 1, 0), euler[1]) *
1075 create_quaternion(Vec3d(0, 0, 1), euler[2]);
1076 }
1077
create_quaternion(const Mat3d & mat)1078 static Quaternion<T> create_quaternion (const Mat3d& mat) {
1079 Quaternion<T> q;
1080
1081 T trace, s;
1082 T diag1 = mat(0, 0);
1083 T diag2 = mat(1, 1);
1084 T diag3 = mat(2, 2);
1085
1086 trace = diag1 + diag2 + diag3;
1087
1088 if (trace >= FLT_EPSILON)
1089 {
1090 s = 2.0 * (T) sqrt(trace + 1.0);
1091 q.w = 0.25 * s;
1092 q.v[0] = (mat(2, 1) - mat(1, 2)) / s;
1093 q.v[1] = (mat(0, 2) - mat(2, 0)) / s;
1094 q.v[2] = (mat(1, 0) - mat(0, 1)) / s;
1095 }
1096 else
1097 {
1098 char max_diag = (diag1 > diag2) ? ((diag1 > diag3) ? 1 : 3) : ((diag2 > diag3) ? 2 : 3);
1099
1100 if (max_diag == 1)
1101 {
1102 s = 2.0 * (T) sqrt(1.0 + mat(0, 0) - mat(1, 1) - mat(2, 2));
1103 q.w = (mat(2, 1) - mat(1, 2)) / s;
1104 q.v[0] = 0.25 * s;
1105 q.v[1] = (mat(0, 1) + mat(1, 0)) / s;
1106 q.v[2] = (mat(0, 2) + mat(2, 0)) / s;
1107 }
1108 else if (max_diag == 2)
1109 {
1110 s = 2.0 * (T) sqrt(1.0 + mat(1, 1) - mat(0, 0) - mat(2, 2));
1111 q.w = (mat(0, 2) - mat(2, 0)) / s;
1112 q.v[0] = (mat(0, 1) + mat(1, 0)) / s;
1113 q.v[1] = 0.25 * s;
1114 q.v[2] = (mat(1, 2) + mat(2, 1)) / s;
1115 }
1116 else
1117 {
1118 s = 2.0 * (T) sqrt(1.0 + mat(2, 2) - mat(0, 0) - mat(1, 1));
1119 q.w = (mat(1, 0) - mat(0, 1)) / s;
1120 q.v[0] = (mat(0, 2) + mat(2, 0)) / s;
1121 q.v[1] = (mat(1, 2) + mat(2, 1)) / s;
1122 q.v[2] = 0.25 * s;
1123 }
1124 }
1125
1126 return q;
1127 }
1128
rotation_axis()1129 inline Vec4d rotation_axis () {
1130 Vec4d rot_axis;
1131
1132 T cos_theta_over_two = w;
1133 rot_axis[4] = (T) std::acos( cos_theta_over_two ) * 2.0f;
1134
1135 T sin_theta_over_two = (T) sqrt( 1.0 - cos_theta_over_two * cos_theta_over_two );
1136 if ( fabs( sin_theta_over_two ) < 0.0005 ) sin_theta_over_two = 1;
1137 rot_axis[0] = v[0] / sin_theta_over_two;
1138 rot_axis[1] = v[1] / sin_theta_over_two;
1139 rot_axis[2] = v[2] / sin_theta_over_two;
1140
1141 return rot_axis;
1142 }
1143
1144 /*
1145 psi=atan2(2.*(Q(:,1).*Q(:,4)-Q(:,2).*Q(:,3)),(Q(:,4).^2-Q(:,1).^2-Q(:,2).^2+Q(:,3).^2));
1146 theta=asin(2.*(Q(:,1).*Q(:,3)+Q(:,2).*Q(:,4)));
1147 phi=atan2(2.*(Q(:,3).*Q(:,4)-Q(:,1).*Q(:,2)),(Q(:,4).^2+Q(:,1).^2-Q(:,2).^2-Q(:,3).^2));
1148 */
euler_angles()1149 inline Vec3d euler_angles () {
1150 Vec3d euler;
1151
1152 // atan2(2*(qx*qw-qy*qz) , qw2-qx2-qy2+qz2)
1153 euler[0] = atan2(2 * (v[0] * w - v[1] * v[2]),
1154 w * w - v[0] * v[0] - v[1] * v[1] + v[2] * v[2]);
1155
1156 // asin(2*(qx*qz + qy*qw)
1157 euler[1] = asin(2 * (v[0] * v[2] + v[1] * w));
1158
1159 // atan2(2*(qz*qw- qx*qy) , qw2 + qx2 - qy2 - qz2)
1160 euler[2] = atan2(2 * (v[2] * w - v[0] * v[1]),
1161 w * w + v[0] * v[0] - v[1] * v[1] - v[2] * v[2]);
1162
1163 return euler;
1164 }
1165
rotation_matrix()1166 inline Mat3d rotation_matrix () {
1167 Mat3d mat;
1168
1169 T xx = v[0] * v[0];
1170 T xy = v[0] * v[1];
1171 T xz = v[0] * v[2];
1172 T xw = v[0] * w;
1173
1174 T yy = v[1] * v[1];
1175 T yz = v[1] * v[2];
1176 T yw = v[1] * w;
1177
1178 T zz = v[2] * v[2];
1179 T zw = v[2] * w;
1180
1181 mat(0, 0) = 1 - 2 * (yy + zz);
1182 mat(0, 1) = 2 * (xy - zw);
1183 mat(0, 2) = 2 * (xz + yw);
1184 mat(1, 0) = 2 * (xy + zw);
1185 mat(1, 1) = 1 - 2 * (xx + zz);
1186 mat(1, 2) = 2 * (yz - xw);
1187 mat(2, 0) = 2 * (xz - yw);
1188 mat(2, 1) = 2 * (yz + xw);
1189 mat(2, 2) = 1 - 2 * (xx + yy);
1190
1191 return mat;
1192 }
1193 };
1194
1195
1196 typedef Quaternion<double> Quaternd;
1197
1198 }
1199
1200 #endif //XCAM_VECTOR_MATRIX_H
1201