127 lines
3.6 KiB
C++
127 lines
3.6 KiB
C++
//
|
|
// Author: Joshua Cohen
|
|
// Copyright 2016
|
|
//
|
|
|
|
#include <cmath>
|
|
#include <cstdio>
|
|
#include <cstdlib>
|
|
#include <vector>
|
|
#include "Constants.h"
|
|
#include "LinAlg.h"
|
|
#include "PegTrans.h"
|
|
using std::vector;
|
|
|
|
// Default constructor
|
|
PegTrans::PegTrans() :
|
|
mat(3,vector<double>(3)),
|
|
matinv(3,vector<double>(3)),
|
|
ov(3) {
|
|
radcur = 0.0;
|
|
}
|
|
|
|
// Copy constructor
|
|
PegTrans::PegTrans(const PegTrans& pt) {
|
|
mat = pt.mat;
|
|
matinv = pt.matinv;
|
|
ov = pt.ov;
|
|
radcur = pt.radcur;
|
|
}
|
|
|
|
void PegTrans::convert_sch_to_xyz(vector<double> &schv, vector<double> &xyzv, int type) {
|
|
vector<double> schvt(3), llh(3);
|
|
|
|
Ellipsoid sph(radcur,0.0);
|
|
LinAlg linalg;
|
|
|
|
if (type == SCH_2_XYZ) {
|
|
llh[0] = schv[1] / radcur;
|
|
llh[1] = schv[0] / radcur;
|
|
llh[2] = schv[2];
|
|
|
|
sph.latlon(schvt,llh,LLH_2_XYZ);
|
|
linalg.matvec(mat,schvt,xyzv);
|
|
linalg.lincomb(1.0,xyzv,1.0,ov,xyzv);
|
|
} else if (type == XYZ_2_SCH) {
|
|
linalg.lincomb(1.0,xyzv,-1.0,ov,schvt);
|
|
linalg.matvec(matinv,schvt,schv);
|
|
sph.latlon(schv,llh,XYZ_2_LLH);
|
|
schv[0] = radcur * llh[1];
|
|
schv[1] = radcur * llh[0];
|
|
schv[2] = llh[2];
|
|
} else {
|
|
printf("Error in PegTrans::convert_sch_to_xyz - Unknown method passed as type.\n");
|
|
exit(1);
|
|
}
|
|
}
|
|
|
|
void PegTrans::convert_schdot_to_xyzdot(vector<double> &sch, vector<double> &xyz, vector<double> &schdot, vector<double> &xyzdot, int type) {
|
|
vector<vector<double> > schxyzmat(3,vector<double>(3)), xyzschmat(3,vector<double>(3));
|
|
|
|
LinAlg linalg;
|
|
schbasis(sch,xyzschmat,schxyzmat);
|
|
|
|
if (type == SCH_2_XYZ) linalg.matvec(schxyzmat,schdot,xyzdot);
|
|
else if (type == XYZ_2_SCH) linalg.matvec(xyzschmat,xyzdot,schdot);
|
|
else {
|
|
printf("Error in PegTrans::convert_schdot_to_xyzdot - Unknown method passed as type.\n");
|
|
exit(1);
|
|
}
|
|
}
|
|
|
|
void PegTrans::schbasis(vector<double> &sch, vector<vector<double> > &xyzschmat, vector<vector<double> > &schxyzmat) {
|
|
vector<vector<double> > matschxyzp(3,vector<double>(3));
|
|
double coss,cosc,sins,sinc;
|
|
|
|
coss = cos(sch[0] / radcur);
|
|
sins = sin(sch[0] / radcur);
|
|
cosc = cos(sch[1] / radcur);
|
|
sinc = sin(sch[1] / radcur);
|
|
matschxyzp[0][0] = -sins;
|
|
matschxyzp[0][1] = -sinc * coss;
|
|
matschxyzp[0][2] = coss * cosc;
|
|
matschxyzp[1][0] = coss;
|
|
matschxyzp[1][1] = -sinc * sins;
|
|
matschxyzp[1][2] = sins * cosc;
|
|
matschxyzp[2][0] = 0.0;
|
|
matschxyzp[2][1] = cosc;
|
|
matschxyzp[2][2] = sinc;
|
|
|
|
LinAlg linalg;
|
|
linalg.matmat(mat,matschxyzp,schxyzmat);
|
|
linalg.tranmat(schxyzmat,xyzschmat);
|
|
}
|
|
|
|
void PegTrans::radar_to_xyz(Ellipsoid &elp, Peg &peg) {
|
|
vector<double> llh(3), p(3), up(3);
|
|
double plat = peg.lat;
|
|
double plon = peg.lon;
|
|
double phdg = peg.hdg;
|
|
|
|
mat[0][0] = cos(plat) * cos(plon);
|
|
mat[0][1] = (-sin(phdg) * sin(plon)) - (sin(plat) * cos(plon) * cos(phdg));
|
|
mat[0][2] = (sin(plon) * cos(phdg)) - (sin(plat) * cos(plon) * sin(phdg));
|
|
mat[1][0] = cos(plat) * sin(plon);
|
|
mat[1][1] = (cos(plon) * sin(phdg)) - (sin(plat) * sin(plon) * cos(phdg));
|
|
mat[1][2] = (-cos(plon) * cos(phdg)) - (sin(plat) * sin(plon) * sin(phdg));
|
|
mat[2][0] = sin(plat);
|
|
mat[2][1] = cos(plat) * cos(phdg);
|
|
mat[2][2] = cos(plat) * sin(phdg);
|
|
|
|
for (int i=0; i<3; i++) for (int j=0; j<3; j++) matinv[i][j] = mat[j][i];
|
|
|
|
radcur = elp.rdir(phdg,plat);
|
|
|
|
llh[0] = plat;
|
|
llh[1] = plon;
|
|
llh[2] = 0.0;
|
|
elp.latlon(p,llh,LLH_2_XYZ);
|
|
|
|
up[0] = cos(plat) * cos(plon);
|
|
up[1] = cos(plat) * sin(plon);
|
|
up[2] = sin(plat);
|
|
|
|
for (int i=0; i<3; i++) ov[i] = p[i] - (radcur * up[i]);
|
|
}
|
|
|