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
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
|
/*************************************************************************
* Copyright (c) 2011 AT&T Intellectual Property
* All rights reserved. This program and the accompanying materials
* are made available under the terms of the Eclipse Public License v1.0
* which accompanies this distribution, and is available at
* https://www.eclipse.org/legal/epl-v10.html
*
* Contributors: Details at https://graphviz.org
*************************************************************************/
#include "config.h"
#include <edgepaint/intersection.h>
#include <sparse/general.h>
#include <math.h>
static double cross(double *u, double *v){
return u[0]*v[1] - u[1]*v[0];
}
/*
There's a nice approach to this problem that uses vector cross
products. Define the 2-dimensional vector cross product v * w to be
vxwy \[Minus] vywx (this is the magnitude of the 3-dimensional cross
product).
Suppose the two line segments run from p to p + r and from q to q +s.
Then any point on the first line is representable as p + t r (for a
scalar parameter t) and any point on the second line as q + u s (for a
scalar parameter u).
The two lines intersect if we can find t and u such that:
p + t r = q + u s
cross both sides with s, getting
(p + t r) * s = (q + u s) * s
And since s * s = 0, this means
t(r * s) = (q \[Minus] p) * s
And therefore, solving for t:
t = (q \[Minus] p) * s / (r * s)
In the same way, we can solve for u:
u = (q \[Minus] p) * r / (r * s)
Now if r * s = 0 then the two lines are parallel.
(There are two cases: if (q \[Minus] p) * r = 0 too,
then the lines are collinear, otherwise they never intersect.)
Otherwise the intersection point is on the original pair
of line segments if 0 <= t <= 1 and 0 <= u <= 1.
*/
static double dist(int dim, double *x, double *y){
int k;
double d = 0;
for (k = 0; k < dim; k++) d += (x[k] - y[k])*(x[k]-y[k]);
return sqrt(d);
}
static double point_line_distance(double *p, double *q, double *r){
/* distance between point p and line q--r */
enum {dim = 2};
double t = 0, b = 0;
int i;
double tmp;
/* t = ((p - q).(r - q))/((r - q).(r - q)) gives the position of the project of p on line r--q */
for (i = 0; i < dim; i++){
t += (p[i] - q[i])*(r[i] - q[i]);
b += (r[i] - q[i])*(r[i] - q[i]);
}
if (b <= MACHINEACC) return dist(dim, p, q);
t = t/b;
/* pointLine = Norm[p - (q + t (r - q))];
If [t >= 0 && t <= 1, pointLine, Min[{Norm[p - q], Norm[p - r]}]]];
*/
if (t >= 0 && t <= 1){
b = 0;
for (i = 0; i < dim; i++){
tmp = p[i] - (q[i] + t*(r[i] - q[i]));
b += tmp*tmp;
}
return sqrt(b);
}
t = dist(dim, p, q);
b = dist(dim, p, r);
return MIN(t, b);
}
static double line_segments_distance(double *p1, double *p2, double *q1, double *q2){
/* distance between line segments p1--p2 and q1--q2 */
double t1, t2, t3, t4;
t1 = point_line_distance(p1, q1, q2);
t2 = point_line_distance(p2, q1, q2);
t3 = point_line_distance(q1, p1, p2);
t4 = point_line_distance(q2, p1, p2);
t1 = MIN(t1,t2);
t3 = MIN(t3,t4);
return MIN(t1, t3);
}
double intersection_angle(double *p1, double *p2, double *q1, double *q2){
/* give two lines p1--p2 and q1--q2, find their intersection angle
and return Abs[Cos[theta]] of that angle.
- If the two lines are very close, treat as if they intersect.
- If they do not intersect or being very close, return -2.
- If the return value is close to 1, the two lines intersects and is close to an angle of 0 o Pi;
. lines that intersect at close to 90 degree give return value close to 0
- in the special case of two lines sharing an end point, we return Cos[theta], instead of
. the absolute value, where theta
. is the angle of the two rays emitting from the shared end point, thus the value can be
. from -1 to 1.
*/
enum {dim = 2};
double r[dim], s[dim], qp[dim];
double rnorm = 0, snorm = 0, b, t, u;
// double epsilon = sqrt(MACHINEACC), close = 0.01;
//this may be better. Apply to ngk10_4 and look at double edge between 28 and 43. double epsilon = sin(10/180.), close = 0.1;
double epsilon = sin(1/180.), close = 0.01;
int line_dist_close;
int i;
double res;
for (i = 0; i < dim; i++) {
r[i] = p2[i] - p1[i];
rnorm += r[i]*r[i];
}
rnorm = sqrt(rnorm);
for (i = 0; i < dim; i++) {
s[i] = q2[i] - q1[i];
snorm += s[i]*s[i];
}
snorm = sqrt(snorm);
b = cross(r, s);
line_dist_close = (line_segments_distance(p1, p2, q1, q2) <= close*MAX(rnorm, snorm));
if (fabs(b) <= epsilon*snorm*rnorm){/* parallel */
if (line_dist_close) {/* two parallel lines that are close */
return 1;
}
return -2;/* parallel but not close */
}
for (i = 0; i < dim; i++) qp[i] = q1[i] - p1[i];
t = cross(qp, s)/b;
u = cross(qp, r)/b;
if ((t >= 0 && t <= 1 && u >= 0 && u <= 1) /* they intersect */
|| line_dist_close){/* or lines are close */
double rs = 0;
if (rnorm*snorm < MACHINEACC) return 0;
for (i = 0; i < dim; i++){
rs += r[i]*s[i];
}
res = rs/(rnorm*snorm);
/* if the two lines share an end point */
if (p1[0] == q1[0] && p1[1] == q1[1]){
return res;
} else if (p1[0] == q2[0] && p1[1] == q2[1]){
return -res;
} else if (p2[0] == q1[0] && p2[1] == q1[1]){
return -res;
} else if (p2[0] == q2[0] && p2[1] == q2[1]){
return res;
}
/* normal case of intersect or very close */
return fabs(res);
}
return -2;/* no intersection, and lines are not even close */
}
|
