distance_calculator

1

3

6

8{

10{

11

12double DistanceDrivenCalculator::Calculate(const osi3::GroundTruth& ground_truth)

13{

14 double distance = 0.0;

15 auto host_vehicle_index = std::make_optional<int>();

16

17 const auto& host_vehicle_id = ground_truth.host_vehicle_id();

18 const auto moving_objects_count = ground_truth.moving_object_size();

19

20 for (int i = 0; i < moving_objects_count; ++i)

21 {

22 if (host_vehicle_id.value() == ground_truth.moving_object(i).id().value())

23 {

24 host_vehicle_index.value() = i;

25 break;

26 }

27 }

28

29 if (!host_vehicle_index.has_value())

30 {

31 return distance;

32 }

33

34 const auto& host_vehicle_base = ground_truth.moving_object(host_vehicle_index.value()).base();

35

36 if (!is_initialized_)

37 {

38 is_initialized_ = true;

39 last_xpos_ = host_vehicle_base.position().x();

40 last_ypos_ = host_vehicle_base.position().y();

41 return 0;

42 }

43

44 double current_xpos = host_vehicle_base.position().x();

45 double current_ypos = host_vehicle_base.position().y();

46

47 // Values to be substituted in the standard Euclidean distance formula

48 const auto delta_xpos = last_xpos_.value() - current_xpos;

49 const auto delta_ypos = last_ypos_.value() - current_ypos;

50

51 // Euclidean distance formula to calculate distance between two points/positions.

52 distance = std::sqrt((delta_xpos * delta_xpos) + (delta_ypos * delta_ypos));

53

54 // Update value of last position values with the recent current position value.

55 last_xpos_ = current_xpos;

56 last_ypos_ = current_ypos;

57

58 return distance;

59}

60} // namespace evaluator

61

62} // namespace simulation_framework