source: rrlib_geometry/test/test_geometries.cpp @ 17:3daa58ced492

Last change on this file since 17:3daa58ced492 was 17:3daa58ced492, checked in by Tobias Föhst <foehst@…>, 9 years ago

Merged with changes from rrlab

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