math - How to convert a spherical velocity coordinates into cartesian

Question

I have a velocity vector in altitude, longitude, altitude, I would like to convert it to Cartesian coordinates, vx,vy,vz. The format is from WGS84 standard.

here is the formula

  //------------------------------------------------------------------------------
    template <class T> 
    TVectorXYZ<T> WGS84::ToCartesian(T latitude, T longitude, T elevation)
    //------------------------------------------------------------------------------
    {
        double sinlat, coslat;
        double sinlon, coslon;
        sincos_degree(latitude,  sinlat, coslat);
        sincos_degree(longitude, sinlon, coslon);  

        const double v = a / sqrt(1 - WGS84::ee * sinlat*sinlat);

        TVectorXYZ<T> coord
        (
            static_cast<T>((v + elevation) * coslat * sinlon),
            static_cast<T>(((1 - WGS84::ee) * v + elevation) * sinlat),
            static_cast<T>((v + elevation) * coslat * coslon)                                    
        );

        return coord;
    }

Answer 1

OK based on your previous question and long comment flow lets assume your input is:

lon [rad], lat [rad], alt [m] // WGS84 position
vlon [m/s], vlat [m/s], alt [m/s] // speed in WGS84  lon,lat,alt directions but in [m/s]

And want output:

x,y,z // Cartesian position [m/s]
vx,vy,vz // Cartesian velocity [m/s]

And have valid transformation to Cartesian coordinates for positions at your disposal this is mine:

void WGS84toXYZ(double &x,double &y,double &z,double lon,double lat,double alt) // [rad,rad,m] -> [m,m,m]
    {
    const double  _earth_a=6378137.00000;   // [m] WGS84 equator radius
    const double  _earth_b=6356752.31414;   // [m] WGS84 epolar radius
    const double  _earth_e=8.1819190842622e-2; //  WGS84 eccentricity
    const double _aa=_earth_a*_earth_a;
    const double _ee=_earth_e*_earth_e;
    double  a,b,x,y,z,h,l,c,s;
    a=lon;
    b=lat;
    h=alt;
    c=cos(b);
    s=sin(b);
    // WGS84 from eccentricity
    l=_earth_a/sqrt(1.0-(_ee*s*s));
    x=(l+h)*c*cos(a);
    y=(l+h)*c*sin(a);
    z=(((1.0-_ee)*l)+h)*s;
    }

And routine for normalize vector to unit size:

void normalize(double &x,double &y,double &z)
    {
    double l=sqrt(x*x+y*y+z*z);
    if (l>1e-6) l=1.0/l;
    x*=l; y*=l; z*=l;
    }

Yes you can try to derive the formula lihe @MvG suggest but from your rookie mistakes I strongly doubt it would lead to successful result. Instead you can do this:

1. obtain lon,lat,alt direction vectors for your position (x,y,z)

   that is easy just use some small step increment in WGS84 position convert to Cartesian substract and normalize to unit vectors. Let call these direction basis vectors U,V,W.

   double Ux,Uy,Uz;    // [m]
   double Vx,Vy,Vz;    // [m]
   double Wx,Wy,Wz;    // [m]
   double da=1.567e-7; // [rad] angular step ~ 1.0 m in lon direction
   double dl=1.0;      // [m] altitide step 1.0 m
   WGS84toXYZ( x, y, z,lon   ,lat,alt   ); // actual position
   WGS84toXYZ(Ux,Uy,Uz,lon+da,lat,alt   ); // lon direction Nort
   WGS84toXYZ(Vx,Vy,Vz,lon,lat+da,alt   ); // lat direction East
   WGS84toXYZ(Wx,Wy,Wz,lon,lat   ,alt+dl); // alt direction High/Up
   Ux-=x; Uy-=y; Uz-=z;
   Vx-=x; Vy-=y; Vz-=z;
   Wx-=x; Wy-=y; Wz-=z;
   normalize(Ux,Uy,Uz);
   normalize(Vx,Vy,Vz);
   normalize(Wx,Wy,Wz);
   

