void LineOrderOptimizer::optimize() { int gridSize = 5000; // the size of the cells in the hash grid. BucketGrid2D<unsigned int> line_bucket_grid(gridSize); bool* picked = new bool[polygons.size()]; // bool picked[polygons.size()]; memset(picked, false, sizeof(bool) * polygons.size());/// initialized as falses for(unsigned int i_polygon=0 ; i_polygon<polygons.size() ; i_polygon++) /// find closest point to initial starting point within each polygon +initialize picked { int best = -1; float bestDist = std::numeric_limits<float>::infinity(); PolygonRef poly = polygons[i_polygon]; for(unsigned int i_point=0; i_point<poly.size(); i_point++) /// get closest point from polygon { float dist = vSize2f(poly[i_point] - startPoint); if (dist < bestDist) { best = i_point; bestDist = dist; } } polyStart.push_back(best); assert(poly.size() == 2); line_bucket_grid.insert(poly[0], i_polygon); line_bucket_grid.insert(poly[1], i_polygon); } Point incommingPerpundicularNormal(0, 0); Point prev_point = startPoint; for(unsigned int i_polygon=0 ; i_polygon<polygons.size() ; i_polygon++) /// actual path order optimizer { int best = -1; float bestDist = std::numeric_limits<float>::infinity(); for(unsigned int i_close_line_polygon : line_bucket_grid.findNearbyObjects(prev_point)) /// check if single-line-polygon is close to last point { if (picked[i_close_line_polygon] || polygons[i_close_line_polygon].size() < 1) continue; checkIfLineIsBest(i_close_line_polygon, best, bestDist, prev_point, incommingPerpundicularNormal); } if (best == -1) /// if single-line-polygon hasn't been found yet { for(unsigned int i_polygon=0 ; i_polygon<polygons.size() ; i_polygon++) { if (picked[i_polygon] || polygons[i_polygon].size() < 1) /// skip single-point-polygons continue; assert(polygons[i_polygon].size() == 2); checkIfLineIsBest(i_polygon, best, bestDist, prev_point, incommingPerpundicularNormal); } } if (best > -1) /// should always be true; we should have been able to identify the best next polygon { assert(polygons[best].size() == 2); int endIdx = polyStart[best] * -1 + 1; /// 1 -> 0 , 0 -> 1 prev_point = polygons[best][endIdx]; incommingPerpundicularNormal = crossZ(normal(polygons[best][endIdx] - polygons[best][polyStart[best]], 1000)); picked[best] = true; polyOrder.push_back(best); } else logError("Failed to find next closest line.\n"); } prev_point = startPoint; for(unsigned int n=0; n<polyOrder.size(); n++) /// decide final starting points in each polygon { int nr = polyOrder[n]; PolygonRef poly = polygons[nr]; int best = -1; float bestDist = std::numeric_limits<float>::infinity(); bool orientation = poly.orientation(); for(unsigned int i=0;i<poly.size(); i++) { float dist = vSize2f(polygons[nr][i] - prev_point); Point n0 = normal(poly[(i+poly.size()-1)%poly.size()] - poly[i], 2000); Point n1 = normal(poly[i] - poly[(i + 1) % poly.size()], 2000); float dot_score = dot(n0, n1) - dot(crossZ(n0), n1); if (orientation) dot_score = -dot_score; if (dist + dot_score < bestDist) { best = i; bestDist = dist + dot_score; } } polyStart[nr] = best; assert(poly.size() == 2); prev_point = poly[best *-1 + 1]; /// 1 -> 0 , 0 -> 1 } }
void PathOrderOptimizer::optimize() { const float incommingPerpundicularNormalScale = 0.0001f; std::map<uint32_t, std::vector<unsigned int>> location_to_polygon_map; std::vector<bool> picked; for(unsigned int i=0; i<polygons.size(); i++) { int best = -1; float bestDist = 0xFFFFFFFFFFFFFFFFLL; PolygonRef poly = polygons[i]; for(unsigned int j=0; j<poly.size(); j++) { float dist = vSize2f(poly[j] - startPoint); if (dist < bestDist) { best = j; bestDist = dist; } } polyStart.push_back(best); picked.push_back(false); if (poly.size() == 2) { Point p0 = poly[0]; Point p1 = poly[1]; location_to_polygon_map[hashPoint(p0)].push_back(i); location_to_polygon_map[hashPoint(p1)].push_back(i); } } Point incommingPerpundicularNormal(0, 0); Point p0 = startPoint; for(unsigned int n=0; n<polygons.size(); n++) { int best = -1; float bestDist = 0xFFFFFFFFFFFFFFFFLL; for(unsigned int i : location_to_polygon_map[hashPoint(p0)]) { if (picked[i] || polygons[i].size() < 1) continue; float dist = vSize2f(polygons[i][0] - p0); dist += abs(dot(incommingPerpundicularNormal, normal(polygons[i][1] - polygons[i][0], 1000))) * incommingPerpundicularNormalScale; if (dist < bestDist) { best = i; bestDist = dist; polyStart[i] = 0; } dist = vSize2f(polygons[i][1] - p0); dist += abs(dot(incommingPerpundicularNormal, normal(polygons[i][0] - polygons[i][1], 1000))) * incommingPerpundicularNormalScale; if (dist < bestDist) { best = i; bestDist = dist; polyStart[i] = 1; } } if (best == -1) { for(unsigned int i=0; i<polygons.size(); i++) { if (picked[i] || polygons[i].size() < 1) continue; if (polygons[i].size() == 2) { float dist = vSize2f(polygons[i][0] - p0); dist += abs(dot(incommingPerpundicularNormal, normal(polygons[i][1] - polygons[i][0], 1000))) * incommingPerpundicularNormalScale; if (dist < bestDist) { best = i; bestDist = dist; polyStart[i] = 0; } dist = vSize2f(polygons[i][1] - p0); dist += abs(dot(incommingPerpundicularNormal, normal(polygons[i][0] - polygons[i][1], 1000))) * incommingPerpundicularNormalScale; if (dist < bestDist) { best = i; bestDist = dist; polyStart[i] = 1; } } else { float dist = vSize2f(polygons[i][polyStart[i]] - p0); if (dist < bestDist) { best = i; bestDist = dist; } } } } if (best > -1) { if (polygons[best].size() == 2) { int endIdx = (polyStart[best] + 1) % 2; p0 = polygons[best][endIdx]; incommingPerpundicularNormal = crossZ(normal(polygons[best][endIdx] - polygons[best][polyStart[best]], 1000)); } else { p0 = polygons[best][polyStart[best]]; incommingPerpundicularNormal = Point(0, 0); } picked[best] = true; polyOrder.push_back(best); } } p0 = startPoint; for(int nr : polyOrder) { PolygonRef poly = polygons[nr]; if (poly.size() > 2) { int best = -1; float bestDist = 0xFFFFFFFFFFFFFFFFLL; bool orientation = poly.orientation(); for(unsigned int i=0; i<poly.size(); i++) { const int64_t dot_score_scale = 2000; float dist = vSize2f(polygons[nr][i] - p0); Point n0 = normal(poly[(i+poly.size()-1)%poly.size()] - poly[i], dot_score_scale); Point n1 = normal(poly[i] - poly[(i + 1) % poly.size()], dot_score_scale); float dot_score = dot(n0, n1) - dot(crossZ(n0), n1); if (orientation) dot_score = -dot_score; dist += dot_score; if (dist < bestDist) { best = i; bestDist = dist; } } polyStart[nr] = best; } if (poly.size() <= 2) { p0 = poly[(polyStart[nr] + 1) % 2]; } else { p0 = poly[polyStart[nr]]; } } }