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 LineOrderOptimizer::optimize()
{
int gridSize = 5000; // the size of the cells in the hash grid. TODO
SparsePointGridInclusive<unsigned int> line_bucket_grid(gridSize);
bool picked[polygons.size()];
memset(picked, false, sizeof(bool) * polygons.size());/// initialized as falses
for (unsigned int poly_idx = 0; poly_idx < polygons.size(); poly_idx++) /// find closest point to initial starting point within each polygon +initialize picked
{
int best_point_idx = -1;
float best_point_dist = std::numeric_limits<float>::infinity();
PolygonRef poly = polygons[poly_idx];
for (unsigned int point_idx = 0; point_idx < poly.size(); point_idx++) /// get closest point from polygon
{
float dist = vSize2f(poly[point_idx] - startPoint);
if (dist < best_point_dist)
{
best_point_idx = point_idx;
best_point_dist = dist;
}
}
polyStart.push_back(best_point_idx);
assert(poly.size() == 2);
line_bucket_grid.insert(poly[0], poly_idx);
line_bucket_grid.insert(poly[1], poly_idx);
}
Point incoming_perpundicular_normal(0, 0);
Point prev_point = startPoint;
for (unsigned int order_idx = 0; order_idx < polygons.size(); order_idx++) /// actual path order optimizer
{
int best_line_idx = -1;
float best_score = std::numeric_limits<float>::infinity(); // distance score for the best next line
/// check if single-line-polygon is close to last point
for(unsigned int close_line_idx :
line_bucket_grid.getNearbyVals(prev_point, gridSize))
{
if (picked[close_line_idx] || polygons[close_line_idx].size() < 1)
{
continue;
}
updateBestLine(close_line_idx, best_line_idx, best_score, prev_point, incoming_perpundicular_normal);
}
if (best_line_idx == -1) /// if single-line-polygon hasn't been found yet
{
for (unsigned int poly_idx = 0; poly_idx < polygons.size(); poly_idx++)
{
if (picked[poly_idx] || polygons[poly_idx].size() < 1) /// skip single-point-polygons
{
continue;
}
assert(polygons[poly_idx].size() == 2);
updateBestLine(poly_idx, best_line_idx, best_score, prev_point, incoming_perpundicular_normal);
}
}
if (best_line_idx > -1) /// should always be true; we should have been able to identify the best next polygon
{
PolygonRef best_line = polygons[best_line_idx];
assert(best_line.size() == 2);
int line_start_point_idx = polyStart[best_line_idx];
int line_end_point_idx = line_start_point_idx * -1 + 1; /// 1 -> 0 , 0 -> 1
Point& line_start = best_line[line_start_point_idx];
Point& line_end = best_line[line_end_point_idx];
prev_point = line_end;
incoming_perpundicular_normal = turn90CCW(normal(line_end - line_start, 1000));
picked[best_line_idx] = true;
polyOrder.push_back(best_line_idx);
}
else
{
logError("Failed to find next closest line.\n");
}
}
}
