1// Copyright 2005 Google Inc. All Rights Reserved.
2
3#include <algorithm>
4using std::min;
5using std::max;
6using std::swap;
7using std::reverse;
8
9#include "s2edgeutil.h"
10
11#include "base/logging.h"
12
13bool S2EdgeUtil::SimpleCrossing(S2Point const& a, S2Point const& b,
14 S2Point const& c, S2Point const& d) {
15 // We compute SimpleCCW() for triangles ACB, CBD, BDA, and DAC. All
16 // of these triangles need to have the same orientation (CW or CCW)
17 // for an intersection to exist. Note that this is slightly more
18 // restrictive than the corresponding definition for planar edges,
19 // since we need to exclude pairs of line segments that would
20 // otherwise "intersect" by crossing two antipodal points.
21
22 S2Point ab = a.CrossProd(b);
23 double acb = -(ab.DotProd(c));
24 double bda = ab.DotProd(d);
25 if (acb * bda <= 0) return false;
26
27 S2Point cd = c.CrossProd(d);
28 double cbd = -(cd.DotProd(b));
29 double dac = cd.DotProd(a);
30 return (acb * cbd > 0) && (acb * dac > 0);
31}
32
33int S2EdgeUtil::RobustCrossing(S2Point const& a, S2Point const& b,
34 S2Point const& c, S2Point const& d) {
35 S2EdgeUtil::EdgeCrosser crosser(&a, &b, &c);
36 return crosser.RobustCrossing(&d);
37}
38
39bool S2EdgeUtil::VertexCrossing(S2Point const& a, S2Point const& b,
40 S2Point const& c, S2Point const& d) {
41 // If A == B or C == D there is no intersection. We need to check this
42 // case first in case 3 or more input points are identical.
43 if (a == b || c == d) return false;
44
45 // If any other pair of vertices is equal, there is a crossing if and only
46 // if OrderedCCW() indicates that the edge AB is further CCW around the
47 // shared vertex O (either A or B) than the edge CD, starting from an
48 // arbitrary fixed reference point.
49 if (a == d) return S2::OrderedCCW(S2::Ortho(a), c, b, a);
50 if (b == c) return S2::OrderedCCW(S2::Ortho(b), d, a, b);
51 if (a == c) return S2::OrderedCCW(S2::Ortho(a), d, b, a);
52 if (b == d) return S2::OrderedCCW(S2::Ortho(b), c, a, b);
53
54 LOG(DFATAL) << "VertexCrossing called with 4 distinct vertices";
55 return false;
56}
57
58bool S2EdgeUtil::EdgeOrVertexCrossing(S2Point const& a, S2Point const& b,
59 S2Point const& c, S2Point const& d) {
60 int crossing = RobustCrossing(a, b, c, d);
61 if (crossing < 0) return false;
62 if (crossing > 0) return true;
63 return VertexCrossing(a, b, c, d);
64}
65
66static void ReplaceIfCloser(S2Point const& x, S2Point const& y,
67 double *dmin2, S2Point* vmin) {
68 // If the squared distance from x to y is less than dmin2, then replace
69 // vmin by y and update dmin2 accordingly.
70
71 double d2 = (x - y).Norm2();
72 if (d2 < *dmin2 || (d2 == *dmin2 && y < *vmin)) {
73 *dmin2 = d2;
74 *vmin = y;
75 }
76}
77
78S2Point S2EdgeUtil::GetIntersection(S2Point const& a0, S2Point const& a1,
79 S2Point const& b0, S2Point const& b1) {
80 DCHECK_GT(RobustCrossing(a0, a1, b0, b1), 0);
81
82 // We use RobustCrossProd() to get accurate results even when two endpoints
83 // are close together, or when the two line segments are nearly parallel.
84
85 S2Point a_norm = S2::RobustCrossProd(a0, a1).Normalize();
86 S2Point b_norm = S2::RobustCrossProd(b0, b1).Normalize();
87 S2Point x = S2::RobustCrossProd(a_norm, b_norm).Normalize();
88
89 // Make sure the intersection point is on the correct side of the sphere.
90 // Since all vertices are unit length, and edges are less than 180 degrees,
91 // (a0 + a1) and (b0 + b1) both have positive dot product with the
92 // intersection point. We use the sum of all vertices to make sure that the
93 // result is unchanged when the edges are reversed or exchanged.
94
95 if (x.DotProd((a0 + a1) + (b0 + b1)) < 0) x = -x;
96
97 // The calculation above is sufficient to ensure that "x" is within
98 // kIntersectionTolerance of the great circles through (a0,a1) and (b0,b1).
99 // However, if these two great circles are very close to parallel, it is
100 // possible that "x" does not lie between the endpoints of the given line
101 // segments. In other words, "x" might be on the great circle through
102 // (a0,a1) but outside the range covered by (a0,a1). In this case we do
103 // additional clipping to ensure that it does.
104
105 if (S2::OrderedCCW(a0, x, a1, a_norm) && S2::OrderedCCW(b0, x, b1, b_norm))
106 return x;
107
108 // Find the acceptable endpoint closest to x and return it. An endpoint is
109 // acceptable if it lies between the endpoints of the other line segment.
110 double dmin2 = 10;
111 S2Point vmin = x;
112 if (S2::OrderedCCW(b0, a0, b1, b_norm)) ReplaceIfCloser(x, a0, &dmin2, &vmin);
113 if (S2::OrderedCCW(b0, a1, b1, b_norm)) ReplaceIfCloser(x, a1, &dmin2, &vmin);
114 if (S2::OrderedCCW(a0, b0, a1, a_norm)) ReplaceIfCloser(x, b0, &dmin2, &vmin);
115 if (S2::OrderedCCW(a0, b1, a1, a_norm)) ReplaceIfCloser(x, b1, &dmin2, &vmin);
116
117 DCHECK(S2::OrderedCCW(a0, vmin, a1, a_norm));
118 DCHECK(S2::OrderedCCW(b0, vmin, b1, b_norm));
119 return vmin;
120}
121
122// IEEE floating-point operations have a maximum error of 0.5 ULPS (units in
123// the last place). For double-precision numbers, this works out to 2**-53
124// (about 1.11e-16) times the magnitude of the result. It is possible to
125// analyze the calculation done by GetIntersection() and work out the
126// worst-case rounding error. I have done a rough version of this, and my
127// estimate is that the worst case distance from the intersection point X to
128// the great circle through (a0, a1) is about 12 ULPS, or about 1.3e-15.
129// This needs to be increased by a factor of (1/0.866) to account for the
130// edge_splice_fraction() in S2PolygonBuilder. Note that the maximum error
131// measured by the unittest in 1,000,000 trials is less than 3e-16.
132S1Angle const S2EdgeUtil::kIntersectionTolerance = S1Angle::Radians(1.5e-15);
133
134double S2EdgeUtil::GetDistanceFraction(S2Point const& x,
135 S2Point const& a0, S2Point const& a1) {
136 DCHECK_NE(a0, a1);
137 double d0 = x.Angle(a0);
138 double d1 = x.Angle(a1);
139 return d0 / (d0 + d1);
140}
141
142S2Point S2EdgeUtil::InterpolateAtDistance(S1Angle const& ax,
143 S2Point const& a, S2Point const& b,
144 S1Angle const& ab) {
145 DCHECK(S2::IsUnitLength(a));
146 DCHECK(S2::IsUnitLength(b));
147
148 // As of crosstool v14, gcc tries to calculate sin(ax_radians),
149 // cos(ax_radians), sin(ab_radians), cos(ab_radians) in the
150 // following section by two sincos() calls. However, for some
151 // inputs, sincos() returns significantly different values between
152 // AMD and Intel.
153 //
154 // As a temporary workaround, "volatile" is added to ax_radians and
155 // ab_radians, to prohibit the compiler to use such sincos() call,
156 // because sin() and cos() don't seem to have the problem. See
157 // b/3088321 for details.
158 volatile double ax_radians = ax.radians();
159 volatile double ab_radians = ab.radians();
160
161 // The result X is some linear combination X = e*A + f*B of the input
162 // points. The fractions "e" and "f" can be derived by looking at the
163 // components of this equation that are parallel and perpendicular to A.
164 // Let E = e*A and F = f*B. Then OEXF is a parallelogram. You can obtain
165 // the distance f = OF by considering the similar triangles produced by
166 // dropping perpendiculars from the segments OF and OB to OA.
167 double f = sin(ax_radians) / sin(ab_radians);
168
169 // Form the dot product of the first equation with A to obtain
170 // A.X = e*A.A + f*A.B. Since A, B, and X are all unit vectors,
171 // cos(ax) = e*1 + f*cos(ab), so
172 double e = cos(ax_radians) - f * cos(ab_radians);
173
174 // Mathematically speaking, if "a" and "b" are unit length then the result
175 // is unit length as well. But we normalize it anyway to prevent points
176 // from drifting away from unit length when multiple interpolations are done
177 // in succession (i.e. the result of one interpolation is fed into another).
178 return (e * a + f * b).Normalize();
179}
180
181S2Point S2EdgeUtil::InterpolateAtDistance(S1Angle const& ax,
182 S2Point const& a, S2Point const& b) {
183 return InterpolateAtDistance(ax, a, b, S1Angle(a, b));
184}
185
186S2Point S2EdgeUtil::Interpolate(double t, S2Point const& a, S2Point const& b) {
187 if (t == 0) return a;
188 if (t == 1) return b;
189 S1Angle ab(a, b);
190 return InterpolateAtDistance(t * ab, a, b, ab);
191}
192
193S1Angle S2EdgeUtil::GetDistance(S2Point const& x,
194 S2Point const& a, S2Point const& b,
195 S2Point const& a_cross_b) {
196 DCHECK(S2::IsUnitLength(a));
197 DCHECK(S2::IsUnitLength(b));
198 DCHECK(S2::IsUnitLength(x));
199
200 // There are three cases. If X is located in the spherical wedge defined by
201 // A, B, and the axis A x B, then the closest point is on the segment AB.
202 // Otherwise the closest point is either A or B; the dividing line between
203 // these two cases is the great circle passing through (A x B) and the
204 // midpoint of AB.
205
206 if (S2::SimpleCCW(a_cross_b, a, x) && S2::SimpleCCW(x, b, a_cross_b)) {
207 // The closest point to X lies on the segment AB. We compute the distance
208 // to the corresponding great circle. The result is accurate for small
209 // distances but not necessarily for large distances (approaching Pi/2).
210
211 DCHECK_NE(a, b); // Due to the guarantees of SimpleCCW().
212 double sin_dist = fabs(x.DotProd(a_cross_b)) / a_cross_b.Norm();
213 return S1Angle::Radians(asin(min(1.0, sin_dist)));
214 }
215 // Otherwise, the closest point is either A or B. The cheapest method is
216 // just to compute the minimum of the two linear (as opposed to spherical)
217 // distances and convert the result to an angle. Again, this method is
218 // accurate for small but not large distances (approaching Pi).
219
220 double linear_dist2 = min((x-a).Norm2(), (x-b).Norm2());
221 return S1Angle::Radians(2 * asin(min(1.0, 0.5 * sqrt(linear_dist2))));
222}
223
224S1Angle S2EdgeUtil::GetDistance(S2Point const& x,
225 S2Point const& a, S2Point const& b) {
226 return GetDistance(x, a, b, S2::RobustCrossProd(a, b));
227}
228
229S2Point S2EdgeUtil::GetClosestPoint(S2Point const& x,
230 S2Point const& a, S2Point const& b,
231 S2Point const& a_cross_b) {
232 DCHECK(S2::IsUnitLength(a));
233 DCHECK(S2::IsUnitLength(b));
234 DCHECK(S2::IsUnitLength(x));
235
236 // Find the closest point to X along the great circle through AB.
237 S2Point p = x - (x.DotProd(a_cross_b) / a_cross_b.Norm2()) * a_cross_b;
238
239 // If this point is on the edge AB, then it's the closest point.
240 if (S2::SimpleCCW(a_cross_b, a, p) && S2::SimpleCCW(p, b, a_cross_b))
241 return p.Normalize();
242
243 // Otherwise, the closest point is either A or B.
244 return ((x - a).Norm2() <= (x - b).Norm2()) ? a : b;
245}
246
247S2Point S2EdgeUtil::GetClosestPoint(S2Point const& x,
248 S2Point const& a, S2Point const& b) {
249 return GetClosestPoint(x, a, b, S2::RobustCrossProd(a, b));
250}
251
252bool S2EdgeUtil::IsEdgeBNearEdgeA(S2Point const& a0, S2Point const& a1,
253 S2Point const& b0, S2Point const& b1,
254 S1Angle const& tolerance) {
255 DCHECK_LT(tolerance.radians(), M_PI / 2);
256 DCHECK_GT(tolerance.radians(), 0);
257 // The point on edge B=b0b1 furthest from edge A=a0a1 is either b0, b1, or
258 // some interior point on B. If it is an interior point on B, then it must be
259 // one of the two points where the great circle containing B (circ(B)) is
260 // furthest from the great circle containing A (circ(A)). At these points,
261 // the distance between circ(B) and circ(A) is the angle between the planes
262 // containing them.
263
264 S2Point a_ortho = S2::RobustCrossProd(a0, a1).Normalize();
265 S2Point const a_nearest_b0 = GetClosestPoint(b0, a0, a1, a_ortho);
266 S2Point const a_nearest_b1 = GetClosestPoint(b1, a0, a1, a_ortho);
267 // If a_nearest_b0 and a_nearest_b1 have opposite orientation from a0 and a1,
268 // we invert a_ortho so that it points in the same direction as a_nearest_b0 x
269 // a_nearest_b1. This helps us handle the case where A and B are oppositely
270 // oriented but otherwise might be near each other. We check orientation and
271 // invert rather than computing a_nearest_b0 x a_nearest_b1 because those two
272 // points might be equal, and have an unhelpful cross product.
273 if (S2::RobustCCW(a_ortho, a_nearest_b0, a_nearest_b1) < 0)
274 a_ortho *= -1;
275
276 // To check if all points on B are within tolerance of A, we first check to
277 // see if the endpoints of B are near A. If they are not, B is not near A.
278 S1Angle const b0_distance(b0, a_nearest_b0);
279 S1Angle const b1_distance(b1, a_nearest_b1);
280 if (b0_distance > tolerance || b1_distance > tolerance)
281 return false;
282
283 // If b0 and b1 are both within tolerance of A, we check to see if the angle
284 // between the planes containing B and A is greater than tolerance. If it is
285 // not, no point on B can be further than tolerance from A (recall that we
286 // already know that b0 and b1 are close to A, and S2Edges are all shorter
287 // than 180 degrees). The angle between the planes containing circ(A) and
288 // circ(B) is the angle between their normal vectors.
289 S2Point const b_ortho = S2::RobustCrossProd(b0, b1).Normalize();
290 S1Angle const planar_angle(a_ortho, b_ortho);
291 if (planar_angle <= tolerance)
292 return true;
293
294
295 // As planar_angle approaches M_PI, the projection of a_ortho onto the plane
296 // of B approaches the null vector, and normalizing it is numerically
297 // unstable. This makes it unreliable or impossible to identify pairs of
298 // points where circ(A) is furthest from circ(B). At this point in the
299 // algorithm, this can only occur for two reasons:
300 //
301 // 1.) b0 and b1 are closest to A at distinct endpoints of A, in which case
302 // the opposite orientation of a_ortho and b_ortho means that A and B are
303 // in opposite hemispheres and hence not close to each other.
304 //
305 // 2.) b0 and b1 are closest to A at the same endpoint of A, in which case
306 // the orientation of a_ortho was chosen arbitrarily to be that of a0
307 // cross a1. B must be shorter than 2*tolerance and all points in B are
308 // close to one endpoint of A, and hence to A.
309 //
310 // The logic applies when planar_angle is robustly greater than M_PI/2, but
311 // may be more computationally expensive than the logic beyond, so we choose a
312 // value close to M_PI.
313 if (planar_angle >= S1Angle::Radians(M_PI - 0.01)) {
314 return (S1Angle(b0, a0) < S1Angle(b0, a1)) ==
315 (S1Angle(b1, a0) < S1Angle(b1, a1));
316 }
317
318 // Finally, if either of the two points on circ(B) where circ(B) is furthest
319 // from circ(A) lie on edge B, edge B is not near edge A.
320 //
321 // The normalized projection of a_ortho onto the plane of circ(B) is one of
322 // the two points along circ(B) where it is furthest from circ(A). The other
323 // is -1 times the normalized projection.
324 S2Point furthest = (a_ortho - a_ortho.DotProd(b_ortho) * b_ortho).Normalize();
325 DCHECK(S2::IsUnitLength(furthest));
326 S2Point furthest_inv = -1 * furthest;
327
328 // A point p lies on B if you can proceed from b_ortho to b0 to p to b1 and
329 // back to b_ortho without ever turning right. We test this for furthest and
330 // furthest_inv, and return true if neither point lies on B.
331 return !((S2::RobustCCW(b_ortho, b0, furthest) > 0 &&
332 S2::RobustCCW(furthest, b1, b_ortho) > 0) ||
333 (S2::RobustCCW(b_ortho, b0, furthest_inv) > 0 &&
334 S2::RobustCCW(furthest_inv, b1, b_ortho) > 0));
335}
336
337
338bool S2EdgeUtil::WedgeContains(S2Point const& a0, S2Point const& ab1,
339 S2Point const& a2, S2Point const& b0,
340 S2Point const& b2) {
341 // For A to contain B (where each loop interior is defined to be its left
342 // side), the CCW edge order around ab1 must be a2 b2 b0 a0. We split
343 // this test into two parts that test three vertices each.
344 return S2::OrderedCCW(a2, b2, b0, ab1) && S2::OrderedCCW(b0, a0, a2, ab1);
345}
346
347bool S2EdgeUtil::WedgeIntersects(S2Point const& a0, S2Point const& ab1,
348 S2Point const& a2, S2Point const& b0,
349 S2Point const& b2) {
350 // For A not to intersect B (where each loop interior is defined to be
351 // its left side), the CCW edge order around ab1 must be a0 b2 b0 a2.
352 // Note that it's important to write these conditions as negatives
353 // (!OrderedCCW(a,b,c,o) rather than Ordered(c,b,a,o)) to get correct
354 // results when two vertices are the same.
355 return !(S2::OrderedCCW(a0, b2, b0, ab1) && S2::OrderedCCW(b0, a2, a0, ab1));
356}
357
358S2EdgeUtil::WedgeRelation S2EdgeUtil::GetWedgeRelation(
359 S2Point const& a0, S2Point const& ab1, S2Point const& a2,
360 S2Point const& b0, S2Point const& b2) {
361 // There are 6 possible edge orderings at a shared vertex (all
362 // of these orderings are circular, i.e. abcd == bcda):
363 //
364 // (1) a2 b2 b0 a0: A contains B
365 // (2) a2 a0 b0 b2: B contains A
366 // (3) a2 a0 b2 b0: A and B are disjoint
367 // (4) a2 b0 a0 b2: A and B intersect in one wedge
368 // (5) a2 b2 a0 b0: A and B intersect in one wedge
369 // (6) a2 b0 b2 a0: A and B intersect in two wedges
370 //
371 // We do not distinguish between 4, 5, and 6.
372 // We pay extra attention when some of the edges overlap.When edges
373 // overlap, several of these orderings can be satisfied, and we take
374 // the most specific.
375 if (a0 == b0 && a2 == b2) return WEDGE_EQUALS;
376
377 if (S2::OrderedCCW(a0, a2, b2, ab1)) {
378 // The cases with this vertex ordering are 1, 5, and 6,
379 // although case 2 is also possible if a2 == b2.
380 if (S2::OrderedCCW(b2, b0, a0, ab1)) return WEDGE_PROPERLY_CONTAINS;
381
382 // We are in case 5 or 6, or case 2 if a2 == b2.
383 return (a2 == b2) ? WEDGE_IS_PROPERLY_CONTAINED : WEDGE_PROPERLY_OVERLAPS;
384 }
385
386 // We are in case 2, 3, or 4.
387 if (S2::OrderedCCW(a0, b0, b2, ab1)) return WEDGE_IS_PROPERLY_CONTAINED;
388 return S2::OrderedCCW(a0, b0, a2, ab1) ?
389 WEDGE_IS_DISJOINT : WEDGE_PROPERLY_OVERLAPS;
390}
391
392int S2EdgeUtil::EdgeCrosser::RobustCrossingInternal(S2Point const* d) {
393 // ACB and BDA have the appropriate orientations, so now we check the
394 // triangles CBD and DAC.
395 S2Point c_cross_d = c_->CrossProd(*d);
396 int cbd = -S2::RobustCCW(*c_, *d, *b_, c_cross_d);
397 if (cbd != acb_) return -1;
398
399 int dac = S2::RobustCCW(*c_, *d, *a_, c_cross_d);
400 return (dac == acb_) ? 1 : -1;
401}
402
403void S2EdgeUtil::RectBounder::AddPoint(S2Point const* b) {
404 DCHECK(S2::IsUnitLength(*b));
405 S2LatLng b_latlng(*b);
406 if (bound_.is_empty()) {
407 bound_.AddPoint(b_latlng);
408 } else {
409 // We can't just call bound_.AddPoint(b_latlng) here, since we need to
410 // ensure that all the longitudes between "a" and "b" are included.
411 bound_ = bound_.Union(S2LatLngRect::FromPointPair(a_latlng_, b_latlng));
412
413 // Check whether the min/max latitude occurs in the edge interior. We find
414 // the normal to the plane containing AB, and then a vector "dir" in this
415 // plane that also passes through the equator. We use RobustCrossProd to
416 // ensure that the edge normal is accurate even when the two points are very
417 // close together.
418 S2Point a_cross_b = S2::RobustCrossProd(*a_, *b);
419 S2Point dir = a_cross_b.CrossProd(S2Point(0, 0, 1));
420 double da = dir.DotProd(*a_);
421 double db = dir.DotProd(*b);
422 if (da * db < 0) {
423 // Minimum/maximum latitude occurs in the edge interior.
424 double abs_lat = acos(fabs(a_cross_b[2] / a_cross_b.Norm()));
425 if (da < 0) {
426 // It's possible that abs_lat < lat_.lo() due to numerical errors.
427 bound_.mutable_lat()->set_hi(max(abs_lat, bound_.lat().hi()));
428 } else {
429 bound_.mutable_lat()->set_lo(min(-abs_lat, bound_.lat().lo()));
430 }
431
432 // If the edge comes very close to the north or south pole then we may
433 // not be certain which side of the pole it is on. We handle this by
434 // expanding the longitude bounds if the maximum absolute latitude is
435 // approximately Pi/2.
436 if (abs_lat >= M_PI_2 - 1e-15) {
437 *bound_.mutable_lng() = S1Interval::Full();
438 }
439 }
440 }
441 a_ = b;
442 a_latlng_ = b_latlng;
443}
444
445S2EdgeUtil::LongitudePruner::LongitudePruner(S1Interval const& interval,
446 S2Point const& v0)
447 : interval_(interval), lng0_(S2LatLng::Longitude(v0).radians()) {
448}
449