Update costOptimization.cpp