正確に斜めへ移動したい、ってことだったら、

1.
Pa地点からPb地点までのベクトルをABとする。
Pa - Pb = AB

さらに、その単位ベクトルを求め、Eとする。
√(AB.x^2 + AB.y^2 + AB.z^2) = |AB|
(1 / |AB|) * AB = E

2.
速度もしくは加速度にEを掛け、1フレームに進んだ距離を求める。
なお、1フレームに進んだ距離をDとする。
v_t * E = D_t

主人公の位置をMとし、現在位置に進んだ距離Dを足す。
M_t + D_t = M_t+1

おわり。たぶんこっちのほうが早い。