📄 coordtrans.cpp
字号:
#include "stdafx.h"
#include "coordtrans.h"
#include "math.h"
#include "readon.h"
#include "matrix.h"
#ifndef _NO_NAMESPACE
using namespace std;
using namespace math;
#define STD std
#else
#define STD
#endif
#ifndef _NO_TEMPLATE
typedef matrix<double> Matrix;
#else
typedef matrix Matrix;
#endif
#ifndef _NO_EXCEPTION
# define TRYBEGIN() try {
# define CATCHERROR() } catch (const STD::exception& e) { \
cerr << "Error: " << e.what() << endl; }
#else
# define TRYBEGIN()
# define CATCHERROR()
#endif
CCoordTrans::CCoordTrans()
{
}
CCoordTrans::~CCoordTrans()
{
}
///////////////////////////////////////////////////////////////
void CCoordTrans::angletoarc(int a,int b,double c,double *arc)
{
*arc = (a+b/60.0+c/3600.0)*PI/180.0;
}
void CCoordTrans::arctoangle(double arc, int *a, int *b, double *c)
{
*a = (int)(arc * 180 / PI);
*b = (int)((arc *180 *60/ PI)-(*a) * 60);
*c = (arc * 180 * 3600 / PI) - (*b) * 60 - (*a) * 3600;
}
void CCoordTrans::BJtoPLANE(COORDINATEGEODETIC geodetic,int projection/*,double Ls*/,COORDINATEPLANE* plane,double *MiddleOfLat)
{
long delt;
if (projection == 3) {
*MiddleOfLat = floor ((geodetic.dLongtitude * 180.0 / PI + 1.5) / 3.0) * 3.0;
delt = (long) (*MiddleOfLat / 3.0) * 1000000;
}
if (projection == 6) {
*MiddleOfLat = (floor (geodetic.dLongtitude * 180.0 / (6.0 * PI)) + 1 ) * 6.0 - 3.0;
delt = (long) (floor (geodetic.dLongtitude * 180.0 / (6.0 * PI)) + 1 ) * 1000000;
}
// *MiddleOfLat=111;
double l;
double N;
double a0,a4,a6,a3,a5;
double cb2;
cb2 = cos(geodetic.dLatitude) * cos(geodetic.dLatitude);
l = geodetic.dLongtitude - (*MiddleOfLat) * PI / 180.0;
N = 6399698.902 - (21562.267 - (108.973 - 0.612 * cb2) * cb2) * cb2;
a0 = 32140.404 - (135.3302 - (0.7092 - 0.004 * cb2) * cb2) * cb2;
a4 = (0.25 + 0.00252 * cb2) * cb2 -0.04166;
a6 = (0.166 * cb2 - 0.084) * cb2;
a3 = (0.3333333 + 0.001123 * cb2) * cb2 - 0.1666667;
a5 = 0.0083 - (0.1667 - (0.1968 + 0.004 * cb2) * cb2) * cb2;
plane->dX = 6367558.4969 * geodetic.dLatitude - (a0 - (0.5 + (a4 + a6 * l * l) * l * l) * l * l * N) * sin(geodetic.dLatitude) * cos(geodetic.dLatitude);
plane->dY = (1 + (a3 + a5 * l * l) * l * l) * l * N * cos(geodetic.dLatitude) + 500000;
}
void CCoordTrans::XItoPLANE(COORDINATEGEODETIC geodetic, int projection, COORDINATEPLANE *plane,double *MiddleOfLat)
{
double deltaL;
double M0;
double M2;
double M4;
double M6;
double M8;
double a0;
double a2;
double a4;
double a6;
double X;
double t;
double YITA2;
double B2;
double dXI80e2;
double N;
double W;
double w;
double we;
long delt;
if (projection == 3) {
*MiddleOfLat = floor ((geodetic.dLongtitude * 180.0 / PI + 1.5) / 3.0) * 3.0;
delt = (long) (*MiddleOfLat / 3.0) * 1000000;
}
if (projection == 6) {
*MiddleOfLat = (floor (geodetic.dLongtitude * 180.0 / (6.0 * PI)) + 1 ) * 6.0 - 3.0;
delt = (long) (floor (geodetic.dLongtitude * 180.0 / (6.0 * PI)) + 1 ) * 1000000;
}
M0 = XI80a * (1 - XI80e2);
M2 = 3 * XI80e2 * M0 / 2;
M4 = 5 * XI80e2 * M2 / 4;
M6 = 7 * XI80e2 * M4 / 6;
M8 = 9 * XI80e2 * M6 / 8;
a0 = M0 + M2 / 2.0 + 3.0 * M4 / 8.0 + 5.0 * M6 / 16.0 + 35.0 * M8 / 128.0;
a2 = M2 / 2.0 + M4 / 2.0 + 15.0 * M6 / 32.0 + 7.0 * M8 / 16.0;
a4 = M4 / 8.0 + 3.0 * M6 / 16.0 + 7.0 * M8 / 32.0;
a6 = M6 / 32.0 + M8 / 16.0;
X = a0 * geodetic.dLatitude - a2 * sin (2 * geodetic.dLatitude) / 2.0
+ a4 * sin (4 * geodetic.dLatitude) / 4.0 - a6 * sin (6 * geodetic.dLatitude) / 6.0;
deltaL = geodetic.dLongtitude - *MiddleOfLat * PI / 180.0;
t = tan (geodetic.dLatitude);
B2 = XI80a * XI80a-XI80e2 * XI80a * XI80a;
dXI80e2 = XI80e2 * XI80a * XI80a / B2;
YITA2 = dXI80e2 * cos (geodetic.dLatitude) * cos (geodetic.dLatitude);
W = sqrt (1 - XI80e2 * sin(geodetic.dLatitude) * sin(geodetic.dLatitude));
N = XI80a / W;
w = cos (geodetic.dLatitude) * cos (geodetic.dLatitude) * cos (geodetic.dLatitude);
we = w * cos (geodetic.dLatitude) * cos (geodetic.dLatitude);
plane->dX = X + N * sin (geodetic.dLatitude) * cos (geodetic.dLatitude)
* deltaL * deltaL / 2.0 + N * sin (geodetic.dLatitude) * w * (5 - t * t
+ 9 * YITA2 + 4 * YITA2 * YITA2) * deltaL * deltaL * deltaL * deltaL / 24.0
+ N * sin (geodetic.dLatitude) * we * (61 - 58 * t * t + t * t* t* t) * deltaL * deltaL * deltaL * deltaL * deltaL * deltaL / 720.0;
plane->dY = N * cos (geodetic.dLatitude) * deltaL + N * w * (1 - t * t + YITA2) * deltaL * deltaL * deltaL / 6.0
+ N * we * (5 - 18 * t * t + t * t * t * t + 14 * YITA2 - 58 * YITA2 * t * t) * deltaL * deltaL * deltaL * deltaL * deltaL / 120.0 ;//+ 500000 +delt;
}
void CCoordTrans::PLANEtoBJ(COORDINATEPLANE plane,double MiddleOfLat,COORDINATEGEODETIC *geodetic)
{
///////////////////////////////////电算公式///////////////////////////////////////////////
double Bf,Bf1,FBf;
double Z;
double b2,b3,b4,b5;
double Nf;
double cbf2;
double l;
Bf = plane.dX / 111134.8611;
Bf = Bf * PI / 180.0;
do
{
Bf1 = Bf;
FBf = -16036.4803 * sin(2 * Bf1) + 16.8281 * sin(4 * Bf1) - 0.022 * sin(6 * Bf1);
Bf = (plane.dX - FBf) / 111134.8611;
Bf = Bf * PI / 180.0;
}while(fabs(Bf-Bf1) > EPSILON);
cbf2 = cos(Bf) * cos(Bf);
Nf = 6399698.902 - (21562.267 - (108.973 - 0.612 * cbf2) * cbf2) * cbf2;
Z = plane.dY / (Nf * cos(Bf));
b2 = (0.5 + 0.003369 * cbf2) * sin(Bf) * cos(Bf);
b3 = 0.333333 - (0.166667 -0.001123 * cbf2) * cbf2;
b4 = 0.25 +(0.16161 + 0.00562 * cbf2) * cbf2;
b5 = 0.2 - (0.1667 - 0.0088 * cbf2) * cbf2;
geodetic->dLatitude = Bf - (1 - (b4 - 0.12 * Z * Z) * Z * Z) * Z * Z * b2;
l = (1 - (b3 - b5 * Z * Z) * Z * Z) * Z;
geodetic->dLongtitude = MiddleOfLat * PI / 180.0 + l;
}
////////////////////////七参数转换////////////////////////////////
void CCoordTrans::seventrans(COMMANCOORD coord,SEVENPARAMETER *sp)
{
// COORDINATECARTESIAN cartesian1;
// COORDINATECARTESIAN cartesian2;
// Matrix x1(12,1);
Matrix B(12,7);
Matrix x2(12,1);
Matrix v(7,1);
int i;
double x,y,z;
/* double x;
// double N;
// double N1;
N = WGSa/sqrt (1-WGSe2*sin(geodetic.dLatitude)*sin(geodetic.dLatitude)) ;
cartesian1.dX = (N+geodetic.dHeight)*cos(geodetic.dLatitude)*cos(geodetic.dLongtitude);
cartesian1.dY = (N+geodetic.dHeight)*cos(geodetic.dLatitude)*sin(geodetic.dLongtitude);
cartesian1.dZ = (N*(1-WGSe2)+geodetic.dHeight)*sin(geodetic.dLatitude);
N1 = BJ54a/sqrt (1-BJ54e2*sin(geodetic1.dLatitude)*sin(geodetic1.dLatitude)) ;
cartesian2.dX = (N1+geodetic1.dHeight)*cos(geodetic1.dLatitude)*cos(geodetic1.dLongtitude);
cartesian2.dY = (N1+geodetic1.dHeight)*cos(geodetic1.dLatitude)*sin(geodetic1.dLongtitude);
cartesian2.dZ = (N1*(1-BJ54e2)+geodetic1.dHeight)*sin(geodetic1.dLatitude);
*/
for(i=0;i<4;i++)
{
x2(i*3,0) = coord.x2[i];//北京54坐标系直角坐标
x2(i*3+1,0) = coord.y2[i];
x2(i*3+2,0) = coord.z2[i];
B(i*3,0) = 1; //WGS84下的直角坐标
B(i*3,1) = 0;
B(i*3,2) = 0;
B(i*3,3) = coord.x1[i];
B(i*3,4) = 0;
B(i*3,5) = -coord.z1[i];
B(i*3,6) = coord.y1[i];
B(i*3+1,0) = 0;
B(i*3+1,1) = 1;
B(i*3+1,2) = 0;
B(i*3+1,3) = coord.y1[i];
B(i*3+1,4) = coord.z1[i];
B(i*3+1,5) = 0;
B(i*3+1,6) = -coord.x1[i];
B(i*3+2,0) = 0;
B(i*3+2,1) = 0;
B(i*3+2,2) = 1;
B(i*3+2,3) = coord.z1[i];
B(i*3+2,4) = -coord.y1[i];
B(i*3+2,5) = coord.x1[i];
B(i*3+2,6) = 0;
}
v = (!(~B*B))*(~B)*x2;
sp->dx = v(0,0);
sp->dy = v(1,0);
sp->dz = v(2,0);
sp->m = v(3,0) - 1;
sp->ex = v(4,0)/v(3,0);
sp->ey = v(5,0)/v(3,0);
sp->ez = v(6,0)/v(3,0);
x2 = B * v;
x = x2(3,0);
y = x2(4,0);
z = x2(5,0);
}
void CCoordTrans::WGStoBJ54(COORDINATECARTESIAN cartesian2, COORDINATECARTESIAN* cartesian1)
{
/*
COORDINATEGEODETIC oldgeodetic;
double N;
double p;
double q;
p = sqrt (cartesian2.dX * cartesian2.dX + cartesian2.dY * cartesian2.dY);
geodetic->dLatitude = atan2 (cartesian2.dZ, p);
geodetic->dLongtitude = atan2 (cartesian2.dY, cartesian2.dX);
⌨️ 快捷键说明
复制代码
Ctrl + C
搜索代码
Ctrl + F
全屏模式
F11
切换主题
Ctrl + Shift + D
显示快捷键
?
增大字号
Ctrl + =
减小字号
Ctrl + -