source: rrlib_geometry/test/test_geometries.cpp @ 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: 13.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    test_geometries.cpp
23 *
24 * \author  Tobias Foehst
25 *
26 * \date    2010-12-26
27 *
28 */
29//----------------------------------------------------------------------
30
31//----------------------------------------------------------------------
32// External includes (system with <>, local with "")
33//----------------------------------------------------------------------
34#include <cstdlib>
35#include <iostream>
36#include <vector>
37
38#include "rrlib/highgui_wrapper/tWindow.h"
39
40//----------------------------------------------------------------------
41// Internal includes with ""
42//----------------------------------------------------------------------
43#include "rrlib/geometry/tShape.h"
44#include "rrlib/geometry/tLine.h"
45#include "rrlib/geometry/curves/tBezierCurve.h"
46#include "rrlib/geometry/curves/tUniformBSplineCurve.h"
47#include "rrlib/geometry/curves/tCardinalSplineCurve.h"
48#include "rrlib/geometry/functions.h"
49
50//----------------------------------------------------------------------
51// Debugging
52//----------------------------------------------------------------------
53#include <cassert>
54
55//----------------------------------------------------------------------
56// Namespace usage
57//----------------------------------------------------------------------
58using namespace rrlib::highgui;
59
60//----------------------------------------------------------------------
61// Forward declarations / typedefs / enums
62//----------------------------------------------------------------------
63typedef double tElement;
64typedef rrlib::math::tVector<2, tElement> tVector;
65typedef rrlib::geometry::tShape<2, tElement> tShape;
66typedef rrlib::geometry::tPoint<2, tElement> tPoint;
67typedef rrlib::geometry::tLine<2, tElement> tLine;
68typedef rrlib::geometry::tLineSegment<2, tElement> tLineSegment;
69typedef rrlib::geometry::tBezierCurve<2, tElement, 3> tBezierCurve;
70typedef rrlib::geometry::tSplineCurve<2, tElement, 3> tSplineCurve;
71typedef rrlib::geometry::tUniformBSplineCurve<2, tElement> tConcreteSplineCurve;
72//typedef rrlib::geometry::tCardinalSplineCurve<2, tElement> tConcreteSplineCurve;
73
74//----------------------------------------------------------------------
75// Const values
76//----------------------------------------------------------------------
77
78//----------------------------------------------------------------------
79// Implementation
80//----------------------------------------------------------------------
81
82void DrawBoundingBox(tWindow &window, const tShape &shape)
83{
84  window.DrawRectangleNormalized(shape.BoundingBox().Min().X(), shape.BoundingBox().Min().Y(),
85                                 shape.BoundingBox().Max().X(), shape.BoundingBox().Max().Y());
86}
87
88void DrawPoint(tWindow &window, const tPoint &point)
89{
90  window.DrawCircleNormalized(point.X(), point.Y(), 0.01, true);
91}
92
93void DrawLineSegment(tWindow &window, const tLineSegment &line_segment)
94{
95  window.DrawLineNormalized(line_segment.Begin().X(), line_segment.Begin().Y(), line_segment.End().X(), line_segment.End().Y());
96}
97
98void DrawLine(tWindow &window, const tLine &line)
99{
100  tPoint end_points[4];
101  size_t index = 0;
102  index += line.GetIntersection(end_points[index], tLineSegment(tPoint(0, 0), tPoint(0, 1))) ? 1 : 0;
103  index += line.GetIntersection(end_points[index], tLineSegment(tPoint(0, 1), tPoint(1, 1))) ? 1 : 0;
104  index += line.GetIntersection(end_points[index], tLineSegment(tPoint(1, 1), tPoint(1, 0))) ? 1 : 0;
105  index += line.GetIntersection(end_points[index], tLineSegment(tPoint(1, 0), tPoint(0, 0))) ? 1 : 0;
106  assert(index == 2);
107  DrawLineSegment(window, tLineSegment(end_points[0], end_points[1]));
108}
109
110void DrawControlPoints(tWindow &window, const std::vector<tPoint> &data)
111{
112  for (std::vector<tPoint>::const_iterator it = data.begin(); it != data.end(); it++)
113  {
114    DrawPoint(window, *it);
115  }
116}
117
118void DrawControlPolygon(tWindow &window, const tBezierCurve &bezier_curve)
119{
120  for (size_t i = 1; i < bezier_curve.NumberOfControlPoints(); ++i)
121  {
122    const tPoint &start(bezier_curve.ControlPoints()[i - 1]);
123    const tPoint &stop(bezier_curve.ControlPoints()[i]);
124
125    window.DrawLineNormalized(start.X(), start.Y(), stop.X(), stop.Y());
126  }
127}
128
129void DrawControlPolygon(tWindow &window, const tSplineCurve &spline_curve)
130{
131  for (size_t i = 1; i < spline_curve.NumberOfControlPoints(); ++i)
132  {
133    const tPoint &start(spline_curve.ControlPoints()[i - 1]);
134    const tPoint &stop(spline_curve.ControlPoints()[i]);
135
136    window.DrawLineNormalized(start.X(), start.Y(), stop.X(), stop.Y());
137  }
138}
139
140void DrawBezierCurve(tWindow &window, const tBezierCurve &bezier_curve, float epsilon = 1.0E-6)
141{
142  if (bezier_curve.GetTwist() < epsilon)
143  {
144    DrawControlPolygon(window, bezier_curve);
145    return;
146  }
147
148  std::pair<tBezierCurve, tBezierCurve> subdivision(bezier_curve.GetSubdivision());
149  DrawBezierCurve(window, subdivision.first, epsilon);
150  DrawBezierCurve(window, subdivision.second, epsilon);
151}
152
153void DrawSplineCurve(tWindow &window, const tSplineCurve &spline_curve, float epsilon = 1.0E-6)
154{
155  for (size_t i = 0; i < spline_curve.NumberOfSegments(); ++i)
156  {
157    DrawBezierCurve(window, *spline_curve.GetBezierCurveForSegment(i), epsilon);
158  }
159}
160
161int main(int argc, char **argv)
162{
163  tWindow &window(tWindow::GetInstance("Test Geometries", 500, 500));
164
165  std::cout << std::endl << "=== A line segment with its bounding box ===" << std::endl;
166
167  tLineSegment line_segment(tPoint(0.1, 0.2), tPoint(0.9, 0.4));
168
169  window.Clear();
170  window.SetColor(0);
171  DrawLineSegment(window, line_segment);
172  window.SetColor(1);
173  DrawBoundingBox(window, line_segment);
174  window.Render();
175
176  std::cout << std::endl << "=== A line ===" << std::endl;
177
178  tLine line(tPoint(0.1, 0.2), tPoint(0.9, 0.4));
179
180  window.Clear();
181  window.SetColor(1);
182  DrawLine(window, line);
183  window.Render();
184
185  std::cout << std::endl << "=== Control points ===" << std::endl;
186
187  std::vector<tPoint> control_points;
188  control_points.push_back(tPoint(0.1, 0.2));
189  control_points.push_back(tPoint(0.3, 0.5));
190  control_points.push_back(tPoint(0.7, 0.6));
191  control_points.push_back(tPoint(0.5, 0.2));
192
193  window.Clear();
194  window.SetColor(0);
195  DrawControlPoints(window, control_points);
196  window.Render();
197
198  std::cout << std::endl << "=== Control polygon and bounding box ===" << std::endl;
199
200  tBezierCurve bezier_curve(control_points.begin(), control_points.end());
201
202  window.Clear();
203  window.SetColor(1);
204  DrawControlPolygon(window, bezier_curve);
205  window.SetColor(2);
206  DrawBoundingBox(window, bezier_curve);
207  window.Render();
208
209  std::cout << std::endl << "=== Bezier curve ===" << std::endl;
210
211  window.SetColor(3);
212  DrawBezierCurve(window, bezier_curve);
213  window.Render();
214
215
216  for (int i = 0; i <= 100; ++i)
217  {
218    double t = i / 100.0;
219    tPoint p = bezier_curve(t);
220    window.Clear();
221    DrawBezierCurve(window, bezier_curve);
222    DrawPoint(window, p);
223
224    double curvature = rrlib::geometry::GetCurvature(bezier_curve, t);
225    if (curvature > 0.0)
226    {
227      double radius = 1.0 / curvature;
228
229      tBezierCurve::tDerivative first(bezier_curve.GetDerivative());
230      tPoint m(p + (first(t).Normalized().Rotated(-M_PI_2) * radius));
231
232      window.SetColor(1);
233      window.DrawCircleNormalized(m.X(), m.Y(), radius, false);
234    }
235
236    window.Render();
237  }
238
239  std::cout << std::endl << "=== Bezier curve intersects line ===" << std::endl;
240
241  window.Clear();
242  window.SetColor(0);
243  line.Translate(tVector(0, 0.1));
244  DrawLine(window, line);
245  window.SetColor(1);
246  DrawBezierCurve(window, bezier_curve);
247
248  window.Render();
249
250  std::vector<tPoint> intersection_points;
251  std::vector<tElement> intersection_parameters;
252  bezier_curve.GetIntersections(intersection_points, intersection_parameters, line);
253  std::cout << "number of intersections: " << intersection_points.size() << std::endl;
254
255  window.SetColor(2);
256  for (std::vector<tPoint>::iterator it = intersection_points.begin(); it != intersection_points.end(); ++it)
257  {
258    std::cout << *it << std::endl;
259    DrawPoint(window, *it);
260  }
261  window.Render();
262
263  std::cout << std::endl << "=== Bezier curve intersects curve ===" << std::endl;
264
265  tBezierCurve bezier_curve2(tPoint(0.2, 0.1), tPoint(0.5, 0.3), tPoint(0.6, 0.7), tPoint(0.4, 0.9));
266
267  rrlib::math::tAngleDeg angle = -40;
268  tPoint position = bezier_curve2.CenterOfGravity();
269  bezier_curve2.Translate(-position);
270  bezier_curve2.Rotate(rrlib::math::Get2DRotationMatrix<double>(angle));
271  bezier_curve2.Translate(position);
272  bezier_curve2.Translate(tPoint(0, -0.1));
273
274  window.Clear();
275  window.SetColor(0);
276  DrawBezierCurve(window, bezier_curve2);
277  window.SetColor(1);
278  DrawBezierCurve(window, bezier_curve);
279
280  window.Render();
281
282  intersection_points.clear();
283  intersection_parameters.clear();
284
285  bezier_curve.GetIntersections(intersection_points, intersection_parameters, bezier_curve2);
286  std::cout << "number of intersections: " << intersection_points.size() << std::endl;
287
288  window.SetColor(2);
289  for (std::vector<tPoint>::iterator it = intersection_points.begin(); it != intersection_points.end(); ++it)
290  {
291    DrawPoint(window, *it);
292  }
293  window.Render();
294
295  std::cout << std::endl << "=== Spline with bounding box ===" << std::endl;
296
297  control_points.clear();
298  control_points.push_back(tPoint(0.1, 0.2));
299  control_points.push_back(tPoint(0.1, 0.2));
300  control_points.push_back(tPoint(0.1, 0.2));
301  control_points.push_back(tPoint(0.3, 0.5));
302  control_points.push_back(tPoint(0.7, 0.6));
303  control_points.push_back(tPoint(0.9, 0.4));
304
305  tConcreteSplineCurve spline(control_points.begin(), control_points.end());
306
307  window.Clear();
308  window.SetColor(1);
309  DrawControlPolygon(window, spline);
310  window.SetColor(2);
311  DrawBoundingBox(window, spline);
312  window.SetColor(3);
313  DrawSplineCurve(window, spline);
314  window.Render();
315
316  std::cout << std::endl << "=== Adding point ===" << std::endl;
317
318  spline.AppendControlPoint(tPoint(0.75, 0.7));
319  window.Clear();
320  window.SetColor(1);
321  DrawControlPolygon(window, spline);
322  window.SetColor(2);
323  DrawBoundingBox(window, spline);
324  window.SetColor(3);
325  DrawSplineCurve(window, spline);
326  window.Render();
327
328  std::cout << std::endl << "=== Adding point ===" << std::endl;
329
330  spline.AppendControlPoint(tPoint(0.5, 0.8));
331  window.Clear();
332  window.SetColor(1);
333  DrawControlPolygon(window, spline);
334  window.SetColor(2);
335  DrawBoundingBox(window, spline);
336  window.SetColor(3);
337  DrawSplineCurve(window, spline);
338  window.Render();
339
340  std::cout << std::endl << "=== Adding point ===" << std::endl;
341
342  spline.AppendControlPoint(tPoint(0.2, 0.4));
343  window.Clear();
344  window.SetColor(1);
345  DrawControlPolygon(window, spline);
346  window.SetColor(2);
347  DrawBoundingBox(window, spline);
348  window.SetColor(3);
349  DrawSplineCurve(window, spline);
350  window.Render();
351
352  std::cout << std::endl << "=== Adding point ===" << std::endl;
353
354  spline.AppendControlPoint(tPoint(0.2, 0.4));
355  window.Clear();
356  window.SetColor(1);
357  DrawControlPolygon(window, spline);
358  window.SetColor(2);
359  DrawBoundingBox(window, spline);
360  window.SetColor(3);
361  DrawSplineCurve(window, spline);
362  window.Render();
363
364  std::cout << std::endl << "=== Adding point ===" << std::endl;
365
366  spline.AppendControlPoint(tPoint(0.2, 0.4));
367  window.Clear();
368  window.SetColor(1);
369  DrawControlPolygon(window, spline);
370  window.SetColor(2);
371  DrawBoundingBox(window, spline);
372  window.SetColor(3);
373  DrawSplineCurve(window, spline);
374  window.Render();
375
376  std::cout << std::endl << "=== Spline intersects line ===" << std::endl;
377
378  window.Clear();
379  window.SetColor(0);
380  line.Translate(tVector(0, 0.1));
381  DrawLine(window, line);
382  window.SetColor(1);
383  DrawSplineCurve(window, spline);
384
385  window.Render();
386
387  intersection_points.clear();
388  intersection_parameters.clear();
389  spline.GetIntersections(intersection_points, intersection_parameters, line);
390  std::cout << "number of intersections: " << intersection_points.size() << std::endl;
391
392  window.SetColor(2);
393  for (std::vector<tPoint>::iterator it = intersection_points.begin(); it != intersection_points.end(); ++it)
394  {
395    std::cout << *it << std::endl;
396    DrawPoint(window, *it);
397  }
398  window.Render();
399
400  std::cout << std::endl << "=== Closest point to Bezier curve ===" << std::endl;
401
402  window.Clear();
403  window.SetColor(0);
404  DrawBezierCurve(window, bezier_curve);
405  window.Render();
406
407  for (int i = 0; i < 10; ++i)
408  {
409    tPoint reference_point(drand48(), drand48());
410    tPoint closest_point(bezier_curve.GetClosestPoint(reference_point));
411
412    window.SetColor(3);
413    DrawLineSegment(window, tLineSegment(reference_point, closest_point));
414    window.SetColor(1);
415    DrawPoint(window, reference_point);
416    window.SetColor(2);
417    DrawPoint(window, closest_point);
418    window.Render();
419  }
420
421  std::cout << std::endl << "=== Closest point to spline curve ===" << std::endl;
422
423  window.Clear();
424  window.SetColor(0);
425  DrawSplineCurve(window, spline);
426  window.Render();
427
428  for (int i = 0; i < 10; ++i)
429  {
430    tPoint reference_point(drand48(), drand48());
431    tPoint closest_point(spline.GetClosestPoint(reference_point));
432
433    window.SetColor(3);
434    DrawLineSegment(window, tLineSegment(reference_point, closest_point));
435    window.SetColor(1);
436    DrawPoint(window, reference_point);
437    window.SetColor(2);
438    DrawPoint(window, closest_point);
439    window.Render();
440  }
441
442  window.Render();
443
444  tWindow::ReleaseAllInstances();
445
446  return EXIT_SUCCESS;
447}
Note: See TracBrowser for help on using the repository browser.