Geometry.h

#ifndef GEOMETRY_H
#define GEOMETRY_H

#include "P3d-pkg.h"
#include "API/Device/robot.hpp"
#include <Eigen/Eigen>

namespace m3dGeometry
{


    p3d_point eigenToP3d(const Eigen::Vector3d & v);
    p3d_matrix4* eigenToP3d(const Eigen::Affine3d & f);




    Eigen::Vector3d p3dToEigen(const p3d_point v);
    Eigen::Affine3d p3dToEigen(const p3d_matrix4 pos);



    p3d_point getTranslation(const p3d_matrix4 t);

    Eigen::Vector2d getConfBase2DPos(const Configuration &q);

    void setBaseOrientation(Robot* rob,const Eigen::Matrix3d & rot_mat);
    void setBaseOrientation(Robot *rob, const Eigen::Vector3d & angles);
    void setBasePosition(Robot *rob, const Eigen::Vector3d & pos);
    void setBasePosition2D(Robot *rob, const Eigen::Vector2d & pos);
    void setBase6D(Robot *rob,const Eigen::Vector3d &pos, const Eigen::Vector3d &angles);
    void setBase4D(Robot *rob, const Eigen::Vector3d &pos, double theta);
    void setBaseTransform(Robot* rob, const Eigen::Affine3d & tf);

    void setBaseFromConf(Robot *rob, const Configuration & conf);


    Eigen::Affine3d changeFrame(const Eigen::Affine3d &posF1,
                                const Eigen::Affine3d &F1,
                                const Eigen::Affine3d &F2);
    Eigen::Vector3d changeFramePoint(const Eigen::Vector3d p1,
                                     const Eigen::Affine3d &F1,
                                     const Eigen::Affine3d &F2);

    Eigen::Affine3d absoluteFramePos();


    void test();

}

#endif // GEOMETRY_H