utils

1 

5 

6 #pragma once

7 

8 #include <cmath>

9 

10 namespace simulation_framework

11 {

12 namespace utils

13 {

14 

16 struct Point2d

17 {

18  double x;

19  double y;

20 };

21 

23 struct BoundingBox

24 {

25  double x; // X-coordinate of center

26  double y; // Y-coordinate of center

27  double width; // Width of the rectangle

28  double length; // Length of the rectangle

29  double yaw; // Yaw angle in radians

30  double velocity_x; // Velocity in X-Axis

31  double velocity_y; // Velocity in Y-Axis

32 };

33 

38 inline double Calculate2dVectorNorm(const Point2d& point1, const Point2d& point2)

39 {

40  const auto difference_x = point2.x - point1.x;

41  const auto difference_y = point2.y - point1.y;

42  return std::sqrt(difference_x * difference_x + difference_y * difference_y);

43 }

44 

50 inline bool ValueInRange(const double value, const double min, const double max)

51 {

52  return (value >= min) && (value <= max);

53 }

54 

60 inline bool AreBoundingBoxesOverlapped(const BoundingBox& box1, const BoundingBox& box2)

61 {

62  bool x_overlap =

63  ValueInRange(box1.x, box2.x, box2.x + box2.width) || ValueInRange(box2.x, box1.x, box1.x + box1.width);

64 

65  bool y_overlap =

66  ValueInRange(box1.y, box2.y, box2.y + box2.length) || ValueInRange(box2.y, box1.y, box1.y + box1.length);

67 

68  return x_overlap && y_overlap;

69 }

70 

76 inline bool CanPotentiallyHit(const BoundingBox& box1, const BoundingBox& box2)

77 {

78  // Transform box2's position and velocity into box1's coordinate system

79  auto dx = box2.x - box1.x;

80  auto dy = box2.y - box1.y;

81 

82  auto transformed_x = dx * std::cos(-box1.yaw) - dy * std::sin(-box1.yaw);

83  auto transformed_y = dx * std::sin(-box1.yaw) + dy * std::cos(-box1.yaw);

84  auto transformed_velocity_x = box2.velocity_x * std::cos(-box1.yaw) - box2.velocity_y * std::sin(-box1.yaw);

85  auto transformed_velocity_y = box2.velocity_x * std::sin(-box1.yaw) + box2.velocity_y * std::cos(-box1.yaw);

86 

87  auto d_vx = box1.velocity_x - transformed_velocity_x;

88  auto d_vy = box1.velocity_y - transformed_velocity_y;

89 

90  // Check if the dot product in x or y axis is positive

91  if (d_vx * transformed_x > 0.0 || d_vy * transformed_y > 0.0)

92  {

93  return true; // box2 is potentially hit by box1

94  }

95 

96  return false; // box2 is not in danger of being hit by box1

97 }

98 

99 } // namespace utils

100 } // namespace simulation_framework