Proper solver for rigid body simulation