private static void TestInterpolate(S2Point a, S2Point b, double t, S2Point expected) { a = a.Normalize(); b = b.Normalize(); expected = expected.Normalize(); // We allow a bit more than the usual 1e-15 error tolerance because // interpolation uses trig functions. S1Angle kError = S1Angle.FromRadians(3e-15); Assert.True(new S1Angle(S2.Interpolate(a, b, t), expected) <= kError); // Now test the other interpolation functions. S1Angle r = t * new S1Angle(a, b); Assert.True(new S1Angle(S2.GetPointOnLine(a, b, r), expected) <= kError); if (a.DotProd(b) == 0) { // Common in the test cases below. Assert.True(new S1Angle(S2.GetPointOnRay(a, b, r), expected) <= kError); } if (r.Radians >= 0 && r.Radians < 0.99 * S2.M_PI) { S1ChordAngle r_ca = new(r); Assert.True(new S1Angle(S2.GetPointOnLine(a, b, r_ca), expected) <= kError); if (a.DotProd(b) == 0) { Assert.True(new S1Angle(S2.GetPointOnRay(a, b, r_ca), expected) <= kError); } } }
// Returns the point at distance "r" along the ray with the given origin and // direction. "dir" is required to be perpendicular to "origin" (since this // is how directions on the sphere are represented). // // This function is similar to S2::GetPointOnLine() except that (1) the first // two arguments are required to be perpendicular and (2) it is much faster. // It can be used as an alternative to repeatedly calling GetPointOnLine() by // computing "dir" as // // S2Point dir = S2::RobustCrossProd(a, b).CrossProd(a).Normalize(); // // REQUIRES: "origin" and "dir" are perpendicular to within the tolerance // of the calculation above. public static S2Point GetPointOnRay(S2Point origin, S2Point dir, S1Angle r) { // See comments above. System.Diagnostics.Debug.Assert(origin.IsUnitLength()); System.Diagnostics.Debug.Assert(dir.IsUnitLength()); System.Diagnostics.Debug.Assert(origin.DotProd(dir) <= S2.kRobustCrossProdError + 0.75 * S2.DoubleEpsilon); return((r.Cos() * origin + r.Sin() * dir).Normalize()); }
// Faster than the function above, but cannot accurately represent distances // near 180 degrees due to the limitations of S1ChordAngle. public static S2Point GetPointOnRay(S2Point origin, S2Point dir, S1ChordAngle r) { System.Diagnostics.Debug.Assert(origin.IsUnitLength()); System.Diagnostics.Debug.Assert(dir.IsUnitLength()); // The error bound below includes the error in computing the dot product. System.Diagnostics.Debug.Assert(origin.DotProd(dir) <= S2.kRobustCrossProdError + 0.75 * S2.DoubleEpsilon); // Mathematically the result should already be unit length, but we normalize // it anyway to ensure that the error is within acceptable bounds. // (Otherwise errors can build up when the result of one interpolation is // fed into another interpolation.) // // Note that it is much cheaper to compute the sine and cosine of an // S1ChordAngle than an S1Angle. return((r.Cos() * origin + r.Sin() * dir).Normalize()); }
private S1ChordAngle EstimateMaxError(R2Point pa, S2Point a, R2Point pb, S2Point b) { // See the algorithm description at the top of this file. // We always tessellate edges longer than 90 degrees on the sphere, since the // approximation below is not robust enough to handle such edges. if (a.DotProd(b) < -1e-14) { return(S1ChordAngle.Infinity); } const double t1 = kInterpolationFraction; const double t2 = 1 - kInterpolationFraction; S2Point mid1 = S2.Interpolate(a, b, t1); S2Point mid2 = S2.Interpolate(a, b, t2); S2Point pmid1 = proj_.Unproject(Projection.Interpolate(t1, pa, pb)); S2Point pmid2 = proj_.Unproject(Projection.Interpolate(t2, pa, pb)); return(S1ChordAngle.Max(new S1ChordAngle(mid1, pmid1), new S1ChordAngle(mid2, pmid2))); }
/** * A relatively expensive calculation invoked by RobustCCW() if the sign of * the determinant is uncertain. */ private static int ExpensiveCcw(S2Point a, S2Point b, S2Point c) { // Return zero if and only if two points are the same. This ensures (1). if (a.Equals(b) || b.Equals(c) || c.Equals(a)) { return 0; } // Now compute the determinant in a stable way. Since all three points are // unit length and we know that the determinant is very close to zero, this // means that points are very nearly colinear. Furthermore, the most common // situation is where two points are nearly identical or nearly antipodal. // To get the best accuracy in this situation, it is important to // immediately reduce the magnitude of the arguments by computing either // A+B or A-B for each pair of points. Note that even if A and B differ // only in their low bits, A-B can be computed very accurately. On the // other hand we can't accurately represent an arbitrary linear combination // of two vectors as would be required for Gaussian elimination. The code // below chooses the vertex opposite the longest edge as the "origin" for // the calculation, and computes the different vectors to the other two // vertices. This minimizes the sum of the lengths of these vectors. // // This implementation is very stable numerically, but it still does not // return consistent results in all cases. For example, if three points are // spaced far apart from each other along a great circle, the sign of the // result will basically be random (although it will still satisfy the // conditions documented in the header file). The only way to return // consistent results in all cases is to compute the result using // arbitrary-precision arithmetic. I considered using the Gnu MP library, // but this would be very expensive (up to 2000 bits of precision may be // needed to store the intermediate results) and seems like overkill for // this problem. The MP library is apparently also quite particular about // compilers and compilation options and would be a pain to maintain. // We want to handle the case of nearby points and nearly antipodal points // accurately, so determine whether A+B or A-B is smaller in each case. double sab = (a.DotProd(b) > 0) ? -1 : 1; double sbc = (b.DotProd(c) > 0) ? -1 : 1; double sca = (c.DotProd(a) > 0) ? -1 : 1; var vab = a + (b * sab); var vbc = b + (c * sbc); var vca = c + (a * sca); var dab = vab.Norm2; var dbc = vbc.Norm2; var dca = vca.Norm2; // Sort the difference vectors to find the longest edge, and use the // opposite vertex as the origin. If two difference vectors are the same // length, we break ties deterministically to ensure that the symmetry // properties guaranteed in the header file will be true. double sign; if (dca < dbc || (dca == dbc && a < b)) { if (dab < dbc || (dab == dbc && a < c)) { // The "sab" factor converts A +/- B into B +/- A. sign = S2Point.CrossProd(vab, vca).DotProd(a)*sab; // BC is longest // edge } else { sign = S2Point.CrossProd(vca, vbc).DotProd(c)*sca; // AB is longest // edge } } else { if (dab < dca || (dab == dca && b < c)) { sign = S2Point.CrossProd(vbc, vab).DotProd(b)*sbc; // CA is longest // edge } else { sign = S2Point.CrossProd(vca, vbc).DotProd(c)*sca; // AB is longest // edge } } if (sign > 0) { return 1; } if (sign < 0) { return -1; } // The points A, B, and C are numerically indistinguishable from coplanar. // This may be due to roundoff error, or the points may in fact be exactly // coplanar. We handle this situation by perturbing all of the points by a // vector (eps, eps**2, eps**3) where "eps" is an infinitesmally small // positive number (e.g. 1 divided by a googolplex). The perturbation is // done symbolically, i.e. we compute what would happen if the points were // perturbed by this amount. It turns out that this is equivalent to // checking whether the points are ordered CCW around the origin first in // the Y-Z plane, then in the Z-X plane, and then in the X-Y plane. var ccw = PlanarOrderedCcw(new R2Vector(a.Y, a.Z), new R2Vector(b.Y, b.Z), new R2Vector(c.Y, c.Z)); if (ccw == 0) { ccw = PlanarOrderedCcw(new R2Vector(a.Z, a.X), new R2Vector(b.Z, b.X), new R2Vector(c.Z, c.X)); if (ccw == 0) { ccw = PlanarOrderedCcw( new R2Vector(a.X, a.Y), new R2Vector(b.X, b.Y), new R2Vector(c.X, c.Y)); // assert (ccw != 0); } } return ccw; }
/** * A more efficient version of RobustCCW that allows the precomputed * cross-product of A and B to be specified. * * Note: a, b and c are expected to be of unit length. Otherwise, the results * are undefined */ public static int RobustCcw(S2Point a, S2Point b, S2Point c, S2Point aCrossB) { // assert (isUnitLength(a) && isUnitLength(b) && isUnitLength(c)); // There are 14 multiplications and additions to compute the determinant // below. Since all three points are normalized, it is possible to show // that the average rounding error per operation does not exceed 2**-54, // the maximum rounding error for an operation whose result magnitude is in // the range [0.5,1). Therefore, if the absolute value of the determinant // is greater than 2*14*(2**-54), the determinant will have the same sign // even if the arguments are rotated (which produces a mathematically // equivalent result but with potentially different rounding errors). const double kMinAbsValue = 1.6e-15; // 2 * 14 * 2**-54 var det = aCrossB.DotProd(c); // Double-check borderline cases in debug mode. // assert ((Math.Abs(det) < kMinAbsValue) || (Math.Abs(det) > 1000 * kMinAbsValue) // || (det * expensiveCCW(a, b, c) > 0)); if (det > kMinAbsValue) { return 1; } if (det < -kMinAbsValue) { return -1; } return ExpensiveCcw(a, b, c); }
private void TestFaceClipping(S2Point a_raw, S2Point b_raw) { S2Point a = a_raw.Normalize(); S2Point b = b_raw.Normalize(); // First we test GetFaceSegments. FaceSegmentVector segments = new(); GetFaceSegments(a, b, segments); int n = segments.Count; Assert.True(n >= 1); var msg = new StringBuilder($"\nA={a_raw}\nB={b_raw}\nN={S2.RobustCrossProd(a, b)}\nSegments:\n"); int i1 = 0; foreach (var s in segments) { msg.AppendLine($"{i1++}: face={s.face}, a={s.a}, b={s.b}"); } _logger.WriteLine(msg.ToString()); R2Rect biunit = new(new R1Interval(-1, 1), new R1Interval(-1, 1)); var kErrorRadians = kFaceClipErrorRadians; // The first and last vertices should approximately equal A and B. Assert.True(a.Angle(S2.FaceUVtoXYZ(segments[0].face, segments[0].a)) <= kErrorRadians); Assert.True(b.Angle(S2.FaceUVtoXYZ(segments[n - 1].face, segments[n - 1].b)) <= kErrorRadians); S2Point norm = S2.RobustCrossProd(a, b).Normalize(); S2Point a_tangent = norm.CrossProd(a); S2Point b_tangent = b.CrossProd(norm); for (int i = 0; i < n; ++i) { // Vertices may not protrude outside the biunit square. Assert.True(biunit.Contains(segments[i].a)); Assert.True(biunit.Contains(segments[i].b)); if (i == 0) { continue; } // The two representations of each interior vertex (on adjacent faces) // must correspond to exactly the same S2Point. Assert.NotEqual(segments[i - 1].face, segments[i].face); Assert.Equal(S2.FaceUVtoXYZ(segments[i - 1].face, segments[i - 1].b), S2.FaceUVtoXYZ(segments[i].face, segments[i].a)); // Interior vertices should be in the plane containing A and B, and should // be contained in the wedge of angles between A and B (i.e., the dot // products with a_tangent and b_tangent should be non-negative). S2Point p = S2.FaceUVtoXYZ(segments[i].face, segments[i].a).Normalize(); Assert.True(Math.Abs(p.DotProd(norm)) <= kErrorRadians); Assert.True(p.DotProd(a_tangent) >= -kErrorRadians); Assert.True(p.DotProd(b_tangent) >= -kErrorRadians); } // Now we test ClipToPaddedFace (sometimes with a padding of zero). We do // this by defining an (x,y) coordinate system for the plane containing AB, // and converting points along the great circle AB to angles in the range // [-Pi, Pi]. We then accumulate the angle intervals spanned by each // clipped edge; the union over all 6 faces should approximately equal the // interval covered by the original edge. double padding = S2Testing.Random.OneIn(10) ? 0.0 : 1e-10 * Math.Pow(1e-5, S2Testing.Random.RandDouble()); S2Point x_axis = a, y_axis = a_tangent; S1Interval expected_angles = new(0, a.Angle(b)); S1Interval max_angles = expected_angles.Expanded(kErrorRadians); S1Interval actual_angles = new(); for (int face = 0; face < 6; ++face) { if (ClipToPaddedFace(a, b, face, padding, out var a_uv, out var b_uv)) { S2Point a_clip = S2.FaceUVtoXYZ(face, a_uv).Normalize(); S2Point b_clip = S2.FaceUVtoXYZ(face, b_uv).Normalize(); Assert.True(Math.Abs(a_clip.DotProd(norm)) <= kErrorRadians); Assert.True(Math.Abs(b_clip.DotProd(norm)) <= kErrorRadians); if (a_clip.Angle(a) > kErrorRadians) { Assert2.DoubleEqual(1 + padding, Math.Max(Math.Abs(a_uv[0]), Math.Abs(a_uv[1]))); } if (b_clip.Angle(b) > kErrorRadians) { Assert2.DoubleEqual(1 + padding, Math.Max(Math.Abs(b_uv[0]), Math.Abs(b_uv[1]))); } double a_angle = Math.Atan2(a_clip.DotProd(y_axis), a_clip.DotProd(x_axis)); double b_angle = Math.Atan2(b_clip.DotProd(y_axis), b_clip.DotProd(x_axis)); // Rounding errors may cause b_angle to be slightly less than a_angle. // We handle this by constructing the interval with FromPointPair(), // which is okay since the interval length is much less than Math.PI. S1Interval face_angles = S1Interval.FromPointPair(a_angle, b_angle); Assert.True(max_angles.Contains(face_angles)); actual_angles = actual_angles.Union(face_angles); } } Assert.True(actual_angles.Expanded(kErrorRadians).Contains(expected_angles)); }
private static IEnumerable <T> GetSurfaceIntegral <T>(S2PointLoopSpan loop, Func <S2Point, S2Point, S2Point, T> f_tri) { // We sum "f_tri" over a collection T of oriented triangles, possibly // overlapping. Let the sign of a triangle be +1 if it is CCW and -1 // otherwise, and let the sign of a point "x" be the sum of the signs of the // triangles containing "x". Then the collection of triangles T is chosen // such that every point in the loop interior has the same sign x, and every // point in the loop exterior has the same sign (x - 1). Furthermore almost // always it is true that x == 0 or x == 1, meaning that either // // (1) Each point in the loop interior has sign +1, and sign 0 otherwise; or // (2) Each point in the loop exterior has sign -1, and sign 0 otherwise. // // The triangles basically consist of a "fan" from vertex 0 to every loop // edge that does not include vertex 0. However, what makes this a bit // tricky is that spherical edges become numerically unstable as their // length approaches 180 degrees. Of course there is not much we can do if // the loop itself contains such edges, but we would like to make sure that // all the triangle edges under our control (i.e., the non-loop edges) are // stable. For example, consider a loop around the equator consisting of // four equally spaced points. This is a well-defined loop, but we cannot // just split it into two triangles by connecting vertex 0 to vertex 2. // // We handle this type of situation by moving the origin of the triangle fan // whenever we are about to create an unstable edge. We choose a new // location for the origin such that all relevant edges are stable. We also // create extra triangles with the appropriate orientation so that the sum // of the triangle signs is still correct at every point. // The maximum length of an edge for it to be considered numerically stable. // The exact value is fairly arbitrary since it depends on the stability of // the "f_tri" function. The value below is quite conservative but could be // reduced further if desired. const double kMaxLength = Math.PI - 1e-5; if (loop.Count < 3) { yield break; } S2Point origin = loop[0]; for (int i = 1; i + 1 < loop.Count; ++i) { // Let V_i be loop[i], let O be the current origin, and let length(A, B) // be the length of edge (A, B). At the start of each loop iteration, the // "leading edge" of the triangle fan is (O, V_i), and we want to extend // the triangle fan so that the leading edge is (O, V_i+1). // // Invariants: // 1. length(O, V_i) < kMaxLength for all (i > 1). // 2. Either O == V_0, or O is approximately perpendicular to V_0. // 3. "sum" is the oriented integral of f over the area defined by // (O, V_0, V_1, ..., V_i). System.Diagnostics.Debug.Assert(i == 1 || origin.Angle(loop[i]) < kMaxLength); System.Diagnostics.Debug.Assert(origin == loop[0] || Math.Abs(origin.DotProd(loop[0])) < S2.DoubleError); if (loop[i + 1].Angle(origin) > kMaxLength) { // We are about to create an unstable edge, so choose a new origin O' // for the triangle fan. S2Point old_origin = origin; if (origin == loop[0]) { // The following point O' is well-separated from V_i and V_0 (and // therefore V_i+1 as well). Moving the origin transforms the leading // edge of the triangle fan into a two-edge chain (V_0, O', V_i). origin = S2.RobustCrossProd(loop[0], loop[i]).Normalize(); } else if (loop[i].Angle(loop[0]) < kMaxLength) { // All edges of the triangle (O, V_0, V_i) are stable, so we can // revert to using V_0 as the origin. This changes the leading edge // chain (V_0, O, V_i) back into a single edge (V_0, V_i). origin = loop[0]; } else { // (O, V_i+1) and (V_0, V_i) are antipodal pairs, and O and V_0 are // perpendicular. Therefore V_0.CrossProd(O) is approximately // perpendicular to all of {O, V_0, V_i, V_i+1}, and we can choose // this point O' as the new origin. // // NOTE(ericv): The following line is the reason why in rare cases the // triangle sum can have a sign other than -1, 0, or 1. To fix this // we would need to choose either "-origin" or "origin" below // depending on whether the signed area of the triangles chosen so far // is positive or negative respectively. This is easy in the case of // GetSignedArea() but would be extra work for GetCentroid(). In any // case this does not cause any problems in practice. origin = loop[0].CrossProd(old_origin); // The following two triangles transform the leading edge chain from // (V_0, O, V_i) to (V_0, O', V_i+1). // // First we advance the edge (V_0, O) to (V_0, O'). yield return(f_tri(loop[0], old_origin, origin)); } // Advance the edge (O, V_i) to (O', V_i). yield return(f_tri(old_origin, loop[i], origin)); } // Advance the edge (O, V_i) to (O, V_i+1). yield return(f_tri(origin, loop[i], loop[i + 1])); } // If the origin is not V_0, we need to sum one more triangle. if (origin != loop[0]) { // Advance the edge (O, V_n-1) to (O, V_0). yield return(f_tri(origin, loop[^ 1], loop[0]));
/** * Returns the point on edge AB closest to X. x, a and b must be of unit * length. Throws IllegalArgumentException if this is not the case. * */ public static S2Point GetClosestPoint(S2Point x, S2Point a, S2Point b) { Preconditions.CheckArgument(S2.IsUnitLength(x)); Preconditions.CheckArgument(S2.IsUnitLength(a)); Preconditions.CheckArgument(S2.IsUnitLength(b)); var crossProd = S2.RobustCrossProd(a, b); // Find the closest point to X along the great circle through AB. var p = x - (crossProd*x.DotProd(crossProd)/crossProd.Norm2); // If p is on the edge AB, then it's the closest point. if (S2.SimpleCcw(crossProd, a, p) && S2.SimpleCcw(p, b, crossProd)) { return S2Point.Normalize(p); } // Otherwise, the closest point is either A or B. return (x - a).Norm2 <= (x - b).Norm2 ? a : b; }
/** * A slightly more efficient version of getDistance() where the cross product * of the two endpoints has been precomputed. The cross product does not need * to be normalized, but should be computed using S2.robustCrossProd() for the * most accurate results. */ public static S1Angle GetDistance(S2Point x, S2Point a, S2Point b, S2Point aCrossB) { Preconditions.CheckArgument(S2.IsUnitLength(x)); Preconditions.CheckArgument(S2.IsUnitLength(a)); Preconditions.CheckArgument(S2.IsUnitLength(b)); // There are three cases. If X is located in the spherical wedge defined by // A, B, and the axis A x B, then the closest point is on the segment AB. // Otherwise the closest point is either A or B; the dividing line between // these two cases is the great circle passing through (A x B) and the // midpoint of AB. if (S2.SimpleCcw(aCrossB, a, x) && S2.SimpleCcw(x, b, aCrossB)) { // The closest point to X lies on the segment AB. We compute the distance // to the corresponding great circle. The result is accurate for small // distances but not necessarily for large distances (approaching Pi/2). var sinDist = Math.Abs(x.DotProd(aCrossB))/aCrossB.Norm; return S1Angle.FromRadians(Math.Asin(Math.Min(1.0, sinDist))); } // Otherwise, the closest point is either A or B. The cheapest method is // just to compute the minimum of the two linear (as opposed to spherical) // distances and convert the result to an angle. Again, this method is // accurate for small but not large distances (approaching Pi). var linearDist2 = Math.Min((x - a).Norm2, (x - b).Norm2); return S1Angle.FromRadians(2*Math.Asin(Math.Min(1.0, 0.5*Math.Sqrt(linearDist2)))); }
public void Test_S2Cap_S2CellMethods() { // For each cube face, we construct some cells on // that face and some caps whose positions are relative to that face, // and then check for the expected intersection/containment results. for (var face = 0; face < 6; ++face) { // The cell consisting of the entire face. S2Cell root_cell = S2Cell.FromFace(face); // A leaf cell at the midpoint of the v=1 edge. S2Cell edge_cell = new(S2.FaceUVtoXYZ(face, 0, 1 - kEps)); // A leaf cell at the u=1, v=1 corner. S2Cell corner_cell = new(S2.FaceUVtoXYZ(face, 1 - kEps, 1 - kEps)); // Quick check for full and empty caps. Assert.True(S2Cap.Full.Contains(root_cell)); Assert.False(S2Cap.Empty.MayIntersect(root_cell)); // Check intersections with the bounding caps of the leaf cells that are // adjacent to 'corner_cell' along the Hilbert curve. Because this corner // is at (u=1,v=1), the curve stays locally within the same cube face. S2CellId first = corner_cell.Id.Advance(-3); S2CellId last = corner_cell.Id.Advance(4); for (S2CellId id = first; id < last; id = id.Next()) { S2Cell cell = new(id); Assert.Equal(id == corner_cell.Id, cell.GetCapBound().Contains(corner_cell)); Assert.Equal(id.Parent().Contains(corner_cell.Id), cell.GetCapBound().MayIntersect(corner_cell)); } var anti_face = (face + 3) % 6; // Opposite face. for (var cap_face = 0; cap_face < 6; ++cap_face) { // A cap that barely contains all of 'cap_face'. S2Point center = S2.GetNorm(cap_face); S2Cap covering = new(center, S1Angle.FromRadians(kFaceRadius + kEps)); Assert.Equal(cap_face == face, covering.Contains(root_cell)); Assert.Equal(cap_face != anti_face, covering.MayIntersect(root_cell)); Assert.Equal(center.DotProd(edge_cell.Center()) > 0.1, covering.Contains(edge_cell)); Assert.Equal(covering.MayIntersect(edge_cell), covering.Contains(edge_cell)); Assert.Equal(cap_face == face, covering.Contains(corner_cell)); Assert.Equal(center.DotProd(corner_cell.Center()) > 0, covering.MayIntersect(corner_cell)); // A cap that barely intersects the edges of 'cap_face'. S2Cap bulging = new(center, S1Angle.FromRadians(S2.M_PI_4 + kEps)); Assert.False(bulging.Contains(root_cell)); Assert.Equal(cap_face != anti_face, bulging.MayIntersect(root_cell)); Assert.Equal(cap_face == face, bulging.Contains(edge_cell)); Assert.Equal(center.DotProd(edge_cell.Center()) > 0.1, bulging.MayIntersect(edge_cell)); Assert.False(bulging.Contains(corner_cell)); Assert.False(bulging.MayIntersect(corner_cell)); // A singleton cap. S2Cap singleton = new(center, S1Angle.Zero); Assert.Equal(cap_face == face, singleton.MayIntersect(root_cell)); Assert.False(singleton.MayIntersect(edge_cell)); Assert.False(singleton.MayIntersect(corner_cell)); } } }