function Qmat = eulerangle(r_set, angles)
    %Created by Alyssa Novelia -- 4/30/2013
    %Revised by Daniel Kawano -- 11/30/2017
    
    %Sample input:
    %r_seq = [3 2 1];           %for 321 euler angle
    %angles = [pi/2 1 pi/3];    %in radians
    
    %Given Euler angles and sequence, calculate a rotation matrix
    %equivalent to the product of 3 rotations and animate the rotation
    %sequence.
    
    %Animation: View how fixed basis {p1,p2,p3} is transformed into
    %{t1,t2,t3}, and how Euler basis {g1,g2,g3} is formed. Code works when
    %angles entered correspond to the singularity of the basis, however one
    %can see that two of the Euler basis are parallel to each other and
    %can't span 3D space.
    
    anim = 1;   %set to 0 to disable animation
    steps = 50; %number of animation steps
    psi = angles(1); theta = angles(2); phi = angles(3);
    arrow_color = {'k' 'b' 'r' 'g' 'c'};    %color of the basis vectors
    
    %limit r_set into 3 or 2 or 1 - quit function otherwise
    if isempty(find(r_set > 3))
        if isempty(find(r_set < 1))
            
            %define fixed basis, rotate psi rad about g1
            p1 = [1; 0; 0];
            p2 = [0; 1; 0];
            p3 = [0; 0; 1];
            eval(['g1 = p' num2str(r_set(1)) ';']);     %g1 is p1
            [t1p_set t2p_set t3p_set Rmat_1st_set] = calcRmatrix(psi, g1, p1, p2, p3, steps);

            %after rotating psi rad about g1, rotate theta rad about g2
            t1p = t1p_set(:,end);
            t2p = t2p_set(:,end);
            t3p = t3p_set(:,end);
            eval(['g2 = t' num2str(r_set(2)) 'p;']);    %g2 is t2p
            [t1pp_set t2pp_set t3pp_set Rmat_2nd_set] = calcRmatrix(theta, g2, t1p, t2p, t3p, steps);

            %after rotating psi rad about g1 and theta rad about g2, rotate phi rad
            %about g3
            t1pp = t1pp_set(:,end);
            t2pp = t2pp_set(:,end);
            t3pp = t3pp_set(:,end);
            eval(['g3 = t' num2str(r_set(3)) 'pp;']);   %g3 is t3pp
            [t1_set t2_set t3_set Rmat_3rd_set] = calcRmatrix(phi, g3, t1pp, t2pp, t3pp, steps);

            %transformed fixed basis
            t1 = t1_set(:,end);
            t2 = t2_set(:,end);
            t3 = t3_set(:,end);

            %rotation matrix
            Qmat = [t1 t2 t3];

            if anim == 1
                f = figure('color', 'w', 'position', [375 198 622 388]); hold on;
                axis equal; axis([-1 1 -1 1 -1 1])
                xlabel('\itx\rm_1'); ylabel('\itx\rm_2'); zlabel('\itx\rm_3', 'rotation', 0);
                view([135 30]);
                set(f, 'name', ([num2str(r_set(1)) '-' num2str(r_set(2)) '-' num2str(r_set(3)) ' set of Euler angles']), 'color', 'w');
                grid on;

                %fixed
                p1v = quiver3(0,0,0,p1(1),p1(2),p1(3),'color', arrow_color{1}, 'LineWidth',2);
                p2v = quiver3(0,0,0,p2(1),p2(2),p2(3),'color', arrow_color{1}, 'LineWidth',2);
                p3v = quiver3(0,0,0,p3(1),p3(2),p3(3),'color', arrow_color{1}, 'LineWidth',2);
                text(p1(1), p1(2), p1(3), '\bfp\rm_1');
                text(p2(1), p2(2), p2(3), '\bfp\rm_2');
                text(p3(1), p3(2), p3(3), '\bfp\rm_3');

                %moving
                e1v = quiver3(0,0,0,p1(1),p1(2),p1(3),'color', arrow_color{1},'LineWidth',2);
                e2v = quiver3(0,0,0,p2(1),p2(2),p2(3),'color', arrow_color{1},'LineWidth',2);
                e3v = quiver3(0,0,0,p3(1),p3(2),p3(3),'color', arrow_color{1},'LineWidth',2);
                
                %path traced by the moving basis
                e1pa = line(0,0,0,'Color', 'k', 'LineWidth', 1, 'LineStyle', '-.');
                e2pa = line(0,0,0,'Color', 'k', 'LineWidth', 1, 'LineStyle', '-.');
                e3pa = line(0,0,0,'Color', 'k', 'LineWidth', 1, 'LineStyle', '-.');

                for kkk = 1:steps
                    title(['Rotate \psi = ' num2str(angles(1)) ' rad about p_' num2str(r_set(3))]);
                    if kkk == 1
                        pause;
                        g1v = line([0 g1(1)],[0 g1(2)],[0 g1(3)],'Color',arrow_color{5},'LineStyle','--','LineWidth',2);
                    end
                    set(e1v, 'xdata', 0, 'ydata', 0, 'zdata', 0, 'udata', t1p_set(1,kkk), 'vdata', t1p_set(2,kkk), 'wdata', t1p_set(3,kkk), 'Color', arrow_color{2}, 'LineWidth', 2);
                    set(e2v, 'xdata', 0, 'ydata', 0, 'zdata', 0, 'udata', t2p_set(1,kkk), 'vdata', t2p_set(2,kkk), 'wdata', t2p_set(3,kkk), 'Color', arrow_color{2}, 'LineWidth', 2);
                    set(e3v, 'xdata', 0, 'ydata', 0, 'zdata', 0, 'udata', t3p_set(1,kkk), 'vdata', t3p_set(2,kkk), 'wdata', t3p_set(3,kkk), 'Color', arrow_color{2}, 'LineWidth', 2);
                    
                    set(e1pa, 'xdata', 0.5*t1p_set(1,1:kkk), 'ydata', 0.5*t1p_set(2,1:kkk), 'zdata', 0.5*t1p_set(3,1:kkk));
                    set(e2pa, 'xdata', 0.5*t2p_set(1,1:kkk), 'ydata', 0.5*t2p_set(2,1:kkk), 'zdata', 0.5*t2p_set(3,1:kkk));
                    set(e3pa, 'xdata', 0.5*t3p_set(1,1:kkk), 'ydata', 0.5*t3p_set(2,1:kkk), 'zdata', 0.5*t3p_set(3,1:kkk));
                    drawnow;
                end
                t1p_txt = text(t1p(1), t1p(2), t1p(3), '\bft\rm_{1}^{\prime}');
                t2p_txt = text(t2p(1), t2p(2), t2p(3), '\bft\rm_{2}^{\prime}');
                t3p_txt = text(t3p(1), t3p(2), t3p(3), '\bft\rm_{3}^{\prime}');
                
                t1p_end = quiver3(0,0,0,t1p(1), t1p(2), t1p(3),'color', arrow_color{2},'LineWidth',2);
                t2p_end = quiver3(0,0,0,t1p(1), t1p(2), t1p(3),'color', arrow_color{2},'LineWidth',2);
                t3p_end = quiver3(0,0,0,t1p(1), t1p(2), t1p(3),'color', arrow_color{2},'LineWidth',2);
                
                for kkk = 1:steps
                    title(['Rotate \theta = ' num2str(angles(2)) ' rad about t_' num2str(r_set(2)) '^{\prime}']);
                    if kkk == 1
                        pause;
                        g2v = line([0 g2(1)],[0 g2(2)],[0 g2(3)],'Color',arrow_color{5},'LineStyle','--','LineWidth',2);
                        delete(t1p_txt);
                        delete(t2p_txt);
                        delete(t3p_txt);
                    end
                    set(e1v, 'xdata', 0, 'ydata', 0, 'zdata', 0, 'udata', t1pp_set(1,kkk), 'vdata', t1pp_set(2,kkk), 'wdata', t1pp_set(3,kkk), 'Color', arrow_color{3}, 'LineWidth', 2);
                    set(e2v, 'xdata', 0, 'ydata', 0, 'zdata', 0, 'udata', t2pp_set(1,kkk), 'vdata', t2pp_set(2,kkk), 'wdata', t2pp_set(3,kkk), 'Color', arrow_color{3}, 'LineWidth', 2);
                    set(e3v, 'xdata', 0, 'ydata', 0, 'zdata', 0, 'udata', t3pp_set(1,kkk), 'vdata', t3pp_set(2,kkk), 'wdata', t3pp_set(3,kkk), 'Color', arrow_color{3}, 'LineWidth', 2);
                    
                    set(e1pa, 'xdata', 0.5*t1pp_set(1,1:kkk), 'ydata', 0.5*t1pp_set(2,1:kkk), 'zdata', 0.5*t1pp_set(3,1:kkk));
                    set(e2pa, 'xdata', 0.5*t2pp_set(1,1:kkk), 'ydata', 0.5*t2pp_set(2,1:kkk), 'zdata', 0.5*t2pp_set(3,1:kkk));
                    set(e3pa, 'xdata', 0.5*t3pp_set(1,1:kkk), 'ydata', 0.5*t3pp_set(2,1:kkk), 'zdata', 0.5*t3pp_set(3,1:kkk));
                    drawnow;
                end
                t1pp_txt = text(t1pp(1), t1pp(2), t1pp(3), '\bft\rm_{1}^{\prime\prime}');
                t2pp_txt = text(t2pp(1), t2pp(2), t2pp(3), '\bft\rm_{2}^{\prime\prime}');
                t3pp_txt = text(t3pp(1), t3pp(2), t3pp(3), '\bft\rm_{3}^{\prime\prime}');
                
                t1pp_end = quiver3(0,0,0,t1pp(1), t1pp(2), t1pp(3),'color', arrow_color{3},'LineWidth',2);
                t2pp_end = quiver3(0,0,0,t1pp(1), t1pp(2), t1pp(3),'color', arrow_color{3},'LineWidth',2);
                t3pp_end = quiver3(0,0,0,t1pp(1), t1pp(2), t1pp(3),'color', arrow_color{3},'LineWidth',2);

                for kkk = 1:steps
                    title(['Rotate \phi = ' num2str(angles(3)) ' rad about t_' num2str(r_set(3)) '^{\prime\prime}']);
                    if kkk == 1
                        pause;
                        g3v = line([0 g3(1)],[0 g3(2)],[0 g3(3)],'Color',arrow_color{5},'LineStyle','--','LineWidth',2);
                        delete(t1pp_txt);
                        delete(t2pp_txt);
                        delete(t3pp_txt);
                    end
                    set(e1v, 'xdata', 0, 'ydata', 0, 'zdata', 0, 'udata', t1_set(1,kkk), 'vdata', t1_set(2,kkk), 'wdata', t1_set(3,kkk), 'Color', arrow_color{4}, 'LineWidth', 2);
                    set(e2v, 'xdata', 0, 'ydata', 0, 'zdata', 0, 'udata', t2_set(1,kkk), 'vdata', t2_set(2,kkk), 'wdata', t2_set(3,kkk), 'Color', arrow_color{4}, 'LineWidth', 2);
                    set(e3v, 'xdata', 0, 'ydata', 0, 'zdata', 0, 'udata', t3_set(1,kkk), 'vdata', t3_set(2,kkk), 'wdata', t3_set(3,kkk), 'Color', arrow_color{4}, 'LineWidth', 2);
                    
                    set(e1pa, 'xdata', 0.5*t1_set(1,1:kkk), 'ydata', 0.5*t1_set(2,1:kkk), 'zdata', 0.5*t1_set(3,1:kkk));
                    set(e2pa, 'xdata', 0.5*t2_set(1,1:kkk), 'ydata', 0.5*t2_set(2,1:kkk), 'zdata', 0.5*t2_set(3,1:kkk));
                    set(e3pa, 'xdata', 0.5*t3_set(1,1:kkk), 'ydata', 0.5*t3_set(2,1:kkk), 'zdata', 0.5*t3_set(3,1:kkk));
                    drawnow;
                end
                
                text(t1(1), t1(2), t1(3), '\bft\rm_{1}');
                text(t2(1), t2(2), t2(3), '\bft\rm_{2}');
                text(t3(1), t3(2), t3(3), '\bft\rm_{3}');
                text(g1(1)+0.1, g1(2)+0.1, g1(3)+0.1, '\bfg\rm_{1}');
                text(g2(1)+0.1, g2(2)+0.1, g2(3)+0.1, '\bfg\rm_{2}');
                text(g3(1)+0.1, g3(2)+0.1, g3(3)+0.1, '\bfg\rm_{3}');
                
                t1_end = quiver3(0,0,0,t1(1), t1(2), t1(3),'color', arrow_color{4},'LineWidth',2);
                t2_end = quiver3(0,0,0,t1(1), t1(2), t1(3),'color', arrow_color{4},'LineWidth',2);
                t3_end = quiver3(0,0,0,t1(1), t1(2), t1(3),'color', arrow_color{4},'LineWidth',2);
            end
        else
            error('Invalid Euler angle set');
        end
    else
        error('Invalid Euler angle set');
    end
