private static void TestRotate(S2Point p, S2Point axis, S1Angle angle)
{
S2Point result = S2.Rotate(p, axis, angle);
// "result" should be unit length.
Assert.True(result.IsUnitLength());
// "result" and "p" should be the same distance from "axis".
const double kMaxPositionError = S2.DoubleError;
Assert.True((new S1Angle(result, axis) - new S1Angle(p, axis)).Abs() <= kMaxPositionError);
// Check that the rotation angle is correct. We allow a fixed error in the
// *position* of the result, so we need to convert this into a rotation
// angle. The allowable error can be very large as "p" approaches "axis".
double axis_distance = p.CrossProd(axis).Norm();
double max_rotation_error;
if (axis_distance < kMaxPositionError)
{
max_rotation_error = S2.M_2_PI;
}
else
{
max_rotation_error = Math.Asin(kMaxPositionError / axis_distance);
}
double actual_rotation = S2.TurnAngle(p, axis, result) + Math.PI;
double rotation_error = Math.IEEERemainder(angle.Radians - actual_rotation,
S2.M_2_PI);
Assert.True(rotation_error <= max_rotation_error);
}
public void Test_S2_CoincidentZeroLengthEdgesThatDontTouch()
{
// It is important that the edge primitives can handle vertices that exactly
// exactly proportional to each other, i.e. that are not identical but are
// nevertheless exactly coincident when projected onto the unit sphere.
// There are various ways that such points can arise. For example,
// Normalize() itself is not idempotent: there exist distinct points A,B
// such that Normalize(A) == B and Normalize(B) == A. Another issue is
// that sometimes calls to Normalize() are skipped when the result of a
// calculation "should" be unit length mathematically (e.g., when computing
// the cross product of two orthonormal vectors).
//
// This test checks pairs of edges AB and CD where A,B,C,D are exactly
// coincident on the sphere and the norms of A,B,C,D are monotonically
// increasing. Such edge pairs should never intersect. (This is not
// obvious, since it depends on the particular symbolic perturbations used
// by S2Pred.Sign(). It would be better to replace this with a test that
// says that the CCW results must be consistent with each other.)
int kIters = 1000;
for (int iter = 0; iter < kIters; ++iter)
{
// Construct a point P where every component is zero or a power of 2.
var t = new double[3];
for (int i = 0; i < 3; ++i)
{
int binary_exp = S2Testing.Random.Skewed(11);
t[i] = (binary_exp > 1022) ? 0 : Math.Pow(2, -binary_exp);
}
// If all components were zero, try again. Note that normalization may
// convert a non-zero point into a zero one due to underflow (!)
var p = new S2Point(t).Normalize();
if (p == S2Point.Empty)
{
--iter; continue;
}
// Now every non-zero component should have exactly the same mantissa.
// This implies that if we scale the point by an arbitrary factor, every
// non-zero component will still have the same mantissa. Scale the points
// so that they are all distinct and are still very likely to satisfy
// S2.IsUnitLength (which allows for a small amount of error in the norm).
S2Point a = (1 - 3e-16) * p;
S2Point b = (1 - 1e-16) * p;
S2Point c = p;
S2Point d = (1 + 2e-16) * p;
if (!a.IsUnitLength() || !d.IsUnitLength())
{
--iter;
continue;
}
// Verify that the expected edges do not cross.
Assert.True(0 > S2.CrossingSign(a, b, c, d));
S2EdgeCrosser crosser = new(a, b, c);
Assert.True(0 > crosser.CrossingSign(d));
Assert.True(0 > crosser.CrossingSign(c));
}
}
// 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());
}
public void Test_S2_RepeatedInterpolation()
{
// Check that points do not drift away from unit length when repeated
// interpolations are done.
for (int i = 0; i < 100; ++i)
{
S2Point a = S2Testing.RandomPoint();
S2Point b = S2Testing.RandomPoint();
for (int j = 0; j < 1000; ++j)
{
a = S2.Interpolate(a, b, 0.01);
}
Assert.True(a.IsUnitLength());
}
}
// 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());
}
// Returns the true centroid of the spherical triangle ABC multiplied by the
// signed area of spherical triangle ABC. The reasons for multiplying by the
// signed area are (1) this is the quantity that needs to be summed to compute
// the centroid of a union or difference of triangles, and (2) it's actually
// easier to calculate this way. All points must have unit length.
//
// Note that the result of this function is defined to be S2Point.Empty if
// the triangle is degenerate (and that this is intended behavior).
public static S2Point TrueCentroid(S2Point a, S2Point b, S2Point c)
{
System.Diagnostics.Debug.Assert(a.IsUnitLength());
System.Diagnostics.Debug.Assert(b.IsUnitLength());
System.Diagnostics.Debug.Assert(c.IsUnitLength());
// I couldn't find any references for computing the true centroid of a
// spherical triangle... I have a truly marvellous demonstration of this
// formula which this margin is too narrow to contain :)
// Use Angle() in order to get accurate results for small triangles.
var angle_a = b.Angle(c);
var angle_b = c.Angle(a);
var angle_c = a.Angle(b);
var ra = (angle_a == 0) ? 1 : (angle_a / Math.Sin(angle_a));
var rb = (angle_b == 0) ? 1 : (angle_b / Math.Sin(angle_b));
var rc = (angle_c == 0) ? 1 : (angle_c / Math.Sin(angle_c));
// Now compute a point M such that:
//
// [Ax Ay Az] [Mx] [ra]
// [Bx By Bz] [My] = 0.5 * det(A,B,C) * [rb]
// [Cx Cy Cz] [Mz] [rc]
//
// To improve the numerical stability we subtract the first row (A) from the
// other two rows; this reduces the cancellation error when A, B, and C are
// very close together. Then we solve it using Cramer's rule.
//
// The result is the true centroid of the triangle multiplied by the
// triangle's area.
//
// TODO(b/205027737): This code still isn't as numerically stable as it could
// be. The biggest potential improvement is to compute B-A and C-A more
// accurately so that (B-A)x(C-A) is always inside triangle ABC.
var x = new S2Point(a.X, b.X - a.X, c.X - a.X);
var y = new S2Point(a.Y, b.Y - a.Y, c.Y - a.Y);
var z = new S2Point(a.Z, b.Z - a.Z, c.Z - a.Z);
var r = new S2Point(ra, rb - ra, rc - ra);
return(0.5 * new S2Point(
y.CrossProd(z).DotProd(r),
z.CrossProd(x).DotProd(r),
x.CrossProd(y).DotProd(r)));
}
// A slightly more efficient version of Project() 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. Requires that x, a, and b have unit length.
public static S2Point Project(S2Point x, S2Point a, S2Point b, S2Point a_cross_b)
{
System.Diagnostics.Debug.Assert(a.IsUnitLength());
System.Diagnostics.Debug.Assert(b.IsUnitLength());
System.Diagnostics.Debug.Assert(x.IsUnitLength());
// TODO(ericv): When X is nearly perpendicular to the plane containing AB,
// the result is guaranteed to be close to the edge AB but may be far from
// the true projected result. This could be fixed by computing the product
// (A x B) x X x (A x B) using methods similar to S2::RobustCrossProd() and
// S2::GetIntersection(). However note that the error tolerance would need
// to be significantly larger in order for this calculation to succeed in
// double precision most of the time. For example to avoid higher precision
// when X is within 60 degrees of AB the minimum error would be 18 * DBL_ERR,
// and to avoid higher precision when X is within 87 degrees of AB the
// minimum error would be 120 * DBL_ERR.
// The following is not necessary to meet accuracy guarantees but helps
// to avoid unexpected results in unit tests.
if (x == a || x == b)
{
return(x);
}
// Find the closest point to X along the great circle through AB. Note that
// we use "n" rather than a_cross_b in the final cross product in order to
// avoid the possibility of underflow.
S2Point n = a_cross_b.Normalize();
S2Point p = S2.RobustCrossProd(n, x).CrossProd(n).Normalize();
// If this point is on the edge AB, then it's the closest point.
S2Point pn = p.CrossProd(n);
if (S2Pred.Sign(p, n, a, pn) > 0 && S2Pred.Sign(p, n, b, pn) < 0)
{
return(p);
}
// Otherwise, the closest point is either A or B.
return(((x - a).Norm2() <= (x - b).Norm2()) ? a : b);
}
// Return the area of triangle ABC. This method combines two different
// algorithms to get accurate results for both large and small triangles.
// The maximum error is about 5e-15 (about 0.25 square meters on the Earth's
// surface), the same as GirardArea() below, but unlike that method it is
// also accurate for small triangles. Example: when the true area is 100
// square meters, Area() yields an error about 1 trillion times smaller than
// GirardArea().
//
// All points should be unit length, and no two points should be antipodal.
// The area is always positive.
public static double Area(S2Point a, S2Point b, S2Point c)
{
System.Diagnostics.Debug.Assert(a.IsUnitLength());
System.Diagnostics.Debug.Assert(b.IsUnitLength());
System.Diagnostics.Debug.Assert(c.IsUnitLength());
// This method is based on l'Huilier's theorem,
//
// tan(E/4) = Math.Sqrt(tan(s/2) tan((s-a)/2) tan((s-b)/2) tan((s-c)/2))
//
// where E is the spherical excess of the triangle (i.e. its area),
// a, b, c, are the side lengths, and
// s is the semiperimeter (a + b + c) / 2 .
//
// The only significant source of error using l'Huilier's method is the
// cancellation error of the terms (s-a), (s-b), (s-c). This leads to a
// *relative* error of about 1e-16 * s / Math.Min(s-a, s-b, s-c). This compares
// to a relative error of about 1e-15 / E using Girard's formula, where E is
// the true area of the triangle. Girard's formula can be even worse than
// this for very small triangles, e.g. a triangle with a true area of 1e-30
// might evaluate to 1e-5.
//
// So, we prefer l'Huilier's formula unless dmin < s * (0.1 * E), where
// dmin = Math.Min(s-a, s-b, s-c). This basically includes all triangles
// except for extremely long and skinny ones.
//
// Since we don't know E, we would like a conservative upper bound on
// the triangle area in terms of s and dmin. It's possible to show that
// E <= k1 * s * Math.Sqrt(s * dmin), where k1 = 2*Math.Sqrt(3)/Pi (about 1).
// Using this, it's easy to show that we should always use l'Huilier's
// method if dmin >= k2 * s^5, where k2 is about 1e-2. Furthermore,
// if dmin < k2 * s^5, the triangle area is at most k3 * s^4, where
// k3 is about 0.1. Since the best case error using Girard's formula
// is about 1e-15, this means that we shouldn't even consider it unless
// s >= 3e-4 or so.
//
// TODO(ericv): Implement rigorous error bounds (analysis already done).
double sa = b.Angle(c);
double sb = c.Angle(a);
double sc = a.Angle(b);
double s = 0.5 * (sa + sb + sc);
if (s >= 3e-4)
{
// Consider whether Girard's formula might be more accurate.
double s2 = s * s;
double dmin = s - Math.Max(sa, Math.Max(sb, sc));
if (dmin < 1e-2 * s * s2 * s2)
{
// This triangle is skinny enough to consider using Girard's formula.
// We increase the area by the approximate maximum error in the Girard
// calculation in order to ensure that this test is conservative.
double area = GirardArea(a, b, c);
if (dmin < s * (0.1 * (area + 5e-15)))
{
return(area);
}
}
}
// Use l'Huilier's formula.
return(4 * Math.Atan(Math.Sqrt(Math.Max(0.0, Math.Tan(0.5 * s) * Math.Tan(0.5 * (s - sa)) *
Math.Tan(0.5 * (s - sb)) * Math.Tan(0.5 * (s - sc))))));
}
// This method is called to add a vertex to the chain when the vertex is
// represented as an S2Point. Requires that 'b' has unit length. Repeated
// vertices are ignored.
public void AddPoint(S2Point b)
{
System.Diagnostics.Debug.Assert(b.IsUnitLength());
AddInternal(b, new S2LatLng(b));
}
