source: rrlib_geometry/examples/geometries.cpp @ 93:a5e38d1d01b4

14.08
Last change on this file since 93:a5e38d1d01b4 was 93:a5e38d1d01b4, checked in by Tobias Föhst <tobias.foehst@…>, 12 months ago

Adds functions to get the curvature from a spline curve and to find the maximum curvature on Bezier curves and splines

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