| 665 | }; |
| 666 | |
| 667 | inline quaternion quatFromMat(const Vector &vx, const Vector &vy, const Vector &vz) { |
| 668 | // vx = first column of rot matrix |
| 669 | // vy = second column |
| 670 | // vz = third column |
| 671 | double xx = vx(0); |
| 672 | double yy = vy(1); |
| 673 | double zz = vz(2); |
| 674 | double tr = xx + yy + zz; |
| 675 | quaternion Q; |
| 676 | if ((tr > xx) && (tr > yy) && (tr > zz)) |
| 677 | { |
| 678 | double S = std::sqrt(tr + 1.0) * 2.0; |
| 679 | Q.w = 0.25 * S; |
| 680 | Q.x = (vz(1) - vy(2)) / S; |
| 681 | Q.y = (vx(2) - vz(0)) / S; |
| 682 | Q.z = (vy(0) - vx(1)) / S; |
| 683 | } |
| 684 | else if ((xx > yy) && (xx > zz)) |
| 685 | { |
| 686 | double S = std::sqrt(1.0 + xx - yy - zz) * 2.0; |
| 687 | Q.w = (vz(1) - vy(2)) / S; |
| 688 | Q.x = 0.25 * S; |
| 689 | Q.y = (vx(1) + vy(0)) / S; |
| 690 | Q.z = (vx(2) + vz(0)) / S; |
| 691 | } |
| 692 | else if (yy > zz) |
| 693 | { |
| 694 | double S = std::sqrt(1.0 + yy - xx - zz) * 2.0; |
| 695 | Q.w = (vx(2) - vz(0)) / S; |
| 696 | Q.x = (vx(1) + vy(0)) / S; |
| 697 | Q.y = 0.25 * S; |
| 698 | Q.z = (vy(2) + vz(1)) / S; |
| 699 | } |
| 700 | else |
| 701 | { |
| 702 | double S = std::sqrt(1.0 + zz - xx - yy) * 2.0; |
| 703 | Q.w = (vy(0) - vx(1)) / S; |
| 704 | Q.x = (vx(2) + vz(0)) / S; |
| 705 | Q.y = (vy(2) + vz(1)) / S; |
| 706 | Q.z = 0.25 * S; |
| 707 | } |
| 708 | // safe normalization |
| 709 | double squared_norm = Q.x*Q.x + Q.y*Q.y + Q.z*Q.z + Q.w*Q.w; |
| 710 | if (squared_norm > 0.0 && squared_norm != 1.0) { |
| 711 | double n = std::sqrt(squared_norm); |
| 712 | Q.x /= n; |
| 713 | Q.y /= n; |
| 714 | Q.z /= n; |
| 715 | Q.w /= n; |
| 716 | } |
| 717 | return Q; |
| 718 | } |
| 719 | } |
| 720 | |
| 721 | /* |