11#include " orca_lines.hpp"
22
3- // TODO: Translate comments from russian to english
43std::vector<ORCALine>
54computeORCALines (ORCAParams params, xt::xtensor<double , 1 > agent_state, xt::xtensor<double , 2 > neighbors_states,
65 xt::xtensor<double , 1 > neighbors_sizes) {
@@ -27,7 +26,6 @@ computeORCALines(ORCAParams params, xt::xtensor<double, 1> agent_state, xt::xten
2726 rel_position = neighbor_pos - agent_pos; // (P_b - P_a)
2827 curr_dist = xt::linalg::norm (rel_position, 2 );
2928 if (curr_dist > params.ignore_radius ) {
30- // std::cout << curr_dist << " " << params.ignore_radius << "\n";
3129 continue ;
3230 }
3331
@@ -42,16 +40,13 @@ computeORCALines(ORCAParams params, xt::xtensor<double, 1> agent_state, xt::xten
4240 curr_dist_sq = curr_dist * curr_dist;
4341
4442 if (curr_dist >= radius_sum) {
45- // w -- вектор на плоскости скоростей от центра малой окружности (основания VO) до скорости другого агента относительно этого
4643 w = rel_velocity - (rel_position * inv_time_boundary);
4744
4845 w_length = xt::linalg::norm (w, 2 );
4946
5047 double sq_w_length = w_length * w_length;
5148 double w_proj = xt::linalg::dot (w, rel_position)[0 ];
5249
53- // если эти условия выполняются, то вектор w отложенный из центра окружности-основания VO будет своим концом ближе к
54- // этой самой окружности, а значит и ближайшая точка на границе VO -- это какая-то точка на этой окружнрости
5550
5651 if (w_proj < 0 .0f and (w_proj * w_proj) > sq_w_length * radius_sum_sq) {
5752 nw = w / w_length;
@@ -60,17 +55,13 @@ computeORCALines(ORCAParams params, xt::xtensor<double, 1> agent_state, xt::xten
6055 u = nw * ((radius_sum * inv_time_boundary) - w_length);
6156 }
6257 else {
63- // иначе проекция на стороны VO
64- // длина проекции вектора относительных положений на сторону VO
6558 double leg = std::sqrt (curr_dist_sq - radius_sum_sq);
6659 if (determinant2D (rel_position, w) > 0 .0f ) {
67- // если точка ближе к левой стороне VO
6860 curr_line.dir [0 ] = rel_position[0 ] * leg - rel_position[1 ] * radius_sum;
6961 curr_line.dir [1 ] = rel_position[0 ] * radius_sum + rel_position[1 ] * leg;
7062 curr_line.dir /= curr_dist_sq;
7163 }
7264 else {
73- // если точка ближе к правой стороне VO
7465 curr_line.dir [0 ] = rel_position[0 ] * leg + rel_position[1 ] * radius_sum;
7566 curr_line.dir [1 ] = -rel_position[0 ] * radius_sum + rel_position[1 ] * leg;
7667 curr_line.dir *= -1 ;
@@ -133,7 +124,6 @@ bool linearProgram1(const std::vector<ORCALine> &lines, unsigned long curr, floa
133124 float discriminant = dot_product * dot_product + radius * radius - pow (xt::linalg::norm (lines[curr].point_lies_on ), 2 );
134125
135126 if (discriminant < 0 .0f ) {
136- // Максимальная скорость не позволяет удовлетворить это условие
137127 return false ;
138128 }
139129
@@ -147,7 +137,6 @@ bool linearProgram1(const std::vector<ORCALine> &lines, unsigned long curr, floa
147137 const double numerator = determinant2D (lines[i].dir , lines[curr].point_lies_on - lines[i].point_lies_on );
148138
149139 if (std::fabs (denominator) <= 0.00001 ) {
150- // Текущая и сравниваемая линии параллельны
151140 if (numerator < 0 .0f ) {
152141 return false ;
153142 }
@@ -159,11 +148,9 @@ bool linearProgram1(const std::vector<ORCALine> &lines, unsigned long curr, floa
159148 const double t = numerator / denominator;
160149
161150 if (denominator >= 0 .0f ) {
162- /* Line i bounds line lineNo on the right. */
163151 t_right = std::min (t_right, t);
164152 }
165153 else {
166- /* Line i bounds line lineNo on the left. */
167154 t_left = std::max (t_left, t);
168155 }
169156
@@ -173,18 +160,14 @@ bool linearProgram1(const std::vector<ORCALine> &lines, unsigned long curr, floa
173160 }
174161
175162 if (direction_opt) {
176- /* Optimize direction. */
177163 if (xt::linalg::dot (opt_velocity, lines[curr].dir )[0 ] > 0 .0f ) {
178- /* Take right extreme. */
179164 result = lines[curr].point_lies_on + lines[curr].dir * t_right;
180165 }
181166 else {
182- /* Take left extreme. */
183167 result = lines[curr].point_lies_on + lines[curr].dir * t_left;
184168 }
185169 }
186170 else {
187- /* Optimize closest point. */
188171 const double t = xt::linalg::dot (lines[curr].dir , opt_velocity - lines[curr].point_lies_on )[0 ];
189172
190173 if (t < t_left) {
0 commit comments