changelog:
- increased performance by removing unnecessary loops
- implemented discretization of acc., vel. and points in time
- outsourced trajectory generation into a dedicated function (TODO - implement it)