Added state init code and moved vector zeroing to mathtypes.c