Thanks to visit codestin.com
Credit goes to github.com

Skip to content

Commit e85324b

Browse files
rsneddsymonds
rsned
authored and
dsymonds
committed
s2: Add Intersection function.
Signed-off-by: David Symonds <[email protected]>
1 parent d7dd449 commit e85324b

File tree

2 files changed

+356
-3
lines changed

2 files changed

+356
-3
lines changed

‎s2/edge_crossings.go‎

Lines changed: 240 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -17,6 +17,9 @@ limitations under the License.
1717
package s2
1818

1919
import (
20+
"math"
21+
22+
"github.com/golang/geo/r3"
2023
"github.com/golang/geo/s1"
2124
)
2225

@@ -164,5 +167,240 @@ func EdgeOrVertexCrossing(a, b, c, d Point) bool {
164167
}
165168
}
166169

167-
// TODO(roberts): Differences from C++
168-
// Intersection related methods
170+
// Intersection returns the intersection point of two edges AB and CD that cross
171+
// (CrossingSign(a,b,c,d) == Crossing).
172+
//
173+
// Useful properties of Intersection:
174+
//
175+
// (1) Intersection(b,a,c,d) == Intersection(a,b,d,c) == Intersection(a,b,c,d)
176+
// (2) Intersection(c,d,a,b) == Intersection(a,b,c,d)
177+
//
178+
// The returned intersection point X is guaranteed to be very close to the
179+
// true intersection point of AB and CD, even if the edges intersect at a
180+
// very small angle.
181+
func Intersection(a0, a1, b0, b1 Point) Point {
182+
// It is difficult to compute the intersection point of two edges accurately
183+
// when the angle between the edges is very small. Previously we handled
184+
// this by only guaranteeing that the returned intersection point is within
185+
// intersectionError of each edge. However, this means that when the edges
186+
// cross at a very small angle, the computed result may be very far from the
187+
// true intersection point.
188+
//
189+
// Instead this function now guarantees that the result is always within
190+
// intersectionError of the true intersection. This requires using more
191+
// sophisticated techniques and in some cases extended precision.
192+
//
193+
// - intersectionStable computes the intersection point using
194+
// projection and interpolation, taking care to minimize cancellation
195+
// error.
196+
//
197+
// - intersectionExact computes the intersection point using precision
198+
// arithmetic and converts the final result back to an Point.
199+
pt, ok := intersectionStable(a0, a1, b0, b1)
200+
if !ok {
201+
pt = intersectionExact(a0, a1, b0, b1)
202+
}
203+
204+
// Make sure the intersection point is on the correct side of the sphere.
205+
// Since all vertices are unit length, and edges are less than 180 degrees,
206+
// (a0 + a1) and (b0 + b1) both have positive dot product with the
207+
// intersection point. We use the sum of all vertices to make sure that the
208+
// result is unchanged when the edges are swapped or reversed.
209+
if pt.Dot((a0.Add(a1.Vector)).Add(b0.Add(b1.Vector))) < 0 {
210+
pt = Point{pt.Mul(-1)}
211+
}
212+
213+
return pt
214+
}
215+
216+
// Computes the cross product of two vectors, normalized to be unit length.
217+
// Also returns the length of the cross
218+
// product before normalization, which is useful for estimating the amount of
219+
// error in the result. For numerical stability, the vectors should both be
220+
// approximately unit length.
221+
func robustNormalWithLength(x, y r3.Vector) (r3.Vector, float64) {
222+
var pt r3.Vector
223+
// This computes 2 * (x.Cross(y)), but has much better numerical
224+
// stability when x and y are unit length.
225+
tmp := x.Sub(y).Cross(x.Add(y))
226+
length := tmp.Norm()
227+
if length != 0 {
228+
pt = tmp.Mul(1 / length)
229+
}
230+
return pt, 0.5 * length // Since tmp == 2 * (x.Cross(y))
231+
}
232+
233+
/*
234+
// intersectionSimple is not used by the C++ so it is skipped here.
235+
*/
236+
237+
// projection returns the projection of aNorm onto X (x.Dot(aNorm)), and a bound
238+
// on the error in the result. aNorm is not necessarily unit length.
239+
//
240+
// The remaining parameters (the length of aNorm (aNormLen) and the edge endpoints
241+
// a0 and a1) allow this dot product to be computed more accurately and efficiently.
242+
func projection(x, aNorm r3.Vector, aNormLen float64, a0, a1 Point) (proj, bound float64) {
243+
// The error in the dot product is proportional to the lengths of the input
244+
// vectors, so rather than using x itself (a unit-length vector) we use
245+
// the vectors from x to the closer of the two edge endpoints. This
246+
// typically reduces the error by a huge factor.
247+
x0 := x.Sub(a0.Vector)
248+
x1 := x.Sub(a1.Vector)
249+
x0Dist2 := x0.Norm2()
250+
x1Dist2 := x1.Norm2()
251+
252+
// If both distances are the same, we need to be careful to choose one
253+
// endpoint deterministically so that the result does not change if the
254+
// order of the endpoints is reversed.
255+
var dist float64
256+
if x0Dist2 < x1Dist2 || (x0Dist2 == x1Dist2 && x0.Cmp(x1) == -1) {
257+
dist = math.Sqrt(x0Dist2)
258+
proj = x0.Dot(aNorm)
259+
} else {
260+
dist = math.Sqrt(x1Dist2)
261+
proj = x1.Dot(aNorm)
262+
}
263+
264+
// This calculation bounds the error from all sources: the computation of
265+
// the normal, the subtraction of one endpoint, and the dot product itself.
266+
// dblEpsilon appears because the input points are assumed to be
267+
// normalized in double precision.
268+
//
269+
// For reference, the bounds that went into this calculation are:
270+
// ||N'-N|| <= ((1 + 2 * sqrt(3))||N|| + 32 * sqrt(3) * dblEpsilon) * epsilon
271+
// |(A.B)'-(A.B)| <= (1.5 * (A.B) + 1.5 * ||A|| * ||B||) * epsilon
272+
// ||(X-Y)'-(X-Y)|| <= ||X-Y|| * epsilon
273+
bound = (((3.5+2*math.Sqrt(3))*aNormLen+32*math.Sqrt(3)*dblEpsilon)*dist + 1.5*math.Abs(proj)) * epsilon
274+
return proj, bound
275+
}
276+
277+
// compareEdges reports whether (a0,a1) is less than (b0,b1) with respect to a total
278+
// ordering on edges that is invariant under edge reversals.
279+
func compareEdges(a0, a1, b0, b1 Point) bool {
280+
if a0.Cmp(a1.Vector) != -1 {
281+
a0, a1 = a1, a0
282+
}
283+
if b0.Cmp(b1.Vector) != -1 {
284+
b0, b1 = b1, b0
285+
}
286+
return a0.Cmp(b0.Vector) == -1 || (a0 == b0 && b0.Cmp(b1.Vector) == -1)
287+
}
288+
289+
// intersectionStable returns the intersection point of the edges (a0,a1) and
290+
// (b0,b1) if it can be computed to within an error of at most intersectionError
291+
// by this function.
292+
//
293+
// The intersection point is not guaranteed to have the correct sign because we
294+
// choose to use the longest of the two edges first. The sign is corrected by
295+
// Intersection.
296+
func intersectionStable(a0, a1, b0, b1 Point) (Point, bool) {
297+
// Sort the two edges so that (a0,a1) is longer, breaking ties in a
298+
// deterministic way that does not depend on the ordering of the endpoints.
299+
// This is desirable for two reasons:
300+
// - So that the result doesn't change when edges are swapped or reversed.
301+
// - It reduces error, since the first edge is used to compute the edge
302+
// normal (where a longer edge means less error), and the second edge
303+
// is used for interpolation (where a shorter edge means less error).
304+
aLen2 := a1.Sub(a0.Vector).Norm2()
305+
bLen2 := b1.Sub(b0.Vector).Norm2()
306+
if aLen2 < bLen2 || (aLen2 == bLen2 && compareEdges(a0, a1, b0, b1)) {
307+
return intersectionStableSorted(b0, b1, a0, a1)
308+
}
309+
return intersectionStableSorted(a0, a1, b0, b1)
310+
}
311+
312+
// intersectionStableSorted is a helper function for intersectionStable.
313+
// It expects that the edges (a0,a1) and (b0,b1) have been sorted so that
314+
// the first edge passed in is longer.
315+
func intersectionStableSorted(a0, a1, b0, b1 Point) (Point, bool) {
316+
var pt Point
317+
318+
// Compute the normal of the plane through (a0, a1) in a stable way.
319+
aNorm := a0.Sub(a1.Vector).Cross(a0.Add(a1.Vector))
320+
aNormLen := aNorm.Norm()
321+
bLen := b1.Sub(b0.Vector).Norm()
322+
323+
// Compute the projection (i.e., signed distance) of b0 and b1 onto the
324+
// plane through (a0, a1). Distances are scaled by the length of aNorm.
325+
b0Dist, b0Error := projection(b0.Vector, aNorm, aNormLen, a0, a1)
326+
b1Dist, b1Error := projection(b1.Vector, aNorm, aNormLen, a0, a1)
327+
328+
// The total distance from b0 to b1 measured perpendicularly to (a0,a1) is
329+
// |b0Dist - b1Dist|. Note that b0Dist and b1Dist generally have
330+
// opposite signs because b0 and b1 are on opposite sides of (a0, a1). The
331+
// code below finds the intersection point by interpolating along the edge
332+
// (b0, b1) to a fractional distance of b0Dist / (b0Dist - b1Dist).
333+
//
334+
// It can be shown that the maximum error in the interpolation fraction is
335+
//
336+
// (b0Dist * b1Error - b1Dist * b0Error) / (distSum * (distSum - errorSum))
337+
//
338+
// We save ourselves some work by scaling the result and the error bound by
339+
// "distSum", since the result is normalized to be unit length anyway.
340+
distSum := math.Abs(b0Dist - b1Dist)
341+
errorSum := b0Error + b1Error
342+
if distSum <= errorSum {
343+
return pt, false // Error is unbounded in this case.
344+
}
345+
346+
x := b1.Mul(b0Dist).Sub(b0.Mul(b1Dist))
347+
err := bLen*math.Abs(b0Dist*b1Error-b1Dist*b0Error)/
348+
(distSum-errorSum) + 2*distSum*epsilon
349+
350+
// Finally we normalize the result, compute the corresponding error, and
351+
// check whether the total error is acceptable.
352+
xLen := x.Norm()
353+
maxError := intersectionError
354+
if err > (float64(maxError)-epsilon)*xLen {
355+
return pt, false
356+
}
357+
358+
return Point{x.Mul(1 / xLen)}, true
359+
}
360+
361+
// intersectionExact returns the intersection point of (a0, a1) and (b0, b1)
362+
// using precise arithmetic. Note that the result is not exact because it is
363+
// rounded down to double precision at the end. Also, the intersection point
364+
// is not guaranteed to have the correct sign (i.e., the return value may need
365+
// to be negated).
366+
func intersectionExact(a0, a1, b0, b1 Point) Point {
367+
// Since we are using presice arithmetic, we don't need to worry about
368+
// numerical stability.
369+
a0P := r3.PreciseVectorFromVector(a0.Vector)
370+
a1P := r3.PreciseVectorFromVector(a1.Vector)
371+
b0P := r3.PreciseVectorFromVector(b0.Vector)
372+
b1P := r3.PreciseVectorFromVector(b1.Vector)
373+
aNormP := a0P.Cross(a1P)
374+
bNormP := b0P.Cross(b1P)
375+
xP := aNormP.Cross(bNormP)
376+
377+
// The final Normalize() call is done in double precision, which creates a
378+
// directional error of up to 2*dblEpsilon. (Precise conversion and Normalize()
379+
// each contribute up to dblEpsilon of directional error.)
380+
x := xP.Vector()
381+
382+
if x == (r3.Vector{}) {
383+
// The two edges are exactly collinear, but we still consider them to be
384+
// "crossing" because of simulation of simplicity. Out of the four
385+
// endpoints, exactly two lie in the interior of the other edge. Of
386+
// those two we return the one that is lexicographically smallest.
387+
x = r3.Vector{10, 10, 10} // Greater than any valid S2Point
388+
389+
aNorm := Point{aNormP.Vector()}
390+
bNorm := Point{bNormP.Vector()}
391+
if OrderedCCW(b0, a0, b1, bNorm) && a0.Cmp(x) == -1 {
392+
return a0
393+
}
394+
if OrderedCCW(b0, a1, b1, bNorm) && a1.Cmp(x) == -1 {
395+
return a1
396+
}
397+
if OrderedCCW(a0, b0, a1, aNorm) && b0.Cmp(x) == -1 {
398+
return b0
399+
}
400+
if OrderedCCW(a0, b1, a1, aNorm) && b1.Cmp(x) == -1 {
401+
return b1
402+
}
403+
}
404+
405+
return Point{x}
406+
}

