88 lines
1.8 KiB
C
88 lines
1.8 KiB
C
|
#include "linalg3.h"
|
||
|
|
#include "geometry.h"
|
||
|
|
|
||
|
|
const double d2r = 0.017453292519943295;
|
||
|
|
|
||
|
|
void printPoint(double x[3])
|
||
|
|
{
|
||
|
|
printf("%f , %f , %f \n", x[0], x[1], x[2]);
|
||
|
|
}
|
||
|
|
|
||
|
|
int main()
|
||
|
|
{
|
||
|
|
|
||
|
|
cPeg peg;
|
||
|
|
cPegtrans trans;
|
||
|
|
cEllipsoid elp;
|
||
|
|
double llh1[3];
|
||
|
|
double llh2[3];
|
||
|
|
double xyz1[3];
|
||
|
|
double xyz2[3];
|
||
|
|
|
||
|
|
double dist, hdg, rad;
|
||
|
|
|
||
|
|
|
||
|
|
elp.a = 6378137.0;
|
||
|
|
elp.e2 = 0.0066943799901;
|
||
|
|
|
||
|
|
printf("Testing LLH to XYZ conversion: \n");
|
||
|
|
llh1[0] = 40.15*d2r; llh1[1] = -104.97*d2r; llh1[2]=2119.0;
|
||
|
|
xyz2[0] = -1261499.8108277766;
|
||
|
|
xyz2[1] = -4717861.0677524200;
|
||
|
|
xyz2[2] = 4092096.6400047773;
|
||
|
|
|
||
|
|
latlon_C(&elp, xyz1, llh1, LLH_2_XYZ);
|
||
|
|
|
||
|
|
printf("Pt1 : ");
|
||
|
|
printPoint(xyz1);
|
||
|
|
printf("Pt2 : ");
|
||
|
|
printPoint(xyz2);
|
||
|
|
|
||
|
|
xyz2[0] -= xyz1[0];
|
||
|
|
xyz2[1] -= xyz1[1];
|
||
|
|
xyz2[2] -= xyz1[2];
|
||
|
|
printf("Vector: ");
|
||
|
|
printPoint(xyz2);
|
||
|
|
|
||
|
|
dist = norm_C(xyz2);
|
||
|
|
printf("Estimated Error: %f \n", dist);
|
||
|
|
printf("\n \n");
|
||
|
|
|
||
|
|
printf("Testing XYZ to LLH conversion : \n");
|
||
|
|
latlon_C(&elp, xyz1, llh2, XYZ_2_LLH);
|
||
|
|
printf("Pt1 : ");
|
||
|
|
printPoint(llh2);
|
||
|
|
printf("Pt2 : ");
|
||
|
|
printPoint(llh1);
|
||
|
|
|
||
|
|
llh1[0] -= llh2[0];
|
||
|
|
llh1[1] -= llh2[1];
|
||
|
|
llh1[2] -= llh2[2];
|
||
|
|
|
||
|
|
printf("Vector: ");
|
||
|
|
printPoint(llh1);
|
||
|
|
dist = norm_C(llh1);
|
||
|
|
printf("Estimated error : %f \n", dist);
|
||
|
|
printf("\n \n");
|
||
|
|
|
||
|
|
|
||
|
|
printf("Testing radius of curvature: ");
|
||
|
|
llh1[0] = 40.0*d2r; llh1[1] = -105.0*d2r; llh1[2]=2000.0;
|
||
|
|
xyz1[0] = 6386976.165976;
|
||
|
|
xyz1[1] = 6361815.825934;
|
||
|
|
xyz1[2] = 6386976.165976;
|
||
|
|
xyz2[0] = reast_C(&elp, llh1[0]);
|
||
|
|
xyz2[1] = rnorth_C(&elp, llh1[0]);
|
||
|
|
xyz2[2] = rdir_C(&elp, 90.0*d2r, llh1[0]);
|
||
|
|
printf("Pt 1: ");
|
||
|
|
printPoint(llh1);
|
||
|
|
printf("Radii : ");
|
||
|
|
printPoint(xyz2);
|
||
|
|
printf("Ref: ");
|
||
|
|
printPoint(xyz1);
|
||
|
|
|
||
|
|
|
||
|
|
return 0;
|
||
|
|
}
|
||
|
|
|