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.

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

ign -> gz Migrate Ignition Headers : gz-math #483

Merged
merged 2 commits into from
Aug 30, 2022
Merged
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
The table of contents is too big for display.
Diff view
Diff view
  •  
  •  
  •  
2 changes: 1 addition & 1 deletion BUILD.bazel
Original file line number Diff line number Diff line change
Expand Up @@ -43,7 +43,7 @@ ign_export_header(

public_headers_no_gen = glob([
"include/ignition/math/*.hh",
"include/ignition/math/detail/*.hh",
"include/gz/math/detail/*.hh",
"include/ignition/math/graph/*.hh",
])

Expand Down
5 changes: 4 additions & 1 deletion CMakeLists.txt
Original file line number Diff line number Diff line change
Expand Up @@ -16,7 +16,10 @@ find_package(ignition-cmake2 2.13.0 REQUIRED)
#============================================================================
set (c++standard 17)
set (CMAKE_CXX_STANDARD 17)
ign_configure_project(VERSION_SUFFIX)
ign_configure_project(
REPLACE_IGNITION_INCLUDE_PATH gz/math
chapulina marked this conversation as resolved.
Show resolved Hide resolved
VERSION_SUFFIX
)

#============================================================================
# Set project-specific options
Expand Down
4 changes: 2 additions & 2 deletions Changelog.md
Original file line number Diff line number Diff line change
Expand Up @@ -516,7 +516,7 @@

### Ignition Math 6.1.0

1. eigen3: add conversion functions for Eigen::AlignedBox3d <=> ignition::math::AxisAlignedBox
1. eigen3: add conversion functions for Eigen::AlignedBox3d <=> gz::math::AxisAlignedBox
* [BitBucket pull request 302](https://osrf-migration.github.io/ignition-gh-pages/#!/ignitionrobotics/ign-math/pull-requests/302)

### Ignition Math 6.0.0
Expand Down Expand Up @@ -547,7 +547,7 @@
* [BitBucket pull request 301](https://osrf-migration.github.io/ignition-gh-pages/#!/ignitionrobotics/ign-math/pull-requests/301)
* [Issue 60](https://github.com/ignitionrobotics/ign-math/issues/60)

1. eigen3: add conversion functions for Eigen::AlignedBox3d <=> ignition::math::AxisAlignedBox
1. eigen3: add conversion functions for Eigen::AlignedBox3d <=> gz::math::AxisAlignedBox
* [BitBucket pull request 302](https://osrf-migration.github.io/ignition-gh-pages/#!/ignitionrobotics/ign-math/pull-requests/302)


Expand Down
2 changes: 2 additions & 0 deletions eigen3/include/CMakeLists.txt
Original file line number Diff line number Diff line change
@@ -0,0 +1,2 @@
add_subdirectory(gz)
install(DIRECTORY ignition DESTINATION ${IGN_INCLUDE_INSTALL_DIR_FULL})
File renamed without changes.
210 changes: 210 additions & 0 deletions eigen3/include/gz/math/eigen3/Conversions.hh
Original file line number Diff line number Diff line change
@@ -0,0 +1,210 @@
/*
* Copyright (C) 2018 Open Source Robotics Foundation
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*
*/

#ifndef GZ_MATH_EIGEN3_CONVERSIONS_HH_
#define GZ_MATH_EIGEN3_CONVERSIONS_HH_

#include <Eigen/Geometry>
#include <gz/math/AxisAlignedBox.hh>
#include <gz/math/Matrix3.hh>
#include <gz/math/Matrix6.hh>
#include <gz/math/Pose3.hh>
#include <gz/math/Quaternion.hh>
#include <gz/math/Vector3.hh>

namespace ignition
{
namespace math
{
namespace eigen3
{
/// \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)
{
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)
{
for (std::size_t j=0; j < 3; ++j)
{
matrix(i, j) = _m(i, j);
}
}

return matrix;
}

/// \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;
}

/// \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;
}

/// \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;
}

/// \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;
}

/// \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;
}

/// \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)
{
matrix(i, j) = _m(i, j);
}
}

return matrix;
}

/// \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;
}

/// \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;
}

/// \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;
}
}
}
}

#endif
Loading