Rigid Body Dynamics with Constraints