Non-Linear Rigid Body Dynamics: Energy and Momentum Conserving Algorithm