Fix/remove linear algebra

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

@@ -0,0 +1,35 @@
+// Copyright 2024 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__VECTOR3__INNER_PRODUCT_HPP_
+#define GEOMETRY__VECTOR3__INNER_PRODUCT_HPP_
+
+#include <geometry/vector3/is_like_vector3.hpp>
+
+namespace math
+{
+namespace geometry
+{
+template <
+  typename T, typename U,
+  std::enable_if_t<std::conjunction_v<IsLikeVector3<T>, IsLikeVector3<U>>, std::nullptr_t> =
+    nullptr>
+auto innerProduct(const T & v0, const U & v1)
+{
+  return v0.x * v1.x + v0.y * v1.y + v0.z * v1.z;
+}
+}  // namespace geometry
+}  // namespace math
+
+#endif  // GEOMETRY__VECTOR3__INNER_PRODUCT_HPP_

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

@@ -0,0 +1,45 @@
+// Copyright 2024 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__VECTOR3__INTERNAL_ANGLE_HPP_
+#define GEOMETRY__VECTOR3__INTERNAL_ANGLE_HPP_
+
+#include <scenario_simulator_exception/exception.hpp>
+
+#include <geometry/vector3/is_like_vector3.hpp>
+#include <geometry/vector3/norm.hpp>
+#include <geometry/vector3/inner_product.hpp>
+
+namespace math
+{
+namespace geometry
+{
+template <
+  typename T, typename U,
+  std::enable_if_t<std::conjunction_v<IsLikeVector3<T>, IsLikeVector3<U>>, std::nullptr_t> =
+    nullptr>
+auto getInternalAngle(const T & v0, const U & v1)
+{
+    const auto val = innerProduct(v0, v1) / (norm(v0) * norm(v1));
+    if (-1 <= val && val <= 1) {
+        return std::acos(val);
+    }
+    THROW_SIMULATION_ERROR(
+        "value of v0*v1/(size(v0)*size(v1)) in vector v0 : ", v0.x, ",", v0.y, ",", v0.z,
+        " and v1 : ", v1.x, ",", v1.y, ",", v1.z, "is out of range, value = ", val);
+}
+}  // namespace geometry
+}  // namespace math
+
+#endif  // GEOMETRY__VECTOR3__INTERNAL_ANGLE_HPP_

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

@@ -22,17 +22,44 @@ namespace math
 {
 namespace geometry
 {
+template <typename T, typename = void>
+struct HasMemberW : std::false_type {};
+
+template <typename T>
+struct HasMemberW<T, std::void_t<decltype(std::declval<T>().w)>> : std::true_type {};
+
 template <typename T, typename = void>
 struct IsLikeVector3 : public std::false_type
 {
 };
 
 template <typename T>
 struct IsLikeVector3<
-  T, std::void_t<decltype(std::declval<T>().x, std::declval<T>().y, std::declval<T>().z)>>
+  T, std::void_t<
+      decltype(std::declval<T>().x),
+      decltype(std::declval<T>().y),
+      decltype(std::declval<T>().z),
+      std::enable_if_t<!HasMemberW<T>::value>
+  >>
 : public std::true_type
 {
 };
+
+template <typename T, typename = void>
+struct IsLikeQuaternion : std::false_type {};
+
+template <typename T>
+struct IsLikeQuaternion<
+  T, std::void_t<
+      decltype(std::declval<T>().x),
+      decltype(std::declval<T>().y),
+      decltype(std::declval<T>().z),
+      decltype(std::declval<T>().w)
+  >>
+: std::true_type
+{
+};
+
 }  // namespace geometry
 }  // namespace math
 

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

@@ -16,6 +16,7 @@
 #define GEOMETRY__VECTOR3__NORMALIZE_HPP_
 
 #include <geometry/vector3/is_like_vector3.hpp>
