Advertisement
Not a member of Pastebin yet?
Sign Up,
it unlocks many cool features!
- double DistBetweenSegments(Vector p0, Vector u, Vector q0, Vector v) {
- // находим расстояние между прямыми, содержащими отрезки
- pair <long double, bool> dist_lines = DistBetweenLines(p0, u, q0, v);
- if (dist_lines.second) { // если минимум достигается на точках отрезков
- return dist_lines.first;
- }
- Vector w0(p0.x - q0.x, p0.y - q0.y, p0.z - q0.z);
- // ищем минимум на границе параболоида (s == 0 || s == 1) && (t == 0 || t == 1)
- Vector w1(w0.x + u.x - v.x, w0.y + u.y - v.y, w0.z + u.z - v.z);
- long double minn = sqrt(ScalarProduct(w1, w1));
- Vector w2(w0.x + u.x, w0.y + u.y, w0.z + u.z);
- minn = min(minn, (long double)sqrt(ScalarProduct(w2, w2)));
- Vector w3(w0.x, w0.y, w0.z);
- minn = min(minn, (long double)sqrt(ScalarProduct(w3, w3)));
- Vector w4(w0.x - v.x, w0.y - v.y, w0.z - v.z);
- minn = min(minn, (long double)sqrt(ScalarProduct(w4, w4)));
- // ищем минимум на границе параболоида (s == 0 || s == 1 || t == 0 || t == 1)
- // хотим минимизировать:
- // |w|^2 = |p0 + s*u - q0 - t*v|^2 =
- // = |w0|^2 + s*s*|u|^2 + t*t*|v|^2 + 2*s*(u, w0) - 2*t*(v, w0) - 2*s*t(u, v)
- // будем по очереди присваивать параметрам s и t зн-ия 0 и 1
- // s = 0 => хотим минимизировать t*t*(v, v) - 2*t*(v, w0) - 2*t*(u, v)
- // тогда min t = (v, w0) / (v, v) как вершина параболы
- long double s_param = 0;
- long double t_param = ScalarProduct(v, w0) / ScalarProduct(v, v);
- Vector w5(w0.x + s_param*u.x - t_param*v.x, w0.y + s_param*u.y - t_param*v.y, w0.z + s_param*u.z - t_param*v.z);
- if (t_param >= 0 && t_param <= 1) {
- minn = min(minn, (long double)sqrt(ScalarProduct(w5, w5)));
- }
- // s = 1 => хотим минимизировать t*t*(v, v) - 2*t*(v, w0)
- // тогда min t = ((v, w0) + (v, u)) / (v, v) как вершина параболы
- s_param = 1;
- t_param = (ScalarProduct(v, w0) + ScalarProduct(v, u)) / ScalarProduct(v, v);
- Vector w6(w0.x + s_param*u.x - t_param*v.x, w0.y + s_param*u.y - t_param*v.y, w0.z + s_param*u.z - t_param*v.z);
- if (t_param >= 0 && t_param <= 1) {
- minn = min(minn, (long double)sqrt(ScalarProduct(w6, w6)));
- }
- // t = 0 => хотим минимизировать s*s*(u, u) + 2*s*(u, w0)
- // тогда min s = - (u, w0) / (u, u) как вершина параболы
- s_param = - ScalarProduct(u, w0) / ScalarProduct(u, u);
- t_param = 0;
- Vector w7(w0.x + s_param*u.x - t_param*v.x, w0.y + s_param*u.y - t_param*v.y, w0.z + s_param*u.z - t_param*v.z);
- if (s_param >= 0 && s_param <= 1) {
- minn = min(minn, (long double)sqrt(ScalarProduct(w7, w7)));
- }
- // t = 1 => хотим минимизировать s*s*(u, u) + 2*s*(u, w0) - 2*s*(u,v)
- // тогда min s = ((u,v) - (u, w0)) / (u, u) как вершина параболы
- s_param = (ScalarProduct(u, v) - ScalarProduct(u, w0)) / ScalarProduct(u, u);
- t_param = 1;
- Vector w8(w0.x + s_param*u.x - t_param*v.x, w0.y + s_param*u.y - t_param*v.y, w0.z + s_param*u.z - t_param*v.z);
- if (s_param >= 0 && s_param <= 1) {
- minn = min(minn, (long double)sqrt(ScalarProduct(w8, w8)));
- }
- return minn;
- };
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement