diff --git a/common/math/geometry/include/geometry/linear_algebra.hpp b/common/math/geometry/include/geometry/linear_algebra.hpp
deleted file mode 100644
index a520ff1a6c6..00000000000
--- a/common/math/geometry/include/geometry/linear_algebra.hpp
+++ /dev/null
@@ -1,55 +0,0 @@
-// 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__LINEAR_ALGEBRA_HPP_
-#define GEOMETRY__LINEAR_ALGEBRA_HPP_
-
-#include <math.h>
-
-#include <geometry_msgs/msg/point.hpp>
-#include <geometry_msgs/msg/vector3.hpp>
-#include <limits>
-#include <scenario_simulator_exception/exception.hpp>
-
-namespace math
-{
-namespace geometry
-{
-geometry_msgs::msg::Vector3 vector3(double x, double y, double z);
-double getSize(geometry_msgs::msg::Vector3 vec);
-geometry_msgs::msg::Vector3 normalize(geometry_msgs::msg::Vector3 vec);
-double innerProduct(const geometry_msgs::msg::Vector3 & v0, const geometry_msgs::msg::Vector3 & v1);
-double getInternalAngle(
-  const geometry_msgs::msg::Vector3 & v0, const geometry_msgs::msg::Vector3 & v1);
-}  // namespace geometry
-}  // namespace math
-
-geometry_msgs::msg::Vector3 operator/(const geometry_msgs::msg::Vector3 & vec, double value);
-geometry_msgs::msg::Vector3 operator*(const geometry_msgs::msg::Vector3 & vec, double value);
-geometry_msgs::msg::Vector3 operator*(double value, const geometry_msgs::msg::Vector3 & vec);
-geometry_msgs::msg::Point operator*(const geometry_msgs::msg::Point & point, const double value);
-geometry_msgs::msg::Point operator*(const double value, const geometry_msgs::msg::Point & point);
-geometry_msgs::msg::Point operator+(
-  const geometry_msgs::msg::Point & v0, const geometry_msgs::msg::Vector3 & v1);
-geometry_msgs::msg::Vector3 operator+(
-  const geometry_msgs::msg::Vector3 & v0, const geometry_msgs::msg::Vector3 & v1);
-geometry_msgs::msg::Point operator+(
-  const geometry_msgs::msg::Point & v0, const geometry_msgs::msg::Point & v1);
-geometry_msgs::msg::Point operator-(
-  const geometry_msgs::msg::Point & v0, const geometry_msgs::msg::Vector3 & v1);
-geometry_msgs::msg::Vector3 operator-(
-  const geometry_msgs::msg::Vector3 & v0, const geometry_msgs::msg::Vector3 & v1);
-geometry_msgs::msg::Point operator-(
-  const geometry_msgs::msg::Point & v0, const geometry_msgs::msg::Point & v1);
-#endif  // GEOMETRY__LINEAR_ALGEBRA_HPP_
diff --git a/common/math/geometry/include/geometry/quaternion/is_like_quaternion.hpp b/common/math/geometry/include/geometry/quaternion/is_like_quaternion.hpp
new file mode 100644
index 00000000000..b653d2b3cd7
--- /dev/null
+++ b/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_
diff --git a/common/math/geometry/include/geometry/quaternion/make_quaternion.hpp b/common/math/geometry/include/geometry/quaternion/make_quaternion.hpp
new file mode 100644
index 00000000000..2439eab10fb
--- /dev/null
+++ b/common/math/geometry/include/geometry/quaternion/make_quaternion.hpp
@@ -0,0 +1,36 @@
+// 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)
+{
+  return geometry_msgs::build<geometry_msgs::msg::Quaternion>().x(x).y(y).z(z).w(w);
+}
+}  // namespace geometry
+}  // namespace math
+
+#endif  // GEOMETRY__QUATERNION__MAKE_QUATERNION_HPP_
diff --git a/common/math/geometry/include/geometry/quaternion/operator.hpp b/common/math/geometry/include/geometry/quaternion/operator.hpp
new file mode 100644
index 00000000000..624dd686852
--- /dev/null
+++ b/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_
diff --git a/common/math/geometry/include/geometry/vector3/inner_product.hpp b/common/math/geometry/include/geometry/vector3/inner_product.hpp
new file mode 100644
index 00000000000..4aead00069b
--- /dev/null
+++ b/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_
diff --git a/common/math/geometry/include/geometry/vector3/internal_angle.hpp b/common/math/geometry/include/geometry/vector3/internal_angle.hpp
new file mode 100644
index 00000000000..30168a29533
--- /dev/null
+++ b/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_
diff --git a/common/math/geometry/include/geometry/vector3/is_like_vector3.hpp b/common/math/geometry/include/geometry/vector3/is_like_vector3.hpp
index 615897baf2c..84a1ec1f76f 100644
--- a/common/math/geometry/include/geometry/vector3/is_like_vector3.hpp
+++ b/common/math/geometry/include/geometry/vector3/is_like_vector3.hpp
@@ -22,6 +22,16 @@ 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
 {
@@ -29,8 +39,9 @@ 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
diff --git a/common/math/geometry/include/geometry/vector3/normalize.hpp b/common/math/geometry/include/geometry/vector3/normalize.hpp
index a44dfc68912..1097b331efb 100644
--- a/common/math/geometry/include/geometry/vector3/normalize.hpp
+++ b/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
diff --git a/common/math/geometry/include/geometry/vector3/operator.hpp b/common/math/geometry/include/geometry/vector3/operator.hpp
index 752ac923757..091e6ec8028 100644
--- a/common/math/geometry/include/geometry/vector3/operator.hpp
+++ b/common/math/geometry/include/geometry/vector3/operator.hpp
@@ -16,6 +16,8 @@
 #define GEOMETRY__VECTOR3__OPERATOR_HPP_
 
 #include <geometry/vector3/is_like_vector3.hpp>
+#include <geometry_msgs/msg/point.hpp>
+#include <geometry_msgs/msg/quaternion.hpp>
 #include <geometry_msgs/msg/vector3.hpp>
 
 namespace math
@@ -28,11 +30,19 @@ 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 +51,19 @@ 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 +72,19 @@ 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 +93,19 @@ 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 <
diff --git a/common/math/geometry/include/geometry/vector3/vector3.hpp b/common/math/geometry/include/geometry/vector3/vector3.hpp
new file mode 100644
index 00000000000..d4be1173c22
--- /dev/null
+++ b/common/math/geometry/include/geometry/vector3/vector3.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__VECTOR3__VECTOR3_HPP_
+#define GEOMETRY__VECTOR3__VECTOR3_HPP_
+
+#include <geometry/vector3/is_like_vector3.hpp>
+#include <geometry_msgs/msg/vector3.hpp>
+
+namespace math
+{
+namespace geometry
+{
+template <
+  typename T, typename U, typename V,
+  std::enable_if_t<
+    std::conjunction_v<std::is_scalar<T>, std::is_scalar<U>, std::is_scalar<V>>, std::nullptr_t> =
+    nullptr>
+auto vector3(const T & x, const U & y, const V & z)
+{
+  geometry_msgs::msg::Vector3 vec;
+  vec.x = x;
+  vec.y = y;
+  vec.z = z;
+  return vec;
+}
+}  // namespace geometry
+}  // namespace math
+
+#endif  // GEOMETRY__VECTOR3__VECTOR3_HPP_
diff --git a/common/math/geometry/src/linear_algebra.cpp b/common/math/geometry/src/linear_algebra.cpp
deleted file mode 100644
index 51ab41ac767..00000000000
--- a/common/math/geometry/src/linear_algebra.cpp
+++ /dev/null
@@ -1,164 +0,0 @@
-// 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 <geometry/linear_algebra.hpp>
-#include <scenario_simulator_exception/exception.hpp>
-
-namespace math
-{
-namespace geometry
-{
-double innerProduct(const geometry_msgs::msg::Vector3 & v0, const geometry_msgs::msg::Vector3 & v1)
-{
-  return v0.x * v1.x + v0.y * v1.y + v0.z * v1.z;
-}
-
-double getInternalAngle(
-  const geometry_msgs::msg::Vector3 & v0, const geometry_msgs::msg::Vector3 & v1)
-{
-  const auto val = innerProduct(v0, v1) / (getSize(v0) * getSize(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);
-}
-
-geometry_msgs::msg::Vector3 vector3(double x, double y, double z)
-{
-  geometry_msgs::msg::Vector3 vec;
-  vec.x = x;
-  vec.y = y;
-  vec.z = z;
-  return vec;
-}
-
-double getSize(geometry_msgs::msg::Vector3 vec)
-{
-  return std::sqrt(vec.x * vec.x + vec.y * vec.y + vec.z * vec.z);
-}
-
-geometry_msgs::msg::Vector3 normalize(geometry_msgs::msg::Vector3 vec)
-{
-  double size = getSize(vec);
-  if (std::fabs(size) <= std::numeric_limits<double>::epsilon()) {
-    THROW_SIMULATION_ERROR(
-      "size of vector (", vec.x, ",", vec.y, ",", vec.z, ") is, ", size,
-      " size of the vector you want to normalize should be over ",
-      std::numeric_limits<double>::epsilon());
-  }
-  vec.x = vec.x / size;
-  vec.y = vec.y / size;
-  vec.z = vec.z / size;
-  return vec;
-}
-}  // namespace geometry
-}  // namespace math
-
-geometry_msgs::msg::Vector3 operator/(const geometry_msgs::msg::Vector3 & vec, double value)
-{
-  geometry_msgs::msg::Vector3 ret;
-  ret.x = vec.x / value;
-  ret.y = vec.y / value;
-  ret.z = vec.z / value;
-  return ret;
-}
-
-geometry_msgs::msg::Vector3 operator*(const geometry_msgs::msg::Vector3 & vec, double value)
-{
-  geometry_msgs::msg::Vector3 ret;
-  ret.x = vec.x * value;
-  ret.y = vec.y * value;
-  ret.z = vec.z * value;
-  return ret;
-}
-
-geometry_msgs::msg::Vector3 operator*(double value, const geometry_msgs::msg::Vector3 & vec)
-{
-  return vec * value;
-}
-
-geometry_msgs::msg::Point operator*(const geometry_msgs::msg::Point & point, const double value)
-{
-  return geometry_msgs::build<geometry_msgs::msg::Point>()
-    .x(point.x * value)
-    .y(point.y * value)
-    .z(point.z * value);
-}
-
-geometry_msgs::msg::Point operator*(const double value, const geometry_msgs::msg::Point & point)
-{
-  return point * value;
-}
-
-geometry_msgs::msg::Point operator+(
-  const geometry_msgs::msg::Point & v0, const geometry_msgs::msg::Vector3 & v1)
-{
-  geometry_msgs::msg::Point ret;
-  ret.x = v0.x + v1.x;
-  ret.y = v0.y + v1.y;
-  ret.z = v0.z + v1.z;
-  return ret;
-}
-
-geometry_msgs::msg::Vector3 operator+(
-  const geometry_msgs::msg::Vector3 & v0, const geometry_msgs::msg::Vector3 & v1)
-{
-  geometry_msgs::msg::Vector3 ret;
-  ret.x = v0.x + v1.x;
-  ret.y = v0.y + v1.y;
-  ret.z = v0.z + v1.z;
-  return ret;
-}
-
-geometry_msgs::msg::Point operator+(
-  const geometry_msgs::msg::Point & v0, const geometry_msgs::msg::Point & v1)
-{
-  geometry_msgs::msg::Point ret;
-  ret.x = v0.x + v1.x;
-  ret.y = v0.y + v1.y;
-  ret.z = v0.z + v1.z;
-  return ret;
-}
-
-geometry_msgs::msg::Point operator-(
-  const geometry_msgs::msg::Point & v0, const geometry_msgs::msg::Vector3 & v1)
-{
-  geometry_msgs::msg::Point ret;
-  ret.x = v0.x - v1.x;
-  ret.y = v0.y - v1.y;
-  ret.z = v0.z - v1.z;
-  return ret;
-}
-
-geometry_msgs::msg::Vector3 operator-(
-  const geometry_msgs::msg::Vector3 & v0, const geometry_msgs::msg::Vector3 & v1)
-{
-  geometry_msgs::msg::Vector3 ret;
-  ret.x = v0.x - v1.x;
-  ret.y = v0.y - v1.y;
-  ret.z = v0.z - v1.z;
-  return ret;
-}
-
-geometry_msgs::msg::Point operator-(
-  const geometry_msgs::msg::Point & v0, const geometry_msgs::msg::Point & v1)
-{
-  geometry_msgs::msg::Point ret;
-  ret.x = v0.x - v1.x;
-  ret.y = v0.y - v1.y;
-  ret.z = v0.z - v1.z;
-  return ret;
-}
diff --git a/common/math/geometry/src/polygon/line_segment.cpp b/common/math/geometry/src/polygon/line_segment.cpp
index 827d529660c..b942d1d2f8e 100644
--- a/common/math/geometry/src/polygon/line_segment.cpp
+++ b/common/math/geometry/src/polygon/line_segment.cpp
@@ -252,7 +252,13 @@ auto LineSegment::getIntersection2D(const LineSegment & line) const
 
 auto LineSegment::getVector() const -> geometry_msgs::msg::Vector3
 {
-  return end_point - start_point;
+  using math::geometry::operator-;
+  const auto result_pt = end_point - start_point;
+  auto result = geometry_msgs::msg::Vector3();
+  result.x = result_pt.x;
+  result.y = result_pt.y;
+  result.z = result_pt.z;
+  return result;
 }
 
 /**
diff --git a/common/math/geometry/src/spline/catmull_rom_spline.cpp b/common/math/geometry/src/spline/catmull_rom_spline.cpp
index 11c2143373d..523b6f28099 100644
--- a/common/math/geometry/src/spline/catmull_rom_spline.cpp
+++ b/common/math/geometry/src/spline/catmull_rom_spline.cpp
@@ -12,7 +12,6 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#include <geometry/linear_algebra.hpp>
 #include <geometry/spline/catmull_rom_spline.hpp>
 #include <iostream>
 #include <limits>
diff --git a/common/math/geometry/test/CMakeLists.txt b/common/math/geometry/test/CMakeLists.txt
index 71741d743ea..dfd0a09460b 100644
--- a/common/math/geometry/test/CMakeLists.txt
+++ b/common/math/geometry/test/CMakeLists.txt
@@ -6,21 +6,21 @@ ament_add_gtest(test_catmull_rom_subspline test_catmull_rom_subspline.cpp)
 ament_add_gtest(test_collision test_collision.cpp)
 ament_add_gtest(test_distance test_distance.cpp)
 ament_add_gtest(test_hermite_curve test_hermite_curve.cpp)
-ament_add_gtest(test_linear_algebra test_linear_algebra.cpp)
 ament_add_gtest(test_polygon test_polygon.cpp)
 ament_add_gtest(test_polynomial_solver test_polynomial_solver.cpp)
 ament_add_gtest(test_transform test_transform.cpp)
 ament_add_gtest(test_intersection test_intersection.cpp)
 ament_add_gtest(test_line_segment test_line_segment.cpp)
+ament_add_gtest(test_quaternion test_quaternion.cpp)
 target_link_libraries(test_bounding_box geometry)
 target_link_libraries(test_catmull_rom_spline geometry)
 target_link_libraries(test_catmull_rom_subspline geometry)
 target_link_libraries(test_collision geometry)
 target_link_libraries(test_distance geometry)
 target_link_libraries(test_hermite_curve geometry)
-target_link_libraries(test_linear_algebra geometry)
 target_link_libraries(test_polygon geometry)
 target_link_libraries(test_polynomial_solver geometry)
 target_link_libraries(test_transform geometry)
 target_link_libraries(test_intersection geometry)
 target_link_libraries(test_line_segment geometry)
+target_link_libraries(test_quaternion geometry)
diff --git a/common/math/geometry/test/test_linear_algebra.cpp b/common/math/geometry/test/test_linear_algebra.cpp
deleted file mode 100644
index 567f25d3b42..00000000000
--- a/common/math/geometry/test/test_linear_algebra.cpp
+++ /dev/null
@@ -1,158 +0,0 @@
-// 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 <gtest/gtest.h>
-
-#include <geometry/linear_algebra.hpp>
-#include <limits>
-
-#include "expect_eq_macros.hpp"
-#include "test_utils.hpp"
-
-constexpr double EPS = 1e-3;
-
-TEST(LINEAR_ALGEBRA, GET_SIZE_ZERO)
-{
-  geometry_msgs::msg::Vector3 vec;
-  EXPECT_DOUBLE_EQ(math::geometry::getSize(vec), 0.0);
-}
-
-TEST(LINEAR_ALGEBRA, GET_SIZE)
-{
-  geometry_msgs::msg::Vector3 vec = makeVector(1.0, 0.0, 3.0);
-  EXPECT_DOUBLE_EQ(math::geometry::getSize(vec), std::sqrt(10.0));
-}
-
-TEST(LINEAR_ALGEBRA, NORMALIZE_ZERO)
-{
-  geometry_msgs::msg::Vector3 vec;
-  EXPECT_THROW(math::geometry::normalize(vec), common::SimulationError);
-}
-
-TEST(LINEAR_ALGEBRA, NORMALIZE)
-{
-  geometry_msgs::msg::Vector3 vec = makeVector(1.0, 0.0, 3.0);
-  vec = math::geometry::normalize(vec);
-
-  geometry_msgs::msg::Vector3 ans = makeVector(0.31622776601683794, 0.0, 0.94868329805051377);
-  EXPECT_VECTOR3_EQ(vec, ans);
-  EXPECT_DOUBLE_EQ(math::geometry::getSize(vec), 1.0);
-}
-
-TEST(LINEAR_ALGEBRA, INNER_PRODUCT)
-{
-  geometry_msgs::msg::Vector3 vec0 = makeVector(1.0, 0.0, 3.0), vec1 = makeVector(-1.0, 0.0, -3.0);
-  EXPECT_DOUBLE_EQ(math::geometry::innerProduct(vec0, vec1), -10.0);
-}
-
-TEST(LINEAR_ALGEBRA, INNER_PRODUCT_IDENTICAL)
-{
-  geometry_msgs::msg::Vector3 vec0 = makeVector(1.0, 0.0, 3.0);
-  EXPECT_DOUBLE_EQ(math::geometry::innerProduct(vec0, vec0), 10.0);
-}
-
-TEST(LINEAR_ALGEBRA, INNER_PRODUCT_ZERO)
-{
-  geometry_msgs::msg::Vector3 vec0, vec1 = makeVector(1.0, 0.0, 3.0);
-  EXPECT_DOUBLE_EQ(math::geometry::innerProduct(vec0, vec1), 0.0);
-}
-
-TEST(LINEAR_ALGEBRA, GET_INTERNAL_ANGLE)
-{
-  geometry_msgs::msg::Vector3 vec0 = makeVector(1.0, 0.0, 3.0), vec1 = makeVector(-1.0, 0.0, -3.0);
-  EXPECT_NEAR(math::geometry::getInternalAngle(vec0, vec1), M_PI, EPS);
-}
-
-TEST(LINEAR_ALGEBRA, GET_INTERNAL_ANGLE_IDENTICAL)
-{
-  geometry_msgs::msg::Vector3 vec0 = makeVector(1.0, 0.0, 3.0);
-  EXPECT_NEAR(math::geometry::getInternalAngle(vec0, vec0), 0.0, EPS);
-}
-
-TEST(LINEAR_ALGEBRA, GET_INTERNAL_ANGLE_ZERO)
-{
-  geometry_msgs::msg::Vector3 vec0, vec1 = makeVector(1.0, 0.0, 3.0);
-  EXPECT_THROW(math::geometry::getInternalAngle(vec0, vec1), common::SimulationError);
-}
-
-TEST(LINEAR_ALGEBRA, DIVIDE)
-{
-  geometry_msgs::msg::Vector3 vec = makeVector(0.0, 3.0, 1.0);
-  EXPECT_VECTOR3_EQ((vec / 2.0), makeVector(0.0, 1.5, 0.5));
-}
-
-TEST(LINEAR_ALGEBRA, DIVIDE_ZERO)
-{
-  geometry_msgs::msg::Vector3 vec = makeVector(0.0, 3.0, 1.0);
-  vec = vec / 0.0;
-  EXPECT_TRUE(std::isnan(vec.x));
-  EXPECT_TRUE(std::isinf(vec.y));
-  EXPECT_TRUE(std::isinf(vec.z));
-}
-
-TEST(LINEAR_ALGEBRA, MULTIPLY_FIRST)
-{
-  geometry_msgs::msg::Vector3 vec = makeVector(0.0, 3.0, 1.0);
-  EXPECT_VECTOR3_EQ((vec * 2.0), makeVector(0.0, 6.0, 2.0));
-}
-
-TEST(LINEAR_ALGEBRA, MULTIPLY_SECOND)
-{
-  geometry_msgs::msg::Vector3 vec = makeVector(0.0, 3.0, 1.0);
-  EXPECT_VECTOR3_EQ((2.0 * vec), makeVector(0.0, 6.0, 2.0));
-}
-
-TEST(LINEAR_ALGEBRA, ADDITION_POINT_VECTOR)
-{
-  geometry_msgs::msg::Vector3 vec = makeVector(0.0, 3.0, 1.0);
-  geometry_msgs::msg::Point p = makePoint(0.0, 3.0, 1.0);
-  EXPECT_VECTOR3_EQ((p + vec), makeVector(0.0, 6.0, 2.0));
-}
-
-TEST(LINEAR_ALGEBRA, ADDITION_VECTOR_VECTOR)
-{
-  geometry_msgs::msg::Vector3 vec = makeVector(0.0, 3.0, 1.0);
-  EXPECT_VECTOR3_EQ((vec + vec), makeVector(0.0, 6.0, 2.0));
-}
-
-TEST(LINEAR_ALGEBRA, ADDITION_POINT_POINT)
-{
-  geometry_msgs::msg::Point p0 = makePoint(0.0, 3.0, 1.0), p1 = makePoint(2.0, 3.0, 1.0);
-  EXPECT_VECTOR3_EQ((p0 + p1), makePoint(2.0, 6.0, 2.0));
-}
-
-TEST(LINEAR_ALGEBRA, SUBTRACTION_POINT_VECTOR)
-{
-  geometry_msgs::msg::Vector3 vec = makeVector(0.0, 3.0, 1.0);
-  geometry_msgs::msg::Point p = makePoint(0.0, 3.0, 1.0);
-  EXPECT_VECTOR3_EQ((p - vec), geometry_msgs::msg::Vector3());
-}
-
-TEST(LINEAR_ALGEBRA, SUBTRACTION_VECTOR_VECTOR)
-{
-  geometry_msgs::msg::Vector3 vec = makeVector(0.0, 3.0, 1.0);
-  EXPECT_VECTOR3_EQ((vec - vec), geometry_msgs::msg::Vector3());
-}
-
-TEST(LINEAR_ALGEBRA, SUBTRACTION_POINT_POINT)
-{
-  geometry_msgs::msg::Point p0 = makePoint(0.0, 3.0, 1.0), p1 = makePoint(2.0, 3.0, 1.0);
-  EXPECT_VECTOR3_EQ((p0 - p1), makePoint(-2.0, 0.0, 0.0));
-}
-
-int main(int argc, char ** argv)
-{
-  testing::InitGoogleTest(&argc, argv);
-  return RUN_ALL_TESTS();
-}
diff --git a/common/math/geometry/test/test_quaternion.cpp b/common/math/geometry/test/test_quaternion.cpp
new file mode 100644
index 00000000000..7dfd1285664
--- /dev/null
+++ b/common/math/geometry/test/test_quaternion.cpp
@@ -0,0 +1,70 @@
+// 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 <gtest/gtest.h>
+
+#include <geometry/quaternion/is_like_quaternion.hpp>
+#include <geometry/quaternion/make_quaternion.hpp>
+#include <geometry/quaternion/operator.hpp>
+
+#include "expect_eq_macros.hpp"
+#include "test_utils.hpp"
+
+constexpr double EPS = 1e-6;
+
+TEST(Quaternion, testCase1)
+{
+  using math::geometry::operator+;
+
+  auto q1 = math::geometry::makeQuaternion(0, 1, 0, 1);
+  auto q2 = math::geometry::makeQuaternion(0, 1, 0, 1);
+  auto ans = q1 + q2;
+  EXPECT_QUATERNION_EQ(ans, math::geometry::makeQuaternion(0, 2, 0, 2))
+}
+
+TEST(Quaternion, testCase2)
+{
+  using math::geometry::operator-;
+
+  auto q1 = math::geometry::makeQuaternion(0, 1, 0, 1);
+  auto q2 = math::geometry::makeQuaternion(0, 1, 0, 1);
+  auto ans = q1 - q2;
+  EXPECT_QUATERNION_EQ(ans, math::geometry::makeQuaternion(0, 0, 0, 0))
+}
+
+TEST(Quaternion, testCase3)
+{
+  using math::geometry::operator*;
+
+  auto q1 = math::geometry::makeQuaternion(0, 1, 0, 1);
+  auto q2 = math::geometry::makeQuaternion(0, 1, 0, 1);
+  auto ans = q1 * q2;
+  EXPECT_QUATERNION_EQ(ans, math::geometry::makeQuaternion(0, 2, 0, 0))
+}
+
+TEST(Quaternion, testCase4)
+{
+  using math::geometry::operator+=;
+
+  auto q1 = math::geometry::makeQuaternion(0, 1, 0, 1);
+  auto q2 = math::geometry::makeQuaternion(0, 1, 0, 1);
+  q1 += q2;
+  EXPECT_QUATERNION_EQ(q1, math::geometry::makeQuaternion(0, 2, 0, 2))
+}
+
+int main(int argc, char ** argv)
+{
+  testing::InitGoogleTest(&argc, argv);
+  return RUN_ALL_TESTS();
+}
diff --git a/common/math/geometry/test/vector3/test_vector3.cpp b/common/math/geometry/test/vector3/test_vector3.cpp
index 6f47513d8a0..b07cfede363 100644
--- a/common/math/geometry/test/vector3/test_vector3.cpp
+++ b/common/math/geometry/test/vector3/test_vector3.cpp
@@ -15,8 +15,9 @@
 #include <gtest/gtest.h>
 
 #include <cmath>
-#include <geometry/linear_algebra.hpp>
 #include <geometry/vector3/hypot.hpp>
+#include <geometry/vector3/inner_product.hpp>
+#include <geometry/vector3/internal_angle.hpp>
 #include <geometry/vector3/norm.hpp>
 #include <geometry/vector3/normalize.hpp>
 #include <geometry/vector3/operator.hpp>
@@ -183,6 +184,70 @@ TEST(Vector3, additionAssignment_customVector)
   EXPECT_VECTOR3_EQ(vec0, makeVector(0.0, 0.0, 2.0));
 }
 
+TEST(Vector3, vector3_getSizeZero)
+{
+  geometry_msgs::msg::Vector3 vec;
+  EXPECT_DOUBLE_EQ(math::geometry::norm(vec), 0.0);
+}
+
+TEST(Vector3, vector3_getSize)
+{
+  geometry_msgs::msg::Vector3 vec = makeVector(1.0, 0.0, 3.0);
+  EXPECT_DOUBLE_EQ(math::geometry::norm(vec), std::sqrt(10.0));
+}
+
+TEST(Vector3, vector3_normalizeZero)
+{
+  geometry_msgs::msg::Vector3 vec;
+  EXPECT_THROW(math::geometry::normalize(vec), common::SimulationError);
+}
+
+TEST(Vector3, vector3_normalize)
+{
+  geometry_msgs::msg::Vector3 vec = makeVector(1.0, 0.0, 3.0);
+  vec = math::geometry::normalize(vec);
+
+  geometry_msgs::msg::Vector3 ans = makeVector(0.31622776601683794, 0.0, 0.94868329805051377);
+  EXPECT_VECTOR3_EQ(vec, ans);
+  EXPECT_DOUBLE_EQ(math::geometry::norm(vec), 1.0);
+}
+
+TEST(Vector3, innerProduct_getInnerProduct)
+{
+  geometry_msgs::msg::Vector3 vec0 = makeVector(1.0, 0.0, 3.0), vec1 = makeVector(-1.0, 0.0, -3.0);
+  EXPECT_DOUBLE_EQ(math::geometry::innerProduct(vec0, vec1), -10.0);
+}
+
+TEST(Vector3, innerProduct_identical)
+{
+  geometry_msgs::msg::Vector3 vec0 = makeVector(1.0, 0.0, 3.0);
+  EXPECT_DOUBLE_EQ(math::geometry::innerProduct(vec0, vec0), 10.0);
+}
+
+TEST(Vector3, innerProduct_zero)
+{
+  geometry_msgs::msg::Vector3 vec0, vec1 = makeVector(1.0, 0.0, 3.0);
+  EXPECT_DOUBLE_EQ(math::geometry::innerProduct(vec0, vec1), 0.0);
+}
+
+TEST(Vector3, innerAngle_getInnerAngle)
+{
+  geometry_msgs::msg::Vector3 vec0 = makeVector(1.0, 0.0, 3.0), vec1 = makeVector(-1.0, 0.0, -3.0);
+  EXPECT_NEAR(math::geometry::getInternalAngle(vec0, vec1), M_PI, EPS);
+}
+
+TEST(Vector3, innerAngle_angleIdentical)
+{
+  geometry_msgs::msg::Vector3 vec0 = makeVector(1.0, 0.0, 3.0);
+  EXPECT_NEAR(math::geometry::getInternalAngle(vec0, vec0), 0.0, EPS);
+}
+
+TEST(Vector3, innerAngle_angleZero)
+{
+  geometry_msgs::msg::Vector3 vec0, vec1 = makeVector(1.0, 0.0, 3.0);
+  EXPECT_THROW(math::geometry::getInternalAngle(vec0, vec1), common::SimulationError);
+}
+
 int main(int argc, char ** argv)
 {
   testing::InitGoogleTest(&argc, argv);
diff --git a/simulation/do_nothing_plugin/src/plugin.cpp b/simulation/do_nothing_plugin/src/plugin.cpp
index 76a4baec2fe..2bc20fe9041 100644
--- a/simulation/do_nothing_plugin/src/plugin.cpp
+++ b/simulation/do_nothing_plugin/src/plugin.cpp
@@ -13,8 +13,8 @@
 // limitations under the License.
 
 #include <do_nothing_plugin/plugin.hpp>
-#include <geometry/linear_algebra.hpp>
 #include <geometry/vector3/hypot.hpp>
+#include <geometry/vector3/operator.hpp>
 
 namespace entity_behavior
 {
@@ -67,6 +67,10 @@ auto interpolateEntityStatusFromPolylineTrajectory(
   const std::shared_ptr<traffic_simulator::CanonicalizedEntityStatus> & entity_status,
   double current_time, double step_time) -> std::optional<traffic_simulator::EntityStatus>
 {
+  using math::geometry::operator*;
+  using math::geometry::operator-;
+  using math::geometry::operator+;
+
   if (!trajectory) {
     return std::nullopt;
   }
diff --git a/simulation/traffic_simulator/src/behavior/follow_trajectory.cpp b/simulation/traffic_simulator/src/behavior/follow_trajectory.cpp
index 7670a2d95f4..978dc4f7c9d 100644
--- a/simulation/traffic_simulator/src/behavior/follow_trajectory.cpp
+++ b/simulation/traffic_simulator/src/behavior/follow_trajectory.cpp
@@ -15,8 +15,8 @@
 #include <quaternion_operation/quaternion_operation.h>
 
 #include <arithmetic/floating_point/comparison.hpp>
-#include <geometry/linear_algebra.hpp>
 #include <geometry/vector3/hypot.hpp>
+#include <geometry/vector3/inner_product.hpp>
 #include <geometry/vector3/norm.hpp>
 #include <geometry/vector3/normalize.hpp>
 #include <geometry/vector3/operator.hpp>
diff --git a/simulation/traffic_simulator/src/behavior/longitudinal_speed_planning.cpp b/simulation/traffic_simulator/src/behavior/longitudinal_speed_planning.cpp
index bb49afa620e..d2efc60ff89 100644
--- a/simulation/traffic_simulator/src/behavior/longitudinal_speed_planning.cpp
+++ b/simulation/traffic_simulator/src/behavior/longitudinal_speed_planning.cpp
@@ -13,7 +13,7 @@
 // limitations under the License.
 
 #include <algorithm>
-#include <geometry/linear_algebra.hpp>
+#include <geometry/vector3/operator.hpp>
 #include <iostream>
 #include <rclcpp/rclcpp.hpp>
 #include <scenario_simulator_exception/exception.hpp>
@@ -290,6 +290,9 @@ auto LongitudinalSpeedPlanner::forward(
   const traffic_simulator_msgs::msg::DynamicConstraints & constraints) const
   -> geometry_msgs::msg::Twist
 {
+  using math::geometry::operator*;
+  using math::geometry::operator+;
+
   geometry_msgs::msg::Twist ret = twist;
   ret.linear = ret.linear + accel.linear * step_time;
   ret.linear.x = std::clamp(ret.linear.x, -1 * constraints.max_speed, constraints.max_speed);
@@ -301,6 +304,9 @@ auto LongitudinalSpeedPlanner::timeDerivative(
   const geometry_msgs::msg::Twist & before, const geometry_msgs::msg::Twist & after) const
   -> geometry_msgs::msg::Accel
 {
+  using math::geometry::operator-;
+  using math::geometry::operator/;
+
   geometry_msgs::msg::Accel ret;
   ret.linear = (after.linear - before.linear) / step_time;
   ret.angular = (after.angular - before.angular) / step_time;
diff --git a/simulation/traffic_simulator/src/entity/entity_manager.cpp b/simulation/traffic_simulator/src/entity/entity_manager.cpp
index 7a1e372be26..d3d9c7759d0 100644
--- a/simulation/traffic_simulator/src/entity/entity_manager.cpp
+++ b/simulation/traffic_simulator/src/entity/entity_manager.cpp
@@ -16,8 +16,8 @@
 #include <geometry/bounding_box.hpp>
 #include <geometry/distance.hpp>
 #include <geometry/intersection/collision.hpp>
-#include <geometry/linear_algebra.hpp>
 #include <geometry/transform.hpp>
+#include <geometry/vector3/operator.hpp>
 #include <limits>
 #include <memory>
 #include <optional>
@@ -39,6 +39,9 @@ namespace entity
 {
 void EntityManager::broadcastEntityTransform()
 {
+  using math::geometry::operator/;
+  using math::geometry::operator*;
+  using math::geometry::operator+=;
   std::vector<std::string> names = getEntityNames();
   /**
    * @note This part of the process is intended to ensure that frames are issued in a position that makes 
@@ -69,9 +72,9 @@ void EntityManager::broadcastEntityTransform()
         .pose(geometry_msgs::build<geometry_msgs::msg::Pose>()
                 .position(std::accumulate(
                   names.begin(), names.end(), geometry_msgs::msg::Point(),
-                  [this, names](geometry_msgs::msg::Point & point, const std::string & name) {
-                    return point +
-                           (getMapPose(name).position * (1.0 / static_cast<double>(names.size())));
+                  [this, names](geometry_msgs::msg::Point point, const std::string & name) {
+                    point += getMapPose(name).position * (1.0 / static_cast<double>(names.size()));
+                    return point;
                   }))
                 .orientation(geometry_msgs::msg::Quaternion())),
       true);
diff --git a/simulation/traffic_simulator/src/hdmap_utils/hdmap_utils.cpp b/simulation/traffic_simulator/src/hdmap_utils/hdmap_utils.cpp
index 8704432f23a..c0b4affc83d 100644
--- a/simulation/traffic_simulator/src/hdmap_utils/hdmap_utils.cpp
+++ b/simulation/traffic_simulator/src/hdmap_utils/hdmap_utils.cpp
@@ -27,10 +27,13 @@
 #include <boost/geometry/geometries/point_xy.hpp>
 #include <boost/geometry/geometries/polygon.hpp>
 #include <deque>
-#include <geometry/linear_algebra.hpp>
+#include <geometry/quaternion/operator.hpp>
 #include <geometry/spline/catmull_rom_spline.hpp>
 #include <geometry/spline/hermite_curve.hpp>
 #include <geometry/transform.hpp>
+#include <geometry/vector3/inner_product.hpp>
+#include <geometry/vector3/normalize.hpp>
+#include <geometry/vector3/operator.hpp>
 #include <lanelet2_extension/io/autoware_osm_parser.hpp>
 #include <lanelet2_extension/projection/mgrs_projector.hpp>
 #include <lanelet2_extension/utility/message_conversion.hpp>
@@ -1398,6 +1401,8 @@ auto HdMapUtils::toMapPose(
   const traffic_simulator_msgs::msg::LaneletPose & lanelet_pose, const bool fill_pitch) const
   -> geometry_msgs::msg::PoseStamped
 {
+  using math::geometry::operator*;
+  using math::geometry::operator+=;
   if (
     const auto pose = std::get<std::optional<traffic_simulator_msgs::msg::LaneletPose>>(
       canonicalizeLaneletPose(lanelet_pose))) {
@@ -1406,8 +1411,8 @@ auto HdMapUtils::toMapPose(
     const auto spline = getCenterPointsSpline(pose->lanelet_id);
     ret.pose = spline->getPose(pose->s);
     const auto normal_vec = spline->getNormalVector(pose->s);
-    const auto diff = math::geometry::normalize(normal_vec) * pose->offset;
-    ret.pose.position = ret.pose.position + diff;
+    const auto diff = math::geometry::normalize(normal_vec) * pose->offset;  //this
+    ret.pose.position += diff;
     const auto tangent_vec = spline->getTangentVector(pose->s);
     geometry_msgs::msg::Vector3 rpy;
     rpy.x = 0.0;
diff --git a/test_runner/random_test_runner/src/lanelet_utils.cpp b/test_runner/random_test_runner/src/lanelet_utils.cpp
index e8159857e68..aeaf2e85e07 100644
--- a/test_runner/random_test_runner/src/lanelet_utils.cpp
+++ b/test_runner/random_test_runner/src/lanelet_utils.cpp
@@ -22,7 +22,8 @@
 #include <lanelet2_traffic_rules/TrafficRulesFactory.h>
 
 #include <geographic_msgs/msg/geo_point.hpp>
-#include <geometry/linear_algebra.hpp>
+#include <geometry/vector3/normalize.hpp>
+#include <geometry/vector3/operator.hpp>
 #include <lanelet2_extension/projection/mgrs_projector.hpp>
 #include <optional>
 #include <traffic_simulator/hdmap_utils/hdmap_utils.hpp>
@@ -93,6 +94,9 @@ std::optional<traffic_simulator_msgs::msg::LaneletPose> LaneletUtils::getOpposit
   // TODO: Multiple same-direction lane support
   // TODO: Find lane width for current s value
 
+  using math::geometry::operator*;
+  using math::geometry::operator+;
+
   if (!lanelet_map_ptr_->laneletLayer.exists(pose.lanelet_id)) {
     return {};
   }