Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017 #include <limits>
00018 #include <algorithm>
00019
00020 template<typename Base>
00021 _Quaternion<Base>::_Quaternion(): _Vector<4, Base>(Base(0), Base(0), Base(0), Base(1)){
00022 }
00023
00024 template<typename Base>
00025 _Quaternion<Base>::_Quaternion(const _Vector<4, Base>& v): _Vector<4, Base>(v){
00026 }
00027
00028 template<typename Base>
00029 _Quaternion<Base>::_Quaternion(Base _x, Base _y, Base _z, Base _w): _Vector<4, Base>(_x,_y,_z,_w){
00030 }
00031
00032 template<typename Base>
00033 _Quaternion<Base>::_Quaternion(const _RotationMatrix3<Base>& _r){
00034 this->x()=_r[2][1]-_r[1][2];
00035 this->y()=_r[0][2]-_r[2][0];
00036 this->z()=_r[1][0]-_r[0][1];
00037 this->w()=Base(1.)+_r[0][0]+_r[1][1]+_r[2][2];
00038 this->normalize();
00039 }
00040
00041 template<typename Base>
00042 _Quaternion<Base>::_Quaternion(const _Vector<3, Base>& vec){
00043 *this=_Quaternion(_RotationMatrix3<Base>(vec));
00044 }
00045
00046 template<typename Base>
00047 _Quaternion<Base>::_Quaternion(Base roll, Base pitch, Base yaw){
00048 *this=_Quaternion(_RotationMatrix3<Base>(roll, pitch, yaw));
00049 }
00050
00051 template<typename Base>
00052 inline _Quaternion<Base> _Quaternion<Base>::inverse() const{
00053 return _Quaternion<Base>(-this->x(), -this->y(), -this->z(), this->w());
00054 }
00055
00056 template<typename Base>
00057 inline _Quaternion<Base>& _Quaternion<Base>::normalize(){
00058 Base n = this->w()>0 ? this->norm() : -this->norm();
00059 if (fabs(n) > 1e-9){
00060 this->_Vector<4, Base>::operator*=(Base(1.)/n);
00061 }
00062 else
00063 *this= _Quaternion<Base>(0.,0.,0.,1.);
00064 return *this;
00065 }
00066
00067 template<typename Base>
00068 inline _Quaternion<Base> _Quaternion<Base>::normalized() const{
00069 _Quaternion<Base> q(*this);
00070 return q.normalize();
00071 }
00072
00073
00074 template<typename Base>
00075 inline _Vector<3, Base> _Quaternion<Base>::operator * (const _Vector<3, Base>& point) const{
00076 const double sa = point[0]*this->x() + point[1]*this->y() + point[2]*this->z();
00077 const double sb = point[0]*this->w() - point[1]*this->z() + point[2]*this->y();
00078 const double sc = point[0]*this->z() + point[1]*this->w() - point[2]*this->x();
00079 const double sd = - point[0]*this->y() + point[1]*this->x() + point[2]*this->w();
00080 _Vector<3, Base> out;
00081 out[0] = this->w() * sb + this->x() * sa + this->y() * sd - this->z() * sc;
00082 out[1] = this->w() * sc - this->x() * sd + this->y() * sa + this->z() * sb;
00083 out[2] = this->w() * sd + this->x() * sc - this->y() * sb + this->z() * sa;
00084 return out;
00085 }
00086
00087 template<typename Base>
00088 inline _Quaternion<Base> _Quaternion<Base>::operator * (const _Quaternion<Base>& q2) const{
00089 _Quaternion<Base> q(this->y()*q2.z() - q2.y()*this->z() + this->w()*q2.x() + q2.w()*this->x(),
00090 this->z()*q2.x() - q2.z()*this->x() + this->w()*q2.y() + q2.w()*this->y(),
00091 this->x()*q2.y() - q2.x()*this->y() + this->w()*q2.z() + q2.w()*this->z(),
00092 this->w()*q2.w() - this->x()*q2.x() - this->y()*q2.y() - this->z()*q2.z());
00093
00094 return q.normalize();
00095
00096 }
00097
00098 template<typename Base>
00099 inline _Quaternion<Base>& _Quaternion<Base>::operator *= (const _Quaternion<Base>& q2){
00100 _Quaternion<Base> q=(*this*q2);
00101 *this=q.normalize();
00102 return *this;
00103 }
00104
00105
00106 template<typename Base>
00107 inline _Vector<3, Base> _Quaternion<Base>::angles() const
00108 {
00109 return rotationMatrix().angles();
00110 }
00111
00112 template<typename Base>
00113 _RotationMatrix3<Base> _Quaternion<Base>::rotationMatrix() const
00114 {
00115 _RotationMatrix3<Base> rot;
00116 Base w2=this->w()*this->w();
00117 Base x2=this->x()*this->x();
00118 Base y2=this->y()*this->y();
00119 Base z2=this->z()*this->z();
00120 Base xy=this->x()*this->y();
00121 Base xz=this->x()*this->z();
00122 Base xw=this->x()*this->w();
00123 Base yz=this->y()*this->z();
00124 Base yw=this->y()*this->w();
00125 Base zw=this->z()*this->w();
00126 rot[0][0] = w2 + x2 - y2 - z2; rot[0][1] = 2.0 * (xy - zw); rot[0][2] = 2.0 * (xz + yw);
00127 rot[1][0] = 2.0 * (xy + zw); rot[1][1] = w2 - x2 + y2 - z2; rot[1][2] = 2.0 * (yz - xw);
00128 rot[2][0] = 2.0 * (xz - yw); rot[2][1] = 2.0 * (yz + xw); rot[2][2] = w2 - x2 - y2 + z2;
00129 return rot;
00130 }
00131
00132 template<typename Base>
00133 inline Base _Quaternion<Base>::angle() const{
00134 _Quaternion<Base> q=normalized();
00135 double a=2*atan2(sqrt(q.x()*q.x() + q.y()*q.y() + q.z()*q.z()), q.w());
00136 return atan2(sin(a), cos(a));
00137 }
00138
00139 template<typename Base>
00140 inline _Quaternion<Base> _Quaternion<Base>::slerp(const _Quaternion<Base>& from, const _Quaternion<Base>& to, Base lambda){
00141 Base _cos_omega = _Vector<4, Base>(from)*_Vector<4, Base>(to);
00142 _cos_omega = (_cos_omega>1)?1:_cos_omega;
00143 _cos_omega = (_cos_omega<-1)?-1:_cos_omega;
00144 Base _omega = acos(_cos_omega);
00145 assert (!isnan(_cos_omega));
00146 if (fabs(_omega) < 1e-9)
00147 return to;
00148
00149
00150 _Quaternion<Base> _pq = from - to;
00151 _Quaternion<Base> _pmq = from + to;
00152 Base _first = _pq.norm();
00153 Base _alternativ = _pmq.norm();
00154
00155 _Quaternion<Base> q1 = from;
00156 _Quaternion<Base> q2 = to;
00157
00158 if (_first > _alternativ)
00159 q2._Vector<4, Base>::operator*=(Base(-1.));
00160
00161
00162 Base alpha=sin((1-lambda)*_omega)/(sin(_omega));
00163 Base beta=sin((lambda)*_omega)/(sin(_omega));
00164 _Quaternion<Base> ret((_Vector<4, Base>)q1*(alpha) + (_Vector<4, Base>)q2*(beta));
00165 assert (!(isnan(ret.w()) || isnan(ret.x()) || isnan(ret.y()) || isnan(ret.z())));
00166 return ret.normalized();
00167 }
00168
00169 template<typename Base>
00170 template<typename Base2>
00171 _Quaternion<Base>::_Quaternion(const _Quaternion<Base2>& other){
00172 (*this)[0] = other[0];
00173 (*this)[1] = other[1];
00174 (*this)[2] = other[2];
00175 (*this)[3] = other[3];
00176 }