138 lines
4.3 KiB
C++
138 lines
4.3 KiB
C++
//
|
|
// Author: Joshua Cohen
|
|
// Copyright 2017
|
|
//
|
|
|
|
#include <math.h>
|
|
#include <stdio.h>
|
|
#include "isceLibConstants.h"
|
|
#include "Ellipsoid.h"
|
|
#include "LinAlg.h"
|
|
#include "Peg.h"
|
|
#include "Pegtrans.h"
|
|
using isceLib::Ellipsoid;
|
|
using isceLib::LinAlg;
|
|
using isceLib::Peg;
|
|
using isceLib::Pegtrans;
|
|
using isceLib::SCH_2_XYZ;
|
|
using isceLib::XYZ_2_SCH;
|
|
using isceLib::XYZ_2_LLH;
|
|
using isceLib::LLH_2_XYZ;
|
|
|
|
Pegtrans::Pegtrans() {
|
|
// Empty constructor
|
|
|
|
return;
|
|
}
|
|
|
|
Pegtrans::Pegtrans(const Pegtrans &p) {
|
|
// Copy constructor
|
|
|
|
for (int i=0; i<3; i++) {
|
|
ov[i] = p.ov[i];
|
|
for (int j=0; j<3; j++) {
|
|
mat[i][j] = p.mat[i][j];
|
|
matinv[i][j] = p.matinv[i][j];
|
|
}
|
|
}
|
|
radcur = p.radcur;
|
|
}
|
|
|
|
void Pegtrans::radarToXYZ(Ellipsoid &elp, Peg &peg) {
|
|
/*
|
|
* Computes the transformation matrix and translation vector needed to convert
|
|
* between radar (s,c,h) coordinates and WGS-84 (x,y,z) coordinates
|
|
*/
|
|
|
|
double llh[3], p[3], up[3];
|
|
|
|
mat[0][0] = cos(peg.lat) * cos(peg.lon);
|
|
mat[0][1] = -(sin(peg.hdg) * sin(peg.lon)) - (sin(peg.lat) * cos(peg.lon) * cos(peg.hdg));
|
|
mat[0][2] = (sin(peg.lon) * cos(peg.hdg)) - (sin(peg.lat) * cos(peg.lon) * sin(peg.hdg));
|
|
mat[1][0] = cos(peg.lat) * sin(peg.lon);
|
|
mat[1][1] = (cos(peg.lon) * sin(peg.hdg)) - (sin(peg.lat) * sin(peg.lon) * cos(peg.hdg));
|
|
mat[1][2] = -(cos(peg.lon) * cos(peg.hdg)) - (sin(peg.lat) * sin(peg.lon) * sin(peg.hdg));
|
|
mat[2][0] = sin(peg.lat);
|
|
mat[2][1] = cos(peg.lat) * cos(peg.hdg);
|
|
mat[2][2] = cos(peg.lat) * sin(peg.hdg);
|
|
for (int i=0; i<3; i++) {
|
|
for (int j=0; j<3; j++) {
|
|
matinv[i][j] = mat[j][i];
|
|
}
|
|
}
|
|
radcur = elp.rDir(peg.hdg, peg.lat);
|
|
llh[0] = peg.lat;
|
|
llh[1] = peg.lon;
|
|
llh[2] = 0.;
|
|
elp.latLon(p, llh, LLH_2_XYZ);
|
|
up[0] = cos(peg.lat) * cos(peg.lon);
|
|
up[1] = cos(peg.lat) * sin(peg.lon);
|
|
up[2] = sin(peg.lat);
|
|
for (int i=0; i<3; i++) ov[i] = p[i] - (radcur * up[i]);
|
|
}
|
|
|
|
void Pegtrans::convertSCHtoXYZ(double schv[3], double xyzv[3], int ctype) {
|
|
/*
|
|
* Applies the affine matrix provided to convert from the radar sch coordinates
|
|
* to WGS-84 xyz coordinates or vice-versa
|
|
*/
|
|
|
|
double schvt[3], llh[3];
|
|
Ellipsoid sph;
|
|
LinAlg alg;
|
|
|
|
sph.a = radcur;
|
|
sph.e2 = 0.;
|
|
if (ctype == SCH_2_XYZ) {
|
|
llh[0] = schv[1] / radcur;
|
|
llh[1] = schv[0] / radcur;
|
|
llh[2] = schv[2];
|
|
sph.latLon(schvt, llh, LLH_2_XYZ);
|
|
alg.matVec(mat, schvt, xyzv);
|
|
alg.linComb(1., xyzv, 1., ov, xyzv);
|
|
} else if (ctype == XYZ_2_SCH) {
|
|
alg.linComb(1., xyzv, -1., ov, schvt);
|
|
alg.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: Unrecognized conversion type in Pegtrans::convertSCHtoXYZ (received %d).\n", ctype);
|
|
}
|
|
}
|
|
|
|
void Pegtrans::convertSCHdotToXYZdot(double sch[3], double xyz[3], double schdot[3], double xyzdot[3], int ctype) {
|
|
/*
|
|
* Applies the affine matrix provided to convert from the radar sch velocity
|
|
* to WGS-84 xyz velocity or vice-versa
|
|
*/
|
|
|
|
double schxyzmat[3][3], xyzschmat[3][3];
|
|
LinAlg alg;
|
|
|
|
SCHbasis(sch, xyzschmat, schxyzmat);
|
|
if (ctype == SCH_2_XYZ) alg.matVec(schxyzmat, schdot, xyzdot);
|
|
else if (ctype == XYZ_2_SCH) alg.matVec(xyzschmat, xyzdot, schdot);
|
|
else printf("Error: Unrecognized conversion type in Pegtrans::convertSCHdotToXYZdot (received %d).\n", ctype);
|
|
}
|
|
|
|
void Pegtrans::SCHbasis(double sch[3], double xyzschmat[3][3], double schxyzmat[3][3]) {
|
|
// Computes the transformation matrix from xyz to a local sch frame
|
|
|
|
double matschxyzp[3][3];
|
|
LinAlg alg;
|
|
|
|
matschxyzp[0][0] = -sin(sch[0] / radcur);
|
|
matschxyzp[0][1] = -(sin(sch[1] / radcur) * cos(sch[0] / radcur));
|
|
matschxyzp[0][2] = cos(sch[0] / radcur) * cos(sch[1] / radcur);
|
|
matschxyzp[1][0] = cos(sch[0] / radcur);
|
|
matschxyzp[1][1] = -(sin(sch[1] / radcur) * sin(sch[0] / radcur));
|
|
matschxyzp[1][2] = sin(sch[0] / radcur) * cos(sch[1] / radcur);
|
|
matschxyzp[2][0] = 0.;
|
|
matschxyzp[2][1] = cos(sch[1] / radcur);
|
|
matschxyzp[2][2] = sin(sch[1] / radcur);
|
|
alg.matMat(mat, matschxyzp, schxyzmat);
|
|
alg.tranMat(schxyzmat, xyzschmat);
|
|
}
|