MRPT  1.9.9
slerp.cpp
Go to the documentation of this file.
1 /* +------------------------------------------------------------------------+
2  | Mobile Robot Programming Toolkit (MRPT) |
3  | https://www.mrpt.org/ |
4  | |
5  | Copyright (c) 2005-2019, Individual contributors, see AUTHORS file |
6  | See: https://www.mrpt.org/Authors - All rights reserved. |
7  | Released under BSD License. See: https://www.mrpt.org/License |
8  +------------------------------------------------------------------------+ */
9 
10 #include "math-precomp.h" // Precompiled headers
11 
12 #include <mrpt/math/slerp.h>
13 
14 using namespace mrpt;
15 using namespace mrpt::math;
16 
18  const TPose3D& p0, const TPose3D& p1, const double t, TPose3D& p)
19 {
20  CQuaternionDouble q0, q1, q;
21  p0.getAsQuaternion(q0);
22  p1.getAsQuaternion(q1);
23  // The quaternion part (this will raise exception on t not in [0,1])
24  mrpt::math::slerp(q0, q1, t, q);
25  q.rpy(p.roll, p.pitch, p.yaw);
26  // XYZ:
27  p.x = (1 - t) * p0.x + t * p1.x;
28  p.y = (1 - t) * p0.y + t * p1.y;
29  p.z = (1 - t) * p0.z + t * p1.z;
30 }
31 
33  const mrpt::math::TPose3D& q0, const mrpt::math::TPose3D& q1,
34  const double t, mrpt::math::TPose3D& p)
35 {
38  q0.getAsQuaternion(quat0);
39  q1.getAsQuaternion(quat1);
40  mrpt::math::slerp(quat0, quat1, t, q);
41 
42  p.x = p.y = p.z = 0;
43  q.rpy(p.roll, p.pitch, p.yaw);
44 }
GLdouble GLdouble t
Definition: glext.h:3695
void slerp_ypr(const mrpt::math::TPose3D &q0, const mrpt::math::TPose3D &q1, const double t, mrpt::math::TPose3D &p)
Definition: slerp.cpp:32
void slerp(const CQuaternion< T > &q0, const CQuaternion< T > &q1, const double t, CQuaternion< T > &q)
SLERP interpolation between two quaternions.
Definition: slerp.h:32
GLdouble GLdouble GLdouble GLdouble q
Definition: glext.h:3727
void getAsQuaternion(mrpt::math::CQuaternion< double > &q, mrpt::optional_ref< mrpt::math::CMatrixFixed< double, 4, 3 >> out_dq_dr=std::nullopt) const
Returns the quaternion associated to the rotation of this object (NOTE: XYZ translation is ignored) ...
double x
X,Y,Z, coords.
Definition: TPose3D.h:32
This base provides a set of functions for maths stuff.
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
Lightweight 3D pose (three spatial coordinates, plus three angular coordinates).
Definition: TPose3D.h:24
A quaternion, which can represent a 3D rotation as pair , with a real part "r" and a 3D vector ...
Definition: CQuaternion.h:44
GLfloat GLfloat p
Definition: glext.h:6398



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: cb8dd5fc8 Sat Dec 7 21:55:39 2019 +0100 at sáb dic 7 22:00:13 CET 2019