201 lines
6.2 KiB
Plaintext
201 lines
6.2 KiB
Plaintext
#include <iostream>
|
||
#include <memory>
|
||
#include <cmath>
|
||
#include <complex>
|
||
#include <device_launch_parameters.h>
|
||
#include <cuda_runtime.h>
|
||
#include <cublas_v2.h>
|
||
#include <cuComplex.h>
|
||
|
||
#include "BaseConstVariable.h"
|
||
#include "GPUTool.cuh"
|
||
#include "GPUBPTool.cuh"
|
||
|
||
#include <cmath>
|
||
#include <stdio.h>
|
||
|
||
|
||
|
||
// 向量运算
|
||
__device__ __host__ Vector3 vec_sub(Vector3 a, Vector3 b) {
|
||
return { a.x - b.x, a.y - b.y, a.z - b.z };
|
||
}
|
||
|
||
__device__ __host__ double vec_dot(Vector3 a, Vector3 b) {
|
||
return a.x * b.x + a.y * b.y + a.z * b.z;
|
||
}
|
||
|
||
__device__ __host__ Vector3 vec_cross(Vector3 a, Vector3 b) {
|
||
return { a.y * b.z - a.z * b.y,
|
||
a.z * b.x - a.x * b.z,
|
||
a.x * b.y - a.y * b.x };
|
||
}
|
||
|
||
__device__ __host__ Vector3 vec_normalize(Vector3 v) {
|
||
double len = sqrt(vec_dot(v, v));
|
||
return (len > 1e-12) ? Vector3 { v.x / len, v.y / len, v.z / len } : v;
|
||
}
|
||
|
||
// 计算视线交点T
|
||
extern __device__ __host__ Vector3 compute_T(Vector3 S, Vector3 ray, double H) {
|
||
Vector3 dir = vec_normalize(ray);
|
||
double a_h = WGS84_A + H;
|
||
|
||
double A = (dir.x * dir.x + dir.y * dir.y) / (a_h * a_h) + dir.z * dir.z / (WGS84_B * WGS84_B);
|
||
double B = 2.0 * (S.x * dir.x / (a_h * a_h) + S.y * dir.y / (a_h * a_h) + S.z * dir.z / (WGS84_B * WGS84_B));
|
||
double C = (S.x * S.x + S.y * S.y) / (a_h * a_h) + S.z * S.z / (WGS84_B * WGS84_B) - 1.0;
|
||
|
||
double disc = B * B - 4 * A * C;
|
||
if (disc < 0) return Vector3 { NAN, NAN, NAN };
|
||
|
||
double sqrt_d = sqrt(disc);
|
||
double t = fmax((-B - sqrt_d) / (2 * A), (-B + sqrt_d) / (2 * A));
|
||
return (t > 1e-6) ? Vector3 { S.x + dir.x * t, S.y + dir.y * t, S.z + dir.z * t }
|
||
: Vector3 { NAN, NAN, NAN };
|
||
}
|
||
|
||
// 构建平面基底
|
||
extern __device__ __host__ void compute_basis(Vector3 S, Vector3 T, Vector3* e1, Vector3* e2) {
|
||
Vector3 ST = vec_normalize(vec_sub(T, S));
|
||
Vector3 SO = vec_normalize(vec_sub(Vector3 { 0, 0, 0 }, S)); // S->O方向
|
||
|
||
*e1 = vec_normalize(vec_cross(ST, SO));
|
||
*e2 = vec_normalize(vec_cross(*e1, ST));
|
||
}
|
||
|
||
// 牛顿迭代法
|
||
extern __device__ __host__ int newton_solve(Vector3 S, Vector3 e1, Vector3 e2,
|
||
double R, double H, double* u, double* v) {
|
||
double a_h = WGS84_A + H;
|
||
|
||
for (int iter = 0; iter < MAX_ITER; ++iter) {
|
||
Vector3 P = {
|
||
S.x + e1.x * (*u) + e2.x * (*v),
|
||
S.y + e1.y * (*u) + e2.y * (*v),
|
||
S.z + e1.z * (*u) + e2.z * (*v)
|
||
};
|
||
|
||
// 残差计算
|
||
double f1 = (P.x * P.x + P.y * P.y) / (a_h * a_h) + P.z * P.z / (WGS84_B * WGS84_B) - 1.0;
|
||
double f2 = (*u) * (*u) + (*v) * (*v) - R * R;
|
||
if (fabs(f1) < 1e-8 && fabs(f2) < 1e-8) return 1;
|
||
|
||
// 雅可比矩阵
|
||
double J11 = (2 * (S.x + e1.x * (*u) + e2.x * (*v)) * e1.x) / (a_h * a_h)
|
||
+ (2 * (S.z + e1.z * (*u) + e2.z * (*v)) * e1.z) / (WGS84_B * WGS84_B);
|
||
double J12 = (2 * (S.x + e1.x * (*u) + e2.x * (*v)) * e2.x) / (a_h * a_h)
|
||
+ (2 * (S.z + e1.z * (*u) + e2.z * (*v)) * e2.z) / (WGS84_B * WGS84_B);
|
||
double J21 = 2 * (*u);
|
||
double J22 = 2 * (*v);
|
||
|
||
// 矩阵求逆
|
||
double det = J11 * J22 - J12 * J21;
|
||
if (fabs(det) < 1e-12) break;
|
||
|
||
double delta_u = (-J22 * f1 + J12 * f2) / det;
|
||
double delta_v = (J21 * f1 - J11 * f2) / det;
|
||
|
||
*u += delta_u;
|
||
*v += delta_v;
|
||
}
|
||
return 0;
|
||
}
|
||
|
||
// 主计算函数A
|
||
extern __device__ __host__ Vector3 compute_P(Vector3 S, Vector3 T, double R, double H) {
|
||
Vector3 e1, e2;
|
||
compute_basis(S, T, &e1, &e2);
|
||
|
||
// 计算参考角度方向
|
||
Vector3 ST_vec = vec_sub(T, S);
|
||
Vector3 SO_vec = vec_sub(Vector3 { 0, 0, 0 }, S);
|
||
Vector3 ref_cross = vec_cross(SO_vec, ST_vec);
|
||
double ref_sign = ref_cross.z; // 取Z分量判断方向
|
||
|
||
Vector3 best_P = { NAN, NAN, NAN };
|
||
double min_dist = INFINITY;
|
||
|
||
// 圆周采样
|
||
const int samples = 36;
|
||
for (int i = 0; i < samples; ++i) {
|
||
double angle = 2 * M_PI * i / samples;
|
||
double u = R * cos(angle);
|
||
double v = R * sin(angle);
|
||
|
||
if (!newton_solve(S, e1, e2, R, H, &u, &v)) continue;
|
||
|
||
Vector3 P = {
|
||
S.x + e1.x * u + e2.x * v,
|
||
S.y + e1.y * u + e2.y * v,
|
||
S.z + e1.z * u + e2.z * v
|
||
};
|
||
|
||
// 椭球验证
|
||
double check = (P.x * P.x + P.y * P.y) / ((WGS84_A + H) * (WGS84_A + H))
|
||
+ P.z * P.z / (WGS84_B * WGS84_B);
|
||
if (fabs(check - 1.0) > 1e-6) continue;
|
||
|
||
// 角度方向验证
|
||
Vector3 SP_vec = vec_sub(P, S);
|
||
Vector3 cur_cross = vec_cross(SP_vec, ST_vec);
|
||
if (ref_sign * cur_cross.z < 0) continue;
|
||
|
||
// 选择最近点
|
||
double dist = vec_dot(vec_sub(P, T), vec_sub(P, T));
|
||
if (dist < min_dist) {
|
||
min_dist = dist;
|
||
best_P = P;
|
||
}
|
||
}
|
||
return best_P;
|
||
}
|
||
|
||
//// 参数校验与主函数
|
||
//int main() {
|
||
// Vector3 S = { -2.8e6, -4.2e6, 3.5e6 }; // 卫星位置 (m)
|
||
// Vector3 ray = { 0.6, 0.4, -0.7 }; // 视线方向
|
||
// double H = 500.0; // 平均高程
|
||
// double R = 1000.0; // 目标距离
|
||
//
|
||
// // 参数校验
|
||
// if (R <= 0 || H < -WGS84_A * 0.1 || H > WGS84_A * 0.1) {
|
||
// printf("参数错误：\n H范围：±%.1f km\n R必须>0\n", WGS84_A * 0.1 / 1000);
|
||
// return 1;
|
||
// }
|
||
//
|
||
// // Step 1: 计算交点T
|
||
// Vector3 T = compute_T(S, ray, H);
|
||
// if (isnan(T.x)) {
|
||
// printf("错误：视线未与椭球相交\n");
|
||
// return 1;
|
||
// }
|
||
//
|
||
// // Step 2: 计算目标点P
|
||
// Vector3 P = compute_P(S, T, R, H);
|
||
//
|
||
// if (!isnan(P.x)) {
|
||
// printf("计算结果：\n");
|
||
// printf("P = (%.3f, %.3f, %.3f) m\n", P.x, P.y, P.z);
|
||
//
|
||
// // 验证距离
|
||
// Vector3 SP = vec_sub(P, S);
|
||
// double dist = sqrt(vec_dot(SP, SP));
|
||
// printf("实际距离：%.3f m (期望：%.1f m)\n", dist, R);
|
||
//
|
||
// // 验证椭球
|
||
// double check = (P.x * P.x + P.y * P.y) / ((WGS84_A + H) * (WGS84_A + H))
|
||
// + P.z * P.z / (WGS84_B * WGS84_B);
|
||
// printf("椭球验证：%.6f (期望：1.0)\n", check);
|
||
//
|
||
// // 验证最近距离
|
||
// Vector3 PT = vec_sub(P, T);
|
||
// printf("到T点距离：%.3f m\n", sqrt(vec_dot(PT, PT)));
|
||
// }
|
||
// else {
|
||
// printf("未找到有效解\n");
|
||
// }
|
||
//
|
||
// return 0;
|
||
//}
|
||
//
|