1 2 // Copyright 2016 Google Inc. All Rights Reserved. 3 // 4 // Licensed under the Apache License, Version 2.0 (the "License"); 5 // you may not use this file except in compliance with the License. 6 // You may obtain a copy of the License at 7 // 8 // http://www.apache.org/licenses/LICENSE-2.0 9 // 10 // Unless required by applicable law or agreed to in writing, software 11 // distributed under the License is distributed on an "AS-IS" BASIS, 12 // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 // See the License for the specific language governing permissions and 14 // limitations under the License. 15 16 // Original author: ericv@google.com (Eric Veach) 17 // Converted to D: madric@gmail.com (Vijay Nayar) 18 19 module s2.s2predicates; 20 21 // This class contains various predicates that are guaranteed to produce 22 // correct, consistent results. They are also relatively efficient. This is 23 // achieved by computing conservative error bounds and falling back to high 24 // precision or even exact arithmetic when the result is uncertain. Such 25 // predicates are useful in implementing robust algorithms. 26 // 27 // See also S2EdgeCrosser, which implements various exact 28 // edge-crossing predicates more efficiently than can be done here. 29 // 30 // TODO(ericv): Add InCircleSign() (the Voronoi/Delaunay predicate). 31 // (This is trickier than the usual textbook implementations because we want 32 // to model S2Points as lying exactly on the mathematical unit sphere.) 33 34 import algorithm = std.algorithm; 35 import math = std.math; 36 import s2.logger; 37 import s2.s1chord_angle; 38 import s2.s2point; 39 import s2.s2pointutil; 40 import s2.util.math.exactfloat; 41 import s2.util.math.vector; 42 import s2pointutil = s2.s2pointutil; 43 import std.exception; 44 import std.exception; 45 import traits = std.traits; 46 47 /// A predefined S1ChordAngle representing (approximately) 45 degrees. 48 private immutable S1ChordAngle DEGREES_45 = S1ChordAngle.fromLength2(2 - M_SQRT2); 49 50 // All error bounds in this file are expressed in terms of the maximum 51 // rounding error for a floating-point type. The rounding error is half of 52 // the epsilon value. 53 immutable double DBL_ERR = roundingEpsilon!double(); 54 immutable real REAL_ERR = roundingEpsilon!real(); 55 56 T roundingEpsilon(T)() 57 if (traits.isFloatingPoint!T) { 58 return T.epsilon / 2.0; 59 } 60 61 // S2EdgeUtil contains the following exact predicates that test for edge 62 // crossings. (Usually you will want to use S2EdgeCrosser, which 63 // implements them much more efficiently.) 64 // 65 // int CrossingSign(const S2Point& a0, const S2Point& a1, 66 // const S2Point& b0, const S2Point& b1); 67 // 68 // bool EdgeOrVertexCrossing(const S2Point& a0, const S2Point& a1, 69 // const S2Point& b0, const S2Point& b1); 70 71 // Returns +1 if the points A, B, C are counterclockwise, -1 if the points 72 // are clockwise, and 0 if any two points are the same. This function is 73 // essentially like taking the sign of the determinant of ABC, except that 74 // it has additional logic to make sure that the above properties hold even 75 // when the three points are coplanar, and to deal with the limitations of 76 // floating-point arithmetic. 77 // 78 // Sign satisfies the following conditions: 79 // 80 // (1) Sign(a,b,c) == 0 if and only if a == b, b == c, or c == a 81 // (2) Sign(b,c,a) == Sign(a,b,c) for all a,b,c 82 // (3) Sign(c,b,a) == -Sign(a,b,c) for all a,b,c 83 // 84 // In other words: 85 // 86 // (1) The result is zero if and only if two points are the same. 87 // (2) Rotating the order of the arguments does not affect the result. 88 // (3) Exchanging any two arguments inverts the result. 89 // 90 // On the other hand, note that it is not true in general that 91 // Sign(-a,b,c) == -Sign(a,b,c), or any similar identities 92 // involving antipodal points. 93 int sign(in S2Point a, in S2Point b, in S2Point c) { 94 // We don't need RobustCrossProd() here because Sign() does its own 95 // error estimation and calls ExpensiveSign() if there is any uncertainty 96 // about the result. 97 return sign(a, b, c, a.crossProd(b)); 98 } 99 100 // Compute the determinant in a numerically stable way. Unlike TriageSign(), 101 // this method can usually compute the correct determinant sign even when all 102 // three points are as collinear as possible. For example if three points are 103 // spaced 1km apart along a random line on the Earth's surface using the 104 // nearest representable points, there is only a 0.4% chance that this method 105 // will not be able to find the determinant sign. The probability of failure 106 // decreases as the points get closer together; if the collinear points are 107 // 1 meter apart, the failure rate drops to 0.0004%. 108 // 109 // This method could be extended to also handle nearly-antipodal points (and 110 // in fact an earlier version of this code did exactly that), but antipodal 111 // points are rare in practice so it seems better to simply fall back to 112 // exact arithmetic in that case. 113 package int stableSign(in S2Point a, in S2Point b, in S2Point c) { 114 Vector3_d ab = b - a; 115 Vector3_d bc = c - b; 116 Vector3_d ca = a - c; 117 double ab2 = ab.norm2(); 118 double bc2 = bc.norm2(); 119 double ca2 = ca.norm2(); 120 121 // Now compute the determinant ((A-C)x(B-C)).C, where the vertices have been 122 // cyclically permuted if necessary so that AB is the longest edge. (This 123 // minimizes the magnitude of cross product.) At the same time we also 124 // compute the maximum error in the determinant. Using a similar technique 125 // to the one used for kMaxDetError, the error is at most 126 // 127 // |d| <= (3 + 6/sqrt(3)) * |A-C| * |B-C| * e 128 // 129 // where e = 0.5 * DBL_EPSILON. If the determinant magnitude is larger than 130 // this value then we know its sign with certainty. 131 const double kDetErrorMultiplier = 3.2321 * double.epsilon; // see above 132 double det; 133 double max_error; 134 if (ab2 >= bc2 && ab2 >= ca2) { 135 // AB is the longest edge, so compute (A-C)x(B-C).C. 136 det = -(ca.crossProd(bc).dotProd(c)); 137 max_error = kDetErrorMultiplier * math.sqrt(ca2 * bc2); 138 } else if (bc2 >= ca2) { 139 // BC is the longest edge, so compute (B-A)x(C-A).A. 140 det = -(ab.crossProd(ca).dotProd(a)); 141 max_error = kDetErrorMultiplier * math.sqrt(ab2 * ca2); 142 } else { 143 // CA is the longest edge, so compute (C-B)x(A-B).B. 144 det = -(bc.crossProd(ab).dotProd(b)); 145 max_error = kDetErrorMultiplier * math.sqrt(bc2 * ab2); 146 } 147 return (math.fabs(det) <= max_error) ? 0 : (det > 0) ? 1 : -1; 148 } 149 150 // The following function returns the sign of the determinant of three points 151 // A, B, C under a model where every possible S2Point is slightly perturbed by 152 // a unique infinitesmal amount such that no three perturbed points are 153 // collinear and no four points are coplanar. The perturbations are so small 154 // that they do not change the sign of any determinant that was non-zero 155 // before the perturbations, and therefore can be safely ignored unless the 156 // determinant of three points is exactly zero (using multiple-precision 157 // arithmetic). 158 // 159 // Since the symbolic perturbation of a given point is fixed (i.e., the 160 // perturbation is the same for all calls to this method and does not depend 161 // on the other two arguments), the results of this method are always 162 // self-consistent. It will never return results that would correspond to an 163 // "impossible" configuration of non-degenerate points. 164 // 165 // Requirements: 166 // The 3x3 determinant of A, B, C must be exactly zero. 167 // The points must be distinct, with A < B < C in lexicographic order. 168 // 169 // Returns: 170 // +1 or -1 according to the sign of the determinant after the symbolic 171 // perturbations are taken into account. 172 // 173 // Reference: 174 // "Simulation of Simplicity" (Edelsbrunner and Muecke, ACM Transactions on 175 // Graphics, 1990). 176 // 177 private int symbolicallyPerturbedSign( 178 in Vector3_xf a, in Vector3_xf b, 179 in Vector3_xf c, in Vector3_xf b_cross_c) 180 in { 181 // This method requires that the points are sorted in lexicographically 182 // increasing order. This is because every possible S2Point has its own 183 // symbolic perturbation such that if A < B then the symbolic perturbation 184 // for A is much larger than the perturbation for B. 185 // 186 // Alternatively, we could sort the points in this method and keep track of 187 // the sign of the permutation, but it is more efficient to do this before 188 // converting the inputs to the multi-precision representation, and this 189 // also lets us re-use the result of the cross product B x C. 190 assert(a < b && b < c); 191 } do { 192 // Every input coordinate x[i] is assigned a symbolic perturbation dx[i]. 193 // We then compute the sign of the determinant of the perturbed points, 194 // i.e. 195 // | a[0]+da[0] a[1]+da[1] a[2]+da[2] | 196 // | b[0]+db[0] b[1]+db[1] b[2]+db[2] | 197 // | c[0]+dc[0] c[1]+dc[1] c[2]+dc[2] | 198 // 199 // The perturbations are chosen such that 200 // 201 // da[2] > da[1] > da[0] > db[2] > db[1] > db[0] > dc[2] > dc[1] > dc[0] 202 // 203 // where each perturbation is so much smaller than the previous one that we 204 // don't even need to consider it unless the coefficients of all previous 205 // perturbations are zero. In fact, it is so small that we don't need to 206 // consider it unless the coefficient of all products of the previous 207 // perturbations are zero. For example, we don't need to consider the 208 // coefficient of db[1] unless the coefficient of db[2]*da[0] is zero. 209 // 210 // The follow code simply enumerates the coefficients of the perturbations 211 // (and products of perturbations) that appear in the determinant above, in 212 // order of decreasing perturbation magnitude. The first non-zero 213 // coefficient determines the sign of the result. The easiest way to 214 // enumerate the coefficients in the correct order is to pretend that each 215 // perturbation is some tiny value "eps" raised to a power of two: 216 // 217 // eps** 1 2 4 8 16 32 64 128 256 218 // da[2] da[1] da[0] db[2] db[1] db[0] dc[2] dc[1] dc[0] 219 // 220 // Essentially we can then just count in binary and test the corresponding 221 // subset of perturbations at each step. So for example, we must test the 222 // coefficient of db[2]*da[0] before db[1] because eps**12 > eps**16. 223 // 224 // Of course, not all products of these perturbations appear in the 225 // determinant above, since the determinant only contains the products of 226 // elements in distinct rows and columns. Thus we don't need to consider 227 // da[2]*da[1], db[1]*da[1], etc. Furthermore, sometimes different pairs of 228 // perturbations have the same coefficient in the determinant; for example, 229 // da[1]*db[0] and db[1]*da[0] have the same coefficient (c[2]). Therefore 230 // we only need to test this coefficient the first time we encounter it in 231 // the binary order above (which will be db[1]*da[0]). 232 // 233 // The sequence of tests below also appears in Table 4-ii of the paper 234 // referenced above, if you just want to look it up, with the following 235 // translations: [a,b,c] -> [i,j,k] and [0,1,2] -> [1,2,3]. Also note that 236 // some of the signs are different because the opposite cross product is 237 // used (e.g., B x C rather than C x B). 238 239 int det_sign = b_cross_c[2].sign(); // da[2] 240 if (det_sign != 0) return det_sign; 241 det_sign = b_cross_c[1].sign(); // da[1] 242 if (det_sign != 0) return det_sign; 243 det_sign = b_cross_c[0].sign(); // da[0] 244 if (det_sign != 0) return det_sign; 245 246 det_sign = (c[0] * a[1] - c[1] * a[0]).sign(); // db[2] 247 if (det_sign != 0) return det_sign; 248 det_sign = c[0].sign(); // db[2] * da[1] 249 if (det_sign != 0) return det_sign; 250 det_sign = -(c[1].sign()); // db[2] * da[0] 251 if (det_sign != 0) return det_sign; 252 det_sign = (c[2] * a[0] - c[0] * a[2]).sign(); // db[1] 253 if (det_sign != 0) return det_sign; 254 det_sign = c[2].sign(); // db[1] * da[0] 255 if (det_sign != 0) return det_sign; 256 // The following test is listed in the paper, but it is redundant because 257 // the previous tests guarantee that C == (0, 0, 0). 258 enforce(0 == (c[1] * a[2] - c[2] * a[1]).sign()); // db[0] 259 260 det_sign = (a[0] * b[1] - a[1] * b[0]).sign(); // dc[2] 261 if (det_sign != 0) return det_sign; 262 det_sign = -(b[0].sign()); // dc[2] * da[1] 263 if (det_sign != 0) return det_sign; 264 det_sign = b[1].sign(); // dc[2] * da[0] 265 if (det_sign != 0) return det_sign; 266 det_sign = a[0].sign(); // dc[2] * db[1] 267 if (det_sign != 0) return det_sign; 268 return 1; // dc[2] * db[1] * da[0] 269 } 270 271 // Given 4 points on the unit sphere, return true if the edges OA, OB, and 272 // OC are encountered in that order while sweeping CCW around the point O. 273 // You can think of this as testing whether A <= B <= C with respect to the 274 // CCW ordering around O that starts at A, or equivalently, whether B is 275 // contained in the range of angles (inclusive) that starts at A and extends 276 // CCW to C. Properties: 277 // 278 // (1) If OrderedCCW(a,b,c,o) && OrderedCCW(b,a,c,o), then a == b 279 // (2) If OrderedCCW(a,b,c,o) && OrderedCCW(a,c,b,o), then b == c 280 // (3) If OrderedCCW(a,b,c,o) && OrderedCCW(c,b,a,o), then a == b == c 281 // (4) If a == b or b == c, then OrderedCCW(a,b,c,o) is true 282 // (5) Otherwise if a == c, then OrderedCCW(a,b,c,o) is false 283 bool orderedCCW(in S2Point a, in S2Point b, in S2Point c, in S2Point o) { 284 // The last inequality below is ">" rather than ">=" so that we return true 285 // if A == B or B == C, and otherwise false if A == C. Recall that 286 // Sign(x,y,z) == -Sign(z,y,x) for all x,y,z. 287 288 int sum = 0; 289 if (sign(b, o, a) >= 0) { 290 ++sum; 291 } 292 if (sign(c, o, b) >= 0) { 293 ++sum; 294 } 295 if (sign(a, o, c) > 0) { 296 ++sum; 297 } 298 return sum >= 2; 299 } 300 301 // Returns -1, 0, or +1 according to whether AX < BX, A == B, or AX > BX 302 // respectively. Distances are measured with respect to the positions of X, 303 // A, and B as though they were reprojected to lie exactly on the surface of 304 // the unit sphere. Furthermore, this method uses symbolic perturbations to 305 // ensure that the result is non-zero whenever A != B, even when AX == BX 306 // exactly, or even when A and B project to the same point on the sphere. 307 // Such results are guaranteed to be self-consistent, i.e. if AB < BC and 308 // BC < AC, then AB < AC. 309 int compareDistances(in S2Point x, in S2Point a, in S2Point b) { 310 // We start by comparing distances using dot products (i.e., cosine of the 311 // angle), because (1) this is the cheapest technique, and (2) it is valid 312 // over the entire range of possible angles. (We can only use the sin^2 313 // technique if both angles are less than 90 degrees or both angles are 314 // greater than 90 degrees.) 315 int sign = triageCompareCosDistances!double(x, a, b); 316 if (sign != 0) { 317 return sign; 318 } 319 320 // Optimization for (a == b) to avoid falling back to exact arithmetic. 321 if (a == b) { 322 return 0; 323 } 324 325 // It is much better numerically to compare distances using cos(angle) if 326 // the distances are near 90 degrees and sin^2(angle) if the distances are 327 // near 0 or 180 degrees. We only need to check one of the two angles when 328 // making this decision because the fact that the test above failed means 329 // that angles "a" and "b" are very close together. 330 double cos_ax = a.dotProd(x); 331 if (cos_ax > M_SQRT1_2) { 332 // Angles < 45 degrees. 333 sign = compareSin2Distances(x, a, b); 334 } else if (cos_ax < -M_SQRT1_2) { 335 // Angles > 135 degrees. sin^2(angle) is decreasing in this range. 336 sign = -compareSin2Distances(x, a, b); 337 } else { 338 // We've already tried double precision, so continue with "long double". 339 sign = triageCompareCosDistances!real(Vector3_r.from(x), Vector3_r.from(a), Vector3_r.from(b)); 340 } 341 if (sign != 0) { 342 return sign; 343 } 344 sign = exactCompareDistances(Vector3_xf.from(x), Vector3_xf.from(a), Vector3_xf.from(b)); 345 if (sign != 0) { 346 return sign; 347 } 348 return symbolicCompareDistances(x, a, b); 349 } 350 351 int triageCompareCosDistance(T)(in Vector!(T, 3) x, in Vector!(T, 3) y, T r2) { 352 T T_ERR = roundingEpsilon!T(); 353 T cos_xy_error; 354 T cos_xy = getCosDistance(x, y, cos_xy_error); 355 T cos_r = 1 - 0.5 * r2; 356 T cos_r_error = 2 * T_ERR * cos_r; 357 T diff = cos_xy - cos_r; 358 T error = cos_xy_error + cos_r_error; 359 return (diff > error) ? -1 : (diff < -error) ? 1 : 0; 360 } 361 362 int triageCompareSin2Distance(T)(in Vector!(T, 3) x, in Vector!(T, 3) y, T r2) 363 in { 364 assert(r2 < 2.0); // Only valid for distance limits < 90 degrees. 365 } do { 366 T T_ERR = roundingEpsilon!T(); 367 T sin2_xy_error; 368 T sin2_xy = getSin2Distance(x, y, sin2_xy_error); 369 T sin2_r = r2 * (1 - 0.25 * r2); 370 T sin2_r_error = 3 * T_ERR * sin2_r; 371 T diff = sin2_xy - sin2_r; 372 T error = sin2_xy_error + sin2_r_error; 373 return (diff > error) ? 1 : (diff < -error) ? -1 : 0; 374 } 375 376 package int exactCompareDistance(in Vector3_xf x, in Vector3_xf y, in ExactFloat r2) { 377 // This code produces the same result as though all points were reprojected 378 // to lie exactly on the surface of the unit sphere. It is based on 379 // comparing the cosine of the angle XY (when both points are projected to 380 // lie exactly on the sphere) to the given threshold. 381 ExactFloat cos_xy = x.dotProd(y); 382 ExactFloat cos_r = ExactFloat(1) - ExactFloat(0.5) * r2; 383 // If the two values have different signs, we need to handle that case now 384 // before squaring them below. 385 int xy_sign = cos_xy.sign(), r_sign = cos_r.sign(); 386 if (xy_sign != r_sign) { 387 return (xy_sign > r_sign) ? -1 : 1; // If cos(XY) > cos(r), then XY < r. 388 } 389 ExactFloat cmp = cos_r * cos_r * x.norm2() * y.norm2() - cos_xy * cos_xy; 390 return xy_sign * cmp.sign(); 391 } 392 393 // Returns -1, 0, or +1 according to whether the distance XY is less than, 394 // equal to, or greater than "r" respectively. Distances are measured with 395 // respect the positions of all points as though they are projected to lie 396 // exactly on the surface of the unit sphere. 397 int compareDistance(in S2Point x, in S2Point y, in S1ChordAngle r) { 398 // As with CompareDistances(), we start by comparing dot products because 399 // the sin^2 method is only valid when the distance XY and the limit "r" are 400 // both less than 90 degrees. 401 int sign = triageCompareCosDistance(x, y, r.length2()); 402 if (sign != 0) { 403 return sign; 404 } 405 406 // Unlike with CompareDistances(), it's not worth using the sin^2 method 407 // when the distance limit is near 180 degrees because the S1ChordAngle 408 // representation itself has has a rounding error of up to 2e-8 radians for 409 // distances near 180 degrees. 410 if (r < DEGREES_45) { 411 sign = triageCompareSin2Distance(x, y, r.length2()); 412 if (sign != 0) { 413 return sign; 414 } 415 sign = triageCompareSin2Distance( 416 Vector3_r.from(x), Vector3_r.from(y), cast(real) r.length2()); 417 } else { 418 sign = triageCompareCosDistance( 419 Vector3_r.from(x), Vector3_r.from(y), cast(real) r.length2()); 420 } 421 if (sign != 0) { 422 return sign; 423 } 424 return exactCompareDistance(Vector3_xf.from(x), Vector3_xf.from(y), ExactFloat(r.length2())); 425 } 426 427 // Helper function that compares the distance XY against the squared chord 428 // distance "r2" using the given precision "T". 429 private int triageCompareDistance(T)(in Vector!(T, 3) x, in Vector!(T, 3) y, T r2) { 430 // The Sin2 method is much more accurate for small distances, but it is only 431 // valid when the actual distance and the distance limit are both less than 432 // 90 degrees. So we always start with the Cos method. 433 int sign = triageCompareCosDistance(x, y, r2); 434 if (sign == 0 && r2 < DEGREES_45.length2()) { 435 sign = triageCompareSin2Distance(x, y, r2); 436 } 437 return sign; 438 } 439 440 // Helper function that returns "a0" or "a1", whichever is closer to "x". 441 // Also returns the squared distance from the returned point to "x" in "ax2". 442 private Vector3!T getClosestVertex(T)( 443 in Vector!(T, 3) x, in Vector!(T, 3) a0, in Vector!(T, 3) a1, out T ax2) { 444 T a0x2 = (a0 - x).norm2(); 445 T a1x2 = (a1 - x).norm2(); 446 if (a0x2 < a1x2 || (a0x2 == a1x2 && a0 < a1)) { 447 ax2 = a0x2; 448 return a0; 449 } else { 450 ax2 = a1x2; 451 return a1; 452 } 453 } 454 455 // Helper function that returns -1, 0, or +1 according to whether the distance 456 // from "x" to the great circle through (a0, a1) is less than, equal to, or 457 // greater than the given squared chord length "r2". This method computes the 458 // squared sines of the distances involved, which is more accurate when the 459 // distances are small (less than 45 degrees). 460 // 461 // The remaining parameters are functions of (a0, a1) and are passed in 462 // because they have already been computed: n = (a0 - a1) x (a0 + a1), 463 // n1 = n.Norm(), and n2 = n.Norm2(). 464 private int triageCompareLineSin2Distance(T)( 465 in Vector!(T, 3) x, in Vector!(T, 3) a0, 466 in Vector!(T, 3) a1, T r2, 467 in Vector!(T, 3) n, T n1, T n2) { 468 T T_ERR = roundingEpsilon!T(); 469 470 // The minimum distance is to a point on the edge interior. Since the true 471 // distance to the edge is always less than 90 degrees, we can return 472 // immediately if the limit is 90 degrees or larger. 473 if (r2 >= 2.0) return -1; // distance < limit 474 475 // Otherwise we compute sin^2(distance to edge) to get the best accuracy 476 // when the distance limit is small (e.g., S2::kIntersectionError). 477 T n2sin2_r = n2 * r2 * (1 - 0.25 * r2); 478 T n2sin2_r_error = 6 * T_ERR * n2sin2_r; 479 T ax2, xDn = (x - getClosestVertex(x, a0, a1, ax2)).dotProd(n); 480 T xDn2 = xDn * xDn; 481 const T c1 = (((3.5 + 2 * math.sqrt(3.0)) * n1 + 32 * math.sqrt(3.0) * DBL_ERR) * 482 T_ERR * math.sqrt(ax2)); 483 T xDn2_error = 4 * T_ERR * xDn2 + (2 * math.fabs(xDn) + c1) * c1; 484 485 // If we are using extended precision, then it is worthwhile to recompute 486 // the length of X more accurately. Otherwise we use the fact that X is 487 // guaranteed to be unit length to with a tolerance of 4 * DBL_ERR. 488 if (T_ERR < DBL_ERR) { 489 n2sin2_r *= x.norm2(); 490 n2sin2_r_error += 4 * T_ERR * n2sin2_r; 491 } else { 492 n2sin2_r_error += 8 * DBL_ERR * n2sin2_r; 493 } 494 T diff = xDn2 - n2sin2_r; 495 T error = xDn2_error + n2sin2_r_error; 496 return (diff > error) ? 1 : (diff < -error) ? -1 : 0; 497 } 498 499 // Like TriageCompareLineSin2Distance, but this method computes the squared 500 // cosines of the distances involved. It is more accurate when the distances 501 // are large (greater than 45 degrees). 502 private int triageCompareLineCos2Distance(T)( 503 in Vector!(T, 3) x, in Vector!(T, 3) a0, 504 in Vector!(T, 3) a1, T r2, 505 in Vector!(T, 3) n, T n1, T n2) { 506 T T_ERR = roundingEpsilon!T(); 507 508 // The minimum distance is to a point on the edge interior. Since the true 509 // distance to the edge is always less than 90 degrees, we can return 510 // immediately if the limit is 90 degrees or larger. 511 if (r2 >= 2.0) return -1; // distance < limit 512 513 // Otherwise we compute cos^2(distance to edge). 514 T cos_r = 1 - 0.5 * r2; 515 T n2cos2_r = n2 * cos_r * cos_r; 516 T n2cos2_r_error = 7 * T_ERR * n2cos2_r; 517 518 // The length of M = X.CrossProd(N) is the cosine of the distance. 519 T m2 = x.crossProd(n).norm2(); 520 T m1 = math.sqrt(m2); 521 T m1_error = ((1 + 8 / math.sqrt(3.0)) * n1 + 32 * math.sqrt(3.0) * DBL_ERR) * T_ERR; 522 T m2_error = 3 * T_ERR * m2 + (2 * m1 + m1_error) * m1_error; 523 524 // If we are using extended precision, then it is worthwhile to recompute 525 // the length of X more accurately. Otherwise we use the fact that X is 526 // guaranteed to be unit length to within a tolerance of 4 * DBL_ERR. 527 if (T_ERR < DBL_ERR) { 528 n2cos2_r *= x.norm2(); 529 n2cos2_r_error += 4 * T_ERR * n2cos2_r; 530 } else { 531 n2cos2_r_error += 8 * DBL_ERR * n2cos2_r; 532 } 533 T diff = m2 - n2cos2_r; 534 T error = m2_error + n2cos2_r_error; 535 return (diff > error) ? -1 : (diff < -error) ? 1 : 0; 536 } 537 538 private int triageCompareLineDistance(T)( 539 in Vector!(T, 3) x, in Vector!(T, 3) a0, 540 in Vector!(T, 3) a1, T r2, 541 in Vector!(T, 3) n, T n1, T n2) { 542 if (r2 < DEGREES_45.length2()) { 543 return triageCompareLineSin2Distance(x, a0, a1, r2, n, n1, n2); 544 } else { 545 return triageCompareLineCos2Distance(x, a0, a1, r2, n, n1, n2); 546 } 547 } 548 549 package int triageCompareEdgeDistance(T)( 550 in Vector!(T, 3) x, in Vector!(T, 3) a0, in Vector!(T, 3) a1, T r2) { 551 T T_ERR = roundingEpsilon!T(); 552 // First we need to decide whether the closest point is an edge endpoint or 553 // somewhere in the interior. To determine this we compute a plane 554 // perpendicular to (a0, a1) that passes through X. Letting M be the normal 555 // to this plane, the closest point is in the edge interior if and only if 556 // a0.M < 0 and a1.M > 0. Note that we can use "<" rather than "<=" because 557 // if a0.M or a1.M is zero exactly then it doesn't matter which code path we 558 // follow (since the distance to an endpoint and the distance to the edge 559 // interior are exactly the same in this case). 560 Vector3!T n = (a0 - a1).crossProd(a0 + a1); 561 Vector3!T m = n.crossProd(x); 562 // For better accuracy when the edge (a0,a1) is very short, we subtract "x" 563 // before computing the dot products with M. 564 Vector3!T a0_dir = a0 - x; 565 Vector3!T a1_dir = a1 - x; 566 T a0_sign = a0_dir.dotProd(m); 567 T a1_sign = a1_dir.dotProd(m); 568 T n2 = n.norm2(); 569 T n1 = math.sqrt(n2); 570 T n1_error = ((3.5 + 8 / math.sqrt(3.0)) * n1 + 32 * math.sqrt(3.0) * DBL_ERR) * T_ERR; 571 T a0_sign_error = n1_error * a0_dir.norm(); 572 T a1_sign_error = n1_error * a1_dir.norm(); 573 if (math.fabs(a0_sign) < a0_sign_error || math.fabs(a1_sign) < a1_sign_error) { 574 // It is uncertain whether minimum distance is to an edge vertex or to the 575 // edge interior. We handle this by computing both distances and checking 576 // whether they yield the same result. 577 int vertex_sign = algorithm.min( 578 triageCompareDistance(x, a0, r2), triageCompareDistance(x, a1, r2)); 579 int line_sign = triageCompareLineDistance(x, a0, a1, r2, n, n1, n2); 580 return (vertex_sign == line_sign) ? line_sign : 0; 581 } 582 if (a0_sign >= 0 || a1_sign <= 0) { 583 // The minimum distance is to an edge endpoint. 584 return algorithm.min( 585 triageCompareDistance(x, a0, r2), triageCompareDistance(x, a1, r2)); 586 } else { 587 // The minimum distance is to the edge interior. 588 return triageCompareLineDistance(x, a0, a1, r2, n, n1, n2); 589 } 590 } 591 592 // REQUIRES: the closest point to "x" is in the interior of edge (a0, a1). 593 private int exactCompareLineDistance( 594 in Vector3_xf x, in Vector3_xf a0, 595 in Vector3_xf a1, in ExactFloat r2) { 596 // Since we are given that the closest point is in the edge interior, the 597 // true distance is always less than 90 degrees (which corresponds to a 598 // squared chord length of 2.0). 599 if (r2 >= 2.0) return -1; // distance < limit 600 601 // Otherwise compute the edge normal 602 Vector3_xf n = a0.crossProd(a1); 603 ExactFloat sin_d = x.dotProd(n); 604 ExactFloat sin2_r = r2 * (1 - 0.25 * r2); 605 ExactFloat cmp = sin_d * sin_d - sin2_r * x.norm2() * n.norm2(); 606 return cmp.sign(); 607 } 608 609 package int exactCompareEdgeDistance( 610 in S2Point x, in S2Point a0, 611 in S2Point a1, S1ChordAngle r) { 612 // Even if previous calculations were uncertain, we might not need to do 613 // *all* the calculations in exact arithmetic here. For example it may be 614 // easy to determine whether "x" is closer to an endpoint or the edge 615 // interior. The only calculation where we always use exact arithmetic is 616 // when measuring the distance to the extended line (great circle) through 617 // "a0" and "a1", since it is virtually certain that the previous floating 618 // point calculations failed in that case. 619 // 620 // CompareEdgeDirections also checks that no edge has antipodal endpoints. 621 if (compareEdgeDirections(a0, a1, a0, x) > 0 && 622 compareEdgeDirections(a0, a1, x, a1) > 0) { 623 // The closest point to "x" is along the interior of the edge. 624 return exactCompareLineDistance( 625 Vector3_xf.from(x), Vector3_xf.from(a0), Vector3_xf.from(a1), ExactFloat(r.length2())); 626 } else { 627 // The closest point to "x" is one of the edge endpoints. 628 return algorithm.min(compareDistance(x, a0, r), compareDistance(x, a1, r)); 629 } 630 } 631 632 /** 633 * Returns -1, 0, or +1 according to whether the distance from the point X to 634 * the edge A is less than, equal to, or greater than "r" respectively. 635 * Distances are measured with respect the positions of all points as though 636 * they were projected to lie exactly on the surface of the unit sphere. 637 * 638 * REQUIRES: A0 and A1 do not project to antipodal points (e.g., A0 == -A1). 639 * This requires that (A0 != C * A1) for any constant C < 0. 640 * 641 * NOTE(ericv): All of the predicates defined here could be extended to handle 642 * edges consisting of antipodal points by implementing additional symbolic 643 * perturbation logic (similar to Sign) in order to rigorously define the 644 * direction of such edges. 645 */ 646 int compareEdgeDistance(in S2Point x, in S2Point a0, in S2Point a1, in S1ChordAngle r) 647 in { 648 // Check that the edge does not consist of antipodal points. (This catches 649 // the most common case -- the full test is in ExactCompareEdgeDistance.) 650 assert(a0 != -a1); 651 } do { 652 int sign = triageCompareEdgeDistance(x, a0, a1, r.length2()); 653 if (sign != 0) { 654 return sign; 655 } 656 657 // Optimization for the case where the edge is degenerate. 658 if (a0 == a1) { 659 return compareDistance(x, a0, r); 660 } 661 662 sign = triageCompareEdgeDistance( 663 Vector3_r.from(x), Vector3_r.from(a0), Vector3_r.from(a1), r.length2()); 664 if (sign != 0) { 665 return sign; 666 } 667 return exactCompareEdgeDistance(x, a0, a1, r); 668 } 669 670 /** 671 * Returns -1, 0, or +1 according to whether the normal of edge A has 672 * negative, zero, or positive dot product with the normal of edge B. This 673 * essentially measures whether the edges A and B are closer to proceeding in 674 * the same direction or in opposite directions around the sphere. 675 * 676 * This method returns an exact result, i.e. the result is zero if and only if 677 * the two edges are exactly perpendicular or at least one edge is degenerate. 678 * (i.e., both edge endpoints project to the same point on the sphere). 679 * 680 * CAVEAT: This method does not use symbolic perturbations. Therefore it can 681 * return zero even when A0 != A1 and B0 != B1, e.g. if (A0 == C * A1) exactly 682 * for some constant C > 0 (which is possible even when both points are 683 * considered "normalized"). 684 * 685 * REQUIRES: Neither edge can consist of antipodal points (e.g., A0 == -A1) 686 * (see comments in CompareEdgeDistance). 687 */ 688 int compareEdgeDirections(in S2Point a0, in S2Point a1, in S2Point b0, in S2Point b1) 689 in { 690 // Check that no edge consists of antipodal points. (This catches the most 691 // common case -- the full test is in ExactCompareEdgeDirections.) 692 assert(a0 != -a1); 693 assert(b0 != -b1); 694 } do { 695 int sign = triageCompareEdgeDirections(a0, a1, b0, b1); 696 if (sign != 0) { 697 return sign; 698 } 699 700 // Optimization for the case where either edge is degenerate. 701 if (a0 == a1 || b0 == b1) { 702 return 0; 703 } 704 705 sign = triageCompareEdgeDirections( 706 Vector3_r.from(a0), Vector3_r.from(a1), Vector3_r.from(b0), Vector3_r.from(b1)); 707 if (sign != 0) { 708 return sign; 709 } 710 return exactCompareEdgeDirections( 711 Vector3_xf.from(a0), Vector3_xf.from(a1), Vector3_xf.from(b0), Vector3_xf.from(b1)); 712 } 713 714 /** 715 * If triangle ABC has positive sign, returns its circumcenter. If ABC has 716 * negative sign, returns the negated circumcenter. 717 */ 718 private Vector3!T getCircumcenter(T)( 719 in Vector!(T, 3) a, in Vector!(T, 3) b, 720 in Vector!(T, 3) c, out T error) { 721 T T_ERR = roundingEpsilon!T(); 722 723 // We compute the circumcenter using the intersection of the perpendicular 724 // bisectors of AB and BC. The formula is essentially 725 // 726 // Z = ((A x B) x (A + B)) x ((B x C) x (B + C)), 727 // 728 // except that we compute the cross product (A x B) as (A - B) x (A + B) 729 // (and similarly for B x C) since this is much more stable when the inputs 730 // are unit vectors. 731 Vector3!T ab_diff = a - b, ab_sum = a + b; 732 Vector3!T bc_diff = b - c, bc_sum = b + c; 733 Vector3!T nab = ab_diff.crossProd(ab_sum); 734 T nab_len = nab.norm(); 735 T ab_len = ab_diff.norm(); 736 Vector3!T nbc = bc_diff.crossProd(bc_sum); 737 T nbc_len = nbc.norm(); 738 T bc_len = bc_diff.norm(); 739 Vector3!T mab = nab.crossProd(ab_sum); 740 Vector3!T mbc = nbc.crossProd(bc_sum); 741 error = (((16 + 24 * math.sqrt(3.0)) * T_ERR 742 + 8 * DBL_ERR * (ab_len + bc_len)) * nab_len * nbc_len 743 + 128 * math.sqrt(3.0) * DBL_ERR * T_ERR * (nab_len + nbc_len) 744 + 3 * 4096 * DBL_ERR * DBL_ERR * T_ERR * T_ERR); 745 return mab.crossProd(mbc); 746 } 747 748 package int triageEdgeCircumcenterSign(T)( 749 in Vector!(T, 3) x0, in Vector!(T, 3) x1, 750 in Vector!(T, 3) a, in Vector!(T, 3) b, 751 in Vector!(T, 3) c, int abc_sign) { 752 T T_ERR = roundingEpsilon!T(); 753 754 // Compute the circumcenter Z of triangle ABC, and then test which side of 755 // edge X it lies on. 756 T z_error; 757 Vector3!T z = getCircumcenter(a, b, c, z_error); 758 Vector3!T nx = (x0 - x1).crossProd(x0 + x1); 759 // If the sign of triangle ABC is negative, then we have computed -Z and the 760 // result should be negated. 761 T result = abc_sign * nx.dotProd(z); 762 763 T z_len = z.norm(); 764 T nx_len = nx.norm(); 765 T nx_error = ((1 + 2 * math.sqrt(3.0)) * nx_len + 32 * math.sqrt(3.0) * DBL_ERR) * T_ERR; 766 T result_error = ((3 * T_ERR * nx_len + nx_error) * z_len + z_error * nx_len); 767 return (result > result_error) ? 1 : (result < -result_error) ? -1 : 0; 768 } 769 770 package int exactEdgeCircumcenterSign( 771 in Vector3_xf x0, in Vector3_xf x1, 772 in Vector3_xf a, in Vector3_xf b, 773 in Vector3_xf c, int abc_sign) { 774 // Return zero if the edge X is degenerate. (Also see the comments in 775 // SymbolicEdgeCircumcenterSign.) 776 if (arePointsLinearlyDependent(x0, x1)) { 777 enforce(x0.dotProd(x1) > 0); // Antipodal edges not allowed. 778 return 0; 779 } 780 // The simplest predicate for testing whether the sign is positive is 781 // 782 // (1) (X0 x X1) . (|C|(A x B) + |A|(B x C) + |B|(C x A)) > 0 783 // 784 // where |A| denotes A.Norm() and the expression after the "." represents 785 // the circumcenter of triangle ABC. (This predicate is terrible from a 786 // numerical accuracy point of view, but that doesn't matter since we are 787 // going to use exact arithmetic.) This predicate also assumes that 788 // triangle ABC is CCW (positive sign); we correct for that below. 789 // 790 // The only problem with evaluating this inequality is that computing |A|, 791 // |B| and |C| requires square roots. To avoid this problem we use the 792 // standard technique of rearranging the inequality to isolate at least one 793 // square root and then squaring both sides. We need to repeat this process 794 // twice in order to eliminate all the square roots, which leads to a 795 // polynomial predicate of degree 20 in the input arguments. 796 // 797 // Rearranging (1) we get 798 // 799 // (X0 x X1) . (|C|(A x B) + |A|(B x C)) > |B|(X0 x X1) . (A x C) 800 // 801 // Before squaring we need to check the sign of each side. If the signs are 802 // different then we know the result without squaring, and if the signs are 803 // both negative then after squaring both sides we need to invert the 804 // result. Define 805 // 806 // dAB = (X0 x X1) . (A x B) 807 // dBC = (X0 x X1) . (B x C) 808 // dCA = (X0 x X1) . (C x A) 809 // 810 // Then we can now write the inequality above as 811 // 812 // (2) |C| dAB + |A| dBC > -|B| dCA 813 // 814 // The RHS of (2) is positive if dCA < 0, and the LHS of (2) is positive if 815 // (|C| dAB + |A| dBC) > 0. Since the LHS has square roots, we need to 816 // eliminate them using the same process. Rewriting the LHS as 817 // 818 // (3) |C| dAB > -|A| dBC 819 // 820 // we again need to check the signs of both sides. Let's start with that. 821 // We also precompute the following values because they are used repeatedly 822 // when squaring various expressions below: 823 // 824 // abc2 = |A|^2 dBC^2 825 // bca2 = |B|^2 dCA^2 826 // cab2 = |C|^2 dAB^2 827 Vector3_xf nx = x0.crossProd(x1); 828 ExactFloat dab = nx.dotProd(a.crossProd(b)); 829 ExactFloat dbc = nx.dotProd(b.crossProd(c)); 830 ExactFloat dca = nx.dotProd(c.crossProd(a)); 831 ExactFloat abc2 = a.norm2() * (dbc * dbc); 832 ExactFloat bca2 = b.norm2() * (dca * dca); 833 ExactFloat cab2 = c.norm2() * (dab * dab); 834 835 // If the two sides of (3) have different signs (including the case where 836 // one side is zero) then we know the result. Also, if both sides are zero 837 // then we know the result. The following logic encodes this. 838 int lhs3_sgn = dab.sign(); 839 int rhs3_sgn = -dbc.sign(); 840 int lhs2_sgn = algorithm.max(-1, algorithm.min(1, lhs3_sgn - rhs3_sgn)); 841 if (lhs2_sgn == 0 && lhs3_sgn != 0) { 842 // Both sides of (3) have the same non-zero sign, so square both sides. 843 // If both sides were negative then invert the result. 844 lhs2_sgn = (cab2 - abc2).sign() * lhs3_sgn; 845 } 846 // Now if the two sides of (2) have different signs then we know the result 847 // of this entire function. 848 int rhs2_sgn = -dca.sign(); 849 int result = algorithm.max(-1, algorithm.min(1, lhs2_sgn - rhs2_sgn)); 850 if (result == 0 && lhs2_sgn != 0) { 851 // Both sides of (2) have the same non-zero sign, so square both sides. 852 // (If both sides were negative then we invert the result below.) 853 // This gives 854 // 855 // |C|^2 dAB^2 + |A|^2 dBC^2 + 2 |A| |C| dAB dBC > |B|^2 dCA^2 856 // 857 // This expression still has square roots (|A| and |C|), so we rewrite as 858 // 859 // (4) 2 |A| |C| dAB dBC > |B|^2 dCA^2 - |C|^2 dAB^2 - |A|^2 dBC^2 . 860 // 861 // Again, if the two sides have different signs then we know the result. 862 int lhs4_sgn = dab.sign() * dbc.sign(); 863 ExactFloat rhs4 = bca2 - cab2 - abc2; 864 result = algorithm.max(-1, algorithm.min(1, lhs4_sgn - rhs4.sign())); 865 if (result == 0 && lhs4_sgn != 0) { 866 // Both sides of (4) have the same non-zero sign, so square both sides. 867 // If both sides were negative then invert the result. 868 result = (4 * abc2 * cab2 - rhs4 * rhs4).sign() * lhs4_sgn; 869 } 870 // Correct the sign if both sides of (2) were negative. 871 result *= lhs2_sgn; 872 } 873 // If the sign of triangle ABC is negative, then we have computed -Z and the 874 // result should be negated. 875 return abc_sign * result; 876 } 877 878 /** 879 * Like Sign, except this method does not use symbolic perturbations when 880 * the input points are exactly coplanar with the origin (i.e., linearly 881 * dependent). Clients should never use this method, but it is useful here in 882 * order to implement the combined pedestal/axis-aligned perturbation scheme 883 * used by some methods (such as EdgeCircumcenterSign). 884 */ 885 int unperturbedSign(in S2Point a, in S2Point b, in S2Point c) { 886 int sign = triageSign(a, b, c, a.crossProd(b)); 887 if (sign == 0) sign = expensiveSign(a, b, c, false /*perturb*/); 888 return sign; 889 } 890 891 /** 892 * Given arguments such that ExactEdgeCircumcenterSign(x0, x1, a, b, c) == 0, 893 * returns the value of Sign(X0, X1, Z) (where Z is the circumcenter of 894 * triangle ABC) after symbolic perturbations are taken into account. The 895 * result is zero only if X0 == X1, A == B, B == C, or C == A. (It is nonzero 896 * if these pairs are exactly proportional to each other but not equal.) 897 */ 898 int symbolicEdgeCircumcenterSign( 899 in S2Point x0, in S2Point x1, 900 in S2Point a_arg, in S2Point b_arg, in S2Point c_arg) { 901 // We use the same perturbation strategy as SymbolicCompareDistances. Note 902 // that pedestal perturbations of X0 and X1 do not affect the result, 903 // because Sign(X0, X1, Z) does not change when its arguments are scaled 904 // by a positive factor. Therefore we only need to consider A, B, C. 905 // Suppose that A is the smallest lexicographically and therefore has the 906 // largest perturbation. This has the effect of perturbing the circumcenter 907 // of ABC slightly towards A, and since the circumcenter Z was previously 908 // exactly collinear with edge X, this implies that after the perturbation 909 // Sign(X0, X1, Z) == UnperturbedSign(X0, X1, A). (We want the result 910 // to be zero if X0, X1, and A are linearly dependent, rather than using 911 // symbolic perturbations, because these perturbations are defined to be 912 // much, much smaller than the pedestal perturbation of B and C that are 913 // considered below.) 914 // 915 // If A is also exactly collinear with edge X, then we move on to the next 916 // smallest point lexicographically out of {B, C}. It is easy to see that 917 // as long as A, B, C are all distinct, one of these three Sign calls 918 // will be nonzero, because if A, B, C are all distinct and collinear with 919 // edge X then their circumcenter Z coincides with the normal of X, and 920 // therefore Sign(X0, X1, Z) is nonzero. 921 // 922 // This function could be extended to handle the case where X0 and X1 are 923 // linearly dependent as follows. First, suppose that every point has both 924 // a pedestal peturbation as described above, and also the three 925 // axis-aligned perturbations described in the "Simulation of Simplicity" 926 // paper, where all pedestal perturbations are defined to be much, much 927 // larger than any axis-aligned perturbation. Note that since pedestal 928 // perturbations have no effect on Sign, we can use this model for *all* 929 // the S2 predicates, which ensures that all the various predicates are 930 // fully consistent with each other. 931 // 932 // With this model, the strategy described above yields the correct result 933 // unless X0 and X1 are exactly linearly dependent. When that happens, then 934 // no perturbation (pedestal or axis-aligned) of A,B,C affects the result, 935 // and no pedestal perturbation of X0 or X1 affects the result, therefore we 936 // need to consider the smallest axis-aligned perturbation of X0 or X1. The 937 // first perturbation that makes X0 and X1 linearly independent yields the 938 // result. Supposing that X0 < X1, this is the perturbation of X0[2] unless 939 // both points are multiples of [0, 0, 1], in which case it is the 940 // perturbation of X0[1]. The sign test can be implemented by computing the 941 // perturbed cross product of X0 and X1 and taking the dot product with the 942 // exact value of Z. For example if X0[2] is perturbed, the perturbed cross 943 // product is proportional to (0, 0, 1) x X1 = (-X1[1], x1[0], 0). Note 944 // that if the dot product with Z is exactly zero, then it is still 945 // necessary to fall back to pedestal perturbations of A, B, C, but one of 946 // these perturbations is now guaranteed to succeed. 947 948 // If any two triangle vertices are equal, the result is zero. 949 if (a_arg == b_arg || b_arg == c_arg || c_arg == a_arg) return 0; 950 951 // Sort A, B, C in lexicographic order. 952 const(S2Point)* a = &a_arg; 953 const(S2Point)* b = &b_arg; 954 const(S2Point)* c = &c_arg; 955 if (*b < *a) algorithm.swap(a, b); 956 if (*c < *b) algorithm.swap(b, c); 957 if (*b < *a) algorithm.swap(a, b); 958 959 // Now consider the perturbations in decreasing order of size. 960 int sign = unperturbedSign(x0, x1, *a); 961 if (sign != 0) return sign; 962 sign = unperturbedSign(x0, x1, *b); 963 if (sign != 0) return sign; 964 return unperturbedSign(x0, x1, *c); 965 } 966 967 enum Excluded { FIRST, SECOND, NEITHER, UNCERTAIN } 968 969 package Excluded triageVoronoiSiteExclusion(T)( 970 in Vector!(T, 3) a, in Vector!(T, 3) b, 971 in Vector!(T, 3) x0, in Vector!(T, 3) x1, 972 T r2) { 973 const T T_ERR = roundingEpsilon!T(); 974 975 // Define the "coverage disc" of a site S to be the disc centered at S with 976 // radius r (i.e., squared chord angle length r2). Similarly, define the 977 // "coverage interval" of S along an edge X to be the intersection of X with 978 // the coverage disc of S. The coverage interval can be represented as the 979 // point at the center of the interval and an angle that measures the 980 // semi-width or "radius" of the interval. 981 // 982 // To test whether site A excludes site B along the input edge X, we test 983 // whether the coverage interval of A contains the coverage interval of B. 984 // Let "ra" and "rb" be the radii (semi-widths) of the two intervals, and 985 // let "d" be the angle between their center points. Then "a" properly 986 // contains "b" if (ra - rb > d), and "b" contains "a" if (rb - ra > d). 987 // Note that only one of these conditions can be true. Therefore we can 988 // determine whether one site excludes the other by checking whether 989 // 990 // (1) |rb - ra| > d 991 // 992 // and use the sign of (rb - ra) to determine which site is excluded. 993 // 994 // The actual code is based on the following. Let A1 and B1 be the unit 995 // vectors A and B scaled by cos(r) (these points are inside the sphere). 996 // The planes perpendicular to OA1 and OA2 cut off two discs of radius r 997 // around A and B. Now consider the two lines (inside the sphere) where 998 // these planes intersect the plane containing the input edge X, and let A2 999 // and B2 be the points on these lines that are closest to A and B. The 1000 // coverage intervals of A and B can be represented as an interval along 1001 // each of these lines, centered at A2 and B2. Let P1 and P2 be the 1002 // endpoints of the coverage interval for A, and let Q1 and Q2 be the 1003 // endpoints of the coverage interval for B. We can view each coverage 1004 // interval as either a chord through the sphere's interior, or as a segment 1005 // of the original edge X (by projecting the chord onto the sphere's 1006 // surface). 1007 // 1008 // To check whether B's interval is contained by A's interval, we test 1009 // whether both endpoints of B's interval (Q1 and Q2) are contained by A's 1010 // interval. E.g., we could test whether Qi.DotProd(A2) > A2.Norm2(). 1011 // 1012 // However rather than constructing the actual points A1, A2, and so on, it 1013 // turns out to be more efficient to compute the sines and cosines 1014 // ("components") of the various angles and then use trigonometric 1015 // identities. Predicate (1) can be expressed as 1016 // 1017 // |sin(rb - ra)| > sin(d) 1018 // 1019 // provided that |d| <= Pi/2 (which must be checked), and then expanded to 1020 // 1021 // (2) |sin(rb) cos(ra) - sin(ra) cos(rb)| > sin(d) . 1022 // 1023 // The components of the various angles can be expressed using dot and cross 1024 // products based on the construction above: 1025 // 1026 // sin(ra) = sqrt(sin^2(r) |a|^2 |n|^2 - |a.n|^2) / |aXn| 1027 // cos(ra) = cos(r) |a| |n| / |aXn| 1028 // sin(rb) = sqrt(sin^2(r) |b|^2 |n|^2 - |b.n|^2) / |bXn| 1029 // cos(rb) = cos(r) |b| |n| / |bXn| 1030 // sin(d) = (aXb).n |n| / (|aXn| |bXn|) 1031 // cos(d) = (aXn).(bXn) / (|aXn| |bXn|) 1032 // 1033 // Also, the squared chord length r2 is equal to 4 * sin^2(r / 2), which 1034 // yields the following relationships: 1035 // 1036 // sin(r) = sqrt(r2 (1 - r2 / 4)) 1037 // cos(r) = 1 - r2 / 2 1038 // 1039 // We then scale both sides of (2) by |aXn| |bXn| / |n| (in order to 1040 // minimize the number of calculations and to avoid divisions), which gives: 1041 // 1042 // cos(r) ||a| sqrt(sin^2(r) |b|^2 |n|^2 - |b.n|^2) - 1043 // |b| sqrt(sin^2(r) |a|^2 |n|^2 - |a.n|^2)| > (aXb).n 1044 // 1045 // Furthermore we can substitute |a| = |b| = 1 (as long as this is taken 1046 // into account in the error bounds), yielding 1047 // 1048 // (3) cos(r) |sqrt(sin^2(r) |n|^2 - |b.n|^2) - 1049 // sqrt(sin^2(r) |n|^2 - |a.n|^2)| > (aXb).n 1050 // 1051 // The code below is more complicated than this because many expressions 1052 // have been modified for better numerical stability. For example, dot 1053 // products between unit vectors are computed using (x - y).DotProd(x + y), 1054 // and the dot product between a point P and the normal N of an edge X is 1055 // measured using (P - Xi).DotProd(N) where Xi is the endpoint of X that is 1056 // closer to P. 1057 1058 Vector3!T n = (x0 - x1).crossProd(x0 + x1); // 2 * x0.CrossProd(x1) 1059 T n2 = n.norm2(); 1060 T n1 = math.sqrt(n2); 1061 // This factor is used in the error terms of dot products with "n" below. 1062 T Dn_error = ((3.5 + 2 * math.sqrt(3.0)) * n1 + 32 * math.sqrt(3.0) * DBL_ERR) * T_ERR; 1063 1064 T cos_r = 1 - 0.5 * r2; 1065 T sin2_r = r2 * (1 - 0.25 * r2); 1066 T n2sin2_r = n2 * sin2_r; 1067 1068 // "ra" and "rb" denote sin(ra) and sin(rb) after the scaling above. 1069 T ax2; 1070 T aDn = (a - getClosestVertex(a, x0, x1, ax2)).dotProd(n); 1071 T aDn2 = aDn * aDn; 1072 T aDn_error = Dn_error * math.sqrt(ax2); 1073 T ra2 = n2sin2_r - aDn2; 1074 T ra2_error = (8 * DBL_ERR + 4 * T_ERR) * aDn2 1075 + (2 * math.fabs(aDn) + aDn_error) * aDn_error + 6 * T_ERR * n2sin2_r; 1076 // This is the minimum possible value of ra2, which is used to bound the 1077 // derivative of sqrt(ra2) in computing ra_error below. 1078 T min_ra2 = ra2 - ra2_error; 1079 if (min_ra2 < 0) return Excluded.UNCERTAIN; 1080 T ra = math.sqrt(ra2); 1081 // Includes the ra2 subtraction error above. 1082 T ra_error = 1.5 * T_ERR * ra + 0.5 * ra2_error / math.sqrt(min_ra2); 1083 1084 T bx2; 1085 T bDn = (b - getClosestVertex(b, x0, x1, bx2)).dotProd(n); 1086 T bDn2 = bDn * bDn; 1087 T bDn_error = Dn_error * math.sqrt(bx2); 1088 T rb2 = n2sin2_r - bDn2; 1089 T rb2_error = (8 * DBL_ERR + 4 * T_ERR) * bDn2 + 1090 (2 * math.fabs(bDn) + bDn_error) * bDn_error + 6 * T_ERR * n2sin2_r; 1091 T min_rb2 = rb2 - rb2_error; 1092 if (min_rb2 < 0) return Excluded.UNCERTAIN; 1093 T rb = math.sqrt(rb2); 1094 // Includes the rb2 subtraction error above. 1095 T rb_error = 1.5 * T_ERR * rb + 0.5 * rb2_error / math.sqrt(min_rb2); 1096 1097 // The sign of LHS(3) determines which site may be excluded by the other. 1098 T lhs3 = cos_r * (rb - ra); 1099 T abs_lhs3 = math.fabs(lhs3); 1100 T lhs3_error = cos_r * (ra_error + rb_error) + 3 * T_ERR * abs_lhs3; 1101 1102 // Now we evaluate the RHS of (3), which is proportional to sin(d). 1103 Vector3!T aXb = (a - b).crossProd(a + b); // 2 * a.CrossProd(b) 1104 T aXb1 = aXb.norm(); 1105 T sin_d = 0.5 * aXb.dotProd(n); 1106 T sin_d_error = (4 * DBL_ERR + (2.5 + 2 * math.sqrt(3.0)) * T_ERR) * aXb1 * n1 1107 + 16 * math.sqrt(3.0) * DBL_ERR * T_ERR * (aXb1 + n1); 1108 1109 // If LHS(3) is definitely less than RHS(3), neither site excludes the other. 1110 T result = abs_lhs3 - sin_d; 1111 T result_error = lhs3_error + sin_d_error; 1112 if (result < -result_error) return Excluded.NEITHER; 1113 1114 // Otherwise, before proceeding further we need to check that |d| <= Pi/2. 1115 // In fact, |d| < Pi/2 is enough because of the requirement that r < Pi/2. 1116 // The following expression represents cos(d) after scaling; it is 1117 // equivalent to (aXn).(bXn) but has about 30% less error. 1118 T cos_d = a.dotProd(b) * n2 - aDn * bDn; 1119 T cos_d_error = 1120 ((8 * DBL_ERR + 5 * T_ERR) * math.fabs(aDn) + aDn_error) * math.fabs(bDn) 1121 + (math.fabs(aDn) + aDn_error) * bDn_error + (8 * DBL_ERR + 8 * T_ERR) * n2; 1122 if (cos_d <= -cos_d_error) return Excluded.NEITHER; 1123 1124 // Potential optimization: if the sign of cos(d) is uncertain, then instead 1125 // we could check whether cos(d) >= cos(r). Unfortunately this is fairly 1126 // expensive since it requires computing denominator |aXn||bXn| of cos(d) 1127 // and the associated error bounds. In any case this case is relatively 1128 // rare so it seems better to punt. 1129 if (cos_d < cos_d_error) return Excluded.UNCERTAIN; 1130 1131 // Normally we have d > 0 because the sites are sorted so that A is closer 1132 // to X0 and B is closer to X1. However if the edge X is longer than Pi/2, 1133 // and the sites A and B are beyond its endpoints, then AB can wrap around 1134 // the sphere in the opposite direction from X. In this situation d < 0 but 1135 // each site is closest to one endpoint of X, so neither excludes the other. 1136 // 1137 // It turns out that this can happen only when the site that is further away 1138 // from edge X is less than 90 degrees away from whichever endpoint of X it 1139 // is closer to. It is provable that if this distance is less than 90 1140 // degrees, then it is also less than r2, and therefore the Voronoi regions 1141 // of both sites intersect the edge. 1142 if (sin_d < -sin_d_error) { 1143 T r90 = S1ChordAngle.right().length2(); 1144 // "ca" is negative if Voronoi region A definitely intersects edge X. 1145 int ca = (lhs3 < -lhs3_error) ? -1 : triageCompareCosDistance(a, x0, r90); 1146 int cb = (lhs3 > lhs3_error) ? -1 : triageCompareCosDistance(b, x1, r90); 1147 if (ca < 0 && cb < 0) return Excluded.NEITHER; 1148 if (ca <= 0 && cb <= 0) return Excluded.UNCERTAIN; 1149 if (abs_lhs3 <= lhs3_error) return Excluded.UNCERTAIN; 1150 } else if (sin_d <= sin_d_error) { 1151 return Excluded.UNCERTAIN; 1152 } 1153 // Now we can finish checking the results of predicate (3). 1154 if (result <= result_error) return Excluded.UNCERTAIN; 1155 enforce(abs_lhs3 > lhs3_error); 1156 return (lhs3 > 0) ? Excluded.FIRST : Excluded.SECOND; 1157 } 1158 1159 Excluded exactVoronoiSiteExclusion( 1160 in Vector3_xf a, in Vector3_xf b, 1161 in Vector3_xf x0, in Vector3_xf x1, 1162 in ExactFloat r2) 1163 in { 1164 assert(!arePointsAntipodal(x0, x1)); 1165 } do { 1166 1167 // Recall that one site excludes the other if 1168 // 1169 // (1) |sin(rb - ra)| > sin(d) 1170 // 1171 // and that the sign of (rb - ra) determines which site is excluded (see the 1172 // comments in TriageVoronoiSiteExclusion). To evaluate this using exact 1173 // arithmetic, we expand this to the same predicate as before: 1174 // 1175 // (2) cos(r) ||a| sqrt(sin^2(r) |b|^2 |n|^2 - |b.n|^2) - 1176 // |b| sqrt(sin^2(r) |a|^2 |n|^2 - |a.n|^2)| > (aXb).n 1177 // 1178 // We also need to verify that d <= Pi/2, which is implemented by checking 1179 // that sin(d) >= 0 and cos(d) >= 0. 1180 // 1181 // To eliminate the square roots we use the standard technique of 1182 // rearranging the inequality to isolate at least one square root and then 1183 // squaring both sides. We need to repeat this process twice in order to 1184 // eliminate all the square roots, which leads to a polynomial predicate of 1185 // degree 20 in the input arguments (i.e., degree 4 in each of "a", "b", 1186 // "x0", "x1", and "r2"). 1187 // 1188 // Before squaring we need to check the sign of each side. We also check 1189 // the condition that cos(d) >= 0. Given what else we need to compute, it 1190 // is cheaper use the identity (aXn).(bXn) = (a.b) |n|^2 - (a.n)(b.n) . 1191 Vector3_xf n = x0.crossProd(x1); 1192 ExactFloat n2 = n.norm2(); 1193 ExactFloat aDn = a.dotProd(n); 1194 ExactFloat bDn = b.dotProd(n); 1195 ExactFloat cos_d = a.dotProd(b) * n2 - aDn * bDn; 1196 if (cos_d.sign() < 0) return Excluded.NEITHER; 1197 1198 // Otherwise we continue evaluating the LHS of (2), defining 1199 // sa = |b| sqrt(sin^2(r) |a|^2 |n|^2 - |a.n|^2) 1200 // sb = |a| sqrt(sin^2(r) |b|^2 |n|^2 - |b.n|^2) . 1201 // The sign of the LHS of (2) (before taking the absolute value) determines 1202 // which coverage interval is larger and therefore which site is potentially 1203 // being excluded. 1204 ExactFloat a2 = a.norm2(); 1205 ExactFloat b2 = b.norm2(); 1206 ExactFloat n2sin2_r = r2 * (1 - 0.25 * r2) * n2; 1207 ExactFloat sa2 = b2 * (n2sin2_r * a2 - aDn * aDn); 1208 ExactFloat sb2 = a2 * (n2sin2_r * b2 - bDn * bDn); 1209 int lhs2_sgn = (sb2 - sa2).sign(); 1210 1211 // If the RHS of (2) is negative (corresponding to sin(d) < 0), then we need 1212 // to consider the possibility that the edge AB wraps around the sphere in 1213 // the opposite direction from edge X, with the result that neither site 1214 // excludes the other (see TriageVoronoiSiteExclusion). 1215 ExactFloat rhs2 = a.crossProd(b).dotProd(n); 1216 int rhs2_sgn = rhs2.sign(); 1217 if (rhs2_sgn < 0) { 1218 ExactFloat r90 = S1ChordAngle.right().length2(); 1219 int ca = (lhs2_sgn < 0) ? -1 : exactCompareDistance(a, x0, r90); 1220 int cb = (lhs2_sgn > 0) ? -1 : exactCompareDistance(b, x1, r90); 1221 if (ca <= 0 && cb <= 0) return Excluded.NEITHER; 1222 enforce(ca != 1 || cb != 1); 1223 return ca == 1 ? Excluded.FIRST : Excluded.SECOND; 1224 } 1225 if (lhs2_sgn == 0) { 1226 // If the RHS of (2) is zero as well (i.e., d == 0) then both sites are 1227 // equidistant from every point on edge X. This case requires symbolic 1228 // perturbations, but it should already have been handled in 1229 // GetVoronoiSiteExclusion() (see the call to CompareDistances). 1230 return Excluded.NEITHER; 1231 } 1232 // Next we square both sides of (2), yielding 1233 // 1234 // cos^2(r) (sb^2 + sa^2 - 2 sa sb) > (aXb.n)^2 1235 // 1236 // which can be rearranged to give 1237 // 1238 // (3) cos^2(r) (sb^2 + sa^2) - (aXb.n)^2 > 2 cos^2(r) sa sb . 1239 // 1240 // The RHS of (3) is always non-negative, but we still need to check the 1241 // sign of the LHS. 1242 ExactFloat cos_r = 1 - 0.5 * r2; 1243 ExactFloat cos2_r = cos_r * cos_r; 1244 ExactFloat lhs3 = cos2_r * (sa2 + sb2) - rhs2 * rhs2; 1245 if (lhs3.sign() < 0) return Excluded.NEITHER; 1246 1247 // Otherwise we square both sides of (3) to obtain: 1248 // 1249 // (4) LHS(3)^2 > 4 cos^4(r) sa^2 sb^2 1250 ExactFloat lhs4 = lhs3 * lhs3; 1251 ExactFloat rhs4 = 4 * cos2_r * cos2_r * sa2 * sb2; 1252 int result = (lhs4 - rhs4).sign(); 1253 if (result < 0) return Excluded.NEITHER; 1254 if (result == 0) { 1255 // We have |rb - ra| = d and d > 0. This implies that one coverage 1256 // interval contains the other, but not properly: the two intervals share 1257 // a common endpoint. The distance from each site to that point is 1258 // exactly "r", therefore we need to use symbolic perturbations. Recall 1259 // that site A is considered closer to an equidistant point if and only if 1260 // A > B. Therefore if (rb > ra && A > B) or (ra > rb && B > A) then each 1261 // site is closer to at least one point and neither site is excluded. 1262 // 1263 // Ideally this logic would be in a separate SymbolicVoronoiSiteExclusion 1264 // method for better testing, but this is not convenient because it needs 1265 // lhs_sgn (which requires exact arithmetic to compute). 1266 if ((lhs2_sgn > 0) == (a > b)) return Excluded.NEITHER; 1267 } 1268 // At this point we know that one of the two sites is excluded. The sign of 1269 // the LHS of (2) (before the absolute value) determines which one. 1270 return (lhs2_sgn > 0) ? Excluded.FIRST : Excluded.SECOND; 1271 } 1272 1273 /** 1274 * This is a specialized method that is used to compute the intersection of an 1275 * edge X with the Voronoi diagram of a set of points, where each Voronoi 1276 * region is intersected with a disc of fixed radius "r". 1277 * 1278 * Given two sites A and B and an edge (X0, X1) such that d(A,X0) < d(B,X0) 1279 * and both sites are within the given distance "r" of edge X, this method 1280 * intersects the Voronoi region of each site with a disc of radius r and 1281 * determines whether either region has an empty intersection with edge X. It 1282 * returns FIRST if site A has an empty intersection, SECOND if site B has an 1283 * empty intersection, NEITHER if neither site has an empty intersection, or 1284 * UNCERTAIN if A == B exactly. Note that it is not possible for both 1285 * intersections to be empty because of the requirement that both sites are 1286 * within distance r of edge X. (For example, the only reason that Voronoi 1287 * region A can have an empty intersection with X is that site B is closer to 1288 * all points on X that are within radius r of site A.) 1289 * 1290 * The result is determined with respect to the positions of all points as 1291 * though they were projected to lie exactly on the surface of the unit 1292 * sphere. Furthermore this method uses symbolic perturbations to compute a 1293 * consistent non-zero result even when A and B lie on opposite sides of X 1294 * such that the Voronoi edge between them exactly coincides with edge X, or 1295 * when A and B are distinct but project to the same point on the sphere 1296 * (i.e., they are linearly dependent). 1297 * 1298 * REQUIRES: r < S1ChordAngle::Right() (90 degrees) 1299 * REQUIRES: s2pred::CompareDistances(x0, a, b) < 0 1300 * REQUIRES: s2pred::CompareEdgeDistance(a, x0, x1, r) <= 0 1301 * REQUIRES: s2pred::CompareEdgeDistance(b, x0, x1, r) <= 0 1302 * REQUIRES: X0 and X1 do not project to antipodal points (e.g., X0 == -X1) 1303 * (see comments in CompareEdgeDistance). 1304 */ 1305 Excluded getVoronoiSiteExclusion( 1306 in S2Point a, in S2Point b, 1307 in S2Point x0, in S2Point x1, 1308 S1ChordAngle r) 1309 in { 1310 import std.conv; 1311 assert(r < S1ChordAngle.right(), "r=" ~ r.toString()); 1312 //debug assert(compareDistances(x0, a, b) < 0, "x0=", x0, ", a=", a, ", b=", b); // (implies a != b) 1313 assert(compareEdgeDistance(a, x0, x1, r) <= 0); 1314 assert(compareEdgeDistance(b, x0, x1, r) <= 0); 1315 // Check that the edge does not consist of antipodal points. (This catches 1316 // the most common case -- the full test is in ExactVoronoiSiteExclusion.) 1317 assert(x0 != -x1); 1318 } do { 1319 1320 // If one site is closer than the other to both endpoints of X, then it is 1321 // closer to every point on X. Note that this also handles the case where A 1322 // and B are equidistant from every point on X (i.e., X is the perpendicular 1323 // bisector of AB), because CompareDistances uses symbolic perturbations to 1324 // ensure that either A or B is considered closer (in a consistent way). 1325 // This also ensures that the choice of A or B does not depend on the 1326 // direction of X. 1327 if (compareDistances(x1, a, b) < 0) { 1328 return Excluded.SECOND; // Site A is closer to every point on X. 1329 } 1330 1331 Excluded result = triageVoronoiSiteExclusion(a, b, x0, x1, r.length2()); 1332 if (result != Excluded.UNCERTAIN) return result; 1333 1334 result = triageVoronoiSiteExclusion( 1335 Vector3_r.from(a), Vector3_r.from(b), Vector3_r.from(x0), Vector3_r.from(x1), r.length2()); 1336 if (result != Excluded.UNCERTAIN) return result; 1337 1338 return exactVoronoiSiteExclusion(Vector3_xf.from(a), Vector3_xf.from(b), Vector3_xf.from(x0), 1339 Vector3_xf.from(x1), ExactFloat(r.length2())); 1340 } 1341 1342 package int triageCompareEdgeDirections(T)( 1343 in Vector!(T, 3) a0, in Vector!(T, 3) a1, 1344 in Vector!(T, 3) b0, in Vector!(T, 3) b1) { 1345 T T_ERR = roundingEpsilon!T(); 1346 Vector3!T na = (a0 - a1).crossProd(a0 + a1); 1347 Vector3!T nb = (b0 - b1).crossProd(b0 + b1); 1348 T na_len = na.norm(), nb_len = nb.norm(); 1349 T cos_ab = na.dotProd(nb); 1350 T cos_ab_error = ((5 + 4 * math.sqrt(3.0)) * na_len * nb_len + 1351 32 * math.sqrt(3.0) * DBL_ERR * (na_len + nb_len)) * T_ERR; 1352 return (cos_ab > cos_ab_error) ? 1 : (cos_ab < -cos_ab_error) ? -1 : 0; 1353 } 1354 1355 private bool arePointsLinearlyDependent(in Vector3_xf x, in Vector3_xf y) { 1356 Vector3_xf n = x.crossProd(y); 1357 return n[0].sign() == 0 && n[1].sign() == 0 && n[2].sign() == 0; 1358 } 1359 1360 private bool arePointsAntipodal(in Vector3_xf x, in Vector3_xf y) { 1361 return arePointsLinearlyDependent(x, y) && x.dotProd(y).sign() < 0; 1362 } 1363 1364 package int exactCompareEdgeDirections( 1365 in Vector3_xf a0, in Vector3_xf a1, 1366 in Vector3_xf b0, in Vector3_xf b1) 1367 in { 1368 assert(!arePointsAntipodal(a0, a1)); 1369 assert(!arePointsAntipodal(b0, b1)); 1370 } do { 1371 return a0.crossProd(a1).dotProd(b0.crossProd(b1)).sign(); 1372 } 1373 1374 /** 1375 * Returns Sign(X0, X1, Z) where Z is the circumcenter of triangle ABC. 1376 * The return value is -1 if Z is to the left of edge X, and +1 if Z is to the 1377 * right of edge X. The return value is zero if A == B, B == C, or C == A 1378 * (exactly), and also if X0 and X1 project to identical points on the sphere 1379 * (e.g., X0 == X1). 1380 * 1381 * The result is determined with respect to the positions of all points as 1382 * though they were projected to lie exactly on the surface of the unit 1383 * sphere. Furthermore this method uses symbolic perturbations to compute a 1384 * consistent non-zero result even when Z lies exactly on edge X. 1385 * 1386 * REQUIRES: X0 and X1 do not project to antipodal points (e.g., X0 == -X1) 1387 * (see comments in CompareEdgeDistance). 1388 */ 1389 int edgeCircumcenterSign( 1390 in S2Point x0, in S2Point x1, in S2Point a, in S2Point b, in S2Point c) 1391 in { 1392 // Check that the edge does not consist of antipodal points. (This catches 1393 // the most common case -- the full test is in ExactEdgeCircumcenterSign.) 1394 assert(x0 != -x1); 1395 } do { 1396 int abc_sign = sign(a, b, c); 1397 int sign = triageEdgeCircumcenterSign(x0, x1, a, b, c, abc_sign); 1398 if (sign != 0) { 1399 return sign; 1400 } 1401 1402 // Optimization for the cases that are going to return zero anyway, in order 1403 // to avoid falling back to exact arithmetic. 1404 if (x0 == x1 || a == b || b == c || c == a) { 1405 return 0; 1406 } 1407 1408 sign = triageEdgeCircumcenterSign( 1409 Vector3_r.from(x0), Vector3_r.from(x1), Vector3_r.from(a), Vector3_r.from(b), 1410 Vector3_r.from(c), abc_sign); 1411 if (sign != 0) { 1412 return sign; 1413 } 1414 sign = exactEdgeCircumcenterSign( 1415 Vector3_xf.from(x0), Vector3_xf.from(x1), Vector3_xf.from(a), Vector3_xf.from(b), 1416 Vector3_xf.from(c), abc_sign); 1417 if (sign != 0) { 1418 return sign; 1419 } 1420 1421 // Unlike the other methods, SymbolicEdgeCircumcenterSign does not depend 1422 // on the sign of triangle ABC. 1423 return symbolicEdgeCircumcenterSign(x0, x1, a, b, c); 1424 } 1425 1426 /////////////////////////// Low-Level Methods //////////////////////////// 1427 // 1428 // Most clients will not need the following methods. They can be slightly 1429 // more efficient but are harder to use, since they require the client to do 1430 // all the actual crossing tests. 1431 1432 /** 1433 * A more efficient version of Sign that allows the precomputed 1434 * cross-product of A and B to be specified. (Unlike the 3 argument 1435 * version this method is also inlined.) 1436 */ 1437 int sign(in S2Point a, in S2Point b, in S2Point c, in Vector3_d a_cross_b) { 1438 int sign = triageSign(a, b, c, a_cross_b); 1439 if (sign == 0) { 1440 sign = expensiveSign(a, b, c); 1441 } 1442 return sign; 1443 } 1444 1445 /** 1446 * This version of Sign returns +1 if the points are definitely CCW, -1 if 1447 * they are definitely CW, and 0 if two points are identical or the result 1448 * is uncertain. Uncertain cases can be resolved, if desired, by calling 1449 * ExpensiveSign. 1450 * 1451 * The purpose of this method is to allow additional cheap tests to be done, 1452 * where possible, in order to avoid calling ExpensiveSign unnecessarily. 1453 */ 1454 int triageSign(in S2Point a, in S2Point b, in S2Point c, in Vector3_d a_cross_b) { 1455 if (!s2pointutil.isUnitLength(a)) logger.logWarn("Invalid"); 1456 if (!s2pointutil.isUnitLength(b)) logger.logWarn("Invalid"); 1457 if (!s2pointutil.isUnitLength(c)) logger.logWarn("Invalid"); 1458 // MAX_DET_ERROR is the maximum error in computing (AxB).C where all vectors 1459 // are unit length. Using standard inequalities, it can be shown that 1460 // 1461 // fl(AxB) = AxB + D where |D| <= (|AxB| + (2/sqrt(3))*|A|*|B|) * e 1462 // 1463 // where "fl()" denotes a calculation done in floating-point arithmetic, 1464 // |x| denotes either absolute value or the L2-norm as appropriate, and 1465 // e = 0.5*DBL_EPSILON. Similarly, 1466 // 1467 // fl(B.C) = B.C + d where |d| <= (1.5*|B.C| + 1.5*|B|*|C|) * e . 1468 // 1469 // Applying these bounds to the unit-length vectors A,B,C and neglecting 1470 // relative error (which does not affect the sign of the result), we get 1471 // 1472 // fl((AxB).C) = (AxB).C + d where |d| <= (2.5 + 2/sqrt(3)) * e 1473 // 1474 // which is about 3.6548 * e, or 1.8274 * DBL_EPSILON. 1475 const double MAX_DET_ERROR = 1.8274 * double.epsilon; 1476 double det = a_cross_b.dotProd(c); 1477 1478 // Double-check borderline cases in debug mode. 1479 //enforce(math.fabs(det) <= MAX_DET_ERROR || 1480 // math.fabs(det) >= 100 * MAX_DET_ERROR || 1481 // det * expensiveSign(a, b, c) > 0); 1482 1483 if (det > MAX_DET_ERROR) { 1484 return 1; 1485 } 1486 if (det < -MAX_DET_ERROR) { 1487 return -1; 1488 } 1489 return 0; 1490 } 1491 1492 alias Vector3_xf = Vector3!ExactFloat; 1493 1494 /** 1495 * This function is invoked by Sign() if the sign of the determinant is 1496 * uncertain. It always returns a non-zero result unless two of the input 1497 * points are the same. It uses a combination of multiple-precision 1498 * arithmetic and symbolic perturbations to ensure that its results are 1499 * always self-consistent (cf. Simulation of Simplicity, Edelsbrunner and 1500 * Muecke). The basic idea is to assign an infinitesimal symbolic 1501 * perturbation to every possible S2Point such that no three S2Points are 1502 * collinear and no four S2Points are coplanar. These perturbations are so 1503 * small that they do not affect the sign of any determinant that was 1504 * non-zero before the perturbations. If "perturb" is false, then instead 1505 * the exact sign of the unperturbed input points is returned, which can be 1506 * zero even when all three points are distinct. 1507 * 1508 * Unlike Sign(), this method does not require the input points to be 1509 * normalized. 1510 */ 1511 int expensiveSign(in S2Point a, in S2Point b, in S2Point c, bool perturb = true) { 1512 // Return zero if and only if two points are the same. This ensures (1). 1513 if (a == b || b == c || c == a) { 1514 return 0; 1515 } 1516 1517 // Next we try recomputing the determinant still using floating-point 1518 // arithmetic but in a more precise way. This is more expensive than the 1519 // simple calculation done by TriageSign(), but it is still *much* cheaper 1520 // than using arbitrary-precision arithmetic. This optimization is able to 1521 // compute the correct determinant sign in virtually all cases except when 1522 // the three points are truly collinear (e.g., three points on the equator). 1523 int det_sign = stableSign(a, b, c); 1524 if (det_sign != 0) { 1525 return det_sign; 1526 } 1527 1528 // TODO(ericv): Create a templated version of StableSign so that we can 1529 // retry in "long double" precision before falling back to ExactFloat. 1530 1531 // TODO(ericv): Optimize ExactFloat so that it stores up to 32 bytes of 1532 // mantissa inline (without requiring memory allocation). 1533 1534 // Otherwise fall back to exact arithmetic and symbolic permutations. 1535 return exactSign(a, b, c, perturb); 1536 } 1537 1538 //// 1539 // Private methods for internal implementation. 1540 //// 1541 1542 package int exactSign(in S2Point a, in S2Point b, in S2Point c, bool perturb) 1543 in { 1544 assert(a != b && b != c && c != a); 1545 } do { 1546 // Sort the three points in lexicographic order, keeping track of the sign 1547 // of the permutation. (Each exchange inverts the sign of the determinant.) 1548 int perm_sign = 1; 1549 const(S2Point)* pa = &a; 1550 const(S2Point)* pb = &b; 1551 const(S2Point)* pc = &c; 1552 1553 if (*pa > *pb) { 1554 algorithm.swap(pa, pb); 1555 perm_sign = -perm_sign; 1556 } 1557 if (*pb > *pc) { 1558 algorithm.swap(pb, pc); 1559 perm_sign = -perm_sign; 1560 } 1561 if (*pa > *pb) { 1562 algorithm.swap(pa, pb); 1563 perm_sign = -perm_sign; 1564 } 1565 enforce(*pa < *pb && *pb < *pc); 1566 1567 // Construct multiple-precision versions of the sorted points and compute 1568 // their exact 3x3 determinant. 1569 Vector3_xf xa = Vector3_xf.from(*pa); 1570 Vector3_xf xb = Vector3_xf.from(*pb); 1571 Vector3_xf xc = Vector3_xf.from(*pc); 1572 Vector3_xf xb_cross_xc = xb.crossProd(xc); 1573 ExactFloat det = xa.dotProd(xb_cross_xc); 1574 1575 // The precision of ExactFloat is high enough that the result should always 1576 // be exact (no rounding was performed). 1577 enforce(!det.isNan()); 1578 enforce(det.prec() < det.maxPrec()); 1579 1580 // If the exact determinant is non-zero, we're done. 1581 int det_sign = det.sign(); 1582 if (det_sign == 0 && perturb) { 1583 // Otherwise, we need to resort to symbolic perturbations to resolve the 1584 // sign of the determinant. 1585 det_sign = symbolicallyPerturbedSign(xa, xb, xc, xb_cross_xc); 1586 enforce(0 != det_sign); 1587 } 1588 return perm_sign * det_sign; 1589 } 1590 1591 package int triageCompareCosDistances(T)( 1592 in Vector!(T, 3) x, in Vector!(T, 3) a, in Vector!(T, 3) b) { 1593 T cos_ax_error, cos_bx_error; 1594 T cos_ax = getCosDistance(a, x, cos_ax_error); 1595 T cos_bx = getCosDistance(b, x, cos_bx_error); 1596 T diff = cos_ax - cos_bx; 1597 T error = cos_ax_error + cos_bx_error; 1598 return (diff > error) ? -1 : (diff < -error) ? 1 : 0; 1599 } 1600 1601 package int triageCompareSin2Distances(T)( 1602 in Vector!(T, 3) x, in Vector!(T, 3) a, in Vector!(T, 3) b) { 1603 T sin2_ax_error, sin2_bx_error; 1604 T sin2_ax = getSin2Distance(a, x, sin2_ax_error); 1605 T sin2_bx = getSin2Distance(b, x, sin2_bx_error); 1606 T diff = sin2_ax - sin2_bx; 1607 T error = sin2_ax_error + sin2_bx_error; 1608 return (diff > error) ? 1 : (diff < -error) ? -1 : 0; 1609 } 1610 1611 package int exactCompareDistances(in Vector3_xf x, in Vector3_xf a, in Vector3_xf b) { 1612 // This code produces the same result as though all points were reprojected 1613 // to lie exactly on the surface of the unit sphere. It is based on testing 1614 // whether x.DotProd(a.Normalize()) < x.DotProd(b.Normalize()), reformulated 1615 // so that it can be evaluated using exact arithmetic. 1616 ExactFloat cos_ax = x.dotProd(a); 1617 ExactFloat cos_bx = x.dotProd(b); 1618 // If the two values have different signs, we need to handle that case now 1619 // before squaring them below. 1620 int a_sign = cos_ax.sign(), b_sign = cos_bx.sign(); 1621 if (a_sign != b_sign) { 1622 return (a_sign > b_sign) ? -1 : 1; // If cos(AX) > cos(BX), then AX < BX. 1623 } 1624 ExactFloat cmp = cos_bx * cos_bx * a.norm2() - cos_ax * cos_ax * b.norm2(); 1625 return a_sign * cmp.sign(); 1626 } 1627 1628 // Given three points such that AX == BX (exactly), returns -1, 0, or +1 1629 // according whether AX < BX, AX == BX, or AX > BX after symbolic 1630 // perturbations are taken into account. 1631 package int symbolicCompareDistances(in S2Point x, in S2Point a, in S2Point b) { 1632 // Our symbolic perturbation strategy is based on the following model. 1633 // Similar to "simulation of simplicity", we assign a perturbation to every 1634 // point such that if A < B, then the symbolic perturbation for A is much, 1635 // much larger than the symbolic perturbation for B. We imagine that 1636 // rather than projecting every point to lie exactly on the unit sphere, 1637 // instead each point is positioned on its own tiny pedestal that raises it 1638 // just off the surface of the unit sphere. This means that the distance AX 1639 // is actually the true distance AX plus the (symbolic) heights of the 1640 // pedestals for A and X. The pedestals are infinitesmally thin, so they do 1641 // not affect distance measurements except at the two endpoints. If several 1642 // points project to exactly the same point on the unit sphere, we imagine 1643 // that they are placed on separate pedestals placed close together, where 1644 // the distance between pedestals is much, much less than the height of any 1645 // pedestal. (There are a finite number of S2Points, and therefore a finite 1646 // number of pedestals, so this is possible.) 1647 // 1648 // If A < B, then A is on a higher pedestal than B, and therefore AX > BX. 1649 return (a < b) ? 1 : (a > b) ? -1 : 0; 1650 } 1651 1652 // Returns cos(XY), and sets "error" to the maximum error in the result. 1653 // REQUIRES: "x" and "y" satisfy S2::IsNormalized(). 1654 private double getCosDistance(in S2Point x, in S2Point y, out double error) { 1655 double c = x.dotProd(y); 1656 error = 9.5 * DBL_ERR * math.fabs(c) + 1.5 * DBL_ERR; 1657 return c; 1658 } 1659 1660 // A high precision "long double" version of the function above. 1661 private real getCosDistance(in Vector3_r x, in Vector3_r y, out real error) { 1662 // With "long double" precision it is worthwhile to compensate for length 1663 // errors in "x" and "y", since they are only unit length to within the 1664 // precision of "double". (This would also reduce the error constant 1665 // slightly in the method above but is not worth the additional effort.) 1666 real c = x.dotProd(y) / math.sqrt(x.norm2() * y.norm2()); 1667 error = 7 * REAL_ERR * math.fabs(c) + 1.5 * REAL_ERR; 1668 return c; 1669 } 1670 1671 // Returns sin**2(XY), where XY is the angle between X and Y, and sets "error" 1672 // to the maximum error in the result. 1673 // 1674 // REQUIRES: "x" and "y" satisfy S2::IsNormalized(). 1675 private double getSin2Distance(in S2Point x, in S2Point y, out double error) { 1676 // The (x-y).CrossProd(x+y) trick eliminates almost all of error due to "x" 1677 // and "y" being not quite unit length. This method is extremely accurate 1678 // for small distances; the *relative* error in the result is O(DBL_ERR) for 1679 // distances as small as DBL_ERR. 1680 S2Point n = (x - y).crossProd(x + y); 1681 double d2 = 0.25 * n.norm2(); 1682 error = ((21 + 4 * math.sqrt(3.0)) * DBL_ERR * d2 + 1683 32 * math.sqrt(3.0) * DBL_ERR * DBL_ERR * math.sqrt(d2) + 1684 768 * DBL_ERR * DBL_ERR * DBL_ERR * DBL_ERR); 1685 return d2; 1686 } 1687 1688 // A high precision "long double" version of the function above. 1689 private real getSin2Distance(in Vector3_r x, in Vector3_r y, out real error) { 1690 // In "long double" precision it is worthwhile to compensate for length 1691 // errors in "x" and "y", since they are only unit length to within the 1692 // precision of "double". Otherwise the "d2" error coefficient below would 1693 // be (16 * DBL_ERR + (5 + 4 * sqrt(3)) * LD_ERR), which is much larger. 1694 // (Dividing by the squared norms of "x" and "y" would also reduce the error 1695 // constant slightly in the double-precision version, but this is not worth 1696 // the additional effort.) 1697 Vector3_r n = (x - y).crossProd(x + y); 1698 real d2 = 0.25 * n.norm2() / (x.norm2() * y.norm2()); 1699 error = ((13 + 4 * math.sqrt(3.0)) * REAL_ERR * d2 + 1700 32 * math.sqrt(3.0) * DBL_ERR * REAL_ERR * math.sqrt(d2) + 1701 768 * DBL_ERR * DBL_ERR * REAL_ERR * REAL_ERR); 1702 return d2; 1703 } 1704 1705 private int compareSin2Distances(in S2Point x, in S2Point a, in S2Point b) { 1706 int sign = triageCompareSin2Distances(x, a, b); 1707 if (sign != 0) return sign; 1708 return triageCompareSin2Distances(Vector3_r.from(x), Vector3_r.from(a), Vector3_r.from(b)); 1709 } 1710