00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025 #include "orsa_integrator.h"
00026 #include "orsa_error.h"
00027
00028 #include <iostream>
00029 #include <cstring>
00030 #include <cmath>
00031
00032 #include "support.h"
00033
00034 namespace orsa {
00035
00036 using std::memset;
00037 using std::fabs;
00038 using std::pow;
00039
00040 Radau15::Radau15() : VariableTimestepIntegrator() {
00041 init();
00042 }
00043
00044 Radau15::Radau15(const Radau15 & i) : VariableTimestepIntegrator() {
00045 type = i.type;
00046 timestep = i.timestep;
00047 accuracy = i.accuracy;
00048
00049 init();
00050 }
00051
00052 Integrator * Radau15::clone() const {
00053 return new Radau15(*this);
00054 }
00055
00056 void Radau15::init() {
00057
00058 type = RA15;
00059
00060 h[0] = 0.0;
00061 h[1] = 0.05626256053692215;
00062 h[2] = 0.18024069173689236;
00063 h[3] = 0.35262471711316964;
00064 h[4] = 0.54715362633055538;
00065 h[5] = 0.73421017721541053;
00066 h[6] = 0.88532094683909577;
00067 h[7] = 0.97752061356128750;
00068
00069 xc[0] = 0.5;
00070 xc[1] = 0.16666666666666667;
00071 xc[2] = 0.08333333333333333;
00072 xc[3] = 0.05;
00073 xc[4] = 0.03333333333333333;
00074 xc[5] = 0.02380952380952381;
00075 xc[6] = 0.01785714285714286;
00076 xc[7] = 0.01388888888888889;
00077
00078 vc[0] = 0.5;
00079 vc[1] = 0.3333333333333333;
00080 vc[2] = 0.25;
00081 vc[3] = 0.2;
00082 vc[4] = 0.1666666666666667;
00083 vc[5] = 0.1428571428571429;
00084 vc[6] = 0.125;
00085
00086
00087
00088 int j,k,l;
00089 l=0;
00090 for (j=1;j<8;++j) {
00091 for(k=0;k<j;++k) {
00092 r[l] = 1.0 / (h[j] - h[k]);
00093 ++l;
00094 }
00095 }
00096
00097
00098
00099
00100
00101
00102
00103
00104
00105
00106 c[0] = -h[1];
00107 d[0] = h[1];
00108 l=0;
00109 for (j=2;j<7;++j) {
00110 ++l;
00111 c[l] = -h[j] * c[l-j+1];
00112 d[l] = h[1] * d[l-j+1];
00113 for(k=2;k<j;++k) {
00114 ++l;
00115 c[l] = c[l-j] - h[j] * c[l-j+1];
00116 d[l] = d[l-j] + h[k] * d[l-j+1];
00117 }
00118 ++l;
00119 c[l] = c[l-j] - h[j];
00120 d[l] = d[l-j] + h[j];
00121 }
00122
00123
00124
00125
00126
00127
00128
00129 nv = 0;
00130 niter = 6;
00131
00132
00133
00134 size = 0;
00135 }
00136
00137 Radau15::~Radau15() {
00138
00139 }
00140
00141 void Radau15::Bodies_Mass_or_N_Bodies_Changed(const Frame &frame) {
00142
00143
00144
00145 nv = 3*frame.size();
00146
00147 if (nv > x.size()) {
00148 g.resize(7);
00149 b.resize(7);
00150 e.resize(7);
00151
00152 unsigned int l;
00153 for (l=0;l<7;++l) {
00154 g[l].resize(nv);
00155 b[l].resize(nv);
00156 e[l].resize(nv);
00157 }
00158
00159 x.resize(nv);
00160 v.resize(nv);
00161 a.resize(nv);
00162
00163 x1.resize(nv);
00164 v1.resize(nv);
00165 a1.resize(nv);
00166
00167 acc.resize(frame.size());
00168 mass.resize(frame.size());
00169 }
00170
00171
00172
00173
00174
00175
00176
00177
00178
00179
00180 memset(&b[0][0],0,7*nv);
00181 memset(&e[0][0],0,7*nv);
00182
00183
00184
00185
00186
00187
00188
00189
00190
00191
00192
00193
00194 for(unsigned int k=0;k<frame.size();++k) {
00195 mass[k] = frame[k].mass();
00196 }
00197
00198 size = frame.size();
00199 }
00200
00201 void Radau15::Step(const Frame & frame_in, Frame & frame_out, Interaction * interaction) {
00202
00203
00204
00205
00206
00207
00208
00209
00210 frame_out = frame_in;
00211
00212 niter = 2;
00213
00214 if (frame_out.size() != size) {
00215 Bodies_Mass_or_N_Bodies_Changed(frame_out);
00216 niter = 6;
00217 } else {
00218 unsigned int l=0;
00219 while (l != frame_out.size()) {
00220 if (frame_out[l].mass() != mass[l]) {
00221 Bodies_Mass_or_N_Bodies_Changed(frame_out);
00222 niter = 6;
00223 break;
00224 }
00225 ++l;
00226 }
00227 }
00228
00229
00230
00231 interaction->Acceleration(frame_out,acc);
00232
00233 unsigned int j,k;
00234 for(k=0;k<frame_in.size();++k) {
00235 x1[3*k] = frame_in[k].position().x;
00236 x1[3*k+1] = frame_in[k].position().y;
00237 x1[3*k+2] = frame_in[k].position().z;
00238
00239 v1[3*k] = frame_in[k].velocity().x;
00240 v1[3*k+1] = frame_in[k].velocity().y;
00241 v1[3*k+2] = frame_in[k].velocity().z;
00242
00243 a1[3*k] = acc[k].x;
00244 a1[3*k+1] = acc[k].y;
00245 a1[3*k+2] = acc[k].z;
00246 }
00247
00248 for(k=0;k<nv;++k) {
00249 g[0][k] = b[6][k]*d[15] + b[5][k]*d[10] + b[4][k]*d[6] + b[3][k]*d[3] + b[2][k]*d[1] + b[1][k]*d[0] + b[0][k];
00250 g[1][k] = b[6][k]*d[16] + b[5][k]*d[11] + b[4][k]*d[7] + b[3][k]*d[4] + b[2][k]*d[2] + b[1][k];
00251 g[2][k] = b[6][k]*d[17] + b[5][k]*d[12] + b[4][k]*d[8] + b[3][k]*d[5] + b[2][k];
00252 g[3][k] = b[6][k]*d[18] + b[5][k]*d[13] + b[4][k]*d[9] + b[3][k];
00253 g[4][k] = b[6][k]*d[19] + b[5][k]*d[14] + b[4][k];
00254 g[5][k] = b[6][k]*d[20] + b[5][k];
00255 g[6][k] = b[6][k];
00256 }
00257
00258 double tmp,gk;
00259 double q1,q2,q3,q4,q5,q6,q7;
00260
00261 unsigned int main_loop_counter;
00262 for (main_loop_counter=0;main_loop_counter<niter;++main_loop_counter) {
00263 for(j=1;j<8;++j) {
00264
00265
00266 s[0] = timestep.GetDouble() * h[j];
00267 s[1] = s[0] * s[0] * 0.5;
00268 s[2] = s[1] * h[j] * 0.3333333333333333;
00269 s[3] = s[2] * h[j] * 0.5;
00270 s[4] = s[3] * h[j] * 0.6;
00271 s[5] = s[4] * h[j] * 0.6666666666666667;
00272 s[6] = s[5] * h[j] * 0.7142857142857143;
00273 s[7] = s[6] * h[j] * 0.75;
00274 s[8] = s[7] * h[j] * 0.7777777777777778;
00275
00276 for(k=0;k<nv;++k) {
00277 x[k] = ( s[8]*b[6][k] +
00278 s[7]*b[5][k] +
00279 s[6]*b[4][k] +
00280 s[5]*b[3][k] +
00281 s[4]*b[2][k] +
00282 s[3]*b[1][k] +
00283 s[2]*b[0][k] ) +
00284 s[1]*a1[k] +
00285 s[0]*v1[k] +
00286 x1[k];
00287 }
00288
00289
00290 if (interaction->depends_on_velocity()) {
00291
00292 s[0] = timestep.GetDouble() * h[j];
00293 s[1] = s[0] * h[j] * 0.5;
00294 s[2] = s[1] * h[j] * 0.6666666666666667;
00295 s[3] = s[2] * h[j] * 0.75;
00296 s[4] = s[3] * h[j] * 0.8;
00297 s[5] = s[4] * h[j] * 0.8333333333333333;
00298 s[6] = s[5] * h[j] * 0.8571428571428571;
00299 s[7] = s[6] * h[j] * 0.875;
00300
00301 for(k=0;k<nv;++k) {
00302 v[k] = ( s[7]*b[6][k] +
00303 s[6]*b[5][k] +
00304 s[5]*b[4][k] +
00305 s[4]*b[3][k] +
00306 s[3]*b[2][k] +
00307 s[2]*b[1][k] +
00308 s[1]*b[0][k] ) +
00309 s[0]*a1[k] +
00310 v1[k];
00311 }
00312 }
00313
00314 {
00315 Vector rr,vv,drr,dvv;
00316 for(k=0;k<frame_out.size();++k) {
00317
00318 frame_out[k] = frame_in[k];
00319
00320 rr.x = x[3*k];
00321 rr.y = x[3*k+1];
00322 rr.z = x[3*k+2];
00323
00324 drr = rr - frame_in[k].position();
00325 frame_out[k].AddToPosition(drr);
00326
00327 vv.x = v[3*k];
00328 vv.y = v[3*k+1];
00329 vv.z = v[3*k+2];
00330
00331 dvv = vv - frame_in[k].velocity();
00332 frame_out[k].AddToVelocity(dvv);
00333 }
00334 }
00335
00336 if (interaction->IsSkippingJPLPlanets()) {
00337 frame_out.SetTime(frame_in+timestep*h[j]);
00338 frame_out.ForceJPLEphemerisData();
00339 }
00340
00341 interaction->Acceleration(frame_out,acc);
00342
00343 for(k=0;k<frame_out.size();++k) {
00344 a[3*k] = acc[k].x;
00345 a[3*k+1] = acc[k].y;
00346 a[3*k+2] = acc[k].z;
00347 }
00348
00349 switch (j) {
00350 case 1:
00351 for(k=0;k<nv;++k) {
00352 tmp = g[0][k];
00353 g[0][k] = (a[k] - a1[k]) * r[0];
00354 b[0][k] += g[0][k] - tmp;
00355 } break;
00356 case 2:
00357 for(k=0;k<nv;++k) {
00358 tmp = g[1][k];
00359 gk = a[k] - a1[k];
00360 g[1][k] = (gk*r[1] - g[0][k])*r[2];
00361 tmp = g[1][k] - tmp;
00362 b[0][k] += tmp * c[0];
00363 b[1][k] += tmp;
00364 } break;
00365 case 3:
00366 for(k=0;k<nv;++k) {
00367 tmp = g[2][k];
00368 gk = a[k] - a1[k];
00369 g[2][k] = ((gk*r[3] - g[0][k])*r[4] - g[1][k])*r[5];
00370 tmp = g[2][k] - tmp;
00371 b[0][k] += tmp * c[1];
00372 b[1][k] += tmp * c[2];
00373 b[2][k] += tmp;
00374 } break;
00375 case 4:
00376 for(k=0;k<nv;++k) {
00377 tmp = g[3][k];
00378 gk = a[k] - a1[k];
00379 g[3][k] = (((gk*r[6] - g[0][k])*r[7] - g[1][k])*r[8] - g[2][k])*r[9];
00380 tmp = g[3][k] - tmp;
00381 b[0][k] += tmp * c[3];
00382 b[1][k] += tmp * c[4];
00383 b[2][k] += tmp * c[5];
00384 b[3][k] += tmp;
00385 } break;
00386 case 5:
00387 for(k=0;k<nv;++k) {
00388 tmp = g[4][k];
00389 gk = a[k] - a1[k];
00390 g[4][k] = ((((gk*r[10] - g[0][k])*r[11] - g[1][k])*r[12] - g[2][k])*r[13] - g[3][k])*r[14];
00391 tmp = g[4][k] - tmp;
00392 b[0][k] += tmp * c[6];
00393 b[1][k] += tmp * c[7];
00394 b[2][k] += tmp * c[8];
00395 b[3][k] += tmp * c[9];
00396 b[4][k] += tmp;
00397 } break;
00398 case 6:
00399 for(k=0;k<nv;++k) {
00400 tmp = g[5][k];
00401 gk = a[k] - a1[k];
00402 g[5][k] = (((((gk*r[15] - g[0][k])*r[16] - g[1][k])*r[17] - g[2][k])*r[18] - g[3][k])*r[19] - g[4][k])*r[20];
00403 tmp = g[5][k] - tmp;
00404 b[0][k] += tmp * c[10];
00405 b[1][k] += tmp * c[11];
00406 b[2][k] += tmp * c[12];
00407 b[3][k] += tmp * c[13];
00408 b[4][k] += tmp * c[14];
00409 b[5][k] += tmp;
00410 } break;
00411 case 7:
00412 for(k=0;k<nv;++k) {
00413 tmp = g[6][k];
00414 gk = a[k] - a1[k];
00415 g[6][k] = ((((((gk*r[21] - g[0][k])*r[22] - g[1][k])*r[23] - g[2][k])*r[24] - g[3][k])*r[25] - g[4][k])*r[26] - g[5][k])*r[27];
00416 tmp = g[6][k] - tmp;
00417 b[0][k] += tmp * c[15];
00418 b[1][k] += tmp * c[16];
00419 b[2][k] += tmp * c[17];
00420 b[3][k] += tmp * c[18];
00421 b[4][k] += tmp * c[19];
00422 b[5][k] += tmp * c[20];
00423 b[6][k] += tmp;
00424 } break;
00425 default:
00426 ORSA_LOGIC_ERROR("aieeee!!!");
00427 }
00428
00429 }
00430 }
00431
00432 timestep_done = timestep;
00433
00434
00435 tmp = 0.0;
00436 for(k=0;k<nv;++k) {
00437 tmp = MAX(tmp,fabs(b[6][k]));
00438 }
00439
00440
00441 if (tmp!=0.0) tmp /= (72.0 * pow(fabs(timestep.GetDouble()),7));
00442
00443 if (tmp < 1.0e-50) {
00444
00445 timestep = timestep_done * 1.4;
00446 } else {
00447
00448
00449
00450
00451 timestep = copysign(pow(accuracy/tmp,0.1111111111111111),timestep_done.GetDouble());
00452
00453 }
00454
00455
00456 if (fabs(timestep.GetDouble()/timestep_done.GetDouble()) < 1.0) {
00457 timestep = timestep_done * 0.8;
00458
00459 frame_out = frame_in;
00460 niter = 6;
00461 return;
00462 }
00463
00464 if (fabs(timestep.GetDouble()/timestep_done.GetDouble()) > 1.4) timestep = timestep_done * 1.4;
00465
00466
00467
00468
00469 tmp = timestep_done.GetDouble() * timestep_done.GetDouble();
00470 for(k=0;k<nv;++k) {
00471 x1[k] = ( xc[7]*b[6][k] +
00472 xc[6]*b[5][k] +
00473 xc[5]*b[4][k] +
00474 xc[4]*b[3][k] +
00475 xc[3]*b[2][k] +
00476 xc[2]*b[1][k] +
00477 xc[1]*b[0][k] +
00478 xc[0]*a1[k] ) * tmp + v1[k]*timestep_done.GetDouble() + x1[k];
00479
00480 v1[k] = ( vc[6]*b[6][k] +
00481 vc[5]*b[5][k] +
00482 vc[4]*b[4][k] +
00483 vc[3]*b[3][k] +
00484 vc[2]*b[2][k] +
00485 vc[1]*b[1][k] +
00486 vc[0]*b[0][k] +
00487 a1[k]) * timestep_done.GetDouble() + v1[k];
00488 }
00489
00490 {
00491 Vector rr,vv,drr,dvv;
00492 for(k=0;k<frame_out.size();++k) {
00493
00494 frame_out[k] = frame_in[k];
00495
00496 rr.x = x1[3*k];
00497 rr.y = x1[3*k+1];
00498 rr.z = x1[3*k+2];
00499
00500 drr = rr - frame_in[k].position();
00501 frame_out[k].AddToPosition(drr);
00502
00503 vv.x = v1[3*k];
00504 vv.y = v1[3*k+1];
00505 vv.z = v1[3*k+2];
00506
00507 dvv = vv - frame_in[k].velocity();
00508 frame_out[k].AddToVelocity(dvv);
00509 }
00510 }
00511
00512
00513
00514 frame_out.SetTime(frame_in);
00515 frame_out += timestep_done;
00516
00517
00518
00519
00520
00521 q1 = timestep.GetDouble() / timestep_done.GetDouble();
00522 q2 = q1 * q1;
00523 q3 = q1 * q2;
00524 q4 = q2 * q2;
00525 q5 = q2 * q3;
00526 q6 = q3 * q3;
00527 q7 = q3 * q4;
00528
00529 for(k=0;k<nv;++k) {
00530
00531 s[0] = b[0][k] - e[0][k];
00532 s[1] = b[1][k] - e[1][k];
00533 s[2] = b[2][k] - e[2][k];
00534 s[3] = b[3][k] - e[3][k];
00535 s[4] = b[4][k] - e[4][k];
00536 s[5] = b[5][k] - e[5][k];
00537 s[6] = b[6][k] - e[6][k];
00538
00539
00540
00541 e[0][k] = q1*(b[6][k]* 7.0 + b[5][k]* 6.0 + b[4][k]* 5.0 + b[3][k]* 4.0 + b[2][k]* 3.0 + b[1][k]*2.0 + b[0][k]);
00542 e[1][k] = q2*(b[6][k]*21.0 + b[5][k]*15.0 + b[4][k]*10.0 + b[3][k]* 6.0 + b[2][k]* 3.0 + b[1][k]);
00543 e[2][k] = q3*(b[6][k]*35.0 + b[5][k]*20.0 + b[4][k]*10.0 + b[3][k]* 4.0 + b[2][k]);
00544 e[3][k] = q4*(b[6][k]*35.0 + b[5][k]*15.0 + b[4][k]* 5.0 + b[3][k]);
00545 e[4][k] = q5*(b[6][k]*21.0 + b[5][k]* 6.0 + b[4][k]);
00546 e[5][k] = q6*(b[6][k]* 7.0 + b[5][k]);
00547 e[6][k] = q7* b[6][k];
00548
00549 b[0][k] = e[0][k] + s[0];
00550 b[1][k] = e[1][k] + s[1];
00551 b[2][k] = e[2][k] + s[2];
00552 b[3][k] = e[3][k] + s[3];
00553 b[4][k] = e[4][k] + s[4];
00554 b[5][k] = e[5][k] + s[5];
00555 b[6][k] = e[6][k] + s[6];
00556
00557 }
00558
00559
00560
00561 }
00562
00563 }