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
186
187
188
189
190
191
192
193
194
195
196
197
198
|
package tim.prune.function.comparesegments;
import tim.prune.data.*;
import tim.prune.function.PointUtils;
import java.util.List;
/**
* Responsible for looping over a list of lines provided by SegmentData,
* and efficiently only looking forwards through the list rather than skipping back
* to previous lines.
*/
public class SegmentLooper
{
private final List<LineAndBearing> _lines;
private int _currentLine = 0;
private static final double BEARING_TOLERANCE_DEGREES = 20.0;
private static final double DISTANCE_TOLERANCE_RADIANS = Distance.convertDistanceToRadians(25.0, UnitSetLibrary.UNITS_METRES);
private static final double NO_MATCH_FOUND = -1.0;
public SegmentLooper(List<LineAndBearing> inLines) {
_lines = inLines;
}
/**
* Try to match the given point with any line beyond the current one
* @return a result object if an intersection could be found, otherwise null
*/
public IntersectionResult match(PointData inPoint)
{
for (int i=_currentLine; i<_lines.size(); i++)
{
final IntersectionResult result = matchFound(inPoint, _lines.get(i));
if (result != null)
{
_currentLine = i;
return result;
}
}
return null;
}
/**
* Try to match the given point with a single line
* @return a result object if an intersection could be found, otherwise null
*/
private IntersectionResult matchFound(PointData inPoint, LineAndBearing inLine)
{
// Check that two bearings are within a tolerance of BEARING_TOLERANCE_DEGREES
final double bearingDifference = Bearing.angleDifferenceDegrees(inPoint._bearing, inLine._bearing);
if (bearingDifference >= BEARING_TOLERANCE_DEGREES) {
return null;
}
// Project point at right angles to bearing
DataPoint pointRight = PointUtils.projectPoint(inPoint._point, Math.toRadians(inPoint._bearing + 90.0), DISTANCE_TOLERANCE_RADIANS);
DataPoint pointLeft = PointUtils.projectPoint(inPoint._point, Math.toRadians(inPoint._bearing - 90.0), DISTANCE_TOLERANCE_RADIANS);
DataPoint lineFromPoint = inLine.getFromPoint();
DataPoint lineToPoint = inLine.getToPoint();
if (!overlap(pointLeft.getLongitude(), pointRight.getLongitude(), lineFromPoint.getLongitude(), lineToPoint.getLongitude())
|| !overlap(pointLeft.getLatitude(), pointRight.getLatitude(), lineFromPoint.getLatitude(), lineToPoint.getLatitude()))
{
// There is no overlap in latitude or longitude, so there's no overlap
return null;
}
// Now we need to do the actual intersection
final double lineFraction = calculateLineFraction(pointLeft, pointRight, lineFromPoint, lineToPoint);
if (lineFraction < 0.0 || lineFraction > 1.0) {
return null;
}
long totalMillis = inLine.getMilliseconds();
Timestamp timestamp = lineFromPoint.getTimestamp().addOffsetMilliseconds((long) (totalMillis * lineFraction));
double lineDistRadians = inLine._distToFromPointRadians + lineFraction * inLine._distAlongRadians;
final double speed1 = inPoint._speedRadiansPerSec;
final double speed2 = inLine.getInterpolatedSpeed(lineFraction);
// we'll interpolate the cut point too to be able to show it if we need to
DataPoint calculatedPoint = PointUtils.interpolate(lineFromPoint, lineToPoint, lineFraction);
return new IntersectionResult(inPoint._point, calculatedPoint,
inPoint._point.getTimestamp(), timestamp,
inPoint._distanceToHereRadians, lineDistRadians,
speed1, speed2);
}
/**
* Calculate the intersection between two straight lines on a plane
* @param pointLeft start point of perpendicular
* @param pointRight end point of perpendicular
* @param fromPoint start point of line segment to be cut
* @param toPoint end point of line segment to be cut
* @return fractional distance along line segment at which the intersection occurs (from 0 to 1) or -1 if not possible
*/
public static double calculateLineFraction(DataPoint pointLeft, DataPoint pointRight, DataPoint fromPoint, DataPoint toPoint)
{
// Coordinates of perpendicular cutting line
final double x1 = pointLeft.getLongitude().getDouble();
final double x2 = pointRight.getLongitude().getDouble();
final double y1 = pointLeft.getLatitude().getDouble();
final double y2 = pointRight.getLatitude().getDouble();
// Coordinates of segment line
final double u1 = fromPoint.getLongitude().getDouble();
final double u2 = toPoint.getLongitude().getDouble();
final double v1 = fromPoint.getLatitude().getDouble();
final double v2 = toPoint.getLatitude().getDouble();
// Cutting line is defined by:
// x = x1 + s (x2 - x1) where 0 <= s <= 1
// and y = y1 + s (y2 - y1)
// Segment line is similarly defined by:
// u = u1 + t (u2 - u1) where 0 <= t <= 1
// and v = v1 + t (v2 - v1)
// We want to find the intersection point x=u, y=v
// where both s and t are between 0 and 1.
// Then we will return the fraction t if it's valid, or -1 if not
// Special case: x2 == x1, which means original line is horizontal (E/W) and perpendicular is exactly vertical (N/S)
if (isNearlyZero(x2 - x1))
{
// Then: x = x1 = u1 + t (u2 - u1)
// t = (x1 - u1) / (u2 - u1)
if (isNearlyZero(u2 - u1))
{
// u2 == u1 means that segment line is also vertical, but this should have been excluded earlier
// due to the angle tolerance check
return NO_MATCH_FOUND;
}
return checkWithinRange((x1 - u1) / (u2 - u1));
}
// Special case: y2 == y1, which means original line is vertical (N/S) and perpendicular is exactly horizontal (E/W)
if (isNearlyZero(y2 - y1))
{
// Then: y = y1 = v1 + t (v2 - v1)
// t = (y1 - v1) / (v2 - v1)
if (isNearlyZero(v2 - v1))
{
// v2 == v1 means that segment line is also horizontal, but this should have been excluded earlier
// due to the angle tolerance check
return NO_MATCH_FOUND;
}
return checkWithinRange((y1 - v1) / (v2 - v1));
}
// More general case: cutting line is neither horizontal nor vertical, but at some other angle
// x1 + s (x2 - x1) = u1 + t (u2 - u1)
// and y1 + s (y2 - y1) = v1 + t (v2 - v1)
// so s = (u1 - x1 + t (u2 - u1)) / (x2 - x1)
// it's known that (x2 - x1) != 0
final double alpha = (u1 - x1) / (x2 - x1);
final double beta = (u2 - u1) / (x2 - x1);
// therefore s = alpha + t * beta
// y1 + (alpha + t * beta)(y2 - y1) = v1 + t (v2 - v1)
// t * (beta*(y2 - y1) - (v2 - v1)) = v1 - y1 - alpha * (y2 - y1)
final double gamma = beta * (y2 - y1) + v1 - v2;
if (isNearlyZero(gamma)) {
return NO_MATCH_FOUND; // lines are parallel, this shouldn't happen because of angle tolerance check
}
// now we have a value for t
final double t = (v1 - y1 - alpha * (y2 - y1)) / gamma;
final double s = alpha + t * beta;
if (checkWithinRange(s) >= 0.0) {
return checkWithinRange(t);
}
return NO_MATCH_FOUND;
}
/** @return true if the given number is close to zero */
private static boolean isNearlyZero(double gamma) {
return Math.abs(gamma) < 1e-10;
}
/** @return validated value, or NO_MATCH_FOUND if out of range */
private static double checkWithinRange(double inValue)
{
if (inValue < 0.0 || inValue > 1.0) {
return NO_MATCH_FOUND;
}
return inValue;
}
/**
* @return true if there is any overlap between the lat/long ranges (1 to 2) and (3 to 4)
*/
private static boolean overlap(Coordinate inCoord1, Coordinate inCoord2, Coordinate inCoord3, Coordinate inCoord4)
{
if (Math.abs(inCoord1.getDouble() - inCoord2.getDouble()) > 1.0
|| Math.abs(inCoord3.getDouble() - inCoord4.getDouble()) > 1.0)
{
// TODO: Problems here by the date line if projected points are on either side or if line segment crosses over
return false;
}
final double min1 = Math.min(inCoord1.getDouble(), inCoord2.getDouble());
final double max1 = Math.max(inCoord1.getDouble(), inCoord2.getDouble());
final double min2 = Math.min(inCoord3.getDouble(), inCoord4.getDouble());
final double max2 = Math.max(inCoord3.getDouble(), inCoord4.getDouble());
return max2 >= min1 && max1 >= min2;
}
}
|
