melodic/include/tf2/LinearMath/Quaternion.h
Go to the documentation of this file.
1 #include "sick_scan/sick_scan_base.h" /* Base definitions included in all header files, added by add_sick_scan_base_header.py. Do not edit this line. */
2 /*
3 Copyright (c) 2003-2006 Gino van den Bergen / Erwin Coumans http://continuousphysics.com/Bullet/
4 
5 This software is provided 'as-is', without any express or implied warranty.
6 In no event will the authors be held liable for any damages arising from the use of this software.
7 Permission is granted to anyone to use this software for any purpose,
8 including commercial applications, and to alter it and redistribute it freely,
9 subject to the following restrictions:
10 
11 1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
12 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
13 3. This notice may not be removed or altered from any source distribution.
14 */
15 
16 
17 
18 #ifndef TF2_QUATERNION_H_
19 #define TF2_QUATERNION_H_
20 
21 
22 #include "Vector3.h"
23 #include "QuadWord.h"
24 
25 namespace tf2
26 {
27 
29 class Quaternion : public QuadWord {
30 public:
32  Quaternion() {}
33 
34  // template <typename tf2Scalar>
35  // explicit Quaternion(const tf2Scalar *v) : Tuple4<tf2Scalar>(v) {}
37  Quaternion(const tf2Scalar& x, const tf2Scalar& y, const tf2Scalar& z, const tf2Scalar& w)
38  : QuadWord(x, y, z, w)
39  {}
43  Quaternion(const Vector3& axis, const tf2Scalar& angle)
44  {
45  setRotation(axis, angle);
46  }
51  Quaternion(const tf2Scalar& yaw, const tf2Scalar& pitch, const tf2Scalar& roll) // __attribute__((deprecated))
52  {
53 #ifndef TF2_EULER_DEFAULT_ZYX
54  setEuler(yaw, pitch, roll);
55 #else
56  setRPY(roll, pitch, yaw);
57 #endif
58  }
62  void setRotation(const Vector3& axis, const tf2Scalar& angle)
63  {
64  tf2Scalar d = axis.length();
65  tf2Assert(d != tf2Scalar(0.0));
66  tf2Scalar s = tf2Sin(angle * tf2Scalar(0.5)) / d;
67  setValue(axis.x() * s, axis.y() * s, axis.z() * s,
68  tf2Cos(angle * tf2Scalar(0.5)));
69  }
74  void setEuler(const tf2Scalar& yaw, const tf2Scalar& pitch, const tf2Scalar& roll)
75  {
76  tf2Scalar halfYaw = tf2Scalar(yaw) * tf2Scalar(0.5);
77  tf2Scalar halfPitch = tf2Scalar(pitch) * tf2Scalar(0.5);
78  tf2Scalar halfRoll = tf2Scalar(roll) * tf2Scalar(0.5);
79  tf2Scalar cosYaw = tf2Cos(halfYaw);
80  tf2Scalar sinYaw = tf2Sin(halfYaw);
81  tf2Scalar cosPitch = tf2Cos(halfPitch);
82  tf2Scalar sinPitch = tf2Sin(halfPitch);
83  tf2Scalar cosRoll = tf2Cos(halfRoll);
84  tf2Scalar sinRoll = tf2Sin(halfRoll);
85  setValue(cosRoll * sinPitch * cosYaw + sinRoll * cosPitch * sinYaw,
86  cosRoll * cosPitch * sinYaw - sinRoll * sinPitch * cosYaw,
87  sinRoll * cosPitch * cosYaw - cosRoll * sinPitch * sinYaw,
88  cosRoll * cosPitch * cosYaw + sinRoll * sinPitch * sinYaw);
89  }
94  void setRPY(const tf2Scalar& roll, const tf2Scalar& pitch, const tf2Scalar& yaw)
95  {
96  tf2Scalar halfYaw = tf2Scalar(yaw) * tf2Scalar(0.5);
97  tf2Scalar halfPitch = tf2Scalar(pitch) * tf2Scalar(0.5);
98  tf2Scalar halfRoll = tf2Scalar(roll) * tf2Scalar(0.5);
99  tf2Scalar cosYaw = tf2Cos(halfYaw);
100  tf2Scalar sinYaw = tf2Sin(halfYaw);
101  tf2Scalar cosPitch = tf2Cos(halfPitch);
102  tf2Scalar sinPitch = tf2Sin(halfPitch);
103  tf2Scalar cosRoll = tf2Cos(halfRoll);
104  tf2Scalar sinRoll = tf2Sin(halfRoll);
105  setValue(sinRoll * cosPitch * cosYaw - cosRoll * sinPitch * sinYaw, //x
106  cosRoll * sinPitch * cosYaw + sinRoll * cosPitch * sinYaw, //y
107  cosRoll * cosPitch * sinYaw - sinRoll * sinPitch * cosYaw, //z
108  cosRoll * cosPitch * cosYaw + sinRoll * sinPitch * sinYaw); //formerly yzx
109  }
114  void setEulerZYX(const tf2Scalar& yaw, const tf2Scalar& pitch, const tf2Scalar& roll) // __attribute__((deprecated))
115  {
116  setRPY(roll, pitch, yaw);
117  }
121  {
122  m_floats[0] += q.x(); m_floats[1] += q.y(); m_floats[2] += q.z(); m_floats[3] += q.m_floats[3];
123  return *this;
124  }
125 
128  Quaternion& operator-=(const Quaternion& q)
129  {
130  m_floats[0] -= q.x(); m_floats[1] -= q.y(); m_floats[2] -= q.z(); m_floats[3] -= q.m_floats[3];
131  return *this;
132  }
133 
137  {
138  m_floats[0] *= s; m_floats[1] *= s; m_floats[2] *= s; m_floats[3] *= s;
139  return *this;
140  }
141 
145  Quaternion& operator*=(const Quaternion& q)
146  {
147  setValue(m_floats[3] * q.x() + m_floats[0] * q.m_floats[3] + m_floats[1] * q.z() - m_floats[2] * q.y(),
148  m_floats[3] * q.y() + m_floats[1] * q.m_floats[3] + m_floats[2] * q.x() - m_floats[0] * q.z(),
149  m_floats[3] * q.z() + m_floats[2] * q.m_floats[3] + m_floats[0] * q.y() - m_floats[1] * q.x(),
150  m_floats[3] * q.m_floats[3] - m_floats[0] * q.x() - m_floats[1] * q.y() - m_floats[2] * q.z());
151  return *this;
152  }
155  tf2Scalar dot(const Quaternion& q) const
156  {
157  return m_floats[0] * q.x() + m_floats[1] * q.y() + m_floats[2] * q.z() + m_floats[3] * q.m_floats[3];
158  }
159 
161  tf2Scalar length2() const
162  {
163  return dot(*this);
164  }
165 
167  tf2Scalar length() const
168  {
169  return tf2Sqrt(length2());
170  }
171 
175  {
176  return *this /= length();
177  }
178 
182  operator*(const tf2Scalar& s) const
183  {
184  return Quaternion(x() * s, y() * s, z() * s, m_floats[3] * s);
185  }
186 
187 
190  Quaternion operator/(const tf2Scalar& s) const
191  {
192  tf2Assert(s != tf2Scalar(0.0));
193  return *this * (tf2Scalar(1.0) / s);
194  }
195 
198  Quaternion& operator/=(const tf2Scalar& s)
199  {
200  tf2Assert(s != tf2Scalar(0.0));
201  return *this *= tf2Scalar(1.0) / s;
202  }
203 
205  Quaternion normalized() const
206  {
207  return *this / length();
208  }
211  tf2Scalar angle(const Quaternion& q) const
212  {
213  tf2Scalar s = tf2Sqrt(length2() * q.length2());
214  tf2Assert(s != tf2Scalar(0.0));
215  return tf2Acos(dot(q) / s);
216  }
219  tf2Scalar angleShortestPath(const Quaternion& q) const
220  {
221  tf2Scalar s = tf2Sqrt(length2() * q.length2());
222  tf2Assert(s != tf2Scalar(0.0));
223  if (dot(q) < 0) // Take care of long angle case see http://en.wikipedia.org/wiki/Slerp
224  return tf2Acos(dot(-q) / s) * tf2Scalar(2.0);
225  else
226  return tf2Acos(dot(q) / s) * tf2Scalar(2.0);
227  }
229  tf2Scalar getAngle() const
230  {
231  tf2Scalar s = tf2Scalar(2.) * tf2Acos(m_floats[3]);
232  return s;
233  }
234 
237  {
238  tf2Scalar s;
239  if (m_floats[3] >= 0)
240  s = tf2Scalar(2.) * tf2Acos(m_floats[3]);
241  else
242  s = tf2Scalar(2.) * tf2Acos(-m_floats[3]);
243 
244  return s;
245  }
246 
248  Vector3 getAxis() const
249  {
250  tf2Scalar s_squared = tf2Scalar(1.) - tf2Pow(m_floats[3], tf2Scalar(2.));
251  if (s_squared < tf2Scalar(10.) * TF2SIMD_EPSILON) //Check for divide by zero
252  return Vector3(1.0, 0.0, 0.0); // Arbitrary
253  tf2Scalar s = tf2Sqrt(s_squared);
254  return Vector3(m_floats[0] / s, m_floats[1] / s, m_floats[2] / s);
255  }
256 
258  Quaternion inverse() const
259  {
260  return Quaternion(-m_floats[0], -m_floats[1], -m_floats[2], m_floats[3]);
261  }
262 
266  operator+(const Quaternion& q2) const
267  {
268  const Quaternion& q1 = *this;
269  return Quaternion(q1.x() + q2.x(), q1.y() + q2.y(), q1.z() + q2.z(), q1.m_floats[3] + q2.m_floats[3]);
270  }
271 
275  operator-(const Quaternion& q2) const
276  {
277  const Quaternion& q1 = *this;
278  return Quaternion(q1.x() - q2.x(), q1.y() - q2.y(), q1.z() - q2.z(), q1.m_floats[3] - q2.m_floats[3]);
279  }
280 
284  {
285  const Quaternion& q2 = *this;
286  return Quaternion( - q2.x(), - q2.y(), - q2.z(), - q2.m_floats[3]);
287  }
290  {
291  Quaternion diff,sum;
292  diff = *this - qd;
293  sum = *this + qd;
294  if( diff.dot(diff) > sum.dot(sum) )
295  return qd;
296  return (-qd);
297  }
298 
301  {
302  Quaternion diff,sum;
303  diff = *this - qd;
304  sum = *this + qd;
305  if( diff.dot(diff) < sum.dot(sum) )
306  return qd;
307  return (-qd);
308  }
309 
310 
315  Quaternion slerp(const Quaternion& q, const tf2Scalar& t) const
316  {
317  tf2Scalar theta = angleShortestPath(q) / tf2Scalar(2.0);
318  if (theta != tf2Scalar(0.0))
319  {
320  tf2Scalar d = tf2Scalar(1.0) / tf2Sin(theta);
321  tf2Scalar s0 = tf2Sin((tf2Scalar(1.0) - t) * theta);
322  tf2Scalar s1 = tf2Sin(t * theta);
323  if (dot(q) < 0) // Take care of long angle case see http://en.wikipedia.org/wiki/Slerp
324  return Quaternion((m_floats[0] * s0 + -q.x() * s1) * d,
325  (m_floats[1] * s0 + -q.y() * s1) * d,
326  (m_floats[2] * s0 + -q.z() * s1) * d,
327  (m_floats[3] * s0 + -q.m_floats[3] * s1) * d);
328  else
329  return Quaternion((m_floats[0] * s0 + q.x() * s1) * d,
330  (m_floats[1] * s0 + q.y() * s1) * d,
331  (m_floats[2] * s0 + q.z() * s1) * d,
332  (m_floats[3] * s0 + q.m_floats[3] * s1) * d);
333 
334  }
335  else
336  {
337  return *this;
338  }
339  }
340 
341  static const Quaternion& getIdentity()
342  {
343  static const Quaternion identityQuat(tf2Scalar(0.),tf2Scalar(0.),tf2Scalar(0.),tf2Scalar(1.));
344  return identityQuat;
345  }
346 
347  TF2SIMD_FORCE_INLINE const tf2Scalar& getW() const { return m_floats[3]; }
348 
349 
350 };
351 
352 
355 operator-(const Quaternion& q)
356 {
357  return Quaternion(-q.x(), -q.y(), -q.z(), -q.w());
358 }
359 
360 
361 
364 operator*(const Quaternion& q1, const Quaternion& q2) {
365  return Quaternion(q1.w() * q2.x() + q1.x() * q2.w() + q1.y() * q2.z() - q1.z() * q2.y(),
366  q1.w() * q2.y() + q1.y() * q2.w() + q1.z() * q2.x() - q1.x() * q2.z(),
367  q1.w() * q2.z() + q1.z() * q2.w() + q1.x() * q2.y() - q1.y() * q2.x(),
368  q1.w() * q2.w() - q1.x() * q2.x() - q1.y() * q2.y() - q1.z() * q2.z());
369 }
370 
372 operator*(const Quaternion& q, const Vector3& w)
373 {
374  return Quaternion( q.w() * w.x() + q.y() * w.z() - q.z() * w.y(),
375  q.w() * w.y() + q.z() * w.x() - q.x() * w.z(),
376  q.w() * w.z() + q.x() * w.y() - q.y() * w.x(),
377  -q.x() * w.x() - q.y() * w.y() - q.z() * w.z());
378 }
379 
381 operator*(const Vector3& w, const Quaternion& q)
382 {
383  return Quaternion( w.x() * q.w() + w.y() * q.z() - w.z() * q.y(),
384  w.y() * q.w() + w.z() * q.x() - w.x() * q.z(),
385  w.z() * q.w() + w.x() * q.y() - w.y() * q.x(),
386  -w.x() * q.x() - w.y() * q.y() - w.z() * q.z());
387 }
388 
391 dot(const Quaternion& q1, const Quaternion& q2)
392 {
393  return q1.dot(q2);
394 }
395 
396 
399 length(const Quaternion& q)
400 {
401  return q.length();
402 }
403 
406 angle(const Quaternion& q1, const Quaternion& q2)
407 {
408  return q1.angle(q2);
409 }
410 
413 angleShortestPath(const Quaternion& q1, const Quaternion& q2)
414 {
415  return q1.angleShortestPath(q2);
416 }
417 
420 inverse(const Quaternion& q)
421 {
422  return q.inverse();
423 }
424 
431 slerp(const Quaternion& q1, const Quaternion& q2, const tf2Scalar& t)
432 {
433  return q1.slerp(q2, t);
434 }
435 
437 quatRotate(const Quaternion& rotation, const Vector3& v)
438 {
439  Quaternion q = rotation * v;
440  q *= rotation.inverse();
441  return Vector3(q.getX(),q.getY(),q.getZ());
442 }
443 
445 shortestArcQuat(const Vector3& v0, const Vector3& v1) // Game Programming Gems 2.10. make sure v0,v1 are normalized
446 {
447  Vector3 c = v0.cross(v1);
448  tf2Scalar d = v0.dot(v1);
449 
450  if (d < -1.0 + TF2SIMD_EPSILON)
451  {
452  Vector3 n,unused;
453  tf2PlaneSpace1(v0,n,unused);
454  return Quaternion(n.x(),n.y(),n.z(),0.0f); // just pick any vector that is orthogonal to v0
455  }
456 
457  tf2Scalar s = tf2Sqrt((1.0f + d) * 2.0f);
458  tf2Scalar rs = 1.0f / s;
459 
460  return Quaternion(c.getX()*rs,c.getY()*rs,c.getZ()*rs,s * 0.5f);
461 }
462 
465 {
466  v0.normalize();
467  v1.normalize();
468  return shortestArcQuat(v0,v1);
469 }
470 
471 }
472 #endif
473 
474 
475 
476 
tf2::Quaternion::operator+
TF2SIMD_FORCE_INLINE Quaternion operator+(const Quaternion &q2) const
tf2::dot
TF2SIMD_FORCE_INLINE tf2Scalar dot(const Quaternion &q1, const Quaternion &q2)
Calculate the dot product between two quaternions.
Definition: melodic/include/tf2/LinearMath/Quaternion.h:391
geometry_msgs::Vector3
::geometry_msgs::Vector3_< std::allocator< void > > Vector3
Definition: kinetic/include/geometry_msgs/Vector3.h:58
tf2::Quaternion::operator/=
Quaternion & operator/=(const tf2Scalar &s)
tf2::Quaternion::getIdentity
static const Quaternion & getIdentity()
tf2::shortestArcQuatNormalize2
TF2SIMD_FORCE_INLINE Quaternion shortestArcQuatNormalize2(Vector3 &v0, Vector3 &v1)
Definition: melodic/include/tf2/LinearMath/Quaternion.h:464
tf2::Quaternion::getW
const TF2SIMD_FORCE_INLINE tf2Scalar & getW() const
tf2::Quaternion::getAngleShortestPath
tf2Scalar getAngleShortestPath() const
s
XmlRpcServer s
TF2SIMD_EPSILON
#define TF2SIMD_EPSILON
Definition: Scalar.h:203
tf2::Quaternion::setRPY
void setRPY(const tf2Scalar &roll, const tf2Scalar &pitch, const tf2Scalar &yaw)
tf2::inverse
TF2SIMD_FORCE_INLINE Quaternion inverse(const Quaternion &q)
Return the inverse of a quaternion.
Definition: melodic/include/tf2/LinearMath/Quaternion.h:420
tf2::angleShortestPath
TF2SIMD_FORCE_INLINE tf2Scalar angleShortestPath(const Quaternion &q1, const Quaternion &q2)
Return the shortest angle between two quaternions.
Definition: melodic/include/tf2/LinearMath/Quaternion.h:413
tf2Sin
TF2SIMD_FORCE_INLINE tf2Scalar tf2Sin(tf2Scalar x)
Definition: Scalar.h:181
tf2::Quaternion::operator-
TF2SIMD_FORCE_INLINE Quaternion operator-() const
tf2::Quaternion::normalize
Quaternion & normalize()
tf2Sqrt
TF2SIMD_FORCE_INLINE tf2Scalar tf2Sqrt(tf2Scalar x)
Definition: Scalar.h:178
tf2::Quaternion::angle
tf2Scalar angle(const Quaternion &q) const
tf2::Quaternion::operator/
Quaternion operator/(const tf2Scalar &s) const
f
f
tf2::Quaternion::Quaternion
Quaternion()
tf2::Quaternion::slerp
Quaternion slerp(const Quaternion &q, const tf2Scalar &t) const
tf2::Quaternion::getAxis
Vector3 getAxis() const
tf2Acos
TF2SIMD_FORCE_INLINE tf2Scalar tf2Acos(tf2Scalar x)
Definition: Scalar.h:183
tf2::Quaternion::operator+=
TF2SIMD_FORCE_INLINE Quaternion & operator+=(const Quaternion &q)
tf2::operator*
TF2SIMD_FORCE_INLINE Vector3 operator*(const Matrix3x3 &m, const Vector3 &v)
Definition: Matrix3x3.h:602
tf2::Quaternion::getAngle
tf2Scalar getAngle() const
Vector3
tf2Assert
#define tf2Assert(x)
Definition: Scalar.h:145
tf2::Quaternion::length
tf2Scalar length() const
QuadWord.h
TF2SIMD_FORCE_INLINE
#define TF2SIMD_FORCE_INLINE
Definition: Scalar.h:130
d
d
tf2::tf2PlaneSpace1
TF2SIMD_FORCE_INLINE void tf2PlaneSpace1(const Vector3 &n, Vector3 &p, Vector3 &q)
Definition: melodic/include/tf2/LinearMath/Vector3.h:657
tf2::Quaternion::farthest
TF2SIMD_FORCE_INLINE Quaternion farthest(const Quaternion &qd) const
tf2::Quaternion::setRotation
void setRotation(const Vector3 &axis, const tf2Scalar &angle)
tf2::Quaternion::operator*=
Quaternion & operator*=(const Quaternion &q)
tf2Cos
TF2SIMD_FORCE_INLINE tf2Scalar tf2Cos(tf2Scalar x)
Definition: Scalar.h:180
tf2::Quaternion::inverse
Quaternion inverse() const
tf2::Quaternion::dot
tf2Scalar dot(const Quaternion &q) const
sick_scan_xd_api_test.c
c
Definition: sick_scan_xd_api_test.py:445
tf2::quatRotate
TF2SIMD_FORCE_INLINE Vector3 quatRotate(const Quaternion &rotation, const Vector3 &v)
Definition: melodic/include/tf2/LinearMath/Quaternion.h:437
tf2::Quaternion::setEuler
void setEuler(const tf2Scalar &yaw, const tf2Scalar &pitch, const tf2Scalar &roll)
tf2
geometry_msgs::Quaternion
::geometry_msgs::Quaternion_< std::allocator< void > > Quaternion
Definition: kinetic/include/geometry_msgs/Quaternion.h:63
sick_scan_base.h
tf2::angle
TF2SIMD_FORCE_INLINE tf2Scalar angle(const Quaternion &q1, const Quaternion &q2)
Return the half angle between two quaternions.
Definition: melodic/include/tf2/LinearMath/Quaternion.h:406
tf2::Quaternion::setEulerZYX
ROS_DEPRECATED void setEulerZYX(const tf2Scalar &yaw, const tf2Scalar &pitch, const tf2Scalar &roll)
tf2::Quaternion::normalized
Quaternion normalized() const
tf2::slerp
TF2SIMD_FORCE_INLINE Quaternion slerp(const Quaternion &q1, const Quaternion &q2, const tf2Scalar &t)
Return the result of spherical linear interpolation betwen two quaternions.
Definition: melodic/include/tf2/LinearMath/Quaternion.h:431
tf2::length
TF2SIMD_FORCE_INLINE tf2Scalar length(const Quaternion &q)
Return the length of a quaternion.
Definition: melodic/include/tf2/LinearMath/Quaternion.h:399
tf2Scalar
double tf2Scalar
tf2::Quaternion::operator*
TF2SIMD_FORCE_INLINE Quaternion operator*(const tf2Scalar &s) const
tf2::operator-
TF2SIMD_FORCE_INLINE Quaternion operator-(const Quaternion &q)
Return the negative of a quaternion.
Definition: melodic/include/tf2/LinearMath/Quaternion.h:355
tf2::Quaternion::nearest
TF2SIMD_FORCE_INLINE Quaternion nearest(const Quaternion &qd) const
tf2Pow
TF2SIMD_FORCE_INLINE tf2Scalar tf2Pow(tf2Scalar x, tf2Scalar y)
Definition: Scalar.h:189
tf2::Quaternion::operator-=
Quaternion & operator-=(const Quaternion &q)
tf2::Quaternion::angleShortestPath
tf2Scalar angleShortestPath(const Quaternion &q) const
t
geometry_msgs::TransformStamped t
Vector3.h
tf2::shortestArcQuat
TF2SIMD_FORCE_INLINE Quaternion shortestArcQuat(const Vector3 &v0, const Vector3 &v1)
Definition: melodic/include/tf2/LinearMath/Quaternion.h:445
tf2::Quaternion::length2
tf2Scalar length2() const


sick_scan_xd
Author(s): Michael Lehning , Jochen Sprickerhof , Martin Günther
autogenerated on Fri Oct 25 2024 02:47:10