Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

Merge gz-math7 ➡️ main #611

Merged
merged 2 commits into from
Jul 18, 2024
Merged

Merge gz-math7 ➡️ main #611

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
0ebc06e
Clean up nested namespaces (#603)
mjcarroll Jul 2, 2024
1ad6126
Merge branch 'gz-math7' into scpeters/merge_7_main
scpeters Jul 18, 2024
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
299 changes: 146 additions & 153 deletions eigen3/include/gz/math/eigen3/Conversions.hh
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -26,185 +26,178 @@
#include <gz/math/Quaternion.hh>
#include <gz/math/Vector3.hh>

namespace gz
namespace gz::math::eigen3
{
namespace math
/// \brief Convert from gz::math::Vector3d to Eigen::Vector3d.
/// \param[in] _v gz::math::Vector3d to convert
/// \return The equivalent Eigen::Vector3d.
inline Eigen::Vector3d convert(const gz::math::Vector3d &_v)
{
namespace eigen3
return Eigen::Vector3d(_v[0], _v[1], _v[2]);
}

/// \brief Convert from gz::math::AxisAlignedBox to
/// Eigen::AlignedBox3d.
/// \param[in] _b gz::math::AxisAlignedBox to convert
/// \return The equivalent Eigen::AlignedBox3d.
inline Eigen::AlignedBox3d convert(
const gz::math::AxisAlignedBox &_b)
{
return Eigen::AlignedBox3d(convert(_b.Min()), convert(_b.Max()));
}

/// \brief Convert from gz::math::Matrix3d to Eigen::Matrix3d.
/// \param[in] _m gz::math::Matrix3d to convert.
/// \return The equivalent Eigen::Matrix3d.
inline Eigen::Matrix3d convert(const gz::math::Matrix3d &_m)
{
Eigen::Matrix3d matrix;
for (std::size_t i=0; i < 3; ++i)
{
/// \brief Convert from gz::math::Vector3d to Eigen::Vector3d.
/// \param[in] _v gz::math::Vector3d to convert
/// \return The equivalent Eigen::Vector3d.
inline Eigen::Vector3d convert(const gz::math::Vector3d &_v)
for (std::size_t j=0; j < 3; ++j)
{
return Eigen::Vector3d(_v[0], _v[1], _v[2]);
matrix(i, j) = _m(i, j);
}
}

/// \brief Convert from gz::math::AxisAlignedBox to
/// Eigen::AlignedBox3d.
/// \param[in] _b gz::math::AxisAlignedBox to convert
/// \return The equivalent Eigen::AlignedBox3d.
inline Eigen::AlignedBox3d convert(
const gz::math::AxisAlignedBox &_b)
{
return Eigen::AlignedBox3d(convert(_b.Min()), convert(_b.Max()));
}
return matrix;
}

/// \brief Convert from gz::math::Matrix3d to Eigen::Matrix3d.
/// \param[in] _m gz::math::Matrix3d to convert.
/// \return The equivalent Eigen::Matrix3d.
inline Eigen::Matrix3d convert(const gz::math::Matrix3d &_m)
/// \brief Convert from gz::math::Matrix6d to
/// Eigen::Matrix<Precision, 6, 6>.
/// \param[in] _m gz::math::Matrix6d to convert.
/// \return The equivalent Eigen::Matrix<Precision, 6, 6>.
/// \tparam Precision Precision such as int, double or float.
template<typename Precision>
inline
Eigen::Matrix<Precision, 6, 6> convert(const Matrix6<Precision> &_m)
{
Eigen::Matrix<Precision, 6, 6> matrix;
for (std::size_t i = 0; i < 6; ++i)
{
for (std::size_t j = 0; j < 6; ++j)
{
Eigen::Matrix3d matrix;
for (std::size_t i=0; i < 3; ++i)
{
for (std::size_t j=0; j < 3; ++j)
{
matrix(i, j) = _m(i, j);
}
}

return matrix;
matrix(i, j) = _m(i, j);
}
}

/// \brief Convert from gz::math::Matrix6d to
/// Eigen::Matrix<Precision, 6, 6>.
/// \param[in] _m gz::math::Matrix6d to convert.
/// \return The equivalent Eigen::Matrix<Precision, 6, 6>.
/// \tparam Precision Precision such as int, double or float.
template<typename Precision>
inline
Eigen::Matrix<Precision, 6, 6> convert(const Matrix6<Precision> &_m)
{
Eigen::Matrix<Precision, 6, 6> matrix;
for (std::size_t i = 0; i < 6; ++i)
{
for (std::size_t j = 0; j < 6; ++j)
{
matrix(i, j) = _m(i, j);
}
}

return matrix;
}
return matrix;
}

/// \brief Convert gz::math::Quaterniond to Eigen::Quaterniond.
/// \param[in] _q gz::math::Quaterniond to convert.
/// \return The equivalent Eigen::Quaterniond.
inline Eigen::Quaterniond convert(const gz::math::Quaterniond &_q)
{
Eigen::Quaterniond quat;
quat.w() = _q.W();
quat.x() = _q.X();
quat.y() = _q.Y();
quat.z() = _q.Z();
/// \brief Convert gz::math::Quaterniond to Eigen::Quaterniond.
/// \param[in] _q gz::math::Quaterniond to convert.
/// \return The equivalent Eigen::Quaterniond.
inline Eigen::Quaterniond convert(const gz::math::Quaterniond &_q)
{
Eigen::Quaterniond quat;
quat.w() = _q.W();
quat.x() = _q.X();
quat.y() = _q.Y();
quat.z() = _q.Z();

return quat;
}
return quat;
}

/// \brief Convert gz::math::Pose3d to Eigen::Isometry3d.
/// \param[in] _pose gz::math::Pose3d to convert.
/// \return The equivalent Eigen::Isometry3d.
inline Eigen::Isometry3d convert(const gz::math::Pose3d &_pose)
{
Eigen::Isometry3d tf = Eigen::Isometry3d::Identity();
tf.translation() = convert(_pose.Pos());
tf.linear() = Eigen::Matrix3d(convert(_pose.Rot()));
/// \brief Convert gz::math::Pose3d to Eigen::Isometry3d.
/// \param[in] _pose gz::math::Pose3d to convert.
/// \return The equivalent Eigen::Isometry3d.
inline Eigen::Isometry3d convert(const gz::math::Pose3d &_pose)
{
Eigen::Isometry3d tf = Eigen::Isometry3d::Identity();
tf.translation() = convert(_pose.Pos());
tf.linear() = Eigen::Matrix3d(convert(_pose.Rot()));

return tf;
}
return tf;
}

/// \brief Convert Eigen::Vector3d to gz::math::Vector3d.
/// \param[in] _v Eigen::Vector3d to convert.
/// \return The equivalent gz::math::Vector3d.
inline gz::math::Vector3d convert(const Eigen::Vector3d &_v)
{
gz::math::Vector3d vec;
vec.X() = _v[0];
vec.Y() = _v[1];
vec.Z() = _v[2];
/// \brief Convert Eigen::Vector3d to gz::math::Vector3d.
/// \param[in] _v Eigen::Vector3d to convert.
/// \return The equivalent gz::math::Vector3d.
inline gz::math::Vector3d convert(const Eigen::Vector3d &_v)
{
gz::math::Vector3d vec;
vec.X() = _v[0];
vec.Y() = _v[1];
vec.Z() = _v[2];

return vec;
}
return vec;
}

/// \brief Convert Eigen::AlignedBox3d to gz::math::AxisAlignedBox.
/// \param[in] _b Eigen::AlignedBox3d to convert.
/// \return The equivalent gz::math::AxisAlignedBox.
inline gz::math::AxisAlignedBox convert(
const Eigen::AlignedBox3d &_b)
{
gz::math::AxisAlignedBox box;
box.Min() = convert(_b.min());
box.Max() = convert(_b.max());
/// \brief Convert Eigen::AlignedBox3d to gz::math::AxisAlignedBox.
/// \param[in] _b Eigen::AlignedBox3d to convert.
/// \return The equivalent gz::math::AxisAlignedBox.
inline gz::math::AxisAlignedBox convert(
const Eigen::AlignedBox3d &_b)
{
gz::math::AxisAlignedBox box;
box.Min() = convert(_b.min());
box.Max() = convert(_b.max());

return box;
}
return box;
}

/// \brief Convert Eigen::Matrix3d to gz::math::Matrix3d.
/// \param[in] _m Eigen::Matrix3d to convert.
/// \return The equivalent gz::math::Matrix3d.
inline gz::math::Matrix3d convert(const Eigen::Matrix3d &_m)
/// \brief Convert Eigen::Matrix3d to gz::math::Matrix3d.
/// \param[in] _m Eigen::Matrix3d to convert.
/// \return The equivalent gz::math::Matrix3d.
inline gz::math::Matrix3d convert(const Eigen::Matrix3d &_m)
{
gz::math::Matrix3d matrix;
for (std::size_t i=0; i < 3; ++i)
{
for (std::size_t j=0; j < 3; ++j)
{
gz::math::Matrix3d matrix;
for (std::size_t i=0; i < 3; ++i)
{
for (std::size_t j=0; j < 3; ++j)
{
matrix(i, j) = _m(i, j);
}
}

return matrix;
matrix(i, j) = _m(i, j);
}
}

/// \brief Convert Eigen::Matrix<Precision, 6, 6> to
/// gz::math::Matrix6d.
/// \param[in] _m Eigen::Matrix<Precision, 6, 6> to convert.
/// \return The equivalent gz::math::Matrix6d.
/// \tparam Precision Precision such as int, double or float.
template<typename Precision>
inline
Matrix6<Precision> convert(const Eigen::Matrix<Precision, 6, 6> &_m)
{
Matrix6<Precision> matrix;
for (std::size_t i = 0; i < 6; ++i)
{
for (std::size_t j = 0; j < 6; ++j)
{
matrix(i, j) = _m(i, j);
}
}

return matrix;
}
return matrix;
}

/// \brief Convert Eigen::Quaterniond to gz::math::Quaterniond.
/// \param[in] _q Eigen::Quaterniond to convert.
/// \return The equivalent gz::math::Quaterniond.
inline gz::math::Quaterniond convert(const Eigen::Quaterniond &_q)
/// \brief Convert Eigen::Matrix<Precision, 6, 6> to
/// gz::math::Matrix6d.
/// \param[in] _m Eigen::Matrix<Precision, 6, 6> to convert.
/// \return The equivalent gz::math::Matrix6d.
/// \tparam Precision Precision such as int, double or float.
template<typename Precision>
inline
Matrix6<Precision> convert(const Eigen::Matrix<Precision, 6, 6> &_m)
{
Matrix6<Precision> matrix;
for (std::size_t i = 0; i < 6; ++i)
{
for (std::size_t j = 0; j < 6; ++j)
{
gz::math::Quaterniond quat;
quat.W() = _q.w();
quat.X() = _q.x();
quat.Y() = _q.y();
quat.Z() = _q.z();

return quat;
matrix(i, j) = _m(i, j);
}
}

/// \brief Convert Eigen::Isometry3d to gz::math::Pose3d.
/// \param[in] _tf Eigen::Isometry3d to convert.
/// \return The equivalent gz::math::Pose3d.
inline gz::math::Pose3d convert(const Eigen::Isometry3d &_tf)
{
gz::math::Pose3d pose;
pose.Pos() = convert(Eigen::Vector3d(_tf.translation()));
pose.Rot() = convert(Eigen::Quaterniond(_tf.linear()));
return matrix;
}

return pose;
}
}
/// \brief Convert Eigen::Quaterniond to gz::math::Quaterniond.
/// \param[in] _q Eigen::Quaterniond to convert.
/// \return The equivalent gz::math::Quaterniond.
inline gz::math::Quaterniond convert(const Eigen::Quaterniond &_q)
{
gz::math::Quaterniond quat;
quat.W() = _q.w();
quat.X() = _q.x();
quat.Y() = _q.y();
quat.Z() = _q.z();

return quat;
}
}

#endif
/// \brief Convert Eigen::Isometry3d to gz::math::Pose3d.
/// \param[in] _tf Eigen::Isometry3d to convert.
/// \return The equivalent gz::math::Pose3d.
inline gz::math::Pose3d convert(const Eigen::Isometry3d &_tf)
{
gz::math::Pose3d pose;
pose.Pos() = convert(Eigen::Vector3d(_tf.translation()));
pose.Rot() = convert(Eigen::Quaterniond(_tf.linear()));

return pose;
}
} // namespace gz::math::eigen3
#endif // GZ_MATH_EIGEN3_CONVERSIONS_HH_
Loading
Loading