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 28namespace 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 66template<class T> 67class Vector2 68{ 69public: 70 71 T x; 72 T y; 73 74 Vector2 () : x(0), y(0) {}; 75 Vector2 (T _x, T _y) : x(_x), y(_y) {}; 76 77 template <typename New> 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 115 void reset () { 116 this->x = (T) 0; 117 this->y = (T) 0; 118 } 119 120 void set (T _x, T _y) { 121 this->x = _x; 122 this->y = _y; 123 } 124 125 T magnitude () const { 126 return (T) sqrtf(x * x + y * y); 127 } 128 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 133 T dot (const Vector2<T>& vec) const { 134 return (x * vec.x + y * vec.y); 135 } 136 137 inline Vector2<T> lerp (T weight, const Vector2<T>& vec) const { 138 return (*this) + (vec - (*this)) * weight; 139 } 140 141}; 142 143template<class T, uint32_t N> 144class VectorN 145{ 146public: 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 198private: 199 T data[N]; 200 201}; 202 203 204template<class T, uint32_t N> inline 205VectorN<T, N>::VectorN () 206{ 207 for (uint32_t i = 0; i < N; i++) { 208 data[i] = 0; 209 } 210} 211 212template<class T, uint32_t N> inline 213VectorN<T, N>::VectorN (T x) { 214 data[0] = x; 215} 216 217template<class T, uint32_t N> inline 218VectorN<T, N>::VectorN (T x, T y) { 219 if (N >= 2) { 220 data[0] = x; 221 data[1] = y; 222 } 223} 224 225template<class T, uint32_t N> inline 226VectorN<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 234template<class T, uint32_t N> inline 235VectorN<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 244template<class T, uint32_t N> inline 245VectorN<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 254template<class T, uint32_t N> inline 255VectorN<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 263template<class T, uint32_t N> inline 264VectorN<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 272template<class T, uint32_t N> inline 273VectorN<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 282template<class T, uint32_t N> inline 283VectorN<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 292template<class T, uint32_t N> inline 293VectorN<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 302template<class T, uint32_t N> inline 303VectorN<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 312template<class T, uint32_t N> inline 313VectorN<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 320template<class T, uint32_t N> inline 321VectorN<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 328template<class T, uint32_t N> inline 329VectorN<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 336template<class T, uint32_t N> inline 337VectorN<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 344template<class T, uint32_t N> inline 345VectorN<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 354template<class T, uint32_t N> inline 355VectorN<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 364template<class T, uint32_t N> inline 365VectorN<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 374template<class T, uint32_t N> inline 375VectorN<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 384template<class T, uint32_t N> inline 385VectorN<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 393template<class T, uint32_t N> inline 394VectorN<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 402template<class T, uint32_t N> inline 403VectorN<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 411template<class T, uint32_t N> inline 412VectorN<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 420template<class T, uint32_t N> inline 421bool 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 430template <class T, uint32_t N> 431template <typename NEW> 432VectorN<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 441template <class T, uint32_t N> inline 442void VectorN<T, N>::zeros () { 443 for (uint32_t i = 0; i < N; i++) { 444 data[i] = (T)(0); 445 } 446} 447 448template<class T, uint32_t N> inline 449void VectorN<T, N>::set (T x, T y) { 450 if (N >= 2) { 451 data[0] = x; 452 data[1] = y; 453 } 454} 455 456template<class T, uint32_t N> inline 457void 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 465template<class T, uint32_t N> inline 466void 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 475template<class T, uint32_t N> inline 476T 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 485template<class T, uint32_t N> inline 486float 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 495template<class T, uint32_t N> inline 496T 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 505template<class T, uint32_t N> inline 506VectorN<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 511template<class T, uint32_t N> 512class MatrixN 513{ 514public: 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 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 }; 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 536 inline T& operator () (uint32_t row, uint32_t col) { 537 return at (row, col); 538 }; 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 555private: 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 560private: 561 T data[N * N]; 562 563}; 564 565// NxN matrix in row major order 566template<class T, uint32_t N> 567MatrixN<T, N>::MatrixN () { 568 eye (); 569} 570 571template<class T, uint32_t N> 572MatrixN<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 583template<class T, uint32_t N> 584MatrixN<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 600template<class T, uint32_t N> 601MatrixN<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 624template<class T, uint32_t N> inline 625void MatrixN<T, N>::zeros () { 626 for (uint32_t i = 0; i < N * N; i++) { 627 data[i] = 0; 628 } 629} 630 631template<class T, uint32_t N> inline 632void MatrixN<T, N>::eye () { 633 zeros (); 634 for (uint32_t i = 0; i < N; i++) { 635 data[i * N + i] = 1; 636 } 637} 638 639template<class T, uint32_t N> inline 640MatrixN<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 647template<class T, uint32_t N> inline 648MatrixN<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 656template<class T, uint32_t N> inline 657MatrixN<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 665template<class T, uint32_t N> inline 666MatrixN<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 674template<class T, uint32_t N> inline 675MatrixN<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 683template<class T, uint32_t N> inline 684MatrixN<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 692template<class T, uint32_t N> inline 693MatrixN<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 709template<class T, uint32_t N> inline 710VectorN<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 720template<class T, uint32_t N> inline 721MatrixN<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 732template<class T, uint32_t N> inline 733MatrixN<T, N> MatrixN<T, N>::inverse () { 734 MatrixN<T, N> result; 735 736 result = inverse (*this); 737 return result; 738} 739 740template<class T, uint32_t N> inline 741T 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 749template<class T, uint32_t N> inline 750MatrixN<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 768template<class T, uint32_t N> inline 769MatrixN<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 797template<class T, uint32_t N> inline 798MatrixN<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 946typedef VectorN<double, 2> Vec2d; 947typedef VectorN<double, 3> Vec3d; 948typedef VectorN<double, 4> Vec4d; 949typedef MatrixN<double, 2> Mat2d; 950typedef MatrixN<double, 3> Mat3d; 951typedef MatrixN<double, 4> Mat4d; 952 953typedef VectorN<float, 2> Vec2f; 954typedef VectorN<float, 3> Vec3f; 955typedef VectorN<float, 4> Vec4f; 956typedef MatrixN<float, 3> Mat3f; 957typedef MatrixN<float, 4> Mat4f; 958 959template<class T> 960class Quaternion 961{ 962public: 963 964 Vec3d v; 965 T w; 966 967 Quaternion () : v(0, 0, 0), w(0) {}; 968 Quaternion (const Quaternion<T>& q) : v(q.v), w(q.w) {}; 969 970 Quaternion (const Vec3d& vec, T _w) : v(vec), w(_w) {}; 971 Quaternion (const Vec4d& vec) : v(vec[0], vec[1], vec[2]), w(vec[3]) {}; 972 Quaternion (T _x, T _y, T _z, T _w) : v(_x, _y, _z), w(_w) {}; 973 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 */ 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 1016 inline void normalize () 1017 { 1018 T length = magnitude (); 1019 w = w / length; 1020 v = v / length; 1021 } 1022 1023 inline Quaternion<T> conjugate (const Quaternion<T>& quat) const { 1024 return Quaternion<T>(-quat.v, quat.w); 1025 } 1026 1027 inline Quaternion<T> inverse (const Quaternion<T>& quat) const { 1028 return conjugate(quat) * ( 1.0f / magnitude(quat)); 1029 } 1030 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 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 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 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 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 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 */ 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 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 1196typedef Quaternion<double> Quaternd; 1197 1198} 1199 1200#endif //XCAM_VECTOR_MATRIX_H 1201