octomap 1.9.8
Quaternion.h
Go to the documentation of this file.
1/*
2 * OctoMap - An Efficient Probabilistic 3D Mapping Framework Based on Octrees
3 * https://octomap.github.io/
4 *
5 * Copyright (c) 2009-2013, K.M. Wurm and A. Hornung, University of Freiburg
6 * All rights reserved.
7 * License: New BSD
8 *
9 * Redistribution and use in source and binary forms, with or without
10 * modification, are permitted provided that the following conditions are met:
11 *
12 * * Redistributions of source code must retain the above copyright
13 * notice, this list of conditions and the following disclaimer.
14 * * Redistributions in binary form must reproduce the above copyright
15 * notice, this list of conditions and the following disclaimer in the
16 * documentation and/or other materials provided with the distribution.
17 * * Neither the name of the University of Freiburg nor the names of its
18 * contributors may be used to endorse or promote products derived from
19 * this software without specific prior written permission.
20 *
21 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
22 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
23 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
24 * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
25 * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
26 * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
27 * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
28 * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
29 * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
30 * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
31 * POSSIBILITY OF SUCH DAMAGE.
32 */
33
34#ifndef OCTOMATH_QUATERNION_H
35#define OCTOMATH_QUATERNION_H
36
37#include "Vector3.h"
38
39#include <iostream>
40#include <vector>
41
42
43namespace octomath {
44
56 class Quaternion {
57
58 public:
59
66 inline Quaternion() { u() = 1; x() = 0; y() = 0; z() = 0; }
67
71 Quaternion(const Quaternion& other);
72
79 Quaternion(float u, float x, float y, float z);
80
86 Quaternion(const Vector3& other);
87
97 Quaternion(double roll, double pitch, double yaw);
98
99
100
102 Quaternion(const Vector3& axis, double angle);
103
104
111 Vector3 toEuler() const;
112
113 void toRotMatrix(std::vector <double>& rot_matrix_3_3) const;
114
115
116 inline const float& operator() (unsigned int i) const { return data[i]; }
117 inline float& operator() (unsigned int i) { return data[i]; }
118
119 float norm () const;
120 Quaternion normalized () const;
122
123
124 void operator/= (float x);
125 Quaternion& operator= (const Quaternion& other);
126 bool operator== (const Quaternion& other) const;
127
135 Quaternion operator* (const Quaternion& other) const;
136
142 Quaternion operator* (const Vector3 &v) const;
143
149 friend Quaternion operator* (const Vector3 &v, const Quaternion &q);
150
156 inline Quaternion inv() const { return Quaternion(u(), -x(), -y(), -z()); }
157
158
166
176 Vector3 rotate(const Vector3 &v) const;
177
178 inline float& u() { return data[0]; }
179 inline float& x() { return data[1]; }
180 inline float& y() { return data[2]; }
181 inline float& z() { return data[3]; }
182
183 inline const float& u() const { return data[0]; }
184 inline const float& x() const { return data[1]; }
185 inline const float& y() const { return data[2]; }
186 inline const float& z() const { return data[3]; }
187
188 std::istream& read(std::istream &s);
189 std::ostream& write(std::ostream &s) const;
190 std::istream& readBinary(std::istream &s);
191 std::ostream& writeBinary(std::ostream &s) const;
192
193 protected:
194 float data[4];
195
196 };
197
199 std::ostream& operator<<(std::ostream& s, const Quaternion& q);
200
201}
202
203#endif
This class represents a Quaternion.
Definition: Quaternion.h:56
void operator/=(float x)
Definition: Quaternion.cpp:107
Vector3 rotate(const Vector3 &v) const
Rotate a vector.
Definition: Quaternion.cpp:255
float & y()
Definition: Quaternion.h:180
Quaternion & normalize()
Definition: Quaternion.cpp:234
Quaternion & operator=(const Quaternion &other)
Definition: Quaternion.cpp:211
Vector3 toEuler() const
Conversion to Euler angles.
Definition: Quaternion.cpp:124
std::istream & readBinary(std::istream &s)
Definition: Quaternion.cpp:279
Quaternion operator*(const Quaternion &other) const
Quaternion multiplication.
Definition: Quaternion.cpp:219
Quaternion normalized() const
Definition: Quaternion.cpp:241
std::istream & read(std::istream &s)
Definition: Quaternion.cpp:261
float & x()
Definition: Quaternion.h:179
void toRotMatrix(std::vector< double > &rot_matrix_3_3) const
Definition: Quaternion.cpp:167
const float & operator()(unsigned int i) const
Definition: Quaternion.h:116
std::ostream & writeBinary(std::ostream &s) const
Definition: Quaternion.cpp:291
const float & u() const
Definition: Quaternion.h:183
Quaternion inv() const
Inversion.
Definition: Quaternion.h:156
float data[4]
Definition: Quaternion.h:194
Quaternion & inv_IP()
Inversion.
Definition: Quaternion.cpp:248
bool operator==(const Quaternion &other) const
Definition: Quaternion.cpp:113
float norm() const
Definition: Quaternion.cpp:99
const float & z() const
Definition: Quaternion.h:186
float & u()
Definition: Quaternion.h:178
std::ostream & write(std::ostream &s) const
Definition: Quaternion.cpp:270
const float & x() const
Definition: Quaternion.h:184
Quaternion()
Default constructor.
Definition: Quaternion.h:66
const float & y() const
Definition: Quaternion.h:185
float & z()
Definition: Quaternion.h:181
This class represents a three-dimensional vector.
Definition: Vector3.h:50
Namespace of the math library in OctoMap.
std::ostream & operator<<(std::ostream &s, const Pose6D &p)
user friendly output in format (x y z, u x y z) which is (translation, rotation)
Definition: Pose6D.cpp:152