Quaternion.cpp
Go to the documentation of this file.
1 /*
2  * OctoMap - An Efficient Probabilistic 3D Mapping Framework Based on Octrees
3  * http://octomap.github.com/
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 #include <octomap/math/Vector3.h>
36 #include <octomap/math/Utils.h>
37 
38 #include <cassert>
39 #include <math.h>
40 #include <algorithm>
41 
42 
43 // used from Vector: norm2, unit, *
44 
45 namespace octomath {
46 
47 
49  data[0] = other(0);
50  data[1] = other(1);
51  data[2] = other(2);
52  data[3] = other(3);
53  }
54 
55  Quaternion::Quaternion(float uu, float xx, float yy, float zz) {
56  u() = uu;
57  x() = xx;
58  y() = yy;
59  z() = zz;
60  }
61 
63  operator=(Quaternion(other.roll(), other.pitch(), other.yaw()));
64  }
65 
66  Quaternion::Quaternion(double roll, double pitch, double yaw) {
67  double sroll = sin(roll);
68  double spitch = sin(pitch);
69  double syaw = sin(yaw);
70  double croll = cos(roll);
71  double cpitch = cos(pitch);
72  double cyaw = cos(yaw);
73 
74  double m[3][3] = { //create rotational Matrix
75  {cyaw*cpitch, cyaw*spitch*sroll - syaw*croll, cyaw*spitch*croll + syaw*sroll},
76  {syaw*cpitch, syaw*spitch*sroll + cyaw*croll, syaw*spitch*croll - cyaw*sroll},
77  { -spitch, cpitch*sroll, cpitch*croll}
78  };
79 
80  float _u = (float) (sqrt(std::max(0., 1 + m[0][0] + m[1][1] + m[2][2]))/2.0);
81  float _x = (float) (sqrt(std::max(0., 1 + m[0][0] - m[1][1] - m[2][2]))/2.0);
82  float _y = (float) (sqrt(std::max(0., 1 - m[0][0] + m[1][1] - m[2][2]))/2.0);
83  float _z = (float) (sqrt(std::max(0., 1 - m[0][0] - m[1][1] + m[2][2]))/2.0);
84  u() = _u;
85  x() = (m[2][1] - m[1][2])>=0?fabs(_x):-fabs(_x);
86  y() = (m[0][2] - m[2][0])>=0?fabs(_y):-fabs(_y);
87  z() = (m[1][0] - m[0][1])>=0?fabs(_z):-fabs(_z);
88  }
89 
90  Quaternion::Quaternion(const Vector3& axis, double angle) {
91  double sa = sin(angle/2);
92  double ca = cos(angle/2);
93  x() = (float) (axis.x()*sa);
94  y() = (float) (axis.y()*sa);
95  z() = (float) (axis.z()*sa);
96  u() = (float) ca;
97  }
98 
99  float Quaternion::norm () const {
100  double n = 0;
101  for (unsigned int i=0; i<4; i++) {
102  n += operator()(i) * operator()(i);
103  }
104  return (float) sqrt(n);
105  }
106 
108  {
109  for (unsigned int i=0; i<4; ++i)
110  operator()(i) /= x;
111  }
112 
113  bool Quaternion::operator== (const Quaternion& other) const
114  {
115  for (unsigned int i=0; i<4; i++)
116  {
117  if (operator()(i) != other(i))
118  return false;
119  }
120  return true;
121  }
122 
123 
125  // create rotational matrix
126  double n = norm ();
127  double s = n > 0?2./(n*n):0.;
128 
129  double xs = x()*s;
130  double ys = y()*s;
131  double zs = z()*s;
132 
133  double ux = u()*xs;
134  double uy = u()*ys;
135  double uz = u()*zs;
136 
137  double xx = x()*xs;
138  double xy = x()*ys;
139  double xz = x()*zs;
140 
141  double yy = y()*ys;
142  double yz = y()*zs;
143  double zz = z()*zs;
144 
145  double m[3][3];
146 
147  m[0][0] = 1.0 - (yy + zz);
148  m[1][1] = 1.0 - (xx + zz);
149  m[2][2] = 1.0 - (xx + yy);
150 
151  m[1][0] = xy + uz;
152  m[0][1] = xy - uz;
153 
154  m[2][0] = xz - uy;
155  m[0][2] = xz + uy;
156  m[2][1] = yz + ux;
157  m[1][2] = yz - ux;
158 
159  float roll = (float) atan2(m[2][1], m[2][2]);
160  float pitch = (float) atan2(-m[2][0], sqrt(m[2][1]*m[2][1] + m[2][2]*m[2][2]));
161  float yaw = (float) atan2(m[1][0], m[0][0]);
162 
163  return Vector3(roll, pitch, yaw);
164  }
165 
166 
167  void Quaternion::toRotMatrix(std::vector <double>& rot_matrix_3_3) const {
168 
169  // create rotational matrix
170  double n = norm ();
171  double s = n > 0?2./(n*n):0.;
172 
173  double xs = x()*s;
174  double ys = y()*s;
175  double zs = z()*s;
176 
177  double ux = u()*xs;
178  double uy = u()*ys;
179  double uz = u()*zs;
180 
181  double xx = x()*xs;
182  double xy = x()*ys;
183  double xz = x()*zs;
184 
185  double yy = y()*ys;
186  double yz = y()*zs;
187  double zz = z()*zs;
188 
189  double m[3][3];
190  m[0][0] = 1.0 - (yy + zz);
191  m[1][1] = 1.0 - (xx + zz);
192  m[2][2] = 1.0 - (xx + yy);
193 
194  m[1][0] = xy + uz;
195  m[0][1] = xy - uz;
196 
197  m[2][0] = xz - uy;
198  m[0][2] = xz + uy;
199  m[2][1] = yz + ux;
200  m[1][2] = yz - ux;
201 
202  rot_matrix_3_3.clear();
203  rot_matrix_3_3.resize(9,0.);
204  for (unsigned int i=0; i<3; i++) {
205  rot_matrix_3_3[i*3] = m[i][0];
206  rot_matrix_3_3[i*3+1] = m[i][1];
207  rot_matrix_3_3[i*3+2] = m[i][2];
208  }
209  }
210 
212  u() = other.u();
213  x() = other.x();
214  y() = other.y();
215  z() = other.z();
216  return *this;
217  }
218 
220  return Quaternion(u()*other.u() - x()*other.x() - y()*other.y() - z()*other.z(),
221  y()*other.z() - other.y()*z() + u()*other.x() + other.u()*x(),
222  z()*other.x() - other.z()*x() + u()*other.y() + other.u()*y(),
223  x()*other.y() - other.x()*y() + u()*other.z() + other.u()*z());
224  }
225 
227  return *this * Quaternion(0, v(0), v(1), v(2));
228  }
229 
230  Quaternion operator* (const Vector3& v, const Quaternion& q) {
231  return Quaternion(0, v(0), v(1), v(2)) * q;
232  }
233 
235  double len = norm ();
236  if (len > 0)
237  *this /= (float) len;
238  return *this;
239  }
240 
242  Quaternion result(*this);
243  result.normalize ();
244  return result;
245  }
246 
247 
249  x() = -x();
250  y() = -y();
251  z() = -z();
252  return *this;
253  }
254 
256  Quaternion q = *this * v * this->inv();
257  return Vector3(q.x(), q.y(), q.z());
258  }
259 
260 
261  std::istream& Quaternion::read(std::istream &s) {
262  int temp;
263  s >> temp; // should be 4
264  for (unsigned int i=0; i<4; i++)
265  s >> operator()(i);
266  return s;
267  }
268 
269 
270  std::ostream& Quaternion::write(std::ostream &s) const {
271  s << 4;
272  for (unsigned int i=0; i<4; i++)
273  s << " " << operator()(i);
274  return s;
275  }
276 
277 
278 
279  std::istream& Quaternion::readBinary(std::istream &s) {
280  int temp;
281  s.read((char*)&temp, sizeof(temp));
282  double val = 0;
283  for (unsigned int i=0; i<4; i++) {
284  s.read((char*)&val, sizeof(val));
285  operator()(i) = (float) val;
286  }
287  return s;
288  }
289 
290 
291  std::ostream& Quaternion::writeBinary(std::ostream &s) const {
292  int temp = 4;
293  s.write((char*)&temp, sizeof(temp));
294  double val = 0;
295  for (unsigned int i=0; i<4; i++) {
296  val = operator()(i);
297  s.write((char*)&val, sizeof(val));
298  }
299  return s;
300  }
301 
302 
303 
304  std::ostream& operator<<(std::ostream& s, const Quaternion& q) {
305  s << "(" << q.u() << " " << q.x() << " " << q.y() << " " << q.z() << ")";
306  return s;
307  }
308 
309 }
Quaternion & operator=(const Quaternion &other)
Definition: Quaternion.cpp:211
std::ostream & writeBinary(std::ostream &s) const
Definition: Quaternion.cpp:291
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:147
std::istream & read(std::istream &s)
Definition: Quaternion.cpp:261
void toRotMatrix(std::vector< double > &rot_matrix_3_3) const
Definition: Quaternion.cpp:167
float norm() const
Definition: Quaternion.cpp:99
const float & operator()(unsigned int i) const
Definition: Quaternion.h:116
bool operator==(const Quaternion &other) const
Definition: Quaternion.cpp:113
Quaternion inv() const
Inversion.
Definition: Quaternion.h:156
Quaternion normalized() const
Definition: Quaternion.cpp:241
Quaternion & normalize()
Definition: Quaternion.cpp:234
float & y()
Definition: Vector3.h:136
float & x()
Definition: Vector3.h:131
This class represents a three-dimensional vector.
Definition: Vector3.h:50
void operator/=(float x)
Definition: Quaternion.cpp:107
Vector3 toEuler() const
Conversion to Euler angles.
Definition: Quaternion.cpp:124
Vector3 rotate(const Vector3 &v) const
Rotate a vector.
Definition: Quaternion.cpp:255
Quaternion & inv_IP()
Inversion.
Definition: Quaternion.cpp:248
float & roll()
Definition: Vector3.h:161
Quaternion()
Default constructor.
Definition: Quaternion.h:66
float & pitch()
Definition: Vector3.h:166
std::ostream & write(std::ostream &s) const
Definition: Quaternion.cpp:270
Quaternion operator*(const Quaternion &other) const
Quaternion multiplication.
Definition: Quaternion.cpp:219
float & yaw()
Definition: Vector3.h:171
float & z()
Definition: Vector3.h:141
This class represents a Quaternion.
Definition: Quaternion.h:56
std::istream & readBinary(std::istream &s)
Definition: Quaternion.cpp:279


octomap
Author(s): Kai M. Wurm , Armin Hornung
autogenerated on Mon Feb 28 2022 22:58:06