2. convert velocity from lon,lat,alt to vx,vy,vz

   vx = vlon*Ux + vlat*Vx + valt*Wx;
   vy = vlon*Uy + vlat*Vy + valt*Wy;
   vz = vlon*Uz + vlat*Vz + valt*Wz;
   

Hope it is clear enough. As usual be careful about the units deg/rad and m/ft/km because units matters a lot.

Btw U,V,W basis vectors form NEH reference frame and in the same time are the direction derivates MvG is mentioning.

[Edit1] more precise conversions

//---------------------------------------------------------------------------
//--- WGS84 transformations ver: 1.00 ---------------------------------------
//---------------------------------------------------------------------------
#ifndef _WGS84_h
#define _WGS84_h
//---------------------------------------------------------------------------
// http://www.navipedia.net/index.php/Ellipsoidal_and_Cartesian_Coordinates_Conversion
//---------------------------------------------------------------------------
// WGS84(a,b,h) = (long,lat,alt) [rad,rad,m]
// XYZ(x,y,z) [m]
//---------------------------------------------------------------------------
const double  _earth_a=6378137.00000;   // [m] WGS84 equator radius
const double  _earth_b=6356752.31414;   // [m] WGS84 epolar radius
const double  _earth_e=8.1819190842622e-2; //  WGS84 eccentricity
//const double  _earth_e=sqrt(1.0-((_earth_b/_earth_a)*(_earth_b/_earth_a)));
const double  _earth_ee=_earth_e*_earth_e;
//---------------------------------------------------------------------------
const double kmh=1.0/3.6;               // [km/h] -> [m/s]
//---------------------------------------------------------------------------
void XYZtoWGS84       (double *abh                  ,double *xyz                  ); // [m,m,m] -> [rad,rad,m]
void WGS84toXYZ       (double *xyz                  ,double *abh                  ); // [rad,rad,m] -> [m,m,m]
void WGS84toXYZ_posvel(double *xyzpos,double *xyzvel,double *abhpos,double *abhvel); // [rad,rad,m],[m/s,m/s,m/s] -> [m,m,m],[m/s,m/s,m/s]
void WGS84toNEH       (reper &neh                   ,double *abh                  ); // [rad,rad,m] -> NEH [m]
void WGS84_m2rad      (double &da,double &db,double *abh);                           // [rad,rad,m] -> [rad],[rad] representing 1m angle step
void XYZ_interpolate  (double *pt,double *p0,double *p1,double t);                   // [m,m,m] pt = p0 + (p1-p0)*t in ellipsoid space t = <0,1>
//---------------------------------------------------------------------------
void XYZtoWGS84(double *abh,double *xyz)
    {
    int i;
    double  a,b,h,l,n,db,s;
    a=atanxy(xyz[0],xyz[1]);
    l=sqrt((xyz[0]*xyz[0])+(xyz[1]*xyz[1]));
    // estimate lat
    b=atanxy((1.0-_earth_ee)*l,xyz[2]);
    // iterate to improve accuracy
    for (i=0;i<100;i++)
        {
        s=sin(b); db=b;
        n=divide(_earth_a,sqrt(1.0-(_earth_ee*s*s)));
        h=divide(l,cos(b))-n;
        b=atanxy((1.0-divide(_earth_ee*n,n+h))*l,xyz[2]);
        db=fabs(db-b);
        if (db<1e-12) break;
        }
    if (b>0.5*pi) b-=pi2;
    abh[0]=a;
    abh[1]=b;
    abh[2]=h;
    }
//---------------------------------------------------------------------------
void WGS84toXYZ(double *xyz,double *abh)
    {
    double  a,b,h,l,c,s;
    a=abh[0];
    b=abh[1];
    h=abh[2];
    c=cos(b);
    s=sin(b);
    // WGS84 from eccentricity
    l=_earth_a/sqrt(1.0-(_earth_ee*s*s));
    xyz[0]=(l+h)*c*cos(a);
    xyz[1]=(l+h)*c*sin(a);
    xyz[2]=(((1.0-_earth_ee)*l)+h)*s;
    }
