orsa_coord.cc

Go to the documentation of this file.
00001 /* 
00002    ORSA - Orbit Reconstruction, Simulation and Analysis
00003    Copyright (C) 2002-2004 Pasquale Tricarico
00004    
00005    This program is free software; you can redistribute it and/or
00006    modify it under the terms of the GNU General Public License
00007    as published by the Free Software Foundation; either version 2
00008    of the License, or (at your option) any later version.
00009    
00010    As a special exception, Pasquale Tricarico gives permission to
00011    link this program with Qt commercial edition, and distribute the
00012    resulting executable, without including the source code for the Qt
00013    commercial edition in the source distribution.
00014    
00015    This program is distributed in the hope that it will be useful,
00016    but WITHOUT ANY WARRANTY; without even the implied warranty of
00017    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00018    GNU General Public License for more details.
00019    
00020    You should have received a copy of the GNU General Public License
00021    along with this program; if not, write to the Free Software
00022    Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.
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   // rotation
00039   // ANGLES IN RADIANS !!!
00040   /* 
00041      Vector& Vector::rotate (const double omega_per, const double i, const double omega_nod) {      
00042      
00043      double local_x, local_y, local_z;
00044      
00045      // from Mathematica..
00046      local_x = cos(omega_nod)*(x*cos(omega_per) - y*sin(omega_per)) + sin(omega_nod)*(z*sin(i) - cos(i)*(y*cos(omega_per) + x*sin(omega_per)));
00047      local_y = -(z*cos(omega_nod)*sin(i)) + cos(i)*cos(omega_nod)*(y*cos(omega_per) + x*sin(omega_per)) + sin(omega_nod) * (x*cos(omega_per) - y*sin(omega_per));
00048      local_z = z*cos(i) + sin(i)*(y*cos(omega_per) + x*sin(omega_per));
00049      
00050      x = local_x; y = local_y; z = local_z;
00051      
00052      return *this;
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     // from Mathematica..
00084     /* 
00085        const double new_x = cos(omega_nod)*(x*cos(omega_per) - y*sin(omega_per)) + sin(omega_nod)*(z*sin(i) - cos(i)*(y*cos(omega_per) + x*sin(omega_per)));
00086        const double new_y = -(z*cos(omega_nod)*sin(i)) + cos(i)*cos(omega_nod)*(y*cos(omega_per) + x*sin(omega_per)) + sin(omega_nod) * (x*cos(omega_per) - y*sin(omega_per));
00087        const double new_z = z*cos(i) + sin(i)*(y*cos(omega_per) + x*sin(omega_per));
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     // cerr << "Interpolate() call..." << endl;
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     /* cerr << " -----> n_points: " << n_points << endl;
00116        for (j=0;j<n_points;j++)
00117        std::cerr << "vx_in[" << j << "].par = " <<  vx_in[j].par << "\n";
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     // cerr << "local..  v: " << Length(v_out) << "  i_closest: " << i_closest << endl;
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         // cerr << " ++++++++++  d[" << i << "]: " << Length(d[i]) << endl;
00177         // cerr << " ++++++++++  c[" << i << "]: " << Length(c[i]) << endl;
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 } // namespace orsa

Generated on Fri Nov 3 20:37:41 2006 for liborsa by  doxygen 1.4.7