Fix/remove linear algebra

common/math/geometry/include/geometry/quaternion/is_like_quaternion.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__QUATERNION__IS_LIKE_QUATERNION_HPP_
+#define GEOMETRY__QUATERNION__IS_LIKE_QUATERNION_HPP_
+
+#include <type_traits>
+#include <utility>
+
+namespace math
+{
+namespace geometry
+{
+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
+
+#endif  // GEOMETRY__QUATERNION__IS_LIKE_QUATERNION_HPP_

common/math/geometry/include/geometry/quaternion/make_quaternion.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__QUATERNION__MAKE_QUATERNION_HPP_
+#define GEOMETRY__QUATERNION__MAKE_QUATERNION_HPP_
+
+#include <geometry_msgs/msg/quaternion.hpp>
+
+namespace math
+{
+namespace geometry
+{
+template <
+  typename T, typename U, typename V, typename W,
+  std::enable_if_t<
+    std::conjunction_v<std::is_scalar<T>, std::is_scalar<U>, std::is_scalar<V>, std::is_scalar<W>>,
+    std::nullptr_t> = nullptr>
+auto makeQuaternion(const T & x, const U & y, const V & z, const W & w)
+{
+  geometry_msgs::msg::Quaternion quat;
+  quat.x = x;
+  quat.y = y;
+  quat.z = z;
+  quat.w = w;
+  return quat;
+}
+}  // namespace geometry
+}  // namespace math
+
+#endif  // GEOMETRY__QUATERNION__MAKE_QUATERNION_HPP_

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

@@ -0,0 +1,82 @@
+// 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__QUATERNION__OPERATOR_HPP_
+#define GEOMETRY__QUATERNION__OPERATOR_HPP_
+
+#include <geometry/quaternion/is_like_quaternion.hpp>
+#include <geometry_msgs/msg/quaternion.hpp>
+
+namespace math
+{
+namespace geometry
+{
+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)
+{
+  auto v = T();
+  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)
+{
+  auto v = T();
+  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)
+{
+  auto v = T();
+  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
+
+#endif  // GEOMETRY__QUATERNION__OPERATOR_HPP_

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

@@ -0,0 +1,35 @@
+// 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__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,44 @@
+// 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__VECTOR3__INTERNAL_ANGLE_HPP_
+#define GEOMETRY__VECTOR3__INTERNAL_ANGLE_HPP_
+
+#include <geometry/vector3/inner_product.hpp>
+#include <geometry/vector3/is_like_vector3.hpp>
+#include <geometry/vector3/norm.hpp>
+#include <scenario_simulator_exception/exception.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,15 +22,26 @@ 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)>>
-: public std::true_type
+  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
 {
 };
 }  // namespace geometry

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

@@ -16,6 +16,8 @@
 #define GEOMETRY__VECTOR3__NORMALIZE_HPP_
 
 #include <geometry/vector3/is_like_vector3.hpp>
+#include <geometry/vector3/norm.hpp>
+#include <scenario_simulator_exception/exception.hpp>
 
 namespace math
 {
@@ -24,7 +26,14 @@ namespace geometry
 template <typename T, std::enable_if_t<IsLikeVector3<T>::value, std::nullptr_t> = nullptr>
 auto normalize(const T & v)
 {
-  return v / norm(v);
+  const auto n = norm(v);
+  if (std::fabs(n) <= std::numeric_limits<double>::epsilon()) {
+    THROW_SIMULATION_ERROR(
+      "size of norm (", v.x, ",", v.y, ",", v.z, ") is, ", n,
+      " size of the vector you want to normalize should be over ",
+      std::numeric_limits<double>::epsilon());
+  }
+  return v / n;
 }
 }  // namespace geometry
 }  // namespace math