misc.h
Go to the documentation of this file.
1 /*********************************************************************
2  *
3  * Software License Agreement (BSD License)
4  *
5  * Copyright (c) 2016,
6  * TU Dortmund - Institute of Control Theory and Systems Engineering.
7  * All rights reserved.
8  *
9  * Redistribution and use in source and binary forms, with or without
10  * modification, are permitted provided that the following conditions
11  * are met:
12  *
13  * * Redistributions of source code must retain the above copyright
14  * notice, this list of conditions and the following disclaimer.
15  * * Redistributions in binary form must reproduce the above
16  * copyright notice, this list of conditions and the following
17  * disclaimer in the documentation and/or other materials provided
18  * with the distribution.
19  * * Neither the name of the institute nor the names of its
20  * contributors may be used to endorse or promote products derived
21  * from this software without specific prior written permission.
22  *
23  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
24  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
25  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
26  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
27  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
28  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
29  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
30  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
31  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
32  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
33  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
34  * POSSIBILITY OF SUCH DAMAGE.
35  *
36  * Author: Christoph Rösmann
37  *********************************************************************/
38 
39 #ifndef MISC_H_
40 #define MISC_H_
41 
42 #include <algorithm>
43 #include <cmath>
44 
45 namespace costmap_converter
46 {
47 
57 template <typename Point, typename LinePoint>
58 inline double computeDistanceToLine(const Point& point, const LinePoint& line_pt1, const LinePoint& line_pt2)
59 {
60  double dx = line_pt2.x - line_pt1.x;
61  double dy = line_pt2.y - line_pt1.y;
62 
63  double length = std::sqrt(dx*dx + dy*dy);
64 
65  if (length>0)
66  return std::abs(dy * point.x - dx * point.y + line_pt2.x * line_pt1.y - line_pt2.y * line_pt1.x) / length;
67 
68  return std::sqrt(std::pow(point.x - line_pt1.x, 2) + std::pow(point.y - line_pt1.y, 2));
69 }
70 
71 
82 template <typename Point, typename LinePoint>
83 inline double computeSquaredDistanceToLineSegment(const Point& point, const LinePoint& line_start, const LinePoint& line_end, bool* is_inbetween=NULL)
84 {
85  double dx = line_end.x - line_start.x;
86  double dy = line_end.y - line_start.y;
87 
88  double length_sqr = dx*dx + dy*dy;
89 
90  double u = 0;
91 
92  if (length_sqr > 0)
93  u = ((point.x - line_start.x) * dx + (point.y - line_start.y) * dy) / length_sqr;
94 
95  if (is_inbetween)
96  *is_inbetween = (u>=0 && u<=1);
97 
98  if (u <= 0)
99  return std::pow(point.x-line_start.x,2) + std::pow(point.y-line_start.y,2);
100 
101  if (u >= 1)
102  return std::pow(point.x-line_end.x,2) + std::pow(point.y-line_end.y,2);
103 
104  return std::pow(point.x - (line_start.x+u*dx) ,2) + std::pow(point.y - (line_start.y+u*dy),2);
105 }
106 
117 template <typename Point, typename LinePoint>
118 inline double computeDistanceToLineSegment(const Point& point, const LinePoint& line_start, const LinePoint& line_end, bool* is_inbetween=NULL)
119 {
120  return std::sqrt(computeSquaredDistanceToLineSegment(point, line_start, line_end, is_inbetween));
121 }
122 
123 
132 template <typename Point1, typename Point2>
133 inline double norm2d(const Point1& pt1, const Point2& pt2)
134 {
135  return std::sqrt( std::pow(pt2.x - pt1.x, 2) + std::pow(pt2.y - pt1.y, 2) );
136 }
137 
147 template <typename Point1, typename Point2>
148 inline bool isApprox2d(const Point1& pt1, const Point2& pt2, double threshold)
149 {
150  return ( std::abs(pt2.x-pt1.x)<threshold && std::abs(pt2.y-pt1.y)<threshold );
151 }
152 
153 
154 
155 } //end namespace teb_local_planner
156 
157 #endif /* MISC_H_ */
double norm2d(const Point1 &pt1, const Point2 &pt2)
Calculate the distance between two 2d points.
Definition: misc.h:133
double computeDistanceToLineSegment(const Point &point, const LinePoint &line_start, const LinePoint &line_end, bool *is_inbetween=NULL)
Calculate the distance between a point and a straight line segment.
Definition: misc.h:118
TFSIMD_FORCE_INLINE const tfScalar & x() const
double computeDistanceToLine(const Point &point, const LinePoint &line_pt1, const LinePoint &line_pt2)
Calculate the distance between a point and a straight line (with infinite length) ...
Definition: misc.h:58
double computeSquaredDistanceToLineSegment(const Point &point, const LinePoint &line_start, const LinePoint &line_end, bool *is_inbetween=NULL)
Calculate the squared distance between a point and a straight line segment.
Definition: misc.h:83
TFSIMD_FORCE_INLINE const tfScalar & y() const
TFSIMD_FORCE_INLINE tfScalar length(const Quaternion &q)
bool isApprox2d(const Point1 &pt1, const Point2 &pt2, double threshold)
Check if two points are approximately defining the same one.
Definition: misc.h:148


costmap_converter
Author(s): Christoph Rösmann , Franz Albers , Otniel Rinaldo
autogenerated on Sat May 16 2020 03:19:18