原创文章，转载请注明出处！
第一步：计算三维空间内两条弧段的距离d，
We first consider two infinite lines L1: P(s) = P0 + s (P1-P0) = P0 + su and L2: Q(t) = Q0 + t (Q1-Q0) = Q0 + tv. Let w(s,t) = P(s)-Q(t) be a vector between points on the two lines. We want to find the w(s,t) that has a minimum length for all s and t. This can be computed using calculus [Eberly, 2001]. Here, we use a more geometric approach, and end up with the same result as he does. A similar geometric approach was used by [Teller, 2000], but he uses a cross product which restricts his method to 3D space whereas the method we use works in any dimension. Also, the solution given here and by Eberly are faster than Teller's which computes intermediate planes and gets their intersections with the two lines.
In any n-dimensional space, the two lines L1 and L2 are closest at unique points P(sc) and Q(tc) for which w(sc,tc) attains its minimum length. Also, if L1 and L2 are not parallel, then the line segment P(sc)Q(tc) joining the closest points is uniquely perpendicular to both lines at the same time. No other segment between L1 and L2 has this property. That is, the vector wc = w(sc,tc) is uniquely perpendicular to the line direction vectors u and v, and this is equivalent to it satisfying the two equations: u · wc = 0 and v · wc = 0.
We can solve these two equations by substituting wc = P(sc)-Q(tc) = w0+ scu- tcv, where w0 = P0-Q0, into each one to get two simultaneous linear equations:
Then, letting a = u · u, b = u · v, c = v · v, d = u · w0, and e = v · w0, we solve for sc and tc as:
whenever the denominator ac-b2 is nonzero. Note that ac-b2 = |u|2|v|2-(|u||v|cos q)2 = (|u||v|sin q)2 >=0 is always nonnegative. When ac-b2 = 0, the two equations are dependant, the two lines are parallel, and the distance between the lines is constant. We can solve for this parallel distance of separation by fixing the value of one parameter and using either equation to solve for the other. Selecting sc= 0, we get tc = d / b = e / c.
Having solved for sc and tc, we have the points P(sc) and Q(tc) where the two lines L1 and L2 are closest. Then the distance between them is given by:
第二步：如果d<0.000000001，相交
第三步：在XOY平面计算相交点。
在XOY平面内投影线的交点为0时不考虑（考虑在XOZ平面或者YOZ平面相交）；
交点为1时计算交点，根据交点的位置将x，y坐标代入框架坐标向量，求的Z值作为相交点的Z值；
返回值为2时情况暂不考虑。
View Code
1 // dist3D_Segment_to_Segment():
2 // Input: two 3D line segments S1 and S2
3 // Return: the shortest distance between S1 and S2
4 float
5 dist3D_Segment_to_Segment( Segment S1, Segment S2)
6 {
7 Vector u = S1.P1 - S1.P0;
8 Vector v = S2.P1 - S2.P0;
9 Vector w = S1.P0 - S2.P0;
10 float a = dot(u,u); // always >= 0
11 float b = dot(u,v);
12 float c = dot(v,v); // always >= 0
13 float d = dot(u,w);
14 float e = dot(v,w);
15 float D = a*c - b*b; // always >= 0
16 float sc, sN, sD = D; // sc = sN / sD, default sD = D >= 0
17 float tc, tN, tD = D; // tc = tN / tD, default tD = D >= 0
18
19 // compute the line parameters of the two closest points
20 if (D < SMALL_NUM) { // the lines are almost parallel
21 sN = 0.0; // force using point P0 on segment S1
22 sD = 1.0; // to prevent possible division by 0.0 later
23 tN = e;
24 tD = c;
25 }
26 else { // get the closest points on the infinite lines
27 sN = (b*e - c*d);
28 tN = (a*e - b*d);
29 if (sN < 0.0) { // sc < 0 => the s=0 edge is visible
30 sN = 0.0;
31 tN = e;
32 tD = c;
33 }
34 else if (sN > sD) { // sc > 1 => the s=1 edge is visible
35 sN = sD;
36 tN = e + b;
37 tD = c;
38 }
39 }
40
41 if (tN < 0.0) { // tc < 0 => the t=0 edge is visible
42 tN = 0.0;
43 // recompute sc for this edge
44 if (-d < 0.0)
45 sN = 0.0;
46 else if (-d > a)
47 sN = sD;
48 else {
49 sN = -d;
50 sD = a;
51 }
52 }
53 else if (tN > tD) { // tc > 1 => the t=1 edge is visible
54 tN = tD;
55 // recompute sc for this edge
56 if ((-d + b) < 0.0)
57 sN = 0;
58 else if ((-d + b) > a)
59 sN = sD;
60 else {
61 sN = (-d + b);
62 sD = a;
63 }
64 }
65 // finally do the division to get sc and tc
66 sc = (abs(sN) < SMALL_NUM ? 0.0 : sN / sD);
67 tc = (abs(tN) < SMALL_NUM ? 0.0 : tN / tD);
68
69 // get the difference of the two closest points
70 Vector dP = w + (sc * u) - (tc * v); // = S1(sc) - S2(tc)
71
72 return norm(dP); // return the closest distance
73 }
View Code
1 #define SMALL_NUM 0.00000001 // anything that avoids division overflow
2 // dot product (3D) which allows vector operations in arguments
3 #define dot(u,v) ((u).x * (v).x + (u).y * (v).y + (u).z * (v).z)
4 #define perp(u,v) ((u).x * (v).y - (u).y * (v).x) // perp product (2D)
5
6
7 // intersect2D_2Segments(): the intersection of 2 finite 2D segments
8 // Input: two finite segments S1 and S2
9 // Output: *I0 = intersect point (when it exists)
10 // *I1 = endpoint of intersect segment [I0,I1] (when it exists)
11 // Return: 0=disjoint (no intersect)
12 // 1=intersect in unique point I0
13 // 2=overlap in segment from I0 to I1
14 int
15 intersect2D_Segments( Segment S1, Segment S2, Point* I0, Point* I1 )
16 {
17 Vector u = S1.P1 - S1.P0;
18 Vector v = S2.P1 - S2.P0;
19 Vector w = S1.P0 - S2.P0;
20 float D = perp(u,v);
21
22 // test if they are parallel (includes either being a point)
23 if (fabs(D) < SMALL_NUM) { // S1 and S2 are parallel
24 if (perp(u,w) != 0 || perp(v,w) != 0) {
25 return 0; // they are NOT collinear
26 }
27 // they are collinear or degenerate
28 // check if they are degenerate points
29 float du = dot(u,u);
30 float dv = dot(v,v);
31 if (du==0 && dv==0) { // both segments are points
32 if (S1.P0 != S2.P0) // they are distinct points
33 return 0;
34 *I0 = S1.P0; // they are the same point
35 return 1;
36 }
37 if (du==0) { // S1 is a single point
38 if (inSegment(S1.P0, S2) == 0) // but is not in S2
39 return 0;
40 *I0 = S1.P0;
41 return 1;
42 }
43 if (dv==0) { // S2 a single point
44 if (inSegment(S2.P0, S1) == 0) // but is not in S1
45 return 0;
46 *I0 = S2.P0;
47 return 1;
48 }
49 // they are collinear segments - get overlap (or not)
50 float t0, t1; // endpoints of S1 in eqn for S2
51 Vector w2 = S1.P1 - S2.P0;
52 if (v.x != 0) {
53 t0 = w.x / v.x;
54 t1 = w2.x / v.x;
55 }
56 else {
57 t0 = w.y / v.y;
58 t1 = w2.y / v.y;
59 }
60 if (t0 > t1) { // must have t0 smaller than t1
61 float t=t0; t0=t1; t1=t; // swap if not
62 }
63 if (t0 > 1 || t1 < 0) {
64 return 0; // NO overlap
65 }
66 t0 = t0<0? 0 : t0; // clip to min 0
67 t1 = t1>1? 1 : t1; // clip to max 1
68 if (t0 == t1) { // intersect is a point
69 *I0 = S2.P0 + t0 * v;
70 return 1;
71 }
72
73 // they overlap in a valid subsegment
74 *I0 = S2.P0 + t0 * v;
75 *I1 = S2.P0 + t1 * v;
76 return 2;
77 }
78
79 // the segments are skew and may intersect in a point
80 // get the intersect parameter for S1
81 float sI = perp(v,w) / D;
82 if (sI < 0 || sI > 1) // no intersect with S1
83 return 0;
84
85 // get the intersect parameter for S2
86 float tI = perp(u,w) / D;
87 if (tI < 0 || tI > 1) // no intersect with S2
88 return 0;
89
90 *I0 = S1.P0 + sI * u; // compute S1 intersect point
91 return 1;
92 }
参考文献：http://softsurfer.com/algorithms.htm
http://geomalgorithms.com/a07-_distance.html
3535
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
