152 lines
4.2 KiB
C++
152 lines
4.2 KiB
C++
//
|
|
// Author: Joshua Cohen
|
|
// Copyright 2016
|
|
//
|
|
|
|
#include <cmath>
|
|
#include <cstdio>
|
|
#include <cstdlib>
|
|
#include "Constants.h"
|
|
#include "Ellipsoid.h"
|
|
#include "LinAlg.h"
|
|
|
|
// Default constructor
|
|
Ellipsoid::Ellipsoid() {
|
|
a = 0.0;
|
|
e2 = 0.0;
|
|
}
|
|
|
|
// Direct constructor
|
|
Ellipsoid::Ellipsoid(double i1, double i2) {
|
|
a = i1;
|
|
e2 = i2;
|
|
}
|
|
|
|
// Copy constructor
|
|
Ellipsoid::Ellipsoid(const Ellipsoid &elp) {
|
|
a = elp.a;
|
|
e2 = elp.e2;
|
|
}
|
|
|
|
void Ellipsoid::latlon(double v[3], double llh[3], int type) {
|
|
if (type == LLH_2_XYZ) {
|
|
double re;
|
|
|
|
re = a / sqrt(1.0 - (e2 * pow(sin(llh[0]),2)));
|
|
v[0] = (re + llh[2]) * cos(llh[0]) * cos(llh[1]);
|
|
v[1] = (re + llh[2]) * cos(llh[0]) * sin(llh[1]);
|
|
v[2] = ((re * (1.0 - e2)) + llh[2]) * sin(llh[0]);
|
|
} else if (type == XYZ_2_LLH) { // More accurate version derived from newer Python code
|
|
double d,k,p,q,r,rv,s,t,u,w;
|
|
|
|
p = (pow(v[0],2) + pow(v[1],2)) / pow(a,2);
|
|
q = ((1.0 - e2) * pow(v[2],2)) / pow(a,2);
|
|
r = (p + q - pow(e2,2)) / 6.0;
|
|
s = (pow(e2,2) * p * q) / (4.0 * pow(r,3));
|
|
t = pow((1.0 + s + sqrt(s * (2.0 + s))),(1.0/3.0));
|
|
u = r * (1.0 + t + (1.0 / t));
|
|
rv = sqrt(pow(u,2) + (pow(e2,2) * q));
|
|
w = (e2 * (u + rv - q)) / (2.0 * rv);
|
|
k = sqrt(u + rv + pow(w,2)) - w;
|
|
d = (k * sqrt(pow(v[0],2) + pow(v[1],2))) / (k + e2);
|
|
llh[0] = atan2(v[2], d);
|
|
llh[1] = atan2(v[1], v[0]);
|
|
llh[2] = ((k + e2 - 1.0) * sqrt(pow(d,2) + pow(v[2],2))) / k;
|
|
} else if (type == XYZ_2_LLH_OLD) { // Less accurate version derived from original Fortran code
|
|
double b,p,q,q3,re,theta;
|
|
|
|
q = sqrt(1.0 / (1.0 - e2));
|
|
q3 = (1.0 / (1.0 - e2)) - 1.0;
|
|
b = a * sqrt(1.0 - e2);
|
|
llh[1] = atan2(v[1], v[0]);
|
|
p = sqrt(pow(v[0],2) + pow(v[1],2));
|
|
theta = atan((v[2] / p) * q);
|
|
llh[0] = atan((v[2] + (q3 * b * pow(sin(theta),3))) / (p - (e2 * a * pow(cos(theta),3))));
|
|
re = a / sqrt(1.0 - (e2 * pow(sin(llh[0]),2)));
|
|
llh[2] = (p / cos(llh[0])) - re;
|
|
} else {
|
|
printf("Error in Ellipsoid::latlon - Unknown method passed as type.\n");
|
|
exit(1);
|
|
}
|
|
}
|
|
|
|
double Ellipsoid::reast(double lat) {
|
|
double ret;
|
|
|
|
ret = a / sqrt(1.0 - (e2 * pow(sin(lat),2)));
|
|
return ret;
|
|
}
|
|
|
|
double Ellipsoid::rnorth(double lat) {
|
|
double ret;
|
|
|
|
ret = (a * (1.0 - e2)) / pow((1.0 - (e2 * pow(sin(lat),2))),1.5);
|
|
return ret;
|
|
}
|
|
|
|
double Ellipsoid::rdir(double hdg, double lat) {
|
|
double re,rn,ret;
|
|
|
|
re = reast(lat);
|
|
rn = rnorth(lat);
|
|
ret = (re * rn) / ((re * pow(cos(hdg),2)) + (rn * pow(sin(hdg),2)));
|
|
return ret;
|
|
}
|
|
|
|
void Ellipsoid::getangs(double pos[3], double vel[3], double vec[3], double &az, double &lk) {
|
|
double c[3], n[3], t[3], llh[3], temp[3];
|
|
double tvt,tvc,dd,vecnorm;
|
|
|
|
LinAlg linalg;
|
|
|
|
latlon(pos,llh,XYZ_2_LLH);
|
|
n[0] = -cos(llh[0]) * cos(llh[1]);
|
|
n[1] = -cos(llh[0]) * sin(llh[1]);
|
|
n[2] = -sin(llh[0]);
|
|
dd = linalg.dot(n,vec);
|
|
vecnorm = linalg.norm(vec);
|
|
lk = acos(dd / vecnorm);
|
|
linalg.cross(n,vel,temp);
|
|
linalg.unitvec(temp,c);
|
|
linalg.cross(c,n,temp);
|
|
linalg.unitvec(temp,t);
|
|
tvt = linalg.dot(t,vec);
|
|
tvc = linalg.dot(c,vec);
|
|
az = atan2(tvc,tvt);
|
|
}
|
|
|
|
void Ellipsoid::getTVN_TCvec(double pos[3], double vel[3], double vec[3], double TCvec[3]) {
|
|
double c[3], n[3], t[3], llh[3], temp[3];
|
|
double tvt,tvc;
|
|
|
|
LinAlg linalg;
|
|
|
|
latlon(pos,llh,XYZ_2_LLH);
|
|
n[0] = -cos(llh[0]) * cos(llh[1]);
|
|
n[1] = -cos(llh[0]) * sin(llh[1]);
|
|
n[2] = -sin(llh[0]);
|
|
linalg.cross(n,vel,temp);
|
|
linalg.unitvec(temp,c);
|
|
linalg.cross(c,n,temp);
|
|
linalg.unitvec(temp,t);
|
|
tvt = linalg.dot(t,vec);
|
|
tvc = linalg.dot(c,vec);
|
|
for (int i=0; i<3; i++) TCvec[i] = (tvt * t[i]) + (tvc * c[i]);
|
|
}
|
|
|
|
void Ellipsoid::tcnbasis(double pos[3], double vel[3], double t[3], double c[3], double n[3]) {
|
|
double llh[3], temp[3];
|
|
|
|
LinAlg linalg;
|
|
|
|
latlon(pos,llh,XYZ_2_LLH);
|
|
n[0] = -cos(llh[0]) * cos(llh[1]);
|
|
n[1] = -cos(llh[0]) * sin(llh[1]);
|
|
n[2] = -sin(llh[0]);
|
|
linalg.cross(n,vel,temp);
|
|
linalg.unitvec(temp,c);
|
|
linalg.cross(c,n,temp);
|
|
linalg.unitvec(temp,t);
|
|
}
|
|
|