Skip to content
Snippets Groups Projects
Commit 03dc6f77 authored by jcarpent's avatar jcarpent
Browse files

[Geometry] Clean FCL conversion tools

parent 32212dab
Branches
Tags
No related merge requests found
//
// Copyright (c) 2015 CNRS
// Copyright (c) 2015-2016 CNRS
//
// This file is part of Pinocchio
// Pinocchio is free software: you can redistribute it
......@@ -18,10 +18,8 @@
#ifndef __se3_fcl_convertion_hpp__
#define __se3_fcl_convertion_hpp__
#include <Eigen/Geometry>
#include <hpp/fcl/math/transform.h>
#include "pinocchio/spatial/se3.hpp"
# include <hpp/fcl/math/transform.h>
namespace se3
{
......@@ -45,27 +43,24 @@ namespace se3
inline fcl::Vec3f toFclVec3f(const Eigen::Vector3d & vec)
{
return fcl::Vec3f( vec(0),vec(1), vec(2));
return fcl::Vec3f(vec(0),vec(1),vec(2));
}
inline Eigen::Vector3d toVector3d(const fcl::Vec3f & vec)
{
Eigen::Vector3d res;
res << vec[0],vec[1], vec[2];
return res;
return Eigen::Vector3d(vec[0],vec[1],vec[2]);
}
inline fcl::Transform3f toFclTransform3f(const se3::SE3 & m)
inline fcl::Transform3f toFclTransform3f(const SE3 & m)
{
return fcl::Transform3f(toFclMatrix3f(m.rotation()), toFclVec3f(m.translation()));
}
inline se3::SE3 toPinocchioSE3(const fcl::Transform3f & tf)
inline SE3 toPinocchioSE3(const fcl::Transform3f & tf)
{
return se3::SE3(toMatrix3d(tf.getRotation()), toVector3d(tf.getTranslation()));
return SE3(toMatrix3d(tf.getRotation()), toVector3d(tf.getTranslation()));
}
} // namespace se3
#endif // ifndef __se3_fcl_convertion_hpp__
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Please register or to comment