tf2  master
tf2 maintains the relationship between coordinate frames in a tree structure buffered in time, and lets the user transform points, vectors, etc between any two coordinate frames at any desired point in time.
Quaternion.h
Go to the documentation of this file.
1 /*
2 Copyright (c) 2003-2006 Gino van den Bergen / Erwin Coumans http://continuousphysics.com/Bullet/
3 
4 This software is provided 'as-is', without any express or implied warranty.
5 In no event will the authors be held liable for any damages arising from the use of this software.
6 Permission is granted to anyone to use this software for any purpose,
7 including commercial applications, and to alter it and redistribute it freely,
8 subject to the following restrictions:
9 
10 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.
11 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
12 3. This notice may not be removed or altered from any source distribution.
13 */
14 
15 
16 
17 #ifndef TF2_QUATERNION_H_
18 #define TF2_QUATERNION_H_
19 
20 
21 #include "Vector3.h"
22 #include "QuadWord.h"
23 #include "tf2/visibility_control.h"
24 
25 namespace tf2
26 {
27 
29 class Quaternion : public QuadWord {
30 public:
34 
35  // template <typename tf2Scalar>
36  // explicit Quaternion(const tf2Scalar *v) : Tuple4<tf2Scalar>(v) {}
39  Quaternion(const tf2Scalar& x, const tf2Scalar& y, const tf2Scalar& z, const tf2Scalar& w)
40  : QuadWord(x, y, z, w)
41  {}
46  Quaternion(const Vector3& axis, const tf2Scalar& angle)
47  {
48  setRotation(axis, angle);
49  }
54  void setRotation(const Vector3& axis, const tf2Scalar& angle)
55  {
56  tf2Scalar d = axis.length();
57  tf2Assert(d != tf2Scalar(0.0));
58  tf2Scalar s = tf2Sin(angle * tf2Scalar(0.5)) / d;
59  setValue(axis.x() * s, axis.y() * s, axis.z() * s,
60  tf2Cos(angle * tf2Scalar(0.5)));
61  }
67  void setEuler(const tf2Scalar& yaw, const tf2Scalar& pitch, const tf2Scalar& roll)
68  {
69  tf2Scalar halfYaw = tf2Scalar(yaw) * tf2Scalar(0.5);
70  tf2Scalar halfPitch = tf2Scalar(pitch) * tf2Scalar(0.5);
71  tf2Scalar halfRoll = tf2Scalar(roll) * tf2Scalar(0.5);
72  tf2Scalar cosYaw = tf2Cos(halfYaw);
73  tf2Scalar sinYaw = tf2Sin(halfYaw);
74  tf2Scalar cosPitch = tf2Cos(halfPitch);
75  tf2Scalar sinPitch = tf2Sin(halfPitch);
76  tf2Scalar cosRoll = tf2Cos(halfRoll);
77  tf2Scalar sinRoll = tf2Sin(halfRoll);
78  setValue(cosRoll * sinPitch * cosYaw + sinRoll * cosPitch * sinYaw,
79  cosRoll * cosPitch * sinYaw - sinRoll * sinPitch * cosYaw,
80  sinRoll * cosPitch * cosYaw - cosRoll * sinPitch * sinYaw,
81  cosRoll * cosPitch * cosYaw + sinRoll * sinPitch * sinYaw);
82  }
88  void setRPY(const tf2Scalar& roll, const tf2Scalar& pitch, const tf2Scalar& yaw)
89  {
90  tf2Scalar halfYaw = tf2Scalar(yaw) * tf2Scalar(0.5);
91  tf2Scalar halfPitch = tf2Scalar(pitch) * tf2Scalar(0.5);
92  tf2Scalar halfRoll = tf2Scalar(roll) * tf2Scalar(0.5);
93  tf2Scalar cosYaw = tf2Cos(halfYaw);
94  tf2Scalar sinYaw = tf2Sin(halfYaw);
95  tf2Scalar cosPitch = tf2Cos(halfPitch);
96  tf2Scalar sinPitch = tf2Sin(halfPitch);
97  tf2Scalar cosRoll = tf2Cos(halfRoll);
98  tf2Scalar sinRoll = tf2Sin(halfRoll);
99  setValue(sinRoll * cosPitch * cosYaw - cosRoll * sinPitch * sinYaw, //x
100  cosRoll * sinPitch * cosYaw + sinRoll * cosPitch * sinYaw, //y
101  cosRoll * cosPitch * sinYaw - sinRoll * sinPitch * cosYaw, //z
102  cosRoll * cosPitch * cosYaw + sinRoll * sinPitch * sinYaw); //formerly yzx
103  }
107  {
108  m_floats[0] += q.x(); m_floats[1] += q.y(); m_floats[2] += q.z(); m_floats[3] += q.m_floats[3];
109  return *this;
110  }
111 
114  TF2_PUBLIC
116  {
117  m_floats[0] -= q.x(); m_floats[1] -= q.y(); m_floats[2] -= q.z(); m_floats[3] -= q.m_floats[3];
118  return *this;
119  }
120 
123  TF2_PUBLIC
125  {
126  m_floats[0] *= s; m_floats[1] *= s; m_floats[2] *= s; m_floats[3] *= s;
127  return *this;
128  }
129 
133  TF2_PUBLIC
135  {
136  setValue(m_floats[3] * q.x() + m_floats[0] * q.m_floats[3] + m_floats[1] * q.z() - m_floats[2] * q.y(),
137  m_floats[3] * q.y() + m_floats[1] * q.m_floats[3] + m_floats[2] * q.x() - m_floats[0] * q.z(),
138  m_floats[3] * q.z() + m_floats[2] * q.m_floats[3] + m_floats[0] * q.y() - m_floats[1] * q.x(),
139  m_floats[3] * q.m_floats[3] - m_floats[0] * q.x() - m_floats[1] * q.y() - m_floats[2] * q.z());
140  return *this;
141  }
144  TF2_PUBLIC
145  tf2Scalar dot(const Quaternion& q) const
146  {
147  return m_floats[0] * q.x() + m_floats[1] * q.y() + m_floats[2] * q.z() + m_floats[3] * q.m_floats[3];
148  }
149 
151  TF2_PUBLIC
153  {
154  return dot(*this);
155  }
156 
158  TF2_PUBLIC
160  {
161  return tf2Sqrt(length2());
162  }
163 
166  TF2_PUBLIC
168  {
169  return *this /= length();
170  }
171 
175  operator*(const tf2Scalar& s) const
176  {
177  return Quaternion(x() * s, y() * s, z() * s, m_floats[3] * s);
178  }
179 
180 
183  TF2_PUBLIC
185  {
186  tf2Assert(s != tf2Scalar(0.0));
187  return *this * (tf2Scalar(1.0) / s);
188  }
189 
192  TF2_PUBLIC
194  {
195  tf2Assert(s != tf2Scalar(0.0));
196  return *this *= tf2Scalar(1.0) / s;
197  }
198 
200  TF2_PUBLIC
202  {
203  return *this / length();
204  }
207  TF2_PUBLIC
208  tf2Scalar angle(const Quaternion& q) const
209  {
210  tf2Scalar s = tf2Sqrt(length2() * q.length2());
211  tf2Assert(s != tf2Scalar(0.0));
212  return tf2Acos(dot(q) / s);
213  }
216  TF2_PUBLIC
218  {
219  tf2Scalar s = tf2Sqrt(length2() * q.length2());
220  tf2Assert(s != tf2Scalar(0.0));
221  if (dot(q) < 0) // Take care of long angle case see http://en.wikipedia.org/wiki/Slerp
222  return tf2Acos(dot(-q) / s) * tf2Scalar(2.0);
223  else
224  return tf2Acos(dot(q) / s) * tf2Scalar(2.0);
225  }
227  TF2_PUBLIC
229  {
230  tf2Scalar s = tf2Scalar(2.) * tf2Acos(m_floats[3]);
231  return s;
232  }
233 
235  TF2_PUBLIC
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  TF2_PUBLIC
249  Vector3 getAxis() const
250  {
251  tf2Scalar s_squared = tf2Scalar(1.) - tf2Pow(m_floats[3], tf2Scalar(2.));
252  if (s_squared < tf2Scalar(10.) * TF2SIMD_EPSILON) //Check for divide by zero
253  return Vector3(1.0, 0.0, 0.0); // Arbitrary
254  tf2Scalar s = tf2Sqrt(s_squared);
255  return Vector3(m_floats[0] / s, m_floats[1] / s, m_floats[2] / s);
256  }
257 
259  TF2_PUBLIC
261  {
262  return Quaternion(-m_floats[0], -m_floats[1], -m_floats[2], m_floats[3]);
263  }
264 
268  operator+(const Quaternion& q2) const
269  {
270  const Quaternion& q1 = *this;
271  return Quaternion(q1.x() + q2.x(), q1.y() + q2.y(), q1.z() + q2.z(), q1.m_floats[3] + q2.m_floats[3]);
272  }
273 
277  operator-(const Quaternion& q2) const
278  {
279  const Quaternion& q1 = *this;
280  return Quaternion(q1.x() - q2.x(), q1.y() - q2.y(), q1.z() - q2.z(), q1.m_floats[3] - q2.m_floats[3]);
281  }
282 
286  {
287  const Quaternion& q2 = *this;
288  return Quaternion( - q2.x(), - q2.y(), - q2.z(), - q2.m_floats[3]);
289  }
292  {
293  Quaternion diff,sum;
294  diff = *this - qd;
295  sum = *this + qd;
296  if( diff.dot(diff) > sum.dot(sum) )
297  return qd;
298  return (-qd);
299  }
300 
303  {
304  Quaternion diff,sum;
305  diff = *this - qd;
306  sum = *this + qd;
307  if( diff.dot(diff) < sum.dot(sum) )
308  return qd;
309  return (-qd);
310  }
311 
312 
317  TF2_PUBLIC
318  Quaternion slerp(const Quaternion& q, const tf2Scalar& t) const
319  {
320  tf2Scalar theta = angleShortestPath(q) / tf2Scalar(2.0);
321  if (theta != tf2Scalar(0.0))
322  {
323  tf2Scalar d = tf2Scalar(1.0) / tf2Sin(theta);
324  tf2Scalar s0 = tf2Sin((tf2Scalar(1.0) - t) * theta);
325  tf2Scalar s1 = tf2Sin(t * theta);
326  if (dot(q) < 0) // Take care of long angle case see http://en.wikipedia.org/wiki/Slerp
327  return Quaternion((m_floats[0] * s0 + -q.x() * s1) * d,
328  (m_floats[1] * s0 + -q.y() * s1) * d,
329  (m_floats[2] * s0 + -q.z() * s1) * d,
330  (m_floats[3] * s0 + -q.m_floats[3] * s1) * d);
331  else
332  return Quaternion((m_floats[0] * s0 + q.x() * s1) * d,
333  (m_floats[1] * s0 + q.y() * s1) * d,
334  (m_floats[2] * s0 + q.z() * s1) * d,
335  (m_floats[3] * s0 + q.m_floats[3] * s1) * d);
336 
337  }
338  else
339  {
340  return *this;
341  }
342  }
343 
344  TF2_PUBLIC
345  static const Quaternion& getIdentity()
346  {
347  static const Quaternion identityQuat(tf2Scalar(0.),tf2Scalar(0.),tf2Scalar(0.),tf2Scalar(1.));
348  return identityQuat;
349  }
350 
351  TF2SIMD_FORCE_INLINE const tf2Scalar& getW() const { return m_floats[3]; }
352 
353 
354 };
355 
356 
358 TF2SIMD_FORCE_INLINE Quaternion
360 {
361  return Quaternion(-q.x(), -q.y(), -q.z(), -q.w());
362 }
363 
364 
365 
367 TF2SIMD_FORCE_INLINE Quaternion
368 operator*(const Quaternion& q1, const Quaternion& q2) {
369  return Quaternion(q1.w() * q2.x() + q1.x() * q2.w() + q1.y() * q2.z() - q1.z() * q2.y(),
370  q1.w() * q2.y() + q1.y() * q2.w() + q1.z() * q2.x() - q1.x() * q2.z(),
371  q1.w() * q2.z() + q1.z() * q2.w() + q1.x() * q2.y() - q1.y() * q2.x(),
372  q1.w() * q2.w() - q1.x() * q2.x() - q1.y() * q2.y() - q1.z() * q2.z());
373 }
374 
375 TF2SIMD_FORCE_INLINE Quaternion
376 operator*(const Quaternion& q, const Vector3& w)
377 {
378  return Quaternion( q.w() * w.x() + q.y() * w.z() - q.z() * w.y(),
379  q.w() * w.y() + q.z() * w.x() - q.x() * w.z(),
380  q.w() * w.z() + q.x() * w.y() - q.y() * w.x(),
381  -q.x() * w.x() - q.y() * w.y() - q.z() * w.z());
382 }
383 
384 TF2SIMD_FORCE_INLINE Quaternion
385 operator*(const Vector3& w, const Quaternion& q)
386 {
387  return Quaternion( w.x() * q.w() + w.y() * q.z() - w.z() * q.y(),
388  w.y() * q.w() + w.z() * q.x() - w.x() * q.z(),
389  w.z() * q.w() + w.x() * q.y() - w.y() * q.x(),
390  -w.x() * q.x() - w.y() * q.y() - w.z() * q.z());
391 }
392 
395 dot(const Quaternion& q1, const Quaternion& q2)
396 {
397  return q1.dot(q2);
398 }
399 
400 
403 length(const Quaternion& q)
404 {
405  return q.length();
406 }
407 
410 angle(const Quaternion& q1, const Quaternion& q2)
411 {
412  return q1.angle(q2);
413 }
414 
418 {
419  return q1.angleShortestPath(q2);
420 }
421 
423 TF2SIMD_FORCE_INLINE Quaternion
424 inverse(const Quaternion& q)
425 {
426  return q.inverse();
427 }
428 
434 TF2SIMD_FORCE_INLINE Quaternion
435 slerp(const Quaternion& q1, const Quaternion& q2, const tf2Scalar& t)
436 {
437  return q1.slerp(q2, t);
438 }
439 
440 TF2SIMD_FORCE_INLINE Vector3
441 quatRotate(const Quaternion& rotation, const Vector3& v)
442 {
443  Quaternion q = rotation * v;
444  q *= rotation.inverse();
445  return Vector3(q.getX(),q.getY(),q.getZ());
446 }
447 
448 TF2SIMD_FORCE_INLINE Quaternion
449 shortestArcQuat(const Vector3& v0, const Vector3& v1) // Game Programming Gems 2.10. make sure v0,v1 are normalized
450 {
451  Vector3 c = v0.cross(v1);
452  tf2Scalar d = v0.dot(v1);
453 
454  if (d < -1.0 + TF2SIMD_EPSILON)
455  {
456  Vector3 n,unused;
457  tf2PlaneSpace1(v0,n,unused);
458  return Quaternion(n.x(),n.y(),n.z(),0.0f); // just pick any vector that is orthogonal to v0
459  }
460 
461  tf2Scalar s = tf2Sqrt((1.0f + d) * 2.0f);
462  tf2Scalar rs = 1.0f / s;
463 
464  return Quaternion(c.getX()*rs,c.getY()*rs,c.getZ()*rs,s * 0.5f);
465 }
466 
467 TF2SIMD_FORCE_INLINE Quaternion
468 shortestArcQuatNormalize2(Vector3& v0,Vector3& v1)
469 {
470  v0.normalize();
471  v1.normalize();
472  return shortestArcQuat(v0,v1);
473 }
474 
475 }
476 #endif
tf2::Quaternion::operator+
TF2SIMD_FORCE_INLINE Quaternion operator+(const Quaternion &q2) const
Return the sum of this quaternion and the other.
Definition: Quaternion.h:268
tf2::dot
TF2SIMD_FORCE_INLINE tf2Scalar dot(const Quaternion &q1, const Quaternion &q2)
Calculate the dot product between two quaternions.
Definition: Quaternion.h:395
tf2::Quaternion::operator/=
Quaternion & operator/=(const tf2Scalar &s)
Inversely scale this quaternion.
Definition: Quaternion.h:193
tf2::Quaternion::getIdentity
static const Quaternion & getIdentity()
Definition: Quaternion.h:345
tf2::shortestArcQuatNormalize2
TF2SIMD_FORCE_INLINE Quaternion shortestArcQuatNormalize2(Vector3 &v0, Vector3 &v1)
Definition: Quaternion.h:468
tf2::Quaternion::getW
const TF2SIMD_FORCE_INLINE tf2Scalar & getW() const
Definition: Quaternion.h:351
tf2::Quaternion::getAngleShortestPath
tf2Scalar getAngleShortestPath() const
Return the angle [0, Pi] of rotation represented by this quaternion along the shortest path.
Definition: Quaternion.h:236
TF2SIMD_EPSILON
#define TF2SIMD_EPSILON
Definition: Scalar.h:202
tf2::Quaternion::setRPY
void setRPY(const tf2Scalar &roll, const tf2Scalar &pitch, const tf2Scalar &yaw)
Set the quaternion using fixed axis RPY.
Definition: Quaternion.h:88
tf2::inverse
TF2SIMD_FORCE_INLINE Quaternion inverse(const Quaternion &q)
Return the inverse of a quaternion.
Definition: Quaternion.h:424
tf2::angleShortestPath
TF2SIMD_FORCE_INLINE tf2Scalar angleShortestPath(const Quaternion &q1, const Quaternion &q2)
Return the shortest angle between two quaternions.
Definition: Quaternion.h:417
tf2Sin
TF2SIMD_FORCE_INLINE tf2Scalar tf2Sin(tf2Scalar x)
Definition: Scalar.h:180
tf2::Quaternion::operator-
TF2SIMD_FORCE_INLINE Quaternion operator-() const
Return the negative of this quaternion This simply negates each element.
Definition: Quaternion.h:285
tf2::Quaternion::normalize
Quaternion & normalize()
Normalize the quaternion Such that x^2 + y^2 + z^2 +w^2 = 1.
Definition: Quaternion.h:167
tf2Sqrt
TF2SIMD_FORCE_INLINE tf2Scalar tf2Sqrt(tf2Scalar x)
Definition: Scalar.h:177
tf2::Quaternion::angle
tf2Scalar angle(const Quaternion &q) const
Return the half angle between this quaternion and the other.
Definition: Quaternion.h:208
tf2::Quaternion::operator/
Quaternion operator/(const tf2Scalar &s) const
Return an inversely scaled versionof this quaternion.
Definition: Quaternion.h:184
tf2::Quaternion::operator-
TF2SIMD_FORCE_INLINE Quaternion operator-(const Quaternion &q2) const
Return the difference between this quaternion and the other.
Definition: Quaternion.h:277
tf2::Quaternion::Quaternion
Quaternion()
No initialization constructor.
Definition: Quaternion.h:33
tf2::Quaternion::slerp
Quaternion slerp(const Quaternion &q, const tf2Scalar &t) const
Return the quaternion which is the result of Spherical Linear Interpolation between this and the othe...
Definition: Quaternion.h:318
tf2::Quaternion::Quaternion
Quaternion(const tf2Scalar &x, const tf2Scalar &y, const tf2Scalar &z, const tf2Scalar &w)
Constructor from scalars.
Definition: Quaternion.h:39
tf2::Quaternion::getAxis
Vector3 getAxis() const
Return the axis of the rotation represented by this quaternion.
Definition: Quaternion.h:249
tf2Acos
TF2SIMD_FORCE_INLINE tf2Scalar tf2Acos(tf2Scalar x)
Definition: Scalar.h:182
tf2::Quaternion::operator+=
TF2SIMD_FORCE_INLINE Quaternion & operator+=(const Quaternion &q)
Add two quaternions.
Definition: Quaternion.h:106
tf2::operator*
TF2SIMD_FORCE_INLINE Vector3 operator*(const Matrix3x3 &m, const Vector3 &v)
Definition: Matrix3x3.h:611
tf2::Quaternion::getAngle
tf2Scalar getAngle() const
Return the angle [0, 2Pi] of rotation represented by this quaternion.
Definition: Quaternion.h:228
tf2Assert
#define tf2Assert(x)
Definition: Scalar.h:144
tf2::Quaternion::length
tf2Scalar length() const
Return the length of the quaternion.
Definition: Quaternion.h:159
QuadWord.h
TF2SIMD_FORCE_INLINE
#define TF2SIMD_FORCE_INLINE
Definition: Scalar.h:129
tf2::tf2PlaneSpace1
TF2SIMD_FORCE_INLINE void tf2PlaneSpace1(const Vector3 &n, Vector3 &p, Vector3 &q)
Definition: Vector3.h:660
tf2::Quaternion::farthest
TF2SIMD_FORCE_INLINE Quaternion farthest(const Quaternion &qd) const
Definition: Quaternion.h:291
tf2::Quaternion::setRotation
void setRotation(const Vector3 &axis, const tf2Scalar &angle)
Set the rotation using axis angle notation.
Definition: Quaternion.h:54
visibility_control.h
tf2::Quaternion::operator*=
Quaternion & operator*=(const Quaternion &q)
Multiply this quaternion by q on the right.
Definition: Quaternion.h:134
tf2Cos
TF2SIMD_FORCE_INLINE tf2Scalar tf2Cos(tf2Scalar x)
Definition: Scalar.h:179
tf2::Quaternion::inverse
Quaternion inverse() const
Return the inverse of this quaternion.
Definition: Quaternion.h:260
tf2::Quaternion::dot
tf2Scalar dot(const Quaternion &q) const
Return the dot product between this quaternion and another.
Definition: Quaternion.h:145
tf2::quatRotate
TF2SIMD_FORCE_INLINE Vector3 quatRotate(const Quaternion &rotation, const Vector3 &v)
Definition: Quaternion.h:441
tf2::Quaternion::setEuler
void setEuler(const tf2Scalar &yaw, const tf2Scalar &pitch, const tf2Scalar &roll)
Set the quaternion using Euler angles.
Definition: Quaternion.h:67
Vector3.h
tf2
Definition: buffer_core.h:55
TF2_PUBLIC
#define TF2_PUBLIC
Definition: visibility_control.h:58
tf2::angle
TF2SIMD_FORCE_INLINE tf2Scalar angle(const Quaternion &q1, const Quaternion &q2)
Return the half angle between two quaternions.
Definition: Quaternion.h:410
tf2::Quaternion
The Quaternion implements quaternion to perform linear algebra rotations in combination with Matrix3x...
Definition: Quaternion.h:29
tf2::Quaternion::normalized
Quaternion normalized() const
Return a normalized version of this quaternion.
Definition: Quaternion.h:201
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: Quaternion.h:435
tf2::length
TF2SIMD_FORCE_INLINE tf2Scalar length(const Quaternion &q)
Return the length of a quaternion.
Definition: Quaternion.h:403
tf2Scalar
double tf2Scalar
The tf2Scalar type abstracts floating point numbers, to easily switch between double and single float...
Definition: Scalar.h:159
tf2::Quaternion::operator*
TF2SIMD_FORCE_INLINE Quaternion operator*(const tf2Scalar &s) const
Return a scaled version of this quaternion.
Definition: Quaternion.h:175
tf2::operator-
TF2SIMD_FORCE_INLINE Quaternion operator-(const Quaternion &q)
Return the negative of a quaternion.
Definition: Quaternion.h:359
tf2::Quaternion::nearest
TF2SIMD_FORCE_INLINE Quaternion nearest(const Quaternion &qd) const
Definition: Quaternion.h:302
tf2::Quaternion::operator*=
Quaternion & operator*=(const tf2Scalar &s)
Scale this quaternion.
Definition: Quaternion.h:124
tf2Pow
TF2SIMD_FORCE_INLINE tf2Scalar tf2Pow(tf2Scalar x, tf2Scalar y)
Definition: Scalar.h:188
tf2::Quaternion::operator-=
Quaternion & operator-=(const Quaternion &q)
Sutf2ract out a quaternion.
Definition: Quaternion.h:115
tf2::Quaternion::angleShortestPath
tf2Scalar angleShortestPath(const Quaternion &q) const
Return the angle between this quaternion and the other along the shortest path.
Definition: Quaternion.h:217
tf2::shortestArcQuat
TF2SIMD_FORCE_INLINE Quaternion shortestArcQuat(const Vector3 &v0, const Vector3 &v1)
Definition: Quaternion.h:449
tf2::Quaternion::length2
tf2Scalar length2() const
Return the length squared of the quaternion.
Definition: Quaternion.h:152
tf2::Quaternion::Quaternion
Quaternion(const Vector3 &axis, const tf2Scalar &angle)
Axis angle Constructor.
Definition: Quaternion.h:46