Задача. Вычислить расстояние между двумя отрезками [latex]AB[/latex] и [latex]CD[/latex], заданных координатами вершин в четырехмерном пространстве.
Тесты
(1)
№ | [latex]A_0[/latex] | [latex]A_1 [/latex] | [latex]A_2[/latex] | [latex]A_3[/latex] | [latex]B_0[/latex] | [latex]B_1[/latex] | [latex]B_2[/latex] | [latex]B_3[/latex] |
1 | 1 | 0 | 0 | 0 | 2 | 0 | 0 | 0 |
2 | 1 | 0 | 0 | 0 | 3 | 0 | 0 | 0 |
3 | -1 | 1 | 0 | 6 | -2 | -1 | 1 | 6 |
4 | 1 | 1 | 1 | 1 | 2 | 2 | 2 | 2 |
5 | 1 | 1 | 1 | 2 | 8 | 5 | 7 | 10 |
6 | 1 | 1 | 0 | 0 | 1 | 0 | 1 | 1 |
7 | 3 | 4 | 7 | 8 | 9 | 5 | 6 | 2 |
8 | 1 | 2 | 2 | 3 | 1 | 4 | 8 | 9 |
(2)
№ | [latex]C_0[/latex] | [latex]C_1[/latex] | [latex]C_2[/latex] | [latex]C_3[/latex] | [latex]D_0[/latex] | [latex]D_1[/latex] | [latex]D_2[/latex] | [latex]D_3[/latex] | [latex]r[/latex] |
1 | 1 | 1 | 0 | 0 | 2 | 1 | 0 | 0 | 1.000000 |
2 | 2 | 4 | 0 | 0 | 2 | 4 | 3 | 0 | 4.000000 |
3 | -1 | -1 | 0 | 6 | 2 | 1 | 1 | 6 | 1.154701 |
4 | -9 | -9 | -9 | -9 | -5 | -5 | -5 | -5 | 12.000000 |
5 | 8 | 0 | 0 | 0 | 2 | 1 | 1 | 1 | 1.414214 |
6 | 2 | 3 | 2 | 0 | 1 | 4 | 9 | 1 | 3.000000 |
7 | 7 | 4 | 11 | 15 | 15 | 5 | 12 | 9 | 9.000000 |
8 | 5 | 7 | 2 | 23 | 4 | 8 | 8 | 21 | 13.000000 |
Код программы
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75 76 77 78 79 80 81 82 83 84 85 86 87 88 89 90 91 92 93 94 95 96 97 98 99 100 101 102 103 104 105 106 107 108 109 110 111 112 113 114 115 116 117 118 119 120 121 122 123 124 125 126 127 128 129 130 131 132 133 134 135 136 137 138 139 140 141 142 143 144 145 146 147 148 149 150 151 152 153 154 155 156 157 |
#include "stdio.h" #include <cmath> double dot(double * u, double * v); double abs(double * u); double min(double a, double b, double c, double d); void prod(double k, double * u, double * v); void diff(double * a, double * b, double * c); void summ(double * a, double * b, double * c); bool in(double s); bool in(double s, double t); double sd(double s, double t); double _(double s); double P0[4],P1[4],Q0[4],Q1[4]; double u[4],v[4],w0[4]; double InterSegmentDistance4d() //input: four 4d points P0, P1, Q0, Q1 // output: r = minimal distance between lines segments P0P1 and Q0Q1 { double r = 0; //calculation of three vectors //u = P1 - P0, v = Q1 - Q0, w0 = Q0 - P0 diff(P1, P0, u); diff(Q1, Q0, v); diff(Q0, P0, w0); //calculation of supporting scalar variables //a = u*u, b = u*v, c = v*v, d = w0*u, e = w0*v double a, b, c, d, e; a = dot(u,u); // a=u^2; b = dot(u,v); c = dot(v,v); // c=v^2; d = dot(u,w0); e = dot(v,w0); //calculation of main determinant // D = b^2 - a*c double D = b*b - a*c; if (D == 0) //lines are parallel { double t0=-e/c; double t1=-e/c; double s0 = (-e+b)/c ; double s1= (e-c)*b/(c*a); if (in(t0) || in(t1) || in(s0) || in(s1)) { double wq[4]; prod(e/c,v, wq); //projection w0 on line q double wd[4]; diff(w0,wq, wd); r = abs(wd); return r ; //r = |w0 - wq| } //else minimal distance between segments equals minimal distance between ends of segments r = min(sd(0,0), sd(0,1), sd(1,0), sd(1,1)); return r; } // else lines are not parallel { //calculation of determinants double D1 = b*e - c*d, D2 = a*e - b*d; //calculation of parameters for minimal segment between lines double sc = D1/D, tc = D2/D; if(in(sc,tc)) //minimal distance between segments equals minimal distance between lines { return r = sd(sc,tc); // r = |wc| } //else minimal segment is defined by some parameter point (s,t) on edge of square [0,1]x[0,1] { double t1 = 0, s1=_(d/a); double t2 = _(-2*e/c), s2=0; double t3 = 1, s3=_(2*(b+d)/a); double t4 = _(2*(b+e)/c), s4=0; return min(sd(s1,t1), sd(s2,t2),sd(s3,t3), sd(s4,t4)); } } } int main() { scanf("%lf %lf %lf %lf",&P0[0], &P0[1], &P0[2],&P0[3]); scanf("%lf %lf %lf %lf",&P1[0], &P1[1], &P1[2],&P1[3]); scanf("%lf %lf %lf %lf",&Q0[0], &Q0[1], &Q0[2],&Q0[3]); scanf("%lf %lf %lf %lf",&Q1[0], &Q1[1], &Q1[2],&Q1[3]); printf("%lf\n", InterSegmentDistance4d()); return 0; } double dot(double * u, double * v)//scalar production of two vectors { return u[0]*v[0]+u[1]*v[1]+u[2]*v[2]+u[3]*v[3]; } double abs(double * u)//length of vector { return sqrt(dot(u,u)); } double min(double a, double b, double c, double d)//minimal from four values { double m = a; if(m>b) m = b; if(m>c) m = c; if(m>d) m = d; return m; } void diff(double * a, double * b, double * c)//difference of two 4d objects(points or vectors) { c[0]=a[0]-b[0]; c[1]=a[1]-b[1]; c[2]=a[2]-b[2]; c[3]=a[3]-b[3]; } void summ(double * a, double * b, double * c)//sum of two 4d objects(points or vecrors) { c[0]=a[0]+b[0]; c[1]=a[1]+b[1]; c[2]=a[2]+b[2]; c[3]=a[3]+b[3]; } void prod(double k, double * u, double * v)//production of number and vector { v[0] = k * u[0]; v[1] = k * u[1]; v[2] = k * u[2]; v[3] = k * u[3]; } bool in(double s)//checking if the vatue is in [0,1] { return 0 <= s && s <= 1 ; } bool in(double s, double t)//checking if the value is in [0,1]x[0,1] { return in(t) && in(s); } double sd(double s, double t)//distance between two points defined by two parameters s and t { double Pc[4], Qc[4], W[4]; prod(s,u, Pc); prod(t,v, Qc); summ(Pc,P0, Pc); summ(Qc,Q0, Qc); diff(Qc, Pc, W); return abs(W); } double _(double s)//restrict a value s on the interval [0,1] { double s1 = s; if(s > 1)s1 = 1; if(s < 0)s1 = 0; return s1; } |
Алгоритм и его обоснование
Расстояние между отрезками в четырехмерном пространстве находится по-разному, в зависимости от взаимного расположения этих отрезков. Тут мы можем выделить два основных случая:
- Отрезки лежат на параллельных прямых или на одной прямой.
- Отрезки лежат на пересекающихся либо на скрещивающихся прямых.
Чтобы выяснить, с каким случаем мы имеем дело, рассмотрим общую картину взаимного расположения отрезков и опишем ее математически:
По условию нам заданы 4 точки: [latex]A[/latex], [latex]B[/latex], [latex]C[/latex] и [latex]D[/latex] — концы двух отрезков. Для удобства представления уравнений и точек, связанных с ними, обозначим их [latex]P_0[/latex], [latex]P_1[/latex], [latex]Q_0[/latex] и [latex]Q_1[/latex] соответственно. Через эти пары точек мы можем провести 2 прямые [latex]p[/latex] и [latex]q[/latex], параметрические уравнения которых имеют вид:
\begin{matrix}
\vec p = P_0 + \vec u \cdot s \\
\vec q = Q_0 + \vec v \cdot t
\end{matrix}
где векторы:
[latex]\begin{matrix}
\vec u = P_1 — P_0\\
\vec v = Q_1 — Q_0
\end{matrix}
[/latex],а [latex]s[/latex] и [latex]t[/latex] — параметры. При [latex]s=0[/latex] или [latex] t=0[/latex] мы получаем начальную точку соответствующего отрезка, а при [latex]s=1[/latex] или [latex]t=1[/latex] — конечную. При произвольном значении параметра мы получаем произвольную точку на прямой.
Рассмотрим вектор [latex]\vec w = Q — P[/latex] , соединяющий 2 произвольные точки на этих прямых. Легко показать, что вектор [latex]\vec w[/latex] соединяет 2 ближайшие точки [latex]Q_c[/latex] и [latex]P_c[/latex] при условии:
[latex]\vec w \perp p[/latex] и [latex]\vec w \perp q[/latex].Этому условию соответствует система из двух уравнений:
[latex] \begin{cases}\vec u \cdot \vec w = 0\\
\vec v \cdot \vec w = 0
\end{cases} [/latex]
Распишем ее для [latex]\vec w = Q_0 — P_0 + \vec v \cdot t — \vec u \cdot s = \vec w_0 + \vec v \cdot t — \vec u \cdot s[/latex] :
[latex]
\begin{cases}
\vec u \cdot ( \vec w_0 + \vec v \cdot t — \vec u \cdot s ) = 0\\
\vec v \cdot ( \vec w_0 + \vec v \cdot t — \vec u \cdot s ) = 0
\end{cases}
Введем вспомогательные скалярные переменные:
[latex]
\begin{matrix}
a&=&\vec u \cdot \vec u\\
b&=&\vec u \cdot \vec v\\
c&=&\vec v \cdot \vec v\\
d&=&\vec u \cdot \vec w_0\\
e&=&\vec v \cdot \vec w_0
\end{matrix}
[/latex]Теперь наша система будет выглядеть так:
[latex]\begin{cases}
d — a \cdot s + b \cdot t = 0 \\
e — b \cdot s + c \cdot t = 0
\end{cases}
[/latex]
Перепишем систему в удобном для нас виде:
[latex] \begin{cases}a \cdot s — b \cdot t = d \\
b \cdot s — c \cdot t = e
\end{cases}
[/latex]
Решение этой системы мы можем получить, например, методом Крамера.
Главный определитель системы: [latex]D = b^2 — a \cdot c[/latex]
Два вспомогательных определителя:
[latex]
\begin{matrix}
D_1 = b \cdot e — c \cdot d\\
D_2 = a \cdot e — b \cdot d\\
\end{matrix}
[/latex]
Если [latex]D \neq 0[/latex], то существует единственное решение:
\begin{cases}
s_c = \frac{D_1}{D} \\
t_c = \frac{D_2}{D}
\end{cases}
[/latex]
Если же мы получаем [latex]D = 0[/latex], легко показать, что отрезки параллельны. То есть мы имеем дело со случаем 1.
Тогда:
а) Если хотя бы одна точка одного отрезка проецируется на другой отрезок, то расстояние между отрезками равняется расстоянию между прямыми.
Найдем проекцию точки [latex]P_0[/latex] на линию [latex]q[/latex]. Для этого сначала найдем вектор, который является проекцией вектора [latex]\vec w_0[/latex] на линию [latex]q[/latex].
[latex]\vec w_q=(\vec w_0 \cdot \vec v) \cdot \frac{\vec v}{v^2}[/latex].Конец полученного вектора находится в точке [latex]Q_0[/latex], а начало в новой точке [latex]P_{0q}=Q_0-\vec w_q[/latex]. Соединим точки [latex]P_0[/latex] и [latex]P_{0q}[/latex] вектором [latex]\vec w_p = P_{0q} — P_0[/latex]. Длина полученного вектора и будет искомым расстоянием: [latex]r = \left| P_0 P_{0q} \right|[/latex].
Для проверки условия а) необходимо получить проекции остальных исходных точек на отрезки:
[latex]
\begin{matrix}
P_{1q} = P_{0q} + \vec u\\
Q_{0p} = P_0 + \vec w_q\\
Q_{1p} = Q_{0p} + \vec v
\end{matrix}
[/latex]
Если точка [latex]P_{0q}[/latex] лежит на прямой [latex]q[/latex], задаваемой уравнением:
[latex]\vec q = Q_0 + \vec v \cdot t[/latex],
то определить, принадлежит ли точка [latex]P_{0q}[/latex] отрезку [latex]Q_0 Q_1[/latex] можно, решив уравнение:
[latex]P_{0q} = Q_0 + \vec v \cdot t[/latex].
[latex] 0 = \vec w_q + \vec v \cdot t[/latex].
Домножив обе части скалярно на вектор [latex]\vec v[/latex], мы получим уравнение: [latex] 0 = e + c \cdot t[/latex], отсюда [latex]t = \frac{-e}{c}[/latex].
Если [latex]t \in \left[0,1\right][/latex], то точка [latex]P_{0q}[/latex] лежит на отрезке [latex]Q_0 Q_1[/latex]. Если же нет, переходим к аналогичной проверке следующих точек:
[latex]P_{1q}:[/latex] [latex]P_{1q} = Q_0 + \vec v \cdot t[/latex].[latex] 0 = \vec w_q — \vec u + \vec v \cdot t[/latex].
Опять домножив обе части скалярно на вектор [latex]\vec v[/latex], мы получим уравнение: [latex] 0 = e — b + c \cdot t[/latex],
отсюда [latex]t = \frac{-e+b}{c}[/latex].
[latex]Q_{0p}:[/latex] [latex]Q_{0p} = P_0 + \vec u \cdot s[/latex].
[latex]0 =-\vec w_q + \vec u \cdot s[/latex].
Опять домножив обе части скалярно на вектор [latex]\vec u[/latex], мы получим уравнение: [latex] 0 = -\frac{e \cdot b}{c} + a \cdot s[/latex],
отсюда [latex]s = \frac{e \cdot b}{c \cdot a}[/latex].
[latex]Q_{1p}:[/latex] [latex]Q_{1p} = P_0 + \vec u \cdot s[/latex].
[latex] 0 = -\vec w_q — \vec v + \vec u \cdot s[/latex].
Опять домножив обе части скалярно на вектор [latex]\vec u[/latex], мы получим уравнение: [latex] 0 = -\frac{e \cdot b}{c} — b + a \cdot s[/latex],
отсюда [latex]s = \frac{(e — c) \cdot b}{c \cdot a}[/latex].
б) В противном случае, расстояние между отрезками равняется минимальному расстоянию между их концами. Здесь задача предельно упрощается. Мы находим длины отрезков, попарно соединяющих 4 исходные точки, и выбираем наименьший из них.
Если же исходные отрезки лежат на пересекающихся либо на скрещивающихся прямых, мы также рассматриваем 2 случая:
а) Оба конца кратчайшего отрезка, соединяющего прямые, лежат на соответствующих исходных отрезках:
[latex]P_c \in P_0 P_1[/latex] и [latex]Q_c \in Q_0 Q_1[/latex].
В этом случае пара параметров [latex](s_c, \; t_c)[/latex] принадлежит области: [latex](s,t):\left[0,1\right]\times \left[0,1\right].[/latex]
То есть, решение тривиально: ответом будет дина вектора [latex]\vec w_c[/latex]
б) Хотя бы один из концов кратчайшего отрезка, соединяющего прямые, не лежит на исходном отрезке, то есть:
[latex]P_c \not\in P_0 P_1[/latex] или [latex]Q_c \not\in Q_0 Q_1[/latex],
что соответствует значениям параметров [latex]s_c \not\in \left[0,1\right][/latex] или [latex]t_c \not\in \left[0,1\right][/latex].
В этом случае минимальное расстояние между отрезками определяется на границе области: [latex](s,t):\left[0,1\right]\times \left[0,1\right][/latex] (см. рисунок ниже):
Здесь решением является длина кратчайшего отрезка.
Длину отрезка, соединяющего 2 прямые, можно оценивать по квадрату длины вектора [latex]\vec v[/latex]: [latex]w^2=(\vec w)^2=(\vec w_0 — \vec u \cdot s + \vec v \cdot t)^2[/latex].
В частности, минимум [latex]w^2[/latex] достигается в точке [latex](s_c,t_c)[/latex].
Однако в случае б) мы должны найти минимум расстояния на границе нашей области, то есть решить задачу нахождения минимума при ограничениях (решить задачу условной минимизации). В нашем случае ограничения имеют очень простой вид — оси координат, и две линии, параллельные им. Поэтому мы можем решить на четырех границах 4 упрощенные задачи минимизации, а затем выбрать наименьшее решение.
Замечание: В пространстве параметров функция [latex]w^2(s,t)[/latex] представляет из себя эллиптический параболоид. Однако для простоты мы выше изобразили его линии уровня в виде окружностей. Типичный вид эллиптического параболоида и его линий уровня представлен на рисунках ниже:
Рассмотрим поочередно все 4 ограничения и решим задачу для них:
(1) Пусть [latex]t=t_1=0[/latex].
Тогда: [latex]{w^2\mid_{t_1=0}} = (\vec w_0-\vec u \cdot s_1)^2[/latex].
Для определения экстремума приравняем производную к нулю:[latex]
\begin{array}{r}
\frac{d}{ds_1}{(\vec w_0-\vec u \cdot s_1)^2}=0\\
2 \cdot (\vec w_0-\vec u \cdot s_1) \cdot (- \vec u)=0\\
-d +a \cdot s_1=0\\
s_1=\frac{d}{a}
\end{array}
[/latex]
Легко показать, что при [latex]s_1>1[/latex] мы должны присвоить ему значение [latex]s_1=1[/latex], а если [latex]s<0[/latex] — значение [latex]s_1=0[/latex], так как мы не должны выходить за границы исходных отрезков.
Подставим полученное значение [latex]s[/latex] в уравнение прямой [latex]p[/latex] для точки [latex]P_c[/latex]:[latex]P_c = P_0 + \vec u \cdot s.[/latex]
А точка [latex]Q_c[/latex] совпадает с точкой [latex]Q_0[/latex]. Тогда первый минимум равен: [latex]r_1 = Q_0 P_c[/latex].
Аналогично найдем три остальных минимума [latex]r_2, r_3, r_4[/latex], приравняв [latex]s[/latex] к нулю, а затем [latex]t[/latex] и [latex]s[/latex] к единице. Наименьший из них и есть искомое расстояние [latex]r[/latex].
Для отправки комментария необходимо войти на сайт.