Adding RBS and selection of direction

Adding the RigidBodyState calculation and the option for choosing to
invert the direction of the angle.