nux-1.16.0
|
00001 /* 00002 * Copyright 2010 Inalogic® Inc. 00003 * 00004 * This program is free software: you can redistribute it and/or modify it 00005 * under the terms of the GNU Lesser General Public License, as 00006 * published by the Free Software Foundation; either version 2.1 or 3.0 00007 * of the License. 00008 * 00009 * This program is distributed in the hope that it will be useful, but 00010 * WITHOUT ANY WARRANTY; without even the implied warranties of 00011 * MERCHANTABILITY, SATISFACTORY QUALITY or FITNESS FOR A PARTICULAR 00012 * PURPOSE. See the applicable version of the GNU Lesser General Public 00013 * License for more details. 00014 * 00015 * You should have received a copy of both the GNU Lesser General Public 00016 * License along with this program. If not, see <http://www.gnu.org/licenses/> 00017 * 00018 * Authored by: Jay Taoko <jaytaoko@inalogic.com> 00019 * 00020 */ 00021 00022 00023 #include "../NuxCore.h" 00024 #include "Spline.h" 00025 #include "MathFunctions.h" 00026 00027 namespace nux 00028 { 00029 00030 double *CubicSpline::SolveTridiag ( int n, double a[], double b[] ) 00031 { 00032 int i; 00033 double *x; 00034 double xmult; 00035 00036 // 00037 // Check. 00038 // 00039 for ( i = 0; i < n; i++ ) 00040 { 00041 if ( a[1+i*3] == 0.0 ) 00042 { 00043 return NULL; 00044 } 00045 } 00046 00047 x = new double[n]; 00048 00049 for ( i = 0; i < n; i++ ) 00050 { 00051 x[i] = b[i]; 00052 } 00053 00054 for ( i = 1; i < n; i++ ) 00055 { 00056 xmult = a[2+ (i-1) *3] / a[1+ (i-1) *3]; 00057 a[1+i*3] = a[1+i*3] - xmult * a[0+i*3]; 00058 x[i] = x[i] - xmult * x[i-1]; 00059 } 00060 00061 x[n-1] = x[n-1] / a[1+ (n-1) *3]; 00062 00063 for ( i = n - 2; 0 <= i; i-- ) 00064 { 00065 x[i] = ( x[i] - a[0+ (i+1) *3] * x[i+1] ) / a[1+i*3]; 00066 } 00067 00068 return x; 00069 } 00070 00071 00072 CubicSpline::CubicSpline (int numpoint, std::vector<double> x_array, std::vector<double> y_array, int ibcbeg, double ybcbeg, int ibcend, double ybcend ) 00073 { 00074 if ( ( (int) x_array.size() != numpoint) || ( (int) y_array.size() != numpoint) ) 00075 { 00076 NUX_BREAK_ASM_INT3; 00077 } 00078 00079 np = numpoint; 00080 t = new double[np]; 00081 y = new double[np]; 00082 ddy = 0; //new double[np]; 00083 00084 for (int i = 0; i < np; i++) 00085 { 00086 t[i] = x_array[i]; 00087 y[i] = y_array[i]; 00088 //ddy[i] = 0; 00089 } 00090 00091 ibcbeg_ = ibcbeg; 00092 ybcbeg_ = ybcbeg; 00093 ibcend_ = ibcend; 00094 ybcend_ = ybcend; 00095 Compute (ibcbeg_, ybcbeg_, ibcend_, ybcend_ ); 00096 } 00097 00098 CubicSpline::CubicSpline() 00099 { 00100 np = 2; 00101 t = new double[np]; 00102 y = new double[np]; 00103 ddy = 0; //new double[np]; 00104 00105 t[0] = 0.0; 00106 t[1] = 1.0; 00107 y[0] = 0.0; 00108 y[1] = 1.0; 00109 00110 ibcbeg_ = 0; 00111 ybcbeg_ = 0; 00112 ibcend_ = 0; 00113 ybcend_ = 0; 00114 Compute (ibcbeg_, ybcbeg_, ibcend_, ybcend_ ); 00115 00116 } 00117 00118 CubicSpline::CubicSpline (const CubicSpline &Other) 00119 { 00120 if (Other.np == 0) 00121 NUX_BREAK_ASM_INT3; 00122 00123 np = Other.np; 00124 t = new double[np]; 00125 y = new double[np]; 00126 00127 for (int i = 0; i < np; i++) 00128 { 00129 t[i] = Other.t[i]; 00130 y[i] = Other.y[i]; 00131 } 00132 00133 ibcbeg_ = Other.ibcbeg_; 00134 ybcbeg_ = Other.ybcbeg_; 00135 ibcend_ = Other.ibcend_; 00136 ybcend_ = Other.ybcend_; 00137 Compute (ibcbeg_, ybcbeg_, ibcend_, ybcend_ ); 00138 } 00139 00140 CubicSpline &CubicSpline::operator = (const CubicSpline &Other) 00141 { 00142 if (Other.np == 0) 00143 NUX_BREAK_ASM_INT3; 00144 00145 np = Other.np; 00146 t = new double[np]; 00147 y = new double[np]; 00148 00149 for (int i = 0; i < np; i++) 00150 { 00151 t[i] = Other.t[i]; 00152 y[i] = Other.y[i]; 00153 } 00154 00155 ibcbeg_ = Other.ibcbeg_; 00156 ybcbeg_ = Other.ybcbeg_; 00157 ibcend_ = Other.ibcend_; 00158 ybcend_ = Other.ybcend_; 00159 Compute (ibcbeg_, ybcbeg_, ibcend_, ybcend_); 00160 return *this; 00161 } 00162 00163 void CubicSpline::Set (int numpoint, std::vector<double> x_array, std::vector<double> y_array, int ibcbeg, double ybcbeg, int ibcend, double ybcend) 00164 { 00165 if (numpoint <= 1) 00166 { 00167 np = 0; 00168 return; 00169 } 00170 00171 if ( ( (int) x_array.size() != numpoint) || ( (int) y_array.size() != numpoint) ) 00172 { 00173 NUX_BREAK_ASM_INT3; 00174 } 00175 00176 np = numpoint; 00177 00178 if (t) delete [] t; 00179 00180 if (y) delete [] y; 00181 00182 if (ddy) delete [] ddy; 00183 00184 t = new double[np]; 00185 y = new double[np]; 00186 ddy = 0; 00187 00188 for (int i = 0; i < np; i++) 00189 { 00190 t[i] = x_array[i]; 00191 y[i] = y_array[i]; 00192 } 00193 00194 Compute (ibcbeg, ybcbeg, ibcend, ybcend ); 00195 } 00196 00197 CubicSpline::~CubicSpline() 00198 { 00199 if (t) delete [] t; 00200 00201 if (y) delete [] y; 00202 00203 if (ddy) delete [] ddy; 00204 } 00205 00206 double *CubicSpline::Compute (int ibcbeg, double ybcbeg, int ibcend, double ybcend ) 00207 { 00208 double *a; 00209 double *b; 00210 int i; 00211 00212 // 00213 // Check. 00214 // 00215 if ( np <= 1 ) 00216 { 00217 //"Fatal error: The number of data points N must be at least 2.\n"; 00218 NUX_BREAK_ASM_INT3; 00219 return 0; 00220 } 00221 00222 for ( i = 0; i < np - 1; i++ ) 00223 { 00224 if ( t[i+1] <= t[i] ) 00225 { 00226 //"Fatal error: The knots must be strictly increasing, but\n"; 00227 NUX_BREAK_ASM_INT3; 00228 return NULL; 00229 } 00230 } 00231 00232 a = new double[3*np]; 00233 b = new double[np]; 00234 00235 // 00236 // Set up the first equation. 00237 // 00238 if ( ibcbeg == 0 ) 00239 { 00240 b[0] = 0.0; 00241 a[1+0*3] = 1.0; 00242 a[0+1*3] = -1.0; 00243 } 00244 else if ( ibcbeg == 1 ) 00245 { 00246 b[0] = ( y[1] - y[0] ) / ( t[1] - t[0] ) - ybcbeg; 00247 a[1+0*3] = ( t[1] - t[0] ) / 3.0; 00248 a[0+1*3] = ( t[1] - t[0] ) / 6.0; 00249 } 00250 else if ( ibcbeg == 2 ) 00251 { 00252 b[0] = ybcbeg; 00253 a[1+0*3] = 1.0; 00254 a[0+1*3] = 0.0; 00255 } 00256 else 00257 { 00258 //"Fatal error: IBCBEG must be 0, 1 or 2.\n"; 00259 NUX_BREAK_ASM_INT3; 00260 delete [] a; 00261 delete [] b; 00262 return NULL; 00263 } 00264 00265 // 00266 // Set up the intermediate equations. 00267 // 00268 for ( i = 1; i < np - 1; i++ ) 00269 { 00270 b[i] = ( y[i+1] - y[i] ) / ( t[i+1] - t[i] ) - ( y[i] - y[i-1] ) / ( t[i] - t[i-1] ); 00271 00272 a[2+ (i-1) *3] = ( t[i] - t[i-1] ) / 6.0; 00273 00274 a[1+ i *3] = ( t[i+1] - t[i-1] ) / 3.0; 00275 00276 a[0+ (i+1) *3] = ( t[i+1] - t[i] ) / 6.0; 00277 } 00278 00279 // 00280 // Set up the last equation. 00281 // 00282 if ( ibcend == 0 ) 00283 { 00284 b[np-1] = 0.0; 00285 a[2+ (np-2) *3] = -1.0; 00286 a[1+ (np-1) *3] = 1.0; 00287 } 00288 else if ( ibcend == 1 ) 00289 { 00290 b[np-1] = ybcend - ( y[np-1] - y[np-2] ) / ( t[np-1] - t[np-2] ); 00291 a[2+ (np-2) *3] = ( t[np-1] - t[np-2] ) / 6.0; 00292 a[1+ (np-1) *3] = ( t[np-1] - t[np-2] ) / 3.0; 00293 } 00294 else if ( ibcend == 2 ) 00295 { 00296 b[np-1] = ybcend; 00297 a[2+ (np-2) *3] = 0.0; 00298 a[1+ (np-1) *3] = 1.0; 00299 } 00300 else 00301 { 00302 //"Fatal error: IBCEND must be 0, 1 or 2.\n"; 00303 NUX_BREAK_ASM_INT3; 00304 delete [] a; 00305 delete [] b; 00306 return NULL; 00307 } 00308 00309 // 00310 // Solve the linear system. 00311 // 00312 if ( np == 2 && ibcbeg == 0 && ibcend == 0 ) 00313 { 00314 ddy = new double[2]; 00315 00316 ddy[0] = 0.0; 00317 ddy[1] = 0.0; 00318 } 00319 else 00320 { 00321 if (ddy) 00322 { 00323 delete [] ddy; 00324 } 00325 00326 ddy = SolveTridiag ( np, a, b ); 00327 00328 if ( !ddy ) 00329 { 00330 //"Fatal error: The linear system could not be solved.\n"; 00331 NUX_BREAK_ASM_INT3; 00332 delete [] a; 00333 delete [] b; 00334 return NULL; 00335 } 00336 } 00337 00338 delete [] a; 00339 delete [] b; 00340 return ddy; 00341 } 00342 00343 double CubicSpline::Eval (double tval) 00344 { 00345 double dt; 00346 double h; 00347 int i; 00348 int ival; 00349 double yval; 00350 00351 // 00352 // Determine the interval [ T(I), T(I+1) ] that contains TVAL. 00353 // Values below T[0] or above T[N-1] use extrapolation. 00354 // 00355 if (np <= 1) 00356 return 0.0; 00357 00358 ival = np - 2; 00359 00360 if (tval > t[np-1]) 00361 { 00362 tval = t[np-1]; 00363 } 00364 00365 if (tval < t[0]) 00366 { 00367 tval = t[0]; 00368 } 00369 00370 for ( i = 0; i < np - 1; i++ ) 00371 { 00372 if ( tval < t[i+1] ) 00373 { 00374 ival = i; 00375 break; 00376 } 00377 } 00378 00379 // 00380 // In the interval I, the polynomial is in terms of a normalized 00381 // coordinate between 0 and 1. 00382 // 00383 dt = tval - t[ival]; 00384 h = t[ival+1] - t[ival]; 00385 00386 yval = y[ival] + dt * ( ( y[ival+1] - y[ival] ) / h - ( ddy[ival+1] / 6.0 + ddy[ival] / 3.0 ) * h + dt * ( 0.5 * ddy[ival] 00387 + dt * ( ( ddy[ival+1] - ddy[ival] ) / ( 6.0 * h ) ) ) ); 00388 00389 return yval; 00390 } 00391 00392 }