function [Pee,dPee] = CartTrajPlanning( t,tf,XYZ_i,XYZ_f,N )

Pi = XYZ_i;
Pf = XYZ_f;
         
for nn = 1:3,              % Trajectory planning for each direction X,Y,Z
	if abs(XYZ_f(nn)-XYZ_i(nn)) > 1e-10,
    	switch N,
        	case 3,        % 3rd-order polynomial: ZERO initial and final velocities
                    Pee(nn) = -2*(Pf(nn)-Pi(nn))*t^3/tf^3+3*(Pf(nn)-Pi(nn))*t^2/tf^2+Pi(nn);
                    dPee(nn) = -6*(Pf(nn)-Pi(nn))*t^2/tf^3+6*(Pf(nn)-Pi(nn))*t/tf^2;
            case 5,        % 5th-order polynomial: ZERO initial/final vel & acceleration
                    Pee(nn) = 6*(Pf(nn)-Pi(nn))*t^5/tf^5-15*(Pf(nn)-Pi(nn))*t^4/tf^4+10*(Pf(nn)-Pi(nn))*t^3/tf^3+Pi(nn);
                    dPee(nn) = 30*(Pf(nn)-Pi(nn))/tf^5*t^4-60*(Pf(nn)-Pi(nn))/tf^4*t^3+30*(Pf(nn)-Pi(nn))/tf^3*t^2;
            otherwise, 
                        disp('Unknown order!!!')
                        dPee(nn) = NaN, dPee(nn) = NaN;
        end
    else
    	Pee(nn) = XYZ_i(nn);
        dPee(nn) = 0;
    end
end

end