C++ implementation for pivot's function