+#include <geometry/vector3/norm.hpp>
 
 namespace math
 {

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

@@ -17,6 +17,8 @@
 
 #include <geometry/vector3/is_like_vector3.hpp>
 #include <geometry_msgs/msg/vector3.hpp>
+#include <geometry_msgs/msg/quaternion.hpp>
+#include <geometry_msgs/msg/point.hpp>
 
 namespace math
 {
@@ -28,11 +30,22 @@ template <
     nullptr>
 auto operator+(const T & a, const U & b)
 {
-  geometry_msgs::msg::Vector3 v;
-  v.x = a.x + b.x;
-  v.y = a.y + b.y;
-  v.z = a.z + b.z;
-  return v;
+  if constexpr (std::is_same<T, geometry_msgs::msg::Vector3>::value)
+  {
+    geometry_msgs::msg::Vector3 v;
+    v.x = a.x + b.x;
+    v.y = a.y + b.y;
+    v.z = a.z + b.z;
+    return v;
+  }
+  else
+  {
+    geometry_msgs::msg::Point v;
+    v.x = a.x + b.x;
+    v.y = a.y + b.y;
+    v.z = a.z + b.z;
+    return v;
+  }
 }
 
 template <
@@ -41,11 +54,22 @@ template <
     nullptr>
 auto operator-(const T & a, const U & b)
 {
-  geometry_msgs::msg::Vector3 v;
-  v.x = a.x - b.x;
-  v.y = a.y - b.y;
-  v.z = a.z - b.z;
-  return v;
+  if constexpr (std::is_same<T, geometry_msgs::msg::Vector3>::value)
+  {
+    geometry_msgs::msg::Vector3 v;
+    v.x = a.x - b.x;
+    v.y = a.y - b.y;
+    v.z = a.z - b.z;
+    return v;
+  }
+  else
+  {
+    geometry_msgs::msg::Point v;
+    v.x = a.x - b.x;
+    v.y = a.y - b.y;
+    v.z = a.z - b.z;
+    return v;
+  }
 }
 
 template <
@@ -54,11 +78,23 @@ template <
     nullptr>
 auto operator*(const T & a, const U & b)
 {
-  geometry_msgs::msg::Vector3 v;
-  v.x = a.x * b;
-  v.y = a.y * b;
-  v.z = a.z * b;
-  return v;
+  if constexpr (std::is_same<T, geometry_msgs::msg::Vector3>::value)
+  {
+    geometry_msgs::msg::Vector3 v;
+    v.x = a.x * b;
+    v.y = a.y * b;
+    v.z = a.z * b;
+    return v;
+  }
+  else
+  {
+    geometry_msgs::msg::Point v;
+    v.x = a.x * b;
+    v.y = a.y * b;
+    v.z = a.z * b;
+    return v;
+  }
+
 }
 
 template <
@@ -67,11 +103,22 @@ template <
     nullptr>
 auto operator/(const T & a, const U & b)
 {
-  geometry_msgs::msg::Vector3 v;
-  v.x = a.x / b;
-  v.y = a.y / b;
-  v.z = a.z / b;
-  return v;
+  if constexpr (std::is_same<T, geometry_msgs::msg::Vector3>::value)
+  {
+    geometry_msgs::msg::Vector3 v;
+    v.x = a.x / b;
+    v.y = a.y / b;
+    v.z = a.z / b;
+    return v;
+  }
+  else
+  {
+    geometry_msgs::msg::Point v;
+    v.x = a.x / b;
+    v.y = a.y / b;
+    v.z = a.z / b;
+    return v;
+  }
 }
 
 template <
@@ -85,6 +132,61 @@ auto operator+=(T & a, const U & b) -> decltype(auto)
   a.z += b.z;
   return a;
 }
+
+template <
+  typename T, typename U,
+  std::enable_if_t<std::conjunction_v<IsLikeQuaternion<T>, IsLikeQuaternion<U>>, std::nullptr_t> =
+    nullptr>
+auto operator+(const T & a, const U & b)
+{
+  geometry_msgs::msg::Quaternion v;
+  v.x = a.x + b.x;
+  v.y = a.y + b.y;
+  v.z = a.z + b.z;
+  v.w = a.w + b.w;
+  return v;
+}
+
+template <
+  typename T, typename U,
+  std::enable_if_t<std::conjunction_v<IsLikeQuaternion<T>, IsLikeQuaternion<U>>, std::nullptr_t> =
+    nullptr>
+auto operator-(const T & a, const U & b)
+{
+  geometry_msgs::msg::Quaternion v;
+  v.x = a.x - b.x;
+  v.y = a.y - b.y;
+  v.z = a.z - b.z;
+  v.w = a.w - b.w;
+  return v;
+}
+
+template <
+  typename T, typename U,
+  std::enable_if_t<std::conjunction_v<IsLikeQuaternion<T>, IsLikeQuaternion<U>>, std::nullptr_t> =
+    nullptr>
+auto operator*(const T & a, const U & b)
+{
+  geometry_msgs::msg::Quaternion v;
+  v.x = a.w  * b.x - a.z * b.y + a.y * b.z + a.x * b.w;
+  v.y = a.z  * b.x + a.w * b.y - a.x * b.z + a.y * b.w;
+  v.z = -a.y * b.x + a.x * b.y + a.w * b.z + a.z * b.w;
+  v.w = -a.x * b.x - a.y * b.y - a.z * b.z + a.w * b.w;
+  return v;
+}
+
+template <
+  typename T, typename U,
+  std::enable_if_t<std::conjunction_v<IsLikeQuaternion<T>, IsLikeQuaternion<U>>, std::nullptr_t> =
+    nullptr>
+auto operator+=(T & a, const U & b) -> decltype(auto)
+{
+  a.x += b.x;
+  a.y += b.y;
+  a.z += b.z;
+  a.w += b.w;
+  return a;
+}
 }  // namespace geometry
 }  // namespace math