ISCE_INSAR/components/zerodop/GPUtopozero/src/PegTrans.cpp

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]);
}