slerp.cpp

Go to the documentation of this file.

/* +------------------------------------------------------------------------+
   |                     Mobile Robot Programming Toolkit (MRPT)            |
   |                          https://www.mrpt.org/                         |
   |                                                                        |
   | Copyright (c) 2005-2020, Individual contributors, see AUTHORS file     |
   | See: https://www.mrpt.org/Authors - All rights reserved.               |
   | Released under BSD License. See: https://www.mrpt.org/License          |
   +------------------------------------------------------------------------+ */

#include "math-precomp.h"  // Precompiled headers

#include <mrpt/math/slerp.h>

using namespace mrpt;
using namespace mrpt::math;

void mrpt::math::slerp(
    const TPose3D& p0, const TPose3D& p1, const double t, TPose3D& p)
{
    CQuaternionDouble q0, q1, q;
    p0.getAsQuaternion(q0);
    p1.getAsQuaternion(q1);
    // The quaternion part (this will raise exception on t not in [0,1])
    mrpt::math::slerp(q0, q1, t, q);
    q.rpy(p.roll, p.pitch, p.yaw);
    // XYZ:
    p.x = (1 - t) * p0.x + t * p1.x;
    p.y = (1 - t) * p0.y + t * p1.y;
    p.z = (1 - t) * p0.z + t * p1.z;
}

void mrpt::math::slerp_ypr(
    const mrpt::math::TPose3D& q0, const mrpt::math::TPose3D& q1,
    const double t, mrpt::math::TPose3D& p)
{
    mrpt::math::CQuaternionDouble quat0(UNINITIALIZED_QUATERNION),
        quat1(UNINITIALIZED_QUATERNION), q(UNINITIALIZED_QUATERNION);
    q0.getAsQuaternion(quat0);
    q1.getAsQuaternion(quat1);
    mrpt::math::slerp(quat0, quat1, t, q);

    p.x = p.y = p.z = 0;
    q.rpy(p.roll, p.pitch, p.yaw);
}