SUMO - Simulation of Urban MObility
 All Data Structures Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Macros Modules Pages
bezier.cpp
Go to the documentation of this file.
1 /****************************************************************************/
8 // missing_desc
9 /****************************************************************************/
10 // SUMO, Simulation of Urban MObility; see http://sumo.dlr.de/
11 // Copyright (C) 2003-2016 DLR (http://www.dlr.de/) and contributors
12 /****************************************************************************/
13 //
14 // This file is part of SUMO.
15 // SUMO is free software: you can redistribute it and/or modify
16 // it under the terms of the GNU General Public License as published by
17 // the Free Software Foundation, either version 3 of the License, or
18 // (at your option) any later version.
19 //
20 /****************************************************************************/
21 
22 
23 /* Subroutine to generate a Bezier curve.
24  Copyright (c) 2000 David F. Rogers. All rights reserved.
25 
26  b[] = array containing the defining polygon vertices
27  b[1] contains the x-component of the vertex
28  b[2] contains the y-component of the vertex
29  b[3] contains the z-component of the vertex
30  Basis = function to calculate the Bernstein basis value (see MECG Eq 5-65)
31  cpts = number of points to be calculated on the curve
32  Fractrl = function to calculate the factorial of a number
33  j[] = array containing the basis functions for a single value of t
34  npts = number of defining polygon vertices
35  p[] = array containing the curve points
36  p[1] contains the x-component of the point
37  p[2] contains the y-component of the point
38  p[3] contains the z-component of the point
39  t = parameter value 0 <= t <= 1
40 */
41 
42 // ===========================================================================
43 // included modules
44 // ===========================================================================
45 #ifdef _MSC_VER
46 #include <windows_config.h>
47 #else
48 #include <config.h>
49 #endif
50 
51 #include <math.h>
52 #include <iostream>
53 #include <utils/common/StdDefs.h>
54 #include "PositionVector.h"
55 
56 #ifdef CHECK_MEMORY_LEAKS
57 #include <foreign/nvwa/debug_new.h>
58 #endif // CHECK_MEMORY_LEAKS
59 
60 /* function to calculate the factorial */
61 
62 SUMOReal factrl(int n) {
63  static int ntop = 6;
64  static SUMOReal a[33] = {
65  1.0, 1.0, 2.0, 6.0, 24.0, 120.0, 720.0
66  }
67  ; /* fill in the first few values */
68  int j1;
69 
70  if (n < 0) {
71  throw 1;
72  } //cout << "\nNegative factorial in routine FACTRL\n";
73  if (n > 32) {
74  throw 1;
75  } //cout << "\nFactorial value too large in routine FACTRL\n";
76 
77  while (ntop < n) { /* use the precalulated value for n = 0....6 */
78  j1 = ntop++;
79  a[ntop] = a[j1] * ntop;
80  }
81  return a[n]; /* returns the value n! as a SUMOReal */
82 }
83 
84 /* function to calculate the factorial function for Bernstein basis */
85 
86 SUMOReal Ni(int n, int i) {
87  return factrl(n) / (factrl(i) * factrl(n - i));
88 }
89 
90 /* function to calculate the Bernstein basis */
91 
92 SUMOReal Basis(int n, int i, SUMOReal t) {
93  /* handle the special cases to avoid domain problem with pow */
94  const SUMOReal ti = (i == 0) ? 1.0 : pow(t, i); /* this is t^i */
95  const SUMOReal tni = (n == i) ? 1.0 : pow(1 - t, n - i); /* this is (1-t)^(n-i) */
96  return Ni(n, i) * ti * tni;
97 }
98 
99 /* Bezier curve subroutine */
100 void
101 bezier(int npts, SUMOReal b[], int cpts, SUMOReal p[]) {
102  int i;
103  int j;
104  int i1;
105  int icount;
106  int jcount;
107 
108  const SUMOReal step = (SUMOReal) 1.0 / (cpts - 1);
109  SUMOReal t;
110 
111  /* calculate the points on the Bezier curve */
112 
113  icount = 0;
114  t = 0;
115 
116  for (i1 = 1; i1 <= cpts; i1++) { /* main loop */
117 
118  if ((1.0 - t) < 5e-6) {
119  t = 1.0;
120  }
121 
122  for (j = 1; j <= 3; j++) { /* generate a point on the curve */
123  jcount = j;
124  p[icount + j] = 0.;
125  for (i = 1; i <= npts; i++) { /* Do x,y,z components */
126  p[icount + j] = p[icount + j] + Basis(npts - 1, i - 1, t) * b[jcount];
127  jcount = jcount + 3;
128  }
129  }
130 
131  icount = icount + 3;
132  t = t + step;
133  }
134 }
135 
136 
138 bezier(const PositionVector& init, int numPoints) {
139  PositionVector ret;
140  SUMOReal* def = new SUMOReal[1 + (int)init.size() * 3];
141  for (int i = 0; i < (int)init.size(); ++i) {
142  // starts at index 1
143  def[i * 3 + 1] = init[i].x();
144  def[i * 3 + 2] = 0;
145  def[i * 3 + 3] = init[i].y();
146  }
147  SUMOReal* ret_buf = new SUMOReal[numPoints * 3 + 1];
148  bezier((int)init.size(), def, numPoints, ret_buf);
149  delete[] def;
150  Position prev;
151  for (int i = 0; i < (int)numPoints; i++) {
152  Position current(ret_buf[i * 3 + 1], ret_buf[i * 3 + 3], init[0].z());
153  if (prev != current && !ISNAN(current.x()) && !ISNAN(current.y())) {
154  ret.push_back(current);
155  }
156  prev = current;
157  }
158  delete[] ret_buf;
159  return ret;
160 }
161 
162 /****************************************************************************/
163 
SUMOReal factrl(int n)
Definition: bezier.cpp:62
SUMOReal x() const
Returns the x-position.
Definition: Position.h:63
A point in 2D or 3D with translation and scaling methods.
Definition: Position.h:46
A list of positions.
void bezier(int npts, SUMOReal b[], int cpts, SUMOReal p[])
Definition: bezier.cpp:101
T ISNAN(T a)
Definition: StdDefs.h:110
SUMOReal Basis(int n, int i, SUMOReal t)
Definition: bezier.cpp:92
SUMOReal Ni(int n, int i)
Definition: bezier.cpp:86
SUMOReal y() const
Returns the y-position.
Definition: Position.h:68
#define SUMOReal
Definition: config.h:214