ref(geometry): add axis header, improve

common/math/geometry/include/geometry/axis/axis.hpp

@@ -0,0 +1,41 @@
+
+// Copyright 2015 TIER IV, Inc. All rights reserved.
+//
+// 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 GEOMETRY__AXIS__AXIS_HPP_
+#define GEOMETRY__AXIS__AXIS_HPP_
+
+#include <Eigen/Dense>
+#include <geometry_msgs/msg/point.hpp>
+
+namespace math
+{
+namespace geometry
+{
+enum class Axis { X = 0, Y = 1, Z = 2 };
+
+auto axisToEigenAxis(Axis axis) -> Eigen::Vector3d;
+
+auto getMaxValue(const std::vector<geometry_msgs::msg::Point> & points, const Axis & axis)
+  -> double;
+
+auto getMinValue(const std::vector<geometry_msgs::msg::Point> & points, const Axis & axis)
+  -> double;
+
+auto filterByAxis(const std::vector<geometry_msgs::msg::Point> & points, const Axis & axis)
+  -> std::vector<double>;
+}  // namespace geometry
+}  // namespace math
+
+#endif  // GEOMETRY__AXIS__AXIS_HPP_

common/math/geometry/include/geometry/polygon/polygon.hpp

@@ -21,11 +21,6 @@ namespace math
 {
 namespace geometry
 {
-enum class Axis { X = 0, Y = 1, Z = 2 };
-double getMaxValue(const std::vector<geometry_msgs::msg::Point> & points, const Axis & axis);
-double getMinValue(const std::vector<geometry_msgs::msg::Point> & points, const Axis & axis);
-std::vector<double> filterByAxis(
-  const std::vector<geometry_msgs::msg::Point> & points, const Axis & axis);
 std::vector<geometry_msgs::msg::Point> get2DConvexHull(
   const std::vector<geometry_msgs::msg::Point> & points);
 }  // namespace geometry

common/math/geometry/include/geometry/quaternion/normalize.hpp

@@ -27,15 +27,18 @@ namespace geometry
 template <typename T, std::enable_if_t<IsLikeQuaternion<T>::value, std::nullptr_t> = nullptr>
 auto normalize(const T & q)
 {
-  const auto n = norm(q);
-  if (std::fabs(n) <= std::numeric_limits<double>::epsilon()) {
+  if (const auto n = norm(q); std::fabs(n) <= std::numeric_limits<double>::epsilon()) {
     THROW_SIMULATION_ERROR(
-      "Norm of quaternion (", q.w, ",", q.x, ",", q.y, ",", q.z, ") is ", n,
+      "Norm of Quaternion(", q.w, ",", q.x, ",", q.y, ",", q.z, ") is ", n,
       ". The norm of the quaternion you want to normalize should be greater than ",
       std::numeric_limits<double>::epsilon());
+  } else {
+    return geometry_msgs::build<geometry_msgs::msg::Quaternion>()
+      .x(q.x / n)
+      .y(q.y / n)
+      .z(q.z / n)
+      .w(q.w / n);
   }
-  return geometry_msgs::build<geometry_msgs::msg::Quaternion>().x(q.x / n).y(q.y / n).z(q.z / n).w(
-    q.w / n);
 }
 }  // namespace geometry
 }  // namespace math

common/math/geometry/include/geometry/quaternion/operator.hpp

@@ -69,26 +69,26 @@ template <
   typename T, typename U,
   std::enable_if_t<std::conjunction_v<IsLikeQuaternion<T>, IsLikeVector3<U>>, std::nullptr_t> =
     nullptr>
-auto operator*(const T & a, const U & b)
+auto operator*(const T & rotation, const U & vector)
 {
-  T b_quat;
-  b_quat.x = b.x;
-  b_quat.y = b.y;
-  b_quat.z = b.z;
-  b_quat.w = 0.0;
+  T vector_as_quaternion;
+  vector_as_quaternion.x = vector.x;
+  vector_as_quaternion.y = vector.y;
+  vector_as_quaternion.z = vector.z;
+  vector_as_quaternion.w = 0.0;
 
-  T a_inv = a;
-  a_inv.x = -a.x;
-  a_inv.y = -a.y;
-  a_inv.z = -a.z;
-  a_inv.w = a.w;
+  T inverse_rotation = rotation;
+  inverse_rotation.x = -rotation.x;
+  inverse_rotation.y = -rotation.y;
+  inverse_rotation.z = -rotation.z;
+  inverse_rotation.w = rotation.w;
 
-  T result_quat = a * b_quat * a_inv;
+  T rotated_quaternion = rotation * vector_as_quaternion * inverse_rotation;
 
   U rotated_vector;
-  rotated_vector.x = result_quat.x;
-  rotated_vector.y = result_quat.y;
-  rotated_vector.z = result_quat.z;
+  rotated_vector.x = vector_as_quaternion.x;
+  rotated_vector.y = vector_as_quaternion.y;
+  rotated_vector.z = vector_as_quaternion.z;
 
   return rotated_vector;
 }

