Made a small improvement. Now 6 neighbor pixels are used to calculate normal vector. Before only 3 neighbor pixels were used. I've updated the code.