cleanup and reordering of code
use relative masses for ContactPoint contsrtaint