#pragma ident "$Id: PhaseWindup.cpp 293 2006-11-10 16:39:56Z rickmach $"//============================================================================////  This file is part of GPSTk, the GPS Toolkit.////  The GPSTk is free software; you can redistribute it and/or modify//  it under the terms of the GNU Lesser General Public License as published//  by the Free Software Foundation; either version 2.1 of the License, or//  any later version.////  The GPSTk is distributed in the hope that it will be useful,//  but WITHOUT ANY WARRANTY; without even the implied warranty of//  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the//  GNU Lesser General Public License for more details.////  You should have received a copy of the GNU Lesser General Public//  License along with GPSTk; if not, write to the Free Software Foundation,//  Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA//  //  Copyright 2004, The University of Texas at Austin////============================================================================//============================================================================////This software developed by Applied Research Laboratories at the University of//Texas at Austin, under contract to an agency or agencies within the U.S. //Department of Defense. The U.S. Government retains all rights to use,//duplicate, distribute, disclose, or release this software. ////Pursuant to DoD Directive 523024 //// DISTRIBUTION STATEMENT A: This software has been approved for public //                           release, distribution is unlimited.////=============================================================================/** * @file PhaseWindup.cpp * Implement computations of phase windup, solar ephemeris, satellite attitude * and eclipse at the satellite. */ // -----------------------------------------------------------------------------------// GPSTk includes#include "Position.hpp"#include "Matrix.hpp"#include "geometry.hpp"             // DEG_TO_RAD#include "icd_200_constants.hpp"    // TWO_PIusing namespace std;using namespace gpstk;// -----------------------------------------------------------------------------------void SolarPosition(DayTime t, double& lat, double& lon, double& R, double& AR);Matrix<double> SatelliteAttitude(DayTime& tt, Position& SV, double& sf);double shadowFactor(double Rearth, double Rsun, double dES);static double GMST(DayTime t);// -----------------------------------------------------------------------------------// Given a Position, compute unit (ECEF) vectors in the Up, East and North directions// at that position. Use geodetic coordinates, i.e. 'up' is perpendicular to the// geoid. Return the vectors in the form of a 3x3 Matrix<double>, this is in fact the// rotation matrix that will take an ECEF vector into an 'up-east-north' vector.Matrix<double> UpEastNorth(Position& P){try {   Matrix<double> R(3,3);   P.transformTo(Position::Geodetic);   double lat = P.getGeodeticLatitude() * DEG_TO_RAD;      // rad N   double lon = P.getLongitude() * DEG_TO_RAD;             // rad E   double ca = ::cos(lat);   double sa = ::sin(lat);   double co = ::cos(lon);   double so = ::sin(lon);   // This is the rotation matrix which will take X=(x,y,z) into (R*X)(up,east,north)   R(0,0) =  ca*co;  R(0,1) =  ca*so;  R(0,2) = sa;   R(1,0) =    -so;  R(1,1) =     co;  R(1,2) = 0.;   R(2,0) = -sa*co;  R(2,1) = -sa*so;  R(2,2) = ca;   // The rows of R are also the unit vectors, in ECEF, of up,east,north;   //  R = (U && E && N) = transpose(U || E || N).   //Matrix<double> U = R.rowCopy(0);   //Matrix<double> E = R.rowCopy(1);   //Matrix<double> N = R.rowCopy(2);   return R;}catch(Exception& e) { GPSTK_RETHROW(e); }catch(exception& e) { Exception E("std except: "+string(e.what())); GPSTK_THROW(E); }catch(...) { Exception e("Unknown exception"); GPSTK_THROW(e); }}// -----------------------------------------------------------------------------------// Generate a 3x3 rotation Matrix, for direct rotations about one axis// (for XYZ, axis=123), given the rotation angle in radians;// @param angle in radians.// @param axis 1,2,3 as rotation about X,Y,Z.// @return Rotation matrix (3x3).// @throw InvalidInput if axis is anything other than 1, 2 or 3.Matrix<double> SingleAxisRotation(double angle,                                  int axis)   throw(Exception){try {   if(axis < 1 || axis > 3) {      Exception e(string("Invalid axis (1,2,3 <=> X,Y,Z): ")            + StringUtils::asString(axis));      GPSTK_THROW(e);   }   Matrix<double> R(3,3,0.0);   int i1=axis-1;                      // axis = 1 : 0,1,2   int i2=i1+1; if(i2 == 3) i2=0;      // axis = 2 : 1,2,0   int i3=i2+1; if(i3 == 3) i3=0;      // axis = 3 : 2,0,1   R(i1,i1) = 1.0;   R(i2,i2) = R(i3,i3) = ::cos(angle);   R(i3,i2) = -(R(i2,i3) = ::sin(angle));   return R;}catch(Exception& e) { GPSTK_RETHROW(e); }catch(exception& e) { Exception E("std except: "+string(e.what())); GPSTK_THROW(E); }catch(...) { Exception e("Unknown exception"); GPSTK_THROW(e); }}// -----------------------------------------------------------------------------------// Compute the satellite attitude, given the time and the satellite position SV.// Return a 3x3 Matrix which contains, as rows, the unit (ECEF) vectors X,Y,Z in the// body frame of the satellite, namely//    Z = along the boresight (i.e. towards Earth center),//    Y = perpendicular to both Z and the satellite-sun direction, and//    X completing the orthonormal triad. X will generally point toward the sun.// Also return the shadow factor = fraction of the sun's area visible to satellite.Matrix<double> SatelliteAttitude(DayTime& tt, Position& SV, double& sf){try {   int i;   double d,svrange,lat,lon,DistSun,Radsun,Radearth,dES;   Position X,Y,Z,T;   Matrix<double> R(3,3);   // Z points from satellite to Earth center - along the antenna boresight   Z = SV;   Z.transformTo(Position::Cartesian);   svrange = Z.mag();   d = -1.0/Z.mag();   Z = d * Z;                                // reverse and normalize Z   // T points from satellite to sun   SolarPosition(tt, lat, lon, DistSun, Radsun);   Radsun *= DEG_TO_RAD;                     // angular radius of sun at satellite   Radearth = ::asin(6378137.0/svrange);     // angular radius of earth at sat   T.setGeocentric(lat,lon,DistSun);         // vector earth to sun   T.transformTo(Position::Cartesian);   T = T - SV;                               // sat to sun=(E to sun)-(E to sat)   d = 1.0/T.mag();   T = d * T;                                // normalize T   dES = ::acos(Z.dot(T));                   // apparent angular distance, earth                                             // to sun, as seen at satellite   sf = shadowFactor(Radearth, Radsun, dES); // is satellite in eclipse?   //if(sf > 0.999) { ;    // total eclipse }   //else if(sf > 0.0) { ; // partial eclipse }   //else { ;              // no eclipse }   // Y is perpendicular to Z and T, such that ...   Y = Z.cross(T);   d = 1.0/Y.mag();   Y = d * Y;                                // normalize Y   // ... X points generally in the direction of the sun   X = Y.cross(Z);                           // X will be unit vector   if(X.dot(T) < 0) {                        // need to reverse X, hence Y also      X = -1.0 * X;      Y = -1.0 * Y;   }   // fill the matrix and return it   for(i=0; i<3; i++) {      R(0,i) = X[i];      R(1,i) = Y[i];      R(2,i) = Z[i];   }   return R;}catch(Exception& e) { GPSTK_RETHROW(e); }catch(exception& e) { Exception E("std except: "+string(e.what())); GPSTK_THROW(E); }catch(...) { Exception e("Unknown exception"); GPSTK_THROW(e); }}// -----------------------------------------------------------------------------------// Compute the phase windup, in cycles, given the time, the unit vector from receiver// to transmitter, and the west and north unit vectors at the receiver, all in ECEF.// YR is the West unit vector, XR is the North unit vector, at the receiver.