nav_plan_geometry_utils_unittest.cpp

Go to the documentation of this file.

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

#include <gtest/gtest.h>
#include <mrpt/nav/planners/nav_plan_geometry_utils.h>
#include <cmath>

TEST(NavTests, NavGeomUtils_collision_straight_circ_robot)
{
    using namespace mrpt;
    using namespace mrpt::math;
    using namespace mrpt::nav;

    {
        TPoint2D p0(0, 0), p1(1, 1);
        double colDist;
        {
            const double R = 0.1;
            TPoint2D pObs(1.0, 0.0);
            const bool ret = collision_free_dist_segment_circ_robot(
                p0, p1, R, pObs, colDist);
            EXPECT_FALSE(ret);
        }
        {
            const double R = 0.1;
            TPoint2D pObs(0.0, 1.0);
            const bool ret = collision_free_dist_segment_circ_robot(
                p0, p1, R, pObs, colDist);
            EXPECT_FALSE(ret);
        }
        {
            const double R = 0.1;
            TPoint2D pObs(-0.4, -0.4);
            const bool ret = collision_free_dist_segment_circ_robot(
                p0, p1, R, pObs, colDist);
            EXPECT_FALSE(ret);
        }
        {
            const double R = 0.1;
            TPoint2D pObs(1.4, 1.4);
            const bool ret = collision_free_dist_segment_circ_robot(
                p0, p1, R, pObs, colDist);
            EXPECT_FALSE(ret);
        }
        {
            const double R = 0.1;
            TPoint2D pObs(0.45, 0.48);
            const bool ret = collision_free_dist_segment_circ_robot(
                p0, p1, R, pObs, colDist);
            EXPECT_TRUE(ret);
        }
    }

    {
        TPoint2D p0(4, 5), p1(5, 5);
        double colDist;
        const double R = 0.5;
        {
            TPoint2D pObs(5.0, 5.0);
            const bool ret = collision_free_dist_segment_circ_robot(
                p0, p1, R, pObs, colDist);
            EXPECT_TRUE(ret);
            EXPECT_NEAR(colDist, 0.5, 1e-6);
        }
        {
            TPoint2D pObs(4.510, 5.001);
            const bool ret = collision_free_dist_segment_circ_robot(
                p0, p1, R, pObs, colDist);
            EXPECT_TRUE(ret);
            EXPECT_NEAR(colDist, 0.01, 3e-3);
        }
    }
    {
        TPoint2D p0(0, 0), p1(-2, -1);
        double colDist;
        const double R = 0.5;
        TPoint2D pObs(-2.0, -1.0);
        const bool ret =
            collision_free_dist_segment_circ_robot(p0, p1, R, pObs, colDist);
        EXPECT_TRUE(ret);
        EXPECT_NEAR(colDist, std::sqrt(5.0) - 0.5, 1e-6);
    }
}