function Ts = exampleHelperSE3Trajectory( T0, Tf, varargin )
%exampleHelperSE3Trajectory Interpolate between two SE3 poses

% Copyright 2016 The MathWorks, Inc.

    n = -1;
    tvec = [];
    if ~isempty(varargin)
        validateattributes(varargin{1}, {'numeric'}, {'vector'});
        if numel(varargin{1}) == 1 && varargin{1} > 1
            n = varargin{1};
            validateattributes(n, {'numeric'}, {'positive', 'integer'});
        else 
            tvec = varargin{1};
            validateattributes(tvec, {'numeric'}, {'increasing', '>=', 0, '<=', 1 });
        end
    else
        n = 10; 
    end
    
    if n ~= -1
        tvec = linspace(0,1, n);
    end
    
    R0 = T0(1:3, 1:3);
    Rf = Tf(1:3,1:3);
    p0 = T0(1:3,4);
    pf = Tf(1:3,4);
    axang = rotm2axang(Rf'*R0);
    v = axang(1:3);
    quat0 = rotm2quat(R0);
    quatf = rotm2quat(Rf);
    quat = bsxfun(@times,(1-tvec)',quat0) + bsxfun(@times,tvec',quatf);
    
    X = linearSegWithParabolicBlends(p0(1), pf(1), tvec);
    Y = linearSegWithParabolicBlends(p0(2), pf(2), tvec);
    Z = linearSegWithParabolicBlends(p0(3), pf(3), tvec);
    
    ang  = linearSegWithParabolicBlends(0, axang(4), tvec);
    
    Ts = quat2tform(quat);
    
    for i = 1:length(tvec)
        Ts(1,4,i) = X(i); 
        Ts(2,4,i) = Y(i);
        Ts(3,4,i) = Z(i);
    end
end

