#
# Demonstrates and adaptively reduced the degeneracies of Gimbell Lock.
#
#						Gershon Elber, April 1995.
#

#
# Extracts and displays a half a sphere.
#
s = sregion( sphereSrf( 1 ), ROW, 0, 1 );
viewobj( s );

#
# Computes a curve on a sphere between two spheical angles by interpolating
# between the two angles a piecewise linear curve.
#
ComputeSphericalCrv = function( Teta1, Teta2, Phi1, Phi2 ):PtList:t:
    PtList = nil():
    for ( t = 0, 1, 100,
	Teta = (Teta2 * t + Teta1 * (100 - t)) * Pi / (180 * 100):
	Phi = (Phi2 * t + Phi1 * (100 - t)) * Pi / (180 * 100):
	snoc( point( cos(Teta) * cos(Phi), cos(Teta) * sin(Phi), sin(Teta) ),
	      PtList ) ):
    return = cbspline( 2, PtList, list( KV_OPEN ) ):
    attrib( return, "dwidth", 3 ):
    color( return, red );

#
# Computes a unit vector that is perpendicular to the great arc defined
# by the two spherical angular coordinates.
#
ComputeOrthoVector = function( Teta1, Teta2, Phi1, Phi2 ):Pt1:Pt2:Teta1d:Teta2d:Phi1d:Phi2d:
    Teta1d = Teta1 * Pi / 180:
    Teta2d = Teta2 * Pi / 180:
    Phi1d = Phi1 * Pi / 180:
    Phi2d = Phi2 * Pi / 180:
    Pt1 = point( cos(Teta1d) * cos(Phi1d), cos(Teta1d) * sin(Phi1d), sin(Teta1d) ):
    Pt2 = point( cos(Teta2d) * cos(Phi2d), cos(Teta2d) * sin(Phi2d), sin(Teta2d) ):
    return = coerce( normalize(Pt1 ^ Pt2), vector_type ):
    attrib( return, "dwidth", 3 ):
    color( return, green );

#
# Estimates the deviation of the linearly interpolated curve over Teta and
# Phi from the great arc between these two points.
#
EstimateSphericalCrv = function( Teta1, Teta2, Phi1, Phi2 ):OrthoVec:PtList:t:
    OrthoVec = ComputeOrthoVector( Teta1, Teta2, Phi1, Phi2 ):
    return = 0.0:
    for ( t = 0, 1, 100,
	Teta = (Teta2 * t + Teta1 * (100 - t)) * Pi / (180 * 100):
	Phi = (Phi2 * t + Phi1 * (100 - t)) * Pi / (180 * 100):
	Val = coord( OrthoVec, 0 ) * cos(Teta) * cos(Phi) +
	      coord( OrthoVec, 1 ) * cos(Teta) * sin(Phi) +
	      coord( OrthoVec, 2 ) * sin(Teta):
	if ( return < Val, return = Val ) );

#
# Computes the mid point on the great arc from 1 to 2.
#
MidGreatCircPt = function( Teta1, Teta2, Phi1, Phi2 ):x:y:z:Teta1d:Teta2d:Phi1d:Phi2d:
    Teta1d = Teta1 * Pi / 180:
    Teta2d = Teta2 * Pi / 180:
    Phi1d = Phi1 * Pi / 180:
    Phi2d = Phi2 * Pi / 180:
    x = ( cos(Teta1d) * cos(Phi1d) + cos(Teta2d) * cos(Phi2d) ) / 2.0:
    y = ( cos(Teta1d) * sin(Phi1d) + cos(Teta2d) * sin(Phi2d) ) / 2.0:
    z = ( sin(Teta1d) + sin(Teta2d) ) / 2.0:
#   printf("%f %f %f\\n", list(x, y, z)):
    Teta = atan2(z, sqrt(x*x + y*y)) * 180 / Pi:
    Phi = atan2(y, x) * 180 / Pi:
    return = list(Teta, Phi);


#
# Recursive computation of optimal motion.
# 
ComputeOptimalMotion = function( Teta1, Teta2, Phi1, Phi2, Eps ):
    return = 1.0;
ComputeOptimalMotion = function( Teta1, Teta2, Phi1, Phi2, Eps ):Err:NewPt:Teta:Phi:
    Err = EstimateSphericalCrv( Teta1, Teta2, Phi1, Phi2 ):
    printf(" %12g %12g %12g %12g = %f\\n", list( Teta1, Teta2, Phi1, Phi2, Err ) ):
    if ( Err > Eps,
         NewPt = MidGreatCircPt( Teta1, Teta2, Phi1, Phi2 ):
	 Teta = nth( NewPt, 1 ):
	 Phi = nth( NewPt, 2 ):
	 return = ComputeOptimalMotion( Teta1, teta, Phi1, Phi, Eps ) +
		  ComputeOptimalMotion( Teta, Teta2, Phi, Phi2,	Eps ),
	 return = list( ComputeSphericalCrv( Teta1, Teta2, Phi1, Phi2 ) ) );

c = ComputeSphericalCrv( 10, 85, 10, 80 );
v = ComputeOrthoVector( 10, 85, 10, 80 );
interact( list( s, c, v ) );

c2 = ComputeOptimalMotion( 10, 85, 10, 80, 0.5 );
interact( list( s, c2, v ) );

c2 = ComputeOptimalMotion( 10, 85, 10, 80, 0.25 );
interact( list( s, c2, v ) );

c2 = ComputeOptimalMotion( 10, 85, 10, 80, 0.1 );
interact( list( s, c2, v ) );

c2 = ComputeOptimalMotion( 10, 85, 10, 80, 0.05 );
interact( list( s, c2, v ) );





c = ComputeSphericalCrv( 10, 45, 10, 170 );
v = ComputeOrthoVector( 10, 45, 10, 170 );
interact( list( s, c, v ) );

c2 = ComputeOptimalMotion( 10, 45, 10, 170, 0.5 );
interact( list( s, c2, v ) );

c2 = ComputeOptimalMotion( 10, 45, 10, 170, 0.25 );
interact( list( s, c2, v ) );

c2 = ComputeOptimalMotion( 10, 45, 10, 170, 0.1 );
interact( list( s, c2, v ) );

c2 = ComputeOptimalMotion( 10, 45, 10, 170, 0.05 );
interact( list( s, c2, v ) );



c = ComputeSphericalCrv( 35, 85, 40, 70 );
v = ComputeOrthoVector( 35, 85, 40, 70 );
interact( list( s, c, v ) );

c2 = ComputeOptimalMotion( 35, 85, 40, 70, 0.5 );
interact( list( s, c2, v ) );

c2 = ComputeOptimalMotion( 35, 85, 40, 70, 0.1 );
interact( list( s, c2, v ) );

c2 = ComputeOptimalMotion( 35, 85, 40, 70, 0.025 );
interact( list( s, c2, v ) );

c2 = ComputeOptimalMotion( 35, 85, 40, 70, 0.01 );
interact( list( s, c2, v ) );




c = ComputeSphericalCrv( 70, 5, 70, 130 );
v = ComputeOrthoVector( 70, 5, 70, 130 );
interact( list( s, c, v ) );

c2 = ComputeOptimalMotion( 70, 5, 70, 130, 0.1 );
interact( list( s, c2, v ) );

c2 = ComputeOptimalMotion( 70, 5, 70, 130, 0.05 );
interact( list( s, c2, v ) );

c2 = ComputeOptimalMotion( 70, 5, 70, 130, 0.025 );
interact( list( s, c2, v ) );

c2 = ComputeOptimalMotion( 70, 5, 70, 130, 0.01 );
interact( list( s, c2, v ) );

free( c );
free( c2 );
free( v );
free( s );

