Physics package

Started work on rigid body physics. Also added identity constructors to
matrices.