Implementing a matting algorithm - C++