/usr/include/ignition/math2/ignition/math/Line2.hh is in libignition-math2-dev 2.9.0+dfsg1-1.
This file is owned by root:root, with mode 0o644.
The actual contents of the file can be viewed below.
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75 76 77 78 79 80 81 82 83 84 85 86 87 88 89 90 91 92 93 94 95 96 97 98 99 100 101 102 103 104 105 106 107 108 109 110 111 112 113 114 115 116 117 118 119 120 121 122 123 124 125 126 127 128 129 130 131 132 133 134 135 136 137 138 139 140 141 142 143 144 145 146 147 148 149 150 151 152 153 154 155 156 157 158 159 160 161 162 163 164 165 166 167 168 169 170 171 172 173 174 175 176 177 178 179 180 181 182 183 184 185 186 187 188 189 190 191 192 193 194 195 196 197 198 199 200 201 202 203 204 205 206 207 208 209 210 211 212 213 214 215 216 217 218 219 220 221 222 223 224 225 226 227 228 229 230 231 232 233 234 235 236 237 238 239 240 241 242 243 244 245 246 247 248 249 250 251 252 253 254 255 256 257 258 259 260 261 262 263 264 265 266 267 268 269 270 271 272 273 274 275 276 277 278 279 280 281 282 283 284 285 286 287 288 289 290 291 292 293 294 295 296 297 298 299 300 301 302 303 304 305 306 307 308 309 310 311 312 313 314 315 316 317 318 319 | /*
* Copyright (C) 2014 Open Source Robotics Foundation
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*
*/
#ifndef IGNITION_MATH_LINE2_HH_
#define IGNITION_MATH_LINE2_HH_
#include <algorithm>
#include <ignition/math/Vector2.hh>
#include <ignition/math/IndexException.hh>
namespace ignition
{
namespace math
{
/// \class Line2 Line2.hh ignition/math/Line2.hh
/// \brief A two dimensional line segment. The line is defined by a
/// start and end point.
template<typename T>
class Line2
{
/// \brief Constructor.
/// \param[in] _ptA Start point of the line segment
/// \param[in] _ptB End point of the line segment
public: Line2(const math::Vector2<T> &_ptA, const math::Vector2<T> &_ptB)
{
this->Set(_ptA, _ptB);
}
/// \brief Constructor.
/// \param[in] _x1 X coordinate of the start point.
/// \param[in] _y1 Y coordinate of the start point.
/// \param[in] _x2 X coordinate of the end point.
/// \param[in] _y2 Y coordinate of the end point.
public: Line2(double _x1, double _y1, double _x2, double _y2)
{
this->Set(_x1, _y1, _x2, _y2);
}
/// \brief Set the start and end point of the line segment
/// \param[in] _ptA Start point of the line segment
/// \param[in] _ptB End point of the line segment
public: void Set(const math::Vector2<T> &_ptA,
const math::Vector2<T> &_ptB)
{
this->pts[0] = _ptA;
this->pts[1] = _ptB;
}
/// \brief Set the start and end point of the line segment
/// \param[in] _x1 X coordinate of the start point.
/// \param[in] _y1 Y coordinate of the start point.
/// \param[in] _x2 X coordinate of the end point.
/// \param[in] _y2 Y coordinate of the end point.
public: void Set(double _x1, double _y1, double _x2, double _y2)
{
this->pts[0].Set(_x1, _y1);
this->pts[1].Set(_x2, _y2);
}
/// \brief Return the cross product of this line and the given line.
/// Give 'a' as this line and 'b' as given line, the equation is:
/// (a.start.x - a.end.x) * (b.start.y - b.end.y) -
/// (a.start.y - a.end.y) * (b.start.x - b.end.x)
/// \param[in] _line Line for the cross product computation.
/// \return Return the cross product of this line and the given line.
public: double CrossProduct(const Line2<T> &_line) const
{
return (this->pts[0].X() - this->pts[1].X()) *
(_line[0].Y() -_line[1].Y()) -
(this->pts[0].Y() - this->pts[1].Y()) *
(_line[0].X() - _line[1].X());
}
/// \brief Return the cross product of this line and the given point.
/// Given 'a' and 'b' as the start and end points, the equation is:
// (_pt.y - a.y) * (b.x - a.x) - (_pt.x - a.x) * (b.y - a.y)
/// \param[in] _pt Point for the cross product computation.
/// \return Return the cross product of this line and the given point.
public: double CrossProduct(const Vector2<T> &_pt) const
{
return (_pt.Y() - this->pts[0].Y()) *
(this->pts[1].X() - this->pts[0].X()) -
(_pt.X() - this->pts[0].X()) *
(this->pts[1].Y() - this->pts[0].Y());
}
/// \brief Check if the given point is collinear with this line.
/// \param[in] _pt The point to check.
/// \param[in] _epsilon The error bounds within which the collinear
/// check will return true.
/// \return Return true if the point is collinear with this line, false
/// otherwise.
public: bool Collinear(const math::Vector2<T> &_pt,
double _epsilon = 1e-6) const
{
return math::equal(this->CrossProduct(_pt),
static_cast<T>(0), _epsilon);
}
/// \brief Check if the given line is parallel with this line.
/// \param[in] _line The line to check.
/// \param[in] _epsilon The error bounds within which the parallel
/// check will return true.
/// \return Return true if the line is parallel with this line, false
/// otherwise. Return true if either line is a point (line with zero
/// length).
public: bool Parallel(const math::Line2<T> &_line,
double _epsilon = 1e-6) const
{
return math::equal(this->CrossProduct(_line),
static_cast<T>(0), _epsilon);
}
/// \brief Check if the given line is collinear with this line. This
/// is the AND of Parallel and Intersect.
/// \param[in] _line The line to check.
/// \param[in] _epsilon The error bounds within which the collinear
/// check will return true.
/// \return Return true if the line is collinear with this line, false
/// otherwise.
public: bool Collinear(const math::Line2<T> &_line,
double _epsilon = 1e-6) const
{
return this->Parallel(_line, _epsilon) &&
this->Intersect(_line, _epsilon);
}
/// \brief Return whether the given point is on this line segment.
/// \param[in] _pt Point to check.
/// \param[in] _epsilon The error bounds within which the OnSegment
/// check will return true.
/// \return True if the point is on the segement.
public: bool OnSegment(const math::Vector2<T> &_pt,
double _epsilon = 1e-6) const
{
return this->Collinear(_pt, _epsilon) && this->Within(_pt, _epsilon);
}
/// \brief Check if the given point is between the start and end
/// points of the line segment. This does not imply that the point is
/// on the segment.
/// \param[in] _pt Point to check.
/// \param[in] _epsilon The error bounds within which the within
/// check will return true.
/// \return True if the point is on the segement.
public: bool Within(const math::Vector2<T> &_pt,
double _epsilon = 1e-6) const
{
return _pt.X() <= std::max(this->pts[0].X(),
this->pts[1].X()) + _epsilon &&
_pt.X() >= std::min(this->pts[0].X(),
this->pts[1].X()) - _epsilon &&
_pt.Y() <= std::max(this->pts[0].Y(),
this->pts[1].Y()) + _epsilon &&
_pt.Y() >= std::min(this->pts[0].Y(),
this->pts[1].Y()) - _epsilon;
}
/// \brief Check if this line intersects the given line segment.
/// \param[in] _line The line to check for intersection.
/// \param[in] _epsilon The error bounds within which the intersection
/// check will return true.
/// \return True if an intersection was found.
public: bool Intersect(const Line2<T> &_line,
double _epsilon = 1e-6) const
{
static math::Vector2<T> ignore;
return this->Intersect(_line, ignore, _epsilon);
}
/// \brief Check if this line intersects the given line segment. The
/// point of intersection is returned in the _result parameter.
/// \param[in] _line The line to check for intersection.
/// \param[out] _pt The point of intersection. This value is only
/// valid if the return value is true.
/// \param[in] _epsilon The error bounds within which the intersection
/// check will return true.
/// \return True if an intersection was found.
public: bool Intersect(const Line2<T> &_line, math::Vector2<T> &_pt,
double _epsilon = 1e-6) const
{
double d = this->CrossProduct(_line);
// d is zero if the two line are collinear. Must check special
// cases.
if (math::equal(d, 0.0, _epsilon))
{
// Check if _line's starting point is on the line.
if (this->Within(_line[0], _epsilon))
{
_pt = _line[0];
return true;
}
// Check if _line's ending point is on the line.
else if (this->Within(_line[1], _epsilon))
{
_pt = _line[1];
return true;
}
// Other wise return false.
else
return false;
}
_pt.X((_line[0].X() - _line[1].X()) *
(this->pts[0].X() * this->pts[1].Y() -
this->pts[0].Y() * this->pts[1].X()) -
(this->pts[0].X() - this->pts[1].X()) *
(_line[0].X() * _line[1].Y() - _line[0].Y() * _line[1].X()));
_pt.Y((_line[0].Y() - _line[1].Y()) *
(this->pts[0].X() * this->pts[1].Y() -
this->pts[0].Y() * this->pts[1].X()) -
(this->pts[0].Y() - this->pts[1].Y()) *
(_line[0].X() * _line[1].Y() - _line[0].Y() * _line[1].X()));
_pt /= d;
if (_pt.X() < std::min(this->pts[0].X(), this->pts[1].X()) ||
_pt.X() > std::max(this->pts[0].X(), this->pts[1].X()) ||
_pt.X() < std::min(_line[0].X(), _line[1].X()) ||
_pt.X() > std::max(_line[0].X(), _line[1].X()))
{
return false;
}
if (_pt.Y() < std::min(this->pts[0].Y(), this->pts[1].Y()) ||
_pt.Y() > std::max(this->pts[0].Y(), this->pts[1].Y()) ||
_pt.Y() < std::min(_line[0].Y(), _line[1].Y()) ||
_pt.Y() > std::max(_line[0].Y(), _line[1].Y()))
{
return false;
}
return true;
}
/// \brief Get the length of the line
/// \return The length of the line.
public: T Length() const
{
return sqrt((this->pts[0].X() - this->pts[1].X()) *
(this->pts[0].X() - this->pts[1].X()) +
(this->pts[0].Y() - this->pts[1].Y()) *
(this->pts[0].Y() - this->pts[1].Y()));
}
/// \brief Get the slope of the line
/// \return The slope of the line, NAN_D if the line is vertical.
public: double Slope() const
{
if (math::equal(this->pts[1].X(), this->pts[0].X()))
return NAN_D;
return (this->pts[1].Y() - this->pts[0].Y()) /
static_cast<double>(this->pts[1].X() - this->pts[0].X());
}
/// \brief Equality operator.
/// \param[in] _line Line to compare for equality.
/// \return True if the given line is equal to this line
public: bool operator==(const Line2<T> &_line) const
{
return this->pts[0] == _line[0] && this->pts[1] == _line[1];
}
/// \brief Inequality operator.
/// \param[in] _line Line to compare for inequality.
/// \return True if the given line is not to this line
public: bool operator!=(const Line2<T> &_line) const
{
return !(*this == _line);
}
/// \brief Get the start or end point.
/// \param[in] _index 0 = start point, 1 = end point.
/// \throws IndexException if _index is > 1.
public: math::Vector2<T> operator[](size_t _index) const
{
if (_index > 1)
throw IndexException();
return this->pts[_index];
}
/// \brief Stream extraction operator
/// \param[in] _out output stream
/// \param[in] _pt Line2 to output
/// \return The stream
/// \throws N/A.
public: friend std::ostream &operator<<(
std::ostream &_out, const Line2<T> &_line)
{
_out << _line[0] << " " << _line[1];
return _out;
}
private: math::Vector2<T> pts[2];
};
typedef Line2<int> Line2i;
typedef Line2<double> Line2d;
typedef Line2<float> Line2f;
}
}
#endif
|