Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017 template <typename Base>
00018 _AxisAngle<Base>::_AxisAngle():_Vector<3, Base> (Base(0.),Base(0.),Base(0.)) {}
00019
00020 template <typename Base>
00021 _AxisAngle<Base>::_AxisAngle(const _Quaternion<Base>& q) {
00022 Base angle=q.angle();
00023 Base imNorm = sqrt(q.x()*q.x()+q.y()*q.y()+q.z()*q.z());
00024 if (imNorm < std::numeric_limits<Base>::min()){
00025 this->x()=Base(0.);
00026 this->y()=Base(0.);
00027 this->z()=Base(0.);
00028 } else {
00029 Base alpha=angle/imNorm;
00030 this->x()=q.x()*alpha;
00031 this->y()=q.y()*alpha;
00032 this->z()=q.z()*alpha;
00033 }
00034 }
00035
00036 template <typename Base>
00037 _AxisAngle<Base>::_AxisAngle(const _RotationMatrix3<Base>& m) {
00038 *this=_Quaternion<Base>(m);
00039 }
00040
00041 template <typename Base>
00042 _AxisAngle<Base>::_AxisAngle(const _Vector<3, Base>& vec) {
00043 *this=_Quaternion<Base>(vec);
00044 }
00045
00046 template <typename Base>
00047 _AxisAngle<Base>::_AxisAngle(Base roll, Base pitch, Base yaw){
00048 *this=_AxisAngle(_Quaternion<Base>(roll, pitch, yaw));
00049 }
00050
00051 template <typename Base>
00052 _AxisAngle<Base>::_AxisAngle(const _Vector<3, Base>& axis, Base angle){
00053 *this = axis.normalized() * angle;
00054 }
00055
00056 template <typename Base>
00057 _AxisAngle<Base>& _AxisAngle<Base>::operator*=(const _AxisAngle& a){
00058 *this=(*this)*a;
00059 return *this;
00060 }
00061
00062
00063 template <typename Base>
00064 _AxisAngle<Base> _AxisAngle<Base>::operator* (const _AxisAngle& a) const{
00065 return _AxisAngle<Base>(quaternion()*a.quaternion());
00066 }
00067
00068 template <typename Base>
00069 _Vector<3, Base> _AxisAngle<Base>::operator*(const _Vector<3, Base>& v) const {
00070 return rotationMatrix()*v;
00071 }
00072
00073 template <typename Base>
00074 inline _AxisAngle<Base> _AxisAngle<Base>::inverse() const {
00075 _AxisAngle a(*this);
00076 a._Vector<3, Base>::operator*=(Base(-1.));
00077 return a;
00078 }
00079
00080 template <typename Base>
00081 inline _Vector<3, Base> _AxisAngle<Base>::angles() const {
00082 return quaternion().angles();
00083 }
00084
00085 template <typename Base>
00086 _RotationMatrix3<Base> _AxisAngle<Base>::rotationMatrix() const{
00087 return quaternion().rotationMatrix();
00088 }
00089
00090 template <typename Base>
00091 _Quaternion<Base> _AxisAngle<Base>::quaternion() const {
00092 Base n=this->norm();
00093 if (n<=0){
00094 return _Quaternion<Base>();
00095 }
00096 Base s=sin(n*Base(.5))/n;
00097 Base c=cos(n*Base(.5));
00098 return _Quaternion<Base> (this->x()*s, this->y()*s, this->z()*s, c);
00099 }
00100