source: rrlib_geometry/test/test_geometries.cpp @ 14:83a174d09c05

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

Updated and added old/missing copyright notes

File size: 12.3 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
287  tConcreteSplineCurve spline(control_points);
288
289  window.Clear();
290  window.SetColor(1);
291  DrawControlPolygon(window, spline);
292  window.SetColor(2);
293  DrawBoundingBox(window, spline);
294  window.SetColor(3);
295  DrawSplineCurve(window, spline);
296  window.Render();
297
298  std::cout << std::endl << "=== Adding point ===" << std::endl;
299
300  spline.AppendControlPoint(tPoint(0.75, 0.7));
301  window.Clear();
302  window.SetColor(1);
303  DrawControlPolygon(window, spline);
304  window.SetColor(2);
305  DrawBoundingBox(window, spline);
306  window.SetColor(3);
307  DrawSplineCurve(window, spline);
308  window.Render();
309
310  std::cout << std::endl << "=== Adding point ===" << std::endl;
311
312  spline.AppendControlPoint(tPoint(0.5, 0.8));
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.2, 0.4));
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.2, 0.4));
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 << "=== Spline intersects line ===" << std::endl;
359
360  window.Clear();
361  window.SetColor(0);
362  line.Translate(tVector(0, 0.1));
363  DrawLine(window, line);
364  window.SetColor(1);
365  DrawSplineCurve(window, spline);
366
367  window.Render();
368
369  intersection_points.clear();
370  intersection_parameters.clear();
371  spline.GetIntersections(intersection_points, intersection_parameters, line);
372  std::cout << "number of intersections: " << intersection_points.size() << std::endl;
373
374  window.SetColor(2);
375  for (std::vector<tPoint>::iterator it = intersection_points.begin(); it != intersection_points.end(); ++it)
376  {
377    std::cout << *it << std::endl;
378    DrawPoint(window, *it);
379  }
380  window.Render();
381
382
383
384  window.Render();
385
386  tWindow::ReleaseAllInstances();
387
388  return EXIT_SUCCESS;
389}
Note: See TracBrowser for help on using the repository browser.