octomap 1.5.0
include/octomap/math/Quaternion.h
Go to the documentation of this file.
00001 // $Id: Quaternion.h 171 2011-01-20 13:56:28Z kai_wurm $
00002 
00011 /*
00012  * Copyright (c) 2009-2011, K. M. Wurm, A. Hornung, University of Freiburg
00013  * All rights reserved.
00014  *
00015  * Redistribution and use in source and binary forms, with or without
00016  * modification, are permitted provided that the following conditions are met:
00017  *
00018  *     * Redistributions of source code must retain the above copyright
00019  *       notice, this list of conditions and the following disclaimer.
00020  *     * Redistributions in binary form must reproduce the above copyright
00021  *       notice, this list of conditions and the following disclaimer in the
00022  *       documentation and/or other materials provided with the distribution.
00023  *     * Neither the name of the University of Freiburg nor the names of its
00024  *       contributors may be used to endorse or promote products derived from
00025  *       this software without specific prior written permission.
00026  *
00027  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
00028  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
00029  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
00030  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
00031  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
00032  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
00033  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
00034  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
00035  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
00036  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00037  * POSSIBILITY OF SUCH DAMAGE.
00038  */
00039 
00040 #ifndef OCTOMATH_QUATERNION_H
00041 #define OCTOMATH_QUATERNION_H
00042 
00043 #include "Vector3.h"
00044 
00045 #include <iostream>
00046 #include <vector>
00047 
00048 
00049 namespace octomath {
00050 
00062   class Quaternion {
00063 
00064   public:
00065 
00072     inline Quaternion() { u() = 1;  x() = 0; y() = 0; z() = 0;  }
00073 
00077     Quaternion(const Quaternion& other);
00078 
00085     Quaternion(float u, float x, float y, float z);
00086 
00092     Quaternion(const Vector3& other);
00093 
00103     Quaternion(double roll, double pitch, double yaw);
00104 
00105 
00106      
00108     Quaternion(const Vector3& axis, double angle);
00109 
00110 
00117     Vector3 toEuler() const;
00118 
00119     void toRotMatrix(std::vector <double>& rot_matrix_3_3) const;
00120 
00121 
00122     inline const float& operator() (unsigned int i) const { return data[i]; }
00123     inline float& operator() (unsigned int i) { return data[i]; }
00124 
00125     float norm () const;
00126     Quaternion  normalized () const;
00127     Quaternion& normalize ();
00128 
00129 
00130     void operator/= (float x);
00131     Quaternion& operator= (const Quaternion& other);
00132     bool operator== (const Quaternion& other) const;
00133 
00141     Quaternion operator* (const Quaternion& other) const;
00142 
00148     Quaternion operator* (const Vector3 &v) const;
00149 
00155     friend Quaternion operator* (const Vector3 &v, const Quaternion &q);
00156 
00162     inline Quaternion inv() const {  return Quaternion(u(), -x(), -y(), -z()); }
00163 
00164 
00171     Quaternion& inv_IP();
00172 
00182     Vector3 rotate(const Vector3 &v) const;
00183 
00184     inline float& u() { return data[0]; }
00185     inline float& x() { return data[1]; }
00186     inline float& y() { return data[2]; }
00187     inline float& z() { return data[3]; }
00188 
00189     inline const float& u() const { return data[0]; }
00190     inline const float& x() const { return data[1]; }
00191     inline const float& y() const { return data[2]; }
00192     inline const float& z() const { return data[3]; }
00193 
00194     std::istream& read(std::istream &s);
00195     std::ostream& write(std::ostream &s) const;
00196     std::istream& readBinary(std::istream &s);
00197     std::ostream& writeBinary(std::ostream &s) const;
00198 
00199   protected:
00200     float data[4];
00201 
00202   };
00203 
00205   std::ostream& operator<<(std::ostream& s, const Quaternion& q);
00206 
00207 }
00208 
00209 #endif