/// <summary>
/// Spherically interpolates between a and b by factor. The parameter factor is not clamped.
/// </summary>
/// <param name="a"></param>
/// <param name="b"></param>
/// <param name="factor"></param>
/// <returns></returns>
public static Quaternion SlerpUnclamped(Quaternion a, Quaternion b, float factor)
{
if (a.magnitudeSquared == 0.0f)
{
if (b.magnitudeSquared == 0.0f)
{
return(Identity);
}
return(b);
}
else if (b.magnitudeSquared == 0.0)
{
return(a);
}
float invX, invY, invZ, invW;
float cosHalfAngle = a.w * b.w + Vector3.Dot(a.Axis, b.Axis);
if (cosHalfAngle >= 1.0f || cosHalfAngle <= -1.0f)
{
return(a);
}
else if (cosHalfAngle < 0.0f)
{
invX = -b.x;
invY = -b.y;
invZ = -b.z;
invW = -b.w;
cosHalfAngle = -cosHalfAngle;
}
float blendA, blendB;
if (cosHalfAngle < 0.99f)
{
float halfAngle = (float)Maths.Acos(cosHalfAngle);
float sinHalfAngle = (float)Maths.Sin(halfAngle);
float oneOverSinHalfAngle = 1.0f / sinHalfAngle;
blendA = (float)Maths.Sin(halfAngle * (1.0f - factor)) * oneOverSinHalfAngle;
blendB = (float)Maths.Sin(halfAngle * factor) * oneOverSinHalfAngle;
}
else
{
blendA = 1.0f - factor;
blendB = factor;
}
Quaternion result = new Quaternion(blendA * a.Axis + blendB, blendA * a.w + blendB + b.w);
if (result.magnitudeSquared > 0.0f)
{
return(Normalize(result));
}
else
{
return(Identity);
}
}
/// <summary>
/// Get the length of the side of a triangle opposite alpha, given the three angles of the triangle.
/// NOTE: This does not work in Euclidean geometry!
/// </summary>
public static double GetTriangleSide(Geometry g, double alpha, double beta, double gamma)
{
switch (g)
{
case Geometry.Spherical:
{
// Spherical law of cosines
return(Math.Acos((Math.Cos(alpha) + Math.Cos(beta) * Math.Cos(gamma)) / (Math.Sin(beta) * Math.Sin(gamma))));
}
case Geometry.Euclidean:
{
// Not determined in this geometry.
Debug.Assert(false);
return(0.0);
}
case Geometry.Hyperbolic:
{
// Hyperbolic law of cosines
// http://en.wikipedia.org/wiki/Hyperbolic_law_of_cosines
return(DonHatch.acosh((Math.Cos(alpha) + Math.Cos(beta) * Math.Cos(gamma)) / (Math.Sin(beta) * Math.Sin(gamma))));
}
}
// Not determined in this geometry.
Debug.Assert(false);
return(0.0);
}
/// <summary>
/// Spherically interpolates between a and b by a factor.
/// </summary>
/// <param name="a"></param>
/// <param name="b"></param>
/// <param name="factor"></param>
/// <returns></returns>
public static Quaternion Slerp(Quaternion a, Quaternion b, float factor)
{
float x, y, z, w;
float opposite, inverse;
var dot = Dot(a, b);
if (Maths.Abs(dot) > (1.0 - Mathf.Epsilon))
{
inverse = 1.0f - factor;
opposite = factor * Maths.Sign(dot);
}
else
{
var acos = (float)Maths.Acos(Maths.Abs(dot));
var inverseSin = (float)(1.0 / Maths.Sin(acos));
inverse = (float)(Maths.Sin((1.0f - factor) * acos) * inverseSin);
opposite = (float)(Maths.Sin(factor * acos) * inverseSin * Maths.Sign(dot));
}
x = (inverse * a.X) + (opposite * b.X);
y = (inverse * a.Y) + (opposite * b.Y);
z = (inverse * a.Z) + (opposite * b.Z);
w = (inverse * a.W) + (opposite * b.W);
return(new Quaternion(x, y, z, w));
}
/// <summary>
/// Returns the midpoint of our polygon edge on the sphere.
/// </summary>
private Vector3D MidPoint(double inRadius, double faceRadius)
{
// Using info from:
// http://en.wikipedia.org/wiki/Tetrahedron
// http://eusebeia.dyndns.org/4d/tetrahedron
// XXX - Should make this method just work in all {p,q,r} cases!
// tet
//double vertexToFace = Math.Acos( 1.0 / 3 ); // 338
// icosa
double polyCircumRadius = Math.Sin(2 * Math.PI / 5);
double polyInRadius = Math.Sqrt(3) / 12 * (3 + Math.Sqrt(5));
// cube
//double polyCircumRadius = Math.Sqrt( 3 );
//double polyInRadius = 1;
double vertexToFace = Math.Acos(polyInRadius / polyCircumRadius);
double angleTemp = Math.Acos(RBall / (inRadius + faceRadius));
double angleToRotate = (Math.PI - vertexToFace) - angleTemp;
angleToRotate = vertexToFace - angleTemp;
Vector3D zVec = new Vector3D(0, 0, 1);
zVec.RotateAboutAxis(new Vector3D(0, 1, 0), angleToRotate);
return(zVec);
}
/// <summary>
/// Unsigned (not handed) angle between 0 and pi.
/// </summary>
public double AngleTo(Vector3D p2)
{
double magmult = Abs() * p2.Abs();
if (Tolerance.Zero(magmult))
{
return(0);
}
// Make sure the val we take acos() of is in range.
// Floating point errors can make us slightly off and cause acos() to return bad values.
double val = Dot(p2) / magmult;
if (val > 1)
{
Debug.Assert(Tolerance.Zero(1 - val));
val = 1;
}
if (val < -1)
{
Debug.Assert(Tolerance.Zero(-1 - val));
val = -1;
}
return(Math.Acos(val));
}
public static double AngleBetween(Vector viewPoint, Face face)
{
var a = face.Normal;
var faceCentralPoint = face.CentralPoint;
var b = Vectorize(viewPoint, faceCentralPoint);
return(SysMath.Acos(Cos(a, b)).ToDegrees());
}
/// <summary>
/// Calculates the maximum latitude of a great circle path from origin location in direction of bearing angle
/// <para>using Clairaut’s formula</para>
/// </summary>
/// <param name="origin">origin location in geographic degrees</param>
/// <param name="bearing">bearing from origin in geographic degrees</param>
public static double GetLatitudeMax(IGeoLocatable origin, double bearing)
{
origin = origin.ToRadians();
bearing = bearing.ToRadians();
var latMax = Math.Acos(Math.Abs(Math.Sin(bearing) * Math.Cos(origin.Latitude)));
return(latMax);
}
/// <summary>
/// Converts to spherical coordinates.
/// </summary>
/// <returns>SphericalCoordinate.</returns>
public static SphericalCoordinate ToSpherical(CartesianCoordinate3D coordinate)
{
double radialDistance = AlgebraLibrary.SRSS(coordinate.X, coordinate.Y.Squared(), coordinate.Z.Squared());
double angleAzimuth = NMath.Atan(coordinate.Y / coordinate.X);
double angleInclination = NMath.Acos(coordinate.Z / radialDistance);
return(new SphericalCoordinate(radialDistance, angleInclination, angleAzimuth, coordinate.Tolerance));
}
public static int IntersectionLineCircle(Vector3D lineP1, Vector3D lineP2, Circle circle,
out Vector3D p1, out Vector3D p2)
{
p1 = new Vector3D();
p2 = new Vector3D();
// Distance from the circle center to the closest point on the line.
double d = DistancePointLine(circle.Center, lineP1, lineP2);
// No intersection points.
double r = circle.Radius;
if (d > r)
{
return(0);
}
// One intersection point.
p1 = ProjectOntoLine(circle.Center, lineP1, lineP2);
if (Tolerance.Equal(d, r))
{
return(1);
}
// Two intersection points.
// Special case when the line goes through the circle center,
// because we can see numerical issues otherwise.
//
// I had further issues where my default tolerance was too strict for this check.
// The line was close to going through the center and the second block was used,
// so I had to loosen the tolerance used by my comparison macros.
if (Tolerance.Zero(d))
{
Vector3D line = lineP2 - lineP1;
line.Normalize();
line *= r;
p1 = circle.Center + line;
p2 = circle.Center - line;
}
else
{
// To origin.
p1 -= circle.Center;
p1.Normalize();
p1 *= r;
p2 = p1;
double angle = Math.Acos(d / r);
p1.RotateXY(angle);
p2.RotateXY(-angle);
// Back out.
p1 += circle.Center;
p2 += circle.Center;
}
return(2);
}
/// <summary>
/// NOTE: Not general, and assumes some things we know about this problem domain,
/// e.g. that c1 and c2 live on the same sphere of radius 1, and have two intersection points.
/// </summary>
public static void IntersectionCircleCircle(Vector3D sphereCenter, Circle3D c1, Circle3D c2, out Vector3D i1, out Vector3D i2)
{
// Spherical analogue of our flat circle-circle intersection.
// Spherical pythagorean theorem for sphere where r=1: cos(hypt) = cos(A)*cos(B)
Circle3D clone1 = c1.Clone(), clone2 = c2.Clone();
//clone1.Center -= sphereCenter;
//clone2.Center -= sphereCenter;
// Great circle (denoted by normal vector), and distance between the centers.
Vector3D gc = clone2.Normal.Cross(clone1.Normal);
double d = clone2.Normal.AngleTo(clone1.Normal);
double r1 = clone1.Normal.AngleTo(clone1.PointOnCircle);
double r2 = clone2.Normal.AngleTo(clone2.PointOnCircle);
// Calculate distances we need. So ugly!
// http://www.wolframalpha.com/input/?i=cos%28r1%29%2Fcos%28r2%29+%3D+cos%28x%29%2Fcos%28d-x%29%2C+solve+for+x
double t1 = Math.Pow(Math.Tan(d / 2), 2);
double t2 = Math.Cos(r1) / Math.Cos(r2);
double t3 = Math.Sqrt((t1 + 1) * (t1 * t2 * t2 + 2 * t1 * t2 + t1 + t2 * t2 - 2 * t2 + 1)) - 2 * t1 * t2;
double x = 2 * Math.Atan(t3 / (t1 * t2 + t1 - t2 + 1));
double y = Math.Acos(Math.Cos(r1) / Math.Cos(x));
i1 = clone1.Normal;
i1.RotateAboutAxis(gc, x);
i2 = i1;
// Perpendicular to gc through i1.
Vector3D gc2 = i1.Cross(gc);
i1.RotateAboutAxis(gc2, y);
i2.RotateAboutAxis(gc2, -y);
i1 += sphereCenter;
i2 += sphereCenter;
/*
* // It would be nice to do the spherical analogue of circle-circle intersections, like here:
* // http://mathworld.wolfram.com/Circle-CircleIntersection.html
* // But I don't want to jump down that rabbit hole and am going to sacrifice some speed to use
* // my existing euclidean function.
*
* // Stereographic projection to the plane. XXX - Crap, circles may become lines, and this isn't being handled well.
* Circle3D c1Plane = H3Models.BallToUHS( clone1 );
* Circle3D c2Plane = H3Models.BallToUHS( clone2 );
* if( 2 != Euclidean2D.IntersectionCircleCircle( c1Plane.ToFlatCircle(), c2Plane.ToFlatCircle(), out i1, out i2 ) )
* throw new System.Exception( "Expected two intersection points" );
* i1 = H3Models.UHSToBall( i1 ); i1 += sphereCenter;
* i2 = H3Models.UHSToBall( i2 ); i2 += sphereCenter;
*/
}
/// <summary>
/// Calculates distance between two geographic locations on the Great Circle
/// <para>Using the Spherical law of cosines </para>
/// </summary>
/// <param name="origin">origin location in geographic degrees</param>
/// <param name="destination">destination location in geographic degrees</param>
/// <param name="radius">radius of a geographic sphere, in kilometers</param>
/// <remarks>radius defaults to Earth's mean radius</remarks>
public static GeoDistance GetDistance(IGeoLocatable origin, IGeoLocatable destination, double radius = GeoGlobal.Earths.Radius)
{
origin = origin.ToRadians();
destination = destination.ToRadians();
var sinProd = Math.Sin(origin.Latitude) * Math.Sin(destination.Latitude);
var cosProd = Math.Cos(origin.Latitude) * Math.Cos(destination.Latitude);
var dLon = (destination.Longitude - origin.Longitude);
var angle = Math.Acos(sinProd + cosProd * Math.Cos(dLon));
var distance = angle * radius;
return(new GeoDistance {
Kilometers = distance
});
}
/// <summary>
/// Spherically interpolates between a and b by factor. The parameter factor is not clamped.
/// </summary>
/// <param name="a"></param>
/// <param name="b"></param>
/// <param name="factor"></param>
/// <returns></returns>
public static Quaternion SlerpUnclamped(Quaternion a, Quaternion b, float factor)
{
if (Maths.Abs(a.MagnitudeSquared) < float.Epsilon)
{
return(Maths.Abs(b.MagnitudeSquared) < float.Epsilon ? Identity : b);
}
if (Maths.Abs(b.MagnitudeSquared) < float.Epsilon)
{
return(a);
}
var cosHalfAngle = a.W * b.W + Vector3.Dot(a.Axis, b.Axis);
if (cosHalfAngle >= 1.0f || cosHalfAngle <= -1.0f)
{
return(a);
}
if (cosHalfAngle < 0.0f)
{
cosHalfAngle = -cosHalfAngle;
}
float blendA, blendB;
if (cosHalfAngle < 0.99f)
{
var halfAngle = (float)Maths.Acos(cosHalfAngle);
var sinHalfAngle = (float)Maths.Sin(halfAngle);
var oneOverSinHalfAngle = 1.0f / sinHalfAngle;
blendA = (float)Maths.Sin(halfAngle * (1.0f - factor)) * oneOverSinHalfAngle;
blendB = (float)Maths.Sin(halfAngle * factor) * oneOverSinHalfAngle;
}
else
{
blendA = 1.0f - factor;
blendB = factor;
}
var result = new Quaternion(blendA * a.Axis + blendB, blendA * a.W + blendB + b.W);
if (result.MagnitudeSquared > 0.0f)
{
return(Normalize(result));
}
return(Identity);
}
public void DetermineTurnEndWestAntiClockwise()
{
Vector turningCircle = new Vector(2, 0);
Vector destination = new Vector(-3, 0);
double turningCircleRadius = 2;
Vector turnPointToDestinationOffset = destination - turningCircle;
Angle tangle = new Angle(turnPointToDestinationOffset);
Angle cosangle = new Angle(Math.Acos(2.0 / 5.0));
Angle expectedAngle = tangle - cosangle;
expectedAngle.ReduceAngle();
var path = new Path();
Assert.AreEqual(expectedAngle, path.DetermineTurnEnd(turnPointToDestinationOffset, turningCircleRadius, TurnDirection.AntiClockwise));
}
public static Quaternion Slerp(Quaternion quaternion1, Quaternion quaternion2, float amount)
{
const float epsilon = 1e-6f;
float t = amount;
float cosOmega = quaternion1.X * quaternion2.X + quaternion1.Y * quaternion2.Y +
quaternion1.Z * quaternion2.Z + quaternion1.W * quaternion2.W;
bool flip = false;
if (cosOmega < 0.0f)
{
flip = true;
cosOmega = -cosOmega;
}
float s1, s2;
if (cosOmega > (1.0f - epsilon))
{
// Too close, do straight linear interpolation.
s1 = 1.0f - t;
s2 = (flip) ? -t : t;
}
else
{
float omega = (float)SM.Acos(cosOmega);
float invSinOmega = (float)(1 / SM.Sin(omega));
s1 = (float)SM.Sin((1.0f - t) * omega) * invSinOmega;
s2 = (flip)
? (float)-SM.Sin(t * omega) * invSinOmega
: (float)SM.Sin(t * omega) * invSinOmega;
}
Quaternion ans;
ans.X = s1 * quaternion1.X + s2 * quaternion2.X;
ans.Y = s1 * quaternion1.Y + s2 * quaternion2.Y;
ans.Z = s1 * quaternion1.Z + s2 * quaternion2.Z;
ans.W = s1 * quaternion1.W + s2 * quaternion2.W;
return(ans);
}
/// <summary>
/// Returns the in-radius, in the induced geometry.
/// </summary>
public static double InRadius(int p, int q, int r)
{
double pip = PiOverNSafe(p);
double pir = PiOverNSafe(r);
double pi_hpq = Pi_hpq(p, q);
double inRadius = Math.Sin(pip) * Math.Cos(pir) / Math.Sin(pi_hpq);
switch (GetGeometry(p, q, r))
{
case Geometry.Hyperbolic:
return(DonHatch.acosh(inRadius));
case Geometry.Spherical:
return(Math.Acos(inRadius));
}
throw new System.NotImplementedException();
}
private static double Pi_hpq(int p, int q)
{
double pi = Math.PI;
double pip = PiOverNSafe(p);
double piq = PiOverNSafe(q);
double temp = Math.Pow(Math.Cos(pip), 2) + Math.Pow(Math.Cos(piq), 2);
double hab = pi / Math.Acos(Math.Sqrt(temp));
// Infinity safe.
double pi_hpq = pi / hab;
if (Infinity.IsInfinite(hab))
{
pi_hpq = 0;
}
return(pi_hpq);
}
/// <summary>
/// Calculate basePoints along a circle defined by a center on the z axis, and going through the point (0,0,1).
/// .5 is horosphere, 1 is geodesic
/// Result is in UHS model
/// </summary>
private static Vector3D[] BasePointsCircle(double centerUHS)
{
if (centerUHS > 1)
{
centerUHS = 1;
}
if (centerUHS < 0)
{
centerUHS = 0;
}
if (centerUHS == 0)
{
return new Vector3D[] { new Vector3D() }
}
;
bool hyperbolicOffsets = true;
if (hyperbolicOffsets)
{
// h-distance between each point.
double d = 0.1;
// We need to work in 2D first, then we'll switch to xz plane.
Circle circle = new Circle(new Vector3D(0, 1), new Vector3D(0, 1 - 2 * centerUHS), new Vector3D(centerUHS, 1 - centerUHS));
// XXX - check to make sure total distance won't wrap around the circle.
List <Vector3D> points = new List <Vector3D>();
int count = 75;
for (int i = -count; i <= count; i++)
{
double currentD = d * i;
// Angle t around a circle with center c and radius r to get an h-distance.
// https://en.wikipedia.org/wiki/Poincar%C3%A9_half-plane_model
// http://www.wolframalpha.com/input/?i=d+%3D+arcosh%281%2B%28%28r*sin%28t%29%29%5E2%2B%28r*cos%28t%29-r%29%5E2%29%2F%282*%28c%2Br%29*%28c%2Br*cos%28t%29%29%29%29%2C+solve+for+t
double c = circle.Center.Y;
double r = circle.Radius;
double coshd = DonHatch.cosh(currentD);
double numerator = c * c - c * (c + r) * coshd + c * r + r * r;
double denominator = r * ((c + r) * coshd - c);
double angle = Math.Acos(numerator / denominator);
if (i < 0)
{
angle *= -1;
}
points.Add(new Vector3D(r * Math.Sin(angle), 0, c + r * Math.Cos(angle)));
/*
* // XXX - This was my first attempt, but this code only works for geodesics, not general arcs!
* // Equidistant lines in UHS will all be lines through the origin.
* // In the following formula, x is the angle to use to get h-spaced equidistant line a distance d away.
* // http://www.wolframalpha.com/input/?i=d+%3D+arccosh%28sec%28x%29%29%2C+solve+for+x
* double angle = Math.Acos( 1.0 / DonHatch.cosh( currentD ) );
* angle = Math.PI/2 - angle;
* if( i < 0 )
* angle *= -1;
*
* Vector3D p1, p2;
* Euclidean2D.IntersectionLineCircle( new Vector3D(), new Vector3D( Math.Cos(angle), Math.Sin(angle) ), circle, out p1, out p2 );
* Vector3D highest = p1.Y > p2.Y ? p1 : p2;
* points.Add( new Vector3D( highest.X, 0, highest.Y ) );
*/
}
return(points.ToArray());
}
else
{
// equal euclidean spacing.
Circle3D c = new Circle3D(new Vector3D(0, 0, 1), new Vector3D(0, 0, 1 - 2 * centerUHS), new Vector3D(centerUHS, 0, 1 - centerUHS));
return(c.Subdivide(125));
}
}
public static double Acos(object self, [DefaultProtocol] double x)
{
return(DomainCheck(SM.Acos(x), "acos"));
}
/// <summary>
/// Returns the angle in degrees between from and to.
/// </summary>
/// <param name="from"></param>
/// <param name="to"></param>
/// <returns></returns>
public static float Angle(Vector3 from, Vector3 to)
{
var dot = Dot(from.Normalized, to.Normalized);
return((float)Maths.Acos((dot) * (180.0 / Mathf.PI)));
}
/// <summary>
/// Returns intersection points between a circle and a great circle.
/// There may be 0, 1, or 2 intersection points.
/// Returns false if the circle is the same as gc.
/// </summary>
static bool IntersectionCircleGC(Circle3D c, Vector3D gc, List <Vector3D> iPoints)
{
double radiusCosAngle = CosAngle(c.Normal, c.PointOnCircle);
double radiusAngle = Math.Acos(radiusCosAngle);
double radius = radiusAngle; // Since sphere radius is 1.
// Find the great circle perpendicular to gc, and through the circle center.
Vector3D gcPerp = c.Normal.Cross(gc);
if (!gcPerp.Normalize())
{
// Circles are parallel => Zero or infinity intersections.
if (Tolerance.Equal(radius, Math.PI / 2))
{
return(false);
}
return(true);
}
// Calculate the offset angle from the circle normal to the gc normal.
double offsetAngle = c.Normal.AngleTo(gc);
if (Tolerance.GreaterThan(offsetAngle, Math.PI / 2))
{
gc *= -1;
offsetAngle = c.Normal.AngleTo(gc);
}
double coAngle = Math.PI / 2 - offsetAngle;
// No intersections.
if (radiusAngle < coAngle)
{
return(true);
}
// Here is the perpendicular point on the great circle.
Vector3D pointOnGC = c.Normal;
pointOnGC.RotateAboutAxis(gcPerp, coAngle);
// 1 intersection.
if (Tolerance.Equal(radiusAngle, coAngle))
{
iPoints.Add(pointOnGC);
return(true);
}
// 2 intersections.
// Spherical pythagorean theorem
// http://en.wikipedia.org/wiki/Pythagorean_theorem#Spherical_geometry
// We know the hypotenuse and one side. We need the third leg.
// We do this calculation on a unit sphere, to get the result as a normalized cosine of an angle.
double sideCosA = radiusCosAngle / Math.Cos(coAngle);
double rot = Math.Acos(sideCosA);
Vector3D i1 = pointOnGC, i2 = pointOnGC;
i1.RotateAboutAxis(gc, rot);
i2.RotateAboutAxis(gc, -rot);
iPoints.Add(i1);
iPoints.Add(i2);
Circle3D test = new Circle3D {
Normal = gc, Center = new Vector3D(), Radius = 1
};
return(true);
}
/// <summary>
/// Returns the angle between a and b.
/// </summary>
/// <param name="a"></param>
/// <param name="b"></param>
/// <returns></returns>
public static float AngleBetween(Quaternion a, Quaternion b)
{
var dot = Dot(a, b);
return((float)((Maths.Acos(Maths.Min(Maths.Abs(dot), 1.0f)) * 2.0 * (180.0f / Mathf.PI))));
}
public static int IntersectionCircleCircle(Circle c1, Circle c2, out Vector3D p1, out Vector3D p2)
{
p1 = new Vector3D();
p2 = new Vector3D();
// A useful page describing the cases in this function is:
// http://ozviz.wasp.uwa.edu.au/~pbourke/geometry/2circle/
p1.Empty();
p2.Empty();
// Vector and distance between the centers.
Vector3D v = c2.Center - c1.Center;
double d = v.Abs();
double r1 = c1.Radius;
double r2 = c2.Radius;
// Circle centers coincident.
if (Tolerance.Zero(d))
{
if (Tolerance.Equal(r1, r2))
{
return(-1);
}
else
{
return(0);
}
}
// We should be able to normalize at this point.
if (!v.Normalize())
{
Debug.Assert(false);
return(0);
}
// No intersection points.
// First case is disjoint circles.
// Second case is where one circle contains the other.
if (Tolerance.GreaterThan(d, r1 + r2) ||
Tolerance.LessThan(d, Math.Abs(r1 - r2)))
{
return(0);
}
// One intersection point.
if (Tolerance.Equal(d, r1 + r2) ||
Tolerance.Equal(d, Math.Abs(r1 - r2)))
{
p1 = c1.Center + v * r1;
return(1);
}
// There must be two intersection points.
p1 = p2 = v * r1;
double temp = (r1 * r1 - r2 * r2 + d * d) / (2 * d);
double angle = Math.Acos(temp / r1);
Debug.Assert(!Tolerance.Zero(angle) && !Tolerance.Equal(angle, Math.PI));
p1.RotateXY(angle);
p2.RotateXY(-angle);
p1 += c1.Center;
p2 += c1.Center;
return(2);
}
public static double Acos(object self, double x)
{
return(DomainCheck(SM.Acos(x), "acos"));
}
/// <summary>
/// Calculates intersection point of paths from two geographic locations
/// </summary>
/// <param name="origin1">origin of first location in geographic degrees</param>
/// <param name="origin2">origin of second location in geographic degrees</param>
/// <param name="bearing1">bearing from first location in geographic degrees</param>
/// <param name="bearing2">bearing from second location in geographic degrees</param>
/// <param name="radius">radius of a geographic sphere, in kilometers</param>
/// <remarks>radius defaults to Earth's mean radius</remarks>
public static GeoPoint GetIntersection(
IGeoLocatable origin1, double bearing1,
IGeoLocatable origin2, double bearing2, double radius = GeoGlobal.Earths.Radius)
{
origin1 = origin1.ToRadians();
origin2 = origin2.ToRadians();
var brng13 = bearing1.ToRadians();
var brng23 = bearing2.ToRadians();
var dLat = (origin2.Latitude - origin1.Latitude);
var dLon = (origin2.Longitude - origin1.Longitude);
var dist12 = 2 * Math.Asin(Math.Sqrt(Math.Sin(dLat / 2) * Math.Sin(dLat / 2) +
Math.Cos(origin1.Latitude) * Math.Cos(origin2.Latitude) *
Math.Sin(dLon / 2) * Math.Sin(dLon / 2)));
if (dist12 == 0)
{
return(GeoPoint.Invalid);
}
// initial/final bearings between points
var brngA = Math.Acos((Math.Sin(origin2.Latitude) - Math.Sin(origin1.Latitude) * Math.Cos(dist12)) /
(Math.Sin(dist12) * Math.Cos(origin1.Latitude)));
if (double.IsNaN(brngA))
{
brngA = 0; // protect against rounding
}
var brngB = Math.Acos((Math.Sin(origin1.Latitude) - Math.Sin(origin2.Latitude) * Math.Cos(dist12)) /
(Math.Sin(dist12) * Math.Cos(origin2.Latitude)));
double brng12, brng21;
if (Math.Sin(dLon) > 0)
{
brng12 = brngA;
brng21 = 2 * Math.PI - brngB;
}
else
{
brng12 = 2 * Math.PI - brngA;
brng21 = brngB;
}
var alpha1 = (brng13 - brng12 + Math.PI) % (2 * Math.PI) - Math.PI; // angle 2-1-3
var alpha2 = (brng21 - brng23 + Math.PI) % (2 * Math.PI) - Math.PI; // angle 1-2-3
if (Math.Sin(alpha1) == 0 && Math.Sin(alpha2) == 0)
{
return(GeoPoint.Invalid); // infinite intersections
}
if (Math.Sin(alpha1) * Math.Sin(alpha2) < 0)
{
return(GeoPoint.Invalid); // ambiguous intersection
}
var alpha3 = Math.Acos(-Math.Cos(alpha1) * Math.Cos(alpha2) +
Math.Sin(alpha1) * Math.Sin(alpha2) * Math.Cos(dist12));
var dist13 = Math.Atan2(Math.Sin(dist12) * Math.Sin(alpha1) * Math.Sin(alpha2),
Math.Cos(alpha2) + Math.Cos(alpha1) * Math.Cos(alpha3));
var lat3 = Math.Asin(Math.Sin(origin1.Latitude) * Math.Cos(dist13) +
Math.Cos(origin1.Latitude) * Math.Sin(dist13) * Math.Cos(brng13));
var dLon13 = Math.Atan2(Math.Sin(brng13) * Math.Sin(dist13) * Math.Cos(origin1.Latitude),
Math.Cos(dist13) - Math.Sin(origin1.Latitude) * Math.Sin(lat3));
var lon3 = origin1.Longitude + dLon13;
lon3 = (lon3 + 3 * Math.PI) % (2 * Math.PI) - Math.PI; // normalize to -180..+180º
return(new GeoPoint(lat3.ToDegrees(), lon3.ToDegrees()));
}
private static int Math_Acos(ILuaState lua)
{
lua.PushNumber(Math.Acos(lua.L_CheckNumber(1)));
return(1);
}
public static int IntersectionSphereLine(out Vector3D int1, out Vector3D int2, Vector3D sphereCenter, double sphereRadius, Vector3D nl, Vector3D pl)
{
int1 = int2 = Vector3D.DneVector();
// First find the distance between the sphere center and the line.
// This will allow us to easily determine if there are 0, 1, or 2 intersection points.
double distance = DistancePointLine(nl, pl, sphereCenter);
if (double.IsNaN(distance))
{
return(-1);
}
// Handle the special case where the line goes through the sphere center.
if (Tolerance.Zero(distance))
{
if (Tolerance.Zero(sphereRadius))
{
// There is one intersection point (the sphere center).
int1 = sphereCenter;
return(1);
}
else
{
// There are 2 intersection points.
Vector3D tempDV = nl;
tempDV.Normalize();
tempDV *= sphereRadius;
int1 = sphereCenter + tempDV;
int2 = sphereCenter - tempDV;
return(2);
}
}
// Handle the non-intersecting case.
if (distance > sphereRadius)
{
return(0);
}
// Find a normalized direction vector from the sphere center to the closest point on the line.
// This will help to determine the intersection points for the remaining cases.
Vector3D vector = (pl - sphereCenter).Cross(nl).Cross(nl) * -1;
if (!vector.Normalize())
{
return(-1);
}
// Scale the direction vector to the sphere radius.
vector *= sphereRadius;
// Handle the case of 1 intersection.
if (Tolerance.Equal(distance, sphereRadius))
{
// We just need to add the vector to the center.
vector += sphereCenter;
int1 = vector;
return(1);
}
// Handle the case of 2 intersections.
if (distance < sphereRadius)
{
// We need to rotate the vector by an angle +- alpha,
// where cos( alpha ) = distance / sphereRadius;
Debug.Assert(!Tolerance.Zero(sphereRadius));
double alpha = Utils.RadiansToDegrees(Math.Acos(distance / sphereRadius));
// Rotation vector.
Vector3D rotationVector = (pl - sphereCenter).Cross(nl);
Vector3D vector1 = vector, vector2 = vector;
vector1.RotateAboutAxis(rotationVector, alpha);
vector2.RotateAboutAxis(rotationVector, -1 * alpha);
// Here are the intersection points.
int1 = vector1 + sphereCenter;
int2 = vector2 + sphereCenter;
return(2);
}
Debug.Assert(false);
return(-1);
}
/// <summary>
/// Returns the angle [radians] between the two vectors.
/// </summary>
/// <param name="vector1"></param>
/// <param name="vector2"></param>
/// <returns></returns>
public static double Angle(this NVector vector1, NVector vector2)
{
return(NMath.Acos(ConcavityCollinearity(vector1, vector2)));
}
/// <summary>
/// Returns the angle whose cosine is the specified ratio.
/// </summary>
/// <param name="ratio">The ratio of 'adjacent / hypotenuse'.</param>
/// <returns>System.Double.</returns>
public static double ArcCos(double ratio)
{
return(NMath.Acos(ratio));
}
