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_coord.h"
00026
00027 #include <cmath>
00028 #include <cstdlib>
00029 #include <iostream>
00030
00031 using namespace std;
00032
00033 #ifdef HAVE_CONFIG_H
00034 #include <config.h>
00035 #endif // HAVE_CONFIG_H
00036
00037 namespace orsa {
00038
00039
00040
00041
00042
00043
00044
00045
00046
00047
00048
00049
00050
00051
00052
00053
00054
00055
00056
00057 Vector & Vector::rotate (const double omega_per, const double i, const double omega_nod) {
00058
00059 #ifdef HAVE_SINCOS
00060 double s,c;
00061
00062 sincos(i,&s,&c);
00063 const double s_i = s;
00064 const double c_i = c;
00065
00066 sincos(omega_nod,&s,&c);
00067 const double s_on = s;
00068 const double c_on = c;
00069
00070 sincos(omega_per,&s,&c);
00071 const double s_op = s;
00072 const double c_op = c;
00073 #else // HAVE_SINCOS
00074 const double s_i = sin(i);
00075 const double c_i = cos(i);
00076
00077 const double s_on = sin(omega_nod);
00078 const double c_on = cos(omega_nod);
00079
00080 const double s_op = sin(omega_per);
00081 const double c_op = cos(omega_per);
00082 #endif // HAVE_SINCOS
00083
00084
00085
00086
00087
00088
00089
00090
00091 const double new_x = c_on*(x*c_op - y*s_op) + s_on*(z*s_i - c_i*(y*c_op + x*s_op));
00092 const double new_y = -(z*c_on*s_i) + c_i*c_on*(y*c_op + x*s_op) + s_on*(x*c_op - y*s_op);
00093 const double new_z = z*c_i + s_i*(y*c_op + x*s_op);
00094
00095 x = new_x;
00096 y = new_y;
00097 z = new_z;
00098
00099 return * this;
00100 }
00101
00102 void Interpolate(const vector<VectorWithParameter> vx_in, const double x, Vector &v_out, Vector &err_v_out) {
00103
00104
00105
00106 unsigned int i,j,m;
00107
00108 unsigned int i_closest;
00109
00110 double diff,denom;
00111 double ho,hp;
00112 double tmp_double;
00113
00114 unsigned int n_points = vx_in.size();
00115
00116
00117
00118
00119
00120
00121 if (n_points < 2) {
00122 cerr << "too few points..." << endl;
00123 for (j=0;j<n_points;j++) {
00124 v_out = vx_in[0];
00125 err_v_out.Set(0,0,0);
00126 }
00127 return;
00128 }
00129
00130 Vector * c = new Vector[n_points];
00131 Vector * d = new Vector[n_points];
00132
00133 Vector w;
00134
00135 diff = fabs(x-vx_in[0].par);
00136 i_closest = 0;
00137
00138 for (j=0;j<n_points;j++) {
00139
00140 tmp_double = fabs(x-vx_in[j].par);
00141 if (tmp_double < diff) {
00142 diff = tmp_double;
00143 i_closest = j;
00144 }
00145
00146 c[j] = vx_in[j];
00147 d[j] = vx_in[j];
00148 }
00149
00150 v_out = vx_in[i_closest];
00151 err_v_out = vx_in[i_closest];
00152
00153
00154
00155 i_closest--;
00156
00157 for (m=1;m<=(n_points-2);m++) {
00158 for (i=0;i<(n_points-m);i++) {
00159 ho = vx_in[i].par - x;
00160 hp = vx_in[i+m].par - x;
00161
00162 denom = ho - hp;
00163
00164 if (denom == 0.0) {
00165 cerr << "interpolate() --> Error: divide by zero\n";
00166 cerr << "i: " << i << " m: " << m << endl;
00167 for (j=0;j<n_points;j++)
00168 cerr << "vx_in[" << j << "].par = " << vx_in[j].par << "\n";
00169 exit(0);
00170 }
00171
00172 w = c[i+1] - d[i];
00173
00174 d[i] = hp*w/denom;
00175 c[i] = ho*w/denom;
00176
00177
00178
00179 }
00180
00181 if ( (2*i_closest) < (n_points-m) ) {
00182 err_v_out = c[i_closest+1];
00183 } else {
00184 err_v_out = d[i_closest];
00185 i_closest--;
00186 }
00187
00188 v_out += err_v_out;
00189 }
00190
00191 delete [] c;
00192 delete [] d;
00193 }
00194
00195 }