1 // Copyright 2017 Google Inc. All rights reserved. 2 // 3 // Licensed under the Apache License, Version 2.0 (the "License"); 4 // you may not use this file except in compliance with the License. 5 // You may obtain a copy of the License at 6 // 7 // http://www.apache.org/licenses/LICENSE-2.0 8 // 9 // Unless required by applicable law or agreed to in writing, software 10 // distributed under the License is distributed on an "AS IS" BASIS, 11 // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 // See the License for the specific language governing permissions and 13 // limitations under the License. 14 15 package s2 16 17 import ( 18 "fmt" 19 "math" 20 21 "github.com/golang/geo/r3" 22 "github.com/golang/geo/s1" 23 ) 24 25 const ( 26 // intersectionError can be set somewhat arbitrarily, because the algorithm 27 // uses more precision if necessary in order to achieve the specified error. 28 // The only strict requirement is that intersectionError >= dblEpsilon 29 // radians. However, using a larger error tolerance makes the algorithm more 30 // efficient because it reduces the number of cases where exact arithmetic is 31 // needed. 32 intersectionError = s1.Angle(8 * dblError) 33 34 // intersectionMergeRadius is used to ensure that intersection points that 35 // are supposed to be coincident are merged back together into a single 36 // vertex. This is required in order for various polygon operations (union, 37 // intersection, etc) to work correctly. It is twice the intersection error 38 // because two coincident intersection points might have errors in 39 // opposite directions. 40 intersectionMergeRadius = 2 * intersectionError 41 ) 42 43 // A Crossing indicates how edges cross. 44 type Crossing int 45 46 const ( 47 // Cross means the edges cross. 48 Cross Crossing = iota 49 // MaybeCross means two vertices from different edges are the same. 50 MaybeCross 51 // DoNotCross means the edges do not cross. 52 DoNotCross 53 ) 54 55 func (c Crossing) String() string { 56 switch c { 57 case Cross: 58 return "Cross" 59 case MaybeCross: 60 return "MaybeCross" 61 case DoNotCross: 62 return "DoNotCross" 63 default: 64 return fmt.Sprintf("(BAD CROSSING %d)", c) 65 } 66 } 67 68 // CrossingSign reports whether the edge AB intersects the edge CD. 69 // If AB crosses CD at a point that is interior to both edges, Cross is returned. 70 // If any two vertices from different edges are the same it returns MaybeCross. 71 // Otherwise it returns DoNotCross. 72 // If either edge is degenerate (A == B or C == D), the return value is MaybeCross 73 // if two vertices from different edges are the same and DoNotCross otherwise. 74 // 75 // Properties of CrossingSign: 76 // 77 // (1) CrossingSign(b,a,c,d) == CrossingSign(a,b,c,d) 78 // (2) CrossingSign(c,d,a,b) == CrossingSign(a,b,c,d) 79 // (3) CrossingSign(a,b,c,d) == MaybeCross if a==c, a==d, b==c, b==d 80 // (3) CrossingSign(a,b,c,d) == DoNotCross or MaybeCross if a==b or c==d 81 // 82 // This method implements an exact, consistent perturbation model such 83 // that no three points are ever considered to be collinear. This means 84 // that even if you have 4 points A, B, C, D that lie exactly in a line 85 // (say, around the equator), C and D will be treated as being slightly to 86 // one side or the other of AB. This is done in a way such that the 87 // results are always consistent (see RobustSign). 88 func CrossingSign(a, b, c, d Point) Crossing { 89 crosser := NewChainEdgeCrosser(a, b, c) 90 return crosser.ChainCrossingSign(d) 91 } 92 93 // VertexCrossing reports whether two edges "cross" in such a way that point-in-polygon 94 // containment tests can be implemented by counting the number of edge crossings. 95 // 96 // Given two edges AB and CD where at least two vertices are identical 97 // (i.e. CrossingSign(a,b,c,d) == 0), the basic rule is that a "crossing" 98 // occurs if AB is encountered after CD during a CCW sweep around the shared 99 // vertex starting from a fixed reference point. 100 // 101 // Note that according to this rule, if AB crosses CD then in general CD 102 // does not cross AB. However, this leads to the correct result when 103 // counting polygon edge crossings. For example, suppose that A,B,C are 104 // three consecutive vertices of a CCW polygon. If we now consider the edge 105 // crossings of a segment BP as P sweeps around B, the crossing number 106 // changes parity exactly when BP crosses BA or BC. 107 // 108 // Useful properties of VertexCrossing (VC): 109 // 110 // (1) VC(a,a,c,d) == VC(a,b,c,c) == false 111 // (2) VC(a,b,a,b) == VC(a,b,b,a) == true 112 // (3) VC(a,b,c,d) == VC(a,b,d,c) == VC(b,a,c,d) == VC(b,a,d,c) 113 // (3) If exactly one of a,b equals one of c,d, then exactly one of 114 // VC(a,b,c,d) and VC(c,d,a,b) is true 115 // 116 // It is an error to call this method with 4 distinct vertices. 117 func VertexCrossing(a, b, c, d Point) bool { 118 // If A == B or C == D there is no intersection. We need to check this 119 // case first in case 3 or more input points are identical. 120 if a == b || c == d { 121 return false 122 } 123 124 // If any other pair of vertices is equal, there is a crossing if and only 125 // if OrderedCCW indicates that the edge AB is further CCW around the 126 // shared vertex O (either A or B) than the edge CD, starting from an 127 // arbitrary fixed reference point. 128 129 // Optimization: if AB=CD or AB=DC, we can avoid most of the calculations. 130 switch { 131 case a == c: 132 return (b == d) || OrderedCCW(a.referenceDir(), d, b, a) 133 case b == d: 134 return OrderedCCW(b.referenceDir(), c, a, b) 135 case a == d: 136 return (b == c) || OrderedCCW(a.referenceDir(), c, b, a) 137 case b == c: 138 return OrderedCCW(b.referenceDir(), d, a, b) 139 } 140 141 return false 142 } 143 144 // EdgeOrVertexCrossing is a convenience function that calls CrossingSign to 145 // handle cases where all four vertices are distinct, and VertexCrossing to 146 // handle cases where two or more vertices are the same. This defines a crossing 147 // function such that point-in-polygon containment tests can be implemented 148 // by simply counting edge crossings. 149 func EdgeOrVertexCrossing(a, b, c, d Point) bool { 150 switch CrossingSign(a, b, c, d) { 151 case DoNotCross: 152 return false 153 case Cross: 154 return true 155 default: 156 return VertexCrossing(a, b, c, d) 157 } 158 } 159 160 // Intersection returns the intersection point of two edges AB and CD that cross 161 // (CrossingSign(a,b,c,d) == Crossing). 162 // 163 // Useful properties of Intersection: 164 // 165 // (1) Intersection(b,a,c,d) == Intersection(a,b,d,c) == Intersection(a,b,c,d) 166 // (2) Intersection(c,d,a,b) == Intersection(a,b,c,d) 167 // 168 // The returned intersection point X is guaranteed to be very close to the 169 // true intersection point of AB and CD, even if the edges intersect at a 170 // very small angle. 171 func Intersection(a0, a1, b0, b1 Point) Point { 172 // It is difficult to compute the intersection point of two edges accurately 173 // when the angle between the edges is very small. Previously we handled 174 // this by only guaranteeing that the returned intersection point is within 175 // intersectionError of each edge. However, this means that when the edges 176 // cross at a very small angle, the computed result may be very far from the 177 // true intersection point. 178 // 179 // Instead this function now guarantees that the result is always within 180 // intersectionError of the true intersection. This requires using more 181 // sophisticated techniques and in some cases extended precision. 182 // 183 // - intersectionStable computes the intersection point using 184 // projection and interpolation, taking care to minimize cancellation 185 // error. 186 // 187 // - intersectionExact computes the intersection point using precision 188 // arithmetic and converts the final result back to an Point. 189 pt, ok := intersectionStable(a0, a1, b0, b1) 190 if !ok { 191 pt = intersectionExact(a0, a1, b0, b1) 192 } 193 194 // Make sure the intersection point is on the correct side of the sphere. 195 // Since all vertices are unit length, and edges are less than 180 degrees, 196 // (a0 + a1) and (b0 + b1) both have positive dot product with the 197 // intersection point. We use the sum of all vertices to make sure that the 198 // result is unchanged when the edges are swapped or reversed. 199 if pt.Dot((a0.Add(a1.Vector)).Add(b0.Add(b1.Vector))) < 0 { 200 pt = Point{pt.Mul(-1)} 201 } 202 203 return pt 204 } 205 206 // Computes the cross product of two vectors, normalized to be unit length. 207 // Also returns the length of the cross 208 // product before normalization, which is useful for estimating the amount of 209 // error in the result. For numerical stability, the vectors should both be 210 // approximately unit length. 211 func robustNormalWithLength(x, y r3.Vector) (r3.Vector, float64) { 212 var pt r3.Vector 213 // This computes 2 * (x.Cross(y)), but has much better numerical 214 // stability when x and y are unit length. 215 tmp := x.Sub(y).Cross(x.Add(y)) 216 length := tmp.Norm() 217 if length != 0 { 218 pt = tmp.Mul(1 / length) 219 } 220 return pt, 0.5 * length // Since tmp == 2 * (x.Cross(y)) 221 } 222 223 /* 224 // intersectionSimple is not used by the C++ so it is skipped here. 225 */ 226 227 // projection returns the projection of aNorm onto X (x.Dot(aNorm)), and a bound 228 // on the error in the result. aNorm is not necessarily unit length. 229 // 230 // The remaining parameters (the length of aNorm (aNormLen) and the edge endpoints 231 // a0 and a1) allow this dot product to be computed more accurately and efficiently. 232 func projection(x, aNorm r3.Vector, aNormLen float64, a0, a1 Point) (proj, bound float64) { 233 // The error in the dot product is proportional to the lengths of the input 234 // vectors, so rather than using x itself (a unit-length vector) we use 235 // the vectors from x to the closer of the two edge endpoints. This 236 // typically reduces the error by a huge factor. 237 x0 := x.Sub(a0.Vector) 238 x1 := x.Sub(a1.Vector) 239 x0Dist2 := x0.Norm2() 240 x1Dist2 := x1.Norm2() 241 242 // If both distances are the same, we need to be careful to choose one 243 // endpoint deterministically so that the result does not change if the 244 // order of the endpoints is reversed. 245 var dist float64 246 if x0Dist2 < x1Dist2 || (x0Dist2 == x1Dist2 && x0.Cmp(x1) == -1) { 247 dist = math.Sqrt(x0Dist2) 248 proj = x0.Dot(aNorm) 249 } else { 250 dist = math.Sqrt(x1Dist2) 251 proj = x1.Dot(aNorm) 252 } 253 254 // This calculation bounds the error from all sources: the computation of 255 // the normal, the subtraction of one endpoint, and the dot product itself. 256 // dblError appears because the input points are assumed to be 257 // normalized in double precision. 258 // 259 // For reference, the bounds that went into this calculation are: 260 // ||N'-N|| <= ((1 + 2 * sqrt(3))||N|| + 32 * sqrt(3) * dblError) * epsilon 261 // |(A.B)'-(A.B)| <= (1.5 * (A.B) + 1.5 * ||A|| * ||B||) * epsilon 262 // ||(X-Y)'-(X-Y)|| <= ||X-Y|| * epsilon 263 bound = (((3.5+2*math.Sqrt(3))*aNormLen+32*math.Sqrt(3)*dblError)*dist + 1.5*math.Abs(proj)) * epsilon 264 return proj, bound 265 } 266 267 // compareEdges reports whether (a0,a1) is less than (b0,b1) with respect to a total 268 // ordering on edges that is invariant under edge reversals. 269 func compareEdges(a0, a1, b0, b1 Point) bool { 270 if a0.Cmp(a1.Vector) != -1 { 271 a0, a1 = a1, a0 272 } 273 if b0.Cmp(b1.Vector) != -1 { 274 b0, b1 = b1, b0 275 } 276 return a0.Cmp(b0.Vector) == -1 || (a0 == b0 && b0.Cmp(b1.Vector) == -1) 277 } 278 279 // intersectionStable returns the intersection point of the edges (a0,a1) and 280 // (b0,b1) if it can be computed to within an error of at most intersectionError 281 // by this function. 282 // 283 // The intersection point is not guaranteed to have the correct sign because we 284 // choose to use the longest of the two edges first. The sign is corrected by 285 // Intersection. 286 func intersectionStable(a0, a1, b0, b1 Point) (Point, bool) { 287 // Sort the two edges so that (a0,a1) is longer, breaking ties in a 288 // deterministic way that does not depend on the ordering of the endpoints. 289 // This is desirable for two reasons: 290 // - So that the result doesn't change when edges are swapped or reversed. 291 // - It reduces error, since the first edge is used to compute the edge 292 // normal (where a longer edge means less error), and the second edge 293 // is used for interpolation (where a shorter edge means less error). 294 aLen2 := a1.Sub(a0.Vector).Norm2() 295 bLen2 := b1.Sub(b0.Vector).Norm2() 296 if aLen2 < bLen2 || (aLen2 == bLen2 && compareEdges(a0, a1, b0, b1)) { 297 return intersectionStableSorted(b0, b1, a0, a1) 298 } 299 return intersectionStableSorted(a0, a1, b0, b1) 300 } 301 302 // intersectionStableSorted is a helper function for intersectionStable. 303 // It expects that the edges (a0,a1) and (b0,b1) have been sorted so that 304 // the first edge passed in is longer. 305 func intersectionStableSorted(a0, a1, b0, b1 Point) (Point, bool) { 306 var pt Point 307 308 // Compute the normal of the plane through (a0, a1) in a stable way. 309 aNorm := a0.Sub(a1.Vector).Cross(a0.Add(a1.Vector)) 310 aNormLen := aNorm.Norm() 311 bLen := b1.Sub(b0.Vector).Norm() 312 313 // Compute the projection (i.e., signed distance) of b0 and b1 onto the 314 // plane through (a0, a1). Distances are scaled by the length of aNorm. 315 b0Dist, b0Error := projection(b0.Vector, aNorm, aNormLen, a0, a1) 316 b1Dist, b1Error := projection(b1.Vector, aNorm, aNormLen, a0, a1) 317 318 // The total distance from b0 to b1 measured perpendicularly to (a0,a1) is 319 // |b0Dist - b1Dist|. Note that b0Dist and b1Dist generally have 320 // opposite signs because b0 and b1 are on opposite sides of (a0, a1). The 321 // code below finds the intersection point by interpolating along the edge 322 // (b0, b1) to a fractional distance of b0Dist / (b0Dist - b1Dist). 323 // 324 // It can be shown that the maximum error in the interpolation fraction is 325 // 326 // (b0Dist * b1Error - b1Dist * b0Error) / (distSum * (distSum - errorSum)) 327 // 328 // We save ourselves some work by scaling the result and the error bound by 329 // "distSum", since the result is normalized to be unit length anyway. 330 distSum := math.Abs(b0Dist - b1Dist) 331 errorSum := b0Error + b1Error 332 if distSum <= errorSum { 333 return pt, false // Error is unbounded in this case. 334 } 335 336 x := b1.Mul(b0Dist).Sub(b0.Mul(b1Dist)) 337 err := bLen*math.Abs(b0Dist*b1Error-b1Dist*b0Error)/ 338 (distSum-errorSum) + 2*distSum*epsilon 339 340 // Finally we normalize the result, compute the corresponding error, and 341 // check whether the total error is acceptable. 342 xLen := x.Norm() 343 maxError := intersectionError 344 if err > (float64(maxError)-epsilon)*xLen { 345 return pt, false 346 } 347 348 return Point{x.Mul(1 / xLen)}, true 349 } 350 351 // intersectionExact returns the intersection point of (a0, a1) and (b0, b1) 352 // using precise arithmetic. Note that the result is not exact because it is 353 // rounded down to double precision at the end. Also, the intersection point 354 // is not guaranteed to have the correct sign (i.e., the return value may need 355 // to be negated). 356 func intersectionExact(a0, a1, b0, b1 Point) Point { 357 // Since we are using presice arithmetic, we don't need to worry about 358 // numerical stability. 359 a0P := r3.PreciseVectorFromVector(a0.Vector) 360 a1P := r3.PreciseVectorFromVector(a1.Vector) 361 b0P := r3.PreciseVectorFromVector(b0.Vector) 362 b1P := r3.PreciseVectorFromVector(b1.Vector) 363 aNormP := a0P.Cross(a1P) 364 bNormP := b0P.Cross(b1P) 365 xP := aNormP.Cross(bNormP) 366 367 // The final Normalize() call is done in double precision, which creates a 368 // directional error of up to 2*dblError. (Precise conversion and Normalize() 369 // each contribute up to dblError of directional error.) 370 x := xP.Vector() 371 372 if x == (r3.Vector{}) { 373 // The two edges are exactly collinear, but we still consider them to be 374 // "crossing" because of simulation of simplicity. Out of the four 375 // endpoints, exactly two lie in the interior of the other edge. Of 376 // those two we return the one that is lexicographically smallest. 377 x = r3.Vector{10, 10, 10} // Greater than any valid S2Point 378 379 aNorm := Point{aNormP.Vector()} 380 bNorm := Point{bNormP.Vector()} 381 if OrderedCCW(b0, a0, b1, bNorm) && a0.Cmp(x) == -1 { 382 return a0 383 } 384 if OrderedCCW(b0, a1, b1, bNorm) && a1.Cmp(x) == -1 { 385 return a1 386 } 387 if OrderedCCW(a0, b0, a1, aNorm) && b0.Cmp(x) == -1 { 388 return b0 389 } 390 if OrderedCCW(a0, b1, a1, aNorm) && b1.Cmp(x) == -1 { 391 return b1 392 } 393 } 394 395 return Point{x} 396 } 397 398 // AngleContainsVertex reports if the angle ABC contains its vertex B. 399 // Containment is defined such that if several polygons tile the region around 400 // a vertex, then exactly one of those polygons contains that vertex. 401 // Returns false for degenerate angles of the form ABA. 402 // 403 // Note that this method is not sufficient to determine vertex containment in 404 // polygons with duplicate vertices (such as the polygon ABCADE). Use 405 // ContainsVertexQuery for such polygons. AngleContainsVertex(a, b, c) 406 // is equivalent to using ContainsVertexQuery as follows: 407 // 408 // ContainsVertexQuery query(b); 409 // query.AddEdge(a, -1); // incoming 410 // query.AddEdge(c, 1); // outgoing 411 // return query.ContainsVertex() > 0; 412 // 413 // Useful properties of AngleContainsVertex: 414 // 415 // (1) AngleContainsVertex(a,b,a) == false 416 // (2) AngleContainsVertex(a,b,c) == !AngleContainsVertex(c,b,a) unless a == c 417 // (3) Given vertices v_1 ... v_k ordered cyclically CCW around vertex b, 418 // AngleContainsVertex(v_{i+1}, b, v_i) is true for exactly one value of i. 419 // 420 // REQUIRES: a != b && b != c 421 func AngleContainsVertex(a, b, c Point) bool { 422 // A loop with consecutive vertices A, B, C contains vertex B if and only if 423 // the fixed vector R = referenceDir(B) is contained by the wedge ABC. The 424 // wedge is closed at A and open at C, i.e. the point B is inside the loop 425 // if A = R but not if C = R. 426 // 427 // Note that the test below is written so as to get correct results when the 428 // angle ABC is degenerate. If A = C or C = R it returns false, and 429 // otherwise if A = R it returns true. 430 return !OrderedCCW(b.referenceDir(), c, a, b) 431 } 432 433 // TODO(roberts): Differences from C++ 434 // func RobustCrossProd(a, b Point) Point 435 // func symbolicCrossProd(a, b Point) Point 436 // func exactCrossProd(a, b Point) Point 437 // func SignedVertexCrossing(a, b, c, d Point) int 438 // func isNormalizable(p Point) bool 439 // func ensureNormalizable(p Point) Point 440 // func normalizableFromPrecise(p r3.PreciseVector) Point 441