Add physics code from NCrust