Pose.hpp

Go to the documentation of this file.

#ifndef SPIDAR_POSE_HPP
#define SPIDAR_POSE_HPP

namespace Spidar
{
namespace Core
{
template <class V, class Q>
struct Pose
{
    typedef V   VectorType;             
    typedef Q   QuaternionType;         

    VectorType      position;           
    QuaternionType  rotation;           
    VectorType      velocity;           
    VectorType      angularVelocity;    

    void clear(void)
    {
        position = VectorType();
        rotation = QuaternionType();
        velocity = VectorType();
        angularVelocity = VectorType();
    }

    void lerp(const Pose& from, const Pose& to, double t, Pose& result);

}; // end of struct Pose.

template <class V, class Q>
void Pose<V,Q>::lerp(const Pose<V,Q>& from, const Pose<V,Q>& to, double t, Pose<V,Q>& result)
{
    typedef V VectorType;
    typedef Q QuaternionType;

    if (t<0)    t = 0;
    if (t>1)    t = 1;

    result.position = VectorType::lerp(from.position, to.position, t);
    result.velocity = VectorType::lerp(from.velocity, to.velocity, t);
    result.rotation = QuaternionType::slerp(from.rotation, to.rotation, t);
    result.angularVelocity = VectorType::lerp(from.angularVelocity, to.angularVelocity, t);
}

} // end of namespace Core.
} // end of namespace Spidar.

#endif // SPIDAR_POSE_HPP

// end of file.