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 // This file defines a collection of methods for computing the distance to an edge, 18 // interpolating along an edge, projecting points onto edges, etc. 19 20 import ( 21 "math" 22 23 "github.com/golang/geo/s1" 24 ) 25 26 // DistanceFromSegment returns the distance of point X from line segment AB. 27 // The points are expected to be normalized. The result is very accurate for small 28 // distances but may have some numerical error if the distance is large 29 // (approximately pi/2 or greater). The case A == B is handled correctly. 30 func DistanceFromSegment(x, a, b Point) s1.Angle { 31 var minDist s1.ChordAngle 32 minDist, _ = updateMinDistance(x, a, b, minDist, true) 33 return minDist.Angle() 34 } 35 36 // IsDistanceLess reports whether the distance from X to the edge AB is less 37 // than limit. (For less than or equal to, specify limit.Successor()). 38 // This method is faster than DistanceFromSegment(). If you want to 39 // compare against a fixed s1.Angle, you should convert it to an s1.ChordAngle 40 // once and save the value, since this conversion is relatively expensive. 41 func IsDistanceLess(x, a, b Point, limit s1.ChordAngle) bool { 42 _, less := UpdateMinDistance(x, a, b, limit) 43 return less 44 } 45 46 // UpdateMinDistance checks if the distance from X to the edge AB is less 47 // than minDist, and if so, returns the updated value and true. 48 // The case A == B is handled correctly. 49 // 50 // Use this method when you want to compute many distances and keep track of 51 // the minimum. It is significantly faster than using DistanceFromSegment 52 // because (1) using s1.ChordAngle is much faster than s1.Angle, and (2) it 53 // can save a lot of work by not actually computing the distance when it is 54 // obviously larger than the current minimum. 55 func UpdateMinDistance(x, a, b Point, minDist s1.ChordAngle) (s1.ChordAngle, bool) { 56 return updateMinDistance(x, a, b, minDist, false) 57 } 58 59 // UpdateMaxDistance checks if the distance from X to the edge AB is greater 60 // than maxDist, and if so, returns the updated value and true. 61 // Otherwise it returns false. The case A == B is handled correctly. 62 func UpdateMaxDistance(x, a, b Point, maxDist s1.ChordAngle) (s1.ChordAngle, bool) { 63 dist := maxChordAngle(ChordAngleBetweenPoints(x, a), ChordAngleBetweenPoints(x, b)) 64 if dist > s1.RightChordAngle { 65 dist, _ = updateMinDistance(Point{x.Mul(-1)}, a, b, dist, true) 66 dist = s1.StraightChordAngle - dist 67 } 68 if maxDist < dist { 69 return dist, true 70 } 71 72 return maxDist, false 73 } 74 75 // IsInteriorDistanceLess reports whether the minimum distance from X to the edge 76 // AB is attained at an interior point of AB (i.e., not an endpoint), and that 77 // distance is less than limit. (Specify limit.Successor() for less than or equal to). 78 func IsInteriorDistanceLess(x, a, b Point, limit s1.ChordAngle) bool { 79 _, less := UpdateMinInteriorDistance(x, a, b, limit) 80 return less 81 } 82 83 // UpdateMinInteriorDistance reports whether the minimum distance from X to AB 84 // is attained at an interior point of AB (i.e., not an endpoint), and that distance 85 // is less than minDist. If so, the value of minDist is updated and true is returned. 86 // Otherwise it is unchanged and returns false. 87 func UpdateMinInteriorDistance(x, a, b Point, minDist s1.ChordAngle) (s1.ChordAngle, bool) { 88 return interiorDist(x, a, b, minDist, false) 89 } 90 91 // Project returns the point along the edge AB that is closest to the point X. 92 // The fractional distance of this point along the edge AB can be obtained 93 // using DistanceFraction. 94 // 95 // This requires that all points are unit length. 96 func Project(x, a, b Point) Point { 97 aXb := a.PointCross(b) 98 // Find the closest point to X along the great circle through AB. 99 p := x.Sub(aXb.Mul(x.Dot(aXb.Vector) / aXb.Vector.Norm2())) 100 101 // If this point is on the edge AB, then it's the closest point. 102 if Sign(aXb, a, Point{p}) && Sign(Point{p}, b, aXb) { 103 return Point{p.Normalize()} 104 } 105 106 // Otherwise, the closest point is either A or B. 107 if x.Sub(a.Vector).Norm2() <= x.Sub(b.Vector).Norm2() { 108 return a 109 } 110 return b 111 } 112 113 // DistanceFraction returns the distance ratio of the point X along an edge AB. 114 // If X is on the line segment AB, this is the fraction T such 115 // that X == Interpolate(T, A, B). 116 // 117 // This requires that A and B are distinct. 118 func DistanceFraction(x, a, b Point) float64 { 119 d0 := x.Angle(a.Vector) 120 d1 := x.Angle(b.Vector) 121 return float64(d0 / (d0 + d1)) 122 } 123 124 // Interpolate returns the point X along the line segment AB whose distance from A 125 // is the given fraction "t" of the distance AB. Does NOT require that "t" be 126 // between 0 and 1. Note that all distances are measured on the surface of 127 // the sphere, so this is more complicated than just computing (1-t)*a + t*b 128 // and normalizing the result. 129 func Interpolate(t float64, a, b Point) Point { 130 if t == 0 { 131 return a 132 } 133 if t == 1 { 134 return b 135 } 136 ab := a.Angle(b.Vector) 137 return InterpolateAtDistance(s1.Angle(t)*ab, a, b) 138 } 139 140 // InterpolateAtDistance returns the point X along the line segment AB whose 141 // distance from A is the angle ax. 142 func InterpolateAtDistance(ax s1.Angle, a, b Point) Point { 143 aRad := ax.Radians() 144 145 // Use PointCross to compute the tangent vector at A towards B. The 146 // result is always perpendicular to A, even if A=B or A=-B, but it is not 147 // necessarily unit length. (We effectively normalize it below.) 148 normal := a.PointCross(b) 149 tangent := normal.Vector.Cross(a.Vector) 150 151 // Now compute the appropriate linear combination of A and "tangent". With 152 // infinite precision the result would always be unit length, but we 153 // normalize it anyway to ensure that the error is within acceptable bounds. 154 // (Otherwise errors can build up when the result of one interpolation is 155 // fed into another interpolation.) 156 return Point{(a.Mul(math.Cos(aRad)).Add(tangent.Mul(math.Sin(aRad) / tangent.Norm()))).Normalize()} 157 } 158 159 // minUpdateDistanceMaxError returns the maximum error in the result of 160 // UpdateMinDistance (and the associated functions such as 161 // UpdateMinInteriorDistance, IsDistanceLess, etc), assuming that all 162 // input points are normalized to within the bounds guaranteed by r3.Vector's 163 // Normalize. The error can be added or subtracted from an s1.ChordAngle 164 // using its Expanded method. 165 func minUpdateDistanceMaxError(dist s1.ChordAngle) float64 { 166 // There are two cases for the maximum error in UpdateMinDistance(), 167 // depending on whether the closest point is interior to the edge. 168 return math.Max(minUpdateInteriorDistanceMaxError(dist), dist.MaxPointError()) 169 } 170 171 // minUpdateInteriorDistanceMaxError returns the maximum error in the result of 172 // UpdateMinInteriorDistance, assuming that all input points are normalized 173 // to within the bounds guaranteed by Point's Normalize. The error can be added 174 // or subtracted from an s1.ChordAngle using its Expanded method. 175 // 176 // Note that accuracy goes down as the distance approaches 0 degrees or 180 177 // degrees (for different reasons). Near 0 degrees the error is acceptable 178 // for all practical purposes (about 1.2e-15 radians ~= 8 nanometers). For 179 // exactly antipodal points the maximum error is quite high (0.5 meters), 180 // but this error drops rapidly as the points move away from antipodality 181 // (approximately 1 millimeter for points that are 50 meters from antipodal, 182 // and 1 micrometer for points that are 50km from antipodal). 183 // 184 // TODO(roberts): Currently the error bound does not hold for edges whose endpoints 185 // are antipodal to within about 1e-15 radians (less than 1 micron). This could 186 // be fixed by extending PointCross to use higher precision when necessary. 187 func minUpdateInteriorDistanceMaxError(dist s1.ChordAngle) float64 { 188 // If a point is more than 90 degrees from an edge, then the minimum 189 // distance is always to one of the endpoints, not to the edge interior. 190 if dist >= s1.RightChordAngle { 191 return 0.0 192 } 193 194 // This bound includes all source of error, assuming that the input points 195 // are normalized. a and b are components of chord length that are 196 // perpendicular and parallel to a plane containing the edge respectively. 197 b := math.Min(1.0, 0.5*float64(dist)) 198 a := math.Sqrt(b * (2 - b)) 199 return ((2.5+2*math.Sqrt(3)+8.5*a)*a + 200 (2+2*math.Sqrt(3)/3+6.5*(1-b))*b + 201 (23+16/math.Sqrt(3))*dblEpsilon) * dblEpsilon 202 } 203 204 // updateMinDistance computes the distance from a point X to a line segment AB, 205 // and if either the distance was less than the given minDist, or alwaysUpdate is 206 // true, the value and whether it was updated are returned. 207 func updateMinDistance(x, a, b Point, minDist s1.ChordAngle, alwaysUpdate bool) (s1.ChordAngle, bool) { 208 if d, ok := interiorDist(x, a, b, minDist, alwaysUpdate); ok { 209 // Minimum distance is attained along the edge interior. 210 return d, true 211 } 212 213 // Otherwise the minimum distance is to one of the endpoints. 214 xa2, xb2 := (x.Sub(a.Vector)).Norm2(), x.Sub(b.Vector).Norm2() 215 dist := s1.ChordAngle(math.Min(xa2, xb2)) 216 if !alwaysUpdate && dist >= minDist { 217 return minDist, false 218 } 219 return dist, true 220 } 221 222 // interiorDist returns the shortest distance from point x to edge ab, assuming 223 // that the closest point to X is interior to AB. If the closest point is not 224 // interior to AB, interiorDist returns (minDist, false). If alwaysUpdate is set to 225 // false, the distance is only updated when the value exceeds certain the given minDist. 226 func interiorDist(x, a, b Point, minDist s1.ChordAngle, alwaysUpdate bool) (s1.ChordAngle, bool) { 227 // Chord distance of x to both end points a and b. 228 xa2, xb2 := (x.Sub(a.Vector)).Norm2(), x.Sub(b.Vector).Norm2() 229 230 // The closest point on AB could either be one of the two vertices (the 231 // vertex case) or in the interior (the interior case). Let C = A x B. 232 // If X is in the spherical wedge extending from A to B around the axis 233 // through C, then we are in the interior case. Otherwise we are in the 234 // vertex case. 235 // 236 // Check whether we might be in the interior case. For this to be true, XAB 237 // and XBA must both be acute angles. Checking this condition exactly is 238 // expensive, so instead we consider the planar triangle ABX (which passes 239 // through the sphere's interior). The planar angles XAB and XBA are always 240 // less than the corresponding spherical angles, so if we are in the 241 // interior case then both of these angles must be acute. 242 // 243 // We check this by computing the squared edge lengths of the planar 244 // triangle ABX, and testing whether angles XAB and XBA are both acute using 245 // the law of cosines: 246 // 247 // | XA^2 - XB^2 | < AB^2 (*) 248 // 249 // This test must be done conservatively (taking numerical errors into 250 // account) since otherwise we might miss a situation where the true minimum 251 // distance is achieved by a point on the edge interior. 252 // 253 // There are two sources of error in the expression above (*). The first is 254 // that points are not normalized exactly; they are only guaranteed to be 255 // within 2 * dblEpsilon of unit length. Under the assumption that the two 256 // sides of (*) are nearly equal, the total error due to normalization errors 257 // can be shown to be at most 258 // 259 // 2 * dblEpsilon * (XA^2 + XB^2 + AB^2) + 8 * dblEpsilon ^ 2 . 260 // 261 // The other source of error is rounding of results in the calculation of (*). 262 // Each of XA^2, XB^2, AB^2 has a maximum relative error of 2.5 * dblEpsilon, 263 // plus an additional relative error of 0.5 * dblEpsilon in the final 264 // subtraction which we further bound as 0.25 * dblEpsilon * (XA^2 + XB^2 + 265 // AB^2) for convenience. This yields a final error bound of 266 // 267 // 4.75 * dblEpsilon * (XA^2 + XB^2 + AB^2) + 8 * dblEpsilon ^ 2 . 268 ab2 := a.Sub(b.Vector).Norm2() 269 maxError := (4.75*dblEpsilon*(xa2+xb2+ab2) + 8*dblEpsilon*dblEpsilon) 270 if math.Abs(xa2-xb2) >= ab2+maxError { 271 return minDist, false 272 } 273 274 // The minimum distance might be to a point on the edge interior. Let R 275 // be closest point to X that lies on the great circle through AB. Rather 276 // than computing the geodesic distance along the surface of the sphere, 277 // instead we compute the "chord length" through the sphere's interior. 278 // 279 // The squared chord length XR^2 can be expressed as XQ^2 + QR^2, where Q 280 // is the point X projected onto the plane through the great circle AB. 281 // The distance XQ^2 can be written as (X.C)^2 / |C|^2 where C = A x B. 282 // We ignore the QR^2 term and instead use XQ^2 as a lower bound, since it 283 // is faster and the corresponding distance on the Earth's surface is 284 // accurate to within 1% for distances up to about 1800km. 285 c := a.PointCross(b) 286 c2 := c.Norm2() 287 xDotC := x.Dot(c.Vector) 288 xDotC2 := xDotC * xDotC 289 if !alwaysUpdate && xDotC2 > c2*float64(minDist) { 290 // The closest point on the great circle AB is too far away. We need to 291 // test this using ">" rather than ">=" because the actual minimum bound 292 // on the distance is (xDotC2 / c2), which can be rounded differently 293 // than the (more efficient) multiplicative test above. 294 return minDist, false 295 } 296 297 // Otherwise we do the exact, more expensive test for the interior case. 298 // This test is very likely to succeed because of the conservative planar 299 // test we did initially. 300 // 301 // TODO(roberts): Ensure that the errors in test are accurately reflected in the 302 // minUpdateInteriorDistanceMaxError. 303 cx := c.Cross(x.Vector) 304 if a.Sub(x.Vector).Dot(cx) >= 0 || b.Sub(x.Vector).Dot(cx) <= 0 { 305 return minDist, false 306 } 307 308 // Compute the squared chord length XR^2 = XQ^2 + QR^2 (see above). 309 // This calculation has good accuracy for all chord lengths since it 310 // is based on both the dot product and cross product (rather than 311 // deriving one from the other). However, note that the chord length 312 // representation itself loses accuracy as the angle approaches π. 313 qr := 1 - math.Sqrt(cx.Norm2()/c2) 314 dist := s1.ChordAngle((xDotC2 / c2) + (qr * qr)) 315 316 if !alwaysUpdate && dist >= minDist { 317 return minDist, false 318 } 319 320 return dist, true 321 } 322 323 // updateEdgePairMinDistance computes the minimum distance between the given 324 // pair of edges. If the two edges cross, the distance is zero. The cases 325 // a0 == a1 and b0 == b1 are handled correctly. 326 func updateEdgePairMinDistance(a0, a1, b0, b1 Point, minDist s1.ChordAngle) (s1.ChordAngle, bool) { 327 if minDist == 0 { 328 return 0, false 329 } 330 if CrossingSign(a0, a1, b0, b1) == Cross { 331 minDist = 0 332 return 0, true 333 } 334 335 // Otherwise, the minimum distance is achieved at an endpoint of at least 336 // one of the two edges. We ensure that all four possibilities are always checked. 337 // 338 // The calculation below computes each of the six vertex-vertex distances 339 // twice (this could be optimized). 340 var ok1, ok2, ok3, ok4 bool 341 minDist, ok1 = UpdateMinDistance(a0, b0, b1, minDist) 342 minDist, ok2 = UpdateMinDistance(a1, b0, b1, minDist) 343 minDist, ok3 = UpdateMinDistance(b0, a0, a1, minDist) 344 minDist, ok4 = UpdateMinDistance(b1, a0, a1, minDist) 345 return minDist, ok1 || ok2 || ok3 || ok4 346 } 347 348 // updateEdgePairMaxDistance reports the minimum distance between the given pair of edges. 349 // If one edge crosses the antipodal reflection of the other, the distance is pi. 350 func updateEdgePairMaxDistance(a0, a1, b0, b1 Point, maxDist s1.ChordAngle) (s1.ChordAngle, bool) { 351 if maxDist == s1.StraightChordAngle { 352 return s1.StraightChordAngle, false 353 } 354 if CrossingSign(a0, a1, Point{b0.Mul(-1)}, Point{b1.Mul(-1)}) == Cross { 355 return s1.StraightChordAngle, true 356 } 357 358 // Otherwise, the maximum distance is achieved at an endpoint of at least 359 // one of the two edges. We ensure that all four possibilities are always checked. 360 // 361 // The calculation below computes each of the six vertex-vertex distances 362 // twice (this could be optimized). 363 var ok1, ok2, ok3, ok4 bool 364 maxDist, ok1 = UpdateMaxDistance(a0, b0, b1, maxDist) 365 maxDist, ok2 = UpdateMaxDistance(a1, b0, b1, maxDist) 366 maxDist, ok3 = UpdateMaxDistance(b0, a0, a1, maxDist) 367 maxDist, ok4 = UpdateMaxDistance(b1, a0, a1, maxDist) 368 return maxDist, ok1 || ok2 || ok3 || ok4 369 } 370 371 // EdgePairClosestPoints returns the pair of points (a, b) that achieves the 372 // minimum distance between edges a0a1 and b0b1, where a is a point on a0a1 and 373 // b is a point on b0b1. If the two edges intersect, a and b are both equal to 374 // the intersection point. Handles a0 == a1 and b0 == b1 correctly. 375 func EdgePairClosestPoints(a0, a1, b0, b1 Point) (Point, Point) { 376 if CrossingSign(a0, a1, b0, b1) == Cross { 377 x := Intersection(a0, a1, b0, b1) 378 return x, x 379 } 380 // We save some work by first determining which vertex/edge pair achieves 381 // the minimum distance, and then computing the closest point on that edge. 382 var minDist s1.ChordAngle 383 var ok bool 384 385 minDist, _ = updateMinDistance(a0, b0, b1, minDist, true) 386 closestVertex := 0 387 if minDist, ok = UpdateMinDistance(a1, b0, b1, minDist); ok { 388 closestVertex = 1 389 } 390 if minDist, ok = UpdateMinDistance(b0, a0, a1, minDist); ok { 391 closestVertex = 2 392 } 393 if _, ok = UpdateMinDistance(b1, a0, a1, minDist); ok { 394 closestVertex = 3 395 } 396 switch closestVertex { 397 case 0: 398 return a0, Project(a0, b0, b1) 399 case 1: 400 return a1, Project(a1, b0, b1) 401 case 2: 402 return Project(b0, a0, a1), b0 403 case 3: 404 return Project(b1, a0, a1), b1 405 default: 406 panic("illegal case reached") 407 } 408 } 409