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;
}
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));
}
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
Slic3rMultiPoint_to_ClipperPath(const Slic3r::MultiPoint &input, ClipperLib::Path &output)
{
output.clear();
for (Slic3r::Points::const_iterator pit = input.points.begin(); pit != input.points.end(); ++pit) {
output.push_back(ClipperLib::IntPoint( (*pit).x, (*pit).y ));
}
}
ClipperLib::Path
Slic3rMultiPoint_to_ClipperPath(const MultiPoint &input)
{
ClipperLib::Path retval;
for (Points::const_iterator pit = input.points.begin(); pit != input.points.end(); ++pit)
retval.push_back(ClipperLib::IntPoint( (*pit).x, (*pit).y ));
return retval;
}
ClipperLib::Path ElementGeometryClipper::createPathFromBoundingBox()
{
double xMin = quadKeyBbox_.minPoint.longitude, yMin = quadKeyBbox_.minPoint.latitude,
xMax = quadKeyBbox_.maxPoint.longitude, yMax = quadKeyBbox_.maxPoint.latitude;
ClipperLib::Path rect;
rect.push_back(ClipperLib::IntPoint(static_cast<ClipperLib::cInt>(xMin*Scale),
static_cast<ClipperLib::cInt>(yMin*Scale)));
rect.push_back(ClipperLib::IntPoint(static_cast<ClipperLib::cInt>(xMax*Scale),
static_cast<ClipperLib::cInt>(yMin*Scale)));
rect.push_back(ClipperLib::IntPoint(static_cast<ClipperLib::cInt>(xMax*Scale),
static_cast<ClipperLib::cInt>(yMax*Scale)));
rect.push_back(ClipperLib::IntPoint(static_cast<ClipperLib::cInt>(xMin*Scale),
static_cast<ClipperLib::cInt>(yMax*Scale)));
return std::move(rect);
}
ClipperLib::Path polygon::path(base_int denom) const
{
ClipperLib::Path ret;
for(auto v : this->vertexes)
{
ret.push_back(ClipperLib::IntPoint((ClipperLib::cInt)(v.x.numerator() * (denom / v.x.denominator())), (ClipperLib::cInt)(v.y.numerator() * (denom / v.y.denominator()))));
}
return ret;
}
ClipperLib::Path Clipper::toClipper(const ofPolyline& polyline,
ClipperLib::cInt scale)
{
ClipperLib::Path path;
for (auto& vertex: polyline.getVertices())
{
path.push_back(toClipper(vertex, scale));
}
return path;
}
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;
}
// Set the objects (defined by contour points) to be models in the world and scene.
void World::setObjectsToBeModeled(const std::vector<std::vector<cv::Point>> contours) {
int contourSize = (int)contours.size();
for(int i = 0; i < contourSize; i++ )
{
std::vector<cv::Point> currentShape = contours[i];
int numOfPoints = (int)currentShape.size();
b2Vec2 * vertices = new b2Vec2[numOfPoints];
ClipperLib::Paths* polygons = new ClipperLib::Paths();
ClipperLib::Path polygon;
for (int j = 0; j < numOfPoints; j++)
{
vertices[j].x = currentShape[j].x / PTM_RATIO;
vertices[j].y = currentShape[j].y / PTM_RATIO;
//cv::line(m_scene, currentShape[j], currentShape[(j + 1) % numOfPoints], cv::Scalar(0,0,255));
//std::cout << "[" << vertices[j].x << "," <<vertices[j].y << "]" << std::endl;
polygon.push_back(ClipperLib::IntPoint(currentShape[j].x, currentShape[j].y));
}
b2BodyDef objectBodyDef;
objectBodyDef.type = b2_staticBody;
b2Body *objectBody = m_world->CreateBody(&objectBodyDef);
objectBody->SetUserData(polygons);
polygons->push_back(polygon);
b2EdgeShape objectEdgeShape;
b2FixtureDef objectShapeDef;
objectShapeDef.shape = &objectEdgeShape;
for (int j = 0; j < numOfPoints - 1; j++)
{
objectEdgeShape.Set(vertices[j], vertices[j+1]);
objectBody->CreateFixture(&objectShapeDef);
}
objectEdgeShape.Set(vertices[numOfPoints - 1], vertices[0]);
objectBody->CreateFixture(&objectShapeDef);
m_objectBodies.push_back(objectBody);
delete[] vertices;
}
}
ClipperLib::Path pathToClipperPath(const panda::types::Path& path)
{
ClipperLib::Path out;
auto maxVal = std::numeric_limits<ClipperLib::cInt>::max();
ClipperLib::IntPoint prevPt(maxVal, maxVal);
for (const auto& pt : path.points)
{
auto newPt = convert(pt);
if (newPt == prevPt)
continue;
out.push_back(newPt);
prevPt = newPt;
}
if (path.points.size() > 1 && path.points.back() == path.points.front())
out.pop_back();
return out;
}
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]);
}
}
