| 128 | } |
| 129 | |
| 130 | void Polygon::getBoundingBox(Position& center, Length& length) const |
| 131 | { |
| 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(); |
| 136 | for (const auto& vertex : vertices_) { |
| 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(); |
| 141 | } |
| 142 | center.x() = (minX + maxX) / 2.0; |
| 143 | center.y() = (minY + maxY) / 2.0; |
| 144 | length.x() = (maxX - minX); |
| 145 | length.y() = (maxY - minY); |
| 146 | } |
| 147 | |
| 148 | bool Polygon::convertToInequalityConstraints(Eigen::MatrixXd& A, Eigen::VectorXd& b) const |
| 149 | { |