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;
|
||
}
|
||
}
|
||
}
|