pose.h

Go to the documentation of this file.

 
#pragma once
 
#include "./point.h"
#include "./quaternion.h"
 
#include <utility>
 
namespace emlabcpp
{
 
struct pose_distance
{
        float dist;
        float angle_dist;
};
 
 
constexpr std::size_t steps( pose_distance dist, float dist_step, float angle_step )
{
        auto d_steps = std::size_t( 1.f + dist.dist / dist_step );
        auto a_steps = std::size_t( 1.f + dist.angle_dist / angle_step );
        return std::max( d_steps, a_steps );
}
 
struct pose
{
 
        point< 3 > position;
        quaternion orientation;
 
        constexpr pose()
          : position( 0, 0, 0 )
          , orientation( neutral_quat )
        {
        }
 
        explicit constexpr pose( point< 3 > const& position )
          : position( position )
          , orientation( neutral_quat )
        {
        }
 
        explicit constexpr pose( quaternion const& orientation )
          : position( 0, 0, 0 )
          , orientation( orientation )
        {
        }
 
        constexpr pose( point< 3 > const& position, quaternion const& orientation )
          : position( position )
          , orientation( orientation )
        {
        }
};
 
constexpr bool operator<( pose const& x, pose const& y )
{
        if ( x.position == y.position )
                return x.orientation < y.orientation;
        return x.position < y.position;
}
 
constexpr bool operator==( pose const& x, pose const& y )
{
        return x.position == y.position && x.orientation == y.orientation;
}
 
constexpr bool operator!=( pose const& x, pose const& y )
{
        return !( x == y );
}
 
constexpr pose_distance distance_of( pose const& x, pose const& y )
{
        return {
            distance_of( x.position, y.position ),
            angle_shortest_path( x.orientation, y.orientation ) };
}
 
constexpr pose lin_interp( pose const& from, pose const& goal, float factor )
{
        return pose{
            lin_interp( from.position, goal.position, factor ),
            slerp( from.orientation, goal.orientation, factor ) };
}
 
inline std::vector< pose >
lineary_interpolate_path( std::vector< pose > const& ipath, float d_step, float a_step )
{
        std::vector< pose > res;
        if ( ipath.empty() )
                return res;
        for ( std::size_t const i : range( ipath.size() - 1 ) ) {
                pose const&       from      = ipath[i];
                pose const&       to        = ipath[i + 1];
                std::size_t const seg_steps = steps( distance_of( to, from ), d_step, a_step );
                for ( std::size_t const j : range( seg_steps ) )
                        res.push_back( lin_interp( from, to, float( j ) / float( seg_steps ) ) );
        }
        res.push_back( ipath.back() );
        return res;
}
 
constexpr point< 3 > transform( point< 3 > const& a, pose const& transformation )
{
        return rotate( a, transformation.orientation ) + vector_cast( transformation.position );
}
 
constexpr vector< 3 > transform( vector< 3 > const& v, pose const& transformation )
{
        return rotate( v, transformation.orientation ) + vector_cast( transformation.position );
}
 
constexpr pose transform( pose const& x, pose const& transformation )
{
        return pose{
            transform( x.position, transformation ), transformation.orientation * x.orientation };
}
 
template < typename T >
constexpr auto transform( min_max< T > const& mm, pose const& transformation )
{
        return min_max{ transform( mm.min, transformation ), transform( mm.max, transformation ) };
}
 
constexpr pose inverse( pose const& x )
{
        return pose{ -x.position, inverse( x.orientation ) };
}
 
template < typename T >
constexpr auto inverse_transform( T const& item, pose const& transformation )
{
        return transform( item, inverse( transformation ) );
}
 
constexpr pose rotate( pose const& x, quaternion const& quat )
{
        return pose{ rotate( x.position, quat ), quat * x.orientation };
}
 
}  // namespace emlabcpp