Add unit tests that cover more functionality (outside tutorials)

[tgeijten]

The following functionality needs to be covered:

• Walking on tilted plane (see #68 )
• Script functionality:
  • contact forces
  • math functions (quat / vec3)
  • ...
• More optimization tests