00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018 template <int N, typename Base>
00019 _RotationMatrix<N, Base>::_RotationMatrix(){
00020 _Matrix<N, N, Base> *m =this;
00021 *m=_Matrix<N, N, Base>::eye(1.0);
00022 }
00023
00024
00025 template <int N, typename Base>
00026 _RotationMatrix<N, Base>::_RotationMatrix(const _Matrix<N, N, Base> &m){
00027 _Matrix<N, N, Base>* t=this;
00028 *t=m;
00029 }
00030
00031 template <int N, typename Base>
00032 _RotationMatrix<N, Base>& _RotationMatrix<N, Base>::operator*=(const _RotationMatrix<N, Base>& m){
00033 _Matrix<N,N, Base>::operator*=(m);
00034 return *this;
00035 }
00036
00037 template <int N, typename Base>
00038 _RotationMatrix<N, Base> _RotationMatrix<N, Base>::operator*(const _RotationMatrix<N, Base>& m) const{
00039 _RotationMatrix t(*this);
00040 t*=m;
00041 return t;
00042 }
00043
00044 template <int N, typename Base>
00045 _Vector<N, Base> _RotationMatrix<N, Base>::operator*(const _Vector<N, Base>& v) const{
00046 return _Matrix<N,N,Base>::operator*(v);
00047 }
00048
00049 template <int N, typename Base>
00050 _RotationMatrix<N, Base> _RotationMatrix<N, Base>::inverse() const{
00051 _RotationMatrix t=(*this);
00052 t.transposeInPlace();
00053 return t;
00054 }
00055
00056
00057 template <typename Base>
00058 _RotationMatrix2<Base>::_RotationMatrix2(Base angle){
00059 Base c=cos(angle), s=sin(angle);
00060 this->_allocator[0][0] = c;
00061 this->_allocator[0][1] = -s;
00062 this->_allocator[1][0] = s;
00063 this->_allocator[1][1] = c;
00064 }
00065
00066 template <typename Base>
00067 _RotationMatrix2<Base>::_RotationMatrix2(const _Vector<1,Base>& a) {
00068 Base c=cos(a[0]), s=sin(a[0]);
00069 this->_allocator[0][0] = c;
00070 this->_allocator[0][1] = -s;
00071 this->_allocator[1][0] = s;
00072 this->_allocator[1][1] = c;
00073 }
00074
00075 template <typename Base>
00076 Base _RotationMatrix2<Base>::angle() const{
00077 return atan2(this->_allocator[1][0], this->_allocator[0][0]);
00078 }
00079
00080
00081 template <typename Base>
00082 _RotationMatrix3<Base>::_RotationMatrix3(Base roll, Base pitch, Base yaw){
00083 Base sphi = sin(roll);
00084 Base stheta = sin(pitch);
00085 Base spsi = sin(yaw);
00086 Base cphi = cos(roll);
00087 Base ctheta = cos(pitch);
00088 Base cpsi = cos(yaw);
00089
00090 Base _r[3][3] = {
00091 {cpsi*ctheta, cpsi*stheta*sphi - spsi*cphi, cpsi*stheta*cphi + spsi*sphi},
00092 {spsi*ctheta, spsi*stheta*sphi + cpsi*cphi, spsi*stheta*cphi - cpsi*sphi},
00093 { -stheta, ctheta*sphi, ctheta*cphi}
00094 };
00095 for (int i=0; i<3; i++)
00096 for (int j=0; j<3; j++)
00097 this->_allocator[i][j]=_r[i][j];
00098
00099 }
00100
00101 template <typename Base>
00102 _RotationMatrix3<Base>::_RotationMatrix3(const _Vector<3, Base>& angles) {
00103 *this=_RotationMatrix3(angles.roll(), angles.pitch(), angles.yaw());
00104 }
00105
00106 template <typename Base>
00107 _RotationMatrix3<Base>::_RotationMatrix3():_RotationMatrix<3, Base>() {
00108 }
00109
00110 template <typename Base>
00111 _RotationMatrix2<Base>::_RotationMatrix2():_RotationMatrix<2, Base>() {
00112 }
00113
00114
00115 template <typename Base>
00116 _Vector<3, Base> _RotationMatrix3<Base>::angles() const{
00117 _Vector<3, Base> aux;
00118 aux.roll() = atan2(this->_allocator[2][1],this->_allocator[2][2]);
00119 aux.pitch() = atan2(-this->_allocator[2][0],sqrt(this->_allocator[2][1]*this->_allocator[2][1] + this->_allocator[2][2]* this->_allocator[2][2]));
00120 aux.yaw() = atan2(this->_allocator[1][0],this->_allocator[0][0]);
00121 return aux;
00122 }