Better movement tp

The old function was buggy and glitched.