pose.h

Go to the documentation of this file.

#ifndef MF_POSE_H_
#define MF_POSE_H_

#include "../eigen.h"
#include <ostream>
#include <string>
#include "angle.h"

namespace mf {


class pose {
public:
    Eigen_vec3 position; 
    Eigen_quaternion orientation; 
    // TODO make private, enforce normalization, change to matrix?

    pose();
        
    pose(const pose&) = default;
    

    pose(const Eigen_affine3&);
    
    template<typename Translation, typename Rotation>
    pose(const Translation& t, const Rotation& r) :
        position(t),
        orientation(r) { orientation.normalize(); } 
    
    Eigen_affine3 transformation_from_world() const;

    Eigen_affine3 transformation_to_world() const;
    
    Eigen_affine3 transformation_from(const pose& ps) const {
        return transformation_from_world() * ps.transformation_to_world();
    }
    
    Eigen_affine3 transformation_to(const pose& ps) const {
        return ps.transformation_from_world() * transformation_to_world();
    }
        
    Eigen_vec3 euler_angles(std::ptrdiff_t a0 = 0, std::ptrdiff_t a1 = 1, std::ptrdiff_t a2 = 2) const;
    
    std::string to_string() const;
    static pose from_string(const std::string&);
        
    Eigen_vec3 transform_from_world(const Eigen::Vector3f& p) const {
        return transformation_from_world() * p;
    }
    
    Eigen_vec3 transform_to_world(const Eigen::Vector3f& p) const {
        return transformation_to_world() * p;
    }
    
    void look_at(const Eigen_vec3&);
    
    void flip(const Eigen_vec3& axis = Eigen_vec3::UnitY());
    void invert_orientation();
};


std::ostream& operator<<(std::ostream&, const pose&);

}

#endif