source: rrlib_geometry/tLine.hpp @ 60:a22756932ab5

Last change on this file since 60:a22756932ab5 was 60:a22756932ab5, checked in by Tobias Föhst <foehst@…>, 6 years ago

Added and updated license information

File size: 10.9 KB
Line 
1//
2// You received this file as part of RRLib
3// Robotics Research Library
4//
5// Copyright (C) Finroc GbR (finroc.org)
6//
7// This program is free software; you can redistribute it and/or modify
8// it under the terms of the GNU General Public License as published by
9// the Free Software Foundation; either version 2 of the License, or
10// (at your option) any later version.
11//
12// This program is distributed in the hope that it will be useful,
13// but WITHOUT ANY WARRANTY; without even the implied warranty of
14// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
15// GNU General Public License for more details.
16//
17// You should have received a copy of the GNU General Public License along
18// with this program; if not, write to the Free Software Foundation, Inc.,
19// 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.
20//
21//----------------------------------------------------------------------
22/*!\file    tLine.hpp
23 *
24 * \author  Tobias Foehst
25 *
26 * \date    2010-12-27
27 *
28 */
29//----------------------------------------------------------------------
30
31//----------------------------------------------------------------------
32// External includes (system with <>, local with "")
33//----------------------------------------------------------------------
34#include "rrlib/math/utilities.h"
35#include "rrlib/math/tLUDecomposition.h"
36
37//----------------------------------------------------------------------
38// Internal includes with ""
39//----------------------------------------------------------------------
40#include "rrlib/geometry/tLineSegment.h"
41
42//----------------------------------------------------------------------
43// Debugging
44//----------------------------------------------------------------------
45#include <cassert>
46
47//----------------------------------------------------------------------
48// Namespace declaration
49//----------------------------------------------------------------------
50namespace rrlib
51{
52namespace geometry
53{
54
55//----------------------------------------------------------------------
56// Forward declarations / typedefs / enums
57//----------------------------------------------------------------------
58
59//----------------------------------------------------------------------
60// Const values
61//----------------------------------------------------------------------
62
63//----------------------------------------------------------------------
64// Implementation
65//----------------------------------------------------------------------
66
67//----------------------------------------------------------------------
68// tLine constructors
69//----------------------------------------------------------------------
70template <size_t Tdimension, typename TElement>
71tLine<Tdimension, TElement>::tLine()
72{
73  this->direction[0] = 1;
74}
75
76template <size_t Tdimension, typename TElement>
77tLine<Tdimension, TElement>::tLine(const typename tShape::tPoint &support, const math::tVector<Tdimension, TElement> &direction)
78  : support(support),
79    direction(direction.Normalized())
80{}
81
82template <size_t Tdimension, typename TElement>
83tLine<Tdimension, TElement>::tLine(const typename tShape::tPoint &support, const typename tShape::tPoint &second_support)
84  : support(support),
85    direction((second_support - support).Normalized())
86{}
87
88//----------------------------------------------------------------------
89// tLine Set
90//----------------------------------------------------------------------
91template <size_t Tdimension, typename TElement>
92void tLine<Tdimension, TElement>::Set(const typename tShape::tPoint &support, const math::tVector<Tdimension, TElement> &direction)
93{
94  this->support = support;
95  this->direction = direction.Normalized();
96  this->SetChanged();
97}
98
99//----------------------------------------------------------------------
100// tLine Evaluation: operator ()
101//----------------------------------------------------------------------
102template <size_t Tdimension, typename TElement>
103const typename tShape<Tdimension, TElement>::tPoint tLine<Tdimension, TElement>::operator()(tParameter t) const
104{
105  return math::tVector<Tdimension, TElement>(this->support + t * this->direction);
106}
107
108//----------------------------------------------------------------------
109// tLine GetDistanceToPoint
110//----------------------------------------------------------------------
111template <size_t Tdimension, typename TElement>
112const TElement tLine<Tdimension, TElement>::GetDistanceToPoint(const typename tShape::tPoint &point) const
113{
114  return (point - this->GetClosestPoint(point)).Length();
115}
116
117//----------------------------------------------------------------------
118// tLine GetClosestPoint
119//----------------------------------------------------------------------
120template <size_t Tdimension, typename TElement>
121const typename tShape<Tdimension, TElement>::tPoint tLine<Tdimension, TElement>::GetClosestPoint(const typename tShape::tPoint &reference_point) const
122{
123  //          reference_point
124  //                /|sin
125  //               / |
126  // -------support--F-----> direction
127  //               cos
128  math::tVector<Tdimension, TElement> support_to_reference(reference_point - this->support);
129  return math::tVector<Tdimension, TElement>((*this)(EnclosedAngle(this->direction, support_to_reference).Cosine() * support_to_reference.Length()));
130}
131
132namespace
133{
134
135template <size_t Tdimension, typename TElement>
136bool IntersectLineWithLine(typename tLine<Tdimension, TElement>::tPoint &intersection_point, const tLine<Tdimension, TElement> &left, const tLine<Tdimension, TElement> &right)
137{
138  math::tMatrix<Tdimension, 2, TElement> matrix;
139  for (size_t i = 0; i < Tdimension; ++i)
140  {
141    matrix[i][0] = left.Direction()[i];
142    matrix[i][1] = -right.Direction()[i];
143  }
144
145  try
146  {
147    TElement t = math::tLUDecomposition<2, TElement>(matrix).Solve(right.Support() - left.Support())[0];
148    intersection_point = left.Support() + t * left.Direction();
149    return true;
150  }
151  catch (const std::logic_error &)
152  {
153    return false;
154  }
155}
156
157template <size_t Tdimension, typename TElement>
158bool IntersectLineWithLineSegment(typename tLine<Tdimension, TElement>::tPoint &intersection_point, const tLine<Tdimension, TElement> &left, const geometry::tLineSegment<Tdimension, TElement> &right)
159{
160  if (!IntersectLineWithLine(intersection_point, left, right))
161  {
162    return false;
163  }
164  return math::IsEqual(right.GetDistanceToPoint(intersection_point), 0);
165}
166
167template <size_t Tdimension, typename TElement>
168bool IntersectLineSegmentWithLineSegment(typename tLine<Tdimension, TElement>::tPoint &intersection_point, const tLineSegment<Tdimension, TElement> &left, const tLineSegment<Tdimension, TElement> &right)
169{
170  if (!left.BoundingBox().Intersects(right.BoundingBox()))
171  {
172    return false;
173  }
174
175  if (!IntersectLineWithLineSegment(intersection_point, left, right))
176  {
177    return false;
178  }
179  return math::IsEqual(left.GetDistanceToPoint(intersection_point), 0);
180}
181
182}
183
184//----------------------------------------------------------------------
185// tLine GetIntersection
186//----------------------------------------------------------------------
187template <size_t Tdimension, typename TElement>
188const bool tLine<Tdimension, TElement>::GetIntersection(typename tShape::tPoint &intersection_point, const tLine &line) const
189{
190  typedef geometry::tLineSegment<Tdimension, TElement> tLineSegment;
191  if (const tLineSegment *this_segment = dynamic_cast<const tLineSegment *>(this))
192  {
193    if (const tLineSegment *line_segment = dynamic_cast<const tLineSegment *>(&line))
194    {
195      return IntersectLineSegmentWithLineSegment(intersection_point, *this_segment, *line_segment);
196    }
197    return IntersectLineWithLineSegment(intersection_point, line, *this_segment);
198  }
199  if (const tLineSegment *line_segment = dynamic_cast<const tLineSegment *>(&line))
200  {
201    return IntersectLineWithLineSegment(intersection_point, *this, *line_segment);
202  }
203  return IntersectLineWithLine(intersection_point, *this, line);
204}
205
206//----------------------------------------------------------------------
207// tLine Translate
208//----------------------------------------------------------------------
209template <size_t Tdimension, typename TElement>
210tLine<Tdimension, TElement> &tLine<Tdimension, TElement>::Translate(const math::tVector<Tdimension, TElement> &translation)
211{
212  this->support += translation;
213  this->SetChanged();
214  return *this;
215}
216
217//----------------------------------------------------------------------
218// tLine Rotate
219//----------------------------------------------------------------------
220template <size_t Tdimension, typename TElement>
221tLine<Tdimension, TElement> &tLine<Tdimension, TElement>::Rotate(const math::tMatrix<Tdimension, Tdimension, TElement> &rotation)
222{
223  assert(math::IsEqual(rotation.Determinant(), 1));
224  this->support = rotation * this->support;
225  this->direction = rotation * this->direction;
226  this->SetChanged();
227  return *this;
228}
229
230//----------------------------------------------------------------------
231// tLine Transform
232//----------------------------------------------------------------------
233template <size_t Tdimension, typename TElement>
234tLine<Tdimension, TElement> &tLine<Tdimension, TElement>::Transform(const math::tMatrix < Tdimension + 1, Tdimension + 1, TElement > &transformation)
235{
236#ifndef NDEBUG
237  for (size_t i = 0; i < Tdimension; ++i)
238  {
239    assert(math::IsEqual(transformation[Tdimension][i], 0));
240  }
241  assert(math::IsEqual(transformation[Tdimension][Tdimension], 1));
242#endif
243  math::tMatrix<Tdimension, Tdimension, TElement> rotation;
244  for (size_t row = 0; row < Tdimension; ++row)
245  {
246    for (size_t column = 0; column < Tdimension; ++column)
247    {
248      rotation[row][column] = transformation[row][column];
249    }
250  }
251  assert(math::IsEqual(rotation.Determinant(), 1));
252
253  this->support = transformation.MultiplyHomogeneously(this->support);
254  this->direction = rotation * this->direction;
255  this->SetChanged();
256  return *this;
257}
258
259//----------------------------------------------------------------------
260// tLine UpdateBoundingBox
261//----------------------------------------------------------------------
262template <size_t Tdimension, typename TElement>
263void tLine<Tdimension, TElement>::UpdateBoundingBox(typename tShape::tBoundingBox &bounding_box) const
264{}
265
266//----------------------------------------------------------------------
267// tLine UpdateCenterOfGravity
268//----------------------------------------------------------------------
269template <size_t Tdimension, typename TElement>
270void tLine<Tdimension, TElement>::UpdateCenterOfGravity(typename tShape::tPoint &center_of_gravity) const
271{
272  center_of_gravity = this->GetClosestPoint(tShape::tPoint::Zero());
273}
274
275//----------------------------------------------------------------------
276// Operators for rrlib_canvas
277//----------------------------------------------------------------------
278#ifdef _LIB_RRLIB_CANVAS_PRESENT_
279
280template <typename TElement>
281canvas::tCanvas2D &operator << (canvas::tCanvas2D &canvas, const tLine<2, TElement> &line)
282{
283  canvas.DrawLine(line.Support(), line.Direction());
284
285  return canvas;
286}
287
288#endif
289
290//----------------------------------------------------------------------
291// End of namespace declaration
292//----------------------------------------------------------------------
293}
294}
Note: See TracBrowser for help on using the repository browser.