PositionSmoothing: fix getMaxZSpeed calculation

[bresch]

Solved Problem

We cannot first take the norm and then subtract without considering the Pythagorean theorem.

fixes #24223

Solution

Compute xy norm after computing the 3D vectors relative to the target position.

Test coverage

not tested yet

[Luka-Filipovic]

This was also my first attempt to fix the issue 😄
I think this is still a fundamentally wrong approach. Here are the counter example where this logic doesn't work:

1. start point (0, 0, -10), target (0, 0, 0), next (10, 0, 0): this results in a 90 deg turn angle and makes the arrival_z_speed > 0 even tho it needs to be 0
2. start point (0, 0, -10), target (0, 0, 0), next (0, 0, 0.1): this results in a 180 deg turn angle and gives the maximum arrival_z_speed even tho it needs to be able to fully stop at the next waypoint (we don't know if it's the last waypoint or where the path goes after)

The logic needs to account for the speed constraints the trajectory will have once the target is reached.