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



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: 7d5e6d718 Fri Aug 24 01:51:28 2018 +0200 at lun nov 2 08:35:50 CET 2020