WGS84 中 2 "linearly" 个移动物体之间的碰撞检测
Collision detection between 2 "linearly" moving objects in WGS84
[Spektre 根据评论完成重新编辑]
我在 3D (WGS84) 中有两个起点和速度矢量，我如何检查它们是否在指定时间内在 3D 中发生碰撞？
示例输入：
// WGS84 objects positions
const double deg=M_PI/180.0;
double pos0[3]={17.76 *deg,48.780 *deg,6054.0}; // lon[rad],lat[rad],alt[m]
double pos1[3]={17.956532816382374*deg,48.768667387202690*deg,3840.0}; // lon[rad],lat[rad],alt[m]
// WGS84 speeds in [km/h] not in [deg/sec]!!!
double vel0[3]={- 29.346910862289782, 44.526061886823861,0.0}; // [km/h] lon,lat,alt
double vel1[3]={- 44.7 ,-188.0 ,0.0}; // [km/h] lon,lat,alt
这里正确地将位置转换为笛卡尔坐标（使用下面链接的在线转换器）：
double pos0[3]={ 4013988.58505233,1285660.27718040,4779026.13957769 }; // [m]
double pos1[3]={ 4009069.35282446,1299263.86628867,4776529.76526759 }; // [m]
这些使用我从下面链接的 QA 转换而来（差异可能由不同的椭球和/或浮点错误引起）：
double pos0[3] = { 3998801.90188399, 1280796.05923908, 4793000.78262020 }; // [m]
double pos1[3] = { 3993901.28864493, 1294348.18237911, 4790508.28581325 }; // [m]
double vel0[3] = { 11.6185787807449, 41.1080659685389, 0 }; // [km/h]
double vel1[3] = { 17.8265828114202,-173.3281435179590, 0 }; // [km/h]
我的问题是：如何检测物体是否会发生碰撞以及何时发生碰撞？
我真正需要的是碰撞是否在某个指定时间内发生，例如_min_t。
请注意，速度在 [km/h] 方向，朝向局部 North,East,High/Up 向量！有关将此类速度转换为笛卡尔坐标的更多信息，请参阅相关内容：
要validate/check WGS84 位置转换，您可以使用以下在线计算器：
我想尽可能避免使用网格、图元或类似的东西。
这是安德烈试图解决这个问题的尝试（基于我的回答，但缺少速度转换）从原始 post:
bool collisionDetection()
{
const double _min_t = 10.0; // min_time
const double _max_d = 5000; // max_distance
const double _max_t = 0.001; // max_time
double dt;
double x, y, z, d0, d1;
VectorXYZd posObj1 = WGS84::ToCartesian(m_sPos1);
VectorXYZd posObj2 = WGS84::ToCartesian(m_sPos2);
const QList<QVariant> velocity;
if (velocity.size() == 3)
{
dt = _max_t;
x = posObj1 .x - posObj2 .x;
y = posObj1 .y - posObj2 .y;
z = posObj1 .z - posObj2 .z;
d0 = sqrt((x*x) + (y*y) + (z*z));
x = posObj1 .x - posObj2 .x + (m_sVelAV.x - velocity.at(0).toDouble())*dt;
y = posObj1 .y - posObj2 .y + (m_sVelAV.y - velocity.at(1).toDouble())*dt;
z = posObj1 .z - posObj2 .z + (m_sVelAV.z - velocity.at(2).toDouble())*dt;
d1 = sqrt((x*x) + (y*y) + (z*z));
double t = (_max_d - d0)*dt / (d1 - d0);
if (d0 <= _max_d)
{
return true;
}
if (d0 <= d1)
{
return false;
}
if (t < _min_t)
{
return true;
}
}
return false;
}
这应该是有效的笛卡尔变换位置和速度，但由于 x, y, z 参数的顺序错误而导致错误变换。上面的数据是正确的 lon, lat, alt 和 x, y, z 这显然不是：
posObject2 = {x=1296200.8297778680 y=4769355.5802477235 z=4022514.8921807557 }
posObject1 = {x=1301865.2949957885 y=4779902.8263504291 z=4015541.3863254949 }
velocity object 2: x = -178, y = -50, z = 8
velocity object 1: x = 0, y = -88, z = 0;
更不用说速度仍然不是笛卡尔space ...
编辑：新测试用例
m_sPosAV = {North=48.970020901863471 East=18.038928517158574 Altitude=550.00000000000000 }
m_position = {North=48.996515594886858 East=17.989637729707006 Altitude=550.00000000000000 }
d0 = 4654.6937995573062
d1 = 4648.3896597230259
t = 65.904213878080199
dt = 0.1
velocityPoi = {x=104.92401431817457 y=167.91352303897233 z=0.00000000000000000 }
m_sVelAV = {x=0.00000000000000000 y=0.00000000000000000 z=0.00000000000000000 }
另一个测试用例：
m_sPosAV = {North=49.008020930461598 East=17.920928503349856 Altitude=550.00000000000000 }
m_position = {North=49.017421151053824 East=17.989399013104570 Altitude=550.00000000000000 }
d0 = 144495.56021027692
d1 = 144475.91709961568
velocityPoi = {x=104.92401431817457 y=167.91352303897233 z=0.00000000000000000 }
m_sVelAV = {x=0.89000000000000001 y=0.00000000000000000 z=0.
00000000000000000 }
t = 733.05884538126884
测试用例 3 碰撞时间为 0
m_sPosAV = {North=48.745020278145105 East=17.951529239281793 Altitude=4000.0000000000000 }
m_position = {North=48.734919749542570 East=17.943535418223373 Altitude=4000.0000000000000 }
v1 = {61.452929549676597, -58.567847120366054, 8.8118360639107198}
v0 = {0.00000000000000000, 0.00000000000000000, 0.00000000000000000}
pos0 = {0.85076109780503417, 0.31331329099350030, 4000.0000000000000}
pos1 = {0.85058481032472799, 0.31317377249621559, 3993.0000000000000}
d1 = 2262.4742373961790
最后一个测试用例：
p0 = 0x001dc7c4 {3933272.5980855357, 4681348.9804422557, 1864104.1897091190}
p1 = 0x001dc7a4 {3927012.3039519843, 4673002.8791717924, 1856993.0651808924}
dt = 100;
n = 6;
v1 = 0x001dc764 {18.446446996578750, 214.19570794229870, -9.9777430316824578}
v0 = 0x001dc784 {0.00000000000000000, 0.00000000000000000, 0.00000000000000000}
const double _max_d = 2500;
double _max_T = 120;
最终测试用例：
m_sPosAV = {North=49.958099932390311 East=16.958899924978102 Altitude=9000.0000000000000 }
m_position = {North=49.956106045262935 East=16.928683918401916 Altitude=9000.0000000000000 }
p0 = 0x0038c434 {3931578.2438977188, 4678519.9203961492, 1851108.3449359399}
p1 = 0x0038c414 {3933132.4705292359, 4679955.4705412844, 1850478.2954359739}
vel0 = 0x0038c3b4 {0.00000000000000000, 0.00000000000000000, 0.00000000000000000}
vel1 = 0x0038c354 {-55.900000000000006, 185.69999999999999, -8.0000000000000000}
dt = 1; // [sec] initial time step (accuracy = dt/10^(n-1)
n = 5; // accuracy loops
最终代码：
const double _max_d = 2500; // max_distance m
m_Time = 3600.0;
int i, e, n;
double t, dt;
double x, y, z, d0, d1 = 0;
double p0[3], p1[3], v0[3], v1[3];
double vel0[3], pos0[3], pos1[3], vel1[3];
vel0[0] = m_sVelAV.x;
vel0[1] = m_sVelAV.y;
vel0[2] = m_sVelAV.z;
vel1[0] = velocityPoi.x;
vel1[1] = velocityPoi.y;
vel1[2] = velocityPoi.z;
pos0[0] = (m_sPosAV.GetLatitude()*pi) / 180;
pos0[1] = (m_sPosAV.GetLongitude()*pi) / 180;
pos0[2] = m_sPosAV.GetAltitude();
pos1[0] = (poi.Position().GetLatitude()*pi) / 180;
pos1[1] = (poi.Position().GetLongitude()*pi) / 180;
pos1[2] = poi.Position().GetAltitude();
WGS84toXYZ_posvel(p0, v0, pos0, vel0);
WGS84toXYZ_posvel(p1, v1, pos1, vel1);
dt = 1; // [sec] initial time step (accuracy = dt/10^(n-1)
n = 5; // accuracy loops
for (t = 0.0, i = 0; i<n; i++)
for (e = 1; t <= m_Time; t += dt)
{
d0 = d1;
// d1 = relative distance in time t
x = p0[0] - p1[0] + (v0[0] - v1[0])*t;
y = p0[1] - p1[1] + (v0[1] - v1[1])*t;
z = p0[2] - p1[2] + (v0[2] - v1[2])*t;
d1 = sqrt((x*x) + (y*y) + (z*z));
if (e) { e = 0; continue; }
// if bigger then last step stop (and search with 10x smaller time step)
if (d0<d1) { d1 = d0; t -= dt + dt; dt *= 0.1; if (t<0.0) t = 0.0; break; }
}
// handle big distance as no collision
if (d1 > _max_d) return false;
if (t >= m_Time) return false;
qDebug() << "Collision at time t= " << t;
你没有给出任何代码，所以我只给出主要思路，编码留给你。如果您尝试一些代码并卡住了，请回来，但请展示您的努力和到目前为止的代码。
有多种方法可以解决您的问题。一种方法是为每个对象设置参数方程，及时给出你的两个函数 t。将这些函数的结果设置为相等并求解时间。对于给你三个问题的 3D 坐标，每个坐标一个问题，t 的值不太可能对所有三个方程都相同。如果相同，就是你碰撞的时间
另一种允许一些浮点舍入误差的方法是将参照系更改为其中一个对象的参照系。你减去两个速度矢量，比如说 v2-v1，你现在有第二个物体相对于第一个物体的速度。现在找到从现在静止的第一个物体到移动的第二个物体的线的距离。如果您不知道该怎么做，请在您最喜欢的搜索引擎中查找 'distance from point to line'。然后，您会看到该距离是否小到足以让您将其视为碰撞——给定浮点舍入误差，您不太可能获得完美的碰撞、零距离。如果它足够小，您就会看到该碰撞是在未来发生还是在过去发生。您可能希望找到该点在直线上的投影作为最后一次计算的中间值。
清楚了吗？
[Edit5] 如果您需要旧资源，请完成重新编辑，请参阅修订历史
正如 Nico Schertler 所指出的，检查线与线的交点是一种疯狂行为，因为在同一位置和时间相交 2 条轨迹的概率几乎是 none（即使包括舍入精度重叠）。相反，您应该在每个轨迹上找到足够接近（碰撞）的位置，并且两个物体都在相对相同的时间出现。另一个问题是你的轨迹根本不是线性的。是的，它们在 WGS84 展位和笛卡尔坐标系中可以在短时间内呈现线性，但随着时间的增加，轨迹会围绕地球弯曲。此外，您的速度输入值单位使这变得有点困难，所以让我重述一下我将从现在开始处理的标准化值：
输入
由两个对象组成。每个已知其实际位置（WGS84[rad]）和实际速度[m/s]但不是笛卡尔space而是WGS84 局部坐标轴。例如这样的事情：
const double kmh=1.0/3.6;
const double deg=M_PI/180.0;
const double rad=180.0/M_PI;
// lon lat alt
double pos0[3]={ 23.000000*deg, 48.000000*deg,2500.000000 };
double pos1[3]={ 23.000000*deg, 35.000000*deg,2500.000000 };
double vel0[3]={ 100.000000*kmh,-20.000000*kmh, 0.000000*kmh };
double vel1[3]={ 100.000000*kmh, 20.000000*kmh, 0.000000*kmh };
注意我的坐标在 Long,Lat,Alt order/convention !!!
输出
您需要计算两个对象的时间 "collide" 稍后可以添加对解决方案的其他约束。如前所述，我们不是在搜索交叉点，而是 "closest" 满足碰撞条件的方法（比如距离小于某个阈值）。
经过一些教学和测试后，我决定在 WGS84 space 中使用迭代方法。这带来了一些问题，例如如何将 WGS84 space 中的 [m/s] 中的速度转换为 WGS84[=124= 中的 [rad/s] ] space。该比率随物体高度和纬度而变化。实际上，我们需要计算 long 和 lat 轴的角度变化 "precisely" 等于 1m 行进距离，然后将速度乘以它。这可以用弧长方程近似：
l = dang.R
其中 R 是 angular 运动的实际半径，ang 是角度变化，l 是行进距离，所以当 l=1.0 时：
dang = 1.0/R
如果我们有笛卡尔坐标 x,y,z（z 是地球自转轴）那么：
Rlon = sqrt (x*x + y*y)
Rlat = sqrt (x*x + y*y + z*z)
现在我们可以用时间迭代位置，这可以用来估算最接近时间。然而，我们需要限制最大时间步长，这样我们就不会错过大部分地球曲率。此限制取决于使用的速度和目标精度。所以这里找到算法的处理方法：
初始化
将初始时间步长设置为上限，如 dt=1000.0 并计算笛卡尔坐标中展位对象的实际位置 space。从那计算他们的距离 d1.
迭代
设置d0=d1然后计算WGS84中实际位置的实际速度，并将speed*dt添加到每个对象的实际WGS84位置。现在只需计算笛卡尔 space 中的实际位置并计算它们的距离 d1
if d0>d1 那么它意味着我们正在接近最近的方法所以再次转到 #2。
如果d0==d1轨迹是平行的，那么return接近时间t=0.0
如果 d0<d1 我们已经越过最近的路径，那么设置 dt = -0.1*dt 如果 dt>=desired_accuracy 转到 #2 否则停止。
恢复最好t
在 #2 的迭代之后我们应该恢复最好的时间所以 return t+10.0*dt;
现在我们有最接近的时间 t。当心它可能是负面的（如果物体彼此远离）。现在您可以添加约束，例如
if (d0<_max_d)
if ((t>=0.0)&&(t<=_max_T))
return collision ...
此处 C++ 来源：
//---------------------------------------------------------------------------
#include <math.h>
//---------------------------------------------------------------------------
const double kmh=1.0/3.6;
const double deg=M_PI/180.0;
const double rad=180.0/M_PI;
const double _earth_a=6378137.00000; // [m] WGS84 equator radius
const double _earth_b=6356752.31414; // [m] WGS84 epolar radius
const double _earth_e=8.1819190842622e-2; // WGS84 eccentricity
const double _earth_ee=_earth_e*_earth_e;
//--------------------------------------------------------------------------
const double _max_d=2500.0; // [m] collision gap
const double _max_T=3600000.0; // [s] max collision time
const double _max_dt=1000.0; // [s] max iteration time step (for preserving accuracy)
//--------------------------------------------------------------------------
// lon lat alt
double pos0[3]={ 23.000000*deg, 48.000000*deg,2500.000000 }; // [rad,rad,m]
double pos1[3]={ 23.000000*deg, 35.000000*deg,2500.000000 }; // [rad,rad,m]
double vel0[3]={ 100.000000*kmh,-20.000000*kmh, 0.000000*kmh }; // [m/s,m/s,m/s]
double vel1[3]={ 100.000000*kmh,+20.000000*kmh, 0.000000*kmh }; // [m/s,m/s,m/s]
//---------------------------------------------------------------------------
double divide(double x,double y)
{
if ((y>=-1e-30)&&(y<=+1e-30)) return 0.0;
return x/y;
}
void vector_copy(double *c,double *a) { for(int i=0;i<3;i++) c[i]=a[i]; }
double vector_len(double *a) { return sqrt((a[0]*a[0])+(a[1]*a[1])+(a[2]*a[2])); }
void vector_len(double *c,double *a,double l)
{
l=divide(l,sqrt((a[0]*a[0])+(a[1]*a[1])+(a[2]*a[2])));
c[0]=a[0]*l;
c[1]=a[1]*l;
c[2]=a[2]*l;
}
void vector_sub(double *c,double *a,double *b) { for(int i=0;i<3;i++) c[i]=a[i]-b[i]; }
//---------------------------------------------------------------------------
void WGS84toXYZ(double *xyz,double *abh)
{
double a,b,h,l,c,s;
a=abh[0];
b=abh[1];
h=abh[2];
c=cos(b);
s=sin(b);
// WGS84 from eccentricity
l=_earth_a/sqrt(1.0-(_earth_ee*s*s));
xyz[0]=(l+h)*c*cos(a);
xyz[1]=(l+h)*c*sin(a);
xyz[2]=(((1.0-_earth_ee)*l)+h)*s;
}
//---------------------------------------------------------------------------
void WGS84_m2rad(double &da,double &db,double *abh)
{
// WGS84 from eccentricity
double p[3],rr;
WGS84toXYZ(p,abh);
rr=(p[0]*p[0])+(p[1]*p[1]);
da=divide(1.0,sqrt(rr));
rr+=p[2]*p[2];
db=divide(1.0,sqrt(rr));
}
//---------------------------------------------------------------------------
double collision(double *pos0,double *vel0,double *pos1,double *vel1)
{
int e,i,n;
double p0[3],p1[3],q0[3],q1[3],da,db,dt,t,d0,d1,x,y,z;
vector_copy(p0,pos0);
vector_copy(p1,pos1);
// find closest d1[m] approach in time t[sec]
dt=_max_dt; // [sec] initial time step (accuracy = dt/10^(n-1)
n=6; // acuracy loops
for (t=0.0,i=0;i<n;i++)
for (e=0;;e=1)
{
d0=d1;
// compute xyz distance
WGS84toXYZ(q0,p0);
WGS84toXYZ(q1,p1);
vector_sub(q0,q0,q1);
d1=vector_len(q0);
// nearest approach crossed?
if (e)
{
if (d0<d1){ dt*=-0.1; break; } // crossing trajectories
if (fabs(d0-d1)<=1e-10) { i=n; t=0.0; break; } // parallel trajectories
}
// apply time step
t+=dt;
WGS84_m2rad(da,db,p0);
p0[0]+=vel0[0]*dt*da;
p0[1]+=vel0[1]*dt*db;
p0[2]+=vel0[2]*dt;
WGS84_m2rad(da,db,p1);
p1[0]+=vel1[0]*dt*da;
p1[1]+=vel1[1]*dt*db;
p1[2]+=vel1[2]*dt;
}
t+=10.0*dt; // recover original t
// if ((d0<_max_d)&&(t>=0.0)&&(t<=_max_T)) return collision; else return no_collision;
return t;
}
//---------------------------------------------------------------------------
这里是示例的概述：
红色是object0，绿色是object1。白色方块表示在时间 t_coll [s] 与距离 d_coll [m] 处计算的碰撞位置。黄色方块是用户定义时间 t_anim [s] 的位置，距离 d_anim [m] 由用户控制，用于调试目的。如您所见，这种方法也适用于 36 小时等时间 ...
希望我没有忘记复制一些东西（如果是的话评论我，我会添加它）
[Spektre 根据评论完成重新编辑]
我在 3D (WGS84) 中有两个起点和速度矢量，我如何检查它们是否在指定时间内在 3D 中发生碰撞？
示例输入：
// WGS84 objects positions
const double deg=M_PI/180.0;
double pos0[3]={17.76 *deg,48.780 *deg,6054.0}; // lon[rad],lat[rad],alt[m]
double pos1[3]={17.956532816382374*deg,48.768667387202690*deg,3840.0}; // lon[rad],lat[rad],alt[m]
// WGS84 speeds in [km/h] not in [deg/sec]!!!
double vel0[3]={- 29.346910862289782, 44.526061886823861,0.0}; // [km/h] lon,lat,alt
double vel1[3]={- 44.7 ,-188.0 ,0.0}; // [km/h] lon,lat,alt
这里正确地将位置转换为笛卡尔坐标（使用下面链接的在线转换器）：
double pos0[3]={ 4013988.58505233,1285660.27718040,4779026.13957769 }; // [m]
double pos1[3]={ 4009069.35282446,1299263.86628867,4776529.76526759 }; // [m]
这些使用我从下面链接的 QA 转换而来（差异可能由不同的椭球和/或浮点错误引起）：
double pos0[3] = { 3998801.90188399, 1280796.05923908, 4793000.78262020 }; // [m]
double pos1[3] = { 3993901.28864493, 1294348.18237911, 4790508.28581325 }; // [m]
double vel0[3] = { 11.6185787807449, 41.1080659685389, 0 }; // [km/h]
double vel1[3] = { 17.8265828114202,-173.3281435179590, 0 }; // [km/h]
我的问题是：如何检测物体是否会发生碰撞以及何时发生碰撞？
我真正需要的是碰撞是否在某个指定时间内发生，例如_min_t。
请注意，速度在 [km/h] 方向，朝向局部 North,East,High/Up 向量！有关将此类速度转换为笛卡尔坐标的更多信息，请参阅相关内容：
要validate/check WGS84 位置转换，您可以使用以下在线计算器：
我想尽可能避免使用网格、图元或类似的东西。
这是安德烈试图解决这个问题的尝试（基于我的回答，但缺少速度转换）从原始 post:
bool collisionDetection()
{
const double _min_t = 10.0; // min_time
const double _max_d = 5000; // max_distance
const double _max_t = 0.001; // max_time
double dt;
double x, y, z, d0, d1;
VectorXYZd posObj1 = WGS84::ToCartesian(m_sPos1);
VectorXYZd posObj2 = WGS84::ToCartesian(m_sPos2);
const QList<QVariant> velocity;
if (velocity.size() == 3)
{
dt = _max_t;
x = posObj1 .x - posObj2 .x;
y = posObj1 .y - posObj2 .y;
z = posObj1 .z - posObj2 .z;
d0 = sqrt((x*x) + (y*y) + (z*z));
x = posObj1 .x - posObj2 .x + (m_sVelAV.x - velocity.at(0).toDouble())*dt;
y = posObj1 .y - posObj2 .y + (m_sVelAV.y - velocity.at(1).toDouble())*dt;
z = posObj1 .z - posObj2 .z + (m_sVelAV.z - velocity.at(2).toDouble())*dt;
d1 = sqrt((x*x) + (y*y) + (z*z));
double t = (_max_d - d0)*dt / (d1 - d0);
if (d0 <= _max_d)
{
return true;
}
if (d0 <= d1)
{
return false;
}
if (t < _min_t)
{
return true;
}
}
return false;
}
这应该是有效的笛卡尔变换位置和速度，但由于 x, y, z 参数的顺序错误而导致错误变换。上面的数据是正确的 lon, lat, alt 和 x, y, z 这显然不是：
posObject2 = {x=1296200.8297778680 y=4769355.5802477235 z=4022514.8921807557 }
posObject1 = {x=1301865.2949957885 y=4779902.8263504291 z=4015541.3863254949 }
velocity object 2: x = -178, y = -50, z = 8
velocity object 1: x = 0, y = -88, z = 0;
更不用说速度仍然不是笛卡尔space ...
编辑：新测试用例
m_sPosAV = {North=48.970020901863471 East=18.038928517158574 Altitude=550.00000000000000 }
m_position = {North=48.996515594886858 East=17.989637729707006 Altitude=550.00000000000000 }
d0 = 4654.6937995573062
d1 = 4648.3896597230259
t = 65.904213878080199
dt = 0.1
velocityPoi = {x=104.92401431817457 y=167.91352303897233 z=0.00000000000000000 }
m_sVelAV = {x=0.00000000000000000 y=0.00000000000000000 z=0.00000000000000000 }
另一个测试用例：
m_sPosAV = {North=49.008020930461598 East=17.920928503349856 Altitude=550.00000000000000 }
m_position = {North=49.017421151053824 East=17.989399013104570 Altitude=550.00000000000000 }
d0 = 144495.56021027692
d1 = 144475.91709961568
velocityPoi = {x=104.92401431817457 y=167.91352303897233 z=0.00000000000000000 }
m_sVelAV = {x=0.89000000000000001 y=0.00000000000000000 z=0.
00000000000000000 }
t = 733.05884538126884
测试用例 3 碰撞时间为 0
m_sPosAV = {North=48.745020278145105 East=17.951529239281793 Altitude=4000.0000000000000 }
m_position = {North=48.734919749542570 East=17.943535418223373 Altitude=4000.0000000000000 }
v1 = {61.452929549676597, -58.567847120366054, 8.8118360639107198}
v0 = {0.00000000000000000, 0.00000000000000000, 0.00000000000000000}
pos0 = {0.85076109780503417, 0.31331329099350030, 4000.0000000000000}
pos1 = {0.85058481032472799, 0.31317377249621559, 3993.0000000000000}
d1 = 2262.4742373961790
最后一个测试用例：
p0 = 0x001dc7c4 {3933272.5980855357, 4681348.9804422557, 1864104.1897091190}
p1 = 0x001dc7a4 {3927012.3039519843, 4673002.8791717924, 1856993.0651808924}
dt = 100;
n = 6;
v1 = 0x001dc764 {18.446446996578750, 214.19570794229870, -9.9777430316824578}
v0 = 0x001dc784 {0.00000000000000000, 0.00000000000000000, 0.00000000000000000}
const double _max_d = 2500;
double _max_T = 120;
最终测试用例：
m_sPosAV = {North=49.958099932390311 East=16.958899924978102 Altitude=9000.0000000000000 }
m_position = {North=49.956106045262935 East=16.928683918401916 Altitude=9000.0000000000000 }
p0 = 0x0038c434 {3931578.2438977188, 4678519.9203961492, 1851108.3449359399}
p1 = 0x0038c414 {3933132.4705292359, 4679955.4705412844, 1850478.2954359739}
vel0 = 0x0038c3b4 {0.00000000000000000, 0.00000000000000000, 0.00000000000000000}
vel1 = 0x0038c354 {-55.900000000000006, 185.69999999999999, -8.0000000000000000}
dt = 1; // [sec] initial time step (accuracy = dt/10^(n-1)
n = 5; // accuracy loops
最终代码：
const double _max_d = 2500; // max_distance m
m_Time = 3600.0;
int i, e, n;
double t, dt;
double x, y, z, d0, d1 = 0;
double p0[3], p1[3], v0[3], v1[3];
double vel0[3], pos0[3], pos1[3], vel1[3];
vel0[0] = m_sVelAV.x;
vel0[1] = m_sVelAV.y;
vel0[2] = m_sVelAV.z;
vel1[0] = velocityPoi.x;
vel1[1] = velocityPoi.y;
vel1[2] = velocityPoi.z;
pos0[0] = (m_sPosAV.GetLatitude()*pi) / 180;
pos0[1] = (m_sPosAV.GetLongitude()*pi) / 180;
pos0[2] = m_sPosAV.GetAltitude();
pos1[0] = (poi.Position().GetLatitude()*pi) / 180;
pos1[1] = (poi.Position().GetLongitude()*pi) / 180;
pos1[2] = poi.Position().GetAltitude();
WGS84toXYZ_posvel(p0, v0, pos0, vel0);
WGS84toXYZ_posvel(p1, v1, pos1, vel1);
dt = 1; // [sec] initial time step (accuracy = dt/10^(n-1)
n = 5; // accuracy loops
for (t = 0.0, i = 0; i<n; i++)
for (e = 1; t <= m_Time; t += dt)
{
d0 = d1;
// d1 = relative distance in time t
x = p0[0] - p1[0] + (v0[0] - v1[0])*t;
y = p0[1] - p1[1] + (v0[1] - v1[1])*t;
z = p0[2] - p1[2] + (v0[2] - v1[2])*t;
d1 = sqrt((x*x) + (y*y) + (z*z));
if (e) { e = 0; continue; }
// if bigger then last step stop (and search with 10x smaller time step)
if (d0<d1) { d1 = d0; t -= dt + dt; dt *= 0.1; if (t<0.0) t = 0.0; break; }
}
// handle big distance as no collision
if (d1 > _max_d) return false;
if (t >= m_Time) return false;
qDebug() << "Collision at time t= " << t;
你没有给出任何代码，所以我只给出主要思路，编码留给你。如果您尝试一些代码并卡住了，请回来，但请展示您的努力和到目前为止的代码。
有多种方法可以解决您的问题。一种方法是为每个对象设置参数方程，及时给出你的两个函数 t。将这些函数的结果设置为相等并求解时间。对于给你三个问题的 3D 坐标，每个坐标一个问题，t 的值不太可能对所有三个方程都相同。如果相同，就是你碰撞的时间
另一种允许一些浮点舍入误差的方法是将参照系更改为其中一个对象的参照系。你减去两个速度矢量，比如说 v2-v1，你现在有第二个物体相对于第一个物体的速度。现在找到从现在静止的第一个物体到移动的第二个物体的线的距离。如果您不知道该怎么做，请在您最喜欢的搜索引擎中查找 'distance from point to line'。然后，您会看到该距离是否小到足以让您将其视为碰撞——给定浮点舍入误差，您不太可能获得完美的碰撞、零距离。如果它足够小，您就会看到该碰撞是在未来发生还是在过去发生。您可能希望找到该点在直线上的投影作为最后一次计算的中间值。
清楚了吗？
[Edit5] 如果您需要旧资源，请完成重新编辑，请参阅修订历史
正如 Nico Schertler 所指出的，检查线与线的交点是一种疯狂行为，因为在同一位置和时间相交 2 条轨迹的概率几乎是 none（即使包括舍入精度重叠）。相反，您应该在每个轨迹上找到足够接近（碰撞）的位置，并且两个物体都在相对相同的时间出现。另一个问题是你的轨迹根本不是线性的。是的，它们在 WGS84 展位和笛卡尔坐标系中可以在短时间内呈现线性，但随着时间的增加，轨迹会围绕地球弯曲。此外，您的速度输入值单位使这变得有点困难，所以让我重述一下我将从现在开始处理的标准化值：
输入
由两个对象组成。每个已知其实际位置（WGS84
[rad]）和实际速度[m/s]但不是笛卡尔space而是WGS84 局部坐标轴。例如这样的事情：const double kmh=1.0/3.6; const double deg=M_PI/180.0; const double rad=180.0/M_PI; // lon lat alt double pos0[3]={ 23.000000*deg, 48.000000*deg,2500.000000 }; double pos1[3]={ 23.000000*deg, 35.000000*deg,2500.000000 }; double vel0[3]={ 100.000000*kmh,-20.000000*kmh, 0.000000*kmh }; double vel1[3]={ 100.000000*kmh, 20.000000*kmh, 0.000000*kmh };注意我的坐标在
Long,Lat,Altorder/convention !!!输出
您需要计算两个对象的时间 "collide" 稍后可以添加对解决方案的其他约束。如前所述，我们不是在搜索交叉点，而是 "closest" 满足碰撞条件的方法（比如距离小于某个阈值）。
经过一些教学和测试后，我决定在 WGS84 space 中使用迭代方法。这带来了一些问题，例如如何将 WGS84 space 中的 [m/s] 中的速度转换为 WGS84[=124= 中的 [rad/s] ] space。该比率随物体高度和纬度而变化。实际上，我们需要计算 long 和 lat 轴的角度变化 "precisely" 等于 1m 行进距离，然后将速度乘以它。这可以用弧长方程近似：
l = dang.R
其中 R 是 angular 运动的实际半径，ang 是角度变化，l 是行进距离，所以当 l=1.0 时：
dang = 1.0/R
如果我们有笛卡尔坐标 x,y,z（z 是地球自转轴）那么：
Rlon = sqrt (x*x + y*y)
Rlat = sqrt (x*x + y*y + z*z)
现在我们可以用时间迭代位置，这可以用来估算最接近时间。然而，我们需要限制最大时间步长，这样我们就不会错过大部分地球曲率。此限制取决于使用的速度和目标精度。所以这里找到算法的处理方法：
初始化
将初始时间步长设置为上限，如
dt=1000.0并计算笛卡尔坐标中展位对象的实际位置 space。从那计算他们的距离d1.迭代
设置
d0=d1然后计算WGS84中实际位置的实际速度，并将speed*dt添加到每个对象的实际WGS84位置。现在只需计算笛卡尔 space 中的实际位置并计算它们的距离d1if
d0>d1那么它意味着我们正在接近最近的方法所以再次转到 #2。
如果d0==d1轨迹是平行的，那么return接近时间t=0.0
如果d0<d1我们已经越过最近的路径，那么设置dt = -0.1*dt如果dt>=desired_accuracy转到 #2 否则停止。恢复最好
t在 #2 的迭代之后我们应该恢复最好的时间所以 return
t+10.0*dt;
现在我们有最接近的时间 t。当心它可能是负面的（如果物体彼此远离）。现在您可以添加约束，例如
if (d0<_max_d)
if ((t>=0.0)&&(t<=_max_T))
return collision ...
此处 C++ 来源：
//---------------------------------------------------------------------------
#include <math.h>
//---------------------------------------------------------------------------
const double kmh=1.0/3.6;
const double deg=M_PI/180.0;
const double rad=180.0/M_PI;
const double _earth_a=6378137.00000; // [m] WGS84 equator radius
const double _earth_b=6356752.31414; // [m] WGS84 epolar radius
const double _earth_e=8.1819190842622e-2; // WGS84 eccentricity
const double _earth_ee=_earth_e*_earth_e;
//--------------------------------------------------------------------------
const double _max_d=2500.0; // [m] collision gap
const double _max_T=3600000.0; // [s] max collision time
const double _max_dt=1000.0; // [s] max iteration time step (for preserving accuracy)
//--------------------------------------------------------------------------
// lon lat alt
double pos0[3]={ 23.000000*deg, 48.000000*deg,2500.000000 }; // [rad,rad,m]
double pos1[3]={ 23.000000*deg, 35.000000*deg,2500.000000 }; // [rad,rad,m]
double vel0[3]={ 100.000000*kmh,-20.000000*kmh, 0.000000*kmh }; // [m/s,m/s,m/s]
double vel1[3]={ 100.000000*kmh,+20.000000*kmh, 0.000000*kmh }; // [m/s,m/s,m/s]
//---------------------------------------------------------------------------
double divide(double x,double y)
{
if ((y>=-1e-30)&&(y<=+1e-30)) return 0.0;
return x/y;
}
void vector_copy(double *c,double *a) { for(int i=0;i<3;i++) c[i]=a[i]; }
double vector_len(double *a) { return sqrt((a[0]*a[0])+(a[1]*a[1])+(a[2]*a[2])); }
void vector_len(double *c,double *a,double l)
{
l=divide(l,sqrt((a[0]*a[0])+(a[1]*a[1])+(a[2]*a[2])));
c[0]=a[0]*l;
c[1]=a[1]*l;
c[2]=a[2]*l;
}
void vector_sub(double *c,double *a,double *b) { for(int i=0;i<3;i++) c[i]=a[i]-b[i]; }
//---------------------------------------------------------------------------
void WGS84toXYZ(double *xyz,double *abh)
{
double a,b,h,l,c,s;
a=abh[0];
b=abh[1];
h=abh[2];
c=cos(b);
s=sin(b);
// WGS84 from eccentricity
l=_earth_a/sqrt(1.0-(_earth_ee*s*s));
xyz[0]=(l+h)*c*cos(a);
xyz[1]=(l+h)*c*sin(a);
xyz[2]=(((1.0-_earth_ee)*l)+h)*s;
}
//---------------------------------------------------------------------------
void WGS84_m2rad(double &da,double &db,double *abh)
{
// WGS84 from eccentricity
double p[3],rr;
WGS84toXYZ(p,abh);
rr=(p[0]*p[0])+(p[1]*p[1]);
da=divide(1.0,sqrt(rr));
rr+=p[2]*p[2];
db=divide(1.0,sqrt(rr));
}
//---------------------------------------------------------------------------
double collision(double *pos0,double *vel0,double *pos1,double *vel1)
{
int e,i,n;
double p0[3],p1[3],q0[3],q1[3],da,db,dt,t,d0,d1,x,y,z;
vector_copy(p0,pos0);
vector_copy(p1,pos1);
// find closest d1[m] approach in time t[sec]
dt=_max_dt; // [sec] initial time step (accuracy = dt/10^(n-1)
n=6; // acuracy loops
for (t=0.0,i=0;i<n;i++)
for (e=0;;e=1)
{
d0=d1;
// compute xyz distance
WGS84toXYZ(q0,p0);
WGS84toXYZ(q1,p1);
vector_sub(q0,q0,q1);
d1=vector_len(q0);
// nearest approach crossed?
if (e)
{
if (d0<d1){ dt*=-0.1; break; } // crossing trajectories
if (fabs(d0-d1)<=1e-10) { i=n; t=0.0; break; } // parallel trajectories
}
// apply time step
t+=dt;
WGS84_m2rad(da,db,p0);
p0[0]+=vel0[0]*dt*da;
p0[1]+=vel0[1]*dt*db;
p0[2]+=vel0[2]*dt;
WGS84_m2rad(da,db,p1);
p1[0]+=vel1[0]*dt*da;
p1[1]+=vel1[1]*dt*db;
p1[2]+=vel1[2]*dt;
}
t+=10.0*dt; // recover original t
// if ((d0<_max_d)&&(t>=0.0)&&(t<=_max_T)) return collision; else return no_collision;
return t;
}
//---------------------------------------------------------------------------
这里是示例的概述：
红色是object0，绿色是object1。白色方块表示在时间 t_coll [s] 与距离 d_coll [m] 处计算的碰撞位置。黄色方块是用户定义时间 t_anim [s] 的位置，距离 d_anim [m] 由用户控制，用于调试目的。如您所见，这种方法也适用于 36 小时等时间 ...
希望我没有忘记复制一些东西（如果是的话评论我，我会添加它）