‎s2/edge_crossings_test.go‎

Lines changed: 116 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -16,9 +16,124 @@ limitations under the License.
1616

1717
package s2
1818

19+
import (
20+
"math"
21+
"testing"
22+
23+
"github.com/golang/geo/s1"
24+
)
25+
1926
// The various Crossing methods are tested via s2edge_crosser_test
2027

28+
// testIntersectionExact is a helper for the tests to return a positively
29+
// oriented intersection Point of the two line segments (a0,a1) and (b0,b1).
30+
func testIntersectionExact(a0, a1, b0, b1 Point) Point {
31+
x := intersectionExact(a0, a1, b0, b1)
32+
if x.Dot((a0.Add(a1.Vector)).Add(b0.Add(b1.Vector))) < 0 {
33+
x = Point{x.Mul(-1)}
34+
}
35+
return x
36+
}
37+
38+
var distanceAbsError = s1.Angle(3 * dblEpsilon)
39+
40+
func TestEdgeutilIntersectionError(t *testing.T) {
41+
// We repeatedly construct two edges that cross near a random point "p", and
42+
// measure the distance from the actual intersection point "x" to the
43+
// exact intersection point and also to the edges.
44+
45+
var maxPointDist, maxEdgeDist s1.Angle
46+
for iter := 0; iter < 5000; iter++ {
47+
// We construct two edges AB and CD that intersect near "p". The angle
48+
// between AB and CD (expressed as a slope) is chosen randomly between
49+
// 1e-15 and 1e15 such that its logarithm is uniformly distributed.
50+
// Similarly, two edge lengths approximately between 1e-15 and 1 are
51+
// chosen. The edge endpoints are chosen such that they are often very
52+
// close to the other edge (i.e., barely crossing). Taken together this
53+
// ensures that we test both long and very short edges that intersect at
54+
// both large and very small angles.
55+
//
56+
// Sometimes the edges we generate will not actually cross, in which case
57+
// we simply try again.
58+
f := randomFrame()
59+
p := f.col(0)
60+
d1 := f.col(1)
61+
d2 := f.col(2)
62+
63+
slope := 1e-15 * math.Pow(1e30, randomFloat64())
64+
d2 = Point{d1.Add(d2.Mul(slope)).Normalize()}
65+
var a, b, c, d Point
66+
67+
// Find a pair of segments that cross.
68+
for {
69+
abLen := math.Pow(1e-15, randomFloat64())
70+
cdLen := math.Pow(1e-15, randomFloat64())
71+
aFraction := math.Pow(1e-5, randomFloat64())
72+
if oneIn(2) {
73+
aFraction = 1 - aFraction
74+
}
75+
cFraction := math.Pow(1e-5, randomFloat64())
76+
if oneIn(2) {
77+
cFraction = 1 - cFraction
78+
}
79+
a = Point{p.Sub(d1.Mul(aFraction * abLen)).Normalize()}
80+
b = Point{p.Add(d1.Mul((1 - aFraction) * abLen)).Normalize()}
81+
c = Point{p.Sub(d2.Mul(cFraction * cdLen)).Normalize()}
82+
d = Point{p.Add(d2.Mul((1 - cFraction) * cdLen)).Normalize()}
83+
if NewEdgeCrosser(a, b).CrossingSign(c, d) == Cross {
84+
break
85+
}
86+
}
87+
88+
// Each constructed edge should be at most 1.5 * dblEpsilon away from the
89+
// original point P.
90+
if got, want := DistanceFromSegment(p, a, b), s1.Angle(1.5*dblEpsilon)+distanceAbsError; got > want {
91+
t.Errorf("DistanceFromSegment(%v, %v, %v) = %v, want %v", p, a, b, got, want)
92+
}
93+
if got, want := DistanceFromSegment(p, c, d), s1.Angle(1.5*dblEpsilon)+distanceAbsError; got > want {
94+
t.Errorf("DistanceFromSegment(%v, %v, %v) = %v, want %v", p, c, d, got, want)
95+
}
96+
97+
// Verify that the expected intersection point is close to both edges and
98+
// also close to the original point P. (It might not be very close to P
99+
// if the angle between the edges is very small.)
100+
expected := testIntersectionExact(a, b, c, d)
101+
if got, want := DistanceFromSegment(expected, a, b), s1.Angle(3*dblEpsilon)+distanceAbsError; got > want {
102+
t.Errorf("DistanceFromSegment(%v, %v, %v) = %v, want %v", expected, a, b, got, want)
103+
}
104+
if got, want := DistanceFromSegment(expected, c, d), s1.Angle(3*dblEpsilon)+distanceAbsError; got > want {
105+
t.Errorf("DistanceFromSegment(%v, %v, %v) = %v, want %v", expected, c, d, got, want)
106+
}
107+
if got, want := expected.Distance(p), s1.Angle(3*dblEpsilon/slope)+intersectionError; got > want {
108+
t.Errorf("%v.Distance(%v) = %v, want %v", expected, p, got, want)
109+
}
110+
111+
// Now we actually test the Intersection() method.
112+
actual := Intersection(a, b, c, d)
113+
distAB := DistanceFromSegment(actual, a, b)
114+
distCD := DistanceFromSegment(actual, c, d)
115+
pointDist := expected.Distance(actual)
116+
if got, want := distAB, intersectionError+distanceAbsError; got > want {
117+
t.Errorf("DistanceFromSegment(%v, %v, %v) = %v want <= %v", actual, a, b, got, want)
118+
}
119+
if got, want := distCD, intersectionError+distanceAbsError; got > want {
120+
t.Errorf("DistanceFromSegment(%v, %v, %v) = %v want <= %v", actual, c, d, got, want)
121+
}
122+
if got, want := pointDist, intersectionError; got > want {
123+
t.Errorf("%v.Distance(%v) = %v want <= %v", expected, actual, got, want)
124+
}
125+
maxEdgeDist = maxAngle(maxEdgeDist, maxAngle(distAB, distCD))
126+
maxPointDist = maxAngle(maxPointDist, pointDist)
127+
}
128+
}
129+
130+
func maxAngle(a, b s1.Angle) s1.Angle {
131+
if a < b {
132+
return a
133+
}
134+
return b
135+
}
136+
21137
// TODO(roberts): Differences from C++:
22-
// TestEdgeCrossingsIntersectionError
23138
// TestEdgeCrossingsGrazingIntersections
24139
// TestEdgeCrossingsGetIntersectionInvariants

0 commit comments

Comments
 (0)