/// <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;
*/
}