P1 = [50,0,0]';
P2 = [0,50,0]';

Q = MoveTo(P1, P2, 3);