| 146 | } |
| 147 | |
| 148 | bool Polygon::convertToInequalityConstraints(Eigen::MatrixXd& A, Eigen::VectorXd& b) const |
| 149 | { |
| 150 | Eigen::MatrixXd V(nVertices(), 2); |
| 151 | for (unsigned int i = 0; i < nVertices(); ++i) |
| 152 | V.row(i) = vertices_[i]; |
| 153 | |
| 154 | // Create k, a list of indices from V forming the convex hull. |
| 155 | // TODO: Assuming counter-clockwise ordered convex polygon. |
| 156 | // MATLAB: k = convhulln(V); |
| 157 | Eigen::MatrixXi k; |
| 158 | k.resizeLike(V); |
| 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(); |
| 162 | V.rowwise() -= c; |
| 163 | A = Eigen::MatrixXd::Constant(k.rows(), V.cols(), NAN); |
| 164 | |
| 165 | unsigned int rc = 0; |
| 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())); |
| 173 | ++rc; |
| 174 | } |
| 175 | } |
| 176 | |
| 177 | A = A.topRows(rc); |
| 178 | b = Eigen::VectorXd::Ones(A.rows()); |
| 179 | b = b + A * c.transpose(); |
| 180 | |
| 181 | return true; |
| 182 | } |
| 183 | |
| 184 | bool Polygon::thickenLine(const double thickness) |
| 185 | { |