Fixed one bug in rigid body dynamics