12 #include <Eigen/Geometry> 43 return bool(cross % 2);
105 for (
size_t i = 0; i <
vertices_.size(); i++) {
110 return std::abs(area / 2.0);
115 Position centroid = Position::Zero();
117 vertices.push_back(vertices.at(0));
119 for (
size_t i = 0; i < vertices.size() - 1; i++) {
120 const double a = vertices[i].x() * vertices[i+1].y() - vertices[i+1].x() * vertices[i].y();
122 centroid.x() += a * (vertices[i].x() + vertices[i+1].x());
123 centroid.y() += a * (vertices[i].y() + vertices[i+1].y());
126 centroid /= (6.0 * area);
132 double minX = std::numeric_limits<double>::infinity();
133 double maxX = -std::numeric_limits<double>::infinity();
134 double minY = std::numeric_limits<double>::infinity();
135 double maxY = -std::numeric_limits<double>::infinity();
137 if (vertex.x() > maxX) maxX = vertex.x();
138 if (vertex.y() > maxY) maxY = vertex.y();
139 if (vertex.x() < minX) minX = vertex.x();
140 if (vertex.y() < minY) minY = vertex.y();
142 center.x() = (minX + maxX) / 2.0;
143 center.y() = (minY + maxY) / 2.0;
144 length.x() = (maxX - minX);
145 length.y() = (maxY - minY);
151 for (
unsigned int i = 0; i <
nVertices(); ++i)
159 for (
unsigned int i = 0; i < V.rows(); ++i)
160 k.row(i) << i, (i+1) % V.rows();
161 Eigen::RowVectorXd c = V.colwise().mean();
163 A = Eigen::MatrixXd::Constant(k.rows(), V.cols(), NAN);
166 for (
unsigned int ix = 0; ix < k.rows(); ++ix) {
167 Eigen::MatrixXd F(2, V.cols());
168 F.row(0) << V.row(k(ix, 0));
169 F.row(1) << V.row(k(ix, 1));
170 Eigen::FullPivLU<Eigen::MatrixXd> luDecomp(F);
171 if (luDecomp.rank() == F.rows()) {
172 A.row(rc) = F.colPivHouseholderQr().solve(Eigen::VectorXd::Ones(F.rows()));
178 b = Eigen::VectorXd::Ones(A.rows());
179 b = b + A * c.transpose();
188 const Vector orthogonal = thickness *
Vector(connection.y(), -connection.x()).normalized();
189 std::vector<Position> newVertices;
190 newVertices.reserve(4);
191 newVertices.push_back(
vertices_[0] + orthogonal);
192 newVertices.push_back(
vertices_[0] - orthogonal);
193 newVertices.push_back(
vertices_[1] - orthogonal);
194 newVertices.push_back(
vertices_[1] + orthogonal);
203 std::vector<Eigen::Array2i> neighbourIndices;
205 neighbourIndices.resize(n);
206 for (
unsigned int i = 0; i < n; ++i) {
207 neighbourIndices[i] << (i > 0 ? (i-1)%n : n-1), (i + 1) % n;
211 for (
unsigned int i = 0; i < neighbourIndices.size(); ++i) {
216 const double angle = acos(v1.dot(v2));
217 copy[i] += margin / sin(angle) * (v1 + v2);
227 std::vector<Polygon> polygons;
232 polygons.reserve(nPolygons);
236 polygons.push_back(*
this);
239 for (
size_t i = 0; i < nPolygons; ++i) {
241 polygons.push_back((polygon));
251 Eigen::Vector2d centerToVertex(radius, 0.0), centerToVertexTemp;
255 double theta = j * 2 * M_PI / (nVertices - 1);
256 Eigen::Rotation2D<double> rot2d(theta);
257 centerToVertexTemp = rot2d.toRotationMatrix() * centerToVertex;
258 polygon.
addVertex(center + centerToVertexTemp);
264 const Position center2,
const double radius,
267 if (center1 == center2)
return fromCircle(center1, radius, nVertices);
268 Eigen::Vector2d centerToVertex, centerToVertexTemp;
269 centerToVertex = center2 - center1;
270 centerToVertex.normalize();
271 centerToVertex *= radius;
274 for (
int j = 0; j < ceil(nVertices / 2.0); j++) {
275 double theta = M_PI_2 + j * M_PI / (ceil(nVertices / 2.0) - 1);
276 Eigen::Rotation2D<double> rot2d(theta);
277 centerToVertexTemp = rot2d.toRotationMatrix() * centerToVertex;
278 polygon.
addVertex(center1 + centerToVertexTemp);
280 for (
int j = 0; j < ceil(nVertices / 2.0); j++) {
281 double theta = 3 * M_PI_2 + j * M_PI / (ceil(nVertices / 2.0) - 1);
282 Eigen::Rotation2D<double> rot2d(theta);
283 centerToVertexTemp = rot2d.toRotationMatrix() * centerToVertex;
284 polygon.
addVertex(center2 + centerToVertexTemp);
291 std::vector<Position> vertices;
302 if (points.size() <= 3) {
305 std::vector<Position> pointsConvexHull(2 * points.size());
308 auto sortedPoints(points);
309 std::sort(sortedPoints.begin(), sortedPoints.end(),
sortVertices);
314 for (
size_t i = 0; i < sortedPoints.size(); ++i) {
315 while (k >= 2 &&
vectorsMakeClockwiseTurn(pointsConvexHull.at(k - 2), pointsConvexHull.at(k - 1), sortedPoints.at(i))) {
318 pointsConvexHull.at(k++) = sortedPoints.at(i);
322 for (
int i = sortedPoints.size() - 2, t = k + 1; i >= 0; i--) {
323 while (k >= t &&
vectorsMakeClockwiseTurn(pointsConvexHull.at(k - 2), pointsConvexHull.at(k - 1), sortedPoints.at(i))) {
326 pointsConvexHull.at(k++) = sortedPoints.at(i);
328 pointsConvexHull.resize(k - 1);
330 Polygon polygon(pointsConvexHull);
335 const Eigen::Vector2d& vector2)
337 return (vector1.x() < vector2.x()
338 || (vector1.x() == vector2.x() && vector1.y() < vector2.y()));
342 const Eigen::Vector2d& vector2)
344 return (vector1.x() * vector2.y() - vector1.y() * vector2.x());
348 const Eigen::Vector2d &pointA,
349 const Eigen::Vector2d &pointB)
static Polygon convexHullOfTwoCircles(const Position center1, const Position center2, const double radius, const int nVertices=20)
std::vector< Polygon > triangulate(const TriangulationMethods &method=TriangulationMethods::FAN) const
static double computeCrossProduct2D(const Eigen::Vector2d &vector1, const Eigen::Vector2d &vector2)
const std::string & getFrameId() const
Position getCentroid() const
static Polygon monotoneChainConvexHullOfPoints(const std::vector< Position > &points)
uint64_t timestamp_
Timestamp of the polygon (nanoseconds).
bool convertToInequalityConstraints(Eigen::MatrixXd &A, Eigen::VectorXd &b) const
static Polygon fromCircle(const Position center, const double radius, const int nVertices=20)
const Position & operator[](const size_t index) const
bool thickenLine(const double thickness)
const Position & getVertex(const size_t index) const
bool isInside(const Position &point) const
std::string frameId_
Frame id of the polygon.
void addVertex(const Position &vertex)
std::vector< Position > vertices_
Vertices of the polygon.
static Polygon convexHull(Polygon &polygon1, Polygon &polygon2)
static bool sortVertices(const Eigen::Vector2d &vector1, const Eigen::Vector2d &vector2)
void getBoundingBox(Position ¢er, Length &length) const
static double vectorsMakeClockwiseTurn(const Eigen::Vector2d &pointO, const Eigen::Vector2d &pointA, const Eigen::Vector2d &pointB)
void setFrameId(const std::string &frameId)
uint64_t getTimestamp() const
bool offsetInward(const double margin)
const std::vector< Position > & getVertices() const
void setTimestamp(const uint64_t timestamp)