# Untitled

By: a guest on May 7th, 2012  |  syntax: None  |  size: 2.21 KB  |  hits: 7  |  expires: Never
Text below is selected. Please press Ctrl+C to copy to your clipboard. (⌘+C on Mac)
1. #include<iostream>
2. #include<conio.h>
3.
4. using namespace std;
5.
6. // Clamp n to lie within the range [min, max]
7. float Clamp(float n, float min, float max)
8. {
9.     if (n < min) return min;
10.     if (n > max) return max;
11.   return n;
12. }
13. // Computes closest points C1 and C2 of S1(s)=P1+s*(Q1-P1) and
14. // S2(t)=P2+t*(Q2-P2), returning s and t. Function result is squared
15. // distance between between S1(s) and S2(t)
16. float ClosestPtSegmentSegment(Point p1, Point q1, Point p2, Point q2,
17. float &s, float &t, Point &c1, Point &c2)
18. {
19. Vector d1 = q1 - p1; // Direction vector of segment S1
20. Vector d2 = q2 - p2; // Direction vector of segment S2
21. Vector r = p1 - p2;
22. float a = Dot(d1, d1); // Squared length of segment S1, always nonnegative
23. float e = Dot(d2, d2); // Squared length of segment S2, always nonnegative
24. float f = Dot(d2, r);
25. // Check if either or both segments degenerate into points
26. if (a <= EPSILON && e <= EPSILON) {
27. // Both segments degenerate into points
28. s = t = 0.0f;
29. c1 = p1;
30. c2 = p2;
31.
32. return Dot(c1 - c2, c1 - c2);
33. }
34. if (a <= EPSILON) {
35. // First segment degenerates into a point
36. s = 0.0f;
37. t = f / e; // s = 0 => t = (b*s + f) / e = f / e
38. t = Clamp(t, 0.0f, 1.0f);
39. } else {
40. float c = Dot(d1, r);
41. if (e <= EPSILON) {
42. // Second segment degenerates into a point
43. t = 0.0f;
44. s = Clamp(-c / a, 0.0f, 1.0f); // t = 0 => s = (b*t - c) / a = -c / a
45. } else {
46. // The general nondegenerate case starts here
47. float b = Dot(d1, d2);
48. float denom = a*e-b*b; // Always nonnegative
49. // If segments not parallel, compute closest point on L1 to L2 and
50. // clamp to segment S1. Else pick arbitrary s (here 0)
51. if (denom != 0.0f) {
52. s = Clamp((b*f - c*e) / denom, 0.0f, 1.0f);
53. } else s = 0.0f;
54. // Compute point on L2 closest to S1(s) using
55. // t = Dot((P1 + D1*s) - P2,D2) / Dot(D2,D2) = (b*s + f) / e
56. t = (b*s + f) / e;
57. // If t in [0,1] done. Else clamp t, recompute s for the new value
58. // of t using s = Dot((P2 + D2*t) - P1,D1) / Dot(D1,D1)= (t*b - c) / a
59. // and clamp s to [0, 1]
60. if (t < 0.0f) {
61. t = 0.0f;
62. s = Clamp(-c / a, 0.0f, 1.0f);
63. } else if (t > 1.0f) {
64. t = 1.0f;
65. s = Clamp((b - c) / a, 0.0f, 1.0f);
66. }
67. }
68. }
69. c1 = p1 + d1 * s;
70. c2 = p2 + d2 * t;
71. return Dot(c1 - c2, c1 - c2);
72. }
73.
74. void main()
75. {
76.
77. _getch();
78. return;
79. }