Main MRPT website > C++ reference for MRPT 1.5.7
slerp.cpp
Go to the documentation of this file.
1 /* +---------------------------------------------------------------------------+
2  | Mobile Robot Programming Toolkit (MRPT) |
3  | http://www.mrpt.org/ |
4  | |
5  | Copyright (c) 2005-2017, Individual contributors, see AUTHORS file |
6  | See: http://www.mrpt.org/Authors - All rights reserved. |
7  | Released under BSD License. See details in http://www.mrpt.org/License |
8  +---------------------------------------------------------------------------+ */
9 
10 #include "base-precomp.h" // Precompiled headers
11 
12 #include <mrpt/math/slerp.h>
13 #include <mrpt/poses/CPose3D.h>
14 #include <mrpt/poses/CPose3DQuat.h>
15 
16 using namespace mrpt;
17 using namespace mrpt::poses;
18 using namespace mrpt::math;
19 
20 
22  const CPose3D & p0,
23  const CPose3D & p1,
24  const double t,
25  CPose3D & p)
26 {
27  CQuaternionDouble q0,q1,q;
28  p0.getAsQuaternion(q0);
29  p1.getAsQuaternion(q1);
30  // The quaternion part (this will raise exception on t not in [0,1])
31  mrpt::math::slerp(q0,q1,t, q);
32  // XYZ:
33  p = CPose3D(
34  q,
35  (1-t)*p0.x()+t*p1.x(),
36  (1-t)*p0.y()+t*p1.y(),
37  (1-t)*p0.z()+t*p1.z() );
38 }
39 
41  const CPose3DQuat & q0,
42  const CPose3DQuat & q1,
43  const double t,
44  CPose3DQuat & q)
45 {
46  // The quaternion part (this will raise exception on t not in [0,1])
47  mrpt::math::slerp(q0.quat(), q1.quat(),t, q.quat());
48  // XYZ:
49  q.x( (1-t)*q0.x()+t*q1.x() );
50  q.y( (1-t)*q0.y()+t*q1.y() );
51  q.z( (1-t)*q0.z()+t*q1.z() );
52 }
53 
55 {
57  q0.getAsQuaternion(quat0);
58  q1.getAsQuaternion(quat1);
59  mrpt::math::slerp(quat0, quat1, t, q);
60 
61  p.x = p.y = p.z = 0;
62  q.rpy(p.roll, p.pitch, p.yaw);
63 }
64 
mrpt::math::CQuaternionDouble & quat()
Read/Write access to the quaternion representing the 3D rotation.
Definition: CPose3DQuat.h:52
double x() const
Common members of all points & poses classes.
Definition: CPoseOrPoint.h:113
GLdouble GLdouble t
Definition: glext.h:3610
void slerp(const CQuaternion< T > &q0, const CQuaternion< T > &q1, const double t, CQuaternion< T > &q)
SLERP interpolation between two quaternions.
Definition: slerp.h:35
GLdouble GLdouble GLdouble GLdouble q
Definition: glext.h:3626
void getAsQuaternion(mrpt::math::CQuaternion< double > &q, mrpt::math::CMatrixFixedNumeric< double, 4, 3 > *out_dq_dr=NULL) const
Returns the quaternion associated to the rotation of this object (NOTE: XYZ translation is ignored) ...
void BASE_IMPEXP slerp_ypr(const mrpt::math::TPose3D &q0, const mrpt::math::TPose3D &q1, const double t, mrpt::math::TPose3D &p)
Definition: slerp.cpp:54
This base provides a set of functions for maths stuff.
Definition: CArrayNumeric.h:19
A class used to store a 3D pose as a translation (x,y,z) and a quaternion (qr,qx,qy,qz).
Definition: CPose3DQuat.h:41
Classes for 2D/3D geometry representation, both of single values and probability density distribution...
Definition: CPoint.h:17
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:72
Lightweight 3D pose (three spatial coordinates, plus three angular coordinates).
A quaternion, which can represent a 3D rotation as pair , with a real part "r" and a 3D vector ...
Definition: CQuaternion.h:43
GLfloat GLfloat p
Definition: glext.h:5587
void getAsQuaternion(mrpt::math::CQuaternionDouble &q, mrpt::math::CMatrixFixedNumeric< double, 4, 3 > *out_dq_dr=NULL) const
Returns the quaternion associated to the rotation of this object (NOTE: XYZ translation is ignored) ...
Definition: CPose3D.cpp:553



Page generated by Doxygen 1.8.14 for MRPT 1.5.7 Git: 5902e14cc Wed Apr 24 15:04:01 2019 +0200 at lun oct 28 01:39:17 CET 2019