Main MRPT website > C++ reference for MRPT 1.5.7
nav_plan_geometry_utils_unittest.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 
11 #include <cmath>
12 #include <gtest/gtest.h>
13 
14 TEST(NavTests, NavGeomUtils_collision_straight_circ_robot)
15 {
16  using namespace mrpt;
17  using namespace mrpt::math;
18  using namespace mrpt::nav;
19 
20  {
21  TPoint2D p0(0,0), p1(1,1);
22  double colDist;
23  {
24  const double R = 0.1;
25  TPoint2D pObs(1.0,0.0);
26  const bool ret = collision_free_dist_segment_circ_robot(p0, p1, R, pObs, colDist);
27  EXPECT_FALSE(ret);
28  }
29  {
30  const double R = 0.1;
31  TPoint2D pObs(0.0, 1.0);
32  const bool ret = collision_free_dist_segment_circ_robot(p0, p1, R, pObs, colDist);
33  EXPECT_FALSE(ret);
34  }
35  {
36  const double R = 0.1;
37  TPoint2D pObs(-0.4, -0.4);
38  const bool ret = collision_free_dist_segment_circ_robot(p0, p1, R, pObs, colDist);
39  EXPECT_FALSE(ret);
40  }
41  {
42  const double R = 0.1;
43  TPoint2D pObs(1.4, 1.4);
44  const bool ret = collision_free_dist_segment_circ_robot(p0, p1, R, pObs, colDist);
45  EXPECT_FALSE(ret);
46  }
47  {
48  const double R = 0.1;
49  TPoint2D pObs(0.45, 0.48);
50  const bool ret = collision_free_dist_segment_circ_robot(p0, p1, R, pObs, colDist);
51  EXPECT_TRUE(ret);
52  }
53  }
54 
55  {
56  TPoint2D p0(4, 5), p1(5, 5);
57  double colDist;
58  const double R = 0.5;
59  {
60  TPoint2D pObs(5.0, 5.0);
61  const bool ret = collision_free_dist_segment_circ_robot(p0, p1, R, pObs, colDist);
62  EXPECT_TRUE(ret);
63  EXPECT_NEAR(colDist, 0.5, 1e-6);
64  }
65  {
66  TPoint2D pObs(4.510, 5.001);
67  const bool ret = collision_free_dist_segment_circ_robot(p0, p1, R, pObs, colDist);
68  EXPECT_TRUE(ret);
69  EXPECT_NEAR(colDist, 0.01, 3e-3);
70  }
71  }
72  {
73  TPoint2D p0(0, 0), p1(-2, -1);
74  double colDist;
75  const double R = 0.5;
76  TPoint2D pObs(-2.0, -1.0);
77  const bool ret = collision_free_dist_segment_circ_robot(p0, p1, R, pObs, colDist);
78  EXPECT_TRUE(ret);
79  EXPECT_NEAR(colDist, std::sqrt(5.0)-0.5, 1e-6);
80  }
81 }
This base provides a set of functions for maths stuff.
Definition: CArrayNumeric.h:19
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).
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
const float R
Lightweight 2D point.



Page generated by Doxygen 1.8.14 for MRPT 1.5.7 Git: 5902e14cc Wed Apr 24 15:04:01 2019 +0200 at lun oct 28 01:39:17 CET 2019