common/math/geometry/include/geometry/vector3/rotate.hpp

@@ -15,43 +15,30 @@
 #ifndef GEOMETRY__VECTOR3__ROTATE_HPP_
 #define GEOMETRY__VECTOR3__ROTATE_HPP_
 
+#include <geometry/axis/axis.hpp>
 #include <geometry/vector3/is_like_vector3.hpp>
 #include <scenario_simulator_exception/exception.hpp>
 
 namespace math
 {
 namespace geometry
 {
-inline Eigen::Vector3d axisToEigenAxis(Axis axis)
-{
-  switch (axis) {
-    case Axis::X:
-      return Eigen::Vector3d::UnitX();
-    case Axis::Y:
-      return Eigen::Vector3d::UnitY();
-    case Axis::Z:
-      return Eigen::Vector3d::UnitZ();
-    default:
-      THROW_SIMULATION_ERROR("Invalid axis specified.");
-  }
-}
-
 // Rotate a vector by a given angle around a specified axis
 template <
   typename T, std::enable_if_t<std::conjunction_v<IsLikeVector3<T>>, std::nullptr_t> = nullptr>
 auto rotate(T & v, const double angle, const Axis axis)
 {
   if (!std::isfinite(angle)) {
     THROW_SIMULATION_ERROR("The provided angle for rotation is not finite.");
-  }
-
-  const Eigen::Quaterniond rotation(Eigen::AngleAxisd(angle, axisToEigenAxis(axis)));
-  const Eigen::Vector3d eigen_vector(v.x, v.y, v.z);
-  const Eigen::Vector3d rotated_vector = rotation * eigen_vector;
+  } else {
+    const Eigen::Quaterniond rotation(Eigen::AngleAxisd(angle, axisToEigenAxis(axis)));
+    const Eigen::Vector3d eigen_vector(v.x, v.y, v.z);
+    const Eigen::Vector3d rotated_vector = rotation * eigen_vector;
 
-  v.x = rotated_vector.x();
-  v.y = rotated_vector.y();
-  v.z = rotated_vector.z();
+    v.x = rotated_vector.x();
+    v.y = rotated_vector.y();
+    v.z = rotated_vector.z();
+  }
 }
 }  // namespace geometry
 }  // namespace math

common/math/geometry/src/axis/axis.cpp

@@ -0,0 +1,94 @@
+// Copyright 2015 TIER IV, Inc. All rights reserved.
+//
+// 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.
+
+#include <boost/geometry.hpp>
+// #include <boost/geometry/geometries/point_xy.hpp>
+// #include <boost/geometry/geometries/polygon.hpp>
+// #include <geometry/polygon/polygon.hpp>
+// #include <rclcpp/rclcpp.hpp>
+#include <geometry/axis/axis.hpp>
+#include <scenario_simulator_exception/exception.hpp>
+
+namespace math
+{
+namespace geometry
+{
+auto axisToEigenAxis(Axis axis) -> Eigen::Vector3d
+{
+  switch (axis) {
+    case Axis::X:
+      return Eigen::Vector3d::UnitX();
+    case Axis::Y:
+      return Eigen::Vector3d::UnitY();
+    case Axis::Z:
+      return Eigen::Vector3d::UnitZ();
+    default:
+      THROW_SIMULATION_ERROR("Invalid axis specified.");
+  }
+}
+
+auto getMaxValue(const std::vector<geometry_msgs::msg::Point> & points, const Axis & axis) -> double
+{
+  if (const auto values = filterByAxis(points, axis); values.size() == 1) {
+    return values.front();
+  } else {
+    return *std::max_element(values.begin(), values.end());
+  }
+}
+
+auto getMinValue(const std::vector<geometry_msgs::msg::Point> & points, const Axis & axis) -> double
+{
+  if (const auto values = filterByAxis(points, axis); values.size() == 1) {
+    return values.front();
+  } else {
+    return *std::min_element(values.begin(), values.end());
+  }
+}
+
+auto filterByAxis(const std::vector<geometry_msgs::msg::Point> & points, const Axis & axis)
+  -> std::vector<double>
+{
+  if (points.empty()) {
+    THROW_SIMULATION_ERROR(
+      "Invalid point list is specified, while trying to filter ",
+      axis == Axis::X   ? "X"
+      : axis == Axis::Y ? "Y"
+                        : "Z",
+      " axis. ",
+      "The point list in filterByAxis() should have at least one point to filter. "
+      "This message is not originally intended to be displayed, if you see it, please "
+      "contact the developer of traffic_simulator.");
+  } else {
+    const auto axisExtractor =
+      [](const Axis axis) -> std::function<double(const geometry_msgs::msg::Point &)> {
+      switch (axis) {
+        case Axis::X:
+          return [](const geometry_msgs::msg::Point & p) { return p.x; };
+        case Axis::Y:
+          return [](const geometry_msgs::msg::Point & p) { return p.y; };
+        case Axis::Z:
+          return [](const geometry_msgs::msg::Point & p) { return p.z; };
+        default:
+          throw std::invalid_argument("Invalid axis");
+      }
+    };
+
+    std::vector<double> single_axis;
+    std::transform(
+      points.begin(), points.end(), std::back_inserter(single_axis), axisExtractor(axis));
+    return single_axis;
+  }
+}
+}  // namespace geometry
+}  // namespace math

