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_orbit_gsl.h"
00026
00027 #ifdef HAVE_CONFIG_H
00028 #include <config.h>
00029 #endif // HAVE_CONFIG_H
00030
00031 #ifdef HAVE_MPI
00032 #include <mpi.h>
00033 #endif // HAVE_MPI
00034
00035 #include <gsl/gsl_roots.h>
00036 #include <gsl/gsl_errno.h>
00037 #include <gsl/gsl_siman.h>
00038 #include <gsl/gsl_multimin.h>
00039 #include <gsl/gsl_rng.h>
00040 #include <gsl/gsl_multifit_nlin.h>
00041 #include <gsl/gsl_blas.h>
00042 #include <gsl/gsl_diff.h>
00043 #include <gsl/gsl_deriv.h>
00044 #include <gsl/gsl_linalg.h>
00045 #include <gsl/gsl_cblas.h>
00046 #include <gsl/gsl_randist.h>
00047 #include <gsl/gsl_eigen.h>
00048
00049 #include "orsa_file.h"
00050 #include "orsa_error.h"
00051
00052 #include <sys/time.h>
00053 #include <time.h>
00054
00055 #include <string.h>
00056 #include <algorithm>
00057 #include <limits>
00058
00059 using namespace std;
00060
00061 namespace orsa {
00062
00063 void PreliminaryOrbit::ComputeRMS(const vector<Observation> &obs) {
00064 rms = RMS_residuals(obs,*this);
00065 }
00066
00067
00068
00069 double Weight(const Observation &obs1, const Observation &obs2, const double optimal_dt=FromUnits(1,DAY)) {
00070 double w;
00071 double dt = fabs(FromUnits(obs1.date.GetJulian()-obs2.date.GetJulian(),DAY));
00072
00073
00074
00075
00076
00077
00078
00079
00080 if (dt<optimal_dt) {
00081 w = dt/optimal_dt;
00082 } else {
00083 w = 1+optimal_dt/dt;
00084 }
00085
00086 return w;
00087 }
00088
00089 double Weight(vector<Observation> &obs, const double optimal_dt=FromUnits(1,DAY)) {
00090 if (obs.size() < 2) return 0;
00091 double w=0;
00092 sort(obs.begin(),obs.end());
00093 unsigned int k=1;
00094 while (k < obs.size()) {
00095 w += Weight(obs[k-1],obs[k],optimal_dt);
00096 ++k;
00097 }
00098 return w;
00099 }
00100
00101 class ThreeObservations : public vector<Observation> {
00102 public:
00103
00104
00105
00106
00107
00108
00109 inline bool operator < (const ThreeObservations &triobs) const {
00110 return (triobs.w < w);
00111 }
00112 private:
00113 double w;
00114 public:
00115 double Weight() const { return w; }
00116 public:
00117 void ComputeWeight(const double optimal_dt=FromUnits(1,DAY)) {
00118 w = orsa::Weight(*this,optimal_dt);
00119 }
00120 };
00121
00122 void BuildObservationTriplets(const vector<Observation> & obs, vector<ThreeObservations> &triplets, const double optimal_dt=FromUnits(1,DAY)) {
00123
00124 triplets.clear();
00125 unsigned int l,m,n;
00126 const unsigned int s = obs.size();
00127
00128 const double obs_delay_max = FromUnits(180,DAY);
00129
00130 const double obs_delay_ration_max = 10.0;
00131
00132 Observation ol,om,on;
00133 double dt12, dt23, dt_ratio;
00134 ThreeObservations triobs; triobs.resize(3);
00135 l=0;
00136 while (l<s) {
00137 ol=obs[l];
00138 triobs[0] = ol;
00139 m=l+1;
00140 while (m<s) {
00141 om=obs[m];
00142 triobs[1] = om;
00143 n=m+1;
00144 while (n<s) {
00145 on=obs[n];
00146 triobs[2] = on;
00147
00148 triobs.ComputeWeight(optimal_dt);
00149
00150 dt12 = FromUnits(om.date.GetJulian()-ol.date.GetJulian(),DAY);
00151 dt23 = FromUnits(on.date.GetJulian()-om.date.GetJulian(),DAY);
00152
00153 if (dt12>dt23)
00154 dt_ratio = dt12/dt23;
00155 else
00156 dt_ratio = dt23/dt12;
00157
00158 if ((dt12<obs_delay_max) && (dt23<obs_delay_max) && (dt_ratio<obs_delay_ration_max)) triplets.push_back(triobs);
00159
00160
00161 std::cerr << l << "-" << m << "-" << n << std::endl;
00162
00163 ++n;
00164 }
00165 ++m;
00166 }
00167 ++l;
00168 }
00169
00170 sort(triplets.begin(),triplets.end());
00171 }
00172
00173
00174
00175 class MOID_parameters {
00176 public:
00177 Orbit *o1, *o2;
00178 };
00179
00180 double MOID_f (const gsl_vector *v, void *params) {
00181
00182 double M1 = gsl_vector_get(v,0);
00183 double M2 = gsl_vector_get(v,1);
00184
00185 MOID_parameters *p = (MOID_parameters*) params;
00186
00187 p->o1->M = M1;
00188 p->o2->M = M2;
00189
00190 Vector r1,r2,v1,v2;
00191
00192 p->o1->RelativePosVel(r1,v1);
00193 p->o2->RelativePosVel(r2,v2);
00194
00195 return (r1-r2).Length();
00196 }
00197
00198 class MOID_solution {
00199
00200 public:
00201 MOID_solution() { active=false; }
00202
00203 public:
00204 void Insert(const double moid, const double M1, const double M2) {
00205 if ((!active) || ((active) && (moid < _moid))) {
00206 _moid = moid;
00207 _M1 = M1;
00208 _M2 = M2;
00209 active = true;
00210 }
00211 }
00212
00213 public:
00214 double M1() const { return _M1; }
00215 double M2() const { return _M2; }
00216 double moid() const { return _moid; }
00217
00218 private:
00219 double _moid,_M1,_M2;
00220 bool active;
00221 };
00222
00223 double MOID(const Orbit &o1_in, const Orbit &o2_in, Vector &r1, Vector &r2) {
00224
00225 Orbit o1(o1_in);
00226 Orbit o2(o2_in);
00227
00228 MOID_parameters par;
00229 par.o1 = &o1;
00230 par.o2 = &o2;
00231
00232 gsl_multimin_fminimizer *s = gsl_multimin_fminimizer_alloc(gsl_multimin_fminimizer_nmsimplex,2);
00233
00234 gsl_multimin_function moid_function;
00235
00236 moid_function.f = &MOID_f;
00237 moid_function.n = 2;
00238 moid_function.params = ∥
00239
00240 gsl_vector *x = gsl_vector_alloc(2);
00241 gsl_vector *step_size = gsl_vector_alloc(2);
00242
00243
00244
00245
00246 MOID_solution best_solution;
00247
00248 int gsl_solutions_iter=0,max_gsl_solutions_iter=7;
00249 while (gsl_solutions_iter<max_gsl_solutions_iter) {
00250
00251
00252
00253
00254
00255
00256
00257 gsl_vector_set(x,0,fmod(10*twopi+(twopi/(2*max_gsl_solutions_iter))*gsl_solutions_iter -o1.omega_node-o1.omega_pericenter,twopi));
00258 gsl_vector_set(x,1,fmod(10*twopi+(twopi/(2*max_gsl_solutions_iter))*gsl_solutions_iter+pi-o2.omega_node-o2.omega_pericenter,twopi));
00259
00260 gsl_vector_set(step_size,0,pi);
00261 gsl_vector_set(step_size,1,pi);
00262
00263 gsl_multimin_fminimizer_set (s, &moid_function, x, step_size);
00264
00265 size_t iter = 0;
00266 int status;
00267 do {
00268
00269 iter++;
00270
00271
00272 status = gsl_multimin_fminimizer_iterate(s);
00273
00274
00275 status = gsl_multimin_test_size(gsl_multimin_fminimizer_size(s),1.0e-6);
00276
00277
00278
00279
00280
00281
00282
00283
00284
00285
00286 } while (status == GSL_CONTINUE && iter < 200);
00287
00288
00289
00290
00291
00292 if (status == GSL_SUCCESS) {
00293 best_solution.Insert(gsl_multimin_fminimizer_minimum(s),gsl_vector_get(s->x,0),gsl_vector_get(s->x,1));
00294 }
00295
00296 ++gsl_solutions_iter;
00297 }
00298
00299 const double moid = best_solution.moid();
00300 o1.M = best_solution.M1();
00301 o2.M = best_solution.M2();
00302 Vector v1, v2;
00303
00304 o1.RelativePosVel(r1,v1);
00305 o2.RelativePosVel(r2,v2);
00306
00307
00308
00309
00310
00311
00312
00313
00314
00315 gsl_multimin_fminimizer_free(s);
00316 gsl_vector_free(x);
00317 gsl_vector_free(step_size);
00318
00319
00320
00321
00322 if (0) {
00323
00324 Orbit o1(o1_in);
00325 Orbit o2(o2_in);
00326
00327
00328
00329 struct timeval tv;
00330 struct timezone tz;
00331 gettimeofday(&tv,&tz);
00332 const int random_seed = tv.tv_usec;
00333 gsl_rng * rnd = gsl_rng_alloc(gsl_rng_gfsr4);
00334 gsl_rng_set(rnd,random_seed);
00335 int j,k;
00336 Vector r1,v1,r2,v2;
00337 double d, min_d=moid;
00338
00339
00340
00341
00342
00343
00344
00345
00346
00347
00348
00349
00350
00351
00352
00353
00354
00355
00356
00357
00358
00359 j=0;
00360 while (j < 50) {
00361 o1.M = twopi*gsl_rng_uniform(rnd);
00362 o1.RelativePosVel(r1,v1);
00363 k=0;
00364 while (k < 50) {
00365
00366 o2.M = twopi*gsl_rng_uniform(rnd);
00367
00368 o2.RelativePosVel(r2,v2);
00369 d = (r1-r2).Length();
00370 if (d < min_d) {
00371
00372
00373
00374 std::cerr << "(R) ";
00375 min_d = d;
00376 }
00377 ++k;
00378 }
00379 ++j;
00380 }
00381
00382 if (min_d < moid) {
00383
00384 printf("\n WARNING: found another solution for the MOID: %.10f < %.10f delta: %.10f\n",min_d,moid,min_d-moid);
00385
00386 }
00387
00388 gsl_rng_free(rnd);
00389
00390 }
00391
00392 return moid;
00393 }
00394
00395
00396
00397
00398 class MOID2RB_parameters {
00399 public:
00400 Orbit *o1, *o2;
00401 Vector rb1, rb2;
00402 };
00403
00404 double MOID2RB_f (const gsl_vector *v, void *params) {
00405
00406 double M1 = gsl_vector_get(v,0);
00407 double M2 = gsl_vector_get(v,1);
00408
00409 MOID2RB_parameters *p = (MOID2RB_parameters*) params;
00410
00411 p->o1->M = M1;
00412 p->o2->M = M2;
00413
00414 Vector r1,r2,v1,v2;
00415
00416 p->o1->RelativePosVel(r1,v1);
00417 p->o2->RelativePosVel(r2,v2);
00418
00419 return ((p->rb1+r1)-(p->rb2+r2)).Length();
00420 }
00421
00422 class MOID2RB_solution {
00423
00424 public:
00425 MOID2RB_solution() { active=false; }
00426
00427 public:
00428 void Insert(const double moid2rb, const double M1, const double M2) {
00429 if ((!active) || ((active) && (moid2rb < _moid2rb))) {
00430 _moid2rb = moid2rb;
00431 _M1 = M1;
00432 _M2 = M2;
00433 active = true;
00434 }
00435 }
00436
00437 public:
00438 double M1() const { return _M1; }
00439 double M2() const { return _M2; }
00440 double moid2rb() const { return _moid2rb; }
00441
00442 private:
00443 double _moid2rb,_M1,_M2;
00444 bool active;
00445 };
00446
00447 double MOID2RB(const Vector &rb1, const Vector &rb2, const Orbit &o1_in, const Orbit &o2_in, Vector &r1, Vector &r2) {
00448
00449 Orbit o1(o1_in);
00450 Orbit o2(o2_in);
00451
00452 MOID2RB_parameters par;
00453 par.o1 = &o1;
00454 par.o2 = &o2;
00455 par.rb1 = rb1;
00456 par.rb2 = rb2;
00457
00458 gsl_multimin_fminimizer *s = gsl_multimin_fminimizer_alloc(gsl_multimin_fminimizer_nmsimplex,2);
00459
00460 gsl_multimin_function moid2rb_function;
00461
00462 moid2rb_function.f = &MOID2RB_f;
00463 moid2rb_function.n = 2;
00464 moid2rb_function.params = ∥
00465
00466 gsl_vector *x = gsl_vector_alloc(2);
00467 gsl_vector *step_size = gsl_vector_alloc(2);
00468
00469
00470
00471
00472 MOID2RB_solution best_solution;
00473
00474 int gsl_solutions_iter=0,max_gsl_solutions_iter=7;
00475 while (gsl_solutions_iter<max_gsl_solutions_iter) {
00476
00477 gsl_vector_set(x,0,fmod(10*twopi+(twopi/(2*max_gsl_solutions_iter))*gsl_solutions_iter -o1.omega_node-o1.omega_pericenter,twopi));
00478 gsl_vector_set(x,1,fmod(10*twopi+(twopi/(2*max_gsl_solutions_iter))*gsl_solutions_iter+pi-o2.omega_node-o2.omega_pericenter,twopi));
00479
00480 gsl_vector_set(step_size,0,pi);
00481 gsl_vector_set(step_size,1,pi);
00482
00483 gsl_multimin_fminimizer_set (s, &moid2rb_function, x, step_size);
00484
00485 size_t iter = 0;
00486 int status;
00487 do {
00488
00489 iter++;
00490
00491
00492 status = gsl_multimin_fminimizer_iterate(s);
00493
00494
00495 status = gsl_multimin_test_size(gsl_multimin_fminimizer_size(s),1.0e-6);
00496
00497
00498
00499
00500
00501
00502
00503
00504
00505
00506 } while (status == GSL_CONTINUE && iter < 200);
00507
00508
00509
00510
00511
00512 if (status == GSL_SUCCESS) {
00513 best_solution.Insert(gsl_multimin_fminimizer_minimum(s),gsl_vector_get(s->x,0),gsl_vector_get(s->x,1));
00514 }
00515
00516 ++gsl_solutions_iter;
00517 }
00518
00519 const double moid2rb = best_solution.moid2rb();
00520 o1.M = best_solution.M1();
00521 o2.M = best_solution.M2();
00522 Vector v1, v2;
00523
00524 o1.RelativePosVel(r1,v1);
00525 o2.RelativePosVel(r2,v2);
00526
00527
00528
00529
00530
00531
00532
00533
00534
00535
00536
00537
00538 gsl_multimin_fminimizer_free(s);
00539 gsl_vector_free(x);
00540 gsl_vector_free(step_size);
00541
00542 return moid2rb;
00543 }
00544
00545
00546
00547
00548 #define N_TRIES 10
00549
00550
00551 #define ITERS_FIXED_T 3
00552
00553
00554 #define STEP_SIZE 1.0
00555
00556
00557 #define K 1.0e5
00558
00559
00560 #define T_INITIAL 0.01
00561
00562
00563 #define MU_T 1.005
00564 #define T_MIN 1.0e-6
00565
00566 const gsl_siman_params_t params = {N_TRIES, ITERS_FIXED_T, STEP_SIZE, K, T_INITIAL, MU_T, T_MIN};
00567
00568 struct data {
00569 OrbitWithEpoch *orbit;
00570 vector<Observation> *obs;
00571 };
00572
00573 double E1(void *p) {
00574 data *d = (data*) p;
00575 return (RMS_residuals(*d->obs,*d->orbit));
00576 }
00577
00578
00579 const double d_a = 0.1;
00580 const double d_e = 0.01;
00581 const double d_i = 0.1*(pi/180);
00582 const double d_omega_node = 1.0*(pi/180);
00583 const double d_omega_pericenter = 1.0*(pi/180);
00584 const double d_M = 5.0*(pi/180);
00585
00586 const double c_a = secure_pow(1.0/d_a, 2);
00587 const double c_e = secure_pow(1.0/d_e, 2);
00588 const double c_i = secure_pow(1.0/d_i, 2);
00589 const double c_omega_node = secure_pow(1.0/d_omega_node, 2);
00590 const double c_omega_pericenter = secure_pow(1.0/d_omega_pericenter, 2);
00591 const double c_M = secure_pow(1.0/d_M, 2);
00592
00593 double M1(void *p1, void *p2) {
00594 data *d1 = (data*) p1;
00595 data *d2 = (data*) p2;
00596
00597 double dist = sqrt( c_a * secure_pow( d1->orbit->a - d2->orbit->a, 2) +
00598 c_e * secure_pow( d1->orbit->e - d2->orbit->e, 2) +
00599 c_i * secure_pow( d1->orbit->i - d2->orbit->i, 2) +
00600 c_omega_node * secure_pow( d1->orbit->omega_node - d2->orbit->omega_node, 2) +
00601 c_omega_pericenter * secure_pow( d1->orbit->omega_pericenter - d2->orbit->omega_pericenter,2) +
00602 c_M * secure_pow( d1->orbit->M - d2->orbit->M, 2) ) / sqrt(6.0);
00603
00604 std::cerr << "M1: " << dist << endl;
00605
00606 return dist;
00607 }
00608
00609 void S1(const gsl_rng *r, void *p, double step_size) {
00610
00611
00612
00613
00614
00615
00616 data *d = (data*) p;
00617
00618
00619
00620
00621
00622
00623
00624
00625
00626
00627
00628
00629
00630
00631
00632
00633
00634
00635
00636
00637
00638 d->orbit->a += step_size*(gsl_rng_uniform(r)-0.5)*d_a;
00639 d->orbit->e += step_size*(gsl_rng_uniform(r)-0.5)*d_e;
00640 d->orbit->i += step_size*(gsl_rng_uniform(r)-0.5)*d_i;
00641 d->orbit->omega_node += step_size*(gsl_rng_uniform(r)-0.5)*d_omega_node;
00642 d->orbit->omega_pericenter += step_size*(gsl_rng_uniform(r)-0.5)*d_omega_pericenter;
00643 d->orbit->M += step_size*(gsl_rng_uniform(r)-0.5)*d_M;
00644
00645
00646 while (d->orbit->a < 0) d->orbit->a += 0.1*step_size*gsl_rng_uniform(r)*d_a;
00647 while (d->orbit->e < 0) d->orbit->e += 0.1*step_size*gsl_rng_uniform(r)*d_e;
00648
00649 do { d->orbit->i = fmod(d->orbit->i+pi,pi); } while (d->orbit->i < 0);
00650 do { d->orbit->omega_node = fmod(d->orbit->omega_node+twopi,twopi); } while (d->orbit->omega_node < 0);
00651 do { d->orbit->omega_pericenter = fmod(d->orbit->omega_pericenter+twopi,twopi); } while (d->orbit->omega_pericenter < 0);
00652 do { d->orbit->M = fmod(d->orbit->M+twopi,twopi); } while (d->orbit->M < 0);
00653
00654
00655
00656
00657
00658
00659 }
00660
00661
00662
00663
00664
00665
00666
00667
00668
00669
00670
00671
00672
00673
00674
00675
00676
00677
00678
00679
00680
00681
00682
00683
00684
00685
00686
00687
00688
00689
00690
00691
00692
00693
00694
00695
00696
00697
00698
00699
00700
00701
00702
00703
00704
00705
00706
00707
00708
00709
00710
00711
00712
00713
00714
00715
00716
00717
00718
00719
00720
00721
00722
00723
00724
00725
00726
00727
00728
00729
00730
00731
00732
00733
00734
00735
00736
00737
00738
00739
00740
00741
00742
00743
00744
00745
00746
00747
00748
00749
00750
00751
00752
00753
00754
00755
00756
00757
00758
00759
00760
00761
00762
00763
00764
00765
00766
00767
00768
00769
00770
00771
00772
00773
00774
00775
00776
00777
00778
00779
00780
00781
00782
00783
00784
00785
00786
00787
00788
00789
00790
00791
00792
00793
00794
00795
00796
00797
00798
00799
00800
00801
00802
00803
00804
00805
00806 class par_class {
00807 public:
00808 vector<Observation> * obs;
00809 public:
00810
00811
00812
00813 Date orbit_epoch;
00814
00815
00816
00817 gsl_matrix * covar;
00818 bool use_covar;
00819 };
00820
00821 class least_sq_diff_par_class {
00822 public:
00823 OrbitWithEpoch orbit;
00824 Observation obs;
00825 int var_index;
00826 };
00827
00828 int least_sq_f (const gsl_vector * v, void * params, gsl_vector * f) {
00829
00830 OrbitWithEpoch orbit;
00831
00832 orbit.a = gsl_vector_get(v,0);
00833 orbit.e = gsl_vector_get(v,1);
00834 orbit.i = gsl_vector_get(v,2);
00835 orbit.omega_node = gsl_vector_get(v,3);
00836 orbit.omega_pericenter = gsl_vector_get(v,4);
00837 orbit.M = gsl_vector_get(v,5);
00838
00839 {
00840 ORSA_DEBUG(
00841 "least_sq_f():\n"
00842 "orbit.a = %g\n"
00843 "orbit.e = %g\n"
00844 "orbit.i = %g\n",
00845 orbit.a,orbit.e,orbit.i*(180/pi));
00846 }
00847
00848 orbit.mu = GetG()*GetMSun();
00849
00850 par_class * par = (par_class *) params;
00851
00852 vector<Observation> * obs = par->obs;
00853
00854 orbit.epoch = par->orbit_epoch;
00855
00856 OptimizedOrbitPositions opt(orbit);
00857
00858 size_t i;
00859 double arcsec;
00860 for (i=0;i<obs->size();++i) {
00861 arcsec = opt.PropagatedSky_J2000((*obs)[i].date,(*obs)[i].obscode).delta_arcsec((*obs)[i]);
00862 gsl_vector_set(f,i,arcsec);
00863
00864 ORSA_ERROR("f[%02i] = %g",i,arcsec);
00865 }
00866
00867 return GSL_SUCCESS;
00868 }
00869
00870 double least_sq_diff_f(double x, void * params) {
00871
00872 least_sq_diff_par_class * diff_par = (least_sq_diff_par_class *) params;
00873
00874 OrbitWithEpoch orbit(diff_par->orbit);
00875
00876 switch(diff_par->var_index) {
00877 case 0: orbit.a = x; break;
00878 case 1: orbit.e = x; break;
00879 case 2: orbit.i = x; break;
00880 case 3: orbit.omega_node = x; break;
00881 case 4: orbit.omega_pericenter = x; break;
00882 case 5: orbit.M = x; break;
00883 }
00884
00885
00886
00887
00888
00889 return (PropagatedSky_J2000(orbit,diff_par->obs.date,diff_par->obs.obscode).delta_arcsec(diff_par->obs));
00890 }
00891
00892 int least_sq_df(const gsl_vector * v, void * params, gsl_matrix * J) {
00893
00894 OrbitWithEpoch orbit;
00895
00896 orbit.a = gsl_vector_get(v,0);
00897 orbit.e = gsl_vector_get(v,1);
00898 orbit.i = gsl_vector_get(v,2);
00899 orbit.omega_node = gsl_vector_get(v,3);
00900 orbit.omega_pericenter = gsl_vector_get(v,4);
00901 orbit.M = gsl_vector_get(v,5);
00902
00903 orbit.mu = GetG()*GetMSun();
00904
00905 par_class * par = (par_class*) params;
00906
00907 vector<Observation> * obs = par->obs;
00908
00909 orbit.epoch = par->orbit_epoch;
00910
00911 least_sq_diff_par_class diff_par;
00912
00913 diff_par.orbit = orbit;
00914
00915 gsl_function diff_f;
00916 double result, abserr;
00917
00918 diff_f.function = &least_sq_diff_f;
00919 diff_f.params = &diff_par;
00920
00921 size_t i,j;
00922 for (i=0;i<obs->size();++i) {
00923 diff_par.obs = (*obs)[i];
00924 for (j=0;j<6;++j) {
00925 diff_par.var_index = j;
00926
00927
00928
00929
00930
00931
00932
00933
00934
00935
00936
00937
00938
00939
00940
00941
00942
00943
00944
00945
00946
00947
00948
00949
00950
00951
00952
00953
00954
00955
00956
00957
00958
00959
00960
00961
00962
00963
00964
00965
00966
00967
00968
00969
00970
00971
00972
00973
00974
00975
00976
00977
00978
00979
00980
00981
00982
00983
00984
00985
00986
00987
00988
00989
00990
00991
00992
00993
00994
00995
00996
00997
00998
00999
01000
01001
01002 gsl_deriv_central(&diff_f, gsl_vector_get(v,j), gsl_vector_get(v,j)*1.0e4*std::numeric_limits<double>::epsilon(), &result, &abserr);
01003 gsl_matrix_set (J, i, j, result);
01004
01005 fprintf(stderr,"[lim]diff[%03i][%i] = %20f +/- %20f at %20.12f\n",
01006 i,j,result,abserr,gsl_vector_get(v,j));
01007
01008
01009
01010 }
01011 }
01012
01013 return GSL_SUCCESS;
01014 }
01015
01016 int least_sq_fdf (const gsl_vector *v, void *params, gsl_vector *f, gsl_matrix *J) {
01017 least_sq_f (v,params,f);
01018 least_sq_df(v,params,J);
01019 return GSL_SUCCESS;
01020 }
01021
01022
01023 void OrbitDifferentialCorrectionsLeastSquares(OrbitWithCovarianceMatrixGSL &orbit, const vector<Observation> &obs) {
01024
01025 cerr << "inside odcls..." << endl;
01026
01027 if (obs.size() < 6) {
01028 cerr << "at least 6 observations are needed for OrbitDifferentialCorrectionsLeastSquares..." << endl;
01029 return;
01030 }
01031
01032 par_class par;
01033
01034 par.orbit_epoch = orbit.epoch;
01035
01036 par.obs = const_cast< vector<Observation>* > (&obs);
01037
01038 par.covar = gsl_matrix_alloc(6,6);
01039 par.use_covar = false;
01040
01041
01042
01043
01044
01045
01046
01047
01048
01049
01050
01051
01052 gsl_multifit_fdfsolver * s = gsl_multifit_fdfsolver_alloc(gsl_multifit_fdfsolver_lmsder, obs.size(), 6);
01053
01054
01055 gsl_multifit_function_fdf least_sq_function;
01056
01057 least_sq_function.f = &least_sq_f;
01058 least_sq_function.df = &least_sq_df;
01059 least_sq_function.fdf = &least_sq_fdf;
01060 least_sq_function.n = obs.size();
01061 least_sq_function.p = 6;
01062 least_sq_function.params = ∥
01063
01064 gsl_vector * x = gsl_vector_alloc(6);
01065
01066 gsl_vector_set(x, 0, orbit.a);
01067 gsl_vector_set(x, 1, orbit.e);
01068 gsl_vector_set(x, 2, orbit.i);
01069 gsl_vector_set(x, 3, orbit.omega_node);
01070 gsl_vector_set(x, 4, orbit.omega_pericenter);
01071 gsl_vector_set(x, 5, orbit.M);
01072
01073 gsl_multifit_fdfsolver_set(s,&least_sq_function,x);
01074
01075 size_t iter = 0;
01076 int status;
01077 do {
01078
01079 ++iter;
01080
01081 status = gsl_multifit_fdfsolver_iterate(s);
01082
01083 printf("itaration status = %s\n",gsl_strerror(status));
01084
01085
01086
01087
01088
01089
01090
01091
01092
01093
01094
01095
01096
01097 status = gsl_multifit_test_delta (s->dx, s->x, 0.0, 1.0e-8);
01098
01099
01100
01101 printf("convergence status = %s\n",gsl_strerror(status));
01102
01103 {
01104 gsl_matrix * covar = gsl_matrix_alloc(6,6);
01105 gsl_multifit_covar(s->J,0.0,covar);
01106 printf ("more info:\n"
01107 "a = %12.6f +/- %12.6f\n"
01108 "e = %12.6f +/- %12.6f\n"
01109 "i = %12.6f +/- %12.6f\n"
01110 "\n",
01111 gsl_vector_get(s->x,0),sqrt(gsl_matrix_get(covar,0,0)),
01112 gsl_vector_get(s->x,1),sqrt(gsl_matrix_get(covar,1,1)),
01113 gsl_vector_get(s->x,2)*(180/pi),sqrt(gsl_matrix_get(covar,2,2))*(180/pi));
01114 }
01115
01116 gsl_multifit_covar(s->J,0.0,par.covar);
01117 par.use_covar = true;
01118
01119 } while ((status == GSL_CONTINUE) && (iter < 1024));
01120
01121 gsl_matrix * covar = gsl_matrix_alloc(6,6);
01122 gsl_multifit_covar(s->J,0.0,covar);
01123
01124
01125 #define FIT(i) gsl_vector_get(s->x, i)
01126 #define ERR(i) sqrt(gsl_matrix_get(covar,i,i))
01127
01128 printf("a = %12.6f +/- %12.6f\n", FIT(0), ERR(0));
01129 printf("e = %12.6f +/- %12.6f\n", FIT(1), ERR(1));
01130 printf("i = %12.6f +/- %12.6f\n", FIT(2)*(180/pi), ERR(2)*(180/pi));
01131 printf("node = %12.6f +/- %12.6f\n", FIT(3)*(180/pi), ERR(3)*(180/pi));
01132 printf("peri = %12.6f +/- %12.6f\n", FIT(4)*(180/pi), ERR(4)*(180/pi));
01133 printf("M = %12.6f +/- %12.6f\n", FIT(5)*(180/pi), ERR(5)*(180/pi));
01134
01135 printf ("status = %s\n", gsl_strerror (status));
01136
01137
01138 orbit.a = gsl_vector_get (s->x,0);
01139 orbit.e = gsl_vector_get (s->x,1);
01140 orbit.i = gsl_vector_get (s->x,2);
01141 orbit.omega_node = gsl_vector_get (s->x,3);
01142 orbit.omega_pericenter = gsl_vector_get (s->x,4);
01143 orbit.M = gsl_vector_get (s->x,5);
01144
01145
01146 {
01147 double covm[6][6];
01148 unsigned int i,j;
01149 for (i=0;i<6;++i) {
01150 for (j=0;j<6;++j) {
01151 covm[i][j] = gsl_matrix_get(covar,i,j);
01152 }
01153 }
01154 orbit.SetCovarianceMatrix((double**)covm,Osculating);
01155 }
01156
01157
01158 {
01159 OptimizedOrbitPositions opt(orbit);
01160 double arcsec, rms=0.0;
01161 for (unsigned int k=0;k<obs.size();++k) {
01162 arcsec = opt.PropagatedSky_J2000(obs[k].date,obs[k].obscode).delta_arcsec(obs[k]);
01163 rms += secure_pow(arcsec,2);
01164 fprintf(stderr,"delta_arcsec obs[%02i]: %g (%+.2f,%+.2f)\n",k,arcsec,
01165 (opt.PropagatedSky_J2000(obs[k].date,obs[k].obscode).ra().GetRad() -obs[k].ra.GetRad() )*(180/pi)*3600.0,
01166 (opt.PropagatedSky_J2000(obs[k].date,obs[k].obscode).dec().GetRad()-obs[k].dec.GetRad())*(180/pi)*3600.0);
01167 }
01168 rms = sqrt(rms/obs.size());
01169 fprintf(stderr,"\n RMS: %g\n\n",rms);
01170 }
01171
01172
01173 gsl_multifit_fdfsolver_free (s);
01174 gsl_vector_free (x);
01175
01176 cerr << "out of odcls..." << endl;
01177 }
01178
01179
01180
01181
01182
01183 class g_v_class {
01184 public:
01185 vector<Observation> obs;
01186 Vector r;
01187 };
01188
01189 int gauss_v_f (const gsl_vector *v, void * params, gsl_vector *f) {
01190
01191 Vector velocity;
01192
01193 velocity.x = gsl_vector_get(v,0);
01194 velocity.y = gsl_vector_get(v,1);
01195 velocity.z = gsl_vector_get(v,2);
01196
01197 g_v_class *par = (g_v_class*) params;
01198
01199 vector<Observation> &obs(par->obs);
01200
01201
01202
01203
01204 OrbitWithEpoch orbit;
01205 orbit.Compute(par->r,velocity,GetG()*GetMSun(),obs[0].date);
01206
01207
01208
01209 {
01210
01211
01212
01213
01214
01215 ORSA_DEBUG(
01216 "gauss_v_f():\n"
01217 "orbit.a = %g\n"
01218 "orbit.e = %g\n"
01219 "orbit.i = %g\n",
01220 orbit.a,orbit.e,orbit.i*(180/pi));
01221 }
01222
01223 OptimizedOrbitPositions opt(orbit);
01224
01225 size_t i;
01226 double arcsec;
01227 for (i=0;i<obs.size();++i) {
01228 arcsec = opt.PropagatedSky_J2000(obs[i].date,obs[i].obscode).delta_arcsec(obs[i]);
01229 gsl_vector_set(f,i,arcsec);
01230
01231 }
01232
01233 return GSL_SUCCESS;
01234 }
01235
01236 class gauss_v_diff_par_class {
01237 public:
01238 Vector r;
01239 Vector velocity;
01240 Observation obs;
01241 int var_index;
01242 Date orbit_epoch;
01243 };
01244
01245 double gauss_v_diff_f(double x, void *params) {
01246
01247
01248
01249 gauss_v_diff_par_class *diff_par = (gauss_v_diff_par_class*) params;
01250
01251
01252
01253 Vector velocity(diff_par->velocity);
01254
01255 switch(diff_par->var_index) {
01256 case 0: velocity.x = x; break;
01257 case 1: velocity.y = x; break;
01258 case 2: velocity.z = x; break;
01259 }
01260
01261 OrbitWithEpoch orbit;
01262 orbit.Compute(diff_par->r,velocity,GetG()*GetMSun(),diff_par->orbit_epoch);
01263
01264
01265 return (PropagatedSky_J2000(orbit,diff_par->obs.date,diff_par->obs.obscode).delta_arcsec(diff_par->obs));
01266 }
01267
01268 int gauss_v_df (const gsl_vector *v, void *params, gsl_matrix *J) {
01269
01270 Vector velocity;
01271
01272 velocity.x = gsl_vector_get(v,0);
01273 velocity.y = gsl_vector_get(v,1);
01274 velocity.z = gsl_vector_get(v,2);
01275
01276 g_v_class *par = (g_v_class*) params;
01277
01278 vector<Observation> &obs(par->obs);
01279
01280 OrbitWithEpoch orbit;
01281 orbit.Compute(par->r,velocity,GetG()*GetMSun(),obs[0].date);
01282
01283
01284
01285 gauss_v_diff_par_class diff_par;
01286
01287 diff_par.r = par->r;
01288 diff_par.velocity = velocity;
01289
01290 diff_par.orbit_epoch = orbit.epoch;
01291
01292 gsl_function diff_f;
01293 double result, abserr;
01294
01295 diff_f.function = &gauss_v_diff_f;
01296 diff_f.params = &diff_par;
01297
01298 size_t i,j;
01299 for (i=0;i<obs.size();++i) {
01300 diff_par.obs = obs[i];
01301 for (j=0;j<3;++j) {
01302 diff_par.var_index = j;
01303
01304 gsl_deriv_central(&diff_f, gsl_vector_get(v,j), 1.0e-9, &result, &abserr);
01305 gsl_matrix_set (J, i, j, result);
01306
01307 }
01308 }
01309
01310 return GSL_SUCCESS;
01311 }
01312
01313 int gauss_v_fdf (const gsl_vector *v, void *params, gsl_vector *f, gsl_matrix *J) {
01314 gauss_v_f (v,params,f);
01315 gauss_v_df(v,params,J);
01316 return GSL_SUCCESS;
01317 }
01318
01319
01320 void gauss_v(const Vector &r, Vector &v, const vector<Observation> &obs) {
01321
01322 if (obs.size() < 2) {
01323 ORSA_ERROR("gauss_v(...) needs at least two observations...");
01324 return;
01325 }
01326
01327
01328
01329 g_v_class g_v;
01330
01331 g_v.obs = obs;
01332 g_v.r = r;
01333
01334 gsl_multifit_fdfsolver *s = gsl_multifit_fdfsolver_alloc(gsl_multifit_fdfsolver_lmsder,obs.size(),3);
01335
01336 gsl_multifit_function_fdf gauss_v_function;
01337
01338 gauss_v_function.f = &gauss_v_f;
01339 gauss_v_function.df = &gauss_v_df;
01340 gauss_v_function.fdf = &gauss_v_fdf;
01341 gauss_v_function.n = obs.size();
01342 gauss_v_function.p = 3;
01343 gauss_v_function.params = &g_v;
01344
01345 gsl_vector *x = gsl_vector_alloc(3);
01346
01347 cerr << "..inside, Length(r): " << (r).Length() << endl;
01348
01349 gsl_vector_set(x,0,v.x);
01350 gsl_vector_set(x,1,v.y);
01351 gsl_vector_set(x,2,v.z);
01352
01353 gsl_multifit_fdfsolver_set(s,&gauss_v_function,x);
01354
01355 size_t iter = 0;
01356 int status;
01357 do {
01358
01359 ++iter;
01360
01361 status = gsl_multifit_fdfsolver_iterate (s);
01362
01363 printf ("itaration status = %s\n", gsl_strerror (status));
01364
01365
01366
01367
01368
01369
01370
01371
01372
01373
01374
01375 status = gsl_multifit_test_delta (s->dx, s->x, 0.0, 5.0e-2);
01376
01377 printf ("convergence status = %s\n", gsl_strerror (status));
01378
01379 } while ((status == GSL_CONTINUE) && (iter < 10));
01380
01381 printf ("status = %s\n", gsl_strerror (status));
01382
01383
01384 v.x = gsl_vector_get (s->x,0);
01385 v.y = gsl_vector_get (s->x,1);
01386 v.z = gsl_vector_get (s->x,2);
01387
01388
01389 gsl_multifit_fdfsolver_free (s);
01390 gsl_vector_free (x);
01391 }
01392
01393
01394
01395
01396 OrbitWithCovarianceMatrixGSL Compute(const vector<Observation> & obs) {
01397
01398
01399
01400 if (obs.size() < 3) {
01401 ORSA_ERROR("at least three observations are needed.");
01402 OrbitWithCovarianceMatrixGSL o;
01403 return o;
01404 }
01405
01406 if (universe->GetUniverseType() != Real) {
01407 ORSA_ERROR("trying to compute an orbit using the wrong universe type: return.");
01408 OrbitWithCovarianceMatrixGSL o;
01409 return o;
01410 }
01411
01412
01413
01414
01415
01416
01417
01418
01419
01420
01421
01422
01423
01424
01425
01426
01427
01428
01429
01430
01431
01432
01433
01434
01435
01436
01437
01438
01439
01440
01441
01442
01443
01444
01445
01446
01447
01448
01449
01450
01451 vector<PreliminaryOrbit> preliminary_orbits;
01452
01453 if (0) {
01454 vector<Observation> test_obs;
01455
01456 test_obs.push_back(obs[0]);
01457 test_obs.push_back(obs[obs.size()/2]);
01458 test_obs.push_back(obs[obs.size()-1]);
01459
01460 Compute_TestMethod(test_obs,preliminary_orbits);
01461 }
01462
01463 if (0) {
01464 Compute_TestMethod(obs,preliminary_orbits);
01465 }
01466
01467
01468 if (0) {
01469 vector<Observation> test_obs;
01470
01471 test_obs.push_back(obs[0]);
01472 test_obs.push_back(obs[obs.size()/2]);
01473 test_obs.push_back(obs[obs.size()-1]);
01474
01475 Compute_Gauss(test_obs,preliminary_orbits);
01476 }
01477
01478 if (1) {
01479 vector<ThreeObservations> triplet;
01480 triplet.clear();
01481
01482
01483 BuildObservationTriplets(obs, triplet, FromUnits(270,DAY));
01484
01485 cerr << "triplets: " << triplet.size() << endl;
01486
01487 unsigned int trial=0;
01488 while (trial<triplet.size()) {
01489
01490
01491 cerr << "delay 1->2: " << triplet[trial][1].date.GetJulian()-triplet[trial][0].date.GetJulian() << " days" << endl;
01492 cerr << "delay 2->3: " << triplet[trial][2].date.GetJulian()-triplet[trial][1].date.GetJulian() << " days" << endl;
01493
01494
01495 Compute_Gauss(triplet[trial],preliminary_orbits);
01496
01497
01498 ++trial;
01499
01500
01501
01502
01503 if (trial > 500) break;
01504
01505 cerr << "----trial: " << trial << endl;
01506 }
01507
01508 }
01509
01510
01511 {
01512 unsigned int r=0;
01513 while (r<preliminary_orbits.size()) {
01514 preliminary_orbits[r].ComputeRMS(obs);
01515 printf("%20.12f %20.12f %20.12f %20.12f\n",preliminary_orbits[r].a,preliminary_orbits[r].e,preliminary_orbits[r].i*(180/pi),preliminary_orbits[r].GetRMS());
01516 ++r;
01517 }
01518 }
01519
01520
01521 sort(preliminary_orbits.begin(),preliminary_orbits.end());
01522
01523 {
01524 cerr << "total prelim. orbits: " << preliminary_orbits.size() << endl;
01525 unsigned int r=0;
01526 while (r<preliminary_orbits.size()) {
01527 printf("%20.12f %20.12f %20.12f %20.12f\n",preliminary_orbits[r].a,preliminary_orbits[r].e,preliminary_orbits[r].i*(180/pi),preliminary_orbits[r].GetRMS());
01528 ++r;
01529 }
01530 }
01531
01532
01533 OrbitDifferentialCorrectionsLeastSquares(preliminary_orbits[0],obs);
01534
01535 if (0) {
01536 unsigned int r=0;
01537
01538 while (r<preliminary_orbits.size()) {
01539
01540
01541
01542
01543
01544
01545 OrbitDifferentialCorrectionsLeastSquares(preliminary_orbits[r],obs);
01546
01547
01548 if (0) {
01549
01550 OptimizedOrbitPositions opt(preliminary_orbits[r]);
01551
01552 vector<Observation> good_obs;
01553 unsigned int k;
01554 for (k=0;k<obs.size();++k) {
01555
01556
01557 if (opt.PropagatedSky_J2000(obs[k].date,obs[k].obscode).delta_arcsec(obs[k]) < 300.0) {
01558 good_obs.push_back(obs[k]);
01559 }
01560 if (good_obs.size()>12) break;
01561 }
01562
01563 cerr << "good obs.: " << good_obs.size() << endl;
01564
01565
01566 if (good_obs.size() > 3) OrbitDifferentialCorrectionsLeastSquares(preliminary_orbits[r],good_obs);
01567 }
01568
01569 ++r;
01570 }
01571
01572 }
01573
01574
01575
01576 return preliminary_orbits[0];
01577 }
01578
01579
01580 struct poly_8_params {
01581 double coeff_6, coeff_3, coeff_0;
01582 };
01583
01584 double poly_8 (double x, void *params);
01585 double poly_8_df (double x, void *params);
01586 void poly_8_fdf (double x, void *params, double *y, double *dy);
01587
01588 double poly_8 (double x, void *params) {
01589 struct poly_8_params *p = (struct poly_8_params *) params;
01590 return (secure_pow(x,8) - p->coeff_6*secure_pow(x,6) - p->coeff_3*secure_pow(x,3) - p->coeff_0);
01591 }
01592
01593 double poly_8_df (double x, void *params) {
01594 struct poly_8_params *p = (struct poly_8_params *) params;
01595 return (8*secure_pow(x,7) - 6*p->coeff_6*secure_pow(x,5) - 3*p->coeff_3*secure_pow(x,2));
01596 }
01597
01598 void poly_8_fdf (double x, void *params, double *y, double *dy) {
01599 struct poly_8_params *p = (struct poly_8_params *) params;
01600 *y = (secure_pow(x,8) - p->coeff_6*secure_pow(x,6) - p->coeff_3*secure_pow(x,3) - p->coeff_0);
01601 *dy = (8*secure_pow(x,7) - 6*p->coeff_6*secure_pow(x,5) - 3*p->coeff_3*secure_pow(x,2));
01602 }
01603
01604 class poly_8_solution {
01605 public:
01606 double value, error;
01607 };
01608
01609 void poly_8_gsl_solve(poly_8_params ¶ms, vector<poly_8_solution> &solutions) {
01610
01611 const int max_iter = 100;
01612 const double x_start = FromUnits(0.0,AU);
01613 const double x_incr = FromUnits(0.2,AU);
01614
01615 const double nominal_relative_accuracy = 1.0e-5;
01616
01617 solutions.clear();
01618
01619 poly_8_solution tmp_solution;
01620
01621 double x, x0;
01622 int gsl_status;
01623
01624
01625 gsl_root_fdfsolver *s;
01626 gsl_function_fdf FDF;
01627
01628
01629
01630
01631
01632
01633
01634 FDF.f = &poly_8;
01635 FDF.df = &poly_8_df;
01636 FDF.fdf = &poly_8_fdf;
01637 FDF.params = ¶ms;
01638
01639
01640
01641 s = gsl_root_fdfsolver_alloc(gsl_root_fdfsolver_steffenson);
01642
01643 int iter = 0;
01644 while (iter<max_iter) {
01645
01646 ++iter;
01647 x = x_start+iter*x_incr;
01648 gsl_root_fdfsolver_set (s, &FDF, x);
01649
01650 int iter_gsl=0;
01651 const int max_iter_gsl = 100;
01652 do {
01653 ++iter_gsl;
01654 gsl_status = gsl_root_fdfsolver_iterate(s);
01655 x0 = x;
01656 x = gsl_root_fdfsolver_root(s);
01657 gsl_status = gsl_root_test_delta (x, x0, nominal_relative_accuracy, 0);
01658
01659
01660 } while ((gsl_status == GSL_CONTINUE) && (iter_gsl < max_iter_gsl));
01661
01662 if (gsl_status == GSL_SUCCESS) {
01663 tmp_solution.value = x;
01664
01665 tmp_solution.error = nominal_relative_accuracy;
01666
01667 unsigned int k=0;
01668 bool duplicate=false;
01669 while (k<solutions.size()) {
01670 if (fabs(solutions[k].value-tmp_solution.value) < (solutions[k].error+tmp_solution.error)) {
01671 duplicate= true;
01672 break;
01673 }
01674 ++k;
01675 }
01676
01677 if (!duplicate) {
01678 solutions.push_back(tmp_solution);
01679 }
01680 }
01681 }
01682
01683 gsl_root_fdfsolver_free(s);
01684
01685 if (0) {
01686 cerr << "solutions found: " << solutions.size() << endl;
01687 unsigned int k=0;
01688 while (k<solutions.size()) {
01689 cerr << k << ": " << solutions[k].value << " accuracy: " << solutions[k].error << endl;
01690 ++k;
01691 }
01692 }
01693 }
01694
01695 void Compute_Gauss(const vector<Observation> & obs_in, vector<PreliminaryOrbit> & preliminary_orbits) {
01696
01697 cerr << "inside Compute_Gauss(...)" << endl;
01698
01699
01700
01701 PreliminaryOrbit orbit;
01702
01703 vector<Observation> obs(obs_in);
01704
01705 if (obs.size() != 3) {
01706 ORSA_ERROR("Compute_Gauss(): three observations needed.");
01707 return;
01708 }
01709
01710 unsigned int j;
01711
01712
01713
01714 sort(obs.begin(),obs.end());
01715
01716
01717
01718
01719
01720
01721
01722
01723
01724
01725 double tau[3];
01726 const double sqrt_GM = sqrt(GetG()*GetMSun());
01727
01728 tau[2] = sqrt_GM*FromUnits(obs[1].date.GetJulian()-obs[0].date.GetJulian(),DAY);
01729 tau[0] = sqrt_GM*FromUnits(obs[2].date.GetJulian()-obs[1].date.GetJulian(),DAY);
01730 tau[1] = sqrt_GM*FromUnits(obs[2].date.GetJulian()-obs[0].date.GetJulian(),DAY);
01731
01732
01733
01734
01735
01736
01737
01738
01739 Vector r;
01740 Vector R[3];
01741 {
01742
01743
01744
01745
01746
01747
01748
01749
01750
01751 for (j=0;j<3;++j) {
01752 R[j] = jpl_cache->GetJPLBody(EARTH,obs[j].date).position() - jpl_cache->GetJPLBody(SUN ,obs[j].date).position();
01753 }
01754 }
01755
01756
01757 {
01758
01759
01760
01761
01762
01763
01764 for (j=0;j<3;++j) {
01765 R[j] += location_file->ObsPos(obs[j].obscode,obs[j].date);
01766 }
01767 }
01768
01769 Vector u_rho[3];
01770 for (j=0;j<3;++j) {
01771 u_rho[j].x = cos(obs[j].dec) * cos(obs[j].ra);
01772 u_rho[j].y = cos(obs[j].dec) * sin(obs[j].ra);
01773 u_rho[j].z = sin(obs[j].dec);
01774
01775
01776
01777
01778
01779
01780
01781
01782
01783
01784
01785
01786
01787 if (universe->GetReferenceSystem() == ECLIPTIC) {
01788
01789 Angle obl = obleq_J2000();
01790 u_rho[j].rotate(0.0,-obl.GetRad(),0.0);
01791 }
01792
01793 }
01794
01795 Vector cross = Cross(u_rho[0],u_rho[2]);
01796
01797 const Vector f = cross.Normalized();
01798
01799 const double rho_1_f = u_rho[1]*f;
01800
01801 const double R_0_f = R[0]*f;
01802 const double R_1_f = R[1]*f;
01803 const double R_2_f = R[2]*f;
01804
01805 const double A = (tau[0]/tau[1]*R_0_f + tau[2]/tau[1]*R_2_f - R_1_f)/rho_1_f;
01806
01807 const double B = (tau[0]/tau[1]*(tau[1]*tau[1]-tau[0]*tau[0])*R_0_f + tau[2]/tau[1]*(tau[1]*tau[1]-tau[2]*tau[2])*R_2_f)/rho_1_f/6.0;
01808
01809 const double Xl_Ym_Zn = R[1]*u_rho[1];
01810
01811 poly_8_params params;
01812 params.coeff_6 = R[1].LengthSquared() + A*A + 2*A*Xl_Ym_Zn;
01813 params.coeff_3 = 2*A*B + 2*B*Xl_Ym_Zn;
01814 params.coeff_0 = B*B;
01815 vector<poly_8_solution> solutions;
01816 poly_8_gsl_solve(params,solutions);
01817
01818 if (solutions.size() > 0) {
01819
01820 Vector rho[3];
01821 Vector r[3];
01822 Vector v;
01823 double c[3];
01824
01825 double tmp_length;
01826 double tmp_value;
01827 unsigned int p;
01828 for (p=0;p<solutions.size();++p) {
01829
01830
01831
01832
01833 tmp_value = solutions[p].value;
01834 tmp_length = A + (B/(tmp_value*tmp_value*tmp_value));
01835
01836 if (tmp_length <= 0.0) {
01837
01838 continue;
01839 }
01840
01841 rho[1] = u_rho[1]*tmp_length;
01842
01843 r[1] = R[1] + rho[1];
01844
01845
01846 for (j=0;j<3;++j) {
01847
01848 c[j] = tau[j]/tau[1]*(1+(tau[1]*tau[1]-tau[j]*tau[j])/(6*secure_pow(r[1].Length(),3)));
01849
01850 }
01851
01852 {
01853
01854 const Vector v_k = rho[1] - (c[0]*R[0] + c[2]*R[2] - R[1]);
01855 const double k = v_k.Length();
01856
01857 const Vector u_k = v_k.Normalized();
01858
01859 const double s02 = u_rho[0]*u_rho[2];
01860 const double s0k = u_rho[0]*u_k;
01861 const double s2k = u_rho[2]*u_k;
01862
01863
01864
01865 tmp_length = (k*(s0k-s02*s2k)/(1-s02*s02))/c[0];
01866 if (tmp_length <= 0.0) {
01867
01868 continue;
01869 }
01870
01871 rho[0] = u_rho[0]*tmp_length;
01872
01873
01874
01875 tmp_length = (k*(s2k-s02*s0k)/(1-s02*s02))/c[2];
01876 if (tmp_length <= 0.0) {
01877
01878 continue;
01879 }
01880
01881 rho[2] = u_rho[2]*tmp_length;
01882
01883 r[0] = rho[0] + R[0];
01884 r[2] = rho[2] + R[2];
01885
01886 }
01887
01888 if (0) {
01889
01890 int iter_gibbs=0;
01891 double old_c[3];
01892 double delta_c = 1.0;
01893 while ((iter_gibbs<10) && (delta_c > 1.0e-6)) {
01894
01895
01896 double Gibbs_B[3];
01897 Gibbs_B[0] = (tau[1]*tau[2]-secure_pow(tau[0],2))/12;
01898 Gibbs_B[1] = (tau[0]*tau[2]-secure_pow(tau[1],2))/12;
01899 Gibbs_B[2] = (tau[0]*tau[1]-secure_pow(tau[2],2))/12;
01900 double Brm3[3];
01901 for (j=0;j<3;++j) {
01902 Brm3[j] = Gibbs_B[j]/secure_pow(r[j].Length(),3);
01903 }
01904 delta_c = 0.0;
01905 for (j=0;j<3;++j) {
01906 old_c[j] = c[j];
01907 c[j] = tau[j]/tau[1]*(1+Brm3[j])/(1-Brm3[1]);
01908 delta_c += secure_pow(c[j]-old_c[j],2);
01909
01910 }
01911 delta_c /= 3;
01912 delta_c = sqrt(delta_c);
01913
01914 {
01915 const Vector v_k = rho[1] - (c[0]*R[0] + c[2]*R[2] - R[1]);
01916 const double k = v_k.Length();
01917
01918 const Vector u_k = v_k.Normalized();
01919
01920 const double s02 = u_rho[0]*u_rho[2];
01921 const double s0k = u_rho[0]*u_k;
01922 const double s2k = u_rho[2]*u_k;
01923
01924 rho[0] = u_rho[0]*(k*(s0k-s02*s2k)/(1-secure_pow(s02,2)))/c[0];
01925 rho[2] = u_rho[2]*(k*(s2k-s02*s0k)/(1-secure_pow(s02,2)))/c[2];
01926
01927 r[0] = rho[0] + R[0];
01928 r[2] = rho[2] + R[2];
01929 }
01930
01931 ++iter_gibbs;
01932 }
01933
01934 }
01935
01936
01937
01938
01939
01940
01941
01942
01943 Vector v = (r[1]-r[0])/(FromUnits(obs[1].date.GetJulian()-obs[0].date.GetJulian(),DAY));
01944
01945
01946 r[0] += v*(r[0]-R[0]).Length()/GetC();
01947
01948
01949
01950 orbit.Compute(r[0],v,GetG()*GetMSun(),obs[0].date);
01951
01952
01953
01954
01955
01956
01957
01958 if (orbit.e < 1.0) {
01959
01960
01961
01962
01963
01964
01965
01966
01967
01968
01969 orbit.Compute(r[0],v,GetG()*GetMSun(),obs[0].date);
01970
01971
01972 cerr << " ---> orbit.a = " << orbit.a << endl;
01973 cerr << " ---> orbit.e = " << orbit.e << endl;
01974 cerr << " ---> orbit.i = " << orbit.i*(180/pi) << endl;
01975
01976
01977
01978
01979
01980
01981
01982
01983
01984
01985
01986
01987
01988
01989
01990
01991
01992
01993
01994
01995 fprintf(stderr,"limited RMS: %g\n",RMS_residuals(obs,orbit));
01996
01997 if (0) {
01998 UniverseTypeAwareTime utat;
01999 Sky sky;
02000 double hand_RMS=0;
02001 for (j=0;j<3;++j) {
02002
02003 sky.Compute_J2000(r[j]-R[j]);
02004 hand_RMS += ( secure_pow((obs[j].ra.GetRad()-sky.ra().GetRad())*cos(obs[j].dec.GetRad()),2) +
02005 secure_pow(obs[j].dec.GetRad()-sky.dec().GetRad(),2) );
02006 }
02007 hand_RMS /= 3.0;
02008 hand_RMS = sqrt(hand_RMS);
02009 hand_RMS *= (180/pi)*3600;
02010 fprintf(stderr,"RMS by hands: %g\n",hand_RMS);
02011
02012
02013 if (hand_RMS > 1.0) {
02014 cerr << "controlled exit..." << endl;
02015 exit(0);
02016 }
02017
02018 }
02019
02020 } else {
02021
02022 }
02023
02024
02025
02026
02027
02028
02029
02030
02031
02032 if (orbit.e < 1.0) preliminary_orbits.push_back(orbit);
02033 }
02034 }
02035
02036 cerr << "out of Compute_Gauss(...)" << endl;
02037 }
02038
02039
02040
02041
02042 void Compute_TestMethod(const vector<Observation> &obs_in, vector<PreliminaryOrbit> &preliminary_orbits) {
02043
02044 cerr << "inside Compute_TestMethod(...)" << endl;
02045
02046 PreliminaryOrbit orbit;
02047
02048 vector<Observation> obs(obs_in);
02049
02050 unsigned int j;
02051
02052 sort(obs.begin(),obs.end());
02053
02054
02055
02056
02057
02058
02059
02060
02061 const unsigned int size = obs.size();
02062
02063 Vector R[size];
02064 {
02065 Vector tmp_r, tmp_v;
02066 JPLFile jf(config->paths[JPL_EPHEM_FILE]->GetValue().c_str());
02067 for (j=0;j<size;++j) {
02068 jf.GetEph(obs[j].date,EARTH,SUN,tmp_r,tmp_v);
02069 R[j] = tmp_r;
02070 }
02071 }
02072
02073
02074 {
02075
02076
02077
02078
02079
02080
02081
02082 for (j=0;j<size;++j) {
02083 R[j] += location_file->ObsPos(obs[j].obscode,obs[j].date);
02084 }
02085 }
02086
02087 Vector u_rho[size];
02088 for (j=0;j<size;++j) {
02089 u_rho[j].x = cos(obs[j].dec) * cos(obs[j].ra);
02090 u_rho[j].y = cos(obs[j].dec) * sin(obs[j].ra);
02091 u_rho[j].z = sin(obs[j].dec);
02092
02093
02094
02095
02096
02097
02098
02099
02100
02101
02102
02103
02104
02105 if (universe->GetReferenceSystem() == ECLIPTIC) {
02106
02107 Angle obl = obleq_J2000();
02108 u_rho[j].rotate(0.0,-obl.GetRad(),0.0);
02109 }
02110
02111 }
02112
02113 Vector rho[size];
02114 Vector r[size];
02115 Vector v;
02116
02117
02118
02119
02120
02121
02122
02123
02124
02125
02126
02127 double tmp_rho = FromUnits(0.535,AU);
02128 while (tmp_rho < FromUnits(0.536,AU)) {
02129
02130 cerr << "[******] tmp_rho: " << tmp_rho << endl;
02131
02132 for (j=0;j<size;++j) {
02133 rho[j] = u_rho[j]*tmp_rho;
02134 r[j] = R[j] + rho[j];
02135 }
02136
02137 v = (r[1]-r[0])/(FromUnits(obs[0].date.GetJulian()-obs[1].date.GetJulian(),DAY));
02138
02139
02140 gauss_v(r[0],v,obs);
02141
02142 orbit.Compute(r[0],v,GetG()*GetMSun(),obs[0].date);
02143
02144
02145
02146 cerr << " ---> orbit.a = " << orbit.a << endl;
02147 cerr << " ---> orbit.e = " << orbit.e << endl;
02148 cerr << " ---> orbit.i = " << orbit.i*(180/pi) << endl;
02149
02150 if (orbit.e < 1.0) preliminary_orbits.push_back(orbit);
02151
02152
02153 tmp_rho += 0.005;
02154 }
02155
02156 }
02157
02158
02159
02160 OrbitWithCovarianceMatrixGSL::OrbitWithCovarianceMatrixGSL() : OrbitWithEpoch(), bool_have_covariance_matrix(false) {
02161
02162 }
02163
02164 OrbitWithCovarianceMatrixGSL::OrbitWithCovarianceMatrixGSL(Orbit &nominal_orbit) : OrbitWithEpoch(nominal_orbit), bool_have_covariance_matrix(false) {
02165
02166 }
02167
02168 OrbitWithCovarianceMatrixGSL::OrbitWithCovarianceMatrixGSL(Orbit &nominal_orbit, double **covariance_matrix, CovarianceMatrixElements base) : OrbitWithEpoch(nominal_orbit) {
02169 SetCovarianceMatrix(covariance_matrix,base);
02170 }
02171
02172 void OrbitWithCovarianceMatrixGSL::SetCovarianceMatrix(double **covariance_matrix, CovarianceMatrixElements base) {
02173 memcpy((void*)covm, (const void*)covariance_matrix, 6*6*sizeof(double));
02174 cov_base = base;
02175 bool_have_covariance_matrix = true;
02176 }
02177
02178 void OrbitWithCovarianceMatrixGSL::GetCovarianceMatrix(double **covariance_matrix, CovarianceMatrixElements &base) const {
02179 if (have_covariance_matrix()) {
02180 memcpy((void*)covariance_matrix, (const void*)covm, 6*6*sizeof(double));
02181 base = cov_base;
02182 } else {
02183 ORSA_ERROR("called method OrbitWithCovarianceMatrixGSL::GetCovarianceMatrix() from an orbit with undefined covariance matrix");
02184 }
02185 }
02186
02187
02188 void OrbitWithCovarianceMatrixGSL::GenerateUsingCholeskyDecomposition(vector<OrbitWithEpoch> & list, const int num, const int random_seed) const {
02189
02190 list.clear();
02191
02192 if (num < 1) return;
02193
02194 if (!have_covariance_matrix()) {
02195 ORSA_ERROR("called method OrbitWithCovarianceMatrixGSL::GenerateUsingCholeskyDecomposition() from an orbit with undefined covariance matrix.");
02196 return;
02197 }
02198
02199 gsl_vector * mu = gsl_vector_alloc(6);
02200 gsl_vector * x = gsl_vector_alloc(6);
02201 gsl_vector * y = gsl_vector_alloc(6);
02202
02203 switch (cov_base) {
02204 case Osculating: {
02205
02206 gsl_vector_set(mu,0,a);
02207 gsl_vector_set(mu,1,e);
02208 gsl_vector_set(mu,2,i);
02209 gsl_vector_set(mu,3,omega_node);
02210 gsl_vector_set(mu,4,omega_pericenter);
02211 gsl_vector_set(mu,5,M);
02212
02213 break;
02214 }
02215 case Equinoctal: {
02216
02217 gsl_vector_set(mu,0,a);
02218 gsl_vector_set(mu,1,e*sin(omega_node+omega_pericenter));
02219 gsl_vector_set(mu,2,e*cos(omega_node+omega_pericenter));
02220 gsl_vector_set(mu,3,tan(i/2)*sin(omega_node));
02221 gsl_vector_set(mu,4,tan(i/2)*cos(omega_node));
02222 gsl_vector_set(mu,5,fmod(omega_node+omega_pericenter+M,twopi));
02223
02224 break;
02225 }
02226 }
02227
02228 gsl_matrix * A = gsl_matrix_alloc(6,6);
02229
02230 int i,k;
02231 for (i=0;i<6;++i) {
02232 for (k=0;k<6;++k) {
02233 gsl_matrix_set(A,i,k,covm[i][k]);
02234 }
02235 }
02236
02237
02238 int decomp_status = gsl_linalg_cholesky_decomp(A);
02239
02240 if (decomp_status) {
02241
02242 ORSA_WARNING("Cholesky decomposition failed.");
02243
02244 gsl_vector_free(mu);
02245 gsl_vector_free(x);
02246 gsl_vector_free(y);
02247 gsl_matrix_free(A);
02248
02249 return;
02250 }
02251
02252
02253 for (i=0;i<6;++i) {
02254 for (k=i+1;k<6;++k) {
02255 gsl_matrix_set(A,i,k,0.0);
02256 }
02257 }
02258
02259
02260
02261
02262 OrbitWithEpoch gen_orb = (*this);
02263
02264 int generated = 0;
02265
02266
02267 gsl_rng * rnd = gsl_rng_alloc(gsl_rng_gfsr4);
02268 gsl_rng_set(rnd,random_seed);
02269
02270 switch (cov_base) {
02271 case Osculating: {
02272 while (1) {
02273
02274 for (i=0;i<6;++i) gsl_vector_set(x,i,gsl_ran_ugaussian(rnd));
02275
02276
02277 gsl_vector_memcpy(y,mu);
02278 gsl_blas_dgemv(CblasNoTrans,1.0,A,x,1.0,y);
02279
02280 ++generated;
02281
02282 gen_orb.a = gsl_vector_get (y,0);
02283 gen_orb.e = gsl_vector_get (y,1);
02284 gen_orb.i = gsl_vector_get (y,2);
02285 gen_orb.omega_node = gsl_vector_get (y,3);
02286 gen_orb.omega_pericenter = gsl_vector_get (y,4);
02287 gen_orb.M = gsl_vector_get (y,5);
02288
02289 list.push_back(gen_orb);
02290
02291 if (generated>=num) break;
02292 }
02293 break;
02294 }
02295 case Equinoctal: {
02296
02297 while (1) {
02298
02299 for (i=0;i<6;++i) gsl_vector_set(x,i,gsl_ran_ugaussian(rnd));
02300
02301
02302 gsl_vector_memcpy(y,mu);
02303 gsl_blas_dgemv(CblasNoTrans,1.0,A,x,1.0,y);
02304
02305 ++generated;
02306
02307 const double omega_tilde = secure_atan2(gsl_vector_get(y,1),gsl_vector_get(y,2));
02308
02309 gen_orb.a = gsl_vector_get(y,0);
02310 gen_orb.e = sqrt(gsl_vector_get(y,1)*gsl_vector_get(y,1)+gsl_vector_get(y,2)*gsl_vector_get(y,2));
02311 gen_orb.i = 2.0*atan(sqrt(gsl_vector_get(y,3)*gsl_vector_get(y,3)+gsl_vector_get(y,4)*gsl_vector_get(y,4)));
02312 gen_orb.omega_node = fmod(10.0*twopi+secure_atan2(gsl_vector_get(y,3),gsl_vector_get(y,4)),twopi);
02313 gen_orb.omega_pericenter = fmod(10.0*twopi+omega_tilde-omega_node,twopi);
02314 gen_orb.M = fmod(10.0*twopi+gsl_vector_get(y,5)-omega_tilde,twopi);
02315
02316 list.push_back(gen_orb);
02317
02318 if (generated>=num) break;
02319 }
02320 break;
02321 }
02322 }
02323
02324 gsl_vector_free(mu);
02325 gsl_vector_free(x);
02326 gsl_vector_free(y);
02327 gsl_matrix_free(A);
02328
02329
02330 gsl_rng_free(rnd);
02331
02332 }
02333
02334
02335
02336 void OrbitWithCovarianceMatrixGSL::GenerateUsingPrincipalAxisTransformation(vector<OrbitWithEpoch> & list, const int num, const int random_seed) const {
02337
02338 list.clear();
02339
02340 if (num < 1) return;
02341
02342 if (!have_covariance_matrix()) {
02343 ORSA_ERROR("called method OrbitWithCovarianceMatrixGSL::GenerateUsingPrincipalAxisTransformation() from an orbit with undefined covariance matrix.");
02344 return;
02345 }
02346
02347 gsl_vector * mu = gsl_vector_alloc(6);
02348 gsl_vector * x = gsl_vector_alloc(6);
02349 gsl_vector * y = gsl_vector_alloc(6);
02350
02351 switch (cov_base) {
02352 case Osculating: {
02353
02354 gsl_vector_set(mu,0,a);
02355 gsl_vector_set(mu,1,e);
02356 gsl_vector_set(mu,2,i);
02357 gsl_vector_set(mu,3,omega_node);
02358 gsl_vector_set(mu,4,omega_pericenter);
02359 gsl_vector_set(mu,5,M);
02360
02361 break;
02362 }
02363 case Equinoctal: {
02364
02365 gsl_vector_set(mu,0,a);
02366 gsl_vector_set(mu,1,e*sin(omega_node+omega_pericenter));
02367 gsl_vector_set(mu,2,e*cos(omega_node+omega_pericenter));
02368 gsl_vector_set(mu,3,tan(i/2)*sin(omega_node));
02369 gsl_vector_set(mu,4,tan(i/2)*cos(omega_node));
02370 gsl_vector_set(mu,5,fmod(omega_node+omega_pericenter+M,twopi));
02371
02372 break;
02373 }
02374 }
02375
02376 gsl_matrix * A = gsl_matrix_alloc(6,6);
02377
02378 for (unsigned int i=0;i<6;++i) {
02379 for (unsigned int k=0;k<6;++k) {
02380 gsl_matrix_set(A,i,k,covm[i][k]);
02381 }
02382 }
02383
02384
02385
02386
02387 gsl_permutation * p = gsl_permutation_alloc(6);
02388 gsl_matrix * LU = gsl_matrix_alloc(6,6);
02389 gsl_matrix_memcpy(LU,A);
02390 int s;
02391
02392
02393 gsl_linalg_LU_decomp(LU, p, &s);
02394
02395
02396 gsl_matrix * inv = gsl_matrix_alloc(6,6);
02397 gsl_linalg_LU_invert(LU, p, inv);
02398
02399
02400
02401
02402
02403 gsl_eigen_symmv_workspace * w = gsl_eigen_symmv_alloc(6);
02404 gsl_vector * eval = gsl_vector_alloc(6);
02405 gsl_matrix * evec = gsl_matrix_alloc(6,6);
02406
02407 gsl_eigen_symmv (A, eval, evec, w);
02408
02409
02410
02411 {
02412
02413 for (unsigned int i=0;i<6;++i) {
02414 cerr << "eval[" << i << "] = " << gsl_vector_get(eval,i) << endl;
02415 }
02416 }
02417
02418 OrbitWithEpoch gen_orb = (*this);
02419
02420 int generated = 0;
02421
02422
02423 gsl_rng * rnd = gsl_rng_alloc(gsl_rng_gfsr4);
02424 gsl_rng_set(rnd,random_seed);
02425
02426 double sigma[6];
02427 for (unsigned int i=0;i<6;++i) {
02428 if (gsl_vector_get(eval,i) < 0.0) {
02429
02430 ORSA_ERROR("problems with the covariance matrix: negative eigenvalue found.");
02431
02432
02433 gsl_vector_free(mu);
02434 gsl_vector_free(x);
02435 gsl_vector_free(y);
02436 gsl_matrix_free(A);
02437 gsl_permutation_free(p);
02438 gsl_matrix_free(LU);
02439 gsl_matrix_free(inv);
02440 gsl_eigen_symmv_free(w);
02441 gsl_vector_free(eval);
02442 gsl_matrix_free(evec);
02443 gsl_rng_free(rnd);
02444
02445 return;
02446 }
02447
02448 sigma[i] = secure_sqrt(gsl_vector_get(eval,i));
02449 }
02450
02451 switch (cov_base) {
02452 case Osculating: {
02453 while (1) {
02454
02455 for (unsigned int i=0;i<6;++i) {
02456 gsl_vector_set(x,i,gsl_ran_gaussian(rnd,sigma[i]));
02457 }
02458
02459 gsl_vector_memcpy(y,mu);
02460 gsl_blas_dgemv(CblasNoTrans,1.0,evec,x,1.0,y);
02461
02462 ++generated;
02463
02464 gen_orb.a = gsl_vector_get (y,0);
02465 gen_orb.e = gsl_vector_get (y,1);
02466 gen_orb.i = gsl_vector_get (y,2);
02467 gen_orb.omega_node = gsl_vector_get (y,3);
02468 gen_orb.omega_pericenter = gsl_vector_get (y,4);
02469 gen_orb.M = gsl_vector_get (y,5);
02470
02471 list.push_back(gen_orb);
02472
02473 if (generated>=num) break;
02474 }
02475 break;
02476 }
02477 case Equinoctal: {
02478 while (1) {
02479
02480 for (unsigned int i=0;i<6;++i) {
02481 gsl_vector_set(x,i,gsl_ran_gaussian(rnd,sigma[i]));
02482 }
02483
02484 gsl_vector_memcpy(y,mu);
02485 gsl_blas_dgemv(CblasNoTrans,1.0,evec,x,1.0,y);
02486
02487 ++generated;
02488
02489 const double omega_tilde = secure_atan2(gsl_vector_get(y,1),gsl_vector_get(y,2));
02490
02491 gen_orb.a = gsl_vector_get(y,0);
02492 gen_orb.e = sqrt(gsl_vector_get(y,1)*gsl_vector_get(y,1)+gsl_vector_get(y,2)*gsl_vector_get(y,2));
02493 gen_orb.i = 2.0*atan(sqrt(gsl_vector_get(y,3)*gsl_vector_get(y,3)+gsl_vector_get(y,4)*gsl_vector_get(y,4)));
02494 gen_orb.omega_node = fmod(10.0*twopi+secure_atan2(gsl_vector_get(y,3),gsl_vector_get(y,4)),twopi);
02495 gen_orb.omega_pericenter = fmod(10.0*twopi+omega_tilde-omega_node,twopi);
02496 gen_orb.M = fmod(10.0*twopi+gsl_vector_get(y,5)-omega_tilde,twopi);
02497
02498 list.push_back(gen_orb);
02499
02500 if (generated>=num) break;
02501 }
02502 break;
02503 }
02504 }
02505
02506
02507 gsl_vector_free(mu);
02508 gsl_vector_free(x);
02509 gsl_vector_free(y);
02510 gsl_matrix_free(A);
02511 gsl_permutation_free(p);
02512 gsl_matrix_free(LU);
02513 gsl_matrix_free(inv);
02514 gsl_eigen_symmv_free(w);
02515 gsl_vector_free(eval);
02516 gsl_matrix_free(evec);
02517 gsl_rng_free(rnd);
02518
02519 }
02520
02521
02522
02523 class CloseApproaches_gsl_parameters {
02524 public:
02525 Frame f;
02526 unsigned int obj_index, index;
02527 Evolution * e;
02528 };
02529
02530 double CloseApproaches_gsl_f(const gsl_vector * v, void * params) {
02531
02532 CloseApproaches_gsl_parameters * p = (CloseApproaches_gsl_parameters *) params;
02533
02534 Frame f = p->f;
02535
02536 const UniverseTypeAwareTime gsl_time(gsl_vector_get(v,0));
02537
02538 p->e->clear();
02539 p->e->push_back(f);
02540 p->e->SetSamplePeriod(f - gsl_time);
02541 p->e->SetMaxUnsavedSubSteps(10000);
02542 p->e->Integrate(gsl_time,true);
02543
02544 f = (*(p->e))[p->e->size()-1];
02545
02546 return (f[p->obj_index].position() - f[p->index].position()).Length();
02547 }
02548
02549
02550
02551
02552
02553
02554
02555
02556
02557
02558
02559
02560
02561
02562
02563
02564
02565
02566
02567
02568
02569
02570
02571
02572
02573
02574
02575
02576
02577
02578
02579
02580
02581
02582
02583
02584
02585
02586
02587
02588
02589
02590
02591
02592
02593
02594
02595
02596
02597
02598
02599
02600
02601
02602
02603
02604
02605
02606
02607
02608
02609
02610
02611
02612
02613
02614
02615
02616
02617
02618
02619
02620
02621
02622
02623
02624
02625
02626
02627
02628
02629
02630
02631
02632
02633
02634
02635
02636
02637
02638
02639
02640
02641
02642
02643
02644
02645
02646
02647
02648
02649
02650
02651
02652
02653
02654
02655
02656
02657
02658
02659
02660
02661
02662
02663
02664
02665
02666
02667
02668
02669
02670
02671
02672
02673
02674
02675
02676
02677
02678
02679
02680
02681
02682
02683
02684
02685
02686
02687
02688
02689
02690
02691
02692
02693
02694
02695
02696
02697
02698
02699
02700
02701
02702
02703
02704
02705
02706
02707
02708
02709
02710
02711
02712
02713
02714
02715
02716
02717
02718
02719
02720
02721
02722
02723
02724
02725
02726
02727
02728
02729
02730
02731
02732
02733
02734
02735
02736
02737
02738
02739
02740
02741
02742
02743
02744
02745
02746 void SearchCloseApproaches(const Evolution * evol, const unsigned int obj_index, const unsigned int index, std::vector<CloseApproach> & clapp, const double distance_threshold, const double time_accuracy) {
02747
02748 if (index == obj_index) return;
02749
02750
02751 CloseApproaches_gsl_parameters par;
02752 par.e = new Evolution(*evol);
02753
02754 gsl_multimin_fminimizer * s = gsl_multimin_fminimizer_alloc(gsl_multimin_fminimizer_nmsimplex,1);
02755
02756 gsl_multimin_function clapp_function;
02757 clapp_function.f = &CloseApproaches_gsl_f;
02758 clapp_function.n = 1;
02759 clapp_function.params = ∥
02760
02761 gsl_vector * x = gsl_vector_alloc(1);
02762 gsl_vector * step_size = gsl_vector_alloc(1);
02763
02764
02765
02766 vector<double> distance;
02767 distance.resize(evol->size());
02768
02769 CloseApproach ca;
02770
02771 for (unsigned int j=0;j<evol->size();++j) {
02772 distance[j] = ((*evol)[j][obj_index].position() - (*evol)[j][index].position()).Length();
02773 }
02774
02775
02776 for (unsigned int j=1;j<(evol->size()-1);++j) {
02777
02778 if ( (distance[j] <= 1.5*distance_threshold) &&
02779 (distance[j] <= distance[j-1]) &&
02780 (distance[j] <= distance[j+1]) ) {
02781
02782
02783 par.f = (*evol)[j];
02784 par.obj_index = obj_index;
02785 par.index = index;
02786
02787 gsl_vector_set(x,0,(*evol)[j].GetTime());
02788
02789 gsl_vector_set(step_size,0,evol->GetSamplePeriod().GetDouble());
02790
02791 gsl_multimin_fminimizer_set(s, &clapp_function, x, step_size);
02792
02793
02794
02795 size_t iter = 0;
02796 int status;
02797 do {
02798
02799 ++iter;
02800
02801
02802 status = gsl_multimin_fminimizer_iterate(s);
02803
02804
02805
02806
02807
02808 status = gsl_multimin_test_size(gsl_multimin_fminimizer_size(s),time_accuracy);
02809
02810 } while (status == GSL_CONTINUE && iter < 200);
02811
02812
02813
02814 if (status == GSL_SUCCESS) {
02815 if (gsl_multimin_fminimizer_minimum(s) < distance_threshold) {
02816 ca.name = (*evol)[j][index].name();
02817 ca.epoch.SetTime(gsl_vector_get(s->x,0));
02818 ca.distance = gsl_multimin_fminimizer_minimum(s);
02819
02820 {
02821 Frame f = par.f;
02822
02823 const UniverseTypeAwareTime gsl_time(gsl_vector_get(s->x,0));
02824
02825 par.e->clear();
02826 par.e->push_back(f);
02827 par.e->SetSamplePeriod(f - gsl_time);
02828 par.e->SetMaxUnsavedSubSteps(10000);
02829 par.e->Integrate(gsl_time,true);
02830
02831 f = (*(par.e))[par.e->size()-1];
02832
02833 ca.relative_velocity = (f[obj_index].velocity() - f[index].velocity()).Length();
02834
02835
02836 {
02837 const Vector unit_v = (f[obj_index].velocity() - f[index].velocity()).Normalize();
02838 const Vector d = f[obj_index].position() - f[index].position();
02839 const Vector unit_d = d.Normalized();
02840 const Vector unit_q = ExternalProduct(unit_v,unit_d).Normalize();
02841
02842
02843
02844
02845 if (unit_d.z != 0.0) {
02846 const double beta = sqrt(+ unit_q.x*unit_q.x
02847 + unit_q.y*unit_q.y
02848 + unit_q.z*unit_q.z/(unit_d.z*unit_d.z)*(unit_d.x*unit_d.x+unit_d.y*unit_d.y)
02849 - 2*(unit_q.z/unit_d.z)*(unit_q.x*unit_d.y+unit_q.y*unit_d.x));
02850 const double alpha = - beta * unit_q.z / unit_d.z;
02851
02852 const Vector new_unit_d = (alpha*unit_d+beta*unit_q).Normalize();
02853 const Vector new_unit_q = ExternalProduct(unit_v,new_unit_d).Normalize();
02854
02855
02856 if (new_unit_q.z > 0.0) {
02857 ca.tp_xy = d*new_unit_d;
02858 ca.tp_z = d*new_unit_q;
02859 } else {
02860 ca.tp_xy = -d*new_unit_d;
02861 ca.tp_z = -d*new_unit_q;
02862 }
02863
02864
02865
02866
02867
02868
02869
02870
02871
02872
02873
02874 } else {
02875 ca.tp_xy = d.Length();
02876 ca.tp_z = 0.0;
02877 }
02878
02879
02880
02881
02882 }
02883
02884 }
02885
02886 clapp.push_back(ca);
02887 }
02888
02889 }
02890
02891
02892
02893 }
02894
02895 }
02896
02897
02898
02899
02900 gsl_multimin_fminimizer_free(s);
02901 gsl_vector_free(x);
02902 gsl_vector_free(step_size);
02903
02904 delete par.e;
02905 }
02906
02907 }