classdef OrientationPlotter < fusion.theaterPlot.TheaterPlotter
%TrackPlotter Create a new orientation plotter
%
%   See:  <a href="matlab:help theaterPlot/orientationPlotter">orientationPlot/trackPlotter</a> 

%   Copyright 2017 The MathWorks, Inc.

    properties
        %HistoryDepth   Number of previous updates to display
        %   This property specifies the number of previous
        %   updates to render in the display.  It must be a
        %   value between 0 and 100.  If set to 0, then no
        %   previous updates are shown.  
        %
        %   The default is 0.
        HistoryDepth
    end
    
    properties (Dependent)
        %Marker    Marker symbol
        %   Specify Marker as one of the following values:
        %      .   point    s  square       ^ triangle (up)
        %      x   x-mark   d  diamond      v triangle (down)
        %      +   plus     p  pentagram    < triangle (left)
        %      *   star     h  hexagram     > triangle (right)
        %      o   circle (default)
        Marker
        
        %MarkerSize  Marker size
        %   A positive integer specifying the size of the marker.
        %
        %   The default is 10.
        MarkerSize
        
        % MarkerEdgeColor  Marker outline color
        %   Specify MarkerEdgeColor as a 'color' string or an [R, G, B]
        %   vector defining a color for all detections.  
        %
        %   The default is taken from the current theme.
        MarkerEdgeColor
        
        %MarkerFaceColor  Marker fill color  
        %   Specify MarkerFaceColor as a 'color' string or an [R, G, B]
        %   vector defining a color for all detections.   
        %
        %   The default is 'none'.
        MarkerFaceColor
        
        %LocalXAxisColor  Color for local x axis
        %   Specify LocalXAxisColor as a 'color' string or an [R, G, B]
        %   vector defining a color for all detections.   
        %
        %   The default is 'red'.
        LocalXAxisColor
        
        %LocalYAxisColor  Color for local y axis
        %   Specify LocalYAxisColor as a 'color' string or an [R, G, B]
        %   vector defining a color for all detections.   
        %
        %   The default is 'green'.
        LocalYAxisColor
        
        %LocalZAxisColor  Color for local z axis
        %   Specify LocalZAxisColor as a 'color' string or an [R, G, B]
        %   vector defining a color for all detections.   
        %
        %   The default is 'blue'.
        LocalZAxisColor
        
        %FontSize   Font size for detection labels
        %   Specify FontSize as used for labeling detections.  
        %
        %   The default is 10 points.
        FontSize
        
        %LabelOffset  Space between label and its position.
        %   LabelOffset is constant offset used to specify a gap between a 
        %   label and the positional point it annotates. Specify the offset
        %   as a three-element row vector containing the x and y offset 
        %   (in meters).
        %
        %   The default is [0 0 0].
        LabelOffset
        
        %LocalAxesLength  The length of line used to denote an axis
        %   LocalAxesLength is the length of line used to denote each of
        %   the local x- y- and z- axes of the given orientation.
        %
        %   The default is 1.
        LocalAxesLength
    end
    
    properties (Access = private)
        PositionsLine = []
        XAxisLine = []
        YAxisLine = []
        ZAxisLine = []
        HistoryXAxisLine = []
        HistoryYAxisLine = []
        HistoryZAxisLine = []
        HistoryXCoords = {}
        HistoryYCoords = {}
        HistoryZCoords = {}
        LabelText = []
        ParentAxes = []
        LabelFontSize = 10
        LabelTextOffset = [0 0 0]
        AxesLineLength = 1
    end
    
    methods
        function obj = OrientationPlotter(tp, varargin)
            obj@fusion.theaterPlot.TheaterPlotter(tp,varargin{:});
        end
       
        function value = get.Marker(obj)
            pLine = obj.PositionsLine;
            value = pLine.Marker;
        end
        
        function value = get.MarkerSize(obj)
            pLine = obj.PositionsLine;
            value = pLine.MarkerSize;
        end
        
        function value = get.MarkerFaceColor(obj)
            pLine = obj.PositionsLine;
            value = pLine.MarkerFaceColor;
        end
        
        function value = get.MarkerEdgeColor(obj)
            pLine = obj.PositionsLine;
            value = pLine.MarkerEdgeColor;
        end
        
        function value = get.LocalXAxisColor(obj)
            xLine = obj.XAxisLine;
            value = xLine.Color;
        end
        
        function value = get.LocalYAxisColor(obj)
            yLine = obj.YAxisLine;
            value = yLine.Color;
        end
        
        function value = get.LocalZAxisColor(obj)
            zLine = obj.ZAxisLine;
            value = zLine.Color;
        end
        
        function value = get.FontSize(obj)
            value = obj.LabelFontSize;
        end
        
        function value = get.LocalAxesLength(obj)
            value = obj.AxesLineLength;
        end
        
        function value = get.LabelOffset(obj)
            value = obj.LabelTextOffset;
        end
        
        function set.Marker(obj,value)
            pLine = obj.PositionsLine;
            if ishghandle(pLine)
                pLine.Marker = value;
            end
            drawnow limitrate
        end
        
        function set.MarkerSize(obj,value)
            pLine = obj.PositionsLine;
            if ishghandle(pLine)
                pLine.MarkerSize = value;
            end
            drawnow limitrate
        end
        
        function set.MarkerFaceColor(obj,value)
            pLine = obj.PositionsLine;
            if ishghandle(pLine)
                pLine.MarkerFaceColor = value;
            end
            drawnow limitrate
        end
        
        function set.MarkerEdgeColor(obj,value)
            pLine = obj.PositionsLine;
            if ishghandle(pLine)
                pLine.MarkerEdgeColor = value;
            end
            drawnow limitrate
        end
        
        function set.LocalXAxisColor(obj,value)
            hLine = obj.XAxisLine;
            if ishghandle(hLine)
                hLine.Color = value;
            end
            hLine = obj.HistoryXAxisLine;
            if ishghandle(hLine)
                hLine.Color = historyColor(obj,value);
            end
            drawnow limitrate
        end
        function set.LocalYAxisColor(obj,value)
            hLine = obj.YAxisLine;
            if ishghandle(hLine)
                hLine.Color = value;
            end
            hLine = obj.HistoryYAxisLine;
            if ishghandle(hLine)
                hLine.Color = historyColor(obj,value);
            end
            drawnow limitrate
        end
        function set.LocalZAxisColor(obj,value)
            hLine = obj.ZAxisLine;
            if ishghandle(hLine)
                hLine.Color = value;
            end
            hLine = obj.HistoryZAxisLine;
            if ishghandle(hLine)
                hLine.Color = historyColor(obj,value);
            end
            drawnow limitrate
        end
        
        function set.FontSize(obj,value)
            validateattributes(value,{'numeric'},{'positive','finite','real','scalar'});
            set(obj.LabelText(ishghandle(obj.LabelText)),'FontSize',value);
            obj.LabelFontSize = value;
            drawnow limitrate
        end
        
        function set.LocalAxesLength(obj,value)
            validateattributes(value,{'numeric'},{'positive','finite','real','scalar'});
            rescaleAxesLines(obj, obj.AxesLineLength, value);
            obj.AxesLineLength = value;
            drawnow limitrate
        end
        
        function set.LabelOffset(obj,value)
            validateattributes(value,{'numeric'},...
                {'finite','finite','real','size',[1 3]}, ...
                'orientationPlotter', 'LabelOffset');
            repositionLabels(obj, obj.LabelTextOffset, value);
            obj.LabelTextOffset = value;
            drawnow limitrate;
        end
        
        function clearData(obj)
            drawPositions(obj, zeros(0,3));
            drawOrientations(obj, zeros(0,3), zeros(0,3));
            drawLabels(obj, zeros(0,3), {});
            clearHistory(obj);
            drawHistory(obj);
            drawnow limitrate
        end
        
        function set.HistoryDepth(obj,value)
            validateattributes(value,{'numeric'},{'scalar','integer','nonnegative','<',100});
            clearHistory(obj);
            obj.HistoryDepth = value;
            drawHistory(obj);
            drawnow limitrate
        end

        function plotOrientation(obj, varargin)
            %plotOrientation plot a set of orientations in a theaterPlot
            %   plotOrientation(oPlotter, orientations) specifies the orientations
            %   of M objects to be shown for the orientations plotter, oPlotter.
            %   Specify orientations as an Mx1 array of quaternions or a 3x3xM 
            %   array of rotation matrices.
            % 
            %   plotOrientation(oPlotter, roll, pitch, yaw) specifies the 
            %   orientations of M objects whose positions are to be shown for 
            %   the tracks plotter, oPlotter. Specify roll, pitch, and yaw, 
            %   each as Mx1 vectors in degrees.
            % 
            %   plotOrientation(oPlotter, ..., positions) additionally
            %   specifies the positions of the objects as an M-by-3 matrix, 
            %   where each column of the matrix correspond to the X, Y and Z 
            %   coordinates of the objects' locations, respectively.  The 
            %   default value of positions is at the origin.
            % 
            %   plotOrientation(oPlotter, ..., positions, labels)
            %   additionally specifies the labels as an Mx1 cell array of
            %   character vectors that correspond to the M orientations.
            %   If omitted, no labels are plotted.
            %
            %     % Example:  Show the orientation of fused IMU and gyroscope data 
            %     % -------
            %     ld = load('rpy_9axis.mat'); 
            %     accel = ld.sensorData.Acceleration;;
            %     gyro = ld.sensorData.AngularVelocity;
            %     Fs  = ld.Fs;  % Hz
            %     decim = 2;    % Decimate by 2
            %     fuse = imufilter('SampleRate', Fs, 'DecimationFactor', decim);
            %   
            %     % obtain the pose information of the fused data
            %     pose = fuse(accel, gyro);
            %   
            %     % create a theater plot 
            %     tp = theaterPlot('XLimit',[-2 2],'YLimit',[-2 2],'ZLimit',[-2 2]);
            %  
            %     % add an orientation plotter to the theater plot
            %     op = orientationPlotter(tp,'DisplayName','fused data');
            %   
            %     % loop through the pose information
            %     for i=1:numel(pose)
            %         plotOrientation(op, pose(i));
            %         drawnow;
            %     end
    
            [orientations, varargin] = parseOrientations(varargin);
            [positions, varargin] = parseOrientationPositions(orientations, varargin);
            
            drawPositions(obj, positions);
            drawOrientations(obj, positions, orientations);
            
            [labels,varargin] = parseLabels(positions,varargin);
            drawLabels(obj, positions, labels);
            
            if ~isempty(varargin)
                error(message('shared_radarfusion:theaterPlot:UnrecognizedInput'));
            end
            
            drawHistory(obj);
            addHistory(obj,orientations,positions);
            drawnow limitrate
        end
    end
    
    methods (Access = protected)
        function initParser(obj, p)
            initParser@fusion.theaterPlot.TheaterPlotter(obj, p)
            addParameter(p,'HistoryDepth',0);
            addParameter(p,'Marker','o');
            addParameter(p,'MarkerSize',10);
            addParameter(p,'MarkerEdgeColor',[0 0 0]);
            addParameter(p,'MarkerFaceColor',[0 0 0]);
            addParameter(p,'FontSize',10);
            addParameter(p,'LocalAxesLength',1);
            addParameter(p,'LocalXAxisColor',[1 0 0]);
            addParameter(p,'LocalYAxisColor',[0 1 0]);
            addParameter(p,'LocalZAxisColor',[0 0 1]);
            addParameter(p,'LabelOffset',[0 0 0]);
        end

        function value = getPrimaryObject(obj)
            value = obj.PositionsLine;
        end
        
        function createGraphics(obj, hAxes, p)
            pr = p.Results;
            view(hAxes,60,30);
            set(hAxes,'YDir','reverse','ZDir','reverse');
            grid(hAxes,'on');
            grid(hAxes,'minor');
            
            if ~isempty(pr.DisplayName)
                pLine = line(hAxes, 0, 0, ...
                             'DisplayName',pr.DisplayName, ...
                             'Marker',pr.Marker, ...
                             'MarkerSize',pr.MarkerSize, ...
                             'MarkerEdgeColor',pr.MarkerEdgeColor, ...
                             'MarkerFaceColor',pr.MarkerFaceColor, ...
                             'LineStyle','none', ...
                             'Tag', ['tpTrackOrientationPositions_' pr.DisplayName]);
                xLine = line(hAxes, 0, 0, 0, ...
                             'Color', pr.LocalXAxisColor, ...
                             'DisplayName',['x-' pr.DisplayName], ...
                             'Tag', ['tpTrackLocalXAxis_' pr.DisplayName]);
                yLine = line(hAxes, 0, 0, 0, ...
                             'Color', pr.LocalYAxisColor, ...
                             'DisplayName',['y-' pr.DisplayName], ...
                             'Tag', ['tpTrackLocalYAxis_' pr.DisplayName]);
                zLine = line(hAxes, 0, 0, 0, ...
                             'Color', pr.LocalZAxisColor, ...
                             'DisplayName',['z-' pr.DisplayName], ...
                             'Tag', ['tpTrackLocalZAxis_' pr.DisplayName]);
            else
                pLine = line(hAxes, 0, 0, 0,...
                             'HandleVisibility','off', ...
                             'Marker',pr.Marker, ...
                             'MarkerSize',pr.MarkerSize, ...
                             'MarkerEdgeColor',pr.MarkerEdgeColor, ...
                             'MarkerFaceColor',pr.MarkerFaceColor, ...
                             'LineStyle','none', ...
                             'Tag', 'tpTrackOrientationPositions');
                xLine = line(hAxes, 0, 0, 0,...
                             'Color', pr.LocalXAxisColor, ...
                             'HandleVisibility','off', ...
                             'Tag', 'tpTrackLocalXAxis');
                yLine = line(hAxes, 0, 0, 0,...
                             'Color', pr.LocalYAxisColor, ...
                             'HandleVisibility','off', ...
                             'Tag', 'tpTrackLocalYAxis');
                zLine = line(hAxes, 0, 0, 0,...
                             'Color', pr.LocalZAxisColor, ...
                             'HandleVisibility','off', ...
                             'Tag', 'tpTrackLocalZAxis');
            end

            % apply theme defaults when using defaults
            pu = p.UsingDefaults;
            if ismember('MarkerEdgeColor',pu)
                matlab.graphics.internal.themes.specifyThemePropertyMappings( ...
                    pLine, 'MarkerEdgeColor', "--mw-graphics-borderColor-axes-primary");
            end

            if ismember('LocalXAxisColor',pu)
                matlab.graphics.internal.themes.specifyThemePropertyMappings(xLine, 'Color', "--mw-graphics-colorSpace-rgb-red");
            end
            if ismember('LocalYAxisColor',pu)
                matlab.graphics.internal.themes.specifyThemePropertyMappings(yLine, 'Color', "--mw-graphics-colorSpace-rgb-green");
            end
            if ismember('LocalZAxisColor',pu)
                matlab.graphics.internal.themes.specifyThemePropertyMappings(zLine, 'Color', "--mw-graphics-colorSpace-rgb-blue");
            end

            hxLine = line(hAxes, 0, 0, 0, ...
                         'HandleVisibility','off', ...
                         'Color', historyColor(obj, xLine.Color), ...
                         'Tag',['tpTrackHistoryX_' pr.DisplayName]);
            hyLine = line(hAxes, 0, 0, 0, ...
                         'HandleVisibility','off', ...
                         'Color', historyColor(obj, yLine.Color), ...
                         'Tag',['tpTrackHistoryY_' pr.DisplayName]);
            hzLine = line(hAxes, 0, 0, 0, ...
                         'HandleVisibility','off', ...
                         'Color', historyColor(obj, zLine.Color), ...
                         'Tag',['tpTrackHistoryZ_' pr.DisplayName]);
            
            set(pLine,'XData',[],'YData',[],'ZData',[]);
            set(xLine,'XData',[],'YData',[],'ZData',[]);
            set(yLine,'XData',[],'YData',[],'ZData',[]);
            set(zLine,'XData',[],'YData',[],'ZData',[]);
            set(hxLine,'XData',[],'YData',[],'ZData',[]);
            set(hyLine,'XData',[],'YData',[],'ZData',[]);
            set(hzLine,'XData',[],'YData',[],'ZData',[]);

            obj.ParentAxes = hAxes;
            obj.PositionsLine = pLine;
            obj.XAxisLine = xLine;
            obj.YAxisLine = yLine;
            obj.ZAxisLine = zLine;
            obj.HistoryXAxisLine = hxLine;            
            obj.HistoryYAxisLine = hyLine;            
            obj.HistoryZAxisLine = hzLine;            
            obj.HistoryDepth = pr.HistoryDepth;
            obj.LabelFontSize = pr.FontSize;
            obj.LabelOffset = pr.LabelOffset;
            obj.LocalAxesLength = pr.LocalAxesLength;
        end
        
        function deleteGraphics(obj)
            delete(obj.PositionsLine);
            delete(obj.XAxisLine);
            delete(obj.YAxisLine);
            delete(obj.ZAxisLine);
            delete(obj.HistoryXAxisLine);
            delete(obj.HistoryYAxisLine);
            delete(obj.HistoryZAxisLine);
        end
    end
    
    methods (Access = private)
        function color = historyColor(~, colorspec)
            % return faded color
            if isequal(colorspec,'none')
                color = 'none';
            else
                white = [1 1 1];
                color = white - (white - colorspec)/3;
            end
        end
        function drawPositions(obj, positions)
            pLine = obj.PositionsLine;
            if ishghandle(pLine)
                xData = positions(:,1);
                yData = positions(:,2);
                zData = positions(:,3);
                set(pLine,'XData',xData,'YData',yData,'ZData',zData);
            end
        end
        
        function rescaleAxesLine(~, aLine, oldScale, newScale)
            if ishghandle(aLine)
                lineXData = aLine.XData;
                lineYData = aLine.YData;
                lineZData = aLine.ZData;
                lineXData(2:3:end) = lineXData(1:3:end) + newScale/oldScale .* (lineXData(2:3:end) - lineXData(1:3:end));
                lineYData(2:3:end) = lineYData(1:3:end) + newScale/oldScale .* (lineYData(2:3:end) - lineYData(1:3:end));
                lineZData(2:3:end) = lineZData(1:3:end) + newScale/oldScale .* (lineZData(2:3:end) - lineZData(1:3:end));
                set(aLine, ...
                    'XData', lineXData(:), ...
                    'YData', lineYData(:), ...
                    'ZData', lineZData(:));
            end
        end
        
        function rescaleAxesLines(obj, oldScale, newScale)
            rescaleAxesLine(obj, obj.XAxisLine, oldScale, newScale);
            rescaleAxesLine(obj, obj.YAxisLine, oldScale, newScale);
            rescaleAxesLine(obj, obj.ZAxisLine, oldScale, newScale);
            rescaleAxesLine(obj, obj.HistoryXAxisLine, oldScale, newScale);
            rescaleAxesLine(obj, obj.HistoryYAxisLine, oldScale, newScale);
            rescaleAxesLine(obj, obj.HistoryZAxisLine, oldScale, newScale);
        end
        
        function drawOrientations(obj, positions, orientations)
            if ~isempty(orientations)
                % fencepost velocity vectors with NaN
                % to render all as a single line
                lineXXData = cat(1,positions(:,1)', ...
                                  (positions(:,1)+obj.AxesLineLength.*reshape(orientations(1,1,:),[],1))', ...
                                  NaN(1,size(positions,1)));
                lineXYData = cat(1,positions(:,2)', ...
                                  (positions(:,2)+obj.AxesLineLength.*reshape(orientations(1,2,:),[],1))', ...
                                  NaN(1,size(positions,1)));
                lineXZData = cat(1,positions(:,3)', ...
                                  (positions(:,3)+obj.AxesLineLength.*reshape(orientations(1,3,:),[],1))', ...
                                  NaN(1,size(positions,1)));
                lineYXData = cat(1,positions(:,1)', ...
                                  (positions(:,1)+obj.AxesLineLength.*reshape(orientations(2,1,:),[],1))', ...
                                  NaN(1,size(positions,1)));
                lineYYData = cat(1,positions(:,2)', ...
                                  (positions(:,2)+obj.AxesLineLength.*reshape(orientations(2,2,:),[],1))', ...
                                  NaN(1,size(positions,1)));
                lineYZData = cat(1,positions(:,3)', ...
                                  (positions(:,3)+obj.AxesLineLength.*reshape(orientations(2,3,:),[],1))', ...
                                  NaN(1,size(positions,1)));
                lineZXData = cat(1,positions(:,1)', ...
                                  (positions(:,1)+obj.AxesLineLength.*reshape(orientations(3,1,:),[],1))', ...
                                  NaN(1,size(positions,1)));
                lineZYData = cat(1,positions(:,2)', ...
                                  (positions(:,2)+obj.AxesLineLength.*reshape(orientations(3,2,:),[],1))', ...
                                  NaN(1,size(positions,1)));
                lineZZData = cat(1,positions(:,3)', ...
                                  (positions(:,3)+obj.AxesLineLength.*reshape(orientations(3,3,:),[],1))', ...
                                  NaN(1,size(positions,1)));
            else
                lineXXData = [];
                lineXYData = [];
                lineXZData = [];
                lineYXData = [];
                lineYYData = [];
                lineYZData = [];
                lineZXData = [];
                lineZYData = [];
                lineZZData = [];
            end
            
            xLine = obj.XAxisLine;
            yLine = obj.YAxisLine;
            zLine = obj.ZAxisLine;
            if ishghandle(xLine)
                set(xLine, ...
                    'XData', lineXXData(:), ...
                    'YData', lineXYData(:), ...
                    'ZData', lineXZData(:));
            end
            if ishghandle(yLine)
                set(yLine, ...
                    'XData', lineYXData(:), ...
                    'YData', lineYYData(:), ...
                    'ZData', lineYZData(:));
            end
            if ishghandle(zLine)
                set(zLine, ...
                    'XData', lineZXData(:), ...
                    'YData', lineZYData(:), ...
                    'ZData', lineZZData(:));
            end
        end
        
        function repositionLabels(obj, oldOffset, newOffset)
            hText = obj.LabelText;
            for i=1:numel(hText)
                if ishghandle(hText(i))
                    set(hText(i),'Position', get(hText(i),'Position') + newOffset - oldOffset);
                end
            end
        end
        
        function drawLabels(obj, positions, labels)

            hText = obj.LabelText;
            hAxes = obj.ParentAxes;
            fontSize = obj.LabelFontSize;

            if ishghandle(obj.PositionsLine)
               dispName = obj.DisplayName;
                for i=1:numel(labels)
                    if i<=numel(hText) && ishghandle(hText(i))
                        set(hText(i),'Position',positions(i,:)+obj.LabelTextOffset,'String',labels{i});
                    else
                        hText(i) = text(hAxes, positions(i,1),positions(i,2),positions(i,3),labels{i}, ...
                            'VerticalAlignment','bottom', ...
                            'Clipping','on', ...
                            'FontSize',fontSize, ...
                            'Color', 1 - hAxes.Color, ...
                            'Tag',sprintf('tpTrackLabels_%s',dispName));
                    end
                end
                
                % delete unused text objects
                hUnusedText = hText(numel(labels)+1:end);
                delete(hUnusedText(ishghandle(hUnusedText)));
                hText(numel(labels)+1:end) = [];

                obj.LabelText = hText;
            end
        end
                
        function clearHistory(obj)
            obj.HistoryXCoords = {};
            obj.HistoryYCoords = {};
            obj.HistoryZCoords = {};
        end

        function drawHistory(obj)
             [xlineData, ylineData, zlineData] = concatenateHistory(obj);
             hLine = obj.HistoryXAxisLine;
             if ishghandle(hLine)
                 set(hLine,'XData',xlineData(:,1),'YData',xlineData(:,2),'ZData',xlineData(:,3));
             end
             hLine = obj.HistoryYAxisLine;
             if ishghandle(hLine)
                 set(hLine,'XData',ylineData(:,1),'YData',ylineData(:,2),'ZData',ylineData(:,3));
             end
             hLine = obj.HistoryZAxisLine;
             if ishghandle(hLine)
                 set(hLine,'XData',zlineData(:,1),'YData',zlineData(:,2),'ZData',zlineData(:,3));
             end
        end
        
        function [xLineData, yLineData, zLineData] = concatenateHistory(obj)
             xLineCells = obj.HistoryXCoords;
             yLineCells = obj.HistoryYCoords;
             zLineCells = obj.HistoryZCoords;
             xLineData = zeros(0,3);
             yLineData = zeros(0,3);
             zLineData = zeros(0,3);
             for i=1:numel(xLineCells)
                 xLineData = [xLineData; xLineCells{i}]; %#ok<AGROW>
             end
             for i=1:numel(yLineCells)
                 yLineData = [yLineData; yLineCells{i}]; %#ok<AGROW>
             end
             for i=1:numel(zLineCells)
                 zLineData = [zLineData; zLineCells{i}]; %#ok<AGROW>
             end
        end
        
        function addHistory(obj, orientations, positions)
             if obj.HistoryDepth > 0
                 if numel(obj.HistoryXCoords) == obj.HistoryDepth
                     obj.HistoryXCoords(1) = [];
                     obj.HistoryYCoords(1) = [];
                     obj.HistoryZCoords(1) = [];
                 end

                lineXXData = cat(1,positions(:,1)', ...
                                  (positions(:,1)+obj.AxesLineLength.*reshape(orientations(1,1,:),[],1))', ...
                                  NaN(1,size(positions,1)));
                lineXYData = cat(1,positions(:,2)', ...
                                  (positions(:,2)+obj.AxesLineLength.*reshape(orientations(1,2,:),[],1))', ...
                                  NaN(1,size(positions,1)));
                lineXZData = cat(1,positions(:,3)', ...
                                  (positions(:,3)+obj.AxesLineLength.*reshape(orientations(1,3,:),[],1))', ...
                                  NaN(1,size(positions,1)));
                lineYXData = cat(1,positions(:,1)', ...
                                  (positions(:,1)+obj.AxesLineLength.*reshape(orientations(2,1,:),[],1))', ...
                                  NaN(1,size(positions,1)));
                lineYYData = cat(1,positions(:,2)', ...
                                  (positions(:,2)+obj.AxesLineLength.*reshape(orientations(2,2,:),[],1))', ...
                                  NaN(1,size(positions,1)));
                lineYZData = cat(1,positions(:,3)', ...
                                  (positions(:,3)+obj.AxesLineLength.*reshape(orientations(2,3,:),[],1))', ...
                                  NaN(1,size(positions,1)));
                lineZXData = cat(1,positions(:,1)', ...
                                  (positions(:,1)+obj.AxesLineLength.*reshape(orientations(3,1,:),[],1))', ...
                                  NaN(1,size(positions,1)));
                lineZYData = cat(1,positions(:,2)', ...
                                  (positions(:,2)+obj.AxesLineLength.*reshape(orientations(3,2,:),[],1))', ...
                                  NaN(1,size(positions,1)));
                lineZZData = cat(1,positions(:,3)', ...
                                  (positions(:,3)+obj.AxesLineLength.*reshape(orientations(3,3,:),[],1))', ...
                                  NaN(1,size(positions,1)));
                obj.HistoryXCoords{end+1} = [lineXXData(:) lineXYData(:) lineXZData(:)];
                obj.HistoryYCoords{end+1} = [lineYXData(:) lineYYData(:) lineYZData(:)];
                obj.HistoryZCoords{end+1} = [lineZXData(:) lineZYData(:) lineZZData(:)];
             end
        end
    end
end

