% FUNCTION ANG = SPHERICAL_ANGLE( THETA_1, PHI_1, THETA_2, PHI_2 )
%
% Version 1 - 031014
% lathoud@idiap.ch
%
% Compute the angle between two bearings ( THETA_1, PHI_1 ) 
% and ( THETA_2, PHI_2 ) where THETA's are azimuths
% and PHI's are elevations.
%
% All angles are in radians, the result is in 
% the [ 0, pi ] interval.

function ang = spherical_angle( theta_1, phi_1, theta_2, phi_2 )
  
  if nargin < 4
    error( 'spherical_angle needs 4 parameters' );
  end
  
  delta_theta = theta_2 - theta_1;
  delta_phi   = phi_2   - phi_1;
  
  cos_dth = cos( delta_theta );
  sin_dth = sin( delta_theta );
  
  cos_dph = cos( delta_phi );
  sin_dph = sin( delta_phi );
  
  extra_dim = 1 + length( size( theta_1 ) );
  
  X = cat( extra_dim, ...
	   cos_dph .* cos_dth - 1, ...
	   cos_dph .* sin_dth, ...
	   sin_dph );
  
  d = sqrt( sum( X .^ 2, extra_dim ) );
  
  ang = 2 * asin( d / 2 );