345 lines
12 KiB
C
345 lines
12 KiB
C
|
#include "coordtrans.h"
|
|||
|
|
#include <math.h>
|
|||
|
|
|
|||
|
|
struct Datum g_coord_p[12]; // 椭球参数集保存结构
|
|||
|
|
|
|||
|
|
/*! 椭球参数初始化
|
|||
|
|
* @param a 椭球长半轴
|
|||
|
|
* @param f 椭球扁率
|
|||
|
|
* @param L0 本初子午线
|
|||
|
|
*/
|
|||
|
|
void DatumInitialize(struct Datum *newd, double a, double f, int l0) {
|
|||
|
|
if (newd == NULL)
|
|||
|
|
return;
|
|||
|
|
double b = a - f * a;
|
|||
|
|
double e12 = (a * a - b * b) / (a * a);
|
|||
|
|
double e22 = (a * a - b * b) / (b * b);
|
|||
|
|
|
|||
|
|
// local BJ54 = new Datum(6378245, 1/298.3)
|
|||
|
|
// local XA80 = new Datum(6378140, 1/298.257)
|
|||
|
|
// local WGS84 = new Datum(6378137, 1/298.257223563)
|
|||
|
|
// local WGS2000 = new Datum(6378137, 1/298.257222101)
|
|||
|
|
newd->A = a; // 椭球长半轴
|
|||
|
|
newd->F = f; // 椭球扁率
|
|||
|
|
newd->B = b; // 椭球短半轴
|
|||
|
|
newd->E12 = e12; // 椭球第一偏心率平方
|
|||
|
|
newd->E22 = e22; // 椭球第二偏心率平方
|
|||
|
|
newd->L0 = l0; // 本初子午线
|
|||
|
|
newd->zoneWidth = 6; // 度带
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
/*! 设置度带
|
|||
|
|
* @param zoneWidth 度带
|
|||
|
|
*/
|
|||
|
|
void DatumSetZoneWidth(struct Datum *newd, int zoneWidth) {
|
|||
|
|
if (newd == NULL)
|
|||
|
|
return;
|
|||
|
|
if (zoneWidth != 6 && zoneWidth != 3) {
|
|||
|
|
printf("带宽应当为6或者为3");
|
|||
|
|
} else {
|
|||
|
|
newd->zoneWidth = zoneWidth;
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
/*! 获取度带带号
|
|||
|
|
* @param l 当地经度
|
|||
|
|
* @return 度带带号
|
|||
|
|
*/
|
|||
|
|
int DatumGetZoneNo(struct Datum *newd, double l) {
|
|||
|
|
// 6度带
|
|||
|
|
// 这里应为向下取整
|
|||
|
|
if (newd->zoneWidth == 6) {
|
|||
|
|
return floor((l + 6) / 6);
|
|||
|
|
}
|
|||
|
|
// 3度带
|
|||
|
|
return floor((l + 1.5) / 3);
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
/*! 椭球参数计算
|
|||
|
|
* @param val 所指定的坐标系
|
|||
|
|
* @param L0 中央子午线
|
|||
|
|
* @return 各坐标系下的椭球参数信息
|
|||
|
|
*/
|
|||
|
|
void DatumEllipsoidal(int L0, enum GEODETIC_COORD_SYSTEM val, struct Datum *out) {
|
|||
|
|
DatumInitialize(&g_coord_p[BJ54], 6378245, 1 / 298.3, L0);
|
|||
|
|
DatumInitialize(&g_coord_p[XA80], 6378140, 1 / 298.257, L0);
|
|||
|
|
DatumInitialize(&g_coord_p[WGS84], 6378137, 1 / 298.257223563, L0);
|
|||
|
|
DatumInitialize(&g_coord_p[WGS2000], 6378137, 1 / 298.257222101, L0);
|
|||
|
|
/*
|
|||
|
|
double A; // 椭球长半轴
|
|||
|
|
double F; // 椭球扁率
|
|||
|
|
double B; // 椭球短半轴
|
|||
|
|
double E12; // 椭球第一偏心率平方
|
|||
|
|
double E22; // 椭球第二偏心率平方
|
|||
|
|
int L0; // 本初子午线
|
|||
|
|
int zoneWidth; // 度带
|
|||
|
|
*/
|
|||
|
|
/*
|
|||
|
|
printf("XA80: A:%lf, B:%lf, F:%lf, E12:%lf, E22:%lf, L0:%d, zoneWidth:%d\n",
|
|||
|
|
g_coord_p[XA80].A, g_coord_p[XA80].B, g_coord_p[XA80].F, g_coord_p[XA80].E12,
|
|||
|
|
g_coord_p[XA80].E22, g_coord_p[XA80].L0, g_coord_p[XA80].zoneWidth);
|
|||
|
|
printf("WGS84: A:%lf, B:%lf, F:%lf, E12:%lf, E22:%lf, L0:%d, zoneWidth:%d\n",
|
|||
|
|
g_coord_p[WGS84].A, g_coord_p[WGS84].B, g_coord_p[WGS84].F, g_coord_p[WGS84].E12,
|
|||
|
|
g_coord_p[WGS84].E22, g_coord_p[WGS84].L0, g_coord_p[WGS84].zoneWidth);
|
|||
|
|
*/
|
|||
|
|
if (out == NULL)
|
|||
|
|
return;
|
|||
|
|
*out = g_coord_p[val];
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
/*! 输出平面坐标
|
|||
|
|
* @param p 地心大地坐标参数 p.B:纬度，p.L:经度
|
|||
|
|
* @param config 要转换成坐标系下的七参数信息
|
|||
|
|
* @return 输出投影的平面坐标
|
|||
|
|
*/
|
|||
|
|
struct Point d2p(struct Point *p, struct ConfigParam *config) {
|
|||
|
|
struct Point zj = BLH2XYZ(p, &g_coord_p[config->gcsSrc]);
|
|||
|
|
struct Point xzj = XYZ2XYZ(&zj, config->dx, config->dy, config->dz, config->wx, config->wy, config->wz, config->k);
|
|||
|
|
struct Point xdd = XYZ2BLH(&xzj, &g_coord_p[config->gcsDst]);
|
|||
|
|
struct Point xpm = BL2xy(&xdd, &g_coord_p[config->gcsDst], config->height, NULL, NULL);
|
|||
|
|
return xpm;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
/*! 地心大地坐标系 转换到 地心直角坐标系
|
|||
|
|
* @param pointBLH 地心大地坐标系参数 B:维度 L:经度 H:高程
|
|||
|
|
* @param datum 椭球参数
|
|||
|
|
*/
|
|||
|
|
struct Point BLH2XYZ(struct Point *pointBLH, struct Datum *datum) {
|
|||
|
|
double a = datum->A;
|
|||
|
|
double e12 = datum->E12;
|
|||
|
|
double radB = pointBLH->x / 180 * MATH_PI;
|
|||
|
|
double radL = pointBLH->y / 180 * MATH_PI;
|
|||
|
|
double H = pointBLH->z;
|
|||
|
|
double N = a / sqrt(1 - e12 * sin(radB) * sin(radB)); // 卯酉圈半径
|
|||
|
|
struct Point res;
|
|||
|
|
res.x = (N + H) * cos(radB) * cos(radL);
|
|||
|
|
res.y = (N + H) * cos(radB) * sin(radL);
|
|||
|
|
res.z = (N * (1 - e12) + H) * sin(radB);
|
|||
|
|
return res;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
/*! 空间直角坐标系间的 七参数转换
|
|||
|
|
* @note: 不同椭球参数下， 地心直角坐标系之间转换
|
|||
|
|
* @param: dX, dY, dZ: 三个坐标方向的平移参数
|
|||
|
|
* @param: wX, wY, wZ: 三个方向的旋转角参数(单位为弧度)
|
|||
|
|
* @param: Kppm: 尺度参数， 单位是ppm，如果是以米为单位， 需要在传参前 除以1000000
|
|||
|
|
*/
|
|||
|
|
struct Point
|
|||
|
|
XYZ2XYZ(struct Point *source, double dX, double dY, double dZ, double wX, double wY, double wZ, double Kppm) {
|
|||
|
|
double X = source->x;
|
|||
|
|
double Y = source->y;
|
|||
|
|
double Z = source->z;
|
|||
|
|
// wX = wX or 0
|
|||
|
|
// wY = wY or 0
|
|||
|
|
// wZ = wZ or 0
|
|||
|
|
// Kppm = Kppm or 0
|
|||
|
|
wX = wX / 3600 / 180 * MATH_PI;
|
|||
|
|
wY = wY / 3600 / 180 * MATH_PI;
|
|||
|
|
wZ = wZ / 3600 / 180 * MATH_PI;
|
|||
|
|
// wX = wX / math.PI * 3600 * 180
|
|||
|
|
// wY = wY / math.PI * 3600 * 180
|
|||
|
|
// wZ = wZ / math.PI * 3600 * 180
|
|||
|
|
// Kppm = Kppm - 1
|
|||
|
|
struct Point res;
|
|||
|
|
res.x = X + dX + Kppm * X - wY * Z + wZ * Y; // Kppm 公式中尺度因子为 1+Kppm
|
|||
|
|
res.y = Y + dY + Kppm * Y + wX * Z - wZ * X;
|
|||
|
|
res.z = Z + dZ + Kppm * Z - wX * Y + wY * X;
|
|||
|
|
return res;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
/*! 地心直角坐标系 转换到 地心大地坐标系
|
|||
|
|
* @note: 用直接法2 https://wenku.baidu.com/view/30a08f9ddd88d0d233d46a50.html
|
|||
|
|
*/
|
|||
|
|
struct Point
|
|||
|
|
XYZ2BLH(struct Point *pointXYZ, struct Datum *datum) {
|
|||
|
|
double X = pointXYZ->x;
|
|||
|
|
double Y = pointXYZ->y;
|
|||
|
|
double Z = pointXYZ->z;
|
|||
|
|
double L = atan(Y / X);
|
|||
|
|
// 弧度转角度
|
|||
|
|
double degL = L * 180 / MATH_PI;
|
|||
|
|
// Y值为正， 东半球， 否则西半球
|
|||
|
|
if (Y > 0) {
|
|||
|
|
while (degL < 0) {
|
|||
|
|
degL = degL + 180;
|
|||
|
|
}
|
|||
|
|
while (degL > 180) {
|
|||
|
|
degL = degL - 180;
|
|||
|
|
}
|
|||
|
|
} else {
|
|||
|
|
while (degL > 0) {
|
|||
|
|
degL = degL - 180;
|
|||
|
|
}
|
|||
|
|
while (degL < -180) {
|
|||
|
|
degL = degL + 180;
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
double a = datum->A;
|
|||
|
|
double b = datum->B;
|
|||
|
|
double e12 = datum->E12;
|
|||
|
|
double e22 = datum->E22;
|
|||
|
|
|
|||
|
|
double tgU = Z / (sqrt(X * X + Y * Y) * sqrt(1 - e12));
|
|||
|
|
double U = atan(tgU);
|
|||
|
|
|
|||
|
|
double tgB = (Z + b * e22 * pow(sin(U), 3)) / (sqrt(X * X + Y * Y) - a * e12 * pow(cos(U), 3));
|
|||
|
|
double B = atan(tgB);
|
|||
|
|
double degB = B * 180 / MATH_PI; // 弧度转角度
|
|||
|
|
|
|||
|
|
if (Z > 0) { // Z值为正， 北半球， 否则南半球
|
|||
|
|
while (degB < 0) {
|
|||
|
|
degB = degB + 90;
|
|||
|
|
}
|
|||
|
|
while (degB > 90) {
|
|||
|
|
degB = degB - 90;
|
|||
|
|
}
|
|||
|
|
} else {
|
|||
|
|
while (degB > 0) {
|
|||
|
|
degB = degB - 90;
|
|||
|
|
}
|
|||
|
|
while (degB < -90) {
|
|||
|
|
degB = degB + 90;
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
while (degB < 0) {
|
|||
|
|
degB = degB + 360;
|
|||
|
|
}
|
|||
|
|
while (degB > 360) {
|
|||
|
|
degB = degB - 360;
|
|||
|
|
}
|
|||
|
|
double N = a / sqrt(1 - e12 * sin(B) * sin(B)); // 卯酉圈半径
|
|||
|
|
double H = 0;
|
|||
|
|
// B接近极区， 在±90°附近
|
|||
|
|
if (fabs(degB) > 80) {
|
|||
|
|
H = Z / sin(B) - N * (1 - e12);
|
|||
|
|
} else {
|
|||
|
|
H = sqrt(X * X + Y * Y) / cos(B) - N;
|
|||
|
|
}
|
|||
|
|
struct Point res;
|
|||
|
|
res.x = degB;
|
|||
|
|
res.y = degL;
|
|||
|
|
res.z = H;
|
|||
|
|
return res;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
/*! 地心大地坐标系 转换到 大地平面坐标系
|
|||
|
|
* @note: 高斯投影坐标
|
|||
|
|
* @param: prjHeight: 投影面高程
|
|||
|
|
* http://www.cnblogs.com/imeiba/p/5696967.html
|
|||
|
|
*/
|
|||
|
|
struct Point
|
|||
|
|
BL2xy(struct Point *pointBLH, struct Datum *datum, double prjHeight, double *offsetX_o, double *offsetY_o) {
|
|||
|
|
double a = datum->A;
|
|||
|
|
double b = datum->B;
|
|||
|
|
double e12 = datum->E12;
|
|||
|
|
double e22 = datum->E22;
|
|||
|
|
|
|||
|
|
double offsetX;
|
|||
|
|
double offsetY;
|
|||
|
|
// prjHeight = prjHeight or 0
|
|||
|
|
// offsetX = offsetX or 0
|
|||
|
|
if (offsetX_o == NULL)
|
|||
|
|
offsetX = 0;
|
|||
|
|
else
|
|||
|
|
offsetX = *offsetX_o;
|
|||
|
|
// offsetY = offsetY or 500000
|
|||
|
|
if (offsetY_o == NULL)
|
|||
|
|
offsetY = 500000;
|
|||
|
|
else
|
|||
|
|
offsetY = *offsetY_o;
|
|||
|
|
|
|||
|
|
int L0 = datum->L0;
|
|||
|
|
if (L0 == 0) {
|
|||
|
|
int zoneNo = DatumGetZoneNo(datum, pointBLH->y);
|
|||
|
|
L0 = (zoneNo - 0.5) * datum->zoneWidth;
|
|||
|
|
datum->L0 = L0;
|
|||
|
|
}
|
|||
|
|
double radL0 = (((double)L0 / (double)180 )* MATH_PI);
|
|||
|
|
double radB = pointBLH->x / 180 * MATH_PI;
|
|||
|
|
double radL = pointBLH->y / 180 * MATH_PI;
|
|||
|
|
double N = a / sqrt(1 - e12 * sin(radB) * sin(radB)); // 卯酉圈半径
|
|||
|
|
double T = tan(radB) * tan(radB);
|
|||
|
|
double C = e22 * cos(radB) * cos(radB);
|
|||
|
|
double A = (radL - radL0) * cos(radB);
|
|||
|
|
double M = a * ((1 - e12 / 4 - 3 * e12 * e12 / 64 - 5 * e12 * e12 * e12 / 256) * radB -
|
|||
|
|
(3 * e12 / 8 + 3 * e12 * e12 / 32 + 45 * e12 * e12 * e12 / 1024) * sin(2 * radB) +
|
|||
|
|
(15 * e12 * e12 / 256 + 45 * e12 * e12 * e12 / 1024) * sin(4 * radB) -
|
|||
|
|
(35 * e12 * e12 * e12 / 3072) * sin(6 * radB));
|
|||
|
|
// x,y的计算公式见孔祥元等主编武汉大学出版社2002年出版的《控制测量学》的第72页书的的括号有问题，( 和 [ 应该交换
|
|||
|
|
double x = M + N * tan(radB) *
|
|||
|
|
(A * A / 2 + (5 - T + 9 * C + 4 * C * C) * A * A * A * A / 24 +
|
|||
|
|
(61 - 58 * T + T * T + 600 * C - 330 * e22) * A * A * A * A * A * A / 720); // ??x和y的计算公式
|
|||
|
|
double y = N * (A + (1 - T + C) * A * A * A / 6 + (5 - 18 * T * T * T + 72 * C - 58 * e22) * A * A * A * A * A / 120);
|
|||
|
|
struct Point res;
|
|||
|
|
res.x = offsetX + x * (b + prjHeight) / b;
|
|||
|
|
res.y = offsetY + y * (b + prjHeight) / b;
|
|||
|
|
res.z = pointBLH->z;
|
|||
|
|
return res;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
/*! 输出大地坐标
|
|||
|
|
*/
|
|||
|
|
struct Point p2d(struct Point *pointxy, struct ConfigParam *config) {
|
|||
|
|
struct Point bl = xy2BL(pointxy, &g_coord_p[config->gcsSrc], config->height, NULL, NULL);
|
|||
|
|
struct Point zj = BLH2XYZ(&bl, &g_coord_p[config->gcsSrc]);
|
|||
|
|
struct Point xzj =
|
|||
|
|
XYZ2XYZ(&zj, -config->dx, -config->dy, -config->dz, -config->wx, -config->wy, -config->wz, -config->k);
|
|||
|
|
struct Point xdd = XYZ2BLH(&xzj, &g_coord_p[config->gcsDst]);
|
|||
|
|
return xdd;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
/*! 大地平面坐标系 转换到 地心大地坐标系
|
|||
|
|
* @param: prjHeight: 投影面高程
|
|||
|
|
* http://www.cnblogs.com/imeiba/p/5696967.html
|
|||
|
|
*/
|
|||
|
|
struct Point xy2BL(struct Point *pointxy, struct Datum *datum, double prjHeight, double *offsetX_o, double *offsetY_o) {
|
|||
|
|
double a = datum->A;
|
|||
|
|
double b = datum->B;
|
|||
|
|
double e12 = datum->E12;
|
|||
|
|
double e22 = datum->E22;
|
|||
|
|
double offsetX;
|
|||
|
|
double offsetY;
|
|||
|
|
// prjHeight = prjHeight or 0
|
|||
|
|
// offsetX = offsetX or 0
|
|||
|
|
if (offsetX_o == NULL)
|
|||
|
|
offsetX = 0;
|
|||
|
|
else
|
|||
|
|
offsetX = *offsetX_o;
|
|||
|
|
// offsetY = offsetY or 500000
|
|||
|
|
if (offsetY_o == NULL)
|
|||
|
|
offsetY = 500000;
|
|||
|
|
else
|
|||
|
|
offsetY = *offsetY_o;
|
|||
|
|
|
|||
|
|
double e1 = (1 - sqrt(1 - e12)) / (1 + sqrt(1 - e12));
|
|||
|
|
int L0 = datum->L0;
|
|||
|
|
double radL0 = (((double)L0 / (double)180) * MATH_PI);
|
|||
|
|
// 带内大地坐标
|
|||
|
|
double Y = fmod(pointxy->y, 1000000.0000000); // pointxy->y % 1000000.0000000;
|
|||
|
|
double x = (pointxy->x - offsetX) * b / (b + prjHeight);
|
|||
|
|
double y = (Y - offsetY) * b / (b + prjHeight);
|
|||
|
|
|
|||
|
|
double u = x / (a * (1 - e12 / 4 - 3 * e12 * e12 / 64 - 5 * e12 * e12 * e12 / 256));
|
|||
|
|
double fai = u + (3 * e1 / 2 - 27 * e1 * e1 * e1 / 32) * sin(2 * u) +
|
|||
|
|
(21 * e1 * e1 / 16 - 55 * e1 * e1 * e1 * e1 / 32) * sin(4 * u) + (151 * e1 * e1 * e1 / 96) * sin(6 * u) +
|
|||
|
|
(1097 * e1 * e1 * e1 * e1 / 512) * sin(8 * u);
|
|||
|
|
double C = e22 * cos(fai) * cos(fai);
|
|||
|
|
double T = tan(fai) * tan(fai);
|
|||
|
|
double N = a / sqrt(1 - e12 * sin(fai) * sin(fai));
|
|||
|
|
double R = a * (1 - e12) /
|
|||
|
|
sqrt((1 - e12 * sin(fai) * sin(fai)) * (1 - e12 * sin(fai) * sin(fai)) * (1 - e12 * sin(fai) * sin(fai)));
|
|||
|
|
double D = y / N;
|
|||
|
|
|
|||
|
|
double L = radL0 + (D - (1 + 2 * T + C) * D * D * D / 6 +
|
|||
|
|
(5 - 2 * C + 28 * T - 3 * C * C + 8 * e22 + 24 * T * T) * D * D * D * D * D / 120) /
|
|||
|
|
cos(fai);
|
|||
|
|
double B = fai - (N * tan(fai) / R) *
|
|||
|
|
(D * D / 2 - (5 + 3 * T + 10 * C - 4 * C * C - 9 * e22) * D * D * D * D / 24 +
|
|||
|
|
(61 + 90 * T + 298 * C + 45 * T * T - 256 * e22 - 3 * C * C) * D * D * D * D * D * D / 720);
|
|||
|
|
struct Point res;
|
|||
|
|
res.x = B * 180 / MATH_PI;
|
|||
|
|
res.y = L * 180 / MATH_PI;
|
|||
|
|
res.z = pointxy->z;
|
|||
|
|
return res;
|
|||
|
|
}
|