//---------------------------------------------------------------------------
void WGS84toNEH(reper &neh,double *abh)
    {
    double N[3],E[3],H[3];                  // [m]
    double p[3],xyzpos[3];
    const double da=1.567e-7;               // [rad] angular step ~ 1.0 m in lon direction
    const double dl=1.0;                    // [m] altitide step 1.0 m
    vector_copy(p,abh);
    // actual position
    WGS84toXYZ(xyzpos,abh);
    // NEH
    p[0]+=da; WGS84toXYZ(N,p); p[0]-=da;
    p[1]+=da; WGS84toXYZ(E,p); p[1]-=da;
    p[2]+=dl; WGS84toXYZ(H,p); p[2]-=dl;
    vector_sub(N,N,xyzpos);
    vector_sub(E,E,xyzpos);
    vector_sub(H,H,xyzpos);
    vector_one(N,N);
    vector_one(E,E);
    vector_one(H,H);
    neh._rep=1;
    neh._inv=0;
    // axis X
    neh.rep[ 0]=N[0];
    neh.rep[ 1]=N[1];
    neh.rep[ 2]=N[2];
    // axis Y
    neh.rep[ 4]=E[0];
    neh.rep[ 5]=E[1];
    neh.rep[ 6]=E[2];
    // axis Z
    neh.rep[ 8]=H[0];
    neh.rep[ 9]=H[1];
    neh.rep[10]=H[2];
    // gpos
    neh.rep[12]=xyzpos[0];
    neh.rep[13]=xyzpos[1];
    neh.rep[14]=xyzpos[2];
    neh.orto(1);
    }
//---------------------------------------------------------------------------
void WGS84toXYZ_posvel(double *xyzpos,double *xyzvel,double *abhpos,double *abhvel)
    {
    reper neh;
    WGS84toNEH(neh,abhpos);
    neh.gpos_get(xyzpos);
    neh.l2g_dir(xyzvel,abhvel);
    }
//---------------------------------------------------------------------------
void WGS84_m2rad(double &da,double &db,double *abh)
    {
    // WGS84 from eccentricity
    double p[3],rr;
    WGS84toXYZ(p,abh);
    rr=(p[0]*p[0])+(p[1]*p[1]);
    da=divide(1.0,sqrt(rr));
    rr+=p[2]*p[2];
    db=divide(1.0,sqrt(rr));
    }
//---------------------------------------------------------------------------
void XYZ_interpolate(double *pt,double *p0,double *p1,double t)
    {
    const double  mz=_earth_a/_earth_b;
    const double _mz=_earth_b/_earth_a;
    double p[3],r,r0,r1;
    // compute spherical radiuses of input points
    r0=sqrt((p0[0]*p0[0])+(p0[1]*p0[1])+(p0[2]*p0[2]*mz*mz));
    r1=sqrt((p1[0]*p1[0])+(p1[1]*p1[1])+(p1[2]*p1[2]*mz*mz));
    // linear interpolation
    r   = r0   +(r1   -r0   )*t;
    p[0]= p0[0]+(p1[0]-p0[0])*t;
    p[1]= p0[1]+(p1[1]-p0[1])*t;
    p[2]=(p0[2]+(p1[2]-p0[2])*t)*mz;
    // correct radius and rescale back
    r/=sqrt((p[0]*p[0])+(p[1]*p[1])+(p[2]*p[2]));
    pt[0]=p[0]*r;
    pt[1]=p[1]*r;
    pt[2]=p[2]*r*_mz;
    }
//---------------------------------------------------------------------------
#endif
//---------------------------------------------------------------------------

However they require basic 3D vector math see here for equations:

• Understanding 4x4 homogenous transform matrices