## On this page

  

 

 # utils

 Last update: 12.07.2024 

<a name="l00001"></a> 1 

<a name="l00003"></a> 3 

<a name="l00004"></a> 4 \#pragma once

<a name="l00005"></a> 5 

<a name="l00006"></a> 6 \#include &lt;chrono&gt;

<a name="l00007"></a> 7 \#include &lt;cmath&gt;

<a name="l00008"></a> 8 \#include &lt;vector&gt;

<a name="l00009"></a> 9 

<a name="l00010"></a> 10 namespace [simulation\_framework](namespacesimulation__framework.xhtml)

<a name="l00011"></a> 11 {

<a name="l00012"></a> 12 namespace utils

<a name="l00013"></a> 13 {

<a name="l00014"></a> 14 

<a name="l00016"></a> 16 struct Point2d

<a name="l00017"></a> 17 {

<a name="l00018"></a> 18 double x;

<a name="l00019"></a> 19 double y;

<a name="l00020"></a> 20 };

<a name="l00021"></a> 21 

<a name="l00023"></a> 23 struct BoundingBox

<a name="l00024"></a> 24 {

<a name="l00025"></a> 25 double x; // X-coordinate of center

<a name="l00026"></a> 26 double y; // Y-coordinate of center

<a name="l00027"></a> 27 double width; // Width of the rectangle

<a name="l00028"></a> 28 double length; // Length of the rectangle

<a name="l00029"></a> 29 double yaw; // Yaw angle in radians

<a name="l00030"></a> 30 double velocity_x; // Velocity in X-Axis

<a name="l00031"></a> 31 double velocity_y; // Velocity in Y-Axis

<a name="l00032"></a> 32 };

<a name="l00033"></a> 33 

<a name="l00038"></a> 38 inline double Calculate2dVectorNorm(const Point2d&amp; point1, const Point2d&amp; point2)

<a name="l00039"></a> 39 {

<a name="l00040"></a> 40 const auto difference_x = point2.x - point1.x;

<a name="l00041"></a> 41 const auto difference_y = point2.y - point1.y;

<a name="l00042"></a> 42 return std::sqrt(difference_x * difference_x + difference_y * difference_y);

<a name="l00043"></a> 43 }

<a name="l00044"></a> 44 

<a name="l00050"></a> 50 inline bool ValueInRange(const double value, const double min, const double max)

<a name="l00051"></a> 51 {

<a name="l00052"></a> 52 return (value &gt;= min) &amp;&amp; (value &lt;= max);

<a name="l00053"></a> 53 }

<a name="l00054"></a> 54 

<a name="l00060"></a> 60 inline bool AreBoundingBoxesOverlapped(const BoundingBox&amp; box1, const BoundingBox&amp; box2)

<a name="l00061"></a> 61 {

<a name="l00062"></a> 62 bool x_overlap =

<a name="l00063"></a> 63 ValueInRange(box1.x, box2.x, box2.x + box2.width) || ValueInRange(box2.x, box1.x, box1.x + box1.width);

<a name="l00064"></a> 64 

<a name="l00065"></a> 65 bool y_overlap =

<a name="l00066"></a> 66 ValueInRange(box1.y, box2.y, box2.y + box2.length) || ValueInRange(box2.y, box1.y, box1.y + box1.length);

<a name="l00067"></a> 67 

<a name="l00068"></a> 68 return x_overlap &amp;&amp; y_overlap;

<a name="l00069"></a> 69 }

<a name="l00070"></a> 70 

<a name="l00076"></a> 76 inline bool CanPotentiallyHit(const BoundingBox&amp; box1, const BoundingBox&amp; box2)

<a name="l00077"></a> 77 {

<a name="l00078"></a> 78 // Transform box2's position and velocity into box1's coordinate system

<a name="l00079"></a> 79 auto dx = box2.x - box1.x;

<a name="l00080"></a> 80 auto dy = box2.y - box1.y;

<a name="l00081"></a> 81 

<a name="l00082"></a> 82 auto transformed_x = dx * std::cos(-box1.yaw) - dy * std::sin(-box1.yaw);

<a name="l00083"></a> 83 auto transformed_y = dx * std::sin(-box1.yaw) + dy * std::cos(-box1.yaw);

<a name="l00084"></a> 84 auto transformed_velocity_x = box2.velocity_x * std::cos(-box1.yaw) - box2.velocity_y * std::sin(-box1.yaw);

<a name="l00085"></a> 85 auto transformed_velocity_y = box2.velocity_x * std::sin(-box1.yaw) + box2.velocity_y * std::cos(-box1.yaw);

<a name="l00086"></a> 86 

<a name="l00087"></a> 87 auto d_vx = box1.velocity_x - transformed_velocity_x;

<a name="l00088"></a> 88 auto d_vy = box1.velocity_y - transformed_velocity_y;

<a name="l00089"></a> 89 

<a name="l00090"></a> 90 // Check if the dot product in x or y axis is positive

<a name="l00091"></a> 91 if (d_vx * transformed_x &gt; 0.0 || d_vy * transformed_y &gt; 0.0)

<a name="l00092"></a> 92 {

<a name="l00093"></a> 93 return true; // box2 is potentially hit by box1

<a name="l00094"></a> 94 }

<a name="l00095"></a> 95 

<a name="l00096"></a> 96 return false; // box2 is not in danger of being hit by box1

<a name="l00097"></a> 97 }

<a name="l00098"></a> 98 

<a name="l00099"></a> 99 // check how a point is located to a line defined by 2 points

<a name="l00100"></a> 100 inline float CheckPointLocation(const Point2d&amp; p1, const Point2d&amp; p2, const Point2d&amp; p3)

<a name="l00101"></a> 101 {

<a name="l00102"></a> 102 return (p1.x - p3.x) * (p2.y - p3.y) - (p2.x - p3.x) * (p1.y - p3.y);

<a name="l00103"></a> 103 }

<a name="l00104"></a> 104 

<a name="l00105"></a> 105 // check if a point is in a triangle

<a name="l00106"></a> 106 inline bool IsPointInTriangle(const Point2d&amp; pt, const Point2d&amp; v1, const Point2d&amp; v2, const Point2d&amp; v3)

<a name="l00107"></a> 107 {

<a name="l00108"></a> 108 bool b1, b2, b3;

<a name="l00109"></a> 109 b1 = CheckPointLocation(pt, v1, v2) &lt; 0.0f;

<a name="l00110"></a> 110 b2 = CheckPointLocation(pt, v2, v3) &lt; 0.0f;

<a name="l00111"></a> 111 b3 = CheckPointLocation(pt, v3, v1) &lt; 0.0f;

<a name="l00112"></a> 112 return ((b1 == b2) &amp;&amp; (b2 == b3));

<a name="l00113"></a> 113 }

<a name="l00114"></a> 114 

<a name="l00115"></a> 115 // check if 2 bounding box has overlap

<a name="l00116"></a> 116 inline bool IsRectangleOverlapping(const BoundingBox&amp; b1, const BoundingBox&amp; b2)

<a name="l00117"></a> 117 {

<a name="l00118"></a> 118 Point2d b1_tl, b1_tr, b1_bl, b1_br;

<a name="l00119"></a> 119 Point2d b2_tl, b2_tr, b2_bl, b2_br;

<a name="l00120"></a> 120 b1_tl.x = b1.x + (b1.length * 0.5 * cos(b1.yaw) - b1.width * 0.5 * sin(b1.yaw));

<a name="l00121"></a> 121 b1_tl.y = b1.y + (b1.length * 0.5 * sin(b1.yaw) + b1.width * 0.5 * cos(b1.yaw));

<a name="l00122"></a> 122 b1_tr.x = b1.x + (b1.width * 0.5 * sin(b1.yaw) + b1.length * 0.5 * cos(b1.yaw));

<a name="l00123"></a> 123 b1_tr.y = b1.y - (b1.width * 0.5 * cos(b1.yaw) - b1.length * 0.5 * sin(b1.yaw));

<a name="l00124"></a> 124 b1_br.x = b1.x - (b1.length * 0.5 * cos(b1.yaw) - b1.width * 0.5 * sin(b1.yaw));

<a name="l00125"></a> 125 b1_br.y = b1.y - (b1.length * 0.5 * sin(b1.yaw) + b1.width * 0.5 * cos(b1.yaw));

<a name="l00126"></a> 126 b1_bl.x = b1.x - (b1.width * 0.5 * sin(b1.yaw) + b1.length * 0.5 * cos(b1.yaw));

<a name="l00127"></a> 127 b1_bl.y = b1.y + (b1.width * 0.5 * cos(b1.yaw) - b1.length * 0.5 * sin(b1.yaw));

<a name="l00128"></a> 128 

<a name="l00129"></a> 129 b2_tl.x = b2.x + (b2.length * 0.5 * cos(b2.yaw) - b2.width * 0.5 * sin(b2.yaw));

<a name="l00130"></a> 130 b2_tl.y = b2.y + (b2.length * 0.5 * sin(b2.yaw) + b2.width * 0.5 * cos(b2.yaw));

<a name="l00131"></a> 131 b2_tr.x = b2.x + (b2.width * 0.5 * sin(b2.yaw) + b2.length * 0.5 * cos(b2.yaw));

<a name="l00132"></a> 132 b2_tr.y = b2.y - (b2.width * 0.5 * cos(b2.yaw) - b2.length * 0.5 * sin(b2.yaw));

<a name="l00133"></a> 133 b2_br.x = b2.x - (b2.length * 0.5 * cos(b2.yaw) - b2.width * 0.5 * sin(b2.yaw));

<a name="l00134"></a> 134 b2_br.y = b2.y - (b2.length * 0.5 * sin(b2.yaw) + b2.width * 0.5 * cos(b2.yaw));

<a name="l00135"></a> 135 b2_bl.x = b2.x - (b2.width * 0.5 * sin(b2.yaw) + b2.length * 0.5 * cos(b2.yaw));

<a name="l00136"></a> 136 b2_bl.y = b2.y + (b2.width * 0.5 * cos(b2.yaw) - b2.length * 0.5 * sin(b2.yaw));

<a name="l00137"></a> 137 

<a name="l00138"></a> 138 std::vector&lt;Point2d&gt; vec_b2;

<a name="l00139"></a> 139 vec_b2.push_back(b2_tl);

<a name="l00140"></a> 140 vec_b2.push_back(b2_tr);

<a name="l00141"></a> 141 vec_b2.push_back(b2_bl);

<a name="l00142"></a> 142 vec_b2.push_back(b2_br);

<a name="l00143"></a> 143 for (auto point : vec_b2)

<a name="l00144"></a> 144 {

<a name="l00145"></a> 145 if (IsPointInTriangle(point, b1_tl, b1_tr, b1_bl) || IsPointInTriangle(point, b1_br, b1_tr, b1_bl))

<a name="l00146"></a> 146 return true;

<a name="l00147"></a> 147 }

<a name="l00148"></a> 148 return false;

<a name="l00149"></a> 149 }

<a name="l00150"></a> 150 

<a name="l00151"></a> 151 // estimate in dt if there will be a collision between b1 and b2

<a name="l00152"></a> 152 

<a name="l00153"></a> 153 inline std::chrono::milliseconds PredictTimeToCollision(const BoundingBox&amp; b1,

<a name="l00154"></a> 154 const BoundingBox&amp; b2,

<a name="l00155"></a> 155 const uint64_t&amp; predictive_ttc_precision,

<a name="l00156"></a> 156 const uint64_t&amp; predictive_ttc_max)

<a name="l00157"></a> 157 {

<a name="l00158"></a> 158 for (auto t = predictive_ttc_precision; t &lt; predictive_ttc_max; t += predictive_ttc_precision)

<a name="l00159"></a> 159 {

<a name="l00160"></a> 160 auto new_b1 = b1;

<a name="l00161"></a> 161 auto new_b2 = b2;

<a name="l00162"></a> 162 new_b1.x += new_b1.velocity_x * (double)t / 1000.0;

<a name="l00163"></a> 163 new_b1.y += new_b1.velocity_y * (double)t / 1000.0;

<a name="l00164"></a> 164 new_b2.x += new_b2.velocity_x * (double)t / 1000.0;

<a name="l00165"></a> 165 new_b2.y += new_b2.velocity_y * (double)t / 1000.0;

<a name="l00166"></a> 166 if (IsRectangleOverlapping(new_b1, new_b2))

<a name="l00167"></a> 167 {

<a name="l00168"></a> 168 return std::chrono::milliseconds{t};

<a name="l00169"></a> 169 }

<a name="l00170"></a> 170 }

<a name="l00171"></a> 171 return std::chrono::milliseconds{predictive_ttc_max};

<a name="l00172"></a> 172 }

<a name="l00173"></a> 173 

<a name="l00174"></a> 174 } // namespace utils

<a name="l00175"></a> 175 } // namespace simulation\_framework