IceConversions.h

Go to the documentation of this file.

#pragma once
 
#include <Eigen/Core>
#include <Eigen/Geometry>
 
#include <RobotAPI/interface/ArViz/Elements.h>
 
namespace armarx::viz
{
 
    inline Eigen::Matrix4f
    toEigen(data::GlobalPose const& pose)
    {
        Eigen::Matrix4f result;
        result.block<3, 3>(0, 0) =
            Eigen::Quaternionf(pose.qw, pose.qx, pose.qy, pose.qz).toRotationMatrix();
        result.block<3, 1>(0, 3) = Eigen::Vector3f(pose.x, pose.y, pose.z);
        result.block<1, 4>(3, 0) = Eigen::Vector4f(0.0f, 0.0f, 0.0f, 1.0f);
        return result;
    }
 
    inline data::GlobalPose
    poseToIce(Eigen::Vector3f const& pos, Eigen::Quaternionf const& ori)
    {
        data::GlobalPose result;
        result.x = pos.x();
        result.y = pos.y();
        result.z = pos.z();
        result.qw = ori.w();
        result.qx = ori.x();
        result.qy = ori.y();
        result.qz = ori.z();
        return result;
    }
 
} // namespace armarx::viz