Quaternion.cpp
Go to the documentation of this file.
00001 /*
00002  * OctoMap - An Efficient Probabilistic 3D Mapping Framework Based on Octrees
00003  * http://octomap.github.com/
00004  *
00005  * Copyright (c) 2009-2013, K.M. Wurm and A. Hornung, University of Freiburg
00006  * All rights reserved.
00007  * License: New BSD
00008  *
00009  * Redistribution and use in source and binary forms, with or without
00010  * modification, are permitted provided that the following conditions are met:
00011  *
00012  *     * Redistributions of source code must retain the above copyright
00013  *       notice, this list of conditions and the following disclaimer.
00014  *     * Redistributions in binary form must reproduce the above copyright
00015  *       notice, this list of conditions and the following disclaimer in the
00016  *       documentation and/or other materials provided with the distribution.
00017  *     * Neither the name of the University of Freiburg nor the names of its
00018  *       contributors may be used to endorse or promote products derived from
00019  *       this software without specific prior written permission.
00020  *
00021  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
00022  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
00023  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
00024  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
00025  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
00026  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
00027  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
00028  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
00029  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
00030  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00031  * POSSIBILITY OF SUCH DAMAGE.
00032  */
00033 
00034 #include <octomap/math/Vector3.h>
00035 #include <octomap/math/Quaternion.h>
00036 #include <octomap/math/Utils.h>
00037 
00038 #include <cassert>
00039 #include <math.h>
00040 
00041 
00042 // used from Vector: norm2, unit, *
00043 
00044 namespace octomath {
00045 
00046 
00047   Quaternion::Quaternion(const Quaternion &other) {
00048     data[0] = other(0);
00049     data[1] = other(1);
00050     data[2] = other(2);
00051     data[3] = other(3);
00052   }
00053 
00054   Quaternion::Quaternion(float uu, float xx, float yy, float zz) {
00055     u() = uu;
00056     x() = xx;
00057     y() = yy;
00058     z() = zz;
00059   }
00060 
00061   Quaternion::Quaternion(const Vector3 &other) {
00062     operator=(Quaternion(other.roll(), other.pitch(), other.yaw()));
00063   }
00064 
00065   Quaternion::Quaternion(double roll, double pitch, double yaw) {
00066     double sroll   = sin(roll);
00067     double spitch = sin(pitch);
00068     double syaw   = sin(yaw);
00069     double croll   = cos(roll);
00070     double cpitch = cos(pitch);
00071     double cyaw   = cos(yaw);
00072 
00073     double m[3][3] = { //create rotational Matrix
00074       {cyaw*cpitch, cyaw*spitch*sroll - syaw*croll, cyaw*spitch*croll + syaw*sroll},
00075       {syaw*cpitch, syaw*spitch*sroll + cyaw*croll, syaw*spitch*croll - cyaw*sroll},
00076       {    -spitch,                  cpitch*sroll,                  cpitch*croll}
00077     };
00078 
00079     float _u = (float) (sqrt(std::max(0., 1 + m[0][0] + m[1][1] + m[2][2]))/2.0);
00080     float _x = (float) (sqrt(std::max(0., 1 + m[0][0] - m[1][1] - m[2][2]))/2.0);
00081     float _y = (float) (sqrt(std::max(0., 1 - m[0][0] + m[1][1] - m[2][2]))/2.0);
00082     float _z = (float) (sqrt(std::max(0., 1 - m[0][0] - m[1][1] + m[2][2]))/2.0);
00083     u() = _u;
00084     x() = (m[2][1] - m[1][2])>=0?fabs(_x):-fabs(_x);
00085     y() = (m[0][2] - m[2][0])>=0?fabs(_y):-fabs(_y);
00086     z() = (m[1][0] - m[0][1])>=0?fabs(_z):-fabs(_z);
00087   }
00088 
00089   Quaternion::Quaternion(const Vector3& axis, double angle) {
00090     double sa = sin(angle/2);
00091     double ca = cos(angle/2);
00092     x() = (float) (axis.x()*sa);
00093     y() = (float) (axis.y()*sa);
00094     z() = (float) (axis.z()*sa);
00095     u() = (float) ca;
00096   }
00097 
00098   float Quaternion::norm () const {
00099     double n = 0;
00100     for (unsigned int i=0; i<4; i++) {
00101       n += operator()(i) * operator()(i);
00102     }
00103     return (float) sqrt(n);
00104   }
00105 
00106   void Quaternion::operator/= (float x) 
00107   { 
00108     for (unsigned int i=0; i<4; ++i)
00109       operator()(i) /= x;
00110   }
00111 
00112   bool Quaternion::operator== (const Quaternion& other) const 
00113   {
00114     for (unsigned int i=0; i<4; i++) 
00115     {
00116       if (operator()(i) != other(i)) 
00117         return false;
00118     }
00119     return true;
00120   }
00121 
00122 
00123   Vector3 Quaternion::toEuler() const {
00124     // create rotational matrix
00125     double n = norm ();
00126     double s = n > 0?2./(n*n):0.;
00127 
00128     double xs = x()*s;
00129     double ys = y()*s;
00130     double zs = z()*s;
00131 
00132     double ux = u()*xs;
00133     double uy = u()*ys;
00134     double uz = u()*zs;
00135 
00136     double xx = x()*xs;
00137     double xy = x()*ys;
00138     double xz = x()*zs;
00139 
00140     double yy = y()*ys;
00141     double yz = y()*zs;
00142     double zz = z()*zs;
00143 
00144     double m[3][3];
00145 
00146     m[0][0] = 1.0 - (yy + zz);
00147     m[1][1] = 1.0 - (xx + zz);
00148     m[2][2] = 1.0 - (xx + yy);
00149 
00150     m[1][0] = xy + uz;
00151     m[0][1] = xy - uz;
00152 
00153     m[2][0] = xz - uy;
00154     m[0][2] = xz + uy;
00155     m[2][1] = yz + ux;
00156     m[1][2] = yz - ux;
00157 
00158     float roll  = (float) atan2(m[2][1], m[2][2]);
00159     float pitch = (float) atan2(-m[2][0], sqrt(m[2][1]*m[2][1] + m[2][2]*m[2][2]));
00160     float yaw   = (float) atan2(m[1][0], m[0][0]);
00161 
00162     return Vector3(roll, pitch, yaw);
00163   }
00164 
00165 
00166   void Quaternion::toRotMatrix(std::vector <double>& rot_matrix_3_3) const {
00167 
00168     // create rotational matrix
00169     double n = norm ();
00170     double s = n > 0?2./(n*n):0.;
00171 
00172     double xs = x()*s;
00173     double ys = y()*s;
00174     double zs = z()*s;
00175 
00176     double ux = u()*xs;
00177     double uy = u()*ys;
00178     double uz = u()*zs;
00179 
00180     double xx = x()*xs;
00181     double xy = x()*ys;
00182     double xz = x()*zs;
00183 
00184     double yy = y()*ys;
00185     double yz = y()*zs;
00186     double zz = z()*zs;
00187 
00188     double m[3][3];
00189     m[0][0] = 1.0 - (yy + zz);
00190     m[1][1] = 1.0 - (xx + zz);
00191     m[2][2] = 1.0 - (xx + yy);
00192 
00193     m[1][0] = xy + uz;
00194     m[0][1] = xy - uz;
00195 
00196     m[2][0] = xz - uy;
00197     m[0][2] = xz + uy;
00198     m[2][1] = yz + ux;
00199     m[1][2] = yz - ux;
00200 
00201     rot_matrix_3_3.clear();
00202     rot_matrix_3_3.resize(9,0.);
00203     for (unsigned int i=0; i<3; i++) {
00204       rot_matrix_3_3[i*3] = m[i][0];
00205       rot_matrix_3_3[i*3+1] = m[i][1];
00206       rot_matrix_3_3[i*3+2] = m[i][2];
00207     }
00208   }
00209 
00210   Quaternion& Quaternion::operator= (const Quaternion& other) {
00211     u() = other.u();
00212     x() = other.x();
00213     y() = other.y();
00214     z() = other.z();
00215     return *this;
00216   }
00217 
00218   Quaternion Quaternion::operator* (const Quaternion& other) const {
00219     return Quaternion(u()*other.u() - x()*other.x() - y()*other.y() - z()*other.z(),
00220                       y()*other.z() - other.y()*z() + u()*other.x() + other.u()*x(),
00221                       z()*other.x() - other.z()*x() + u()*other.y() + other.u()*y(),
00222                       x()*other.y() - other.x()*y() + u()*other.z() + other.u()*z());
00223   }
00224 
00225   Quaternion Quaternion::operator* (const Vector3& v) const {
00226     return *this * Quaternion(0, v(0), v(1), v(2));
00227   }
00228 
00229   Quaternion operator* (const Vector3& v, const Quaternion& q) {
00230     return Quaternion(0, v(0), v(1), v(2)) * q;
00231   }
00232 
00233   Quaternion& Quaternion::normalize (){
00234     double len = norm ();
00235     if (len > 0)
00236       *this /= (float) len;
00237     return *this;
00238   }
00239 
00240   Quaternion Quaternion::normalized () const {
00241     Quaternion result(*this);
00242     result.normalize ();
00243     return result;
00244   }
00245 
00246 
00247   Quaternion& Quaternion::inv_IP() {
00248     x() = -x();
00249     y() = -y();
00250     z() = -z();
00251     return *this;
00252   }
00253 
00254   Vector3 Quaternion::rotate(const Vector3& v) const {
00255     Quaternion q = *this * v * this->inv();
00256     return Vector3(q.x(), q.y(), q.z());
00257   }
00258 
00259 
00260   std::istream& Quaternion::read(std::istream &s) {
00261     int temp;
00262     s >> temp; // should be 4
00263     for (unsigned int i=0; i<4; i++)
00264       s >> operator()(i);
00265     return s;
00266   }
00267 
00268 
00269   std::ostream& Quaternion::write(std::ostream &s) const {
00270     s << 4;
00271     for (unsigned int i=0; i<4; i++)
00272       s << " " << operator()(i);
00273     return s;
00274   }
00275 
00276 
00277 
00278   std::istream& Quaternion::readBinary(std::istream &s) {
00279     int temp;
00280     s.read((char*)&temp, sizeof(temp));
00281     double val = 0;
00282     for (unsigned int i=0; i<4; i++) {
00283       s.read((char*)&val, sizeof(val));
00284       operator()(i) = (float) val;
00285     }
00286     return s;
00287   }
00288 
00289 
00290   std::ostream& Quaternion::writeBinary(std::ostream &s) const {
00291     int temp = 4;
00292     s.write((char*)&temp, sizeof(temp));
00293     double val = 0;
00294     for (unsigned int i=0; i<4; i++) {
00295       val = operator()(i);
00296       s.write((char*)&val, sizeof(val));
00297     }
00298     return s;
00299   }
00300 
00301 
00302 
00303   std::ostream& operator<<(std::ostream& s, const Quaternion& q) {
00304     s << "(" << q.u() << " " << q.x() << " " << q.y() << " " << q.z() << ")";
00305     return s;
00306   }
00307 
00308 }


octomap
Author(s): Kai M. Wurm , Armin Hornung
autogenerated on Thu Aug 27 2015 14:13:14