void BooleanTool::pathObjectToPolygons(
const PathObject* object,
ClipperLib::Paths& polygons,
PolyMap& polymap)
{
object->update();
polygons.reserve(polygons.size() + object->parts().size());
for (const auto& part : object->parts())
{
const PathCoordVector& path_coords = part.path_coords;
auto path_coords_end = path_coords.size();
if (part.isClosed())
--path_coords_end;
ClipperLib::Path polygon;
for (auto i = 0u; i < path_coords_end; ++i)
{
auto point = MapCoord { path_coords[i].pos };
polygon.push_back(ClipperLib::IntPoint(point.nativeX(), point.nativeY()));
polymap.insertMulti(polygon.back(), std::make_pair(&part, &path_coords[i]));
}
bool orientation = Orientation(polygon);
if ( (&part == &object->parts().front()) != orientation )
{
std::reverse(polygon.begin(), polygon.end());
}
// Push_back shall move the polygon.
static_assert(std::is_nothrow_move_constructible<ClipperLib::Path>::value, "ClipperLib::Path must be nothrow move constructible");
polygons.push_back(polygon);
}
}
void build(utymap::meshing::Polygon& polygon)
{
ClipperLib::ClipperOffset offset;
ClipperLib::Path path;
path.reserve(polygon.points.size() / 2);
auto lastPointIndex = polygon.points.size() - 2;
double min = std::numeric_limits<double>::max();
for (std::size_t i = 0; i < polygon.points.size(); i += 2) {
auto nextIndex = i == lastPointIndex ? 0 : i + 2;
utymap::meshing::Vector2 v1(polygon.points[i], polygon.points[i + 1]);
utymap::meshing::Vector2 v2(polygon.points[nextIndex], polygon.points[nextIndex + 1]);
min = std::min(min, utymap::meshing::Vector2::distance(v1, v2));
path.push_back(ClipperLib::IntPoint(static_cast<ClipperLib::cInt>(v1.x * Scale),
static_cast<ClipperLib::cInt>(v1.y * Scale)));
}
offset.AddPath(path, ClipperLib::JoinType::jtMiter, ClipperLib::EndType::etClosedPolygon);
ClipperLib::Paths solution;
// NOTE: use minimal side value as reference for offsetting.
offset.Execute(solution, -(min / 10) * Scale);
// NOTE: this is unexpected result for algorithm below, fallback to flat roof.
if (solution.size() != 1 || solution[0].size() != path.size()) {
return FlatRoofBuilder::build(polygon);
}
buildMansardShape(polygon, solution[0], findFirstIndex(solution[0][0], polygon));
}
Slic3r::Polylines ClipperPaths_to_Slic3rPolylines(const ClipperLib::Paths &input)
{
Slic3r::Polylines retval;
retval.reserve(input.size());
for (ClipperLib::Paths::const_iterator it = input.begin(); it != input.end(); ++it)
retval.emplace_back(ClipperPath_to_Slic3rPolyline(*it));
return retval;
}
QVector<Path> ClipperHelpers::convert(const ClipperLib::Paths& paths) noexcept {
QVector<Path> p;
p.reserve(paths.size());
for (const ClipperLib::Path& path : paths) {
p.append(convert(path));
}
return p;
}
void ElementGeometryClipper::visitArea(const Area& area)
{
ClipperLib::Path areaShape;
PointLocation pointLocation = setPath(quadKeyBbox_, area, areaShape);
// 1. all geometry inside current quadkey: no need to truncate.
if (pointLocation == PointLocation::AllInside) {
callback_(area, quadKey_);
return;
}
// 2. all geometry outside: skip
if (pointLocation == PointLocation::AllOutside) {
return;
}
ClipperLib::Paths solution;
clipper_.AddPath(areaShape, ClipperLib::ptSubject, true);
clipper_.AddPath(createPathFromBoundingBox(), ClipperLib::ptClip, true);
clipper_.Execute(ClipperLib::ctIntersection, solution);
clipper_.Clear();
// 3. way intersects border only once: store a copy with clipped geometry
if (solution.size() == 1) {
Area clippedArea;
setData(clippedArea, area, solution[0]);
callback_(clippedArea, quadKey_);
}
// 4. in this case, result should be stored as relation (collection of areas)
else {
Relation relation;
relation.id = area.id;
relation.tags = area.tags;
relation.elements.reserve(solution.size());
for (auto it = solution.begin(); it != solution.end(); ++it) {
auto clippedArea = std::make_shared<Area> ();
clippedArea->id = area.id;
setCoordinates(*clippedArea, *it);
relation.elements.push_back(clippedArea);
}
callback_(relation, quadKey_);
}
}
void
ClipperPaths_to_Slic3rMultiPoints(const ClipperLib::Paths &input, T* output)
{
PROFILE_FUNC();
output->clear();
output->reserve(input.size());
for (ClipperLib::Paths::const_iterator it = input.begin(); it != input.end(); ++it) {
typename T::value_type p;
ClipperPath_to_Slic3rMultiPoint(*it, &p);
output->push_back(p);
}
}
DLL_PUBLIC void CDECL execute_offset(ClipperLib::ClipperOffset *ptr, double delta,
void* outputArray, void(*append)(void* outputArray, size_t polyIndex, ClipperLib::IntPoint point)) {
ClipperLib::Paths paths = ClipperLib::Paths();
try {
ptr->Execute(paths, delta);
} catch(ClipperLib::clipperException e) {
printf(e.what());
}
for (size_t i = 0; i < paths.size(); i++) {
for (auto &point: paths[i]) {
append(outputArray, i, point);
}
}
}
geo::Ring<Vector> Environment::inflate(geo::Ring<Vector> const& ring, int inflateRadius) {
ClipperLib::Path subj;
ClipperLib::Paths solution;
for (Vector const& v : ring)
subj << ClipperLib::IntPoint((int)v.x, (int)v.y);
ClipperLib::ClipperOffset co;
co.AddPath(subj, ClipperLib::jtMiter, ClipperLib::etClosedPolygon);
co.Execute(solution, inflateRadius);
#ifdef DEBUG
assert(solution.size() == 1);
#endif
Ring ans;
for (ClipperLib::IntPoint const& v : solution[0])
ans.push_back(Vector(v.X, v.Y));
geo::correct(ans);
return ans;
}
geo::Polygon<geo::Ring<Vector>> Environment::subtract(geo::Polygon<geo::Ring<Vector>> const& poly, geo::Ring<Vector> const& ring) {
ClipperLib::Path subj;
ClipperLib::Paths solution;
ClipperLib::Clipper c;
for (Vector const& v : poly.ering)
subj.push_back(ClipperLib::IntPoint((int)v.x, (int)v.y));
c.AddPath(subj, ClipperLib::ptSubject, true);
for (Ring const& ring : poly.irings) {
subj.clear();
for (Vector const& v : ring)
subj.push_back(ClipperLib::IntPoint((int)v.x, (int)v.y));
std::reverse(subj.begin(), subj.end());
c.AddPath(subj, ClipperLib::ptSubject, true);
}
subj.clear();
for (Vector const& v : ring)
subj.push_back(ClipperLib::IntPoint((int)v.x, (int)v.y));
c.AddPath(subj, ClipperLib::ptClip, true);
c.Execute(ClipperLib::ctDifference, solution);
geo::Polygon<geo::Ring<Vector>> ans;
for (ClipperLib::IntPoint const& pt : solution[0]) {
ans.ering.push_back({pt.X, pt.Y});
}
for (int i = 1; i < solution.size(); ++i) {
ClipperLib::Path const& path = solution[i];
geo::Ring<Vector> ring;
for (ClipperLib::IntPoint const& pt : path)
ring.push_back({pt.X, pt.Y});
ans.irings.push_back(ring);
}
geo::correct(ans);
return ans;
}
double poly_intersection(const ContContainer& poly1, const ContContainer& poly2)
{
/* ************* TEMPORAL ************
* Conversion, we should remove junctions from container
* or define it for our containers */
ClipperLib::Paths paths1(poly1.begin(),poly1.end());
ClipperLib::Paths paths2(poly2.begin(),poly2.end());
/* Get the intersection polygon */
ClipperLib::Clipper clpr;
clpr.AddPaths(paths1, ClipperLib::ptSubject, true);
clpr.AddPaths(paths2, ClipperLib::ptClip , true);
ClipperLib::Paths solution;
clpr.Execute(ClipperLib::ctIntersection, solution, ClipperLib::pftEvenOdd, ClipperLib::pftEvenOdd);
/* Get its area */
double int_area = 0;
for(std::size_t ii=0; ii<solution.size(); ++ii)
int_area += std::abs(ClipperLib::Area(solution[ii]));
return int_area;
}
DLL_PUBLIC bool CDECL execute(ClipperLib::Clipper *ptr, ClipperLib::ClipType clipType,
ClipperLib::PolyFillType subjFillType, ClipperLib::PolyFillType clipFillType,
void* outputArray, void(*append)(void* outputArray, size_t polyIndex, ClipperLib::IntPoint point)) {
ClipperLib::Paths paths = ClipperLib::Paths();
bool result = false;
try {
result = ptr->Execute(clipType, paths, subjFillType, clipFillType);
} catch(ClipperLib::clipperException e) {
printf(e.what());
}
if (!result)
return false;
for (size_t i = 0; i < paths.size(); i++) {
for (auto &point: paths[i]) {
append(outputArray, i, point);
}
}
return true;
}
// This is a safe variant of the polygons offset, tailored for multiple ExPolygons.
// It is required, that the input expolygons do not overlap and that the holes of each ExPolygon don't intersect with their respective outer contours.
// Each ExPolygon is offsetted separately, then the offsetted ExPolygons are united.
ClipperLib::Paths _offset(const Slic3r::ExPolygons &expolygons, const float delta,
ClipperLib::JoinType joinType, double miterLimit)
{
const float delta_scaled = delta * float(CLIPPER_OFFSET_SCALE);
// Offsetted ExPolygons before they are united.
ClipperLib::Paths contours_cummulative;
contours_cummulative.reserve(expolygons.size());
// How many non-empty offsetted expolygons were actually collected into contours_cummulative?
// If only one, then there is no need to do a final union.
size_t expolygons_collected = 0;
for (Slic3r::ExPolygons::const_iterator it_expoly = expolygons.begin(); it_expoly != expolygons.end(); ++ it_expoly) {
// 1) Offset the outer contour.
ClipperLib::Paths contours;
{
ClipperLib::Path input = Slic3rMultiPoint_to_ClipperPath(it_expoly->contour);
scaleClipperPolygon(input);
ClipperLib::ClipperOffset co;
if (joinType == jtRound)
co.ArcTolerance = miterLimit * double(CLIPPER_OFFSET_SCALE);
else
co.MiterLimit = miterLimit;
co.ShortestEdgeLength = double(std::abs(delta_scaled * CLIPPER_OFFSET_SHORTEST_EDGE_FACTOR));
co.AddPath(input, joinType, ClipperLib::etClosedPolygon);
co.Execute(contours, delta_scaled);
}
if (contours.empty())
// No need to try to offset the holes.
continue;
if (it_expoly->holes.empty()) {
// No need to subtract holes from the offsetted expolygon, we are done.
contours_cummulative.insert(contours_cummulative.end(), contours.begin(), contours.end());
++ expolygons_collected;
} else {
// 2) Offset the holes one by one, collect the offsetted holes.
ClipperLib::Paths holes;
{
for (Polygons::const_iterator it_hole = it_expoly->holes.begin(); it_hole != it_expoly->holes.end(); ++ it_hole) {
ClipperLib::Path input = Slic3rMultiPoint_to_ClipperPath_reversed(*it_hole);
scaleClipperPolygon(input);
ClipperLib::ClipperOffset co;
if (joinType == jtRound)
co.ArcTolerance = miterLimit * double(CLIPPER_OFFSET_SCALE);
else
co.MiterLimit = miterLimit;
co.ShortestEdgeLength = double(std::abs(delta_scaled * CLIPPER_OFFSET_SHORTEST_EDGE_FACTOR));
co.AddPath(input, joinType, ClipperLib::etClosedPolygon);
ClipperLib::Paths out;
co.Execute(out, - delta_scaled);
holes.insert(holes.end(), out.begin(), out.end());
}
}
// 3) Subtract holes from the contours.
if (holes.empty()) {
// No hole remaining after an offset. Just copy the outer contour.
contours_cummulative.insert(contours_cummulative.end(), contours.begin(), contours.end());
++ expolygons_collected;
} else if (delta < 0) {
// Negative offset. There is a chance, that the offsetted hole intersects the outer contour.
// Subtract the offsetted holes from the offsetted contours.
ClipperLib::Clipper clipper;
clipper.Clear();
clipper.AddPaths(contours, ClipperLib::ptSubject, true);
clipper.AddPaths(holes, ClipperLib::ptClip, true);
ClipperLib::Paths output;
clipper.Execute(ClipperLib::ctDifference, output, ClipperLib::pftNonZero, ClipperLib::pftNonZero);
if (! output.empty()) {
contours_cummulative.insert(contours_cummulative.end(), output.begin(), output.end());
++ expolygons_collected;
} else {
// The offsetted holes have eaten up the offsetted outer contour.
}
} else {
// Positive offset. As long as the Clipper offset does what one expects it to do, the offsetted hole will have a smaller
// area than the original hole or even disappear, therefore there will be no new intersections.
// Just collect the reversed holes.
contours_cummulative.reserve(contours.size() + holes.size());
contours_cummulative.insert(contours_cummulative.end(), contours.begin(), contours.end());
// Reverse the holes in place.
for (size_t i = 0; i < holes.size(); ++ i)
std::reverse(holes[i].begin(), holes[i].end());
contours_cummulative.insert(contours_cummulative.end(), holes.begin(), holes.end());
++ expolygons_collected;
}
}
}
// 4) Unite the offsetted expolygons.
ClipperLib::Paths output;
if (expolygons_collected > 1 && delta > 0) {
// There is a chance that the outwards offsetted expolygons may intersect. Perform a union.
ClipperLib::Clipper clipper;
clipper.Clear();
clipper.AddPaths(contours_cummulative, ClipperLib::ptSubject, true);
clipper.Execute(ClipperLib::ctUnion, output, ClipperLib::pftNonZero, ClipperLib::pftNonZero);
} else {
// Negative offset. The shrunk expolygons shall not mutually intersect. Just copy the output.
output = std::move(contours_cummulative);
}
// 4) Unscale the output.
unscaleClipperPolygons(output);
return output;
}
void World::update(cv::Mat &homography)
{
this->m_world->Step(dt, 10, 10);
//check contacts
std::vector<MyContact>::iterator pos;
std::map<b2Body*, ClipperLib::Paths*> newBodyMap;
std::vector<b2Body*> removeBarrierList;
for(pos = this->m_contactListener->m_contacts.begin();
pos != this->m_contactListener->m_contacts.end();
++pos)
{
MyContact contact = *pos;
if ((contact.fixtureA == this->m_ballFixture || contact.fixtureB == this->m_ballFixture)
&& (contact.fixtureA->GetBody() != m_groundBody && contact.fixtureB->GetBody() != m_groundBody)
&& (contact.fixtureA->GetBody() != m_paddlesBody && contact.fixtureB->GetBody() != m_paddlesBody))
{
b2Fixture* objectFixture = contact.fixtureA == this->m_ballFixture ? contact.fixtureB : contact.fixtureA;
b2Body *objectBody = objectFixture->GetBody();
if (objectFixture->GetType() == b2Shape::e_edge)
{
cv::Point2f hitPoint = CVUtils::transformPoint(cv::Point2f(contact.contactPoint->x * PTM_RATIO, contact.contactPoint->y * PTM_RATIO), homography);
this->notifyBallHitObservers(hitPoint.x, hitPoint.y);
// change the shape of the fixture
// only go into processing if this body was not processed yet (possible ball hit two fixture of same body)
if (newBodyMap.find(objectBody) == newBodyMap.end())
{
ClipperLib::Paths* bodyPolygons = (ClipperLib::Paths*)objectBody->GetUserData();
b2Vec2* impactVelocity = contact.fixtureA == m_ballFixture ? contact.impactVelocityA : contact.impactVelocityB;
float ballAngle = atan2(impactVelocity->y, impactVelocity->x); // get the angle (in radians) the ball is moving to
float ballPower = impactVelocity->Length() * 0.5; // get the "power" of the ball movement vector
float openingStepInRadians = 10 * CV_PI / 180; // calculate the opening in radians
// create the clipping object/shape - a wedge from ball's center with 30 degree opening over ball direction (angle)
ClipperLib::Path clip;
clip.push_back(ClipperLib::IntPoint(contact.contactPoint->x * PTM_RATIO, contact.contactPoint->y * PTM_RATIO));
for(int step = 9; step > -10; step--)
{
float dx = cos(ballAngle + step * openingStepInRadians) * ballPower;
float dy = sin(ballAngle + step * openingStepInRadians) * ballPower;
clip.push_back(ClipperLib::IntPoint(contact.contactPoint->x * PTM_RATIO + dx, contact.contactPoint->y * PTM_RATIO + dy));
}
ClipperLib::Clipper clipper;
clipper.AddPaths((*bodyPolygons), ClipperLib::ptSubject, true);
clipper.AddPath(clip, ClipperLib::ptClip, true);
// the difference is the new polygon formed by the clipping (collision)
ClipperLib::Paths* newPolygons = new ClipperLib::Paths();
clipper.Execute(ClipperLib::ctDifference, (*newPolygons), ClipperLib::pftEvenOdd, ClipperLib::pftEvenOdd);
// Save the new polygons of this body
objectBody->SetUserData(newPolygons);
newBodyMap[objectBody] = newPolygons;
// now, find the intersection regions - these should be inpainted to the scene
ClipperLib::Paths destroyedParts;
clipper.Execute(ClipperLib::ctIntersection, destroyedParts, ClipperLib::pftEvenOdd, ClipperLib::pftEvenOdd);
// paint the required areas to be coppied
for (size_t i = 0; i < destroyedParts.size(); i++)
{
cv::Point* points = new cv::Point[destroyedParts[i].size()];
for (size_t j = 0; j < destroyedParts[i].size(); j++)
{
points[j].x = (int)destroyedParts[i][j].X;
points[j].y = (int)destroyedParts[i][j].Y;
}
m_destroyedPolygons.push_back(points);
m_destroyedPolygonsPointCount.push_back((int)destroyedParts[i].size());
}
}
}
else if (objectFixture->GetType() == b2Shape::e_circle)
{
// this is a barrier - add it to the remove list
removeBarrierList.push_back(objectBody);
}
}
}
std::map<b2Body*, ClipperLib::Paths*>::iterator iter;
for(iter = newBodyMap.begin(); iter != newBodyMap.end(); iter++)
{
b2Body* objectBody = iter->first;
ClipperLib::Paths* newPolygons = iter->second;
// remove all the current fixtures from this body
for (b2Fixture* f = objectBody->GetFixtureList(); f; )
{
b2Fixture* fixtureToDestroy = f;
f = f->GetNext();
objectBody->DestroyFixture( fixtureToDestroy );
}
if(newPolygons->size() == 0)
{
// there is no more pieces of the object left so remove it from list and world
m_objectBodies.erase(std::find(m_objectBodies.begin(), m_objectBodies.end(), objectBody));
m_world->DestroyBody(objectBody); // TODO: better physics world cleanup
}
else
{
for (size_t i = 0; i < newPolygons->size(); i++)
{
b2EdgeShape objectEdgeShape;
b2FixtureDef objectShapeDef;
objectShapeDef.shape = &objectEdgeShape;
ClipperLib::Path polygon = newPolygons->at(i);
size_t j;
for (j = 0; j < polygon.size() - 1; j++)
{
objectEdgeShape.Set(b2Vec2(polygon[j].X / PTM_RATIO, polygon[j].Y / PTM_RATIO), b2Vec2(polygon[j+1].X / PTM_RATIO, polygon[j+1].Y / PTM_RATIO));
objectBody->CreateFixture(&objectShapeDef);
}
objectEdgeShape.Set(b2Vec2(polygon[j].X / PTM_RATIO, polygon[j].Y / PTM_RATIO), b2Vec2(polygon[0].X / PTM_RATIO, polygon[0].Y / PTM_RATIO));
objectBody->CreateFixture(&objectShapeDef);
}
}
}
for (size_t i = 0; i < removeBarrierList.size(); i++){
cv::Point2f* p = (cv::Point2f*)removeBarrierList[i]->GetUserData();
std::vector<cv::Point2f*>::iterator position = std::find(m_guardLocations.begin(), m_guardLocations.end(), p);
if (position != m_guardLocations.end()){ // == vector.end() means the element was not found
m_guardLocations.erase(position);
}
removeBarrierList[i]->GetWorld()->DestroyBody(removeBarrierList[i]);
}
}
