slerp.cpp

Go to the documentation of this file.

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

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

#include <mrpt/math/slerp.h>
#include <mrpt/poses/CPose3D.h>
#include <mrpt/poses/CPose3DQuat.h>

using namespace mrpt;
using namespace mrpt::poses;
using namespace mrpt::math;


void mrpt::math::slerp(
        const CPose3D  & p0,
        const CPose3D  & p1,
        const double     t,
        CPose3D        & 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);
        // XYZ:
        p = CPose3D(
                q,
                (1-t)*p0.x()+t*p1.x(),
                (1-t)*p0.y()+t*p1.y(),
                (1-t)*p0.z()+t*p1.z() );
}

void mrpt::math::slerp(
        const CPose3DQuat & q0,
        const CPose3DQuat & q1,
        const double        t,
        CPose3DQuat       & q)
{
        // The quaternion part (this will raise exception on t not in [0,1])
        mrpt::math::slerp(q0.quat(), q1.quat(),t, q.quat());
        // XYZ:
        q.x( (1-t)*q0.x()+t*q1.x() );
        q.y( (1-t)*q0.y()+t*q1.y() );
        q.z( (1-t)*q0.z()+t*q1.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);
}