AstroToolbox/Lambert.cpp

   1 // ------------------------------------------------------------------------ //
   2 // This source file is part of the 'ESA Advanced Concepts Team's                        //
   3 // Space Mechanics Toolbox' software.                                       //
   4 //                                                                          //
   5 // The source files are for research use only,                              //
   6 // and are distributed WITHOUT ANY WARRANTY. Use them on your own risk.     //
   7 //                                                                          //
   8 // Copyright (c) 2004-2007 European Space Agency                            //
   9 // ------------------------------------------------------------------------ //
  10
  11
  12 /*
  13  This routine implements a new algorithm that solves Lambert's problem. The
  14  algorithm has two major characteristics that makes it favorable to other
  15  existing ones.
  16
  17    1) It describes the generic orbit solution of the boundary condition
  18    problem through the variable X=log(1+cos(alpha/2)). By doing so the
  19    graphs of the time of flight become defined in the entire real axis and
  20    resembles a straight line. Convergence is granted within few iterations
  21    for all the possible geometries (except, of course, when the transfer
  22    angle is zero). When multiple revolutions are considered the variable is
  23    X=tan(cos(alpha/2)*pi/2).
  24
  25    2) Once the orbit has been determined in the plane, this routine
  26    evaluates the velocity vectors at the two points in a way that is not
  27    singular for the transfer angle approaching to pi (Lagrange coefficient
  28    based methods are numerically not well suited for this purpose).
  29
  30    As a result Lambert's problem is solved (with multiple revolutions
  31    being accounted for) with the same computational effort for all
  32    possible geometries. The case of near 180 transfers is also solved
  33    efficiently.
  34
  35    We note here that even when the transfer angle is exactly equal to pi
  36    the algorithm does solve the problem in the plane (it finds X), but it
  37    is not able to evaluate the plane in which the orbit lies. A solution
  38    to this would be to provide the direction of the plane containing the
  39    transfer orbit from outside. This has not been implemented in this
  40    routine since such a direction would depend on which application the
  41    transfer is going to be used in.
  42
  43  Inputs:
  44            r1=Position vector at departure (column)
  45            r2=Position vector at arrival (column, same units as r1)
  46            t=Transfer time (scalar)
  47            mu=gravitational parameter (scalar, units have to be
  48            consistent with r1,t units)
  49            lw=1 if long way is chosen
  50
  51
  52  Outputs:
  53            v1=Velocity at departure        (consistent units)
  54            v2=Velocity at arrival
  55            a=semi major axis of the solution
  56            p=semi latus rectum of the solution
  57            theta=transfer angle in rad
  58            iter=number of iteration made by the newton solver (usually 6)
  59 */
  60
  61 #include <cmath>
  62 #include <iostream>
  63
  64 #include "Astro_Functions.h"
  65 #include "Lambert.h"
  66
  67 using namespace std;
  68
  69 void LambertI (const double *r1_in, const double *r2_in, double t, const double &mu, //INPUT
  70                    const int &lw, //INPUT
  71                    double *v1, double *v2, double &a, double &p, double &theta, int &iter)//OUTPUT
  72 {
  73         double  V,T,
  74     r2_mod = 0.0,    // R2 module
  75     dot_prod = 0.0, // dot product
  76     c,                  // non-dimensional chord
  77     s,                  // non dimesnional semi-perimeter
  78     am,                 // minimum energy ellipse semi major axis
  79     lambda,             //lambda parameter defined in Battin's Book
  80     x,x1,x2,y1,y2,x_new=0,y_new,err,alfa,beta,psi,eta,eta2,sigma1,vr1,vt1,vt2,vr2,R=0.0;
  81         int i_count, i;
  82         const double tolerance = 1e-11;
  83         double r1[3], r2[3], r2_vers[3];
  84         double ih_dum[3], ih[3], dum[3];
  85
  86         // Increasing the tolerance does not bring any advantage as the
  87         // precision is usually greater anyway (due to the rectification of the tof
  88         // graph) except near particular cases such as parabolas in which cases a
  89         // lower precision allow for usual convergence.
  90
  91         if (t <= 0)
  92     {
  93                 cout << "ERROR in Lambert Solver: Negative Time in input." << endl;
  94         return;
  95     }
  96
  97         for (i = 0; i < 3; i++)
  98     {
  99       r1[i] = r1_in[i];
 100       r2[i] = r2_in[i];
 101           R += r1[i]*r1[i];
 102     }
 103
 104         R = sqrt(R);
 105         V = sqrt(mu/R);
 106         T = R/V;
 107
 108     // working with non-dimensional radii and time-of-flight
 109         t /= T;
 110         for (i = 0;i <3;i++)  // r1 dimension is 3
 111     {
 112                 r1[i] /= R;
 113                 r2[i] /= R;
 114                 r2_mod += r2[i]*r2[i];
 115     }
 116
 117         // Evaluation of the relevant geometry parameters in non dimensional units
 118         r2_mod = sqrt(r2_mod);
 119
 120         for (i = 0;i < 3;i++)
 121       dot_prod += (r1[i] * r2[i]);
 122
 123         theta = acos(dot_prod/r2_mod);
 124
 125         if (lw)
 126                 theta=2*acos(-1.0)-theta;
 127
 128         c = sqrt(1 + r2_mod*(r2_mod - 2.0 * cos(theta)));
 129         s = (1 + r2_mod + c)/2.0;
 130         am = s/2.0;
 131         lambda = sqrt (r2_mod) * cos (theta/2.0)/s;
 132
 133     // We start finding the log(x+1) value of the solution conic:
 134     // NO MULTI REV --> (1 SOL)
 135     //  inn1=-.5233;    //first guess point
 136     //  inn2=.5233;     //second guess point
 137         x1=log(0.4767);
 138         x2=log(1.5233);
 139         y1=log(x2tof(-.5233,s,c,lw))-log(t);
 140         y2=log(x2tof(.5233,s,c,lw))-log(t);
 141
 142         // Newton iterations
 143         err=1;
 144         i_count=0;
 145         while ((err>tolerance) && (y1 != y2))
 146     {
 147                 i_count++;
 148                 x_new=(x1*y2-y1*x2)/(y2-y1);
 149                 y_new=logf(x2tof(expf(x_new)-1,s,c,lw))-logf(t); //[MR] Why ...f() functions? Loss of data!
 150                 x1=x2;
 151                 y1=y2;
 152                 x2=x_new;
 153                 y2=y_new;
 154                 err = fabs(x1-x_new);
 155     }
 156         iter = i_count;
 157         x = expf(x_new)-1; //[MR] Same here... expf -> exp
 158
 159     // The solution has been evaluated in terms of log(x+1) or tan(x*pi/2), we
 160     // now need the conic. As for transfer angles near to pi the lagrange
 161     // coefficient technique goes singular (dg approaches a zero/zero that is
 162     // numerically bad) we here use a different technique for those cases. When
 163     // the transfer angle is exactly equal to pi, then the ih unit vector is not
 164     // determined. The remaining equations, though, are still valid.
 165
 166         a = am/(1 - x*x);
 167
 168         // psi evaluation
 169         if (x < 1)  // ellipse
 170     {
 171                 beta = 2 * asin (sqrt( (s-c)/(2*a) ));
 172                 if (lw) beta = -beta;
 173                 alfa=2*acos(x);
 174                 psi=(alfa-beta)/2;
 175                 eta2=2*a*pow(sin(psi),2)/s;
 176                 eta=sqrt(eta2);
 177     }
 178         else       // hyperbola
 179     {
 180                 beta = 2*asinh(sqrt((c-s)/(2*a)));
 181                 if (lw) beta = -beta;
 182                 alfa = 2*acosh(x);
 183                 psi = (alfa-beta)/2;
 184                 eta2 = -2 * a * pow(sinh(psi),2)/s;
 185                 eta = sqrt(eta2);
 186     }
 187
 188         // parameter of the solution
 189         p = ( r2_mod / (am * eta2) ) * pow (sin (theta/2),2);
 190         sigma1 = (1/(eta * sqrt(am)) )* (2 * lambda * am - (lambda + x * eta));
 191         vett(r1,r2,ih_dum);
 192         vers(ih_dum,ih) ;
 193
 194         if (lw)
 195     {
 196                 for (i = 0; i < 3;i++)
 197                         ih[i]= -ih[i];
 198     }
 199
 200         vr1 = sigma1;
 201         vt1 = sqrt(p);
 202         vett(ih,r1,dum);
 203
 204         for (i = 0;i < 3 ;i++)
 205                 v1[i] = vr1 * r1[i] + vt1 * dum[i];
 206
 207         vt2 = vt1 / r2_mod;
 208         vr2 = -vr1 + (vt1 - vt2)/tan(theta/2);
 209         vers(r2,r2_vers);
 210         vett(ih,r2_vers,dum);
 211         for (i = 0;i < 3 ;i++)
 212                 v2[i] = vr2 * r2[i] / r2_mod + vt2 * dum[i];
 213
 214         for (i = 0;i < 3;i++)
 215     {
 216                 v1[i] *= V;
 217                 v2[i] *= V;
 218     }
 219         a *= R;
 220         p *= R;
 221 }
 222
 223