Working on Rigid Body dynamics