nav_plan_geometry_utils.cpp

Go to the documentation of this file.

/* +------------------------------------------------------------------------+
   |                     Mobile Robot Programming Toolkit (MRPT)            |
   |                          http://www.mrpt.org/                          |
   |                                                                        |
   | Copyright (c) 2005-2017, Individual contributors, see AUTHORS file     |
   | See: http://www.mrpt.org/Authors - All rights reserved.                |
   | Released under BSD License. See details in http://www.mrpt.org/License |
   +------------------------------------------------------------------------+ */

#include "nav-precomp.h"  // Precompiled headers

#include <mrpt/nav/planners/nav_plan_geometry_utils.h>
#include <mrpt/math/poly_roots.h>
#include <mrpt/math/wrap2pi.h>

using namespace mrpt;
using namespace mrpt::math;

bool mrpt::nav::collision_free_dist_segment_circ_robot(
        const mrpt::math::TPoint2D& p0, const mrpt::math::TPoint2D& p1,
        const double R, const mrpt::math::TPoint2D& o, double& out_col_dist)
{
        using mrpt::math::square;

        out_col_dist = -1.0;

        // Unit vector from start -> end:
        mrpt::math::TPoint2D u = (p1 - p0);
        const double L = u.norm();
        ASSERT_ABOVE_(L, 1e-10);
        u *= 1.0 / L;

        /*
        syms x y d ux uy o.x o.y R real
        f=(x+d*ux-o.x)^2+(y+d*uy-o.y)^2-R^2
        coeffs ->
        [ (o.x - x)^2 + (o.y - y)^2 - R^2, - 2*ux*(o.x - x) - 2*uy*(o.y - y), ux^2 +
        uy^2]
        */

        // quadratic eq: a*d^2 + b*d+c=0
        const double a = square(u.x) + square(u.y);
        const double b = -2 * u.x * (o.x - p0.x) - 2 * u.y * (o.y - p0.y);
        const double c = square(o.x - p0.x) + square(o.y - p0.y) - square(R);

        double r1, r2;
        const int nsols = mrpt::math::solve_poly2(a, b, c, r1, r2);

        if (nsols <= 0) return false;
        double r_min;
        if (nsols == 1)
                r_min = r1;
        else
        {
                if (r1 < 0 && r2 < 0) return false;
                if (r1 < 0)
                {
                        r_min = r2;
                }
                else if (r2 < 0)
                {
                        r_min = r1;
                }
                else
                {
                        r_min = std::min(r1, r2);
                }
        }

        if (r_min > L) return false;

        // A real, valid collision:
        out_col_dist = r_min;
        return true;
}

bool mrpt::nav::collision_free_dist_arc_circ_robot(
        const double arc_radius, const double R, const mrpt::math::TPoint2D& o,
        double& out_col_dist)
{
        ASSERT_ABOVE_(std::abs(arc_radius), 1e-10);
        out_col_dist = -1.0;

        const mrpt::math::TPoint2D ptArcCenter(.0, arc_radius);
        const double center2obs_dist = (ptArcCenter - o).norm();
        if (std::abs(center2obs_dist - std::abs(arc_radius)) > R) return false;

        // x:
        const double r = arc_radius;
        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);
        if (discr < 0) return false;
        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;
        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;

        // y:
        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);
        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);

        const mrpt::math::TPoint2D sol0(sol_x0, sol_y0), sol1(sol_x1, sol_y1);

        double th0 = atan2(
                sol0.x - ptArcCenter.x,
                -(sol0.y - ptArcCenter.y));  // (x,y) order is intentionally like this!
        double th1 = atan2(sol1.x - ptArcCenter.x, -(sol1.y - ptArcCenter.y));

        if (r > 0)
        {
                th0 = mrpt::math::wrapTo2Pi(th0);
                th1 = mrpt::math::wrapTo2Pi(th1);
        }
        else
        {
                th0 = mrpt::math::wrapTo2Pi(M_PI - th0);
                th1 = mrpt::math::wrapTo2Pi(M_PI - th1);
        }

        out_col_dist = std::abs(r) * std::min(th0, th1);
        return true;
}