double FastMarchingMethod(Image<Color> &input, Image<DistancePixel> &distance_image) { int w = input.Width(); int h = input.Height(); std::vector<std::pair<int, int> > blackPixels; for (int x = 0; x < w; x++) { for (int y = 0; y < h; y++) { const Color& d = input.GetPixel(x,y); DistancePixel& beefy = distance_image.GetPixel(x,y); beefy.setX(x); beefy.setY(y); // skip all pixels that are not bladk if (d.isBlack()) { blackPixels.push_back(std::make_pair(x,y)); beefy.setValue(0); beefy.makeFinalValue(); } else beefy.setValue(1000000); // ome million m**********r } } double answer = 0; DistancePixel_PriorityQueue theQueue; for(int x = 0; x < blackPixels.size(); x++){ int b = blackPixels[x].first; int j = blackPixels[x].second; DistancePixel& tempPixel = distance_image.GetPixel(b,j); std::vector<std::pair<int, int> > nearbyPixels = getNearbyPixels(b,j, distance_image); for (int v = 0; v < nearbyPixels.size(); v++) { int xVal = nearbyPixels[v].first; int yVal = nearbyPixels[v].second; DistancePixel& somePixelWorkin = distance_image.GetPixel(xVal,yVal); double distance = tempPixel.getValue() + (sqrt((b-xVal)*(b-xVal) + (j-yVal)*(j-yVal))); if(somePixelWorkin.getValue() > distance){ somePixelWorkin.setValue(distance); if(!theQueue.in_heap(&somePixelWorkin)) theQueue.push(&somePixelWorkin); else theQueue.update_position(&somePixelWorkin); } } } while(theQueue.size() > 0){ const DistancePixel* aPixel = theQueue.top(); int b = aPixel->getX(); int j = aPixel->getY(); DistancePixel& anotherPixel = distance_image.GetPixel(b, j); anotherPixel.makeFinalValue(); std::vector<std::pair<int, int> > nearbyPixels = getNearbyPixels(b,j, distance_image); for (int v = 0; v < nearbyPixels.size(); v++) { int xVal = nearbyPixels[v].first; int yVal = nearbyPixels[v].second; DistancePixel& somePixelWorkin = distance_image.GetPixel(xVal,yVal); double distance = anotherPixel.getValue() + (sqrt((b-xVal)*(b-xVal) + (j-yVal)*(j-yVal))); if(somePixelWorkin.getValue() > distance){ somePixelWorkin.setValue(distance); if(!theQueue.in_heap(&somePixelWorkin)) theQueue.push(&somePixelWorkin); else theQueue.update_position(&somePixelWorkin); } } answer = std::max(answer, anotherPixel.getValue()); theQueue.pop(); } return answer; }
//takes pixel and adds surronding valid pixels to priority queue void Propogate(Image<DistancePixel> &distance_image, DistancePixel* & pix, DistancePixel_PriorityQueue & reds) { int w = distance_image.Width(); int h = distance_image.Height(); int x = pix->getX(); int y = pix->getY(); double d = pix->getValue(); DistancePixel* p; if ((x-1) >= 0 and (y-1) >= 0) { //top-left p = &distance_image.GetPixel(x-1,y-1); double distance = sqrt((x-p->getX())*(x-p->getX()) + (y-p->getY())*(y-p->getY())); if (p->getValue() > (d+distance)) { p->setValue(d+distance); if (reds.in_heap(p)) { reds.update_position(p); } else { reds.push(p); } } } if (y-1 >= 0) { //top p = &distance_image.GetPixel(x,y-1); double distance = sqrt((x-p->getX())*(x-p->getX()) + (y-p->getY())*(y-p->getY())); if (p->getValue() > (d+distance)) { p->setValue(d+distance); if (reds.in_heap(p)) { reds.update_position(p); } else { reds.push(p); } } } if (x+1 < w and y-1 >= 0) { //top-right p = &distance_image.GetPixel(x+1,y-1); double distance = sqrt((x-p->getX())*(x-p->getX()) + (y-p->getY())*(y-p->getY())); if (p->getValue() > (d+distance)) { p->setValue(d+distance); if (reds.in_heap(p)) { reds.update_position(p); } else { reds.push(p); } } } if (x-1 >= 0) { //left p = &distance_image.GetPixel(x-1,y); double distance = sqrt((x-p->getX())*(x-p->getX()) + (y-p->getY())*(y-p->getY())); if (p->getValue() > (d+distance)) { p->setValue(d+distance); if (reds.in_heap(p)) { reds.update_position(p); } else { reds.push(p); } } } if (x+1 < w) { //right p = &distance_image.GetPixel(x+1,y); double distance = sqrt((x-p->getX())*(x-p->getX()) + (y-p->getY())*(y-p->getY())); if (p->getValue() > (d+distance)) { p->setValue(d+distance); if (reds.in_heap(p)) { reds.update_position(p); } else { reds.push(p); } } } if (x-1 >= 0 and y+1 < h) { //bottom-left p = &distance_image.GetPixel(x-1,y+1); double distance = sqrt((x-p->getX())*(x-p->getX()) + (y-p->getY())*(y-p->getY())); if (p->getValue() > (d+distance)) { p->setValue(d+distance); if (reds.in_heap(p)) { reds.update_position(p); } else { reds.push(p); } } } if (y+1 < h) { //bottom p = &distance_image.GetPixel(x,y+1); double distance = sqrt((x-p->getX())*(x-p->getX()) + (y-p->getY())*(y-p->getY())); if (p->getValue() > (d+distance)) { p->setValue(d+distance); if (reds.in_heap(p)) { reds.update_position(p); } else { reds.push(p); } } } if (x+1 < w and y+1 < h) { //top-right p = &distance_image.GetPixel(x+1,y+1); double distance = sqrt((x-p->getX())*(x-p->getX()) + (y-p->getY())*(y-p->getY())); if (p->getValue() > (d+distance)) { p->setValue(d+distance); if (reds.in_heap(p)) { reds.update_position(p); } else { reds.push(p); } } } }