| Geant4 Cross Reference |
1 // Copyright (C) 2010, Guy Barrand. All rights 1 // Copyright (C) 2010, Guy Barrand. All rights reserved.
2 // See the file tools.license for terms. 2 // See the file tools.license for terms.
3 3
4 #ifndef tools_spline 4 #ifndef tools_spline
5 #define tools_spline 5 #define tools_spline
6 6
7 // From Federico Carminati code found in root- 7 // From Federico Carminati code found in root-6.08.06/TSpline.h, TSpline.cxx.
8 8
9 #include "mnmx" 9 #include "mnmx"
10 #include <cstddef> 10 #include <cstddef>
11 #include <vector> 11 #include <vector>
12 #include <ostream> 12 #include <ostream>
13 #include <cmath> 13 #include <cmath>
14 14
15 namespace tools { 15 namespace tools {
16 namespace spline { 16 namespace spline {
17 17
18 class base_poly { 18 class base_poly {
19 public: 19 public:
20 base_poly():fX(0),fY(0) {} 20 base_poly():fX(0),fY(0) {}
21 base_poly(double x,double y):fX(x),fY(y) {} 21 base_poly(double x,double y):fX(x),fY(y) {}
22 virtual ~base_poly(){} 22 virtual ~base_poly(){}
23 public: 23 public:
24 base_poly(base_poly const &a_from):fX(a_from 24 base_poly(base_poly const &a_from):fX(a_from.fX),fY(a_from.fY) {}
25 base_poly& operator=(base_poly const &a_from 25 base_poly& operator=(base_poly const &a_from) {
26 if(this==&a_from) return *this; 26 if(this==&a_from) return *this;
27 fX = a_from.fX; 27 fX = a_from.fX;
28 fY = a_from.fY; 28 fY = a_from.fY;
29 return *this; 29 return *this;
30 } 30 }
31 public: 31 public:
32 const double& X() const {return fX;} 32 const double& X() const {return fX;}
33 const double& Y() const {return fY;} 33 const double& Y() const {return fY;}
34 double &X() {return fX;} 34 double &X() {return fX;}
35 double &Y() {return fY;} 35 double &Y() {return fY;}
36 protected: 36 protected:
37 double fX; // abscissa 37 double fX; // abscissa
38 double fY; // constant term 38 double fY; // constant term
39 }; 39 };
40 40
41 class cubic_poly : public base_poly { 41 class cubic_poly : public base_poly {
42 public: 42 public:
43 cubic_poly():fB(0), fC(0), fD(0) {} 43 cubic_poly():fB(0), fC(0), fD(0) {}
44 cubic_poly(double x, double y, double b, dou 44 cubic_poly(double x, double y, double b, double c, double d):base_poly(x,y), fB(b), fC(c), fD(d) {}
45 public: 45 public:
46 cubic_poly(cubic_poly const &a_from) 46 cubic_poly(cubic_poly const &a_from)
47 :base_poly(a_from), fB(a_from.fB), fC(a_from 47 :base_poly(a_from), fB(a_from.fB), fC(a_from.fC), fD(a_from.fD) {}
48 cubic_poly& operator=(cubic_poly const &a_fr 48 cubic_poly& operator=(cubic_poly const &a_from) {
49 if(this==&a_from) return *this; 49 if(this==&a_from) return *this;
50 base_poly::operator=(a_from); 50 base_poly::operator=(a_from);
51 fB = a_from.fB; 51 fB = a_from.fB;
52 fC = a_from.fC; 52 fC = a_from.fC;
53 fD = a_from.fD; 53 fD = a_from.fD;
54 return *this; 54 return *this;
55 } 55 }
56 public: 56 public:
57 double &B() {return fB;} 57 double &B() {return fB;}
58 double &C() {return fC;} 58 double &C() {return fC;}
59 double &D() {return fD;} 59 double &D() {return fD;}
60 double eval(double x) const {double dx=x-fX; 60 double eval(double x) const {double dx=x-fX;return (fY+dx*(fB+dx*(fC+dx*fD)));}
61 protected: 61 protected:
62 double fB; // first order expansion coeffici 62 double fB; // first order expansion coefficient : fB*1! is the first derivative at x
63 double fC; // second order expansion coeffic 63 double fC; // second order expansion coefficient : fC*2! is the second derivative at x
64 double fD; // third order expansion coeffici 64 double fD; // third order expansion coefficient : fD*3! is the third derivative at x
65 }; 65 };
66 66
67 class quintic_poly : public base_poly { 67 class quintic_poly : public base_poly {
68 public: 68 public:
69 quintic_poly():fB(0), fC(0), fD(0), fE(0), f 69 quintic_poly():fB(0), fC(0), fD(0), fE(0), fF(0) {}
70 quintic_poly(double x, double y, double b, d 70 quintic_poly(double x, double y, double b, double c, double d, double e, double f)
71 :base_poly(x,y), fB(b), fC(c), fD(d), fE(e), 71 :base_poly(x,y), fB(b), fC(c), fD(d), fE(e), fF(f) {}
72 public: 72 public:
73 quintic_poly(quintic_poly const &a_from) 73 quintic_poly(quintic_poly const &a_from)
74 :base_poly(a_from) 74 :base_poly(a_from)
75 ,fB(a_from.fB),fC(a_from.fC),fD(a_from.fD),f 75 ,fB(a_from.fB),fC(a_from.fC),fD(a_from.fD),fE(a_from.fE),fF(a_from.fF) {}
76 quintic_poly& operator=(quintic_poly const & 76 quintic_poly& operator=(quintic_poly const &a_from) {
77 if(this==&a_from) return *this; 77 if(this==&a_from) return *this;
78 base_poly::operator=(a_from); 78 base_poly::operator=(a_from);
79 fB = a_from.fB; 79 fB = a_from.fB;
80 fC = a_from.fC; 80 fC = a_from.fC;
81 fD = a_from.fD; 81 fD = a_from.fD;
82 fE = a_from.fE; 82 fE = a_from.fE;
83 fF = a_from.fF; 83 fF = a_from.fF;
84 return *this; 84 return *this;
85 } 85 }
86 public: 86 public:
87 double &B() {return fB;} 87 double &B() {return fB;}
88 double &C() {return fC;} 88 double &C() {return fC;}
89 double &D() {return fD;} 89 double &D() {return fD;}
90 double &E() {return fE;} 90 double &E() {return fE;}
91 double &F() {return fF;} 91 double &F() {return fF;}
92 double eval(double x) const {double dx=x-fX; 92 double eval(double x) const {double dx=x-fX;return (fY+dx*(fB+dx*(fC+dx*(fD+dx*(fE+dx*fF)))));}
93 protected: 93 protected:
94 double fB; // first order expansion coeffici 94 double fB; // first order expansion coefficient : fB*1! is the first derivative at x
95 double fC; // second order expansion coeffic 95 double fC; // second order expansion coefficient : fC*2! is the second derivative at x
96 double fD; // third order expansion coeffici 96 double fD; // third order expansion coefficient : fD*3! is the third derivative at x
97 double fE; // fourth order expansion coeffic 97 double fE; // fourth order expansion coefficient : fE*4! is the fourth derivative at x
98 double fF; // fifth order expansion coeffici 98 double fF; // fifth order expansion coefficient : fF*5! is the fifth derivative at x
99 }; 99 };
100 100
101 ////////////////////////////////////////////// 101 ////////////////////////////////////////////////////////////////////////////////////
102 ////////////////////////////////////////////// 102 ////////////////////////////////////////////////////////////////////////////////////
103 ////////////////////////////////////////////// 103 ////////////////////////////////////////////////////////////////////////////////////
104 class base_spline { 104 class base_spline {
105 protected: 105 protected:
106 base_spline(std::ostream& a_out):m_out(a_out 106 base_spline(std::ostream& a_out):m_out(a_out), fDelta(-1), fXmin(0), fXmax(0), fNp(0), fKstep(false) {}
107 public: 107 public:
108 base_spline(std::ostream& a_out,double delta 108 base_spline(std::ostream& a_out,double delta, double xmin, double xmax, size_t np, bool step)
109 :m_out(a_out),fDelta(delta), fXmin(xmin),fXm 109 :m_out(a_out),fDelta(delta), fXmin(xmin),fXmax(xmax), fNp(np), fKstep(step)
110 {} 110 {}
111 virtual ~base_spline() {} 111 virtual ~base_spline() {}
112 protected: 112 protected:
113 base_spline(const base_spline& a_from) 113 base_spline(const base_spline& a_from)
114 :m_out(a_from.m_out) 114 :m_out(a_from.m_out)
115 ,fDelta(a_from.fDelta),fXmin(a_from.fXmin),f 115 ,fDelta(a_from.fDelta),fXmin(a_from.fXmin),fXmax(a_from.fXmax),fNp(a_from.fNp),fKstep(a_from.fKstep) {}
116 base_spline& operator=(const base_spline& a_ 116 base_spline& operator=(const base_spline& a_from) {
117 if(this==&a_from) return *this; 117 if(this==&a_from) return *this;
118 fDelta=a_from.fDelta; 118 fDelta=a_from.fDelta;
119 fXmin=a_from.fXmin; 119 fXmin=a_from.fXmin;
120 fXmax=a_from.fXmax; 120 fXmax=a_from.fXmax;
121 fNp=a_from.fNp; 121 fNp=a_from.fNp;
122 fKstep=a_from.fKstep; 122 fKstep=a_from.fKstep;
123 return *this; 123 return *this;
124 } 124 }
125 protected: 125 protected:
126 std::ostream& m_out; 126 std::ostream& m_out;
127 double fDelta; // Distance between equi 127 double fDelta; // Distance between equidistant knots
128 double fXmin; // Minimum value of absc 128 double fXmin; // Minimum value of abscissa
129 double fXmax; // Maximum value of absc 129 double fXmax; // Maximum value of abscissa
130 size_t fNp; // Number of knots 130 size_t fNp; // Number of knots
131 bool fKstep; // True of equidistant k 131 bool fKstep; // True of equidistant knots
132 }; 132 };
133 133
134 134
135 ////////////////////////////////////////////// 135 //////////////////////////////////////////////////////////////////////////
136 // 136 // //
137 // cubic 137 // cubic //
138 // 138 // //
139 // Class to create third splines to interpolat 139 // Class to create third splines to interpolate knots //
140 // Arbitrary conditions can be introduced for 140 // Arbitrary conditions can be introduced for first and second //
141 // derivatives at beginning and ending points 141 // derivatives at beginning and ending points //
142 // 142 // //
143 ////////////////////////////////////////////// 143 //////////////////////////////////////////////////////////////////////////
144 144
145 class cubic : public base_spline { 145 class cubic : public base_spline {
146 protected: 146 protected:
147 cubic(std::ostream& a_out) : base_spline(a_o 147 cubic(std::ostream& a_out) : base_spline(a_out) , fPoly(0), fValBeg(0), fValEnd(0), fBegCond(-1), fEndCond(-1) {}
148 public: 148 public:
149 cubic(std::ostream& a_out,size_t a_n,double 149 cubic(std::ostream& a_out,size_t a_n,double a_x[], double a_y[], double a_valbeg = 0, double a_valend = 0)
150 :base_spline(a_out,-1,0,0,a_n,false) 150 :base_spline(a_out,-1,0,0,a_n,false)
151 ,fValBeg(a_valbeg), fValEnd(a_valend), fBegC 151 ,fValBeg(a_valbeg), fValEnd(a_valend), fBegCond(0), fEndCond(0)
152 { 152 {
153 if(!a_n) { 153 if(!a_n) {
154 m_out << "tools::spline::cubic : a_np is 154 m_out << "tools::spline::cubic : a_np is null." << std::endl;
155 return; 155 return;
156 } 156 }
157 fXmin = a_x[0]; 157 fXmin = a_x[0];
158 fXmax = a_x[a_n-1]; 158 fXmax = a_x[a_n-1];
159 fPoly.resize(a_n); 159 fPoly.resize(a_n);
160 for (size_t i=0; i<a_n; ++i) { 160 for (size_t i=0; i<a_n; ++i) {
161 fPoly[i].X() = a_x[i]; 161 fPoly[i].X() = a_x[i];
162 fPoly[i].Y() = a_y[i]; 162 fPoly[i].Y() = a_y[i];
163 } 163 }
164 build_coeff(); // Build the spline coeffic 164 build_coeff(); // Build the spline coefficients
165 } 165 }
166 public: 166 public:
167 cubic(const cubic& a_from) 167 cubic(const cubic& a_from)
168 :base_spline(a_from) 168 :base_spline(a_from)
169 ,fPoly(a_from.fPoly),fValBeg(a_from.fValBeg) 169 ,fPoly(a_from.fPoly),fValBeg(a_from.fValBeg),fValEnd(a_from.fValEnd),fBegCond(a_from.fBegCond),fEndCond(a_from.fEndCond)
170 {} 170 {}
171 cubic& operator=(const cubic& a_from) { 171 cubic& operator=(const cubic& a_from) {
172 if(this==&a_from) return *this; 172 if(this==&a_from) return *this;
173 base_spline::operator=(a_from); 173 base_spline::operator=(a_from);
174 fPoly = a_from.fPoly; 174 fPoly = a_from.fPoly;
175 fValBeg=a_from.fValBeg; 175 fValBeg=a_from.fValBeg;
176 fValEnd=a_from.fValEnd; 176 fValEnd=a_from.fValEnd;
177 fBegCond=a_from.fBegCond; 177 fBegCond=a_from.fBegCond;
178 fEndCond=a_from.fEndCond; 178 fEndCond=a_from.fEndCond;
179 return *this; 179 return *this;
180 } 180 }
181 public: 181 public:
182 double eval(double x) const { 182 double eval(double x) const {
183 if(!fNp) return 0; 183 if(!fNp) return 0;
184 // Eval this spline at x 184 // Eval this spline at x
185 size_t klow = find_x(x); 185 size_t klow = find_x(x);
186 if ( (fNp > 1) && (klow >= (fNp-1))) klow 186 if ( (fNp > 1) && (klow >= (fNp-1))) klow = fNp-2; //see: https://savannah.cern.ch/bugs/?71651
187 return fPoly[klow].eval(x); 187 return fPoly[klow].eval(x);
188 } 188 }
189 protected: 189 protected:
190 template<typename T> 190 template<typename T>
191 static int TMath_Nint(T x) { 191 static int TMath_Nint(T x) {
192 // Round to nearest integer. Rounds half i 192 // Round to nearest integer. Rounds half integers to the nearest even integer.
193 int i; 193 int i;
194 if (x >= 0) { 194 if (x >= 0) {
195 i = int(x + 0.5); 195 i = int(x + 0.5);
196 if ( i & 1 && x + 0.5 == T(i) ) i--; 196 if ( i & 1 && x + 0.5 == T(i) ) i--;
197 } else { 197 } else {
198 i = int(x - 0.5); 198 i = int(x - 0.5);
199 if ( i & 1 && x - 0.5 == T(i) ) i++; 199 if ( i & 1 && x - 0.5 == T(i) ) i++;
200 } 200 }
201 return i; 201 return i;
202 } 202 }
203 static int TMath_FloorNint(double x) { retur 203 static int TMath_FloorNint(double x) { return TMath_Nint(::floor(x)); }
204 204
205 size_t find_x(double x) const { 205 size_t find_x(double x) const {
206 int klow=0, khig=int(fNp-1); << 206 int klow=0, khig=fNp-1;
207 // 207 //
208 // If out of boundaries, extrapolate 208 // If out of boundaries, extrapolate
209 // It may be badly wrong 209 // It may be badly wrong
210 if(x<=fXmin) klow=0; 210 if(x<=fXmin) klow=0;
211 else if(x>=fXmax) klow=khig; 211 else if(x>=fXmax) klow=khig;
212 else { 212 else {
213 if(fKstep) { // Equidistant knots, use h 213 if(fKstep) { // Equidistant knots, use histogramming :
214 klow = TMath_FloorNint((x-fXmin)/fDel 214 klow = TMath_FloorNint((x-fXmin)/fDelta);
215 // Correction for rounding errors 215 // Correction for rounding errors
216 if (x < fPoly[klow].X()) 216 if (x < fPoly[klow].X())
217 klow = mx<int>(klow-1,0); 217 klow = mx<int>(klow-1,0);
218 else if (klow < khig) { 218 else if (klow < khig) {
219 if (x > fPoly[klow+1].X()) ++klow; 219 if (x > fPoly[klow+1].X()) ++klow;
220 } 220 }
221 } else { 221 } else {
222 int khalf; 222 int khalf;
223 // 223 //
224 // Non equidistant knots, binary sear 224 // Non equidistant knots, binary search
225 while((khig-klow)>1) { 225 while((khig-klow)>1) {
226 khalf = (klow+khig)/2; 226 khalf = (klow+khig)/2;
227 if(x>fPoly[khalf].X()) klow=khalf; 227 if(x>fPoly[khalf].X()) klow=khalf;
228 else khig=khalf; 228 else khig=khalf;
229 } 229 }
230 // 230 //
231 // This could be removed, sanity chec 231 // This could be removed, sanity check
232 if( (x<fPoly[klow].X()) || (fPoly[klo 232 if( (x<fPoly[klow].X()) || (fPoly[klow+1].X()<x) ) {
233 m_out << "tools::spline::cubic::fi 233 m_out << "tools::spline::cubic::find_x : Binary search failed"
234 << " x(" << klow << ") = " < 234 << " x(" << klow << ") = " << fPoly[klow].X() << " < x= " << x
235 << " < x(" << klow+1 << ") = 235 << " < x(" << klow+1 << ") = " << fPoly[klow+1].X() << "."
236 << "." << std::endl; 236 << "." << std::endl;
237 } 237 }
238 } 238 }
239 } 239 }
240 return klow; 240 return klow;
241 } 241 }
242 242
243 void build_coeff() { 243 void build_coeff() {
244 /// subroutine cubspl ( tau, c, n, ib 244 /// subroutine cubspl ( tau, c, n, ibcbeg, ibcend )
245 /// from * a practical guide to splines 245 /// from * a practical guide to splines * by c. de boor
246 /// ************************ input * 246 /// ************************ input ***************************
247 /// n = number of data points. assumed 247 /// n = number of data points. assumed to be .ge. 2.
248 /// (tau(i), c(1,i), i=1,...,n) = absc 248 /// (tau(i), c(1,i), i=1,...,n) = abscissae and ordinates of the
249 /// data points. tau is assumed to 249 /// data points. tau is assumed to be strictly increasing.
250 /// ibcbeg, ibcend = boundary conditio 250 /// ibcbeg, ibcend = boundary condition indicators, and
251 /// c(2,1), c(2,n) = boundary conditio 251 /// c(2,1), c(2,n) = boundary condition information. specifically,
252 /// ibcbeg = 0 means no boundary c 252 /// ibcbeg = 0 means no boundary condition at tau(1) is given.
253 /// in this case, the not-a-knot 253 /// in this case, the not-a-knot condition is used, i.e. the
254 /// jump in the third derivative 254 /// jump in the third derivative across tau(2) is forced to
255 /// zero, thus the first and the 255 /// zero, thus the first and the second cubic polynomial pieces
256 /// are made to coincide.) 256 /// are made to coincide.)
257 /// ibcbeg = 1 means that the slop 257 /// ibcbeg = 1 means that the slope at tau(1) is made to equal
258 /// c(2,1), supplied by input. 258 /// c(2,1), supplied by input.
259 /// ibcbeg = 2 means that the seco 259 /// ibcbeg = 2 means that the second derivative at tau(1) is
260 /// made to equal c(2,1), suppli 260 /// made to equal c(2,1), supplied by input.
261 /// ibcend = 0, 1, or 2 has analogo 261 /// ibcend = 0, 1, or 2 has analogous meaning concerning the
262 /// boundary condition at tau(n) 262 /// boundary condition at tau(n), with the additional infor-
263 /// mation taken from c(2,n). 263 /// mation taken from c(2,n).
264 /// *********************** output * 264 /// *********************** output **************************
265 /// c(j,i), j=1,...,4; i=1,...,l (= n- 265 /// c(j,i), j=1,...,4; i=1,...,l (= n-1) = the polynomial coefficients
266 /// of the cubic interpolating spli 266 /// of the cubic interpolating spline with interior knots (or
267 /// joints) tau(2), ..., tau(n-1). 267 /// joints) tau(2), ..., tau(n-1). precisely, in the interval
268 /// (tau(i), tau(i+1)), the spline 268 /// (tau(i), tau(i+1)), the spline f is given by
269 /// f(x) = c(1,i)+h*(c(2,i)+h*(c 269 /// f(x) = c(1,i)+h*(c(2,i)+h*(c(3,i)+h*c(4,i)/3.)/2.)
270 /// where h = x - tau(i). the funct 270 /// where h = x - tau(i). the function program *ppvalu* may be
271 /// used to evaluate f or its deriv 271 /// used to evaluate f or its derivatives from tau,c, l = n-1,
272 /// and k=4. 272 /// and k=4.
273 273
274 int j, l; 274 int j, l;
275 double divdf1,divdf3,dtau,g=0; 275 double divdf1,divdf3,dtau,g=0;
276 // ***** a tridiagonal linear system for t 276 // ***** a tridiagonal linear system for the unknown slopes s(i) of
277 // f at tau(i), i=1,...,n, is generated 277 // f at tau(i), i=1,...,n, is generated and then solved by gauss elim-
278 // ination, with s(i) ending up in c(2,i) 278 // ination, with s(i) ending up in c(2,i), all i.
279 // c(3,.) and c(4,.) are used initiall 279 // c(3,.) and c(4,.) are used initially for temporary storage.
280 l = int(fNp-1); << 280 l = fNp-1;
281 // compute first differences of x sequence 281 // compute first differences of x sequence and store in C also,
282 // compute first divided difference of dat 282 // compute first divided difference of data and store in D.
283 {for (size_t m=1; m<fNp ; ++m) { 283 {for (size_t m=1; m<fNp ; ++m) {
284 fPoly[m].C() = fPoly[m].X() - fPoly[m-1 284 fPoly[m].C() = fPoly[m].X() - fPoly[m-1].X();
285 fPoly[m].D() = (fPoly[m].Y() - fPoly[m- 285 fPoly[m].D() = (fPoly[m].Y() - fPoly[m-1].Y())/fPoly[m].C();
286 }} 286 }}
287 // construct first equation from the bound 287 // construct first equation from the boundary condition, of the form
288 // D[0]*s[0] + C[0]*s[1] = B[0 288 // D[0]*s[0] + C[0]*s[1] = B[0]
289 if(fBegCond==0) { 289 if(fBegCond==0) {
290 if(fNp == 2) { 290 if(fNp == 2) {
291 // no condition at left end and n 291 // no condition at left end and n = 2.
292 fPoly[0].D() = 1.; 292 fPoly[0].D() = 1.;
293 fPoly[0].C() = 1.; 293 fPoly[0].C() = 1.;
294 fPoly[0].B() = 2.*fPoly[1].D(); 294 fPoly[0].B() = 2.*fPoly[1].D();
295 } else { 295 } else {
296 // not-a-knot condition at left e 296 // not-a-knot condition at left end and n .gt. 2.
297 fPoly[0].D() = fPoly[2].C(); 297 fPoly[0].D() = fPoly[2].C();
298 fPoly[0].C() = fPoly[1].C() + fPoly[2 298 fPoly[0].C() = fPoly[1].C() + fPoly[2].C();
299 fPoly[0].B() = ((fPoly[1].C()+2.*fPol 299 fPoly[0].B() = ((fPoly[1].C()+2.*fPoly[0].C())*fPoly[1].D()*fPoly[2].C()+
300 fPoly[1].C()*fPoly[1].C()*fP 300 fPoly[1].C()*fPoly[1].C()*fPoly[2].D())/fPoly[0].C();
301 } 301 }
302 } else if (fBegCond==1) { 302 } else if (fBegCond==1) {
303 // slope prescribed at left end. 303 // slope prescribed at left end.
304 fPoly[0].B() = fValBeg; 304 fPoly[0].B() = fValBeg;
305 fPoly[0].D() = 1.; 305 fPoly[0].D() = 1.;
306 fPoly[0].C() = 0.; 306 fPoly[0].C() = 0.;
307 } else if (fBegCond==2) { 307 } else if (fBegCond==2) {
308 // second derivative prescribed at l 308 // second derivative prescribed at left end.
309 fPoly[0].D() = 2.; 309 fPoly[0].D() = 2.;
310 fPoly[0].C() = 1.; 310 fPoly[0].C() = 1.;
311 fPoly[0].B() = 3.*fPoly[1].D() - fPoly[1 311 fPoly[0].B() = 3.*fPoly[1].D() - fPoly[1].C()/2.*fValBeg;
312 } 312 }
313 bool forward_gauss_elimination = true; 313 bool forward_gauss_elimination = true;
314 if(fNp > 2) { 314 if(fNp > 2) {
315 // if there are interior knots, generat 315 // if there are interior knots, generate the corresp. equations and car-
316 // ry out the forward pass of gauss eli 316 // ry out the forward pass of gauss elimination, after which the m-th
317 // equation reads D[m]*s[m] + C[m]*s 317 // equation reads D[m]*s[m] + C[m]*s[m+1] = B[m].
318 {for (int m=1; m<l; ++m) { 318 {for (int m=1; m<l; ++m) {
319 g = -fPoly[m+1].C()/fPoly[m-1].D(); 319 g = -fPoly[m+1].C()/fPoly[m-1].D();
320 fPoly[m].B() = g*fPoly[m-1].B() + 3.* 320 fPoly[m].B() = g*fPoly[m-1].B() + 3.*(fPoly[m].C()*fPoly[m+1].D()+fPoly[m+1].C()*fPoly[m].D());
321 fPoly[m].D() = g*fPoly[m-1].C() + 2.* 321 fPoly[m].D() = g*fPoly[m-1].C() + 2.*(fPoly[m].C() + fPoly[m+1].C());
322 }} 322 }}
323 // construct last equation from the seco 323 // construct last equation from the second boundary condition, of the form
324 // (-g*D[n-2])*s[n-2] + D[n-1] 324 // (-g*D[n-2])*s[n-2] + D[n-1]*s[n-1] = B[n-1]
325 // if slope is prescribed at right e 325 // if slope is prescribed at right end, one can go directly to back-
326 // substitution, since c array happe 326 // substitution, since c array happens to be set up just right for it
327 // at this point. 327 // at this point.
328 if(fEndCond == 0) { 328 if(fEndCond == 0) {
329 if (fNp > 3 || fBegCond != 0) { 329 if (fNp > 3 || fBegCond != 0) {
330 // not-a-knot and n .ge. 3, an 330 // not-a-knot and n .ge. 3, and either n.gt.3 or also not-a-knot at
331 // left end point. 331 // left end point.
332 g = fPoly[fNp-2].C() + fPoly[fNp-1 332 g = fPoly[fNp-2].C() + fPoly[fNp-1].C();
333 fPoly[fNp-1].B() = ((fPoly[fNp-1]. 333 fPoly[fNp-1].B() = ((fPoly[fNp-1].C()+2.*g)*fPoly[fNp-1].D()*fPoly[fNp-2].C()
334 + fPoly[fNp-1].C()*fP 334 + fPoly[fNp-1].C()*fPoly[fNp-1].C()*(fPoly[fNp-2].Y()-fPoly[fNp-3].Y())/fPoly[fNp-2].C())/g;
335 g = -g/fPoly[fNp-2].D(); 335 g = -g/fPoly[fNp-2].D();
336 fPoly[fNp-1].D() = fPoly[fNp-2].C( 336 fPoly[fNp-1].D() = fPoly[fNp-2].C();
337 } else { 337 } else {
338 // either (n=3 and not-a-knot 338 // either (n=3 and not-a-knot also at left) or (n=2 and not not-a-
339 // knot at left end point). 339 // knot at left end point).
340 fPoly[fNp-1].B() = 2.*fPoly[fNp-1] 340 fPoly[fNp-1].B() = 2.*fPoly[fNp-1].D();
341 fPoly[fNp-1].D() = 1.; 341 fPoly[fNp-1].D() = 1.;
342 g = -1./fPoly[fNp-2].D(); 342 g = -1./fPoly[fNp-2].D();
343 } 343 }
344 } else if (fEndCond == 1) { 344 } else if (fEndCond == 1) {
345 fPoly[fNp-1].B() = fValEnd; 345 fPoly[fNp-1].B() = fValEnd;
346 forward_gauss_elimination = false; 346 forward_gauss_elimination = false;
347 } else if (fEndCond == 2) { 347 } else if (fEndCond == 2) {
348 // second derivative prescribed a 348 // second derivative prescribed at right endpoint.
349 fPoly[fNp-1].B() = 3.*fPoly[fNp-1].D( 349 fPoly[fNp-1].B() = 3.*fPoly[fNp-1].D() + fPoly[fNp-1].C()/2.*fValEnd;
350 fPoly[fNp-1].D() = 2.; 350 fPoly[fNp-1].D() = 2.;
351 g = -1./fPoly[fNp-2].D(); 351 g = -1./fPoly[fNp-2].D();
352 } 352 }
353 } else { 353 } else {
354 if(fEndCond == 0) { 354 if(fEndCond == 0) {
355 if (fBegCond > 0) { 355 if (fBegCond > 0) {
356 // either (n=3 and not-a-knot 356 // either (n=3 and not-a-knot also at left) or (n=2 and not not-a-
357 // knot at left end point). 357 // knot at left end point).
358 fPoly[fNp-1].B() = 2.*fPoly[fNp-1] 358 fPoly[fNp-1].B() = 2.*fPoly[fNp-1].D();
359 fPoly[fNp-1].D() = 1.; 359 fPoly[fNp-1].D() = 1.;
360 g = -1./fPoly[fNp-2].D(); 360 g = -1./fPoly[fNp-2].D();
361 } else { 361 } else {
362 // not-a-knot at right endpoin 362 // not-a-knot at right endpoint and at left endpoint and n = 2.
363 fPoly[fNp-1].B() = fPoly[fNp-1].D( 363 fPoly[fNp-1].B() = fPoly[fNp-1].D();
364 forward_gauss_elimination = false; 364 forward_gauss_elimination = false;
365 } 365 }
366 } else if(fEndCond == 1) { 366 } else if(fEndCond == 1) {
367 fPoly[fNp-1].B() = fValEnd; 367 fPoly[fNp-1].B() = fValEnd;
368 forward_gauss_elimination = false; 368 forward_gauss_elimination = false;
369 } else if(fEndCond == 2) { 369 } else if(fEndCond == 2) {
370 // second derivative prescribed a 370 // second derivative prescribed at right endpoint.
371 fPoly[fNp-1].B() = 3.*fPoly[fNp-1].D( 371 fPoly[fNp-1].B() = 3.*fPoly[fNp-1].D() + fPoly[fNp-1].C()/2.*fValEnd;
372 fPoly[fNp-1].D() = 2.; 372 fPoly[fNp-1].D() = 2.;
373 g = -1./fPoly[fNp-2].D(); 373 g = -1./fPoly[fNp-2].D();
374 } 374 }
375 } 375 }
376 // complete forward pass of gauss eliminat 376 // complete forward pass of gauss elimination.
377 if(forward_gauss_elimination) { 377 if(forward_gauss_elimination) {
378 fPoly[fNp-1].D() = g*fPoly[fNp-2].C() + 378 fPoly[fNp-1].D() = g*fPoly[fNp-2].C() + fPoly[fNp-1].D();
379 fPoly[fNp-1].B() = (g*fPoly[fNp-2].B() + 379 fPoly[fNp-1].B() = (g*fPoly[fNp-2].B() + fPoly[fNp-1].B())/fPoly[fNp-1].D();
380 } 380 }
381 // carry out back substitution 381 // carry out back substitution
382 j = l-1; 382 j = l-1;
383 do { 383 do {
384 fPoly[j].B() = (fPoly[j].B() - fPoly[j] 384 fPoly[j].B() = (fPoly[j].B() - fPoly[j].C()*fPoly[j+1].B())/fPoly[j].D();
385 --j; 385 --j;
386 } while (j>=0); 386 } while (j>=0);
387 // ****** generate cubic coefficients in e 387 // ****** generate cubic coefficients in each interval, i.e., the deriv.s
388 // at its left endpoint, from value and s 388 // at its left endpoint, from value and slope at its endpoints.
389 for (size_t i=1; i<fNp; ++i) { 389 for (size_t i=1; i<fNp; ++i) {
390 dtau = fPoly[i].C(); 390 dtau = fPoly[i].C();
391 divdf1 = (fPoly[i].Y() - fPoly[i-1].Y() 391 divdf1 = (fPoly[i].Y() - fPoly[i-1].Y())/dtau;
392 divdf3 = fPoly[i-1].B() + fPoly[i].B() 392 divdf3 = fPoly[i-1].B() + fPoly[i].B() - 2.*divdf1;
393 fPoly[i-1].C() = (divdf1 - fPoly[i-1].B 393 fPoly[i-1].C() = (divdf1 - fPoly[i-1].B() - divdf3)/dtau;
394 fPoly[i-1].D() = (divdf3/dtau)/dtau; 394 fPoly[i-1].D() = (divdf3/dtau)/dtau;
395 } 395 }
396 } 396 }
397 397
398 protected: 398 protected:
399 std::vector<cubic_poly> fPoly; //[fNp] Array 399 std::vector<cubic_poly> fPoly; //[fNp] Array of polynomial terms
400 double fValBeg; // Initial value 400 double fValBeg; // Initial value of first or second derivative
401 double fValEnd; // End value of f 401 double fValEnd; // End value of first or second derivative
402 int fBegCond; // 0=no beg cond, 402 int fBegCond; // 0=no beg cond, 1=first derivative, 2=second derivative
403 int fEndCond; // 0=no end cond, 403 int fEndCond; // 0=no end cond, 1=first derivative, 2=second derivative
404 }; 404 };
405 405
406 ////////////////////////////////////////////// 406 //////////////////////////////////////////////////////////////////////////
407 // 407 // //
408 // quintic 408 // quintic //
409 // 409 // //
410 // Class to create quintic natural splines to 410 // Class to create quintic natural splines to interpolate knots //
411 // Arbitrary conditions can be introduced for 411 // Arbitrary conditions can be introduced for first and second //
412 // derivatives using double knots (see build_c 412 // derivatives using double knots (see build_coeff) for more on this. //
413 // Double knots are automatically introduced a 413 // Double knots are automatically introduced at ending points //
414 // 414 // //
415 ////////////////////////////////////////////// 415 //////////////////////////////////////////////////////////////////////////
416 class quintic : public base_spline { 416 class quintic : public base_spline {
417 protected: 417 protected:
418 quintic(std::ostream& a_out):base_spline(a_o 418 quintic(std::ostream& a_out):base_spline(a_out),fPoly() {}
419 public: 419 public:
420 quintic(std::ostream& a_out,size_t a_n ,doub 420 quintic(std::ostream& a_out,size_t a_n ,double a_x[], double a_y[])
421 :base_spline(a_out,-1,0,0,a_n,false) { 421 :base_spline(a_out,-1,0,0,a_n,false) {
422 if(!a_n) { 422 if(!a_n) {
423 m_out << "tools::spline::quintic : a_np 423 m_out << "tools::spline::quintic : a_np is null." << std::endl;
424 return; 424 return;
425 } 425 }
426 fXmin = a_x[0]; 426 fXmin = a_x[0];
427 fXmax = a_x[a_n-1]; 427 fXmax = a_x[a_n-1];
428 fPoly.resize(fNp); 428 fPoly.resize(fNp);
429 for (size_t i=0; i<a_n; ++i) { 429 for (size_t i=0; i<a_n; ++i) {
430 fPoly[i].X() = a_x[i]; 430 fPoly[i].X() = a_x[i];
431 fPoly[i].Y() = a_y[i]; 431 fPoly[i].Y() = a_y[i];
432 } 432 }
433 build_coeff(); // Build the spline coeffic 433 build_coeff(); // Build the spline coefficients.
434 } 434 }
435 public: 435 public:
436 quintic(const quintic& a_from):base_spline(a 436 quintic(const quintic& a_from):base_spline(a_from),fPoly(a_from.fPoly) {}
437 quintic& operator=(const quintic& a_from) { 437 quintic& operator=(const quintic& a_from) {
438 if(this==&a_from) return *this; 438 if(this==&a_from) return *this;
439 base_spline::operator=(a_from); 439 base_spline::operator=(a_from);
440 fPoly = a_from.fPoly; 440 fPoly = a_from.fPoly;
441 return *this; 441 return *this;
442 } 442 }
443 public: 443 public:
444 double eval(double x) const {if(!fNp) return 444 double eval(double x) const {if(!fNp) return 0;size_t klow=find_x(x);return fPoly[klow].eval(x);}
445 445
446 protected: 446 protected:
447 size_t find_x(double x) const { 447 size_t find_x(double x) const {
448 int klow=0; 448 int klow=0;
449 449
450 // If out of boundaries, extrapolate 450 // If out of boundaries, extrapolate
451 // It may be badly wrong 451 // It may be badly wrong
452 if(x<=fXmin) klow=0; 452 if(x<=fXmin) klow=0;
453 else if(x>=fXmax) klow=int(fNp-1); << 453 else if(x>=fXmax) klow=fNp-1;
454 else { 454 else {
455 if(fKstep) { // Equidistant knots, use h 455 if(fKstep) { // Equidistant knots, use histogramming :
456 klow = mn<int>(int((x-fXmin)/fDelta),i << 456 klow = mn<int>(int((x-fXmin)/fDelta),fNp-1);
457 } else { 457 } else {
458 int khig=int(fNp-1); << 458 int khig=fNp-1;
459 int khalf; 459 int khalf;
460 // Non equidistant knots, binary searc 460 // Non equidistant knots, binary search
461 while((khig-klow)>1) { 461 while((khig-klow)>1) {
462 khalf = (klow+khig)/2; 462 khalf = (klow+khig)/2;
463 if(x>fPoly[khalf].X()) klow=khalf; 463 if(x>fPoly[khalf].X()) klow=khalf;
464 else khig=khalf; 464 else khig=khalf;
465 } 465 }
466 } 466 }
467 467
468 // This could be removed, sanity check 468 // This could be removed, sanity check
469 if( (x<fPoly[klow].X()) || (fPoly[klow+1 469 if( (x<fPoly[klow].X()) || (fPoly[klow+1].X()<x) ) {
470 m_out << "tools::spline::quintic::find 470 m_out << "tools::spline::quintic::find_x : Binary search failed"
471 << " x(" << klow << ") = " << fP 471 << " x(" << klow << ") = " << fPoly[klow].X() << " < x= " << x
472 << " < x(" << klow+1<< ") = " << 472 << " < x(" << klow+1<< ") = " << fPoly[klow+1].X() << "."
473 << std::endl; 473 << std::endl;
474 } 474 }
475 } 475 }
476 return klow; 476 return klow;
477 } 477 }
478 478
479 void build_coeff() { 479 void build_coeff() {
480 ////////////////////////////////////////// 480 ////////////////////////////////////////////////////////////////////////////////
481 /// algorithm 600, collected algorithm 481 /// algorithm 600, collected algorithms from acm.
482 /// algorithm appeared in acm-trans. m 482 /// algorithm appeared in acm-trans. math. software, vol.9, no. 2,
483 /// jun., 1983, p. 258-259. 483 /// jun., 1983, p. 258-259.
484 /// 484 ///
485 /// quintic computes the coefficients 485 /// quintic computes the coefficients of a quintic natural quintic spli
486 /// s(x) with knots x(i) interpolating 486 /// s(x) with knots x(i) interpolating there to given function values:
487 /// s(x(i)) = y(i) for i = 487 /// s(x(i)) = y(i) for i = 1,2, ..., n.
488 /// in each interval (x(i),x(i+1)) the 488 /// in each interval (x(i),x(i+1)) the spline function s(xx) is a
489 /// polynomial of fifth degree: 489 /// polynomial of fifth degree:
490 /// s(xx) = ((((f(i)*p+e(i))*p+d(i))*p 490 /// s(xx) = ((((f(i)*p+e(i))*p+d(i))*p+c(i))*p+b(i))*p+y(i) (*)
491 /// = ((((-f(i)*q+e(i+1))*q-d(i+ 491 /// = ((((-f(i)*q+e(i+1))*q-d(i+1))*q+c(i+1))*q-b(i+1))*q+y(i+1)
492 /// where p = xx - x(i) and q = x(i 492 /// where p = xx - x(i) and q = x(i+1) - xx.
493 /// (note the first subscript in the s 493 /// (note the first subscript in the second expression.)
494 /// the different polynomials are piec 494 /// the different polynomials are pieced together so that s(x) and
495 /// its derivatives up to s"" are cont 495 /// its derivatives up to s"" are continuous.
496 /// 496 ///
497 /// input: 497 /// input:
498 /// 498 ///
499 /// n number of data points, 499 /// n number of data points, (at least three, i.e. n > 2)
500 /// x(1:n) the strictly increasing 500 /// x(1:n) the strictly increasing or decreasing sequence of
501 /// knots. the spacing mus 501 /// knots. the spacing must be such that the fifth power
502 /// of x(i+1) - x(i) can be 502 /// of x(i+1) - x(i) can be formed without overflow or
503 /// underflow of exponents. 503 /// underflow of exponents.
504 /// y(1:n) the prescribed function 504 /// y(1:n) the prescribed function values at the knots.
505 /// 505 ///
506 /// output: 506 /// output:
507 /// 507 ///
508 /// b,c,d,e,f the computed spline coe 508 /// b,c,d,e,f the computed spline coefficients as in (*).
509 /// (1:n) specifically 509 /// (1:n) specifically
510 /// b(i) = s'(x(i)), c(i) = 510 /// b(i) = s'(x(i)), c(i) = s"(x(i))/2, d(i) = s"'(x(i))/6,
511 /// e(i) = s""(x(i))/24, f 511 /// e(i) = s""(x(i))/24, f(i) = s""'(x(i))/120.
512 /// f(n) is neither used no 512 /// f(n) is neither used nor altered. the five arrays
513 /// b,c,d,e,f must always b 513 /// b,c,d,e,f must always be distinct.
514 /// 514 ///
515 /// option: 515 /// option:
516 /// 516 ///
517 /// it is possible to specify values f 517 /// it is possible to specify values for the first and second
518 /// derivatives of the spline function 518 /// derivatives of the spline function at arbitrarily many knots.
519 /// this is done by relaxing the requi 519 /// this is done by relaxing the requirement that the sequence of
520 /// knots be strictly increasing or de 520 /// knots be strictly increasing or decreasing. specifically:
521 /// 521 ///
522 /// if x(j) = x(j+1) then s(x(j)) = y( 522 /// if x(j) = x(j+1) then s(x(j)) = y(j) and s'(x(j)) = y(j+1),
523 /// if x(j) = x(j+1) = x(j+2) then in 523 /// if x(j) = x(j+1) = x(j+2) then in addition s"(x(j)) = y(j+2).
524 /// 524 ///
525 /// note that s""(x) is discontinuous 525 /// note that s""(x) is discontinuous at a double knot and, in
526 /// addition, s"'(x) is discontinuous 526 /// addition, s"'(x) is discontinuous at a triple knot. the
527 /// subroutine assigns y(i) to y(i+1) 527 /// subroutine assigns y(i) to y(i+1) in these cases and also to
528 /// y(i+2) at a triple knot. the repr 528 /// y(i+2) at a triple knot. the representation (*) remains
529 /// valid in each open interval (x(i), 529 /// valid in each open interval (x(i),x(i+1)). at a double knot,
530 /// x(j) = x(j+1), the output coeffici 530 /// x(j) = x(j+1), the output coefficients have the following values:
531 /// y(j) = s(x(j)) = y(j+1) 531 /// y(j) = s(x(j)) = y(j+1)
532 /// b(j) = s'(x(j)) = b(j+1) 532 /// b(j) = s'(x(j)) = b(j+1)
533 /// c(j) = s"(x(j))/2 = c(j+1) 533 /// c(j) = s"(x(j))/2 = c(j+1)
534 /// d(j) = s"'(x(j))/6 = d(j+1) 534 /// d(j) = s"'(x(j))/6 = d(j+1)
535 /// e(j) = s""(x(j)-0)/24 e(j+1) 535 /// e(j) = s""(x(j)-0)/24 e(j+1) = s""(x(j)+0)/24
536 /// f(j) = s""'(x(j)-0)/120 f(j+1) 536 /// f(j) = s""'(x(j)-0)/120 f(j+1) = s""'(x(j)+0)/120
537 /// at a triple knot, x(j) = x(j+1) = 537 /// at a triple knot, x(j) = x(j+1) = x(j+2), the output
538 /// coefficients have the following va 538 /// coefficients have the following values:
539 /// y(j) = s(x(j)) = y(j+1) 539 /// y(j) = s(x(j)) = y(j+1) = y(j+2)
540 /// b(j) = s'(x(j)) = b(j+1) 540 /// b(j) = s'(x(j)) = b(j+1) = b(j+2)
541 /// c(j) = s"(x(j))/2 = c(j+1) 541 /// c(j) = s"(x(j))/2 = c(j+1) = c(j+2)
542 /// d(j) = s"'((x(j)-0)/6 d(j+1) 542 /// d(j) = s"'((x(j)-0)/6 d(j+1) = 0 d(j+2) = s"'(x(j)+0)/6
543 /// e(j) = s""(x(j)-0)/24 e(j+1) 543 /// e(j) = s""(x(j)-0)/24 e(j+1) = 0 e(j+2) = s""(x(j)+0)/24
544 /// f(j) = s""'(x(j)-0)/120 f(j+1) 544 /// f(j) = s""'(x(j)-0)/120 f(j+1) = 0 f(j+2) = s""'(x(j)+0)/120
545 545
546 size_t i, _m; << 546 size_t i, m;
547 double pqqr, p, q, r, _s, t, u, v, << 547 double pqqr, p, q, r, s, t, u, v,
548 b1, p2, p3, q2, q3, r2, pq, pr, qr; 548 b1, p2, p3, q2, q3, r2, pq, pr, qr;
549 549
550 if (fNp <= 2) return; 550 if (fNp <= 2) return;
551 551
552 // coefficients of a positive definite 552 // coefficients of a positive definite, pentadiagonal matrix,
553 // stored in D, E, F from 1 to n-3. 553 // stored in D, E, F from 1 to n-3.
554 _m = fNp-2; << 554 m = fNp-2;
555 q = fPoly[1].X()-fPoly[0].X(); 555 q = fPoly[1].X()-fPoly[0].X();
556 r = fPoly[2].X()-fPoly[1].X(); 556 r = fPoly[2].X()-fPoly[1].X();
557 q2 = q*q; 557 q2 = q*q;
558 r2 = r*r; 558 r2 = r*r;
559 qr = q+r; 559 qr = q+r;
560 fPoly[0].D() = fPoly[0].E() = 0; 560 fPoly[0].D() = fPoly[0].E() = 0;
561 if (q) fPoly[1].D() = q*6.*q2/(qr*qr); 561 if (q) fPoly[1].D() = q*6.*q2/(qr*qr);
562 else fPoly[1].D() = 0; 562 else fPoly[1].D() = 0;
563 563
564 if (_m > 1) { << 564 if (m > 1) {
565 for (i = 1; i < _m; ++i) { << 565 for (i = 1; i < m; ++i) {
566 p = q; 566 p = q;
567 q = r; 567 q = r;
568 r = fPoly[i+2].X()-fPoly[i+1].X(); 568 r = fPoly[i+2].X()-fPoly[i+1].X();
569 p2 = q2; 569 p2 = q2;
570 q2 = r2; 570 q2 = r2;
571 r2 = r*r; 571 r2 = r*r;
572 pq = qr; 572 pq = qr;
573 qr = q+r; 573 qr = q+r;
574 if (q) { 574 if (q) {
575 q3 = q2*q; 575 q3 = q2*q;
576 pr = p*r; 576 pr = p*r;
577 pqqr = pq*qr; 577 pqqr = pq*qr;
578 fPoly[i+1].D() = q3*6./(qr*qr); 578 fPoly[i+1].D() = q3*6./(qr*qr);
579 fPoly[i].D() += (q+q)*(pr*15.*pr+ 579 fPoly[i].D() += (q+q)*(pr*15.*pr+(p+r)*q
580 *(pr* 20.+q2 580 *(pr* 20.+q2*7.)+q2*
581 ((p2+r2)*8.+ 581 ((p2+r2)*8.+pr*21.+q2+q2))/(pqqr*pqqr);
582 fPoly[i-1].D() += q3*6./(pq*pq); 582 fPoly[i-1].D() += q3*6./(pq*pq);
583 fPoly[i].E() = q2*(p*qr+pq*3.*(qr 583 fPoly[i].E() = q2*(p*qr+pq*3.*(qr+r+r))/(pqqr*qr);
584 fPoly[i-1].E() += q2*(r*pq+qr*3.* 584 fPoly[i-1].E() += q2*(r*pq+qr*3.*(pq+p+p))/(pqqr*pq);
585 fPoly[i-1].F() = q3/pqqr; 585 fPoly[i-1].F() = q3/pqqr;
586 } else 586 } else
587 fPoly[i+1].D() = fPoly[i].E() = f 587 fPoly[i+1].D() = fPoly[i].E() = fPoly[i-1].F() = 0;
588 } 588 }
589 } 589 }
590 if (r) fPoly[_m-1].D() += r*6.*r2/(qr*qr); << 590 if (r) fPoly[m-1].D() += r*6.*r2/(qr*qr);
591 591
592 // First and second order divided diff 592 // First and second order divided differences of the given function
593 // values, stored in b from 2 to n and 593 // values, stored in b from 2 to n and in c from 3 to n
594 // respectively. care is taken of doub 594 // respectively. care is taken of double and triple knots.
595 for (i = 1; i < fNp; ++i) { 595 for (i = 1; i < fNp; ++i) {
596 if (fPoly[i].X() != fPoly[i-1].X()) { 596 if (fPoly[i].X() != fPoly[i-1].X()) {
597 fPoly[i].B() = 597 fPoly[i].B() =
598 (fPoly[i].Y()-fPoly[i-1].Y())/(fP 598 (fPoly[i].Y()-fPoly[i-1].Y())/(fPoly[i].X()-fPoly[i-1].X());
599 } else { 599 } else {
600 fPoly[i].B() = fPoly[i].Y(); 600 fPoly[i].B() = fPoly[i].Y();
601 fPoly[i].Y() = fPoly[i-1].Y(); 601 fPoly[i].Y() = fPoly[i-1].Y();
602 } 602 }
603 } 603 }
604 for (i = 2; i < fNp; ++i) { 604 for (i = 2; i < fNp; ++i) {
605 if (fPoly[i].X() != fPoly[i-2].X()) { 605 if (fPoly[i].X() != fPoly[i-2].X()) {
606 fPoly[i].C() = 606 fPoly[i].C() =
607 (fPoly[i].B()-fPoly[i-1].B())/(fP 607 (fPoly[i].B()-fPoly[i-1].B())/(fPoly[i].X()-fPoly[i-2].X());
608 } else { 608 } else {
609 fPoly[i].C() = fPoly[i].B()*.5; 609 fPoly[i].C() = fPoly[i].B()*.5;
610 fPoly[i].B() = fPoly[i-1].B(); 610 fPoly[i].B() = fPoly[i-1].B();
611 } 611 }
612 } 612 }
613 613
614 // Solve the linear system with c(i+2) 614 // Solve the linear system with c(i+2) - c(i+1) as right-hand side.
615 if (_m > 1) { << 615 if (m > 1) {
616 p=fPoly[0].C()=fPoly[_m-1].E()=fPoly[0] << 616 p=fPoly[0].C()=fPoly[m-1].E()=fPoly[0].F()
617 =fPoly[_m-2].F()=fPoly[_m-1].F()=0; << 617 =fPoly[m-2].F()=fPoly[m-1].F()=0;
618 fPoly[1].C() = fPoly[3].C()-fPoly[2].C( 618 fPoly[1].C() = fPoly[3].C()-fPoly[2].C();
619 fPoly[1].D() = 1./fPoly[1].D(); 619 fPoly[1].D() = 1./fPoly[1].D();
620 620
621 if (_m > 2) { << 621 if (m > 2) {
622 for (i = 2; i < _m; ++i) { << 622 for (i = 2; i < m; ++i) {
623 q = fPoly[i-1].D()*fPoly[i-1].E() 623 q = fPoly[i-1].D()*fPoly[i-1].E();
624 fPoly[i].D() = 1./(fPoly[i].D()-p 624 fPoly[i].D() = 1./(fPoly[i].D()-p*fPoly[i-2].F()-q*fPoly[i-1].E());
625 fPoly[i].E() -= q*fPoly[i-1].F(); 625 fPoly[i].E() -= q*fPoly[i-1].F();
626 fPoly[i].C() = fPoly[i+2].C()-fPo 626 fPoly[i].C() = fPoly[i+2].C()-fPoly[i+1].C()-p*fPoly[i-2].C()
627 -q*fPoly[i-1].C(); 627 -q*fPoly[i-1].C();
628 p = fPoly[i-1].D()*fPoly[i-1].F() 628 p = fPoly[i-1].D()*fPoly[i-1].F();
629 } 629 }
630 } 630 }
631 } 631 }
632 632
633 fPoly[fNp-2].C() = fPoly[fNp-1].C() = 0; 633 fPoly[fNp-2].C() = fPoly[fNp-1].C() = 0;
634 if (fNp > 3) 634 if (fNp > 3)
635 for (i=fNp-3; i > 0; --i) 635 for (i=fNp-3; i > 0; --i)
636 fPoly[i].C() = (fPoly[i].C()-fPoly[i 636 fPoly[i].C() = (fPoly[i].C()-fPoly[i].E()*fPoly[i+1].C()
637 -fPoly[i].F()*fPoly[i 637 -fPoly[i].F()*fPoly[i+2].C())*fPoly[i].D();
638 638
639 // Integrate the third derivative of s 639 // Integrate the third derivative of s(x)
640 _m = fNp-1; << 640 m = fNp-1;
641 q = fPoly[1].X()-fPoly[0].X(); 641 q = fPoly[1].X()-fPoly[0].X();
642 r = fPoly[2].X()-fPoly[1].X(); 642 r = fPoly[2].X()-fPoly[1].X();
643 b1 = fPoly[1].B(); 643 b1 = fPoly[1].B();
644 q3 = q*q*q; 644 q3 = q*q*q;
645 qr = q+r; 645 qr = q+r;
646 if (qr) { 646 if (qr) {
647 v = fPoly[1].C()/qr; 647 v = fPoly[1].C()/qr;
648 t = v; 648 t = v;
649 } else 649 } else
650 v = t = 0; 650 v = t = 0;
651 if (q) fPoly[0].F() = v/q; 651 if (q) fPoly[0].F() = v/q;
652 else fPoly[0].F() = 0; 652 else fPoly[0].F() = 0;
653 for (i = 1; i < _m; ++i) { << 653 for (i = 1; i < m; ++i) {
654 p = q; 654 p = q;
655 q = r; 655 q = r;
656 if (i != _m-1) r = fPoly[i+2].X()-fPoly << 656 if (i != m-1) r = fPoly[i+2].X()-fPoly[i+1].X();
657 else r = 0; 657 else r = 0;
658 p3 = q3; 658 p3 = q3;
659 q3 = q*q*q; 659 q3 = q*q*q;
660 pq = qr; 660 pq = qr;
661 qr = q+r; 661 qr = q+r;
662 _s = t; << 662 s = t;
663 if (qr) t = (fPoly[i+1].C()-fPoly[i].C( 663 if (qr) t = (fPoly[i+1].C()-fPoly[i].C())/qr;
664 else t = 0; 664 else t = 0;
665 u = v; 665 u = v;
666 v = t-_s; << 666 v = t-s;
667 if (pq) { 667 if (pq) {
668 fPoly[i].F() = fPoly[i-1].F(); 668 fPoly[i].F() = fPoly[i-1].F();
669 if (q) fPoly[i].F() = v/q; 669 if (q) fPoly[i].F() = v/q;
670 fPoly[i].E() = _s*5.; << 670 fPoly[i].E() = s*5.;
671 fPoly[i].D() = (fPoly[i].C()-q*_s)*1 << 671 fPoly[i].D() = (fPoly[i].C()-q*s)*10;
672 fPoly[i].C() = 672 fPoly[i].C() =
673 fPoly[i].D()*(p-q)+(fPoly[i+1].B()-f 673 fPoly[i].D()*(p-q)+(fPoly[i+1].B()-fPoly[i].B()+(u-fPoly[i].E())*
674 p3-(v+fPoly[i].E( 674 p3-(v+fPoly[i].E())*q3)/pq;
675 fPoly[i].B() = (p*(fPoly[i+1].B()-v* 675 fPoly[i].B() = (p*(fPoly[i+1].B()-v*q3)+q*(fPoly[i].B()-u*p3))/pq-p
676 *q*(fPoly[i].D()+fPoly[i].E()*(q-p)) 676 *q*(fPoly[i].D()+fPoly[i].E()*(q-p));
677 } else { 677 } else {
678 fPoly[i].C() = fPoly[i-1].C(); 678 fPoly[i].C() = fPoly[i-1].C();
679 fPoly[i].D() = fPoly[i].E() = fPoly[ 679 fPoly[i].D() = fPoly[i].E() = fPoly[i].F() = 0;
680 } 680 }
681 } 681 }
682 682
683 // End points x(1) and x(n) 683 // End points x(1) and x(n)
684 p = fPoly[1].X()-fPoly[0].X(); 684 p = fPoly[1].X()-fPoly[0].X();
685 _s = fPoly[0].F()*p*p*p; << 685 s = fPoly[0].F()*p*p*p;
686 fPoly[0].E() = fPoly[0].D() = 0; 686 fPoly[0].E() = fPoly[0].D() = 0;
687 fPoly[0].C() = fPoly[1].C()-_s*10; << 687 fPoly[0].C() = fPoly[1].C()-s*10;
688 fPoly[0].B() = b1-(fPoly[0].C()+_s)*p; << 688 fPoly[0].B() = b1-(fPoly[0].C()+s)*p;
689 689
690 q = fPoly[fNp-1].X()-fPoly[fNp-2].X(); 690 q = fPoly[fNp-1].X()-fPoly[fNp-2].X();
691 t = fPoly[fNp-2].F()*q*q*q; 691 t = fPoly[fNp-2].F()*q*q*q;
692 fPoly[fNp-1].E() = fPoly[fNp-1].D() = 0; 692 fPoly[fNp-1].E() = fPoly[fNp-1].D() = 0;
693 fPoly[fNp-1].C() = fPoly[fNp-2].C()+t*10; 693 fPoly[fNp-1].C() = fPoly[fNp-2].C()+t*10;
694 fPoly[fNp-1].B() += (fPoly[fNp-1].C()-t)*q 694 fPoly[fNp-1].B() += (fPoly[fNp-1].C()-t)*q;
695 } 695 }
696 protected: 696 protected:
697 std::vector<quintic_poly> fPoly; //[fNp] 697 std::vector<quintic_poly> fPoly; //[fNp] Array of polynomial terms
698 }; 698 };
699 699
700 }} 700 }}
701 701
702 #endif 702 #endif