AVxcelerate Simulation Framework 2024 R2

utils

Last update: 12.07.2024
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 // check how a point is located to a line defined by 2 points
100 inline float CheckPointLocation(const Point2d& p1, const Point2d& p2, const Point2d& p3)
101 {
102  return (p1.x - p3.x) * (p2.y - p3.y) - (p2.x - p3.x) * (p1.y - p3.y);
103 }
104 
105 // check if a point is in a triangle
106 inline bool IsPointInTriangle(const Point2d& pt, const Point2d& v1, const Point2d& v2, const Point2d& v3)
107 {
108  bool b1, b2, b3;
109  b1 = CheckPointLocation(pt, v1, v2) < 0.0f;
110  b2 = CheckPointLocation(pt, v2, v3) < 0.0f;
111  b3 = CheckPointLocation(pt, v3, v1) < 0.0f;
112  return ((b1 == b2) && (b2 == b3));
113 }
114 
115 // check if 2 bounding box has overlap
116 inline bool IsRectangleOverlapping(const BoundingBox& b1, const BoundingBox& b2)
117 {
118  Point2d b1_tl, b1_tr, b1_bl, b1_br;
119  Point2d b2_tl, b2_tr, b2_bl, b2_br;
120  b1_tl.x = b1.x + (b1.length * 0.5 * cos(b1.yaw) - b1.width * 0.5 * sin(b1.yaw));
121  b1_tl.y = b1.y + (b1.length * 0.5 * sin(b1.yaw) + b1.width * 0.5 * cos(b1.yaw));
122  b1_tr.x = b1.x + (b1.width * 0.5 * sin(b1.yaw) + b1.length * 0.5 * cos(b1.yaw));
123  b1_tr.y = b1.y - (b1.width * 0.5 * cos(b1.yaw) - b1.length * 0.5 * sin(b1.yaw));
124  b1_br.x = b1.x - (b1.length * 0.5 * cos(b1.yaw) - b1.width * 0.5 * sin(b1.yaw));
125  b1_br.y = b1.y - (b1.length * 0.5 * sin(b1.yaw) + b1.width * 0.5 * cos(b1.yaw));
126  b1_bl.x = b1.x - (b1.width * 0.5 * sin(b1.yaw) + b1.length * 0.5 * cos(b1.yaw));
127  b1_bl.y = b1.y + (b1.width * 0.5 * cos(b1.yaw) - b1.length * 0.5 * sin(b1.yaw));
128 
129  b2_tl.x = b2.x + (b2.length * 0.5 * cos(b2.yaw) - b2.width * 0.5 * sin(b2.yaw));
130  b2_tl.y = b2.y + (b2.length * 0.5 * sin(b2.yaw) + b2.width * 0.5 * cos(b2.yaw));
131  b2_tr.x = b2.x + (b2.width * 0.5 * sin(b2.yaw) + b2.length * 0.5 * cos(b2.yaw));
132  b2_tr.y = b2.y - (b2.width * 0.5 * cos(b2.yaw) - b2.length * 0.5 * sin(b2.yaw));
133  b2_br.x = b2.x - (b2.length * 0.5 * cos(b2.yaw) - b2.width * 0.5 * sin(b2.yaw));
134  b2_br.y = b2.y - (b2.length * 0.5 * sin(b2.yaw) + b2.width * 0.5 * cos(b2.yaw));
135  b2_bl.x = b2.x - (b2.width * 0.5 * sin(b2.yaw) + b2.length * 0.5 * cos(b2.yaw));
136  b2_bl.y = b2.y + (b2.width * 0.5 * cos(b2.yaw) - b2.length * 0.5 * sin(b2.yaw));
137 
138  std::vector<Point2d> vec_b2;
139  vec_b2.push_back(b2_tl);
140  vec_b2.push_back(b2_tr);
141  vec_b2.push_back(b2_bl);
142  vec_b2.push_back(b2_br);
143  for (auto point : vec_b2)
144  {
145  if (IsPointInTriangle(point, b1_tl, b1_tr, b1_bl) || IsPointInTriangle(point, b1_br, b1_tr, b1_bl))
146  return true;
147  }
148  return false;
149 }
150 
151 // estimate in dt if there will be a collision between b1 and b2
152 
153 inline std::chrono::milliseconds PredictTimeToCollision(const BoundingBox& b1,
154  const BoundingBox& b2,
155  const uint64_t& predictive_ttc_precision,
156  const uint64_t& predictive_ttc_max)
157 {
158  for (auto t = predictive_ttc_precision; t < predictive_ttc_max; t += predictive_ttc_precision)
159  {
160  auto new_b1 = b1;
161  auto new_b2 = b2;
162  new_b1.x += new_b1.velocity_x * (double)t / 1000.0;
163  new_b1.y += new_b1.velocity_y * (double)t / 1000.0;
164  new_b2.x += new_b2.velocity_x * (double)t / 1000.0;
165  new_b2.y += new_b2.velocity_y * (double)t / 1000.0;
166  if (IsRectangleOverlapping(new_b1, new_b2))
167  {
168  return std::chrono::milliseconds{t};
169  }
170  }
171  return std::chrono::milliseconds{predictive_ttc_max};
172 }
173 
174 } // namespace utils
175 } // namespace simulation_framework

Connect with Ansys