684 lines
19 KiB
Dart
684 lines
19 KiB
Dart
|
// ignore_for_file: non_constant_identifier_names
|
|||
|
|
|
|||
|
|
import 'dart:math';
|
|||
|
|
|
|||
|
|
class CoordGPS {
|
|||
|
|
double LAT; //纬度
|
|||
|
|
double LNG; //经度
|
|||
|
|
double? ALT; //高度
|
|||
|
|
CoordGPS({this.ALT, required this.LAT, required this.LNG});
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
class CoordXYH {
|
|||
|
|
double X;
|
|||
|
|
double Y;
|
|||
|
|
double H;
|
|||
|
|
CoordXYH({required this.X, required this.Y, required this.H});
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
class CoordBLH {
|
|||
|
|
double B;
|
|||
|
|
double L;
|
|||
|
|
double H;
|
|||
|
|
CoordBLH({required this.B, required this.H, required this.L});
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
class CoordXYZ {
|
|||
|
|
double X;
|
|||
|
|
double Y;
|
|||
|
|
double Z;
|
|||
|
|
CoordXYZ({required this.X, required this.Y, required this.Z});
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
class PointXY {
|
|||
|
|
double X;
|
|||
|
|
double Y;
|
|||
|
|
PointXY({required this.X, required this.Y});
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
class Region {
|
|||
|
|
double X;
|
|||
|
|
double Y;
|
|||
|
|
double? width;
|
|||
|
|
double? heigth;
|
|||
|
|
double? D;
|
|||
|
|
Region({this.D, required this.X, required this.Y, this.heigth, this.width});
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
class Ellipsoid {
|
|||
|
|
late double A;
|
|||
|
|
late double F;
|
|||
|
|
late double B;
|
|||
|
|
late double E12;
|
|||
|
|
late double E22;
|
|||
|
|
Ellipsoid(String name) {
|
|||
|
|
const Map<String, Map<String, double>> datums = {
|
|||
|
|
'BJ54': {'a': 6378245, 'f': 1 / 298.3},
|
|||
|
|
'XA80': {'a': 6378140, 'f': 1 / 298.257},
|
|||
|
|
'WGS84': {'a': 6378137, 'f': 1 / 298.257223563},
|
|||
|
|
'WGS2000': {'a': 6378137, 'f': 1 / 298.257222101}
|
|||
|
|
};
|
|||
|
|
var Datum = datums[name];
|
|||
|
|
if (Datum != null) {
|
|||
|
|
double a = Datum['a']!;
|
|||
|
|
double f = Datum['f']!;
|
|||
|
|
double b = a - f * a;
|
|||
|
|
double e12 = (a * a - b * b) / (a * a); // 第一偏心率平方
|
|||
|
|
double e22 = (a * a - b * b) / (b * b); // 第二偏心率平方
|
|||
|
|
A = a;
|
|||
|
|
F = f;
|
|||
|
|
B = b;
|
|||
|
|
E12 = e12;
|
|||
|
|
E22 = e22;
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
class TransParm {
|
|||
|
|
late double dx;
|
|||
|
|
late double dy;
|
|||
|
|
late double dz;
|
|||
|
|
late double rx;
|
|||
|
|
late double ry;
|
|||
|
|
late double rz;
|
|||
|
|
late double rota;
|
|||
|
|
late double k;
|
|||
|
|
|
|||
|
|
TransParm(TransOptions options) {
|
|||
|
|
dx = options.dx ?? 0;
|
|||
|
|
dy = options.dy ?? 0;
|
|||
|
|
dz = options.dz ?? 0;
|
|||
|
|
rx = options.rx ?? 0;
|
|||
|
|
ry = options.ry ?? 0;
|
|||
|
|
rz = options.rz ?? 0;
|
|||
|
|
k = options.k ?? 0;
|
|||
|
|
rota = options.rota ?? 0;
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
class TransOptions {
|
|||
|
|
double? dx; //X平移(M)
|
|||
|
|
double? dy; //Y平移(M)
|
|||
|
|
double? dz; //z平移(M)
|
|||
|
|
double? rx; //X旋转(°)
|
|||
|
|
double? ry; //Y旋转(°)
|
|||
|
|
double? rz; //Z旋转(°)
|
|||
|
|
double? rota; //平面旋转，用于4参数
|
|||
|
|
double? k; //尺度(ppm)
|
|||
|
|
double? L0; //中央子午线(经度)
|
|||
|
|
String? srcEllipsoid; //源椭球名称
|
|||
|
|
String? dstEllipsoid; //目标椭球名称
|
|||
|
|
double? zoneWidth; //带宽度，用于未指定中央子午线时自动计算
|
|||
|
|
double? elevation; //投影高程
|
|||
|
|
TransOptions({
|
|||
|
|
this.L0,
|
|||
|
|
this.dstEllipsoid,
|
|||
|
|
this.dx,
|
|||
|
|
this.dy,
|
|||
|
|
this.dz,
|
|||
|
|
this.elevation,
|
|||
|
|
this.k,
|
|||
|
|
this.rota,
|
|||
|
|
this.rx,
|
|||
|
|
this.ry,
|
|||
|
|
this.rz,
|
|||
|
|
this.srcEllipsoid,
|
|||
|
|
});
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
//主天线
|
|||
|
|
class CoordTrans {
|
|||
|
|
late double L0; //中央子午线
|
|||
|
|
late double zoneWidth; //带宽度
|
|||
|
|
late double elevation; //投影高程
|
|||
|
|
late double rota; //平面旋转，用于4参数
|
|||
|
|
late Ellipsoid srcEllipsoid; //源椭球
|
|||
|
|
late Ellipsoid dstEllipsoid; //目标椭球
|
|||
|
|
late TransParm transParm; //7参数
|
|||
|
|
CoordTrans(TransOptions options) {
|
|||
|
|
transParm = TransParm(options);
|
|||
|
|
L0 = options.L0 ?? 0;
|
|||
|
|
elevation = options.elevation ?? 0;
|
|||
|
|
srcEllipsoid = Ellipsoid(options.srcEllipsoid ?? 'WGS84');
|
|||
|
|
dstEllipsoid = Ellipsoid(options.dstEllipsoid ?? 'WGS2000');
|
|||
|
|
zoneWidth = options.zoneWidth ?? 3;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
CoordXYH GPS2XY(CoordGPS gps) {
|
|||
|
|
return d2p(CoordBLH(H: gps.ALT ?? elevation, B: gps.LAT, L: gps.LNG));
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
//经纬度转平面坐标
|
|||
|
|
CoordXYH d2p(CoordBLH blh) {
|
|||
|
|
CoordXYZ zj = BLH2XYZ(blh);
|
|||
|
|
CoordXYZ xzj = XYZ2XYZ(zj);
|
|||
|
|
CoordBLH xdd = XYZ2BLH(xzj);
|
|||
|
|
CoordXYH xpm = BL2XY(xdd);
|
|||
|
|
return xpm;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
//平面坐标转大地坐标
|
|||
|
|
CoordBLH p2d(CoordXYH xyh) {
|
|||
|
|
CoordBLH bl = XY2BL(xyh, true);
|
|||
|
|
CoordXYZ zj = BLH2XYZ(bl, true);
|
|||
|
|
CoordXYZ xzj = XYZ2XYZ(zj, true);
|
|||
|
|
CoordBLH xdd = XYZ2BLH(xzj, true);
|
|||
|
|
return xdd;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
setZoneWidth(width) {
|
|||
|
|
if (width != 6 && width != 3) {
|
|||
|
|
// print('带宽应当为6或者为3');
|
|||
|
|
} else {
|
|||
|
|
zoneWidth = width;
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
setL0(l0) {
|
|||
|
|
L0 = l0;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
int getZoneNo(double lng) {
|
|||
|
|
// 6度带
|
|||
|
|
// 这里应为向下取整
|
|||
|
|
if (zoneWidth == 6) {
|
|||
|
|
return ((lng + 6) / 6).floor();
|
|||
|
|
}
|
|||
|
|
// 3度带
|
|||
|
|
return ((lng + 1.5) / 3).floor();
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
PointXY xy2xyLocal(PointXY source) {
|
|||
|
|
double x = source.X;
|
|||
|
|
double y = source.Y;
|
|||
|
|
double destx = x * transParm.k * cos(rota) -
|
|||
|
|
y * transParm.k * sin(rota) +
|
|||
|
|
transParm.dx;
|
|||
|
|
double desty = x * transParm.k * sin(rota) +
|
|||
|
|
y * transParm.k * cos(rota) +
|
|||
|
|
transParm.dy;
|
|||
|
|
|
|||
|
|
return PointXY(X: destx, Y: desty);
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
CoordXYZ BLH2XYZ(CoordBLH pointBLH, [srcDst = false]) {
|
|||
|
|
Ellipsoid ellipsoid = srcDst ? srcEllipsoid : dstEllipsoid;
|
|||
|
|
double a = ellipsoid.A;
|
|||
|
|
double e12 = ellipsoid.E12;
|
|||
|
|
|
|||
|
|
double radB = pointBLH.B / 180 * pi;
|
|||
|
|
double radL = pointBLH.L / 180 * pi;
|
|||
|
|
double H = pointBLH.H;
|
|||
|
|
|
|||
|
|
double N = a / sqrt(1 - e12 * sin(radB) * sin(radB)); // 卯酉圈半径
|
|||
|
|
|
|||
|
|
double X = (N + H) * cos(radB) * cos(radL);
|
|||
|
|
double Y = (N + H) * cos(radB) * sin(radL);
|
|||
|
|
double Z = (N * (1 - e12) + H) * sin(radB);
|
|||
|
|
|
|||
|
|
return CoordXYZ(X: X, Y: Y, Z: Z);
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
/*
|
|||
|
|
七参数转换
|
|||
|
|
不同椭球参数下， 地心直角坐标系之间转换
|
|||
|
|
dX, dY, dZ: 三个坐标方向的平移参数
|
|||
|
|
wX, wY, wZ: 三个方向的旋转角参数(单位为弧度)
|
|||
|
|
Kppm: 尺度参数， 单位是ppm，如果是以米为单位， 需要在传参前 除以1000000
|
|||
|
|
*/
|
|||
|
|
CoordXYZ XYZ2XYZ(CoordXYZ xyz, [srcDst = false]) {
|
|||
|
|
double X = xyz.X;
|
|||
|
|
double Y = xyz.Y;
|
|||
|
|
double Z = xyz.Z;
|
|||
|
|
double dX = transParm.dx;
|
|||
|
|
double dY = transParm.dy;
|
|||
|
|
double dZ = transParm.dz;
|
|||
|
|
double rX = transParm.rx / 3600 / 180 * pi;
|
|||
|
|
double rY = transParm.ry / 3600 / 180 * pi;
|
|||
|
|
double rZ = transParm.rz / 3600 / 180 * pi;
|
|||
|
|
double Kppm = transParm.k / 1000000;
|
|||
|
|
CoordXYZ result = CoordXYZ(X: 0, Y: 0, Z: 0);
|
|||
|
|
if (srcDst) {
|
|||
|
|
result.X = X - dX - Kppm * X + rY * Z - rZ * Y;
|
|||
|
|
result.Y = Y - dY - Kppm * Y - rX * Z + rZ * X;
|
|||
|
|
result.Z = Z - dZ - Kppm * Z + rX * Y - rY * X;
|
|||
|
|
} else {
|
|||
|
|
result.X = X + dX + Kppm * X - rY * Z + rZ * Y;
|
|||
|
|
result.Y = Y + dY + Kppm * Y + rX * Z - rZ * X;
|
|||
|
|
result.Z = Z + dZ + Kppm * Z - rX * Y + rY * X;
|
|||
|
|
}
|
|||
|
|
return result;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
/*
|
|||
|
|
地心直角坐标系 转换到 地心大地坐标系
|
|||
|
|
用直接法2
|
|||
|
|
https://wenku.baidu.com/view/30a08f9ddd88d0d233d46a50.html
|
|||
|
|
*/
|
|||
|
|
CoordBLH XYZ2BLH(CoordXYZ XYZ, [srcDst = false]) {
|
|||
|
|
double X = XYZ.X;
|
|||
|
|
double Y = XYZ.Y;
|
|||
|
|
double Z = XYZ.Z;
|
|||
|
|
Ellipsoid ellipsoid = srcDst ? srcEllipsoid : dstEllipsoid;
|
|||
|
|
double a = ellipsoid.A;
|
|||
|
|
double b = ellipsoid.B;
|
|||
|
|
double e12 = ellipsoid.E12;
|
|||
|
|
double e22 = ellipsoid.E22;
|
|||
|
|
double L = atan(Y / X);
|
|||
|
|
// 弧度转角度
|
|||
|
|
double degL = L * 180 / pi;
|
|||
|
|
// Y值为正， 东半球， 否则西半球
|
|||
|
|
if (Y > 0) {
|
|||
|
|
while (degL < 0) {
|
|||
|
|
degL += 180;
|
|||
|
|
}
|
|||
|
|
while (degL > 180) {
|
|||
|
|
degL -= 180;
|
|||
|
|
}
|
|||
|
|
} else {
|
|||
|
|
while (degL > 0) {
|
|||
|
|
degL -= 180;
|
|||
|
|
}
|
|||
|
|
while (degL < -180) {
|
|||
|
|
degL += 180;
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
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 / pi; // 弧度转角度
|
|||
|
|
if (Z > 0) {
|
|||
|
|
// Z值为正， 北半球， 否则南半球
|
|||
|
|
while (degB < 0) {
|
|||
|
|
degB += 90;
|
|||
|
|
}
|
|||
|
|
while (degB > 90) {
|
|||
|
|
degB -= 90;
|
|||
|
|
}
|
|||
|
|
} else {
|
|||
|
|
while (degB > 0) {
|
|||
|
|
degB -= 90;
|
|||
|
|
}
|
|||
|
|
while (degB < -90) {
|
|||
|
|
degB += 90;
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
while (degB < 0) {
|
|||
|
|
degB += 360;
|
|||
|
|
}
|
|||
|
|
while (degB > 360) {
|
|||
|
|
degB -= 360;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
double N = a / sqrt(1 - e12 * sin(B) * sin(B)); // 卯酉圈半径
|
|||
|
|
double H = 0;
|
|||
|
|
// B接近极区， 在±90°附近
|
|||
|
|
if ((degB).abs() > 80) {
|
|||
|
|
H = Z / sin(B) - N * (1 - e12);
|
|||
|
|
} else {
|
|||
|
|
H = sqrt(X * X + Y * Y) / cos(B) - N;
|
|||
|
|
}
|
|||
|
|
return CoordBLH(B: degB, L: degL, H: H);
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
/*
|
|||
|
|
地心大地坐标系 转换到 大地平面坐标系
|
|||
|
|
prjHeight: 投影面高程
|
|||
|
|
http://www.cnblogs.com/imeiba/p/5696967.html
|
|||
|
|
*/
|
|||
|
|
CoordXYH BL2XY(CoordBLH BLH,
|
|||
|
|
[srcDst = false, offsetY = 500000, offsetX = 0]) {
|
|||
|
|
Ellipsoid ellipsoid = srcDst ? srcEllipsoid : dstEllipsoid;
|
|||
|
|
double a = ellipsoid.A;
|
|||
|
|
double b = ellipsoid.B;
|
|||
|
|
double e12 = ellipsoid.E12;
|
|||
|
|
double e22 = ellipsoid.E22;
|
|||
|
|
if (L0 == 0) {
|
|||
|
|
int zoneNo = getZoneNo(BLH.L);
|
|||
|
|
L0 = (zoneNo - 0.5) * zoneWidth;
|
|||
|
|
}
|
|||
|
|
double radL0 = L0 / 180 * pi;
|
|||
|
|
|
|||
|
|
double radB = BLH.B / 180 * pi;
|
|||
|
|
double radL = BLH.L / 180 * 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);
|
|||
|
|
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);
|
|||
|
|
|
|||
|
|
x = offsetX + x * (b + elevation) / b;
|
|||
|
|
y = offsetY + y * (b + elevation) / b;
|
|||
|
|
|
|||
|
|
return CoordXYH(
|
|||
|
|
X: (x * 1000).round() / 1000, Y: (y * 1000).round() / 1000, H: BLH.H);
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
/*
|
|||
|
|
大地平面坐标系 转换到 地心大地坐标系
|
|||
|
|
prjHeight: 投影面高程
|
|||
|
|
http://www.cnblogs.com/imeiba/p/5696967.html
|
|||
|
|
*/
|
|||
|
|
CoordBLH XY2BL(CoordXYH xyh,
|
|||
|
|
[srcDst = false, offsetY = 500000, offsetX = 0]) {
|
|||
|
|
Ellipsoid ellipsoid = srcDst ? srcEllipsoid : dstEllipsoid;
|
|||
|
|
double a = ellipsoid.A;
|
|||
|
|
double b = ellipsoid.B;
|
|||
|
|
double e12 = ellipsoid.E12;
|
|||
|
|
double e22 = ellipsoid.E22;
|
|||
|
|
|
|||
|
|
double e1 = (1 - sqrt(1 - e12)) / (1 + sqrt(1 - e12));
|
|||
|
|
double radL0 = L0 / 180 * pi;
|
|||
|
|
// 带内大地坐标
|
|||
|
|
double Y = xyh.Y % 1000000;
|
|||
|
|
double x = (xyh.X - offsetX) * b / (b + elevation);
|
|||
|
|
double y = (Y - offsetY) * b / (b + elevation);
|
|||
|
|
|
|||
|
|
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);
|
|||
|
|
|
|||
|
|
B = B * 180 / pi;
|
|||
|
|
L = L * 180 / pi;
|
|||
|
|
return CoordBLH(B: B, L: L, H: xyh.H);
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
//两点之间的距离公式
|
|||
|
|
dist(PointXY p1, PointXY p2) {
|
|||
|
|
double d = sqrt(pow((p2.X - p1.X), 2) + pow((p2.Y - p1.Y), 2));
|
|||
|
|
return d;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
//求坐标方位角
|
|||
|
|
double fwj(PointXY p1, PointXY p2) {
|
|||
|
|
double dx = p2.X - p1.X;
|
|||
|
|
double dy = p2.Y - p1.Y;
|
|||
|
|
return pi - sign(dy) - atan(dx / dy);
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
// 计算三参数
|
|||
|
|
TransOptions calcThree(CoordBLH p1, CoordXYH p2) {
|
|||
|
|
CoordXYZ source = BLH2XYZ(p1);
|
|||
|
|
CoordBLH bl = XY2BL(p2, true);
|
|||
|
|
CoordXYZ dest = BLH2XYZ(bl, true);
|
|||
|
|
|
|||
|
|
double dX = dest.X - source.X;
|
|||
|
|
double dY = dest.Y - source.Y;
|
|||
|
|
double dZ = dest.Z - source.Z;
|
|||
|
|
return TransOptions(dx: dX, dy: dY, dz: dZ);
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
// TransOptions calcFour(List p1, List p2, int PointCount) {
|
|||
|
|
// double rota = 0;
|
|||
|
|
// double scale = 0;
|
|||
|
|
// double dx = 0;
|
|||
|
|
// double dy = 0;
|
|||
|
|
// if (PointCount == 2) {
|
|||
|
|
// rota = fwj(p2[0] as PointXY, p2[1]) - fwj(p1[0], p1[1]);
|
|||
|
|
// scale = dist(p2[0], p2[1]) / dist(p1[0], p1[1]);
|
|||
|
|
// dx = p2[0].X - scale * cos(rota) * p1[0].X + scale * sin(rota) * p1[0].Y;
|
|||
|
|
// dy = p2[0].Y - scale * sin(rota) * p1[0].X - scale * cos(rota) * p1[0].Y;
|
|||
|
|
// } else if (PointCount > 2) {
|
|||
|
|
// double u = 1.0, v = 0, Dx = 0.0, Dy = 0.0;
|
|||
|
|
// int intCount = PointCount;
|
|||
|
|
|
|||
|
|
// Matrix4 dx1 = Matrix.zeros(4, 1);
|
|||
|
|
// // Matrix4 B1 = Matrix.zeros(2 * intCount, 4);
|
|||
|
|
// Matrix4 B1 = Matrix4.zero();
|
|||
|
|
// for (int i = 0; i < 2 * intCount; i++) {
|
|||
|
|
// for (int j = 0; j < 4; j++) {
|
|||
|
|
// B1.setEntry(i, j, 0);
|
|||
|
|
// }
|
|||
|
|
// }
|
|||
|
|
// Matrix4 W1 = Matrix.zeros(2 * intCount, 1);
|
|||
|
|
// // Matrix4 BT = Matrix.zeros(4, 2 * intCount);
|
|||
|
|
// Matrix4 BT = Matrix4.zero();
|
|||
|
|
// for (int i = 0; i < 4; i++) {
|
|||
|
|
// for (int j = 0; j < 2 * intCount; j++) {
|
|||
|
|
// BT.setEntry(i, j, 0);
|
|||
|
|
// }
|
|||
|
|
// }
|
|||
|
|
// Matrix4 N = Matrix4(
|
|||
|
|
// 0,
|
|||
|
|
// 0,
|
|||
|
|
// 0,
|
|||
|
|
// 0,
|
|||
|
|
// 0,
|
|||
|
|
// 0,
|
|||
|
|
// 0,
|
|||
|
|
// 0,
|
|||
|
|
// 0,
|
|||
|
|
// 0,
|
|||
|
|
// 0,
|
|||
|
|
// 0,
|
|||
|
|
// 0,
|
|||
|
|
// 0,
|
|||
|
|
// 0,
|
|||
|
|
// 0,
|
|||
|
|
// );
|
|||
|
|
// Matrix4 InvN = Matrix4(
|
|||
|
|
// 0,
|
|||
|
|
// 0,
|
|||
|
|
// 0,
|
|||
|
|
// 0,
|
|||
|
|
// 0,
|
|||
|
|
// 0,
|
|||
|
|
// 0,
|
|||
|
|
// 0,
|
|||
|
|
// 0,
|
|||
|
|
// 0,
|
|||
|
|
// 0,
|
|||
|
|
// 0,
|
|||
|
|
// 0,
|
|||
|
|
// 0,
|
|||
|
|
// 0,
|
|||
|
|
// 0,
|
|||
|
|
// );
|
|||
|
|
// Matrix4 BTW = Matrix.zeros(4, 1);
|
|||
|
|
// for (int i = 0; i < intCount; i++) {
|
|||
|
|
// //计算误差方程系数
|
|||
|
|
// B1.setEntry(2 * i, 0, 1);
|
|||
|
|
// B1.setEntry(2 * i, 1, 0);
|
|||
|
|
// B1.setEntry(2 * i, 2, p1[i].X);
|
|||
|
|
// B1.setEntry(2 * i, 3, -p1[i].Y);
|
|||
|
|
|
|||
|
|
// B1.setEntry(2 * i + 1, 0, 0);
|
|||
|
|
// B1.setEntry(2 * i + 1, 1, 1);
|
|||
|
|
// B1.setEntry(2 * i + 1, 2, p1[i].Y);
|
|||
|
|
// B1.setEntry(2 * i + 1, 3, p1[i].X);
|
|||
|
|
// // B1[2 * i][0] = 1;
|
|||
|
|
// // B1[2 * i][1] = 0;
|
|||
|
|
// // B1[2 * i][2] = p1[i].X;
|
|||
|
|
// // B1[2 * i][3] = -p1[i].Y;
|
|||
|
|
|
|||
|
|
// // B1[2 * i + 1][0] = 0;
|
|||
|
|
// // B1[2 * i + 1][1] = 1;
|
|||
|
|
// // B1[2 * i + 1][2] = p1[i].Y;
|
|||
|
|
// // B1[2 * i + 1][3] = p1[i].X;
|
|||
|
|
// }
|
|||
|
|
// for (int i = 0; i < intCount; i++) {
|
|||
|
|
// //计算误差方程系常数
|
|||
|
|
// // W1[2 * i][0] = p2[i].X - u * p1[i].X + v * p1[i].Y - Dx;
|
|||
|
|
// // W1[2 * i + 1][0] = p2[i].Y - u * p1[i].Y - v * p1[i].X - Dy;
|
|||
|
|
// W1.setEntry(2 * i, 0, p2[i].X - u * p1[i].X + v * p1[i].Y - Dx);
|
|||
|
|
// W1.setEntry(2 * i + 1, 0, p2[i].Y - u * p1[i].Y - v * p1[i].X - Dy);
|
|||
|
|
// }
|
|||
|
|
// // 最小二乘求解
|
|||
|
|
// Matrix4 b1 = Matrix4.identity();
|
|||
|
|
// // 矩阵 B1 的转置
|
|||
|
|
// BT = Matrix4.copy(b1);
|
|||
|
|
// BT.transpose();
|
|||
|
|
// // 矩阵乘法
|
|||
|
|
// N = BT * B1;
|
|||
|
|
// // 逆矩阵
|
|||
|
|
// InvN = N.clone();
|
|||
|
|
// InvN.invert();
|
|||
|
|
// BTW = BT * (W1);
|
|||
|
|
// dx1 = InvN * (BTW);
|
|||
|
|
|
|||
|
|
// Dx = Dx + dx1[0][0];
|
|||
|
|
// Dy = Dy + dx1[1][0];
|
|||
|
|
// u = u + dx1[2][0];
|
|||
|
|
// v = v + dx1[3][0];
|
|||
|
|
// dx = Dx;
|
|||
|
|
// dy = Dy;
|
|||
|
|
// rota = atan(v / u);
|
|||
|
|
// scale = u / cos(rota);
|
|||
|
|
// }
|
|||
|
|
// return TransOptions(rota: rota, k: scale, dx: dx, dy: dy);
|
|||
|
|
// }
|
|||
|
|
|
|||
|
|
// // 计算七参数
|
|||
|
|
// TransOptions calcBuras(List p1, List p2, pointCount) {
|
|||
|
|
// let B1 = Matrix.zeros(pointCount * 3, 7);
|
|||
|
|
// let dx1 = Matrix.zeros(7, 1);
|
|||
|
|
// let L = Matrix.zeros(pointCount * 3, 1);
|
|||
|
|
// let BT = Matrix.zeros(7, pointCount * 3);
|
|||
|
|
// let N = Matrix.zeros(7, 7);
|
|||
|
|
// let InvN = Matrix.zeros(7, 7);
|
|||
|
|
// let BTL = Matrix.zeros(7, 1);
|
|||
|
|
// for (int i = 0; i < pointCount * 3; i++) {
|
|||
|
|
// if (i % 3 == 0) {
|
|||
|
|
// L[i][0] = p2[(i / 3).floor()].X;
|
|||
|
|
// } else if (i % 3 == 1) {
|
|||
|
|
// L[i][0] = p2[(i / 3).floor()].Y;
|
|||
|
|
// } else if (i % 3 == 2) {
|
|||
|
|
// L[i][0] = p2[(i / 3).floor()].Z;
|
|||
|
|
// }
|
|||
|
|
// }
|
|||
|
|
|
|||
|
|
// for (int i = 0; i < pointCount * 3; i++) {
|
|||
|
|
// if (i % 3 == 0) {
|
|||
|
|
// int index = (i / 3) as int;
|
|||
|
|
// B1[i][0] = 1;
|
|||
|
|
// B1[i][1] = 0;
|
|||
|
|
// B1[i][2] = 0;
|
|||
|
|
// B1[i][3] = p1[index].X;
|
|||
|
|
// B1[i][4] = 0;
|
|||
|
|
// B1[i][5] = -p1[index].Z;
|
|||
|
|
// B1[i][6] = p1[index].Y;
|
|||
|
|
// } else if (i % 3 == 1) {
|
|||
|
|
// int index = ((i - 1) / 3) as int;
|
|||
|
|
// B1[i][0] = 0;
|
|||
|
|
// B1[i][1] = 1;
|
|||
|
|
// B1[i][2] = 0;
|
|||
|
|
// B1[i][3] = p1[index].Y;
|
|||
|
|
// B1[i][4] = p1[index].Z;
|
|||
|
|
// B1[i][5] = 0;
|
|||
|
|
// B1[i][6] = -p1[index].X;
|
|||
|
|
// } else if (i % 3 == 2) {
|
|||
|
|
// int index = ((i - 2) / 3) as int;
|
|||
|
|
// B1[i][0] = 0;
|
|||
|
|
// B1[i][1] = 0;
|
|||
|
|
// B1[i][2] = 1;
|
|||
|
|
// B1[i][3] = p1[index].Z;
|
|||
|
|
// B1[i][4] = -p1[index].Y;
|
|||
|
|
// B1[i][5] = p1[index].X;
|
|||
|
|
// B1[i][6] = 0;
|
|||
|
|
// }
|
|||
|
|
// }
|
|||
|
|
// Matrix4 b1 = Matrix4.identity();
|
|||
|
|
// BT = B1.transpose();
|
|||
|
|
// N = BT.mmul(B1);
|
|||
|
|
// InvN = inverse(N);
|
|||
|
|
// BTL = BT.mmul(L);
|
|||
|
|
// dx1 = InvN.mmul(BTL);
|
|||
|
|
|
|||
|
|
// double dx = dx1[0][0];
|
|||
|
|
// double dy = dx1[1][0];
|
|||
|
|
// double dz = dx1[2][0];
|
|||
|
|
// double scale = dx1[3][0];
|
|||
|
|
// double rotax = dx1[4][0] / dx1[3][0];
|
|||
|
|
// double rotay = dx1[5][0] / dx1[3][0];
|
|||
|
|
// double rotaz = dx1[6][0] / dx1[3][0];
|
|||
|
|
|
|||
|
|
// return TransOptions(
|
|||
|
|
// dx: dx, dy: dy, dz: dz, k: scale, rx: rotax, ry: rotay, rz: rotaz);
|
|||
|
|
// }
|
|||
|
|
|
|||
|
|
double sign(num number) {
|
|||
|
|
if (number < 0) {
|
|||
|
|
return -1.0;
|
|||
|
|
} else if (number > 0) {
|
|||
|
|
return 1.0;
|
|||
|
|
} else {
|
|||
|
|
return 0.0;
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
}
|