source: rrlib_geometry/test/test_geometries.cpp @ 24:627bf1924b86

Last change on this file since 24:627bf1924b86 was 24:627bf1924b86, checked in by Tobias Föhst <foehst@…>, 8 years ago

Added streaming operators for rrlib_canvas

File size: 14.1 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
8// modify it under the terms of the GNU General Public License
9// as published by the Free Software Foundation; either version 2
10// of the License, or (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
18// along with this program; if not, write to the Free Software
19// Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA  02111-1307, 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;
71//typedef rrlib::geometry::tUniformBSplineCurve<2, tElement> tConcreteSplineCurve;
72typedef 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  std::vector<tPoint> control_points2;
266  control_points2.push_back(tPoint(0.2, 0.1));
267  control_points2.push_back(tPoint(0.5, 0.3));
268  control_points2.push_back(tPoint(0.6, 0.7));
269  control_points2.push_back(tPoint(0.4, 0.9));
270
271  tBezierCurve bezier_curve2(control_points2.begin(), control_points2.end());
272
273  rrlib::math::tAngleDeg angle = -40;
274  tPoint position = bezier_curve2.CenterOfGravity();
275  bezier_curve2.Translate(-position);
276  bezier_curve2.Rotate(rrlib::math::Get2DRotationMatrix<double>(angle));
277  bezier_curve2.Translate(position);
278  bezier_curve2.Translate(tPoint(0, -0.1));
279
280  window.Clear();
281  window.SetColor(0);
282  DrawBezierCurve(window, bezier_curve2);
283  window.SetColor(1);
284  DrawBezierCurve(window, bezier_curve);
285
286  window.Render();
287
288  intersection_points.clear();
289  intersection_parameters.clear();
290
291  bezier_curve.GetIntersections(intersection_points, intersection_parameters, bezier_curve2);
292  std::cout << "number of intersections: " << intersection_points.size() << std::endl;
293
294  window.SetColor(2);
295  for (std::vector<tPoint>::iterator it = intersection_points.begin(); it != intersection_points.end(); ++it)
296  {
297    DrawPoint(window, *it);
298  }
299  window.Render();
300
301  std::cout << std::endl << "=== Spline with bounding box ===" << std::endl;
302
303  control_points.clear();
304  control_points.push_back(tPoint(0.1, 0.2));
305  control_points.push_back(tPoint(0.1, 0.2));
306  control_points.push_back(tPoint(0.1, 0.2));
307  control_points.push_back(tPoint(0.3, 0.5));
308  control_points.push_back(tPoint(0.7, 0.6));
309  control_points.push_back(tPoint(0.9, 0.4));
310
311  tConcreteSplineCurve spline(control_points.begin(), control_points.end());
312
313  window.Clear();
314  window.SetColor(1);
315  DrawControlPolygon(window, spline);
316  window.SetColor(2);
317  DrawBoundingBox(window, spline);
318  window.SetColor(3);
319  DrawSplineCurve(window, spline);
320  window.Render();
321
322  std::cout << std::endl << "=== Adding point ===" << std::endl;
323
324  spline.AppendControlPoint(tPoint(0.75, 0.7));
325  window.Clear();
326  window.SetColor(1);
327  DrawControlPolygon(window, spline);
328  window.SetColor(2);
329  DrawBoundingBox(window, spline);
330  window.SetColor(3);
331  DrawSplineCurve(window, spline);
332  window.Render();
333
334  std::cout << std::endl << "=== Adding point ===" << std::endl;
335
336  spline.AppendControlPoint(tPoint(0.5, 0.8));
337  window.Clear();
338  window.SetColor(1);
339  DrawControlPolygon(window, spline);
340  window.SetColor(2);
341  DrawBoundingBox(window, spline);
342  window.SetColor(3);
343  DrawSplineCurve(window, spline);
344  window.Render();
345
346  std::cout << std::endl << "=== Adding point ===" << std::endl;
347
348  spline.AppendControlPoint(tPoint(0.2, 0.4));
349  window.Clear();
350  window.SetColor(1);
351  DrawControlPolygon(window, spline);
352  window.SetColor(2);
353  DrawBoundingBox(window, spline);
354  window.SetColor(3);
355  DrawSplineCurve(window, spline);
356  window.Render();
357
358  std::cout << std::endl << "=== Adding point ===" << std::endl;
359
360  spline.AppendControlPoint(tPoint(0.2, 0.4));
361  window.Clear();
362  window.SetColor(1);
363  DrawControlPolygon(window, spline);
364  window.SetColor(2);
365  DrawBoundingBox(window, spline);
366  window.SetColor(3);
367  DrawSplineCurve(window, spline);
368  window.Render();
369
370  std::cout << std::endl << "=== Adding point ===" << std::endl;
371
372  spline.AppendControlPoint(tPoint(0.2, 0.4));
373  window.Clear();
374  window.SetColor(1);
375  DrawControlPolygon(window, spline);
376  window.SetColor(2);
377  DrawBoundingBox(window, spline);
378  window.SetColor(3);
379  DrawSplineCurve(window, spline);
380  window.Render();
381
382  std::cout << std::endl << "=== Spline intersects line ===" << std::endl;
383
384  window.Clear();
385  window.SetColor(0);
386  line.Translate(tVector(0, 0.1));
387  DrawLine(window, line);
388  window.SetColor(1);
389  DrawSplineCurve(window, spline);
390
391  window.Render();
392
393  intersection_points.clear();
394  intersection_parameters.clear();
395  spline.GetIntersections(intersection_points, intersection_parameters, line);
396  std::cout << "number of intersections: " << intersection_points.size() << std::endl;
397
398  window.SetColor(2);
399  for (std::vector<tPoint>::iterator it = intersection_points.begin(); it != intersection_points.end(); ++it)
400  {
401    std::cout << *it << std::endl;
402    DrawPoint(window, *it);
403  }
404  window.Render();
405
406  std::cout << std::endl << "=== Closest point to Bezier curve ===" << std::endl;
407
408  window.Clear();
409  window.SetColor(0);
410  DrawBezierCurve(window, bezier_curve);
411  window.Render();
412
413  for (int i = 0; i < 10; ++i)
414  {
415    tPoint reference_point(drand48(), drand48());
416    tPoint closest_point(bezier_curve.GetClosestPoint(reference_point));
417
418    window.SetColor(3);
419    DrawLineSegment(window, tLineSegment(reference_point, closest_point));
420    window.SetColor(1);
421    DrawPoint(window, reference_point);
422    window.SetColor(2);
423    DrawPoint(window, closest_point);
424    window.Render();
425  }
426
427  std::cout << std::endl << "=== Closest point to spline curve ===" << std::endl;
428
429  window.Clear();
430  window.SetColor(0);
431  DrawSplineCurve(window, spline);
432  window.Render();
433
434  for (int i = 0; i < 10; ++i)
435  {
436    tPoint reference_point(drand48(), drand48());
437    tPoint closest_point(spline.GetClosestPoint(reference_point));
438
439    window.SetColor(3);
440    DrawLineSegment(window, tLineSegment(reference_point, closest_point));
441    window.SetColor(1);
442    DrawPoint(window, reference_point);
443    window.SetColor(2);
444    DrawPoint(window, closest_point);
445    window.Render();
446  }
447
448  window.Render();
449
450  tWindow::ReleaseAllInstances();
451
452  return EXIT_SUCCESS;
453}
Note: See TracBrowser for help on using the repository browser.