Skip to main content

AVxcelerate Simulation Framework 2024 R2 SP02

utils

Last update: 16.07.2025
1 
3 
4 #pragma once
5 
6 #include <chrono>
7 #include <cmath>
8 #include <vector>
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 // estimate in dt if there will be a collision between b1 and b2
100 inline std::chrono::milliseconds PredictTimeToCollision(const BoundingBox& b1,
101  const BoundingBox& b2,
102  const uint64_t& predictive_ttc_precision,
103  const uint64_t& predictive_ttc_max)
104 {
105  for (auto t = predictive_ttc_precision; t < predictive_ttc_max; t += predictive_ttc_precision)
106  {
107  auto new_b1 = b1;
108  auto new_b2 = b2;
109  new_b1.x += new_b1.velocity_x * (double)t / 1000.0;
110  new_b1.y += new_b1.velocity_y * (double)t / 1000.0;
111  new_b2.x += new_b2.velocity_x * (double)t / 1000.0;
112  new_b2.y += new_b2.velocity_y * (double)t / 1000.0;
113  if (AreBoundingBoxesOverlapped(new_b1, new_b2))
114  {
115  return std::chrono::milliseconds{t};
116  }
117  }
118  return std::chrono::milliseconds{predictive_ttc_max};
119 }
120 
121 } // namespace utils
122 } // namespace simulation_framework

Connect with Ansys