Lerp.h

Go to the documentation of this file.

/*
 * Copyright (C) 2012 by
 *   MetraLabs GmbH (MLAB), GERMANY
 * and
 *   Neuroinformatics and Cognitive Robotics Labs (NICR) at TU Ilmenau, GERMANY
 * All rights reserved.
 *
 * Contact: info@mira-project.org
 *
 * Commercial Usage:
 *   Licensees holding valid commercial licenses may use this file in
 *   accordance with the commercial license agreement provided with the
 *   software or, alternatively, in accordance with the terms contained in
 *   a written agreement between you and MLAB or NICR.
 *
 * GNU General Public License Usage:
 *   Alternatively, this file may be used under the terms of the GNU
 *   General Public License version 3.0 as published by the Free Software
 *   Foundation and appearing in the file LICENSE.GPL3 included in the
 *   packaging of this file. Please review the following information to
 *   ensure the GNU General Public License version 3.0 requirements will be
 *   met: http://www.gnu.org/copyleft/gpl.html.
 *   Alternatively you may (at your option) use any later version of the GNU
 *   General Public License if such license has been publicly approved by
 *   MLAB and NICR (or its successors, if any).
 *
 * IN NO EVENT SHALL "MLAB" OR "NICR" BE LIABLE TO ANY PARTY FOR DIRECT,
 * INDIRECT, SPECIAL, INCIDENTAL, OR CONSEQUENTIAL DAMAGES ARISING OUT OF
 * THE USE OF THIS SOFTWARE AND ITS DOCUMENTATION, EVEN IF "MLAB" OR
 * "NICR" HAS BEEN ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 *
 * "MLAB" AND "NICR" SPECIFICALLY DISCLAIM ANY WARRANTIES, INCLUDING,
 * BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND
 * FITNESS FOR A PARTICULAR PURPOSE. THE SOFTWARE PROVIDED HEREUNDER IS
 * ON AN "AS IS" BASIS, AND "MLAB" AND "NICR" HAVE NO OBLIGATION TO
 * PROVIDE MAINTENANCE, SUPPORT, UPDATES, ENHANCEMENTS OR MODIFICATIONS.
 */

#ifndef _MIRA_LERP_H_
#define _MIRA_LERP_H_

#include <platform/Platform.h>

#include <Eigen/Eigen>
#include <Eigen/Geometry>

#include <math/Angle.h>

namespace mira {


template<typename T, typename S>
inline T lerp(const T& a, const T& b, S alpha)
{
    return (1-alpha) * a + alpha * b;
}

// specialization for other types follow:

template <typename T, typename UnitTag, typename Derived, typename S>
inline Derived lerpAngle(const AngleBase<T,UnitTag,Derived>& a,
                         const AngleBase<T,UnitTag,Derived>& b,
                         S alpha)
{
    T diff = b.smallestDifferenceValue(a);
    return Derived(a.value() + diff*alpha);
}

// we have to write the specializations for each angle type explicitly.
// Renaming the above lerpAngle method to lerp() will not work, since the
// compilers would then choose the generic lerp() method as best matching
// method.

template<typename T, typename S>
inline Degree<T> lerp(const Degree<T>& a, const Degree<T>& b, S alpha) {
    return lerpAngle(a,b,alpha);
}

template<typename T, typename S>
inline SignedDegree<T> lerp(const SignedDegree<T>& a,
                            const SignedDegree<T>& b, S alpha) {
    return lerpAngle(a,b,alpha);
}

template<typename T, typename S>
inline Radian<T> lerp(const Radian<T>& a, const Radian<T>& b, S alpha) {
    return lerpAngle(a,b,alpha);
}

template<typename T, typename S>
inline SignedRadian<T> lerp(const SignedRadian<T>& a,
                            const SignedRadian<T>& b, S alpha) {
    return lerpAngle(a,b,alpha);
}

template<typename T, typename S>
inline Angle<T> lerp(const Angle<T>& a, const Angle<T>& b, S alpha) {
    return lerpAngle(a,b,alpha);
}

template<typename T, typename S>
inline SignedAngle<T> lerp(const SignedAngle<T>& a,
                           const SignedAngle<T>& b, S alpha) {
    return lerpAngle(a,b,alpha);
}


template<typename T, typename S>
inline Eigen::Rotation2D<T> lerp(const Eigen::Rotation2D<T>& a,
                                 const Eigen::Rotation2D<T>& b,
                                 S alpha)
{
    T result = lerp(SignedAngle<T>(a.angle()),
                    SignedAngle<T>(b.angle()),
                    alpha).value();
    return Eigen::Rotation2D<T>(result);
}

template<typename T, typename S>
inline Eigen::Quaternion<T> lerp(const Eigen::Quaternion<T>& a,
                                 const Eigen::Quaternion<T>& b,
                                 S alpha)
{
    return a.slerp(alpha, b);
}


} // namespace

#endif