Advertisement
Guest User

Untitled

a guest
Nov 15th, 2018
77
0
Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
text 3.42 KB | None | 0 0
  1. double DistBetweenSegments(Vector p0, Vector u, Vector q0, Vector v) {
  2. // находим расстояние между прямыми, содержащими отрезки
  3. pair <long double, bool> dist_lines = DistBetweenLines(p0, u, q0, v);
  4. if (dist_lines.second) { // если минимум достигается на точках отрезков
  5. return dist_lines.first;
  6. }
  7.  
  8. Vector w0(p0.x - q0.x, p0.y - q0.y, p0.z - q0.z);
  9.  
  10. // ищем минимум на границе параболоида (s == 0 || s == 1) && (t == 0 || t == 1)
  11. Vector w1(w0.x + u.x - v.x, w0.y + u.y - v.y, w0.z + u.z - v.z);
  12. long double minn = sqrt(ScalarProduct(w1, w1));
  13.  
  14. Vector w2(w0.x + u.x, w0.y + u.y, w0.z + u.z);
  15. minn = min(minn, (long double)sqrt(ScalarProduct(w2, w2)));
  16.  
  17. Vector w3(w0.x, w0.y, w0.z);
  18. minn = min(minn, (long double)sqrt(ScalarProduct(w3, w3)));
  19.  
  20. Vector w4(w0.x - v.x, w0.y - v.y, w0.z - v.z);
  21. minn = min(minn, (long double)sqrt(ScalarProduct(w4, w4)));
  22.  
  23. // ищем минимум на границе параболоида (s == 0 || s == 1 || t == 0 || t == 1)
  24. // хотим минимизировать:
  25. // |w|^2 = |p0 + s*u - q0 - t*v|^2 =
  26. // = |w0|^2 + s*s*|u|^2 + t*t*|v|^2 + 2*s*(u, w0) - 2*t*(v, w0) - 2*s*t(u, v)
  27. // будем по очереди присваивать параметрам s и t зн-ия 0 и 1
  28.  
  29. // s = 0 => хотим минимизировать t*t*(v, v) - 2*t*(v, w0) - 2*t*(u, v)
  30. // тогда min t = (v, w0) / (v, v) как вершина параболы
  31. long double s_param = 0;
  32. long double t_param = ScalarProduct(v, w0) / ScalarProduct(v, v);
  33. 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);
  34. if (t_param >= 0 && t_param <= 1) {
  35. minn = min(minn, (long double)sqrt(ScalarProduct(w5, w5)));
  36. }
  37.  
  38. // s = 1 => хотим минимизировать t*t*(v, v) - 2*t*(v, w0)
  39. // тогда min t = ((v, w0) + (v, u)) / (v, v) как вершина параболы
  40. s_param = 1;
  41. t_param = (ScalarProduct(v, w0) + ScalarProduct(v, u)) / ScalarProduct(v, v);
  42. 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);
  43. if (t_param >= 0 && t_param <= 1) {
  44. minn = min(minn, (long double)sqrt(ScalarProduct(w6, w6)));
  45. }
  46.  
  47. // t = 0 => хотим минимизировать s*s*(u, u) + 2*s*(u, w0)
  48. // тогда min s = - (u, w0) / (u, u) как вершина параболы
  49. s_param = - ScalarProduct(u, w0) / ScalarProduct(u, u);
  50. t_param = 0;
  51. 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);
  52. if (s_param >= 0 && s_param <= 1) {
  53. minn = min(minn, (long double)sqrt(ScalarProduct(w7, w7)));
  54. }
  55.  
  56. // t = 1 => хотим минимизировать s*s*(u, u) + 2*s*(u, w0) - 2*s*(u,v)
  57. // тогда min s = ((u,v) - (u, w0)) / (u, u) как вершина параболы
  58. s_param = (ScalarProduct(u, v) - ScalarProduct(u, w0)) / ScalarProduct(u, u);
  59. t_param = 1;
  60. 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);
  61. if (s_param >= 0 && s_param <= 1) {
  62. minn = min(minn, (long double)sqrt(ScalarProduct(w8, w8)));
  63. }
  64.  
  65. return minn;
  66. };
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement