Add unit tests for high joint angle cases in testGeometry.cpp and testJoints.cpp