Optic_flow_RF_estimation_Zhang_et_al_2022/code/General/floQ.m

classdef floQ < quaternion
    % floQ is an expansion of quaternion
    % Adding many attributes and functions to make ray tracing with
    % quaternion easy to compute
    
    properties %
        w
        x
        y
        z
    end
    
    properties (SetAccess = private)
        u
        v
    end
    
    methods
        function obj = floQ(inputMat)
            input_size = size(inputMat);
            switch class(inputMat)
                case "double"
                    if input_size(end) == 3
                        Qmat = cat(length(input_size),zeros([input_size(1:end-1),1]),inputMat);
                    elseif input_size(end) == 4
                        Qmat = inputMat;
                    elseif input_size(end) == 2
                        Qmat = reshape(inputMat,[],2);
                        [Qmat_x,Qmat_y,Q_mat_z] = sph2cart(Qmat(:,1),Qmat(:,2),ones(size(Qmat(:,1))));
                        Qmat = [zeros(size(Qmat_x)),Qmat_x,Qmat_y,Q_mat_z];
                    else
                        error("the input array should be N-by-4, instead it is N-by-%i",input_size(end))
                    end
                    Qmat = reshape(Qmat,[],4);
                    if length(input_size)>2
                        Qsize = input_size(1:end-1);
                    else
                        Qsize = [input_size(1),1];
                    end
                case "quaternion"
                    Qmat = inputMat.compact;
                    Qsize = input_size;
                case "floQ"
                    Qmat = inputMat.compact;
                    Qsize = input_size;
            end
            obj@quaternion(Qmat);
            obj  = reshape(obj,Qsize);
