Update ConstrP2Pdistance.cpp