Add unit test for force computation.

Unit test for computation of gravitorial and weight forces.
This test will probably break due to a broken implementation
of the gravitorial force variance computation, but supposed
to be fixed in the upcoming port to Eigen3.