wgs-84高精度空间直角坐标转为CGCS2000坐标程序开发

WGS-84空间直角坐标坐标转CGCS2000空间直角坐标

// 修改后的坐标转换核心方法
private Vector<double> TransformCoordinates(
Vector<double> source,
FrameParams frameParams,
double obsEpoch,
double targetEpoch,
VelocityModel velocityModel)
{
// 1. 计算时间差（年）
double deltaYears = targetEpoch - obsEpoch;

// 2. 计算速度
Vector<double> velocity;
if (velocityModel.ModelType == VelocityType.Euler)
{
velocity = CalculateEulerVelocity(source, velocityModel.EulerValues);
}
else if (velocityModel.ModelType == VelocityType.Linear)
{
velocity = FindGridVelocity(source);
}
else // VelocityType.Custom
{
velocity = velocityModel.CustomValues;
}

// 3. 历元转换：得到目标历元时刻的坐标（仍在原框架下）
Vector<double> coordAtTargetEpoch = source + velocity * deltaYears;

// 4. 将框架转换参数归算到目标历元
FrameParams frameParamsAtTarget = ComputeFrameParamsAtEpoch(frameParams, targetEpoch);

// 5. 框架转换：从原框架（目标历元）转换到目标框架（目标历元）
Vector<double> result = ApplyFrameTransform(coordAtTargetEpoch, frameParamsAtTarget);

return result;
}

// 修复框架参数归算方法
private FrameParams ComputeFrameParamsAtEpoch(FrameParams p, double targetEpoch)
{
double deltaYears = targetEpoch - p.ReferenceEpoch;

return new FrameParams
{
Tx = p.Tx + p.Tx_rate * deltaYears,
Ty = p.Ty + p.Ty_rate * deltaYears,
Tz = p.Tz + p.Tz_rate * deltaYears,
Rx = (p.Rx + p.Rx_rate * deltaYears) * m,
Ry = (p.Ry + p.Ry_rate * deltaYears) * m,
Rz = (p.Rz + p.Rz_rate * deltaYears) * m,
D = p.D + p.D_rate * deltaYears,
// 保留原始速率值（后续不再使用）
Tx_rate = p.Tx_rate,
Ty_rate = p.Ty_rate,
Tz_rate = p.Tz_rate,
Rx_rate = p.Rx_rate,
Ry_rate = p.Ry_rate,
Rz_rate = p.Rz_rate,
D_rate = p.D_rate,
ReferenceEpoch = targetEpoch
};
}
private Vector<double> ApplyFrameTransform(Vector<double> source, FrameParams p)
{
// 实现公式(4)的变换矩阵
var S = Matrix<double>.Build.DenseIdentity(3) * (1 + p.D * 1e-9); // ppb转换为无量纲
var R = Matrix<double>.Build.DenseOfArray(new double[,] {
{ 0, -p.Rz, p.Ry },
{ p.Rz, 0, -p.Rx },
{ -p.Ry, p.Rx, 0 }
});
var T = Vector<double>.Build.Dense(new[] {
p.Tx * 0.001, // mm → m
p.Ty * 0.001,
p.Tz * 0.001
});

return (S + R) * source + T;
}