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 }
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
double roll
Roll coordinate (rotation angle over X coordinate).
Definition: TPose3D.h:38
double x
X,Y,Z, coords.
Definition: TPose3D.h:32
double yaw
Yaw coordinate (rotation angle over Z axis).
Definition: TPose3D.h:34
This base provides a set of functions for maths stuff.
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) ...
Definition: TPose3D.cpp:42
double pitch
Pitch coordinate (rotation angle over Y axis).
Definition: TPose3D.h:36
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
void rpy(T &roll, T &pitch, T &yaw) const
Return the yaw, pitch & roll angles associated to quaternion.
Definition: CQuaternion.h:433



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: 24b95e159 Thu Jan 23 01:15:46 2020 +0100 at jue ene 23 01:30:10 CET 2020