octomap 1.5.0
|
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