end


function [t1, t2, t3, Rmat] = calcRmatrix(theta, Raxis, p1, p2, p3, steps)
    %Use Euler's representation to calculate the rotation matrix Rmat that
    %rotates [p1 p2 p3] to [t1 t2 t3] through axis Raxis and angle theta.
    
    %Raxis, p1, p2, p3 are 3x1 column vectors that consist of the [x y z]'.
    
    % ti = Rmat(theta, Raxis) * pi
    
    %If steps are specified, break down the rotation into rotation sequence
    %for animation purposes. If steps are not specified, assume steps = 1

    if ~exist('steps', 'var')
        steps = 1;
    end
    
    ANGLE = linspace(0,theta,steps);

    Rmat = [];
    t1 = [];
    t2 = [];
    t3 = [];
    
    %Calculate symmetric parts and skew symmetric part
    Rsym1 = [1-Raxis(1)^2 -Raxis(1)*Raxis(2) -Raxis(1)*Raxis(3); -Raxis(2)*Raxis(1) 1-Raxis(2)^2 -Raxis(2)*Raxis(3); -Raxis(3)*Raxis(1) -Raxis(3)*Raxis(2) 1-Raxis(3)^2];
    Rsym2 = [Raxis(1)^2 Raxis(1)*Raxis(2) Raxis(1)*Raxis(3); Raxis(2)*Raxis(1) Raxis(2)^2 Raxis(2)*Raxis(3); Raxis(3)*Raxis(1) Raxis(3)*Raxis(2) Raxis(3)^2];
    Rssym = [0 Raxis(3) -Raxis(2); -Raxis(3) 0 Raxis(1); Raxis(2) -Raxis(1) 0];
    
    for jjj = 1:length(ANGLE)
        Rmat(:,:,jjj) = cos(ANGLE(jjj))*Rsym1 + Rsym2 - sin(ANGLE(jjj))*Rssym;
        t1(:,jjj) = Rmat(:,:,jjj)*p1;
        t2(:,jjj) = Rmat(:,:,jjj)*p2;
        t3(:,jjj) = Rmat(:,:,jjj)*p3;
    end
end