3d hpmodel implementation and test

William Fledeliusrequested to merge
3d_hpmodel_implementation_and_test into main

All directions working, might need some tests for some edge cases, such as up/down immidiately following each other etc.

Merge request reports