Main MRPT website > C++ reference for MRPT 1.5.6
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-2017, 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,
22  const mrpt::math::TPoint2D &o,
23  double &out_col_dist)
24 {
25  using mrpt::math::square;
26 
27  out_col_dist = -1.0;
28 
29  // Unit vector from start -> end:
30  mrpt::math::TPoint2D u = (p1 - p0);
31  const double L = u.norm();
32  ASSERT_ABOVE_(L, 1e-10);
33  u *= 1.0 / L;
34 
35  /*
36  syms x y d ux uy o.x o.y R real
37  f=(x+d*ux-o.x)^2+(y+d*uy-o.y)^2-R^2
38  coeffs ->
39  [ (o.x - x)^2 + (o.y - y)^2 - R^2, - 2*ux*(o.x - x) - 2*uy*(o.y - y), ux^2 + 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) r_min = r1; else {
53  if (r1 < 0 && r2 < 0) return false;
54  if (r1 < 0) {
55  r_min = r2;
56  }
57  else if (r2 < 0) {
58  r_min = r1;
59  }
60  else {
61  r_min = std::min(r1, r2);
62  }
63  }
64 
65  if (r_min > L) return false;
66 
67  // A real, valid collision:
68  out_col_dist = r_min;
69  return true;
70 }
71 
73  const double arc_radius, const double R,
74  const mrpt::math::TPoint2D &o, double &out_col_dist)
75 {
76  ASSERT_ABOVE_(std::abs(arc_radius), 1e-10);
77  out_col_dist = -1.0;
78 
79  const mrpt::math::TPoint2D ptArcCenter(.0, arc_radius);
80  const double center2obs_dist = (ptArcCenter - o).norm();
81  if (std::abs(center2obs_dist - std::abs(arc_radius))>R)
82  return false;
83 
84  // x:
85  const double r = arc_radius;
86  const double discr = (R*r*2.0 - o.y*r*2.0 - R*R + o.x*o.x + o.y*o.y)*(R*r*2.0 + o.y*r*2.0 + R*R - o.x*o.x - o.y*o.y);
87  if (discr < 0)
88  return false;
89  const double sol_x0 = ((R*R)*(-1.0 / 2.0) + (o.x*o.x)*(1.0 / 2.0) + (o.y*o.y)*(1.0 / 2.0) - (o.y*(-(R*R)*o.y + (R*R)*r + (o.x*o.x)*o.y + (o.x*o.x)*r - (o.y*o.y)*r + o.y*o.y*o.y + o.x*sqrt(discr))*(1.0 / 2.0)) / (o.y*r*-2.0 + o.x*o.x + o.y*o.y + r*r) + (r*(-(R*R)*o.y + (R*R)*r + (o.x*o.x)*o.y + (o.x*o.x)*r - (o.y*o.y)*r + o.y*o.y*o.y + o.x*sqrt(discr))*(1.0 / 2.0)) / (o.y*r*-2.0 + o.x*o.x + o.y*o.y + r*r)) / o.x;
90  const double sol_x1 = ((R*R)*(-1.0 / 2.0) + (o.x*o.x)*(1.0 / 2.0) + (o.y*o.y)*(1.0 / 2.0) - (o.y*(-(R*R)*o.y + (R*R)*r + (o.x*o.x)*o.y + (o.x*o.x)*r - (o.y*o.y)*r + o.y*o.y*o.y - o.x*sqrt(discr))*(1.0 / 2.0)) / (o.y*r*-2.0 + o.x*o.x + o.y*o.y + r*r) + (r*(-(R*R)*o.y + (R*R)*r + (o.x*o.x)*o.y + (o.x*o.x)*r - (o.y*o.y)*r + o.y*o.y*o.y - o.x*sqrt(discr))*(1.0 / 2.0)) / (o.y*r*-2.0 + o.x*o.x + o.y*o.y + r*r)) / o.x;
91 
92  // y:
93  const double sol_y0 = ((R*R)*o.y*(-1.0 / 2.0) + (R*R)*r*(1.0 / 2.0) + (o.x*o.x)*o.y*(1.0 / 2.0) + (o.x*o.x)*r*(1.0 / 2.0) - (o.y*o.y)*r*(1.0 / 2.0) + (o.y*o.y*o.y)*(1.0 / 2.0) + o.x*sqrt(discr)*(1.0 / 2.0)) / (o.y*r*-2.0 + o.x*o.x + o.y*o.y + r*r);
94  const double sol_y1 = ((R*R)*o.y*(-1.0 / 2.0) + (R*R)*r*(1.0 / 2.0) + (o.x*o.x)*o.y*(1.0 / 2.0) + (o.x*o.x)*r*(1.0 / 2.0) - (o.y*o.y)*r*(1.0 / 2.0) + (o.y*o.y*o.y)*(1.0 / 2.0) - o.x*sqrt(discr)*(1.0 / 2.0)) / (o.y*r*-2.0 + o.x*o.x + o.y*o.y + r*r);
95 
96  const mrpt::math::TPoint2D sol0(sol_x0, sol_y0), sol1(sol_x1, sol_y1);
97 
98  double th0 = atan2(sol0.x - ptArcCenter.x, -( sol0.y - ptArcCenter.y)); // (x,y) order is intentionally like this!
99  double th1 = atan2(sol1.x - ptArcCenter.x, -(sol1.y - ptArcCenter.y));
100 
101  if (r > 0)
102  {
103  th0 = mrpt::math::wrapTo2Pi(th0);
104  th1 = mrpt::math::wrapTo2Pi(th1);
105  }
106  else
107  {
108  th0 = mrpt::math::wrapTo2Pi(M_PI - th0);
109  th1 = mrpt::math::wrapTo2Pi(M_PI - th1);
110  }
111 
112  out_col_dist = std::abs(r)*std::min(th0, th1);
113  return true;
114 }
GLboolean GLboolean GLboolean GLboolean a
Definition: glew.h:5406
double y
X,Y coordinates.
#define min(a, b)
#define ASSERT_ABOVE_(__A, __B)
const GLfloat * c
Definition: glew.h:10088
bool NAV_IMPEXP 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...
#define M_PI
Definition: bits.h:78
double norm() const
Point norm.
T square(const T x)
Inline function for the square of a number.
Definition: bits.h:52
T wrapTo2Pi(T a)
Modifies the given angle to translate it into the [0,2pi[ range.
Definition: wrap2pi.h:40
bool NAV_IMPEXP 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).
const float R
GLdouble GLdouble GLdouble r
Definition: glew.h:1311
int BASE_IMPEXP solve_poly2(double a, double b, double c, double &r1, double &r2) MRPT_NO_THROWS
Solves equation a*x^2 + b*x + c = 0.
Definition: poly_roots.cpp:302
GLdouble GLdouble GLdouble b
Definition: glew.h:5092
Lightweight 2D point.
CONTAINER::Scalar norm(const CONTAINER &v)



Page generated by Doxygen 1.8.6 for MRPT 1.5.6 Git: 4c65e84 Tue Apr 24 08:18:17 2018 +0200 at mar abr 24 08:26:17 CEST 2018