//Rectangle vs circle
bool hasCollided(sf::Vector2f c1, sf::Vector2f s1, float dir1, sf::Vector2f c2, float r2)
{
//Basic distance check
float dist = getDist(c1, c2);
if (getMaxRad(s1) + r2 < dist) return false;
if (getMinRad(s1) + r2 > dist) return true;
//Transforming vectors
float unitX1 = cos(dir1);
float unitY1 = sin(dir1);
sf::Vector2f rotWidth1(unitX1 * s1.x / 2.f, unitY1 * s1.x / 2.f);
sf::Vector2f rotHeight1(-unitY1 * s1.y / 2.f, unitX1 * s1.y / 2.f);
sf::Vector2f tr1[] = {c1 - rotWidth1 - rotHeight1,
c1 + rotWidth1 - rotHeight1,
c1 + rotWidth1 + rotHeight1,
c1 - rotWidth1 + rotHeight1};
//Inner check
float si1 = crossZ(tr1[1] - tr1[0], c2 - tr1[0]);
float si2 = crossZ(tr1[2] - tr1[1], c2 - tr1[1]);
float si3 = crossZ(tr1[3] - tr1[2], c2 - tr1[2]);
float si4 = crossZ(tr1[0] - tr1[3], c2 - tr1[3]);
if (si1 < 0 && si2 < 0 && si3 < 0 && si4 < 0) return true;
if (si1 > 0 && si2 > 0 && si3 > 0 && si4 > 0) return true;
//Edge cross check
if (getDist(c2, tr1[0], tr1[1]) < r2) return true;
if (getDist(c2, tr1[1], tr1[2]) < r2) return true;
if (getDist(c2, tr1[2], tr1[3]) < r2) return true;
if (getDist(c2, tr1[3], tr1[0]) < r2) return true;
return false;
}
//Point vs rectangle
bool hasCollided(sf::Vector2f c1, sf::Vector2f c2, sf::Vector2f s2, float dir2)
{
//Basic distance check
float dist = getDist(c1, c2);
if (getMaxRad(s2) < dist) return false;
if (getMinRad(s2) > dist) return true;
//Transforming vectors
float unitX2 = cos(dir2);
float unitY2 = sin(dir2);
sf::Vector2f rotWidth2(unitX2 * s2.x / 2.f, unitY2 * s2.x / 2.f);
sf::Vector2f rotHeight2(-unitY2 * s2.y / 2.f, unitX2 * s2.y / 2.f);
sf::Vector2f tr2[] = {c2 - rotWidth2 - rotHeight2,
c2 + rotWidth2 - rotHeight2,
c2 + rotWidth2 + rotHeight2,
c2 - rotWidth2 + rotHeight2};
//Inner check
float si1 = crossZ(tr2[1] - tr2[0], c1 - tr2[0]);
float si2 = crossZ(tr2[2] - tr2[1], c1 - tr2[1]);
float si3 = crossZ(tr2[3] - tr2[2], c1 - tr2[2]);
float si4 = crossZ(tr2[0] - tr2[3], c1 - tr2[3]);
if (si1 < 0 && si2 < 0 && si3 < 0 && si4 < 0) return true;
if (si1 > 0 && si2 > 0 && si3 > 0 && si4 > 0) return true;
return false;
}
Point Comb::getBounderyPointWithOffset(unsigned int polygonNr, unsigned int idx)
{
Point p0 = boundery[polygonNr][(idx > 0) ? (idx - 1) : (boundery[polygonNr].size() - 1)];
Point p1 = boundery[polygonNr][idx];
Point p2 = boundery[polygonNr][(idx < (boundery[polygonNr].size() - 1)) ? (idx + 1) : (0)];
Point off0 = crossZ(normal(p1 - p0, MM2INT(1.0)));
Point off1 = crossZ(normal(p2 - p1, MM2INT(1.0)));
Point n = normal(off0 + off1, MM2INT(0.2));
return p1 + n;
}
Point getBoundaryPointWithOffset(PolygonRef poly, unsigned int point_idx, int64_t offset)
{
Point p0 = poly[(point_idx > 0) ? (point_idx - 1) : (poly.size() - 1)];
Point p1 = poly[point_idx];
Point p2 = poly[(point_idx < (poly.size() - 1)) ? (point_idx + 1) : 0];
Point off0 = crossZ(normal(p1 - p0, MM2INT(1.0))); // 1.0 for some precision
Point off1 = crossZ(normal(p2 - p1, MM2INT(1.0))); // 1.0 for some precision
Point n = normal(off0 + off1, -offset);
return p1 + n;
}
Point Comb::getBoundaryPointWithOffset(unsigned int polygon_idx, unsigned int point_idx)
{
int64_t offset = MM2INT(0.2); // hard coded value
PolygonRef poly = boundary[polygon_idx];
Point p0 = poly[(point_idx > 0) ? (point_idx - 1) : (poly.size() - 1)];
Point p1 = poly[point_idx];
Point p2 = poly[(point_idx < (poly.size() - 1)) ? (point_idx + 1) : 0];
Point off0 = crossZ(normal(p1 - p0, MM2INT(1.0))); // hard coded value (?)
Point off1 = crossZ(normal(p2 - p1, MM2INT(1.0))); // hard coded value (?)
Point n = normal(off0 + off1, offset);
return p1 + n;
}
//Rectangle vs rectangle
bool hasCollided(sf::Vector2f c1, sf::Vector2f s1, float dir1, sf::Vector2f c2, sf::Vector2f s2, float dir2)
{
//Basic distance check
float dist = getDist(c1, c2);
if (getMaxRad(s1) + getMaxRad(s2) < dist) return false;
if (getMinRad(s1) + getMinRad(s2) > dist) return true;
//Transforming vectors
float unitX1 = cos(dir1);
float unitY1 = sin(dir1);
sf::Vector2f rotWidth1(unitX1 * s1.x / 2.f, unitY1 * s1.x / 2.f);
sf::Vector2f rotHeight1(-unitY1 * s1.y / 2.f, unitX1 * s1.y / 2.f);
sf::Vector2f tr1[] = {c1 - rotWidth1 - rotHeight1,
c1 + rotWidth1 - rotHeight1,
c1 + rotWidth1 + rotHeight1,
c1 - rotWidth1 + rotHeight1};
float unitX2 = cos(dir2);
float unitY2 = sin(dir2);
sf::Vector2f rotWidth2(unitX2 * s2.x / 2.f, unitY2 * s2.x / 2.f);
sf::Vector2f rotHeight2(-unitY2 * s2.y / 2.f, unitX2 * s2.y / 2.f);
sf::Vector2f tr2[] = {c2 - rotWidth2 - rotHeight2,
c2 + rotWidth2 - rotHeight2,
c2 + rotWidth2 + rotHeight2,
c2 - rotWidth2 + rotHeight2};
//Axis separation theorem
for (int i = 0; i < 4; i++) {
sf::Vector2f separator = tr1[(i+1)%4] - tr1[i];
float side = crossZ(tr1[(i+2)%4] - tr1[i], separator);
int sign = (side > 0? -1 : 1);
if (crossZ(tr2[0]-tr1[i], separator) * sign > 0 &&
crossZ(tr2[1]-tr1[i], separator) * sign > 0 &&
crossZ(tr2[2]-tr1[i], separator) * sign > 0 &&
crossZ(tr2[3]-tr1[i], separator) * sign > 0)
return false;
}
for (int i = 0; i < 4; i++) {
sf::Vector2f separator = tr2[(i+1)%4] - tr2[i];
float side = crossZ(tr2[(i+2)%4] - tr2[i], separator);
int sign = (side > 0? -1 : 1);
if (crossZ(tr1[0]-tr2[i], separator) * sign > 0 &&
crossZ(tr1[1]-tr2[i], separator) * sign > 0 &&
crossZ(tr1[2]-tr2[i], separator) * sign > 0 &&
crossZ(tr1[3]-tr2[i], separator) * sign > 0)
return false;
}
return true;
}
bool MergeInfillLines::isConvertible(unsigned int path_idx_first_move, Point& first_middle, Point& second_middle, int64_t& line_width, bool use_second_middle_as_first)
{
int64_t max_line_width = nozzle_size * 3 / 2;
unsigned int idx = path_idx_first_move;
if (idx + 3 > paths.size()-1) return false;
if (paths[idx+0].config != &travelConfig) return false;
if (paths[idx+1].points.size() > 1) return false;
if (paths[idx+1].config == &travelConfig) return false;
// if (paths[idx+2].points.size() > 1) return false;
if (paths[idx+2].config != &travelConfig) return false;
if (paths[idx+3].points.size() > 1) return false;
if (paths[idx+3].config == &travelConfig) return false;
Point& a = paths[idx+0].points.back(); // first extruded line from
Point& b = paths[idx+1].points.back(); // first extruded line to
Point& c = paths[idx+2].points.back(); // second extruded line from
Point& d = paths[idx+3].points.back(); // second extruded line to
Point ab = b - a;
Point cd = d - c;
int64_t prod = dot(ab,cd);
if (std::abs(prod) + 400 < vSize(ab) * vSize(cd)) // 400 = 20*20, where 20 micron is the allowed inaccuracy in the dot product, introduced by the inaccurate point locations of a,b,c,d
return false; // extrusion moves not in the same or opposite diraction
if (prod < 0) { ab = ab * -1; }
Point infill_vector = (cd + ab) / 2;
if (!shorterThen(infill_vector, 5 * nozzle_size)) return false; // infill lines too far apart
first_middle = (use_second_middle_as_first)?
second_middle :
(a + b) / 2;
second_middle = (c + d) / 2;
Point dir_vector_perp = crossZ(second_middle - first_middle);
int64_t dir_vector_perp_length = vSize(dir_vector_perp); // == dir_vector_length
if (dir_vector_perp_length == 0) return false;
if (dir_vector_perp_length > 5 * nozzle_size) return false; // infill lines too far apart
line_width = std::abs( dot(dir_vector_perp, infill_vector) / dir_vector_perp_length );
if (line_width > max_line_width) return false; // combined lines would be too wide
if (line_width == 0) return false; // dot is zero, so lines are in each others extension, not next to eachother
{ // check whether the two lines are adjacent
Point ca = first_middle - c;
double ca_size = vSizeMM(ca);
double cd_size = vSizeMM(cd);
double prod = INT2MM(dot(ca, cd));
double fraction = prod / ( ca_size * cd_size );
int64_t line2line_dist = MM2INT(cd_size * std::sqrt(1.0 - fraction * fraction));
if (line2line_dist + 20 > paths[idx+1].config->getLineWidth()) return false; // there is a gap between the two lines
}
return true;
};
void FffPolygonGenerator::processFuzzyWalls(SliceMeshStorage& mesh)
{
int64_t fuzziness = mesh.getSettingInMicrons("magic_fuzzy_skin_thickness");
int64_t avg_dist_between_points = mesh.getSettingInMicrons("magic_fuzzy_skin_point_dist");
int64_t min_dist_between_points = avg_dist_between_points * 3 / 4; // hardcoded: the point distance may vary between 3/4 and 5/4 the supplied value
int64_t range_random_point_dist = avg_dist_between_points / 2;
for (unsigned int layer_nr = 0; layer_nr < mesh.layers.size(); layer_nr++)
{
SliceLayer& layer = mesh.layers[layer_nr];
for (SliceLayerPart& part : layer.parts)
{
Polygons results;
Polygons& skin = (mesh.getSettingAsSurfaceMode("magic_mesh_surface_mode") == ESurfaceMode::SURFACE)? part.outline : part.insets[0];
for (PolygonRef poly : skin)
{
// generate points in between p0 and p1
PolygonRef result = results.newPoly();
int64_t dist_left_over = rand() % (min_dist_between_points / 2); // the distance to be traversed on the line before making the first new point
Point* p0 = &poly.back();
for (Point& p1 : poly)
{ // 'a' is the (next) new point between p0 and p1
Point p0p1 = p1 - *p0;
int64_t p0p1_size = vSize(p0p1);
int64_t dist_last_point = dist_left_over + p0p1_size * 2; // so that p0p1_size - dist_last_point evaulates to dist_left_over - p0p1_size
for (int64_t p0pa_dist = dist_left_over; p0pa_dist < p0p1_size; p0pa_dist += min_dist_between_points + rand() % range_random_point_dist)
{
int r = rand() % (fuzziness * 2) - fuzziness;
Point perp_to_p0p1 = crossZ(p0p1);
Point fuzz = normal(perp_to_p0p1, r);
Point pa = *p0 + normal(p0p1, p0pa_dist) + fuzz;
result.add(pa);
dist_last_point = p0pa_dist;
}
dist_left_over = p0p1_size - dist_last_point;
p0 = &p1;
}
while (result.size() < 3 )
{
unsigned int point_idx = poly.size() - 2;
result.add(poly[point_idx]);
if (point_idx == 0) { break; }
point_idx--;
}
if (result.size() < 3)
{
result.clear();
for (Point& p : poly)
result.add(p);
}
}
skin = results;
sendPolygons(Inset0Type, layer_nr, skin, mesh.getSettingInMicrons("wall_line_width_0"));
}
}
}
bool Comb::moveInside(Point* p, int distance)
{
Point ret = *p;
int64_t bestDist = MM2INT(2.0) * MM2INT(2.0);
for(unsigned int n=0; n<boundery.size(); n++)
{
if (boundery[n].size() < 1)
continue;
Point p0 = boundery[n][boundery[n].size()-1];
for(unsigned int i=0; i<boundery[n].size(); i++)
{
Point p1 = boundery[n][i];
//Q = A + Normal( B - A ) * ((( B - A ) dot ( P - A )) / VSize( A - B ));
Point pDiff = p1 - p0;
int64_t lineLength = vSize(pDiff);
int64_t distOnLine = dot(pDiff, *p - p0) / lineLength;
if (distOnLine < 10)
distOnLine = 10;
if (distOnLine > lineLength - 10)
distOnLine = lineLength - 10;
Point q = p0 + pDiff * distOnLine / lineLength;
int64_t dist = vSize2(q - *p);
if (dist < bestDist)
{
bestDist = dist;
ret = q + crossZ(normal(p1 - p0, distance));
}
p0 = p1;
}
}
if (bestDist < MM2INT(2.0) * MM2INT(2.0))
{
*p = ret;
return true;
}
return false;
}
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;
}
bool MergeInfillLines::isConvertible(const Point& a, const Point& b, const Point& c, const Point& d, int64_t line_width, Point& first_middle, Point& second_middle, int64_t& resulting_line_width, bool use_second_middle_as_first)
{
use_second_middle_as_first = false;
int64_t nozzle_size = line_width; // TODO
int64_t max_line_width = nozzle_size * 3 / 2;
Point ab = b - a;
Point cd = d - c;
if (b == c)
{
return false; // the line segments are connected!
}
int64_t ab_size = vSize(ab);
int64_t cd_size = vSize(cd);
if (ab_size > nozzle_size * 5 || cd_size > nozzle_size * 5)
{
return false; // infill lines are too long; otherwise infill lines might be merged when the next infill line is coincidentally shorter like |, would become \ ...
}
// if the lines are in the same direction then abs( dot(ab,cd) / |ab| / |cd| ) == 1
int64_t prod = dot(ab,cd);
if (std::abs(prod) + 400 < ab_size * cd_size) // 400 = 20*20, where 20 micron is the allowed inaccuracy in the dot product, introduced by the inaccurate point locations of a,b,c,d
return false; // extrusion moves not in the same or opposite diraction
// make lines in the same direction by flipping one
if (prod < 0)
{
ab = ab * -1;
}
else if (prod == 0)
{
return false; // lines are orthogonal!
}
else if (b == d || a == c)
{
return false; // the line segments are connected!
}
first_middle = (use_second_middle_as_first)?
second_middle :
(a + b) / 2;
second_middle = (c + d) / 2;
Point dir_vector_perp = crossZ(second_middle - first_middle);
int64_t dir_vector_perp_length = vSize(dir_vector_perp); // == dir_vector_length
if (dir_vector_perp_length == 0) return false;
if (dir_vector_perp_length > 5 * nozzle_size) return false; // infill lines too far apart
Point infill_vector = (cd + ab) / 2; // (similar to) average line / direction of the infill
// compute the resulting line width
resulting_line_width = std::abs( dot(dir_vector_perp, infill_vector) / dir_vector_perp_length );
if (resulting_line_width > max_line_width) return false; // combined lines would be too wide
if (resulting_line_width == 0) return false; // dot is zero, so lines are in each others extension, not next to eachother
// check whether two lines are adjacent (note: not 'line segments' but 'lines')
Point ac = c - first_middle;
Point infill_vector_perp = crossZ(infill_vector);
int64_t perp_proj = dot(ac, infill_vector_perp);
int64_t infill_vector_perp_length = vSize(infill_vector_perp);
if (std::abs(std::abs(perp_proj) / infill_vector_perp_length - line_width) > 20) // it should be the case that dot(ac, infill_vector_perp) / |infill_vector_perp| == line_width
{
return false; // lines are too far apart or too close together
}
// check whether the two line segments are adjacent.
// full infill in a narrow area might result in line segments with arbitrary distance between them
// the more the narrow passage in the area gets aligned with the infill direction, the further apart the line segments will be
// however, distant line segments might also be due to different narrow passages, so we limit the distance between merged line segments.
if (!LinearAlg2D::lineSegmentsAreCloserThan(a, b, c, d, line_width * 2))
{
return false;
}
return true;
};
bool Comb::moveInside(Point* from, int distance)
{
Point ret = *from;
int64_t maxDist2 = MM2INT(2.0) * MM2INT(2.0);
int64_t bestDist2 = maxDist2;
for(PolygonRef poly : boundary)
{
if (poly.size() < 2)
continue;
Point p0 = poly[poly.size()-2];
Point p1 = poly.back();
bool projected_p_beyond_prev_segment = dot(p1 - p0, *from - p0) > vSize2(p1 - p0);
for(Point& p2 : poly)
{
// X = A + Normal( B - A ) * ((( B - A ) dot ( P - A )) / VSize( A - B ));
// X = P projected on AB
Point& a = p1;
Point& b = p2;
Point& p = *from;
Point ab = b - a;
Point ap = p - a;
int64_t ab_length = vSize(ab);
int64_t ax_length = dot(ab, ap) / ab_length;
if (ax_length < 0) // x is projected to before ab
{
if (projected_p_beyond_prev_segment)
{ // case which looks like: > .
projected_p_beyond_prev_segment = false;
Point& x = p1;
int64_t dist2 = vSize2(x - p);
if (dist2 < bestDist2)
{
bestDist2 = dist2;
ret = x + normal(crossZ(normal(a, distance*4) + normal(p1 - p0, distance*4)), distance); // *4 to retain more precision for the eventual normalization
}
}
else
{
projected_p_beyond_prev_segment = false;
p0 = p1;
p1 = p2;
continue;
}
}
else if (ax_length > ab_length) // x is projected to beyond ab
{
projected_p_beyond_prev_segment = true;
p0 = p1;
p1 = p2;
continue;
}
else
{
projected_p_beyond_prev_segment = false;
Point x = a + ab * ax_length / ab_length;
int64_t dist2 = vSize2(x - *from);
if (dist2 < bestDist2)
{
bestDist2 = dist2;
ret = x + crossZ(normal(ab, distance));
}
}
p0 = p1;
p1 = p2;
}
}
if (bestDist2 < maxDist2)
{
*from = ret;
return true;
}
return false;
}
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]];
}
}
}
unsigned int moveInside(Polygons& polygons, Point& from, int distance, int64_t maxDist2)
{
Point ret = from;
int64_t bestDist2 = maxDist2;
unsigned int bestPoly = NO_INDEX;
for (unsigned int poly_idx = 0; poly_idx < polygons.size(); poly_idx++)
{
PolygonRef poly = polygons[poly_idx];
if (poly.size() < 2)
continue;
Point p0 = poly[poly.size()-2];
Point p1 = poly.back();
bool projected_p_beyond_prev_segment = dot(p1 - p0, from - p0) > vSize2(p1 - p0);
for(Point& p2 : poly)
{
// X = A + Normal( B - A ) * ((( B - A ) dot ( P - A )) / VSize( A - B ));
// X = P projected on AB
Point& a = p1;
Point& b = p2;
Point& p = from;
Point ab = b - a;
Point ap = p - a;
int64_t ab_length = vSize(ab);
int64_t ax_length = dot(ab, ap) / ab_length;
if (ax_length < 0) // x is projected to before ab
{
if (projected_p_beyond_prev_segment)
{ // case which looks like: > .
projected_p_beyond_prev_segment = false;
Point& x = p1;
int64_t dist2 = vSize2(x - p);
if (dist2 < bestDist2)
{
bestDist2 = dist2;
if (distance == 0) { ret = x; }
else { ret = x + normal(crossZ(normal(a, distance*4) + normal(p1 - p0, distance*4)), distance); } // *4 to retain more precision for the eventual normalization
bestPoly = poly_idx;
}
}
else
{
projected_p_beyond_prev_segment = false;
p0 = p1;
p1 = p2;
continue;
}
}
else if (ax_length > ab_length) // x is projected to beyond ab
{
projected_p_beyond_prev_segment = true;
p0 = p1;
p1 = p2;
continue;
}
else
{
projected_p_beyond_prev_segment = false;
Point x = a + ab * ax_length / ab_length;
int64_t dist2 = vSize2(x - from);
if (dist2 < bestDist2)
{
bestDist2 = dist2;
if (distance == 0) { ret = x; }
else { ret = x + crossZ(normal(ab, distance)); }
bestPoly = poly_idx;
}
}
p0 = p1;
p1 = p2;
}
}
if (bestDist2 < maxDist2)
{
from = ret;
return bestPoly;
}
return NO_INDEX;
}