%             obj  = update_parts(obj);
        end
        
        function quat_obj = as_quaternion(obj)
            quat_obj = reshape(quaternion(obj.compact),size(obj));
        end
        
        function Qmatrix = asMatrix(obj)
            Qmatrix = reshape(obj.compact,[size(obj),4]);
        end
        
        function obj = set.w(obj,warray)
            obj.w = warray;
            obj.a = warray;
        end
        
        function qw = get.w(obj)
            qw = obj.a;
        end
        
        function obj = set.x(obj,xarray)
            obj.x = xarray;
            obj.b = xarray;
        end
        
        function qx = get.x(obj)
            qx = obj.b;
        end
        
        function obj = set.y(obj,yarray)
            obj.y = yarray;
            obj.c = yarray;
        end
        
        function qy = get.y(obj)
            qy = obj.c;
        end
        
        function obj = set.z(obj,zarray)
            obj.z = zarray;
            obj.d = zarray;
        end
        
        function qz = get.z(obj)
            qz = obj.d;
        end
        
        function qu = get.u(obj)
            qu = atan2(obj.c,obj.b);
        end
        
        function qv = get.v(obj)
            xynorm = hypot(obj.b,obj.c);
            qv = atan2(obj.d,xynorm);
        end
        
         function azi = azi(obj)
            azi = obj.u;
        end
        
        function elv = elv(obj)
            elv = obj.v;
        end
        
        function qparts = U(obj,unitName)
            if strcmp(unitName,'imag')
                unitName  = 'xyz';
            end
            qU = cell(1,6);
            [qU{1},qU{2},qU{3},qU{4}] = obj.parts;
            [qU{5},qU{6}] = cart2sph(qU{2},qU{3},qU{4});
            [UindexMat,~] = find(~(unitName-('wxyzuv')'));
            qU = qU(UindexMat);
            qparts = cat(obj.ndims+1,qU{:});
        end
        
        function qparts = qU(obj,unitName)
            switch unitName
                case 'imag'
                    unitName  = 'xyz';
                case 'real'
                    unitName  = 'w';
            end
            fn = {'w','x','y','z'};
            [UindexMat,~] = find(~(unitName-('wxyz')'));
            qparts = obj.*0;
            for i = UindexMat'
                qparts.(fn{i}) = obj.(fn{i});
            end
        end
        
        function realQobj = real(obj)
            realQobj = obj.w;
        end
        
        function imagQobj = imag(obj)
            imagQobj = obj.U('xyz');
        end
        
        function imagQobj = Qimag(obj)
            imagQobj = obj;
            imagQobj.w = obj.w.*0;
        end
        
        function realQobj = Qreal(obj)
            realQobj = obj.*0;
            realQobj.w = obj.w;
        end
        
        
        function newobj = horzcat(varargin)
            Q_array = cellfun(@as_quaternion,varargin,'UniformOutput',false);
            newobj = floQ(horzcat(Q_array{:}));
        end
        
        function newobj = vertcat(varargin)
            Q_array = cellfun(@as_quaternion,varargin,'UniformOutput',false);
            newobj = floQ(vertcat(Q_array{:}));
        end
        
        function newobj = cat(dim_cat,varargin)
            Q_array = cellfun(@as_quaternion,varargin,'UniformOutput',false);
            newobj = floQ(cat(dim_cat,Q_array{:}));
        end
        
        function obj = T(obj)
            nd  = ndims(obj);
            if nd == 2
                obj = permute(obj,[2,1]);
            elseif nd > 2
                obj = permute(obj,[2,1,3:nd]);
            end
        end
        
        function obj_sum = sum(obj,varargin)
            mat_obj         = obj.asMatrix;
            sum_mat_obj     = sum(mat_obj,varargin{:});
            obj_sum         = floQ(sum_mat_obj);
        end
        
        function obj_mean = mean(obj,varargin)
            if numel(obj) > 1
                mat_obj         = obj.asMatrix;
                mean_mat_obj    = mean(mat_obj,varargin{:});
                obj_mean        = floQ(mean_mat_obj);
            else
                obj_mean        = obj;
            end
        end
        
        function obj_cs = cumsum(obj,varargin)
            mat_obj         = obj.asMatrix;
            sum_mat_obj     = cumsum(mat_obj,varargin{:});
            obj_cs          = floQ(sum_mat_obj);
        end
        
        function crossProduct = Qcross(obj,obj2,outputForm) %%%%%%%
            if ~exist('outputForm','var')
                outputForm = 'floQ';
            end
            switch outputForm
                case 'floQ'
                    crossProduct = Qimag(obj.*obj2);
                case 'matrix'
                    crossProduct = obj.*obj2;
                    crossProduct = crossProduct.U('imag');
            end
        end
        
        function dotProduct = Qdot(obj,obj2,outputForm) %%%%%%%
            if ~exist('outputForm','var')
                outputForm = 'floQ';
            end
            switch outputForm
                case 'floQ'
                    dotProduct = -Qreal(obj.*obj2);
                case 'matrix'
                    dotProduct = obj.*obj2;
                    dotProduct = -dotProduct.w;
            end
        end
        
        function interAng = Qangle(obj,obj2) %%%%%%%
            % Calculate the angle between two vectors
            interAng    = asin(norm(obj.normalize - obj2.normalize)./2).*2;
        end
        
        function lmMat = get_leftMultiplicationMatrix(obj) %%%%%%%
            lmMat = [obj.a, obj.b, obj.c, obj.d;...
                    obj.b,-obj.a, obj.d,-obj.c;...
                    obj.c,-obj.d,-obj.a, obj.b;...
                    obj.d, obj.c,-obj.b,-obj.a];
        end
        
        function RotVec = asRotVec(obj,rotangle)
            RotVec = exp(rotangle/2.*obj.normalize);
        end
        
        function rotated = Qrot(obj,point2rot,rotangle)
            if ~exist('rotangle','var')
                rotated = obj.*point2rot.*obj.conj;
            else
                obj_rot = obj.asRotVec(rotangle);
                rotated = obj_rot.*point2rot.*obj_rot.conj;
            end
        end
        
        function QrotMat = getRotMat(obj)
            if numel(obj) == 1
                lmMat = obj.get_leftMultiplicationMatrix;
                QrotMat = lmMat*lmMat;
            else
                QrotMat = [];
            end
        end
        
        function reflected = Qreflect(obj,points)
            reflected = obj.*points.*obj;
        end
        
        function QreflectMat = getReflectMat(obj)
            if numel(obj) == 1 && norm(get_real(obj))<10^-5
                lmMat = obj.get_leftMultiplicationMatrix;
                QreflectMat = -lmMat*lmMat;
            else
                QreflectMat = [];
            end
        end
        
        function projected = Qproject(obj,points,proj_type)
            if ~exist('proj_type','var')
                proj_type = 'plane';
            end
%             n_obj = obj.normalize;
            switch proj_type
                case 'plane'
                    projected = (points+obj.*points.*obj)./2;
                case 'line'
                    projected = (points-obj.*points.*obj)./2;
            end
        end
        
        function plength = project_length(obj,points)
            cos_angle = Qangle(obj,points);
            plength = norm(points).*cos_angle;
        end
        
        function QprojectMat = getProjectMat(obj)
            QreflectMat = getReflectMat(obj);
            QprojectMat = (eye(4)+QreflectMat)./2;
        end
        
        function transVec = rotTo(obj,target)
            target     = target.normalize;
            realpart   = Qdot(obj,target);
            imagpart   = Qcross(obj,target);
            realpart   = realpart+norm(realpart+imagpart);
            transVec   = imagpart+realpart;
            transVec   = normalize(transVec);
        end
        
        function [azi,elv,R] = asSphCoord(obj)
            cartcoord = obj.Qimag;
            [azi,elv,R] = cart2sph(cartcoord.x,cartcoord.y,cartcoord.z);
        end
    end
end