Helper function SetSpeed does not change the movement direction

• added nullptr check for entity
• first try to use the existing velocity vector of the entity to derive the direction of the future velocity vector before using the entity orientation

Signed-off-by: Andreas Rauschert andreas.rb.rauschert@bmw.de