Initial code for rigid body dynamics