diff --git a/test/EntityUtilsTest.cpp b/test/EntityUtilsTest.cpp
index 88e2df9119cd8bb4d6976746a03a400cd1c6e22c..4273f2f5e41cee0878003de7bab75d9ff90ee590 100644
--- a/test/EntityUtilsTest.cpp
+++ b/test/EntityUtilsTest.cpp
@@ -44,16 +44,28 @@ TEST_F(EntityUtilsTestFixture,
     reference_vehicle_mock.SetProperties(std::move(reference_vehicle_properties));
 
     EXPECT_CALL(master_vehicle_mock, GetPosition())
-        .WillOnce(::testing::Return(mantle_api::Vec3<units::length::meter_t>{0_m, 0_m, 0_m}));
+        .WillRepeatedly(::testing::Return(mantle_api::Vec3<units::length::meter_t>{0_m, 0_m, 0_m}));
 
     EXPECT_CALL(master_vehicle_mock, GetOrientation())
-        .WillOnce(::testing::Return(mantle_api::Orientation3<units::angle::radian_t>{0_rad, 0_rad, 0_rad}));
+        .WillRepeatedly(::testing::Return(mantle_api::Orientation3<units::angle::radian_t>{0_rad, 0_rad, 0_rad}));
 
     EXPECT_CALL(reference_vehicle_mock, GetPosition())
-        .WillOnce(::testing::Return(mantle_api::Vec3<units::length::meter_t>{10_m, 0_m, 0_m}));
+        .WillRepeatedly(::testing::Return(mantle_api::Vec3<units::length::meter_t>{10_m, 0_m, 0_m}));
 
     EXPECT_CALL(reference_vehicle_mock, GetOrientation())
-        .WillOnce(::testing::Return(mantle_api::Orientation3<units::angle::radian_t>{0_rad, 0_rad, 0_rad}));
+        .WillRepeatedly(::testing::Return(mantle_api::Orientation3<units::angle::radian_t>{0_rad, 0_rad, 0_rad}));
+
+    const auto local_entity_corner_points = EntityUtils::GetBoundingBoxCornerPoints(reference_vehicle_mock);
+    const mantle_api::Vec3<units::length::meter_t>& mock_position_reference{10_m, 0_m, 0_m};
+
+    for (std::size_t i = 0; i < local_entity_corner_points.size(); i++)
+    {
+        ON_CALL(env_->geometry_helper_, TranslateGlobalPositionLocally(
+            reference_vehicle_mock.GetPosition(),
+            reference_vehicle_mock.GetOrientation(),
+            local_entity_corner_points[i]
+        )).WillByDefault(testing::Return(mock_position_reference));
+    }
 
     auto expected_distance = 7.5_m;
     auto actual_distance =
@@ -83,17 +95,32 @@ TEST_F(EntityUtilsTestFixture,
     reference_vehicle_mock.SetProperties(std::move(reference_vehicle_properties));
 
     EXPECT_CALL(master_vehicle_mock, GetPosition())
-        .WillOnce(::testing::Return(mantle_api::Vec3<units::length::meter_t>{0_m, 0_m, 0_m}));
+        .WillRepeatedly(::testing::Return(mantle_api::Vec3<units::length::meter_t>{0_m, 0_m, 0_m}));
 
     EXPECT_CALL(master_vehicle_mock, GetOrientation())
         .WillRepeatedly(::testing::Return(mantle_api::Orientation3<units::angle::radian_t>{0_rad, 0_rad, 0_rad}));
 
     EXPECT_CALL(reference_vehicle_mock, GetPosition())
-        .WillOnce(::testing::Return(mantle_api::Vec3<units::length::meter_t>{10_m, 0_m, 0_m}));
+        .WillRepeatedly(::testing::Return(mantle_api::Vec3<units::length::meter_t>{10_m, 0_m, 0_m}));
 
     EXPECT_CALL(reference_vehicle_mock, GetOrientation())
         .WillRepeatedly(::testing::Return(mantle_api::Orientation3<units::angle::radian_t>{0_rad, 0_rad, 0_rad}));
 
+    const mantle_api::Vec3<units::length::meter_t>& mock_position_reference{10_m, 0_m, 0_m};
+    const mantle_api::Vec3<units::length::meter_t>& mock_position_master{0_m, 0_m, 0_m};
+
+    ON_CALL(env_->geometry_helper_, TranslateGlobalPositionLocally(
+        reference_vehicle_mock.GetPosition(),
+        reference_vehicle_mock.GetOrientation(),
+        -reference_vehicle_mock.GetProperties()->bounding_box.geometric_center
+    )).WillByDefault(testing::Return(mock_position_reference));
+
+    ON_CALL(env_->geometry_helper_, TranslateGlobalPositionLocally(
+        master_vehicle_mock.GetPosition(),
+            master_vehicle_mock.GetOrientation(),
+            -master_vehicle_mock.GetProperties()->bounding_box.geometric_center
+    )).WillByDefault(testing::Return(mock_position_master));
+
     auto expected_distance = 10_m;
     auto actual_distance =
         EntityUtils::CalculateRelativeLongitudinalDistance(env_, master_vehicle_mock, reference_vehicle_mock);
@@ -123,22 +150,55 @@ TEST_F(EntityUtilsTestFixture,
 
     EXPECT_CALL(master_vehicle_mock, GetPosition())
         .WillOnce(::testing::Return(mantle_api::Vec3<units::length::meter_t>{10_m, 0_m, 0_m}))
+        .WillOnce(::testing::Return(mantle_api::Vec3<units::length::meter_t>{10_m, 0_m, 0_m}))
+        .WillOnce(::testing::Return(mantle_api::Vec3<units::length::meter_t>{0_m, 0_m, 0_m}))
         .WillOnce(::testing::Return(mantle_api::Vec3<units::length::meter_t>{0_m, 0_m, 0_m}));
 
+
     EXPECT_CALL(master_vehicle_mock, GetOrientation())
         .WillRepeatedly(::testing::Return(mantle_api::Orientation3<units::angle::radian_t>{0_rad, 0_rad, 0_rad}));
 
     EXPECT_CALL(reference_vehicle_mock, GetPosition())
         .WillOnce(::testing::Return(mantle_api::Vec3<units::length::meter_t>{0_m, 0_m, 0_m}))
+        .WillOnce(::testing::Return(mantle_api::Vec3<units::length::meter_t>{0_m, 0_m, 0_m}))
+        .WillOnce(::testing::Return(mantle_api::Vec3<units::length::meter_t>{10_m, 0_m, 0_m}))
         .WillOnce(::testing::Return(mantle_api::Vec3<units::length::meter_t>{10_m, 0_m, 0_m}));
 
     EXPECT_CALL(reference_vehicle_mock, GetOrientation())
         .WillRepeatedly(::testing::Return(mantle_api::Orientation3<units::angle::radian_t>{0_rad, 0_rad, 0_rad}));
 
+    const mantle_api::Vec3<units::length::meter_t>& mock_position_reference{10_m, 0_m, 0_m};
+    const mantle_api::Vec3<units::length::meter_t>& mock_position_master{0_m, 0_m, 0_m};
+
+    ON_CALL(env_->geometry_helper_, TranslateGlobalPositionLocally(
+        reference_vehicle_mock.GetPosition(),
+        reference_vehicle_mock.GetOrientation(),
+        -reference_vehicle_mock.GetProperties()->bounding_box.geometric_center
+    )).WillByDefault(testing::Return(mock_position_reference));
+
+    ON_CALL(env_->geometry_helper_, TranslateGlobalPositionLocally(
+        master_vehicle_mock.GetPosition(),
+            master_vehicle_mock.GetOrientation(),
+            -master_vehicle_mock.GetProperties()->bounding_box.geometric_center
+    )).WillByDefault(testing::Return(mock_position_master));
+
+
     auto expected_distance = 10_m;
     auto actual_distance =
         EntityUtils::CalculateRelativeLongitudinalDistance(env_, master_vehicle_mock, reference_vehicle_mock);
 
+    ON_CALL(env_->geometry_helper_, TranslateGlobalPositionLocally(
+        reference_vehicle_mock.GetPosition(),
+        reference_vehicle_mock.GetOrientation(),
+        -reference_vehicle_mock.GetProperties()->bounding_box.geometric_center
+    )).WillByDefault(testing::Return(mock_position_reference));
+
+    ON_CALL(env_->geometry_helper_, TranslateGlobalPositionLocally(
+        master_vehicle_mock.GetPosition(),
+            master_vehicle_mock.GetOrientation(),
+            -master_vehicle_mock.GetProperties()->bounding_box.geometric_center
+    )).WillByDefault(testing::Return(mock_position_master));
+
     actual_distance =
         EntityUtils::CalculateRelativeLongitudinalDistance(env_, master_vehicle_mock, reference_vehicle_mock);
 
@@ -199,6 +259,12 @@ TEST_F(
         {-1_m, 1_m, 1_m},
         {-1_m, -1_m, 1_m},
     };
+
+    for (std::size_t i = 0; i < local_corner_points.size(); i++)
+    {
+        ON_CALL(env_->geometry_helper_, TranslateGlobalPositionLocally(position, orientation, local_corner_points[i])).WillByDefault(testing::Return(position));
+    }
+
     auto actual_global_corner_points =
         EntityUtils::GetBoundingBoxCornerPointsInGlobal(env_, position, orientation, local_corner_points);