Equivalent C++ implementation