MRPT  1.9.9
nav_plan_geometry_utils.cpp
Go to the documentation of this file.
1 /* +------------------------------------------------------------------------+
2  | Mobile Robot Programming Toolkit (MRPT) |
3  | https://www.mrpt.org/ |
4  | |
5  | Copyright (c) 2005-2019, Individual contributors, see AUTHORS file |
6  | See: https://www.mrpt.org/Authors - All rights reserved. |
7  | Released under BSD License. See: https://www.mrpt.org/License |
8  +------------------------------------------------------------------------+ */
9
10 #include "nav-precomp.h" // Precompiled headers
11
12 #include <mrpt/core/exceptions.h>
13 #include <mrpt/math/poly_roots.h>
14 #include <mrpt/math/wrap2pi.h>
16
17 using namespace mrpt;
18 using namespace mrpt::math;
19
21  const mrpt::math::TPoint2D& p0, const mrpt::math::TPoint2D& p1,
22  const double R, const mrpt::math::TPoint2D& o, double& out_col_dist)
23 {
24  using mrpt::square;
25
26  out_col_dist = -1.0;
27
28  // Unit vector from start -> end:
29  mrpt::math::TPoint2D u = (p1 - p0);
30  const double L = u.norm();
31  ASSERT_ABOVE_(L, 1e-10);
32  u *= 1.0 / L;
33
34  /*
35  syms x y d ux uy o.x o.y R real
36  f=(x+d*ux-o.x)^2+(y+d*uy-o.y)^2-R^2
37  coeffs ->
38  [ (o.x - x)^2 + (o.y - y)^2 - R^2, - 2*ux*(o.x - x) - 2*uy*(o.y - y), ux^2 +
39  uy^2]
40  */
41
42  // quadratic eq: a*d^2 + b*d+c=0
43  const double a = square(u.x) + square(u.y);
44  const double b = -2 * u.x * (o.x - p0.x) - 2 * u.y * (o.y - p0.y);
45  const double c = square(o.x - p0.x) + square(o.y - p0.y) - square(R);
46
47  double r1, r2;
48  const int nsols = mrpt::math::solve_poly2(a, b, c, r1, r2);
49
50  if (nsols <= 0) return false;
51  double r_min;
52  if (nsols == 1)
53  r_min = r1;
54  else
55  {
56  if (r1 < 0 && r2 < 0) return false;
57  if (r1 < 0)
58  {
59  r_min = r2;
60  }
61  else if (r2 < 0)
62  {
63  r_min = r1;
64  }
65  else
66  {
67  r_min = std::min(r1, r2);
68  }
69  }
70
71  if (r_min > L) return false;
72
73  // A real, valid collision:
74  out_col_dist = r_min;
75  return true;
76 }
77
79  const double arc_radius, const double R, const mrpt::math::TPoint2D& o,
80  double& out_col_dist)
81 {
82  ASSERT_ABOVE_(std::abs(arc_radius), 1e-10);
83  out_col_dist = -1.0;
84
85  const mrpt::math::TPoint2D ptArcCenter(.0, arc_radius);
86  const double center2obs_dist = (ptArcCenter - o).norm();
87  if (std::abs(center2obs_dist - std::abs(arc_radius)) > R) return false;
88
89  // x:
90  const double r = arc_radius;
91  const double discr =
92  (R * r * 2.0 - o.y * r * 2.0 - R * R + o.x * o.x + o.y * o.y) *
93  (R * r * 2.0 + o.y * r * 2.0 + R * R - o.x * o.x - o.y * o.y);
94  if (discr < 0) return false;
95  const double sol_x0 =
96  ((R * R) * (-1.0 / 2.0) + (o.x * o.x) * (1.0 / 2.0) +
97  (o.y * o.y) * (1.0 / 2.0) -
98  (o.y *
99  (-(R * R) * o.y + (R * R) * r + (o.x * o.x) * o.y + (o.x * o.x) * r -
100  (o.y * o.y) * r + o.y * o.y * o.y + o.x * sqrt(discr)) *
101  (1.0 / 2.0)) /
102  (o.y * r * -2.0 + o.x * o.x + o.y * o.y + r * r) +
103  (r *
104  (-(R * R) * o.y + (R * R) * r + (o.x * o.x) * o.y + (o.x * o.x) * r -
105  (o.y * o.y) * r + o.y * o.y * o.y + o.x * sqrt(discr)) *
106  (1.0 / 2.0)) /
107  (o.y * r * -2.0 + o.x * o.x + o.y * o.y + r * r)) /
108  o.x;
109  const double sol_x1 =
110  ((R * R) * (-1.0 / 2.0) + (o.x * o.x) * (1.0 / 2.0) +
111  (o.y * o.y) * (1.0 / 2.0) -
112  (o.y *
113  (-(R * R) * o.y + (R * R) * r + (o.x * o.x) * o.y + (o.x * o.x) * r -
114  (o.y * o.y) * r + o.y * o.y * o.y - o.x * sqrt(discr)) *
115  (1.0 / 2.0)) /
116  (o.y * r * -2.0 + o.x * o.x + o.y * o.y + r * r) +
117  (r *
118  (-(R * R) * o.y + (R * R) * r + (o.x * o.x) * o.y + (o.x * o.x) * r -
119  (o.y * o.y) * r + o.y * o.y * o.y - o.x * sqrt(discr)) *
120  (1.0 / 2.0)) /
121  (o.y * r * -2.0 + o.x * o.x + o.y * o.y + r * r)) /
122  o.x;
123
124  // y:
125  const double sol_y0 =
126  ((R * R) * o.y * (-1.0 / 2.0) + (R * R) * r * (1.0 / 2.0) +
127  (o.x * o.x) * o.y * (1.0 / 2.0) + (o.x * o.x) * r * (1.0 / 2.0) -
128  (o.y * o.y) * r * (1.0 / 2.0) + (o.y * o.y * o.y) * (1.0 / 2.0) +
129  o.x * sqrt(discr) * (1.0 / 2.0)) /
130  (o.y * r * -2.0 + o.x * o.x + o.y * o.y + r * r);
131  const double sol_y1 =
132  ((R * R) * o.y * (-1.0 / 2.0) + (R * R) * r * (1.0 / 2.0) +
133  (o.x * o.x) * o.y * (1.0 / 2.0) + (o.x * o.x) * r * (1.0 / 2.0) -
134  (o.y * o.y) * r * (1.0 / 2.0) + (o.y * o.y * o.y) * (1.0 / 2.0) -
135  o.x * sqrt(discr) * (1.0 / 2.0)) /
136  (o.y * r * -2.0 + o.x * o.x + o.y * o.y + r * r);
137
138  const mrpt::math::TPoint2D sol0(sol_x0, sol_y0), sol1(sol_x1, sol_y1);
139
140  double th0 = atan2(
141  sol0.x - ptArcCenter.x,
142  -(sol0.y - ptArcCenter.y)); // (x,y) order is intentionally like this!
143  double th1 = atan2(sol1.x - ptArcCenter.x, -(sol1.y - ptArcCenter.y));
144
145  if (r > 0)
146  {
147  th0 = mrpt::math::wrapTo2Pi(th0);
148  th1 = mrpt::math::wrapTo2Pi(th1);
149  }
150  else
151  {
152  th0 = mrpt::math::wrapTo2Pi(M_PI - th0);
153  th1 = mrpt::math::wrapTo2Pi(M_PI - th1);
154  }
155
156  out_col_dist = std::abs(r) * std::min(th0, th1);
157  return true;
158 }
T x
X,Y coordinates.
Definition: TPoint2D.h:24
bool collision_free_dist_arc_circ_robot(const double arc_radius, const double robot_radius, const mrpt::math::TPoint2D &obstacle, double &out_col_dist)
Computes the collision-free distance for a forward path (+X) circular arc path segment from pose (0...
This base provides a set of functions for maths stuff.
T square(const T x)
Inline function for the square of a number.
T wrapTo2Pi(T a)
Modifies the given angle to translate it into the [0,2pi[ range.
Definition: wrap2pi.h:38
bool collision_free_dist_segment_circ_robot(const mrpt::math::TPoint2D &p_start, const mrpt::math::TPoint2D &p_end, const double robot_radius, const mrpt::math::TPoint2D &obstacle, double &out_col_dist)
Computes the collision-free distance for a linear segment path between two points, for a circular robot, and a point obstacle (ox,oy).
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
const float R
#define ASSERT_ABOVE_(__A, __B)
Definition: exceptions.h:155
T norm() const
Point norm: |v| = sqrt(x^2+y^2)
Definition: TPoint2D.h:196
CONTAINER::Scalar norm(const CONTAINER &v)
int solve_poly2(double a, double b, double c, double &r1, double &r2) noexcept
Solves equation a*x^2 + b*x + c = 0.
Definition: poly_roots.cpp:394

 Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: 338471620 Mon Feb 17 00:12:39 2020 +0100 at lun feb 17 00:15:10 CET 2020