Added mass center restrain routine to energy.h