Coordinate systems

% This function converts cartesian coördinates X, Y, Z to double polar coördinates (r, elevation, azimuth)
% Double polar coördinates is a non-standard coördinate system used for audiological purposes only.
%
% The algorith uses the inverse of: dotproduct(V1,V2) =
% |V1|*|V2|*cos(angle) to determine the angle.
% To calculate the angle with a plane the dotproduct with the normal of the plane is calculated first.
% The angle with the plane is pi/2 - angle with normal.
% One also needs to take the quadrant into consideration.

% X is left (-) to right (+)
% Y is back (-) to front (+)
% Z is bottom (-) to top (+)
% Middle of the sphere is (0, 0, 0)

% azimuth is angle (in rad) with YZ plane with Y-hat is zero and X-hat is
% pi/2.
% elevation is angle (in rad) with XY plane with Y-hat is zero Z-hat is
% pi/2.

function [azimuth, elevation, r] = cart2double_polar(X, Y, Z)

    r = sqrt(X^2+Y^2+Z^2);
            
    %azimuth: projection to XY plane, angle with Y_hat
    az_angleWithY_hat = getAngle([X,Y,0],[0,1,0]);

    %elevation: projection to YZ plane, angle with Y_hat
    el_angleWithY_hat = getAngle([0,Y,Z],[0,1,0]);
    
    % azimuth from -pi to +pi
    switch findQuadrant(X, Y) % Quadrants are numbered anti-clockwise
        case {1,4}
            azimuth = az_angleWithY_hat;            
        case {2, 3}
            azimuth = - az_angleWithY_hat;
        case 0 % (X=0 or Y=0)
            azimuth = 0;
    end

     % elevation from -pi/2 to +pi/2
    switch findQuadrant(Y, Z) 
        case 1 
            elevation = el_angleWithY_hat;
        case 2
            elevation = pi - el_angleWithY_hat;
        case 3
            elevation = el_angleWithY_hat - pi;
        case 4
            elevation = - el_angleWithY_hat;
        case 0 % (Y=0 or Z=0)
            elevation = 0;
    end