common/math/geometry/src/polygon/polygon.cpp

@@ -47,72 +47,5 @@ std::vector<geometry_msgs::msg::Point> get2DConvexHull(
   }
   return polygon;
 }
-
-double getMaxValue(const std::vector<geometry_msgs::msg::Point> & points, const Axis & axis)
-{
-  if (points.empty()) {
-    THROW_SIMULATION_ERROR(
-      "Invalid point list is specified, while getting max value on ",
-      axis == Axis::X   ? "X"
-      : axis == Axis::Y ? "Y"
-                        : "Z",
-      " axis. ",
-      "The point list in getMaxValue should have at least one point to get the max value from. "
-      "This message is not originally intended to be displayed, if you see it, please "
-      "contact the developer of traffic_simulator.");
-  }
-  const auto values = filterByAxis(points, axis);
-  if (values.size() == 1) {
-    return values.front();
-  }
-  return *std::max_element(values.begin(), values.end());
-}
-
-double getMinValue(const std::vector<geometry_msgs::msg::Point> & points, const Axis & axis)
-{
-  if (points.empty()) {
-    THROW_SIMULATION_ERROR(
-      "Invalid point list is specified, while getting min value on ",
-      axis == Axis::X   ? "X"
-      : axis == Axis::Y ? "Y"
-                        : "Z",
-      " axis. ",
-      "The point list in getMinValue should have at least one point to get the min value from. "
-      "This message is not originally intended to be displayed, if you see it, please "
-      "contact the developer of traffic_simulator.");
-  }
-  const auto values = filterByAxis(points, axis);
-  if (values.size() == 1) {
-    return values.front();
-  }
-  return *std::min_element(values.begin(), values.end());
-}
-
-std::vector<double> filterByAxis(
-  const std::vector<geometry_msgs::msg::Point> & points, const Axis & axis)
-{
-  std::vector<double> ret;
-  switch (axis) {
-    case Axis::X: {
-      std::transform(
-        points.begin(), points.end(), std::back_inserter(ret),
-        [](const geometry_msgs::msg::Point & point) { return point.x; });
-      break;
-    }
-    case Axis::Y: {
-      std::transform(
-        points.begin(), points.end(), std::back_inserter(ret),
-        [](const geometry_msgs::msg::Point & point) { return point.y; });
-      break;
-    }
-    case Axis::Z: {
-      std::transform(
-        points.begin(), points.end(), std::back_inserter(ret),
-        [](const geometry_msgs::msg::Point & point) { return point.z; });
-      break;
-    }
-  }
-  return ret;
-}
 }  // namespace geometry
 }  // namespace math

common/math/geometry/test/src/polygon/test_polygon.cpp

@@ -14,6 +14,7 @@
 
 #include <gtest/gtest.h>
 
+#include <geometry/axis/axis.hpp>
 #include <geometry/polygon/polygon.hpp>
 #include <scenario_simulator_exception/exception.hpp>
 
@@ -41,6 +42,7 @@ TEST(Polygon, filterByAxis)
   EXPECT_DOUBLE_EQ(values_z[2], -3.0);
 }
 
+/// @todo refactor test to throw exception
 TEST(Polygon, filterByAxisEmptyVector)
 {
   std::vector<geometry_msgs::msg::Point> points;

simulation/simple_sensor_simulator/include/simple_sensor_simulator/sensor_simulation/primitives/primitive.hpp

@@ -18,6 +18,7 @@
 #include <embree4/rtcore.h>
 
 #include <algorithm>
+#include <geometry/axis/axis.hpp>
 #include <geometry/polygon/polygon.hpp>
 #include <geometry_msgs/msg/pose.hpp>
 #include <optional>