Energy and momentum conserving integration متلب