293 lines
8.3 KiB
Plaintext
293 lines
8.3 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>
|
||
#include <cassert>
|
||
|
||
#define EPSILON 1e-12
|
||
#define MAX_ITER 50
|
||
|
||
#ifndef M_PI
|
||
#define M_PI 3.14159265358979323846
|
||
#endif
|
||
|
||
__device__ double angleBetweenVectors(Vector3 a, Vector3 b, bool returnDegrees ) {
|
||
// 计算点积
|
||
double dotProduct = a.x * b.x + a.y * b.y + a.z * b.z;
|
||
|
||
// 计算模长
|
||
double magA = sqrt(a.x * a.x + a.y * a.y + a.z * a.z);
|
||
double magB = sqrt(b.x * b.x + b.y * b.y + b.z * b.z);
|
||
|
||
// 处理零向量异常
|
||
if (magA == 0.0 || magB == 0.0) {
|
||
return NAN;
|
||
}
|
||
|
||
double cosTheta = dotProduct / (magA * magB);
|
||
cosTheta = cosTheta < -1 ? -1 : cosTheta>1 ? 1 : cosTheta; // 截断到[-1, 1]
|
||
double angleRad = acos(cosTheta);
|
||
return returnDegrees ? angleRad * 180.0 / M_PI : angleRad;
|
||
}
|
||
|
||
// 向量运算
|
||
__device__ Vector3 vec_sub(Vector3 a, Vector3 b) {
|
||
return { a.x - b.x, a.y - b.y, a.z - b.z };
|
||
}
|
||
|
||
__device__ double vec_dot(Vector3 a, Vector3 b) {
|
||
return a.x * b.x + a.y * b.y + a.z * b.z;
|
||
}
|
||
|
||
__device__ 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__ 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;
|
||
}
|
||
|
||
|
||
// 标准正交基底坐标计算
|
||
__device__ Vector3 coordinates_orthonormal_basis(Vector3& A,
|
||
Vector3& e1,
|
||
Vector3& e2,
|
||
Vector3& e3) {
|
||
//// 验证基底正交性和单位长度（容差1e-10）
|
||
//const double tolerance = 1e-10;
|
||
//assert(fabs(dot(e1, e2)) < tolerance);
|
||
//assert(fabs(dot(e1, e3)) < tolerance);
|
||
//assert(fabs(dot(e2, e3)) < tolerance);
|
||
//assert(fabs(norm(e1) - 1.0) < tolerance);
|
||
//assert(fabs(norm(e2) - 1.0) < tolerance);
|
||
//assert(fabs(norm(e3) - 1.0) < tolerance);
|
||
|
||
// 计算投影坐标
|
||
return Vector3{
|
||
vec_dot(A, e1),
|
||
vec_dot(A, e2),
|
||
vec_dot(A, e3)
|
||
};
|
||
}
|
||
|
||
|
||
|
||
// 计算视线交点T
|
||
extern __device__ 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); // A > 0
|
||
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 = fmin((-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 };
|
||
}
|
||
|
||
// 主计算函数A
|
||
extern __device__ Vector3 compute_P(Vector3 S, Vector3 T, double R, double H) {
|
||
Vector3 ex, ey, ez; // 平面基函数
|
||
Vector3 ST = vec_normalize(vec_sub(T, S));// S->T
|
||
Vector3 SO = vec_normalize(vec_sub(Vector3{ 0, 0, 0 }, S)); // S->O
|
||
Vector3 st1 = vec_sub(T, S);
|
||
double R0 = sqrt(st1.x * st1.x + st1.y * st1.y + st1.z * st1.z);
|
||
|
||
// S (Z .) --------Y
|
||
// |\
|
||
// | \
|
||
// | \
|
||
// X \
|
||
// | -> T
|
||
// | /
|
||
// | /
|
||
// | /
|
||
// |/
|
||
// O
|
||
|
||
|
||
ez = vec_normalize(vec_cross(SO, ST)); // Z 轴
|
||
ey = vec_normalize(vec_cross(ez, SO)); // Y 轴 与 ST 同向 --这个结论在星地几何约束，便是显然的；
|
||
ex = vec_normalize(SO); //X轴
|
||
// 大致的理论推导
|
||
// 这里考虑 成像几何，所以点 P 一定在 ex-0-ey 平面上，所以t3=0;
|
||
// 定义 SP 的向量与 ex的夹角为 theta , 目标长度为 t
|
||
// t1=t*cos(Q);
|
||
// t2=t*sin(Q);
|
||
// h=(a+H)
|
||
// Xp=Sx+t1*ex.x+t2*ey.x +t3*ez.x; //因为 t3=0；
|
||
// Yp=Sy+t1*ex.y+t2*ey.y +t3*ez.y;
|
||
// Zp=Sz+t1*ex.z+t2*ey.z +t3*ez.z;
|
||
// Xp^2+Yp^2 Zp^2 Xp^2+Yp^2 Zp^2
|
||
// ---------- + ------- = 1 ==> ---------- + ------- = 1
|
||
// (a+H)^2 b^2 h^2 b^2
|
||
double h2 = (WGS84_A + H) * (WGS84_A + H);
|
||
double b2 = WGS84_B * WGS84_B;
|
||
double R2 = R * R;
|
||
double A = R2 * ((ex.x * ex.x + ex.y * ex.y) / h2 + (ex.z * ex.z) / b2);
|
||
double B = R2 * ((ex.x * ey.x + ex.y * ey.y) / h2 + (ex.z * ey.z) / b2) * 2;
|
||
double C = R2 * ((ey.x * ey.x + ey.y * ey.y) / h2 + (ey.z * ey.z) / b2);
|
||
double D = 1 - ((S.x * S.x + S.y * S.y) / h2 + (S.z * S.z) / b2);
|
||
double E = 2 * R * ((S.x * ex.x + S.y * ex.y) / h2 + (S.z * ex.z) / b2);
|
||
double F = 2 * R * ((S.x * ey.x + S.y * ey.y) / h2 + (S.z * ey.z) / b2);
|
||
|
||
// f(Q)=Acos^2(Q)+Bsin(Q)cos(Q)+Csin^2(Q)+E*cos(Q)+F*sin(Q)-D
|
||
// f'(Q)=(C−A)sin(2Q)+2Bcos(2Q)-Esin(Q)+Fcos(Q)
|
||
|
||
// 牛顿迭代
|
||
// f(Q)
|
||
// Q(t+1)=Q - -----------
|
||
// f'(Q)
|
||
|
||
// 求解初始值
|
||
|
||
double Q0 = angleBetweenVectors(SO, ST, false);
|
||
double dQ = 0;
|
||
double fQ = 0;
|
||
double dfQ = 0;
|
||
double Q = R < R0 ? Q0 - 1e-3 : Q0 + 1e-3;
|
||
|
||
|
||
// 牛顿迭代法
|
||
for (int iter = 0; iter < MAX_ITER * 10; ++iter) {
|
||
fQ = A * cos(Q) * cos(Q) + B * sin(Q) * cos(Q) + C * sin(Q) * sin(Q) + E * cos(Q) + F * sin(Q) - D;
|
||
dfQ = (C - A) * sin(2 * Q) + B * cos(2 * Q) - E * sin(Q) + F * cos(Q);
|
||
dQ = fQ / dfQ;
|
||
if (abs(dQ) < 1e-8) {
|
||
break;
|
||
}
|
||
else {
|
||
dQ = abs(dQ) < d2r * 2 ? dQ : abs(dQ) / dQ * d2r;
|
||
Q = Q - dQ;
|
||
}
|
||
}
|
||
|
||
double t1 = R * cos(Q);
|
||
double t2 = R * sin(Q);
|
||
Vector3 P = {
|
||
S.x + t1 * ex.x + t2 * ey.x, //因为 t3=0；
|
||
S.y + t1 * ex.y + t2 * ey.y,
|
||
S.z + t1 * ex.z + t2 * ey.z,
|
||
|
||
};
|
||
|
||
|
||
|
||
// 椭球验证
|
||
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 (isnan(Q) || isinf(Q) || fabs(check - 1.0) > 1e-6) {
|
||
return Vector3{ NAN,NAN,NAN };
|
||
printf("check value =%f\n", fabs(check - 1.0));
|
||
}
|
||
|
||
return P;
|
||
}
|
||
|
||
__device__ double angleBetweenVectors_single(Vector3_single a, Vector3_single b, bool returnDegrees)
|
||
{
|
||
// 计算点积
|
||
float dotProduct = a.x * b.x + a.y * b.y + a.z * b.z;
|
||
|
||
// 计算模长
|
||
float magA = sqrtf(a.x * a.x + a.y * a.y + a.z * a.z);
|
||
float magB = sqrtf(b.x * b.x + b.y * b.y + b.z * b.z);
|
||
|
||
// 处理零向量异常
|
||
if (magA == 0.0 || magB == 0.0) {
|
||
return NAN;
|
||
}
|
||
|
||
float cosTheta = dotProduct / (magA * magB);
|
||
cosTheta = cosTheta < -1 ? -1 : cosTheta>1 ? 1 : cosTheta; // 截断到[-1, 1]
|
||
float angleRad = acosf(cosTheta);
|
||
return returnDegrees ? angleRad * 180.0 / M_PI : angleRad;
|
||
}
|
||
|
||
__device__ Vector3_single vec_sub_single(Vector3_single a, Vector3_single b)
|
||
{
|
||
return { a.x - b.x, a.y - b.y, a.z - b.z };
|
||
}
|
||
|
||
__device__ float vec_dot_single(Vector3_single a, Vector3_single b)
|
||
{
|
||
return a.x * b.x + a.y * b.y + a.z * b.z;
|
||
}
|
||
|
||
__device__ Vector3_single vec_cross_single(Vector3_single a, Vector3_single b)
|
||
{
|
||
return Vector3_single{ a.y * b.z - a.z * b.y,
|
||
a.z * b.x - a.x * b.z,
|
||
a.x * b.y - a.y * b.x };
|
||
}
|
||
|
||
|
||
|
||
//// 参数校验与主函数
|
||
//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;
|
||
//}
|
||
//
|