58 lines
1.5 KiB
C++
58 lines
1.5 KiB
C++
//
|
|
// Created by 顾涵彬 on 2019-08-30.
|
|
//
|
|
|
|
#ifndef MATRIX_QUATERNION_H
|
|
#define MATRIX_QUATERNION_H
|
|
|
|
#include "SquareMatrix.h"
|
|
#include <cmath>
|
|
namespace Ctain {
|
|
// using SMatrix<double>;
|
|
template<typename T>
|
|
class Quaternion {
|
|
public:
|
|
Quaternion(){}
|
|
Quaternion(SMatrix<double> m){
|
|
//double f = 1e-10;
|
|
_w = sqrt(m(0)+m(4)+m(8)+1)/2;
|
|
_x = (m(5)-m(7))/(4*_w);
|
|
_y = (m(6)-m(2))/(4*_w);
|
|
_z = (m(1)-m(3))/(4*_w);
|
|
}
|
|
Quaternion(T X,T Y,T Z,T W) : _x(X),_y(Y),_z(Z),_w(W){}
|
|
void normalize() {
|
|
double len;
|
|
len = sqrt(_x*_x+_y*_y+_z*_z+_w*_w);
|
|
_x = _x / len;
|
|
_y = _y / len;
|
|
_z = _z / len;
|
|
_w = _w / len;
|
|
}
|
|
T x(){return _x;}
|
|
T y(){return _y;}
|
|
T z(){return _z;}
|
|
T w(){return _w;}
|
|
SMatrix<double> toRotationMatrix() const {
|
|
SMatrix<double> r(3);
|
|
double q0=_w,q1=_x,q2=_y,q3=_z;
|
|
r(0) = 1 - 2*q2*q2-2*q3*q3;
|
|
r(1) = 2*q1*q2+2*q0*q3;
|
|
r(2) = 2*q1*q3-2*q0*q2;
|
|
r(3) = 2*q1*q2 - 2*q0*q3;
|
|
r(4) = 1-2*q1*q1 - 2*q3*q3;
|
|
r(5) = 2*q2*q3 +2*q0*q1;
|
|
r(6) = 2*q1*q3+2*q0*q2;
|
|
r(7) = 2*q2*q3 - 2*q0*q1;
|
|
r(8) = 1-2*q1*q1 -2*q2*q2;
|
|
return r;
|
|
}
|
|
private:
|
|
T _x;
|
|
T _y;
|
|
T _z;
|
|
T _w;
|
|
};
|
|
}
|
|
#endif //MATRIX_QUATERNION_H
|