1 // Spline.cc - a cubic spline interpolator.
2 //
3 // Vamos Automotive Simulator
4 // Copyright (C) 2001--2004 Sam Varner
5 //
6 // This program is free software; you can redistribute it and/or modify
7 // it under the terms of the GNU General Public License as published by
8 // the Free Software Foundation; either version 2 of the License, or
9 // (at your option) any later version.
10 //
11 // This program is distributed in the hope that it will be useful,
12 // but WITHOUT ANY WARRANTY; without even the implied warranty of
13 // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
14 // GNU General Public License for more details.
15 //
16 // You should have received a copy of the GNU General Public License
17 // along with this program; if not, write to the Free Software
18 // Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
19
20 #include "Spline.h"
21 #include "Numeric.h"
22
23 #include <cmath>
24 #include <cassert>
25
26 using namespace Vamos_Geometry;
27
Spline()28 Spline::Spline ()
29 : m_first_slope_is_set (false),
30 m_last_slope_is_set (false),
31 m_calculated (false),
32 m_slope (0.0),
33 m_periodic (false)
34 {
35 }
36
37 // Construct an empty curve.
Spline(double first_slope,double last_slope)38 Spline::Spline (double first_slope, double last_slope)
39 : m_first_slope (first_slope),
40 m_first_slope_is_set (true),
41 m_last_slope (last_slope),
42 m_last_slope_is_set (true),
43 m_calculated (false),
44 m_slope (0.0),
45 m_periodic (false)
46 {
47 }
48
49 // Construct a cuvre from an array of points.
Spline(const std::vector<Two_Vector> & points)50 Spline::Spline (const std::vector <Two_Vector>& points)
51 : Interpolator (points),
52 m_first_slope_is_set (false),
53 m_last_slope_is_set (false),
54 m_calculated (false),
55 m_slope (0.0),
56 m_periodic (false)
57 {
58 }
59
60 // Construct a cuvre from an array of points.
Spline(const std::vector<Two_Vector> & points,double first_slope,double last_slope)61 Spline::Spline (const std::vector <Two_Vector>& points,
62 double first_slope, double last_slope)
63 : Interpolator (points),
64 m_first_slope (first_slope),
65 m_first_slope_is_set (true),
66 m_last_slope (last_slope),
67 m_last_slope_is_set (true),
68 m_calculated (false),
69 m_slope (0.0),
70 m_periodic (false)
71 {
72 }
73
74 // Add a point to the curve.
75 void
load(const Two_Vector & point)76 Spline::load (const Two_Vector& point)
77 {
78 m_points.push_back (point);
79 m_calculated = false;
80 }
81
82 // Add multiple points to the curve.
83 void
load(const std::vector<Two_Vector> & points)84 Spline::load (const std::vector <Two_Vector>& points)
85 {
86 for (std::vector <Two_Vector>::const_iterator it = points.begin ();
87 it != points.end ();
88 it++)
89 {
90 m_points.push_back (*it);
91 }
92 m_calculated = false;
93 }
94
95 // Remove all points from the curve.
96 void
clear()97 Spline::clear ()
98 {
99 m_points.clear ();
100 clear_cache ();
101 m_calculated = false;
102 }
103
104 void
set_periodic(double end)105 Spline::set_periodic (double end)
106 {
107 load (end, (m_points.size () == 0) ? 0.0 : m_points [0].y);
108 m_periodic = true;
109 }
110
111 // Remove points with x > LIMIT.
112 void
remove_greater(double limit)113 Spline::remove_greater (double limit)
114 {
115 clear_cache ();
116 for (size_t size = 0; size < m_points.size (); size++)
117 {
118 if (m_points [size].x > limit)
119 {
120 m_points.resize (size);
121 m_calculated = false;
122 return;
123 }
124 }
125 }
126
127 // Scale all of the x values by FACTOR.
128 void
scale(double factor)129 Spline::scale (double factor)
130 {
131 for (std::vector <Two_Vector>::iterator it = m_points.begin ();
132 it != m_points.end ();
133 it++)
134 {
135 it->x *= factor;
136 }
137
138 m_calculated = false;
139 }
140
141 // calculate() and interpolate() follow the discussion on cubic
142 // splines found in Numerical Recipes. The implementation here is
143 // original.
144
145 // Return the y value at the x value DISTANCE
146 double
interpolate(double distance) const147 Spline::interpolate (double distance) const
148 {
149 Interpolator::interpolate (distance);
150
151 const size_t n = m_points.size ();
152
153 if ((n < 2) || ((n == 2) && m_periodic))
154 {
155 m_slope = (m_periodic || !m_first_slope_is_set) ? 0.0 : m_first_slope;
156 m_second_derivative = 0.0;
157 Two_Vector p0 = (m_points.empty ()) ? Two_Vector (0.0, 0.0) : m_points [0];
158 return p0.y + m_slope * (distance - p0.x);
159 }
160 if ((n == 2) && (!m_first_slope_is_set || !m_last_slope_is_set))
161 {
162 // Fall back to linear interpolation.
163 m_slope = (m_points [1].y - m_points [0].y)
164 / (m_points [1].x - m_points [0].x);
165 return Vamos_Geometry::interpolate (distance,
166 m_points [0].x, m_points [0].y,
167 m_points [1].x, m_points [1].y);
168 }
169
170 if (m_periodic)
171 distance = wrap (distance, m_points [0].x, m_points [n-1].x);
172
173 // calculate() only needs to be called once for a given set of
174 // points.
175 if (!m_calculated)
176 calculate ();
177
178 const size_t low = low_index (distance);
179 const size_t high = low + 1;
180 const double diff = m_points [high].x - m_points [low].x;
181
182 // Evaluate the coefficients for the cubic spline equation.
183 const double a = (m_points [high].x - distance) / diff;
184 const double b = 1.0 - a;
185 const double sq = diff*diff / 6.0;
186 const double a2 = a*a;
187 const double b2 = b*b;
188
189 // Find the first derivative.
190 m_slope =
191 (m_points [high].y - m_points [low].y)/diff
192 - (3.0 * a2 - 1.0) / 6.0 * diff * m_second_deriv [low]
193 + (3.0 * b2 - 1.0) / 6.0 * diff * m_second_deriv [high];
194
195 m_second_derivative =
196 Vamos_Geometry::interpolate (distance,
197 m_points [low].x, m_second_deriv [low],
198 m_points [high].x, m_second_deriv [high]);
199
200 // Return the interpolated value.
201 return a * m_points [low].y
202 + b * m_points [high].y
203 + a * (a2 - 1.0) * sq * m_second_deriv [low]
204 + b * (b2 - 1.0) * sq * m_second_deriv [high];
205 }
206
207 double
slope(double distance) const208 Spline::slope (double distance) const
209 {
210 // The slope is calculated and stored when interpolate() is called.
211 interpolate (distance);
212 return m_slope;
213 }
214
215 double
second_derivative(double distance) const216 Spline::second_derivative (double distance) const
217 {
218 // The slope is calculated and stored when interpolate() is called.
219 interpolate (distance);
220 return m_second_derivative;
221 }
222
223 void
solve_symmetric_tridiagonal(const double * a,const double * b_in,const double * r_in,double * x,size_t n)224 solve_symmetric_tridiagonal (const double* a,
225 const double* b_in,
226 const double* r_in,
227 double* x,
228 size_t n)
229 {
230 double* b = new double [n];
231 double* r = new double [n];
232
233 b [0] = b_in [0];
234 r [0] = r_in [0];
235
236 // Gauss-Jordan Elimination
237 for (size_t i = 1; i < n; i++)
238 {
239 // Replace row i with row i - k * row (i-1) such that A_{i,i-1} = 0.0.
240 double factor = a [i-1] / b [i-1];
241 // A_{i,i-1} is not used again, so it need not be calculated.
242 b [i] = b_in [i] - factor * a [i-1];
243 // A_{i,i+1} is unchanged because A_{i-1,i+1} = 0.0.
244 r [i] = r_in [i] - factor * r [i-1];
245 }
246
247 // Back-Substitution
248 x [n-1] = r [n-1] / b [n-1];
249 for (int i = n - 2; i >= 0; i--)
250 {
251 // Use the solution for x[i+1] to find x[i].
252 x [i] = (r [i] - a [i] * x [i+1]) / b [i];
253 }
254
255 delete [] r;
256 delete [] b;
257 }
258
259 // Calculate the coefficients for interpolation.
260 void
calculate() const261 Spline::calculate () const
262 {
263 m_calculated = true;
264
265 size_t n = m_points.size ();
266 if (n < 2)
267 return;
268
269 m_second_deriv.resize (n);
270
271 if ((n == 2) && m_first_slope_is_set && m_last_slope_is_set)
272 {
273 Two_Vector delta = m_points [1] - m_points [0];
274 double m3 = 3.0 * delta.y / delta.x;
275 double a = delta.x / 2.0;
276 m_second_deriv [0] = -(2.0 * m_first_slope + m_last_slope - m3) / a;
277 m_second_deriv [1] = (m_first_slope + 2.0 * m_last_slope - m3) / a;
278 return;
279 }
280
281 double* a = new double [n-1];
282 double* b = new double [n-1];
283 double* r = new double [n-1];
284 double* x = new double [n-1];
285
286 for (size_t i = 0; i < n - 2; i++)
287 {
288 double diff_low = m_points [i+1].x - m_points [i].x;
289 double diff_high = m_points [i+2].x - m_points [i+1].x;
290
291 a [i] = diff_high / 6.0;
292 b [i] = (diff_low + diff_high) / 3.0;
293 r [i] = (m_points [i+2].y - m_points [i+1].y) / diff_high
294 - (m_points [i+1].y - m_points [i].y) / diff_low;
295 }
296
297 if (m_periodic)
298 {
299 double diff_low = m_points [n-1].x - m_points [n-2].x;
300 double diff_high = m_points [1].x - m_points [0].x;
301
302 a [n-2] = diff_high / 6.0;
303 b [n-2] = (diff_low + diff_high) / 3.0;
304 r [n-2] = (m_points [1].y - m_points [0].y) / diff_high
305 - (m_points [n-1].y - m_points [n-2].y) / diff_low;
306
307 const double alpha = a [n-2];
308 const double gamma = -b [0];
309 b [0] -= gamma;
310 b [n-2] -= alpha * alpha / gamma;
311
312 solve_symmetric_tridiagonal (a, b, r, x, n-1);
313
314 double* u = new double [n-1];
315 u [0] = gamma;
316 for (size_t i = 1; i < n-2; i++)
317 u [i] = 0.0;
318 u [n-2] = alpha;
319
320 double* z = new double [n-1];
321 solve_symmetric_tridiagonal (a, b, u, z, n-1);
322
323 const double factor = (x [0] + x [n-2] * alpha / gamma)
324 / (1.0 + z [0] + z [n-2] * alpha / gamma);
325
326 for (size_t i = 1; i < n; i++)
327 m_second_deriv [i] = x [i-1] - factor * z [i-1];
328 m_second_deriv [0] = m_second_deriv [n-1];
329
330 delete [] z;
331 delete [] u;
332 }
333 else
334 {
335 solve_symmetric_tridiagonal (a, b, r, x, n - 2);
336
337 for (size_t i = 1; i < n-1; i++)
338 m_second_deriv [i] = x [i-1];
339 m_second_deriv [0] = 0.0;
340 m_second_deriv [n-1] = 0.0;
341
342 if (m_first_slope_is_set)
343 {
344 const double dy = m_points [1].y - m_points [0].y;
345 const double dx = m_points [1].x - m_points [0].x;
346 m_second_deriv [0] = 3.0/dx
347 * (dy/dx - m_first_slope - dx/6.0 * m_second_deriv [1]);
348 }
349 if (m_last_slope_is_set)
350 {
351 const double dy = m_points [n-1].y - m_points [n-2].y;
352 const double dx = m_points [n-1].x - m_points [n-2].x;
353 m_second_deriv [n-1] = -3.0/dx
354 * (dy/dx - m_last_slope + dx/6.0 * m_second_deriv [n-2]);
355 }
356 }
357
358 delete [] x;
359 delete [] r;
360 delete [] b;
361 delete [] a;
362 }
363
364 // Return the normal to the tanget at DISTANCE.
365 Two_Vector
normal(double distance) const366 Spline::normal (double distance) const
367 {
368 interpolate (distance);
369 double theta = std::atan (m_slope);
370 return Two_Vector (-std::sin (theta), std::cos (theta));
371 }
372
373 // Add 'delta' to all points.
374 void
shift(double delta)375 Spline::shift (double delta)
376 {
377 for (std::vector <Two_Vector>::iterator it = m_points.begin ();
378 it != m_points.end ();
379 it++)
380 {
381 it->y += delta;
382 }
383 }
384
385 //-----------------------------------------------------------------------------
Parametric_Spline()386 Parametric_Spline::Parametric_Spline ()
387 {
388 }
389
Parametric_Spline(double first_x_slope,double last_x_slope,double first_y_slope,double last_y_slope)390 Parametric_Spline::Parametric_Spline (double first_x_slope, double last_x_slope,
391 double first_y_slope, double last_y_slope)
392 : m_x (first_x_slope, last_x_slope),
393 m_y (first_y_slope, last_y_slope)
394 {
395 }
396
397 void
load(double parameter,const Two_Vector & point)398 Parametric_Spline::load (double parameter, const Two_Vector& point)
399 {
400 m_x.load (Two_Vector (parameter, point.x));
401 m_y.load (Two_Vector (parameter, point.y));
402 }
403
404 void
clear()405 Parametric_Spline::clear ()
406 {
407 m_x.clear ();
408 m_y.clear ();
409 }
410
411 void
set_periodic(double end)412 Parametric_Spline::set_periodic (double end)
413 {
414 m_x.set_periodic (end);
415 m_y.set_periodic (end);
416 }
417
418 Two_Vector
interpolate(double parameter) const419 Parametric_Spline::interpolate (double parameter) const
420 {
421 return Two_Vector (m_x.interpolate (parameter),
422 m_y.interpolate (parameter));
423 }
424
425 size_t
size() const426 Parametric_Spline::size () const
427 {
428 assert (m_x.size () == m_y.size ());
429 return m_x.size ();
430 }
431
432 Two_Vector
operator [](size_t i) const433 Parametric_Spline::operator [] (size_t i) const
434 {
435 return Two_Vector (m_x [i].y, m_y [i].y);
436 }
437
438 double
parameter(size_t i) const439 Parametric_Spline::parameter (size_t i) const
440 {
441 return m_x [i].x;
442 }
443
444 //-----------------------------------------------------------------------------
Vector_Spline()445 Vector_Spline::Vector_Spline ()
446 {
447 }
448
Vector_Spline(double first_x_slope,double last_x_slope,double first_y_slope,double last_y_slope,double first_z_slope,double last_z_slope)449 Vector_Spline::Vector_Spline (double first_x_slope, double last_x_slope,
450 double first_y_slope, double last_y_slope,
451 double first_z_slope, double last_z_slope)
452 : m_x (first_x_slope, last_x_slope),
453 m_y (first_y_slope, last_y_slope),
454 m_z (first_z_slope, last_z_slope)
455 {
456 }
457
458 void
load(double parameter,const Three_Vector & point)459 Vector_Spline::load (double parameter, const Three_Vector& point)
460 {
461 m_x.load (Two_Vector (parameter, point.x));
462 m_y.load (Two_Vector (parameter, point.y));
463 m_z.load (Two_Vector (parameter, point.z));
464 }
465
466 void
clear()467 Vector_Spline::clear ()
468 {
469 m_x.clear ();
470 m_y.clear ();
471 m_z.clear ();
472 }
473
474 void
set_periodic(double end)475 Vector_Spline::set_periodic (double end)
476 {
477 m_x.set_periodic (end);
478 m_y.set_periodic (end);
479 m_z.set_periodic (end);
480 }
481
482 Three_Vector
interpolate(double parameter) const483 Vector_Spline::interpolate (double parameter) const
484 {
485 return Three_Vector (m_x.interpolate (parameter),
486 m_y.interpolate (parameter),
487 m_z.interpolate (parameter));
488 }
489
490 size_t
size() const491 Vector_Spline::size () const
492 {
493 assert (m_x.size () == m_y.size ());
494 assert (m_x.size () == m_z.size ());
495 return m_x.size ();
496 }
497
498 Three_Vector
operator [](size_t i) const499 Vector_Spline::operator [] (size_t i) const
500 {
501 return Three_Vector (m_x [i].y, m_y [i].y, m_z [i].y);
502 }
503
504 double
parameter(size_t i) const505 Vector_Spline::parameter (size_t i) const
506 {
507 return m_x [i].x;
508 }
509