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