Port remaining Utils tests

engine/BUILD.bazel

@@ -159,7 +159,6 @@ cc_test(
     data = [":open_scenario_engine_test_data"],
     tags = ["test"],
     deps = [
-        # ":open_scenario_builders",
         ":open_scenario_engine",
         ":open_scenario_engine_test_utils",
         "@googletest//:gtest_main",

engine/CMakeLists.txt

@@ -205,6 +205,8 @@ target_sources(
           ${CMAKE_CURRENT_LIST_DIR}/tests/Conversion/OscToMantle/ConvertScenarioTrajectoryRefTest.cpp
           ${CMAKE_CURRENT_LIST_DIR}/tests/Utils/ControllerCreatorTest.cpp
           ${CMAKE_CURRENT_LIST_DIR}/tests/Utils/EntityCreatorTest.cpp
+          ${CMAKE_CURRENT_LIST_DIR}/tests/Utils/ConstantsTest.cpp
+          ${CMAKE_CURRENT_LIST_DIR}/tests/Utils/EntityUtilsTest.cpp
 )
 
 add_custom_command(TARGET ${PROJECT_NAME}Test

engine/tests/Utils/ConstantsTest.cpp

+/*******************************************************************************
+ * Copyright (c) 2023, Bayerische Motoren Werke Aktiengesellschaft (BMW AG)
+ *
+ * This program and the accompanying materials are made
+ * available under the terms of the Eclipse Public License 2.0
+ * which is available at https://www.eclipse.org/legal/epl-2.0/
+ *
+ * SPDX-License-Identifier: EPL-2.0
+ *******************************************************************************/
+
+#include "Utils/Constants.h"
+
+#include <gtest/gtest.h>
+
+namespace OPENSCENARIO
+{
+
+TEST(ConstantsTest, GivenInvalidName_WhenIsDrivingFunctionControlledEntityCalled_ThenReturnFalse)
+{
+    const std::string invalid_name = "arbitrary_invalid_name";
+    EXPECT_FALSE(IsDrivingFunctionControlledEntity(invalid_name));
+}
+
+class ConstantsValidNamesTestFixture : public ::testing::TestWithParam<std::string>
+{
+};
+
+INSTANTIATE_TEST_SUITE_P(ValidNames,
+                         ConstantsValidNamesTestFixture,
+                         ::testing::ValuesIn(std::vector<std::string>{"Ego", "Host", "M1: something"}));
+
+TEST_P(ConstantsValidNamesTestFixture, GivenValidName_WhenIsDrivingFunctionControlledEntityCalled_ThenReturnTrue)
+{
+    std::string valid_name = GetParam();
+
+    EXPECT_TRUE(IsDrivingFunctionControlledEntity(valid_name));
+}
+
+}  // namespace OPENSCENARIO

engine/tests/Utils/EntityUtilsTest.cpp

+/*******************************************************************************
+ * Copyright (c) 2023, Bayerische Motoren Werke Aktiengesellschaft (BMW AG)
+ *
+ * This program and the accompanying materials are made
+ * available under the terms of the Eclipse Public License 2.0
+ * which is available at https://www.eclipse.org/legal/epl-2.0/
+ *
+ * SPDX-License-Identifier: EPL-2.0
+ *******************************************************************************/
+
+#include "Utils/EntityUtils.h"
+#include "TestUtils.h"
+
+#include <gtest/gtest.h>
+
+#include <iostream>
+
+namespace OPENSCENARIO
+{
+using namespace units::literals;
+
+class EntityUtilsTestFixture : public OpenScenarioEngineLibraryTestBase
+{
+};
+
+TEST_F(EntityUtilsTestFixture,
+       GivenTwoEntitiesAt7metersDistance_WhenCalculateLongitudinalFreeSpaceDistance_ThenDistanceBetweenIsCalculated)
+{
+    mantle_api::MockVehicle master_vehicle_mock{};
+    mantle_api::MockVehicle reference_vehicle_mock{};
+
+    mantle_api::VehicleProperties properties{};
+    properties.bounding_box.dimension.length = 5.0_m;
+    properties.bounding_box.dimension.width = 2.0_m;
+    properties.bounding_box.dimension.height = 1.8_m;
+    properties.bounding_box.geometric_center.x = 1.4_m;
+    properties.bounding_box.geometric_center.y = 0.0_m;
+    properties.bounding_box.geometric_center.z = 0.9_m;
+
+    auto master_vehicle_properties = std::make_unique<mantle_api::EntityProperties>(properties);
+    auto reference_vehicle_properties = std::make_unique<mantle_api::EntityProperties>(properties);
+
+    master_vehicle_mock.SetProperties(std::move(master_vehicle_properties));
+    reference_vehicle_mock.SetProperties(std::move(reference_vehicle_properties));
+
+    EXPECT_CALL(master_vehicle_mock, GetPosition())
+        .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())
+        .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 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};
+
+    EXPECT_CALL(dynamic_cast<const mantle_api::MockGeometryHelper&>(*(env_->GetGeometryHelper())),
+                TranslateGlobalPositionLocally(
+                    reference_vehicle_mock.GetPosition(), reference_vehicle_mock.GetOrientation(), testing::_))
+        .WillRepeatedly(testing::Return(mock_position_reference));
+
+    auto expected_distance = 7.5_m;
+    auto actual_distance =
+        EntityUtils::CalculateLongitudinalFreeSpaceDistance(env_, master_vehicle_mock, reference_vehicle_mock);
+
+    EXPECT_EQ(expected_distance, actual_distance);
+}
+
+TEST_F(EntityUtilsTestFixture,
+       GivenTwoEntitiesAt10metersDistance_WhenCalculateRelativeLongitudinalDistance_ThenDistanceBetweenIsCalculated)
+{
+    mantle_api::MockVehicle master_vehicle_mock{};
+    mantle_api::MockVehicle reference_vehicle_mock{};
+
+    mantle_api::VehicleProperties properties{};
+    properties.bounding_box.dimension.length = 5.0_m;
+    properties.bounding_box.dimension.width = 2.0_m;
+    properties.bounding_box.dimension.height = 1.8_m;
+    properties.bounding_box.geometric_center.x = 1.4_m;
+    properties.bounding_box.geometric_center.y = 0.0_m;
+    properties.bounding_box.geometric_center.z = 0.9_m;
+
+    auto master_vehicle_properties = std::make_unique<mantle_api::EntityProperties>(properties);
+    auto reference_vehicle_properties = std::make_unique<mantle_api::EntityProperties>(properties);
+
+    master_vehicle_mock.SetProperties(std::move(master_vehicle_properties));
+    reference_vehicle_mock.SetProperties(std::move(reference_vehicle_properties));
+
+    EXPECT_CALL(master_vehicle_mock, GetPosition())
+        .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())
+        .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};
+
+    EXPECT_CALL(dynamic_cast<const mantle_api::MockGeometryHelper&>(*(env_->GetGeometryHelper())),
+                TranslateGlobalPositionLocally(testing::_, testing::_, testing::_))
+        .WillOnce(testing::Return(mock_position_master))
+        .WillOnce(testing::Return(mock_position_reference));
+
+    auto expected_distance = 10_m;
+    auto actual_distance =
+        EntityUtils::CalculateRelativeLongitudinalDistance(env_, master_vehicle_mock, reference_vehicle_mock);
+
+    EXPECT_EQ(expected_distance, actual_distance);
+}
+
+TEST_F(EntityUtilsTestFixture,
+       GivenTwoEntitiesAt10metersDistance_WhenGetBoundingBoxCornerPoints_ThenReturnObjectCornerPointsOfTheEntity)
+{
+    mantle_api::MockVehicle vehicle_mock{};
+
+    mantle_api::VehicleProperties properties{};
+    properties.bounding_box.dimension.length = 5.0_m;
+    properties.bounding_box.dimension.width = 2.0_m;
+    properties.bounding_box.dimension.height = 1.8_m;
+    properties.bounding_box.geometric_center.x = 1.4_m;
+    properties.bounding_box.geometric_center.y = 0.0_m;
+    properties.bounding_box.geometric_center.z = 0.9_m;
+
+    auto master_vehicle_properties = std::make_unique<mantle_api::EntityProperties>(properties);
+    vehicle_mock.SetProperties(std::move(master_vehicle_properties));
+
+    auto actual_corner_points = EntityUtils::GetBoundingBoxCornerPoints(vehicle_mock);
+
+    std::vector<mantle_api::Vec3<units::length::meter_t>> expected_corner_points{
+        {2.5_m, 1_m, -0.9_m},
+        {2.5_m, -1_m, -0.9_m},
+        {2.5_m, 1_m, 0.9_m},
+        {2.5_m, -1_m, 0.9_m},
+        {-2.5_m, 1_m, -0.9_m},
+        {-2.5_m, -1_m, -0.9_m},
+        {-2.5_m, 1_m, 0.9_m},
+        {-2.5_m, -1_m, 0.9_m},
+    };
+    EXPECT_EQ(expected_corner_points.size(), actual_corner_points.size());
+
+    for (std::size_t i = 0; i < expected_corner_points.size(); i++)
+    {
+        EXPECT_EQ(expected_corner_points[i], actual_corner_points[i]);
+    }
+}
+
+TEST_F(
+    EntityUtilsTestFixture,
+    GivenTwoEntitiesAt10metersDistance_WhenGetBoundingBoxCornerPointsInGlobal_ThenReturnGlobalObjectCornerPointsOfTheEntity)
+{
+
+    const mantle_api::Vec3<units::length::meter_t> position{10_m, 0_m, 0_m};
+    const mantle_api::Orientation3<units::angle::radian_t> orientation{0_rad, 0_rad, 0_rad};
+
+    std::vector<mantle_api::Vec3<units::length::meter_t>> local_corner_points{
+        {1_m, 1_m, -1_m},
+        {1_m, -1_m, -1_m},
+        {1_m, 1_m, 1_m},
+        {1_m, -1_m, 1_m},
+        {-1_m, 1_m, -1_m},
+        {-1_m, -1_m, -1_m},
+        {-1_m, 1_m, 1_m},
+        {-1_m, -1_m, 1_m},
+    };
+
+    EXPECT_CALL(dynamic_cast<const mantle_api::MockGeometryHelper&>(*(env_->GetGeometryHelper())),
+                TranslateGlobalPositionLocally(position, orientation, testing::_))
+        .WillRepeatedly(testing::Return(position));
+    auto actual_global_corner_points =
+        EntityUtils::GetBoundingBoxCornerPointsInGlobal(env_, position, orientation, local_corner_points);
+
+    EXPECT_EQ(local_corner_points.size(), actual_global_corner_points.size());
+
+    for (std::size_t i = 0; i < actual_global_corner_points.size(); i++)
+    {
+        EXPECT_EQ(position, actual_global_corner_points[i]);
+    }
+}
+
+TEST_F(
+    EntityUtilsTestFixture,
+    GivenEntityAndEnvironment_WhenGetCornerPositionsInGlobalSortedByLongitudinalDistanceInLaneDirection_ThenReturnCornerPositionInCorrectOrder)
+{
+    auto& mocked_entity = dynamic_cast<mantle_api::MockVehicle&>(env_->GetEntityRepository().Get("").value().get());
+    auto& mocked_geo_helper = dynamic_cast<const mantle_api::MockGeometryHelper&>(*(env_->GetGeometryHelper()));
+
+    const auto headmost_corner = mantle_api::Vec3<units::length::meter_t>{3.0_m, 0_m, 0_m};
+    const auto rearmost_corner = mantle_api::Vec3<units::length::meter_t>{-1.0_m, 0_m, 0_m};
+
+    const auto other_corner = mantle_api::Vec3<units::length::meter_t>{0.0_m, 0_m, 0_m};
+
+    const auto entity_position = mantle_api::Vec3<units::length::meter_t>{2.0_m, 2.1_m, 0_m};
+    const auto entity_orientation = mantle_api::Orientation3<units::angle::radian_t>{2.2_rad, 2.3_rad, 0_rad};
+    EXPECT_CALL(mocked_entity, GetPosition()).WillOnce(testing::Return(entity_position));
+    EXPECT_CALL(mocked_entity, GetOrientation()).WillOnce(testing::Return(entity_orientation));
+
+    EXPECT_CALL(mocked_geo_helper, TranslateGlobalPositionLocally(entity_position, entity_orientation, testing::_))
+        .Times(8)
+        .WillOnce(::testing::Return(headmost_corner))
+        .WillOnce(::testing::Return(rearmost_corner))
+        .WillRepeatedly(::testing::Return(other_corner));
+
+    const auto local_origin = mantle_api::Vec3<units::length::meter_t>{1.0_m, 1.1_m, 0_m};
+    const auto local_orientation = mantle_api::Orientation3<units::angle::radian_t>{1.2_rad, 1.3_rad, 0_rad};
+    const auto corner_points_in_local = EntityUtils::GetCornerPositionsInLocalSortedByLongitudinalDistance(
+        env_, mocked_entity, local_origin, local_orientation);
+
+    EXPECT_EQ(corner_points_in_local.size(), 8);  // 8 corners
+    EXPECT_EQ(corner_points_in_local.front(), rearmost_corner);
+    EXPECT_EQ(corner_points_in_local.back(), headmost_corner);
+}
+
+}  // namespace OPENSCENARIO