Main MRPT website > C++ reference for MRPT 1.5.6
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
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
double roll
Roll coordinate (rotation angle over X coordinate).
GLdouble GLdouble t
Definition: glew.h:1303
double yaw
Yaw coordinate (rotation angle over Z axis).
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
double x() const
Common members of all points & poses classes.
Definition: CPoseOrPoint.h:113
void rpy(T &roll, T &pitch, T &yaw) const
Return the yaw, pitch & roll angles associated to quaternion.
Definition: CQuaternion.h:317
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
GLfloat GLfloat p
Definition: glew.h:10113
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) ...
double pitch
Pitch coordinate (rotation angle over Y axis).
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).
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
A quaternion, which can represent a 3D rotation as pair , with a real part "r" and a 3D vector ...
Definition: CQuaternion.h:43
GLdouble GLdouble GLdouble GLdouble q
Definition: glew.h:1319
double z
X,Y,Z, coords.



Page generated by Doxygen 1.8.6 for MRPT 1.5.6 Git: 4c65e84 Tue Apr 24 08:18:17 2018 +0200 at mar abr 24 08:26:17 CEST 2018