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 <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   // rotation
00040   // ANGLES IN RADIANS !!!
00041   /* 
00042      Vector& Vector::rotate (const double omega_per, const double i, const double omega_nod) {      
00043      
00044      double local_x, local_y, local_z;
00045      
00046      // from Mathematica..
00047      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)));
00048      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));
00049      local_z = z*cos(i) + sin(i)*(y*cos(omega_per) + x*sin(omega_per));
00050      
00051      x = local_x; y = local_y; z = local_z;
00052      
00053      return *this;
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     // from Mathematica..
00085     /* 
00086        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)));
00087        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));
00088        const double new_z = z*cos(i) + sin(i)*(y*cos(omega_per) + x*sin(omega_per));
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     // cerr << "Interpolate() call..." << endl;
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     /* cerr << " -----> n_points: " << n_points << endl;
00117        for (j=0;j<n_points;j++)
00118        std::cerr << "vx_in[" << j << "].par = " <<  vx_in[j].par << "\n";
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     // cerr << "local..  v: " << Length(v_out) << "  i_closest: " << i_closest << endl;
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         // cerr << " ++++++++++  d[" << i << "]: " << Length(d[i]) << endl;
00178         // cerr << " ++++++++++  c[" << i << "]: " << Length(c[i]) << endl;
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 } // namespace orsa

Generated on Wed May 30 13:04:52 2007 for liborsa by  doxygen 1.5.2