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