-
Notifications
You must be signed in to change notification settings - Fork 293
/
s2edge_crossings.cc
784 lines (710 loc) · 34.4 KB
/
s2edge_crossings.cc
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
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310
311
312
313
314
315
316
317
318
319
320
321
322
323
324
325
326
327
328
329
330
331
332
333
334
335
336
337
338
339
340
341
342
343
344
345
346
347
348
349
350
351
352
353
354
355
356
357
358
359
360
361
362
363
364
365
366
367
368
369
370
371
372
373
374
375
376
377
378
379
380
381
382
383
384
385
386
387
388
389
390
391
392
393
394
395
396
397
398
399
400
401
402
403
404
405
406
407
408
409
410
411
412
413
414
415
416
417
418
419
420
421
422
423
424
425
426
427
428
429
430
431
432
433
434
435
436
437
438
439
440
441
442
443
444
445
446
447
448
449
450
451
452
453
454
455
456
457
458
459
460
461
462
463
464
465
466
467
468
469
470
471
472
473
474
475
476
477
478
479
480
481
482
483
484
485
486
487
488
489
490
491
492
493
494
495
496
497
498
499
500
501
502
503
504
505
506
507
508
509
510
511
512
513
514
515
516
517
518
519
520
521
522
523
524
525
526
527
528
529
530
531
532
533
534
535
536
537
538
539
540
541
542
543
544
545
546
547
548
549
550
551
552
553
554
555
556
557
558
559
560
561
562
563
564
565
566
567
568
569
570
571
572
573
574
575
576
577
578
579
580
581
582
583
584
585
586
587
588
589
590
591
592
593
594
595
596
597
598
599
600
601
602
603
604
605
606
607
608
609
610
611
612
613
614
615
616
617
618
619
620
621
622
623
624
625
626
627
628
629
630
631
632
633
634
635
636
637
638
639
640
641
642
643
644
645
646
647
648
649
650
651
652
653
654
655
656
657
658
659
660
661
662
663
664
665
666
667
668
669
670
671
672
673
674
675
676
677
678
679
680
681
682
683
684
685
686
687
688
689
690
691
692
693
694
695
696
697
698
699
700
701
702
703
704
705
706
707
708
709
710
711
712
713
714
715
716
717
718
719
720
721
722
723
724
725
726
727
728
729
730
731
732
733
734
735
736
737
738
739
740
741
742
743
744
745
746
747
748
749
750
751
752
753
754
755
756
757
758
759
760
761
762
763
764
765
766
767
768
769
770
771
772
773
774
775
776
777
778
779
780
781
782
783
784
// Copyright 2005 Google Inc. All Rights Reserved.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS-IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
//
// Author: ericv@google.com (Eric Veach)
#include "s2/s2edge_crossings.h"
#include <cmath>
#include <cmath>
#include "s2/base/logging.h"
#include "s2/s1angle.h"
#include "s2/s2edge_crosser.h"
#include "s2/s2edge_crossings_internal.h"
#include "s2/s2pointutil.h"
#include "s2/s2predicates.h"
#include "s2/util/math/exactfloat/exactfloat.h"
namespace S2 {
using internal::GetIntersectionExact;
using internal::IntersectionMethod;
using internal::intersection_method_tally_;
using S2::internal::GetStableCrossProd;
using s2pred::DBL_ERR;
using s2pred::ToLD;
using s2pred::ToExact;
using s2pred::kHasLongDouble;
using s2pred::kSqrt3;
using s2pred::rounding_epsilon;
using std::fabs;
using std::max;
using std::sqrt;
using Vector3_ld = s2pred::Vector3_ld;
using Vector3_xf = s2pred::Vector3_xf;
// kRobustCrossProdError can be set somewhat arbitrarily because the algorithm
// uses more precision as needed in order to achieve the specified error. The
// only strict requirement is that kRobustCrossProdError >= DBL_ERR, since
// this is the minimum error even when using exact arithmetic. We set the
// error somewhat larger than this so that virtually all cases can be handled
// using ordinary double-precision arithmetic.
static_assert(kRobustCrossProdError.radians() == 6 * DBL_ERR, "update comment");
// kIntersectionError can also be set somewhat arbitrarily (see above) except
// that in this case the error using exact arithmetic is up to 2 * DBL_ERR,
// and the error limit is set to 8 * DBL_ERR so that virtually all cases can
// be handled using ordinary double-precision arithmetic.
static_assert(kIntersectionError.radians() == 8 * DBL_ERR, "update comment");
namespace internal {
const S1Angle kExactCrossProdError = S1Angle::Radians(DBL_ERR);
const S1Angle kIntersectionExactError = S1Angle::Radians(2 * DBL_ERR);
int* intersection_method_tally_ = nullptr;
const char* GetIntersectionMethodName(IntersectionMethod method) {
switch (method) {
case IntersectionMethod::SIMPLE: return "Simple";
case IntersectionMethod::SIMPLE_LD: return "Simple_ld";
case IntersectionMethod::STABLE: return "Stable";
case IntersectionMethod::STABLE_LD: return "Stable_ld";
case IntersectionMethod::EXACT: return "Exact";
default: return "Unknown";
}
}
// Evaluates the cross product of unit-length vectors "a" and "b" in a
// numerically stable way, returning true if the error in the result is
// guaranteed to be at most kRobustCrossProdError.
template <class T>
inline bool GetStableCrossProd(const Vector3<T>& a, const Vector3<T>& b,
Vector3<T>* result) {
// We compute the cross product (a - b) x (a + b). Mathematically this is
// exactly twice the cross product of "a" and "b", but it has the numerical
// advantage that (a - b) and (a + b) are nearly perpendicular (since "a" and
// "b" are unit length). This yields a result that is nearly orthogonal to
// both "a" and "b" even if these two values differ only very slightly.
//
// The maximum directional error in radians when this calculation is done in
// precision T (where T is a floating-point type) is:
//
// (1 + 2 * sqrt(3) + 32 * sqrt(3) * DBL_ERR / ||N||) * T_ERR
//
// where ||N|| is the norm of the result. To keep this error to at most
// kRobustCrossProdError, assuming this value is much less than 1, we need
//
// (1 + 2 * sqrt(3) + 32 * sqrt(3) * DBL_ERR / ||N||) * T_ERR <= kErr
//
// ||N|| >= 32 * sqrt(3) * DBL_ERR / (kErr / T_ERR - (1 + 2 * sqrt(3)))
//
// From this you can see that in order for this calculation to ever succeed in
// double precision, we must have kErr > (1 + 2 * sqrt(3)) * DBL_ERR, which is
// about 4.46 * DBL_ERR. We actually set kRobustCrossProdError == 6 * DBL_ERR
// (== 3 * DBL_EPSILON) in order to minimize the number of cases where higher
// precision is needed; in particular, higher precision is only necessary when
// "a" and "b" are closer than about 18 * DBL_ERR == 9 * DBL_EPSILON.
// (80-bit precision can handle inputs as close as 2.5 * LDBL_EPSILON.)
constexpr T T_ERR = rounding_epsilon<T>();
constexpr T kMinNorm =
(32 * kSqrt3 * DBL_ERR) /
(kRobustCrossProdError.radians() / T_ERR - (1 + 2 * kSqrt3));
*result = (a - b).CrossProd(a + b);
return result->Norm2() >= kMinNorm * kMinNorm;
}
// Explicitly instantiate this function so that we can use it in tests without
// putting its definition in a header file.
template bool GetStableCrossProd<double>(
const S2Point&, const S2Point&, S2Point*);
template bool GetStableCrossProd<long double>(
const Vector3_ld&, const Vector3_ld&, Vector3_ld*);
} // namespace internal
Vector3_d RobustCrossProd(const S2Point& a, const S2Point& b) {
S2_DCHECK(IsUnitLength(a));
S2_DCHECK(IsUnitLength(b));
// The direction of a.CrossProd(b) becomes unstable as (a + b) or (a - b)
// approaches zero. This leads to situations where a.CrossProd(b) is not
// very orthogonal to "a" and/or "b". To solve this problem robustly requires
// falling back to extended precision, arbitrary precision, and even symbolic
// perturbations to handle the case when "a" and "b" are exactly
// proportional, e.g. a == -b (see s2predicates.cc for details).
Vector3_d result;
if (GetStableCrossProd(a, b, &result)) {
return result;
}
// Handle the (a == b) case now, before doing expensive arithmetic. The only
// result that makes sense mathematically is to return zero, but it turns out
// to reduce the number of special cases in client code if we instead return
// an arbitrary orthogonal vector.
if (a == b) {
return Ortho(a);
}
// Next we try using "long double" precision (if available).
Vector3_ld result_ld;
if (kHasLongDouble && GetStableCrossProd(ToLD(a), ToLD(b), &result_ld)) {
return Vector3_d::Cast(result_ld);
}
// Otherwise we fall back to exact arithmetic, then symbolic perturbations.
return internal::ExactCrossProd(a, b);
}
// Returns the cross product of "a" and "b" after symbolic perturbations.
// (These perturbations only affect the result if "a" and "b" are exactly
// collinear, e.g. if a == -b or a == (1+eps) * b.) The result may not be
// normalizable (i.e., EnsureNormalizable() should be called on the result).
static Vector3_d SymbolicCrossProdSorted(const S2Point& a, const S2Point& b) {
S2_DCHECK(a < b);
S2_DCHECK(s2pred::IsZero(ToExact(a).CrossProd(ToExact(b))));
// The following code uses the same symbolic perturbation model as S2::Sign.
// The particular sequence of tests below was obtained using Mathematica
// (although it would be easy to do it by hand for this simple case).
//
// Just like the function SymbolicallyPerturbedSign() in s2predicates.cc,
// every input coordinate x[i] is assigned a symbolic perturbation dx[i]. We
// then compute the cross product
//
// (a + da).CrossProd(b + db) .
//
// The result is a polynomial in the perturbation symbols. For example if we
// did this in one dimension, the result would be
//
// a * b + b * da + a * db + da * db
//
// where "a" and "b" have numerical values and "da" and "db" are symbols.
// In 3 dimensions the result is similar except that the coefficients are
// 3-vectors rather than scalars.
//
// Every possible S2Point has its own symbolic perturbation in each coordinate
// (i.e., there are about 3 * 2**192 symbols). The magnitudes of the
// perturbations are chosen such that if x < y lexicographically, the
// perturbations for "y" are much smaller than the perturbations for "x".
// Similarly, the perturbations for the coordinates of a given point x are
// chosen such that dx[0] is much smaller than dx[1] which is much smaller
// than dx[2]. Putting this together with fact the inputs to this function
// have been sorted so that a < b lexicographically, this tells us that
//
// da[2] > da[1] > da[0] > db[2] > db[1] > db[0]
//
// where each perturbation is so much smaller than the previous one that we
// don't even need to consider it unless the coefficients of all previous
// perturbations are zero. In fact, each succeeding perturbation is so small
// that we don't need to consider it unless the coefficient of all products of
// the previous perturbations are zero. For example, we don't need to
// consider the coefficient of db[1] unless the coefficient of db[2]*da[0] is
// zero.
//
// The follow code simply enumerates the coefficients of the perturbations
// (and products of perturbations) that appear in the cross product above, in
// order of decreasing perturbation magnitude. The first non-zero
// coefficient determines the result. The easiest way to enumerate the
// coefficients in the correct order is to pretend that each perturbation is
// some tiny value "eps" raised to a power of two:
//
// eps** 1 2 4 8 16 32
// da[2] da[1] da[0] db[2] db[1] db[0]
//
// Essentially we can then just count in binary and test the corresponding
// subset of perturbations at each step. So for example, we must test the
// coefficient of db[2]*da[0] before db[1] because eps**12 > eps**16.
if (b[0] != 0 || b[1] != 0) { // da[2]
return Vector3_d(-b[1], b[0], 0);
}
if (b[2] != 0) { // da[1]
return Vector3_d(b[2], 0, 0); // Note that b[0] == 0.
}
// None of the remaining cases can occur in practice, because we can only get
// to this point if b = (0, 0, 0). Nevertheless, even (0, 0, 0) has a
// well-defined direction under the symbolic perturbation model.
S2_DCHECK(b[1] == 0 && b[2] == 0); // da[0] coefficients (always zero)
if (a[0] != 0 || a[1] != 0) { // db[2]
return Vector3_d(a[1], -a[0], 0);
}
// The following coefficient is always non-zero, so we can stop here.
//
// It may seem strange that we are returning (1, 0, 0) as the cross product
// without even looking at the sign of a[2]. (Wouldn't you expect
// (0, 0, -1) x (0, 0, 0) and (0, 0, 1) x (0, 0, 0) to point in opposite
// directions?) It's worth pointing out that in this function there is *no
// relationship whatsoever* between the vectors "a" and "-a", because the
// perturbations applied to these vectors may be entirely different. This is
// why the identity "RobustCrossProd(-a, b) == -RobustCrossProd(a, b)" does
// not hold whenever "a" and "b" are linearly dependent (i.e., proportional).
// [As it happens the two cross products above actually do point in opposite
// directions, but for example (1, 1, 1) x (2, 2, 2) = (-2, 2, 0) and
// (-1, -1, -1) x (2, 2, 2) = (-2, 2, 0) do not.]
return Vector3_d(1, 0, 0); // db[2] * da[1]
}
// Returns true if the given vector's magnitude is large enough such that the
// angle to another vector of the same magnitude can be measured using Angle()
// without loss of precision due to floating-point underflow. (This requirement
// is also sufficient to ensure that Normalize() can be called without risk of
// precision loss.)
inline static bool IsNormalizable(const Vector3_d& p) {
// Let ab = RobustCrossProd(a, b) and cd = RobustCrossProd(cd). In order for
// ab.Angle(cd) to not lose precision, the squared magnitudes of ab and cd
// must each be at least 2**-484. This ensures that the sum of the squared
// magnitudes of ab.CrossProd(cd) and ab.DotProd(cd) is at least 2**-968,
// which ensures that any denormalized terms in these two calculations do
// not affect the accuracy of the result (since all denormalized numbers are
// smaller than 2**-1022, which is less than DBL_ERR * 2**-968).
//
// The fastest way to ensure this is to test whether the largest component of
// the result has a magnitude of at least 2**-242.
return max(fabs(p[0]), max(fabs(p[1]), fabs(p[2]))) >= ldexp(1, -242);
}
// Scales a 3-vector as necessary to ensure that the result can be normalized
// without loss of precision due to floating-point underflow.
//
// REQUIRES: p != (0, 0, 0)
inline static Vector3_d EnsureNormalizable(const Vector3_d& p) {
S2_DCHECK_NE(p, Vector3_d(0, 0, 0));
if (!IsNormalizable(p)) {
// We can't just scale by a fixed factor because the smallest representable
// double is 2**-1074, so if we multiplied by 2**(1074 - 242) then the
// result might be so large that we couldn't square it without overflow.
//
// Note that we must scale by a power of two to avoid rounding errors,
// and that the calculation of "pmax" is free because IsNormalizable()
// is inline. The code below scales "p" such that the largest component is
// in the range [1, 2).
double p_max = max(fabs(p[0]), max(fabs(p[1]), fabs(p[2])));
// The expression below avoids signed overflow for any value of ilogb().
return ldexp(2, -1 - ilogb(p_max)) * p;
}
return p;
}
// Converts an ExactFloat vector to a double-precision vector, scaling the
// result as necessary to ensure that the result can be normalized without loss
// of precision due to floating-point underflow. (This method doesn't actually
// call Normalize() since that would create additional error in situations
// where normalization is not necessary.)
static Vector3_d NormalizableFromExact(const Vector3_xf& xf) {
Vector3_d x(xf[0].ToDouble(), xf[1].ToDouble(), xf[2].ToDouble());
if (IsNormalizable(x)) {
return x;
}
// Scale so that the largest component magnitude is in the range [0.5, 1).
// Note that the exponents involved could be much smaller than those
// representable by an IEEE double precision float.
int exp = ExactFloat::kMinExp - 1;
for (int i = 0; i < 3; ++i) {
if (xf[i].is_normal()) exp = std::max(exp, xf[i].exp());
}
if (exp < ExactFloat::kMinExp) {
return Vector3_d(0, 0, 0); // The exact result is (0, 0, 0).
}
return Vector3_d(ldexp(xf[0], -exp).ToDouble(),
ldexp(xf[1], -exp).ToDouble(),
ldexp(xf[2], -exp).ToDouble());
}
namespace internal {
Vector3_d SymbolicCrossProd(const S2Point& a, const S2Point& b) {
S2_DCHECK_NE(a, b);
// SymbolicCrossProdSorted() requires that a < b.
if (a < b) {
return EnsureNormalizable(SymbolicCrossProdSorted(a, b));
} else {
return -EnsureNormalizable(SymbolicCrossProdSorted(b, a));
}
}
Vector3_d ExactCrossProd(const S2Point& a, const S2Point& b) {
S2_DCHECK_NE(a, b);
Vector3_xf result_xf = ToExact(a).CrossProd(ToExact(b));
if (!s2pred::IsZero(result_xf)) {
return NormalizableFromExact(result_xf);
}
// SymbolicCrossProd() requires that a < b.
if (a < b) {
return EnsureNormalizable(SymbolicCrossProd(a, b));
} else {
return -EnsureNormalizable(SymbolicCrossProd(b, a));
}
}
} // namespace internal
int CrossingSign(const S2Point& a, const S2Point& b,
const S2Point& c, const S2Point& d) {
S2EdgeCrosser crosser(&a, &b, &c);
return crosser.CrossingSign(&d);
}
bool VertexCrossing(const S2Point& a, const S2Point& b,
const S2Point& c, const S2Point& d) {
// If A == B or C == D there is no intersection. We need to check this
// case first in case 3 or more input points are identical.
if (a == b || c == d) return false;
// If any other pair of vertices is equal, there is a crossing if and only
// if OrderedCCW() indicates that the edge AB is further CCW around the
// shared vertex O (either A or B) than the edge CD, starting from an
// arbitrary fixed reference point.
//
// Optimization: if AB=CD or AB=DC, we can avoid most of the calculations.
if (a == c) return (b == d) || s2pred::OrderedCCW(S2::RefDir(a), d, b, a);
if (b == d) return s2pred::OrderedCCW(S2::RefDir(b), c, a, b);
if (a == d) return (b == c) || s2pred::OrderedCCW(S2::RefDir(a), c, b, a);
if (b == c) return s2pred::OrderedCCW(S2::RefDir(b), d, a, b);
S2_LOG(DFATAL) << "VertexCrossing called with 4 distinct vertices";
return false;
}
int SignedVertexCrossing(const S2Point& a, const S2Point& b,
const S2Point& c, const S2Point& d) {
if (a == b || c == d) return 0;
// See VertexCrossing. The sign of the crossing is +1 if both edges are
// outgoing or both edges are incoming with respect to the common vertex
// and -1 otherwise.
if (a == c) {
return ((b == d) || s2pred::OrderedCCW(S2::RefDir(a), d, b, a)) ? 1 : 0;
}
if (b == d) return s2pred::OrderedCCW(S2::RefDir(b), c, a, b) ? 1 : 0;
if (a == d) {
return ((b == c) || s2pred::OrderedCCW(S2::RefDir(a), c, b, a)) ? -1 : 0;
}
if (b == c) return s2pred::OrderedCCW(S2::RefDir(b), d, a, b) ? -1 : 0;
S2_LOG(DFATAL) << "SignedVertexCrossing called with 4 distinct vertices";
return false;
}
bool EdgeOrVertexCrossing(const S2Point& a, const S2Point& b,
const S2Point& c, const S2Point& d) {
int crossing = CrossingSign(a, b, c, d);
if (crossing < 0) return false;
if (crossing > 0) return true;
return VertexCrossing(a, b, c, d);
}
// Computes the cross product of "x" and "y", normalizes it to be unit length,
// and stores the result in "result". Also returns the length of the cross
// product before normalization, which is useful for estimating the amount of
// error in the result. For numerical stability, "x" and "y" should both be
// approximately unit length.
template <class T>
static T RobustNormalWithLength(const Vector3<T>& x, const Vector3<T>& y,
Vector3<T>* result) {
// This computes 2 * (x.CrossProd(y)), but has much better numerical
// stability when "x" and "y" are unit length.
Vector3<T> tmp = (x - y).CrossProd(x + y);
T length = tmp.Norm();
if (length != 0) {
*result = (1 / length) * tmp;
}
return 0.5 * length; // Since tmp == 2 * (x.CrossProd(y))
}
// If the intersection point of the edges (a0,a1) and (b0,b1) can be computed
// to within an error of at most kIntersectionError by this function, then set
// "result" to the intersection point and return true.
template <class T>
static bool GetIntersectionSimple(const Vector3<T>& a0, const Vector3<T>& a1,
const Vector3<T>& b0, const Vector3<T>& b1,
Vector3<T>* result) {
// The code below computes the intersection point as
//
// (a0.CrossProd(a1)).CrossProd(b0.CrossProd(b1))
//
// except that it has better numerical stability and also computes a
// guaranteed error bound.
//
// Each cross product is computed as (X-Y).CrossProd(X+Y) using unit-length
// input vectors, which eliminates most of the cancellation error. However
// the error in the direction of the cross product can still become large if
// the two points are extremely close together. We can show that as long as
// the length of the cross product is at least (16 * sqrt(3) + 24) * DBL_ERR
// (about 6e-15), then the directional error is at most 5 * T_ERR (about
// 3e-19 when T == "long double"). (DBL_ERR appears in the first formula
// because the inputs are assumed to be normalized in double precision
// rather than in the given type T.)
//
// The third cross product is different because its inputs already have some
// error. Letting "result_len" be the length of the cross product, it can
// be shown that the error is at most
//
// (2 + 2 * sqrt(3) + 12 / result_len) * T_ERR
//
// We want this error to be at most kIntersectionError, which is true as
// long as "result_len" is at least kMinResultLen defined below.
constexpr T T_ERR = rounding_epsilon<T>();
constexpr T kMinNormalLength = (16 * kSqrt3 + 24) * DBL_ERR;
constexpr T kMinResultLen =
12 / (kIntersectionError.radians() / T_ERR - (2 + 2 * kSqrt3));
// On some platforms "long double" is the same as "double", and on these
// platforms this method always returns false (e.g. ARM, Win32). Rather
// than testing this directly, instead we look at kMinResultLen since this
// is a direct measure of whether "long double" has sufficient accuracy to
// be useful. If kMinResultLen >= 0.5, it means that this method will fail
// even for edges that meet at an angle of 30 degrees. (On Intel platforms
// kMinResultLen corresponds to an intersection angle of about 0.04
// degrees.)
if (kMinResultLen >= 0.5) return false;
Vector3<T> a_norm, b_norm;
if (RobustNormalWithLength(a0, a1, &a_norm) >= kMinNormalLength &&
RobustNormalWithLength(b0, b1, &b_norm) >= kMinNormalLength &&
RobustNormalWithLength(a_norm, b_norm, result) >= kMinResultLen) {
// Make sure that we return the intersection point rather than its antipode.
*result *= (a_norm.DotProd(b1 - b0) < 0) ? -1 : 1;
return true;
}
return false;
}
static bool GetIntersectionSimpleLD(const S2Point& a0, const S2Point& a1,
const S2Point& b0, const S2Point& b1,
S2Point* result) {
Vector3_ld result_ld;
if (GetIntersectionSimple(ToLD(a0), ToLD(a1), ToLD(b0), ToLD(b1),
&result_ld)) {
*result = S2Point::Cast(result_ld);
return true;
}
return false;
}
// Given a point X and a vector "a_norm" (not necessarily unit length),
// compute x.DotProd(a_norm) and return a bound on the error in the result.
// The remaining parameters allow this dot product to be computed more
// accurately and efficiently. They include the length of "a_norm"
// ("a_norm_len") and the edge endpoints "a0" and "a1".
template <class T>
static T GetProjection(const Vector3<T>& x,
const Vector3<T>& a_norm, T a_norm_len,
const Vector3<T>& a0, const Vector3<T>& a1,
T* error) {
// The error in the dot product is proportional to the lengths of the input
// vectors, so rather than using "x" itself (a unit-length vector) we use
// the vectors from "x" to the closer of the two edge endpoints. This
// typically reduces the error by a huge factor.
Vector3<T> x0 = x - a0;
Vector3<T> x1 = x - a1;
T x0_dist2 = x0.Norm2();
T x1_dist2 = x1.Norm2();
// If both distances are the same, we need to be careful to choose one
// endpoint deterministically so that the result does not change if the
// order of the endpoints is reversed.
T dist, result;
if (x0_dist2 < x1_dist2 || (x0_dist2 == x1_dist2 && x0 < x1)) {
dist = sqrt(x0_dist2);
result = x0.DotProd(a_norm);
} else {
dist = sqrt(x1_dist2);
result = x1.DotProd(a_norm);
}
// This calculation bounds the error from all sources: the computation of
// the normal, the subtraction of one endpoint, and the dot product itself.
// (DBL_ERR appears because the input points are assumed to be normalized in
// double precision rather than in the given type T.)
//
// For reference, the bounds that went into this calculation are:
// ||N'-N|| <= ((1 + 2 * sqrt(3))||N|| + 32 * sqrt(3) * DBL_ERR) * T_ERR
// |(A.B)'-(A.B)| <= (1.5 * (A.B) + 1.5 * ||A|| * ||B||) * T_ERR
// ||(X-Y)'-(X-Y)|| <= ||X-Y|| * T_ERR
constexpr T T_ERR = rounding_epsilon<T>();
*error = (((3.5 + 2 * kSqrt3) * a_norm_len + 32 * kSqrt3 * DBL_ERR)
* dist + 1.5 * fabs(result)) * T_ERR;
return result;
}
// Helper function for GetIntersectionStable(). It expects that the edges
// (a0,a1) and (b0,b1) have been sorted so that the first edge is longer.
template <class T>
static bool GetIntersectionStableSorted(
const Vector3<T>& a0, const Vector3<T>& a1,
const Vector3<T>& b0, const Vector3<T>& b1, Vector3<T>* result) {
S2_DCHECK_GE((a1 - a0).Norm2(), (b1 - b0).Norm2());
// Compute the normal of the plane through (a0, a1) in a stable way.
Vector3<T> a_norm = (a0 - a1).CrossProd(a0 + a1);
T a_norm_len = a_norm.Norm();
T b_len = (b1 - b0).Norm();
// Compute the projection (i.e., signed distance) of b0 and b1 onto the
// plane through (a0, a1). Distances are scaled by the length of a_norm.
T b0_error, b1_error;
T b0_dist = GetProjection(b0, a_norm, a_norm_len, a0, a1, &b0_error);
T b1_dist = GetProjection(b1, a_norm, a_norm_len, a0, a1, &b1_error);
// The total distance from b0 to b1 measured perpendicularly to (a0,a1) is
// |b0_dist - b1_dist|. Note that b0_dist and b1_dist generally have
// opposite signs because b0 and b1 are on opposite sides of (a0, a1). The
// code below finds the intersection point by interpolating along the edge
// (b0, b1) to a fractional distance of b0_dist / (b0_dist - b1_dist).
//
// It can be shown that the maximum error in the interpolation fraction is
//
// (b0_dist * b1_error - b1_dist * b0_error) /
// (dist_sum * (dist_sum - error_sum))
//
// We save ourselves some work by scaling the result and the error bound by
// "dist_sum", since the result is normalized to be unit length anyway.
//
// Make sure that we return the intersection point rather than its antipode.
// It is sufficient to ensure that (b0_dist - b1_dist) is non-negative.
if (b0_dist < b1_dist) {
b0_dist = -b0_dist;
b1_dist = -b1_dist;
}
T dist_sum = b0_dist - b1_dist;
T error_sum = b0_error + b1_error;
if (dist_sum <= error_sum) {
return false; // Error is unbounded in this case.
}
Vector3<T> x = b0_dist * b1 - b1_dist * b0;
constexpr T T_ERR = rounding_epsilon<T>();
T error = b_len * fabs(b0_dist * b1_error - b1_dist * b0_error) /
(dist_sum - error_sum) + 2 * T_ERR * dist_sum;
// Finally we normalize the result, compute the corresponding error, and
// check whether the total error is acceptable.
T x_len2 = x.Norm2();
if (x_len2 < std::numeric_limits<T>::min()) {
// If x.Norm2() is less than the minimum normalized value of T, x_len might
// lose precision and the result might fail to satisfy S2::IsUnitLength().
// TODO(ericv): Implement S2::RobustNormalize().
return false;
}
T x_len = sqrt(x_len2);
const T kMaxError = kIntersectionError.radians();
if (error > (kMaxError - T_ERR) * x_len) {
return false;
}
*result = (1 / x_len) * x;
return true;
}
// Returns whether (a0,a1) is less than (b0,b1) with respect to a total
// ordering on edges that is invariant under edge reversals.
template <class T>
static bool CompareEdges(const Vector3<T>& a0, const Vector3<T>& a1,
const Vector3<T>& b0, const Vector3<T>& b1) {
const Vector3<T> *pa0 = &a0, *pa1 = &a1;
const Vector3<T> *pb0 = &b0, *pb1 = &b1;
if (*pa0 >= *pa1) std::swap(pa0, pa1);
if (*pb0 >= *pb1) std::swap(pb0, pb1);
return *pa0 < *pb0 || (*pa0 == *pb0 && *pb0 < *pb1);
}
// If the intersection point of the edges (a0,a1) and (b0,b1) can be computed
// to within an error of at most kIntersectionError by this function, then set
// "result" to the intersection point and return true.
template <class T>
static bool GetIntersectionStable(const Vector3<T>& a0, const Vector3<T>& a1,
const Vector3<T>& b0, const Vector3<T>& b1,
Vector3<T>* result) {
// Sort the two edges so that (a0,a1) is longer, breaking ties in a
// deterministic way that does not depend on the ordering of the endpoints.
// This is desirable for two reasons:
// - So that the result doesn't change when edges are swapped or reversed.
// - It reduces error, since the first edge is used to compute the edge
// normal (where a longer edge means less error), and the second edge
// is used for interpolation (where a shorter edge means less error).
T a_len2 = (a1 - a0).Norm2();
T b_len2 = (b1 - b0).Norm2();
if (a_len2 < b_len2 || (a_len2 == b_len2 && CompareEdges(a0, a1, b0, b1))) {
return GetIntersectionStableSorted(b0, b1, a0, a1, result);
} else {
return GetIntersectionStableSorted(a0, a1, b0, b1, result);
}
}
static bool GetIntersectionStableLD(const S2Point& a0, const S2Point& a1,
const S2Point& b0, const S2Point& b1,
S2Point* result) {
Vector3_ld result_ld;
if (GetIntersectionStable(ToLD(a0), ToLD(a1), ToLD(b0), ToLD(b1),
&result_ld)) {
*result = S2Point::Cast(result_ld);
return true;
}
return false;
}
inline static S2Point ToS2Point(const Vector3_xf& xf) {
return NormalizableFromExact(xf).Normalize();
}
namespace internal {
// Compute the intersection point of (a0, a1) and (b0, b1) using exact
// arithmetic. Note that the result is not exact because it is rounded to
// double precision.
S2Point GetIntersectionExact(const S2Point& a0, const S2Point& a1,
const S2Point& b0, const S2Point& b1) {
// Since we are using exact arithmetic, we don't need to worry about
// numerical stability.
Vector3_xf a_norm_xf = ToExact(a0).CrossProd(ToExact(a1));
Vector3_xf b_norm_xf = ToExact(b0).CrossProd(ToExact(b1));
Vector3_xf x_xf = a_norm_xf.CrossProd(b_norm_xf);
// The final Normalize() call is done in double precision, which creates a
// directional error of up to 2 * DBL_ERR. (NormalizableFromExact() and
// Normalize() each contribute up to DBL_ERR of directional error.)
if (!s2pred::IsZero(x_xf)) {
// Make sure that we return the intersection point rather than its antipode.
return s2pred::Sign(a0, a1, b1) * ToS2Point(x_xf);
}
// The two edges are exactly collinear, but we still consider them to be
// "crossing" because of simulation of simplicity. The most principled way to
// handle this situation is to use symbolic perturbations, similar to what
// S2::RobustCrossProd and s2pred::Sign do. This is certainly possible, but
// it turns out that there are approximately 18 cases to consider (compared to
// the 4 cases for RobustCrossProd and 13 for s2pred::Sign).
//
// For now we use a heuristic that simply chooses a plausible intersection
// point. Out of the four endpoints, exactly two lie in the interior of the
// other edge. Of those two we return the one that is lexicographically
// smallest.
S2Point a_norm = ToS2Point(a_norm_xf);
S2Point b_norm = ToS2Point(b_norm_xf);
if (a_norm == S2Point(0, 0, 0)) a_norm = SymbolicCrossProd(a0, a1);
if (b_norm == S2Point(0, 0, 0)) b_norm = SymbolicCrossProd(b0, b1);
S2Point x(10, 10, 10); // Greater than any valid S2Point.
if (s2pred::OrderedCCW(b0, a0, b1, b_norm) && a0 < x) x = a0;
if (s2pred::OrderedCCW(b0, a1, b1, b_norm) && a1 < x) x = a1;
if (s2pred::OrderedCCW(a0, b0, a1, a_norm) && b0 < x) x = b0;
if (s2pred::OrderedCCW(a0, b1, a1, a_norm) && b1 < x) x = b1;
S2_DCHECK(S2::IsUnitLength(x));
return x;
}
} // namespace internal
// Given three points "a", "x", "b", returns true if these three points occur
// in the given order along the edge (a,b) to within the given tolerance.
// More precisely, either "x" must be within "tolerance" of "a" or "b", or
// when "x" is projected onto the great circle through "a" and "b" it must lie
// along the edge (a,b) (i.e., the shortest path from "a" to "b").
static bool ApproximatelyOrdered(const S2Point& a, const S2Point& x,
const S2Point& b, double tolerance) {
if ((x - a).Norm2() <= tolerance * tolerance) return true;
if ((x - b).Norm2() <= tolerance * tolerance) return true;
return s2pred::OrderedCCW(a, x, b, S2::RobustCrossProd(a, b).Normalize());
}
S2Point GetIntersection(const S2Point& a0, const S2Point& a1,
const S2Point& b0, const S2Point& b1) {
S2_DCHECK_GT(CrossingSign(a0, a1, b0, b1), 0);
// It is difficult to compute the intersection point of two edges accurately
// when the angle between the edges is very small. Previously we handled
// this by only guaranteeing that the returned intersection point is within
// kIntersectionError of each edge. However, this means that when the edges
// cross at a very small angle, the computed result may be very far from the
// true intersection point.
//
// Instead this function now guarantees that the result is always within
// kIntersectionError of the true intersection. This requires using more
// sophisticated techniques and in some cases extended precision.
//
// Three different techniques are implemented, but only two are used:
//
// - GetIntersectionSimple() computes the intersection point using
// numerically stable cross products in "long double" precision.
//
// - GetIntersectionStable() computes the intersection point using
// projection and interpolation, taking care to minimize cancellation
// error. This method exists in "double" and "long double" versions.
//
// - GetIntersectionExact() computes the intersection point using exact
// arithmetic and converts the final result back to an S2Point.
//
// We don't actually use the first method (GetIntersectionSimple) because it
// turns out that GetIntersectionStable() is twice as fast and also much
// more accurate (even in double precision). The "long double" version
// (only available on some platforms) uses 80-bit precision and is about
// twice as slow. The exact arithmetic version is about 100x slower.
//
// So our strategy is to first call GetIntersectionStable() in double
// precision; if that doesn't work and this platform supports "long double",
// then we try again in "long double"; if that doesn't work then we fall
// back to exact arithmetic.
constexpr bool kUseSimpleMethod = false;
S2Point result;
IntersectionMethod method;
if (kUseSimpleMethod && GetIntersectionSimple(a0, a1, b0, b1, &result)) {
method = IntersectionMethod::SIMPLE;
} else if (kUseSimpleMethod && kHasLongDouble &&
GetIntersectionSimpleLD(a0, a1, b0, b1, &result)) {
method = IntersectionMethod::SIMPLE_LD;
} else if (GetIntersectionStable(a0, a1, b0, b1, &result)) {
method = IntersectionMethod::STABLE;
} else if (kHasLongDouble &&
GetIntersectionStableLD(a0, a1, b0, b1, &result)) {
method = IntersectionMethod::STABLE_LD;
} else {
result = GetIntersectionExact(a0, a1, b0, b1);
method = IntersectionMethod::EXACT;
}
if (intersection_method_tally_) {
++intersection_method_tally_[static_cast<int>(method)];
}
// Make sure that the intersection point lies on both edges.
S2_DCHECK(ApproximatelyOrdered(a0, result, a1, kIntersectionError.radians()));
S2_DCHECK(ApproximatelyOrdered(b0, result, b1, kIntersectionError.radians()));
return result;
}
} // namespace S2