Added cost. Moved some matrix and vector operations to PMat and PVec.