-
Notifications
You must be signed in to change notification settings - Fork 5
/
lineSegmentIntersect.m
150 lines (126 loc) · 5.1 KB
/
lineSegmentIntersect.m
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
function out = lineSegmentIntersect(XY1,XY2)
%LINESEGMENTINTERSECT Intersections of line segments.
% OUT = LINESEGMENTINTERSECT(XY1,XY2) finds the 2D Cartesian Coordinates of
% intersection points between the set of line segments given in XY1 and XY2.
%
% XY1 and XY2 are N1x4 and N2x4 matrices. Rows correspond to line segments.
% Each row is of the form [x1 y1 x2 y2] where (x1,y1) is the start point and
% (x2,y2) is the end point of a line segment:
%
% Line Segment
% o--------------------------------o
% ^ ^
% (x1,y1) (x2,y2)
%
% OUT is a structure with fields:
%
% 'intAdjacencyMatrix' : N1xN2 indicator matrix where the entry (i,j) is 1 if
% line segments XY1(i,:) and XY2(j,:) intersect.
%
% 'intMatrixXY' : matrix where the entry (1,:) is the X coordinate of the
% intersection point between line segments XY1 and XY2;entry (2,:) is
% the Y coordinate of the intersection point between line segments XY1
% and XY2.
% 'intMatrixX' : N1xN2 matrix where the entry (i,j) is the X coordinate of the
% intersection point between line segments XY1(i,:) and XY2(j,:).
%
% 'intMatrixY' : N1xN2 matrix where the entry (i,j) is the Y coordinate of the
% intersection point between line segments XY1(i,:) and XY2(j,:).
%
% 'intNormalizedDistance1To2' : N1xN2 matrix where the (i,j) entry is the
% normalized distance from the start point of line segment XY1(i,:) to the
% intersection point with XY2(j,:).
%
% 'intNormalizedDistance2To1' : N1xN2 matrix where the (i,j) entry is the
% normalized distance from the start point of line segment XY1(j,:) to the
% intersection point with XY2(i,:).
%
% 'parAdjacencyMatrix' : N1xN2 indicator matrix where the (i,j) entry is 1 if
% line segments XY1(i,:) and XY2(j,:) are parallel.
%
% 'coincAdjacencyMatrix' : N1xN2 indicator matrix where the (i,j) entry is 1
% if line segments XY1(i,:) and XY2(j,:) are coincident.
% Version: 1.00, April 03, 2010
% Version: 1.10, April 10, 2010
% Author: U. Murat Erdem
% CHANGELOG:
%
% Ver. 1.00:
% -Initial release.
%
% Ver. 1.10:
% - Changed the input parameters. Now the function accepts two sets of line
% segments. The intersection analysis is done between these sets and not in
% the same set.
% - Changed and added fields of the output. Now the analysis provides more
% information about the intersections and line segments.
% - Performance tweaks.
% I opted not to call this 'curve intersect' because it would be misleading
% unless you accept that curves are pairwise linear constructs.
% I tried to put emphasis on speed by vectorizing the code as much as possible.
% There should still be enough room to optimize the code but I left those out
% for the sake of clarity.
% The math behind is given in:
% http://local.wasp.uwa.edu.au/~pbourke/geometry/lineline2d/
% If you really are interested in squeezing as much horse power as possible out
% of this code I would advise to remove the argument checks and tweak the
% creation of the OUT a little bit.
%%% Argument check.
%-------------------------------------------------------------------------------
validateattributes(XY1,{'numeric'},{'2d','finite'});
validateattributes(XY2,{'numeric'},{'2d','finite'});
[n_rows_1,n_cols_1] = size(XY1);
[n_rows_2,n_cols_2] = size(XY2);
if n_cols_1 ~= 4 || n_cols_2 ~= 4
error('Arguments must be a Nx4 matrices.');
end
%%% Prepare matrices for vectorized computation of line intersection points.
%-------------------------------------------------------------------------------
X1 = repmat(XY1(:,1),1,n_rows_2);
X2 = repmat(XY1(:,3),1,n_rows_2);
Y1 = repmat(XY1(:,2),1,n_rows_2);
Y2 = repmat(XY1(:,4),1,n_rows_2);
XY2 = XY2';
X3 = repmat(XY2(1,:),n_rows_1,1);
X4 = repmat(XY2(3,:),n_rows_1,1);
Y3 = repmat(XY2(2,:),n_rows_1,1);
Y4 = repmat(XY2(4,:),n_rows_1,1);
X4_X3 = (X4-X3);
Y1_Y3 = (Y1-Y3);
Y4_Y3 = (Y4-Y3);
X1_X3 = (X1-X3);
X2_X1 = (X2-X1);
Y2_Y1 = (Y2-Y1);
numerator_a = X4_X3 .* Y1_Y3 - Y4_Y3 .* X1_X3;
numerator_b = X2_X1 .* Y1_Y3 - Y2_Y1 .* X1_X3;
denominator = Y4_Y3 .* X2_X1 - X4_X3 .* Y2_Y1;
u_a = numerator_a ./ denominator;
u_b = numerator_b ./ denominator;
% Find the adjacency matrix A of intersecting lines.
INT_X = X1+X2_X1.*u_a;
INT_Y = Y1+Y2_Y1.*u_a;
% INT_X=round(INT_X,6);
% INT_Y=round(INT_Y,6);
INT_B = (u_a >= 0) & (u_a <= 1) & (u_b >= 0) & (u_b <= 1);
PAR_B = denominator == 0;
COINC_B = (numerator_a == 0 & numerator_b == 0 & PAR_B);
%Reset the intMatrixXY;
intMatrixXY=zeros(2,sum(sum(INT_B,2)));
A= 1./INT_B; %È¥³ý£¨0£¬0£©µã¸ÉÈÅ
A(isinf(A))=NaN;
B=INT_X.*A;
C=INT_Y.*A;
D=B(isnan(B)==0);
E=C(isnan(C)==0);
intMatrixXY(1,:)=D;
intMatrixXY(2,:)=E;
% Arrange output.
out.intAdjacencyMatrix = INT_B;
out.intMatrixXY=intMatrixXY;
out.intMatrixX = INT_X .* INT_B;
out.intMatrixY = INT_Y .* INT_B;
out.intNormalizedDistance1To2 = u_a;
out.intNormalizedDistance2To1 = u_b;
out.parAdjacencyMatrix = PAR_B;
out.coincAdjacencyMatrix= COINC_B;
end