Vector<Vector2> expand(const Vector<Vector2> &points, const Rect2i &rect, float epsilon = 2.0) {
int size = points.size();
ERR_FAIL_COND_V(size < 2, Vector<Vector2>());
ClipperLib::Path subj;
ClipperLib::PolyTree solution;
ClipperLib::PolyTree out;
for (int i = 0; i < points.size(); i++) {
subj << ClipperLib::IntPoint(points[i].x * PRECISION, points[i].y * PRECISION);
}
ClipperLib::ClipperOffset co;
co.AddPath(subj, ClipperLib::jtMiter, ClipperLib::etClosedPolygon);
co.Execute(solution, epsilon * PRECISION);
ClipperLib::PolyNode *p = solution.GetFirst();
ERR_FAIL_COND_V(!p, points);
while (p->IsHole()) {
p = p->GetNext();
}
//turn the result into simply polygon (AKA, fix overlap)
//clamp into the specified rect
ClipperLib::Clipper cl;
cl.StrictlySimple(true);
cl.AddPath(p->Contour, ClipperLib::ptSubject, true);
//create the clipping rect
ClipperLib::Path clamp;
clamp.push_back(ClipperLib::IntPoint(0, 0));
clamp.push_back(ClipperLib::IntPoint(rect.size.width * PRECISION, 0));
clamp.push_back(ClipperLib::IntPoint(rect.size.width * PRECISION, rect.size.height * PRECISION));
clamp.push_back(ClipperLib::IntPoint(0, rect.size.height * PRECISION));
cl.AddPath(clamp, ClipperLib::ptClip, true);
cl.Execute(ClipperLib::ctIntersection, out);
Vector<Vector2> outPoints;
ClipperLib::PolyNode *p2 = out.GetFirst();
ERR_FAIL_COND_V(!p2, points);
while (p2->IsHole()) {
p2 = p2->GetNext();
}
int lasti = p2->Contour.size() - 1;
Vector2 prev = Vector2(p2->Contour[lasti].X / PRECISION, p2->Contour[lasti].Y / PRECISION);
for (unsigned int i = 0; i < p2->Contour.size(); i++) {
Vector2 cur = Vector2(p2->Contour[i].X / PRECISION, p2->Contour[i].Y / PRECISION);
if (cur.distance_to(prev) > 0.5) {
outPoints.push_back(cur);
prev = cur;
}
}
return outPoints;
}
std::vector<Vec2> AutoPolygon::expand(const std::vector<Vec2>& points, const cocos2d::Rect &rect, const float& epsilon)
{
auto size = points.size();
// if there are less than 3 points, then we have nothing
if(size<3)
{
log("AUTOPOLYGON: cannot expand points for %s with less than 3 points, e: %f", _filename.c_str(), epsilon);
return std::vector<Vec2>();
}
ClipperLib::Path subj;
ClipperLib::PolyTree solution;
ClipperLib::PolyTree out;
for(std::vector<Vec2>::const_iterator it = points.begin(); it<points.end(); it++)
{
subj << ClipperLib::IntPoint(it-> x* PRECISION, it->y * PRECISION);
}
ClipperLib::ClipperOffset co;
co.AddPath(subj, ClipperLib::jtMiter, ClipperLib::etClosedPolygon);
co.Execute(solution, epsilon * PRECISION);
ClipperLib::PolyNode* p = solution.GetFirst();
if(!p)
{
log("AUTOPOLYGON: Clipper failed to expand the points");
return points;
}
while(p->IsHole()){
p = p->GetNext();
}
//turn the result into simply polygon (AKA, fix overlap)
//clamp into the specified rect
ClipperLib::Clipper cl;
cl.StrictlySimple(true);
cl.AddPath(p->Contour, ClipperLib::ptSubject, true);
//create the clipping rect
ClipperLib::Path clamp;
clamp.push_back(ClipperLib::IntPoint(0, 0));
clamp.push_back(ClipperLib::IntPoint(rect.size.width/_scaleFactor * PRECISION, 0));
clamp.push_back(ClipperLib::IntPoint(rect.size.width/_scaleFactor * PRECISION, rect.size.height/_scaleFactor * PRECISION));
clamp.push_back(ClipperLib::IntPoint(0, rect.size.height/_scaleFactor * PRECISION));
cl.AddPath(clamp, ClipperLib::ptClip, true);
cl.Execute(ClipperLib::ctIntersection, out);
std::vector<Vec2> outPoints;
ClipperLib::PolyNode* p2 = out.GetFirst();
while(p2->IsHole()){
p2 = p2->GetNext();
}
auto end = p2->Contour.end();
for(std::vector<ClipperLib::IntPoint>::const_iterator pt = p2->Contour.begin(); pt < end; pt++)
{
outPoints.push_back(Vec2(pt->X/PRECISION, pt->Y/PRECISION));
}
return outPoints;
}
std::vector<polygon> polygon::subtract(const polygon& subtrahend) const
{
ClipperLib::Clipper poly;
auto maxDenom = lcm(this->highestDenominator(), subtrahend.highestDenominator());
poly.AddPath(this->path(maxDenom), ClipperLib::PolyType::ptSubject, true);
poly.AddPath(subtrahend.path(maxDenom), ClipperLib::PolyType::ptClip, true);
ClipperLib::Paths returnedPaths;
poly.Execute(ClipperLib::ClipType::ctDifference, returnedPaths);
return polygon::from(returnedPaths, maxDenom);
}
polygon polygon::createUnion(const std::vector<polygon>& data)
{
base_int maxDenom = 1;
for(auto poly : data)
{
maxDenom = lcm(maxDenom, poly.highestDenominator());
}
maxDenom = std::min(maxDenom, base_int(1000000000L));
//std::cout << "-----" << std::endl;
//std::cout << maxDenom << std::endl;
polygon base = data[0];
for(unsigned int i = 1; i < data.size(); i++)
{
ClipperLib::Clipper clipper;
clipper.AddPath(base.path(maxDenom), ClipperLib::PolyType::ptSubject, true);
clipper.AddPath(data[i].path(maxDenom), ClipperLib::PolyType::ptClip, true);
ClipperLib::Paths returnedPaths;
clipper.Execute(ClipperLib::ClipType::ctUnion, returnedPaths, ClipperLib::pftNonZero, ClipperLib::pftNonZero);
base = polygon::from(returnedPaths, maxDenom)[0];
}
return base;
}
/*
std::vector<Polygon> DecomposePolygonToConvexhulls(const Polygon& polygon) {
using VHACD::IVHACD;
TriangleMesh mesh = TriangulatePolygon(polygon);
std::vector<float> points;
points.reserve(2 * mesh.vertices.size());
for (auto& vertex : mesh.vertices) {
points.emplace_back(vertex(0));
points.emplace_back(vertex(1));
}
std::vector<int> triangle_indices;
triangle_indices.reserve(mesh.faces.size() * 3);
for (auto& tr_idx : mesh.faces) {
triangle_indices.emplace_back(tr_idx[0]);
triangle_indices.emplace_back(tr_idx[1]);
triangle_indices.emplace_back(tr_idx[2]);
}
IVHACD::Parameters params;
//
// params.m_maxNumVerticesPerCH = 8;
params.m_oclAcceleration = false;
IVHACD* vhacd_interface = VHACD::CreateVHACD();
bool res = vhacd_interface->Compute(points.data(), 2, mesh.vertices.size(),
triangle_indices.data(), 3,
mesh.faces.size(), params);
std::vector<Polygon> polygons;
if (res) {
size_t num_hulls = vhacd_interface->GetNConvexHulls();
IVHACD::ConvexHull hull;
for (size_t p = 0; p < num_hulls; ++p) {
vhacd_interface->GetConvexHull(p, hull);
for (size_t v = 0; v < hull.m_nPoints; ++v) {
std::cout << p << " ";
std::cout << hull.m_points[3 * v + 0] << " ";
std::cout << hull.m_points[3 * v + 1] << " ";
std::cout << hull.m_points[3 * v + 2] << "\n";
}
}
} else {
std::cerr << "convex hull decomposition not successfull! fall back to "
"triangulation!\n";
}
vhacd_interface->Clean();
vhacd_interface->Release();
exit(0);
return polygons;
}
*/
std::vector<Polygon2D> ResolveIntersections(const Polygon2D& polygon) {
// the polygon boundary maybe splitted during this process
// auto paths = ResolveIntersectionsClosedPath(polygon.path);
// auto holes = ResolveIntersectionsClosedPaths(polygon.holes);
ClipperLib::Clipper clipper;
ClipperLib::Path scaled_path = UScalePathDiaToClipper(polygon.path);
clipper.AddPath(scaled_path, ClipperLib::ptSubject, true);
/*
for (auto& path : paths) {
ClipperLib::Path scaled_path = UScalePathDiaToClipper(path);
clipper.AddPath(scaled_path, ClipperLib::ptSubject, true);
}*/
for (auto& hole : polygon.holes) {
ClipperLib::Path scaled_hole = UScalePathDiaToClipper(hole);
clipper.AddPath(scaled_hole, ClipperLib::ptClip, true);
}
ClipperLib::PolyTree path_tree;
clipper.StrictlySimple(true);
clipper.Execute(ClipperLib::ctDifference, path_tree, ClipperLib::pftNonZero,
ClipperLib::pftNonZero);
// iterating into the tree
std::vector<Polygon2D> polygons;
// only store the pointer to outer polygons
std::unordered_map<ClipperLib::PolyNode*, size_t> polynode_map;
for (ClipperLib::PolyNode* node_ptr = path_tree.GetFirst(); node_ptr;
node_ptr = node_ptr->GetNext()) {
ClipperLib::PolyNode* poly_ptr = node_ptr;
while (poly_ptr && poly_ptr->IsHole()) {
poly_ptr = poly_ptr->Parent;
}
if (polynode_map.find(poly_ptr) == polynode_map.end()) {
polygons.emplace_back(Polygon2D());
polygons.back().path = DScalePathClipperToDia(poly_ptr->Contour);
polynode_map[poly_ptr] = polygons.size() - 1;
} else {
polygons[polynode_map[poly_ptr]].holes.emplace_back(
DScalePathClipperToDia(node_ptr->Contour));
}
}
return polygons;
}
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;
}
GeometryCollection fixupPolygons(const GeometryCollection& rings) {
ClipperLib::Clipper clipper;
clipper.StrictlySimple(true);
for (const auto& ring : rings) {
clipper.AddPath(toClipperPath(ring), ClipperLib::ptSubject, true);
}
ClipperLib::PolyTree polygons;
clipper.Execute(ClipperLib::ctUnion, polygons, ClipperLib::pftEvenOdd, ClipperLib::pftEvenOdd);
clipper.Clear();
GeometryCollection result;
for (auto * polynode : polygons.Childs) {
processPolynodeBranch(polynode, result);
}
return result;
}
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]);
}
}
bool validate() const
{
mapnik::geometry::geometry<double> geom;
if (!mapnik::from_wkt(wkt_in_, geom))
{
throw std::runtime_error("Failed to parse WKT");
}
if (mapnik::geometry::is_empty(geom))
{
std::clog << "empty geom!\n";
return false;
}
if (!geom.is<mapnik::geometry::polygon<double> >())
{
std::clog << "not a polygon!\n";
return false;
}
mapnik::geometry::polygon<double> & poly = mapnik::util::get<mapnik::geometry::polygon<double> >(geom);
mapnik::geometry::correct(poly);
ClipperLib::Clipper clipper;
mapnik::geometry::line_string<std::int64_t> path;
for (auto const& pt : poly.exterior_ring)
{
double x = pt.x;
double y = pt.y;
path.emplace_back(static_cast<ClipperLib::cInt>(x),static_cast<ClipperLib::cInt>(y));
}
if (!clipper.AddPath(path, ClipperLib::ptSubject, true))
{
std::clog << "ptSubject ext failed!\n";
}
for (auto const& ring : poly.interior_rings)
{
path.clear();
for (auto const& pt : ring)
{
double x = pt.x;
double y = pt.y;
path.emplace_back(static_cast<ClipperLib::cInt>(x),static_cast<ClipperLib::cInt>(y));
}
if (!clipper.AddPath(path, ClipperLib::ptSubject, true))
{
std::clog << "ptSubject ext failed!\n";
}
}
std::cerr << "path size=" << path.size() << std::endl;
mapnik::geometry::line_string<std::int64_t> clip_box;
clip_box.emplace_back(static_cast<ClipperLib::cInt>(extent_.minx()),static_cast<ClipperLib::cInt>(extent_.miny()));
clip_box.emplace_back(static_cast<ClipperLib::cInt>(extent_.maxx()),static_cast<ClipperLib::cInt>(extent_.miny()));
clip_box.emplace_back(static_cast<ClipperLib::cInt>(extent_.maxx()),static_cast<ClipperLib::cInt>(extent_.maxy()));
clip_box.emplace_back(static_cast<ClipperLib::cInt>(extent_.minx()),static_cast<ClipperLib::cInt>(extent_.maxy()));
clip_box.emplace_back(static_cast<ClipperLib::cInt>(extent_.minx()),static_cast<ClipperLib::cInt>(extent_.miny()));
if (!clipper.AddPath( clip_box, ClipperLib::ptClip, true ))
{
std::clog << "ptClip failed!\n";
}
ClipperLib::PolyTree polygons;
clipper.Execute(ClipperLib::ctIntersection, polygons, ClipperLib::pftNonZero, ClipperLib::pftNonZero);
clipper.Clear();
ClipperLib::PolyNode* polynode = polygons.GetFirst();
mapnik::geometry::multi_polygon<double> mp;
mp.emplace_back();
bool first = true;
while (polynode)
{
if (!polynode->IsHole())
{
if (first) first = false;
else mp.emplace_back(); // start new polygon
for (auto const& pt : polynode->Contour)
{
mp.back().exterior_ring.add_coord(pt.x, pt.y);
}
// childrens are interior rings
for (auto const* ring : polynode->Childs)
{
mapnik::geometry::linear_ring<double> hole;
for (auto const& pt : ring->Contour)
{
hole.add_coord(pt.x, pt.y);
}
mp.back().add_hole(std::move(hole));
}
}
polynode = polynode->GetNext();
}
std::string expect = expected_+".png";
std::string actual = expected_+"_actual.png";
mapnik::geometry::geometry<double> geom2(mp);
auto env = mapnik::geometry::envelope(geom2);
if (!mapnik::util::exists(expect) || (std::getenv("UPDATE") != nullptr))
{
std::clog << "generating expected image: " << expect << "\n";
render(mp,env,expect);
}
render(mp,env,actual);
return benchmark::compare_images(actual,expect);
}
