inline int PathOrderOptimizer::getClosestPointInPolygon(Point prev_point, int i_polygon)
{
PolygonRef poly = polygons[i_polygon];
int best = -1;
float bestDist = std::numeric_limits<float>::infinity();
bool orientation = poly.orientation();
for(unsigned int i_point=0 ; i_point<poly.size() ; i_point++)
{
float dist = vSize2f(poly[i_point] - prev_point);
Point n0 = normal(poly[(i_point-1+poly.size())%poly.size()] - poly[i_point], 2000);
Point n1 = normal(poly[i_point] - poly[(i_point + 1) % poly.size()], 2000);
float dot_score = dot(n0, n1) - dot(crossZ(n0), n1); /// prefer binnenbocht
if (orientation)
dot_score = -dot_score;
if (dist + dot_score < bestDist)
{
best = i_point;
bestDist = dist;
}
}
return best;
}
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]];
}
}
}