| Directory: | ./ |
|---|---|
| File: | src/shape/convex.cpp |
| Date: | 2025-04-01 09:23:31 |
| Exec | Total | Coverage | |
|---|---|---|---|
| Lines: | 2 | 19 | 10.5% |
| Branches: | 15 | 78 | 19.2% |
| Line | Branch | Exec | Source |
|---|---|---|---|
| 1 | #include "coal/shape/convex.h" | ||
| 2 | |||
| 3 | #ifdef COAL_HAS_QHULL | ||
| 4 | #include <libqhullcpp/QhullError.h> | ||
| 5 | #include <libqhullcpp/QhullFacet.h> | ||
| 6 | #include <libqhullcpp/QhullLinkedList.h> | ||
| 7 | #include <libqhullcpp/QhullVertex.h> | ||
| 8 | #include <libqhullcpp/QhullVertexSet.h> | ||
| 9 | #include <libqhullcpp/QhullRidge.h> | ||
| 10 | #include <libqhullcpp/Qhull.h> | ||
| 11 | |||
| 12 | using orgQhull::Qhull; | ||
| 13 | using orgQhull::QhullFacet; | ||
| 14 | using orgQhull::QhullPoint; | ||
| 15 | using orgQhull::QhullRidgeSet; | ||
| 16 | using orgQhull::QhullVertexList; | ||
| 17 | using orgQhull::QhullVertexSet; | ||
| 18 | #endif | ||
| 19 | |||
| 20 | namespace coal { | ||
| 21 | |||
| 22 | // Reorders `tri` such that the dot product between the normal of triangle and | ||
| 23 | // the vector `triangle barycentre - convex_tri.center` is positive. | ||
| 24 | ✗ | void reorderTriangle(const Convex<Triangle>* convex_tri, Triangle& tri) { | |
| 25 | ✗ | Vec3s p0, p1, p2; | |
| 26 | ✗ | p0 = (*(convex_tri->points))[tri[0]]; | |
| 27 | ✗ | p1 = (*(convex_tri->points))[tri[1]]; | |
| 28 | ✗ | p2 = (*(convex_tri->points))[tri[2]]; | |
| 29 | |||
| 30 | ✗ | Vec3s barycentre_tri, center_barycenter; | |
| 31 | ✗ | barycentre_tri = (p0 + p1 + p2) / 3; | |
| 32 | ✗ | center_barycenter = barycentre_tri - convex_tri->center; | |
| 33 | |||
| 34 | ✗ | Vec3s edge_tri1, edge_tri2, n_tri; | |
| 35 | ✗ | edge_tri1 = p1 - p0; | |
| 36 | ✗ | edge_tri2 = p2 - p1; | |
| 37 | ✗ | n_tri = edge_tri1.cross(edge_tri2); | |
| 38 | |||
| 39 | ✗ | if (center_barycenter.dot(n_tri) < 0) { | |
| 40 | ✗ | tri.set(tri[1], tri[0], tri[2]); | |
| 41 | } | ||
| 42 | } | ||
| 43 | |||
| 44 | ✗ | ConvexBase* ConvexBase::convexHull(std::shared_ptr<std::vector<Vec3s>>& pts, | |
| 45 | unsigned int num_points, bool keepTriangles, | ||
| 46 | const char* qhullCommand) { | ||
| 47 | COAL_COMPILER_DIAGNOSTIC_PUSH | ||
| 48 | COAL_COMPILER_DIAGNOSTIC_IGNORED_DEPRECECATED_DECLARATIONS | ||
| 49 | ✗ | return ConvexBase::convexHull(pts->data(), num_points, keepTriangles, | |
| 50 | ✗ | qhullCommand); | |
| 51 | COAL_COMPILER_DIAGNOSTIC_POP | ||
| 52 | } | ||
| 53 | |||
| 54 | 1 | ConvexBase* ConvexBase::convexHull(const Vec3s* pts, unsigned int num_points, | |
| 55 | bool keepTriangles, | ||
| 56 | const char* qhullCommand) { | ||
| 57 | #ifdef COAL_HAS_QHULL | ||
| 58 | if (num_points <= 3) { | ||
| 59 | COAL_THROW_PRETTY( | ||
| 60 | "You shouldn't use this function with less than" | ||
| 61 | " 4 points.", | ||
| 62 | std::invalid_argument); | ||
| 63 | } | ||
| 64 | assert(pts[0].data() + 3 == pts[1].data()); | ||
| 65 | |||
| 66 | Qhull qh; | ||
| 67 | const char* command = | ||
| 68 | qhullCommand ? qhullCommand : (keepTriangles ? "Qt" : ""); | ||
| 69 | |||
| 70 | // TODO: add a ifdef not double precision here | ||
| 71 | using Vec3d = Eigen::Vector3d; | ||
| 72 | std::vector<Vec3d> qhull_pts; | ||
| 73 | qhull_pts.reserve(num_points); | ||
| 74 | for (size_t i = 0; i < num_points; ++i) { | ||
| 75 | qhull_pts.push_back(pts[i].template cast<double>()); | ||
| 76 | } | ||
| 77 | qh.runQhull("", 3, static_cast<int>(num_points), qhull_pts[0].data(), | ||
| 78 | command); | ||
| 79 | |||
| 80 | if (qh.qhullStatus() != qh_ERRnone) { | ||
| 81 | if (qh.hasQhullMessage()) std::cerr << qh.qhullMessage() << std::endl; | ||
| 82 | COAL_THROW_PRETTY("Qhull failed", std::logic_error); | ||
| 83 | } | ||
| 84 | |||
| 85 | typedef std::size_t index_type; | ||
| 86 | typedef int size_type; | ||
| 87 | |||
| 88 | // Map index in pts to index in vertices. -1 means not used | ||
| 89 | std::vector<int> pts_to_vertices(num_points, -1); | ||
| 90 | |||
| 91 | // Initialize the vertices | ||
| 92 | size_t nvertex = static_cast<size_t>(qh.vertexCount()); | ||
| 93 | std::shared_ptr<std::vector<Vec3s>> vertices( | ||
| 94 | new std::vector<Vec3s>(size_t(nvertex))); | ||
| 95 | QhullVertexList vertexList(qh.vertexList()); | ||
| 96 | size_t i_vertex = 0; | ||
| 97 | for (QhullVertexList::const_iterator v = vertexList.begin(); | ||
| 98 | v != vertexList.end(); ++v) { | ||
| 99 | QhullPoint pt((*v).point()); | ||
| 100 | pts_to_vertices[(size_t)pt.id()] = (int)i_vertex; | ||
| 101 | (*vertices)[i_vertex] = Vec3s(Scalar(pt[0]), Scalar(pt[1]), Scalar(pt[2])); | ||
| 102 | ++i_vertex; | ||
| 103 | } | ||
| 104 | assert(i_vertex == nvertex); | ||
| 105 | |||
| 106 | Convex<Triangle>* convex_tri(NULL); | ||
| 107 | ConvexBase* convex(NULL); | ||
| 108 | if (keepTriangles) | ||
| 109 | convex = convex_tri = new Convex<Triangle>(); | ||
| 110 | else | ||
| 111 | convex = new ConvexBase; | ||
| 112 | convex->initialize(vertices, static_cast<unsigned int>(nvertex)); | ||
| 113 | |||
| 114 | // Build the neighbors | ||
| 115 | convex->neighbors.reset(new std::vector<Neighbors>(size_t(nvertex))); | ||
| 116 | std::vector<std::set<index_type>> nneighbors(static_cast<size_t>(nvertex)); | ||
| 117 | if (keepTriangles) { | ||
| 118 | convex_tri->num_polygons = static_cast<unsigned int>(qh.facetCount()); | ||
| 119 | convex_tri->polygons.reset( | ||
| 120 | new std::vector<Triangle>(convex_tri->num_polygons)); | ||
| 121 | convex_tri->computeCenter(); | ||
| 122 | } | ||
| 123 | |||
| 124 | unsigned int c_nneighbors = 0; | ||
| 125 | unsigned int i_polygon = 0; | ||
| 126 | |||
| 127 | // Compute the neighbors from the edges of the faces. | ||
| 128 | for (QhullFacet facet = qh.beginFacet(); facet != qh.endFacet(); | ||
| 129 | facet = facet.next()) { | ||
| 130 | if (facet.isSimplicial()) { | ||
| 131 | // In 3D, simplicial faces have 3 vertices. We mark them as neighbors. | ||
| 132 | QhullVertexSet f_vertices(facet.vertices()); | ||
| 133 | size_t n = static_cast<size_t>(f_vertices.count()); | ||
| 134 | assert(n == 3); | ||
| 135 | Triangle tri( | ||
| 136 | static_cast<size_t>( | ||
| 137 | pts_to_vertices[static_cast<size_t>(f_vertices[0].point().id())]), | ||
| 138 | static_cast<size_t>( | ||
| 139 | pts_to_vertices[static_cast<size_t>(f_vertices[1].point().id())]), | ||
| 140 | static_cast<size_t>(pts_to_vertices[static_cast<size_t>( | ||
| 141 | f_vertices[2].point().id())])); | ||
| 142 | if (keepTriangles) { | ||
| 143 | reorderTriangle(convex_tri, tri); | ||
| 144 | (*convex_tri->polygons)[i_polygon++] = tri; | ||
| 145 | } | ||
| 146 | for (size_t j = 0; j < n; ++j) { | ||
| 147 | size_t i = (j == 0) ? n - 1 : j - 1; | ||
| 148 | size_t k = (j == n - 1) ? 0 : j + 1; | ||
| 149 | // Update neighbors of pj; | ||
| 150 | if (nneighbors[tri[j]].insert(tri[i]).second) c_nneighbors++; | ||
| 151 | if (nneighbors[tri[j]].insert(tri[k]).second) c_nneighbors++; | ||
| 152 | } | ||
| 153 | } else { | ||
| 154 | if (keepTriangles) { // TODO I think there is a memory leak here. | ||
| 155 | COAL_THROW_PRETTY( | ||
| 156 | "You requested to keep triangles so you " | ||
| 157 | "must pass option \"Qt\" to qhull via the qhull command argument.", | ||
| 158 | std::invalid_argument); | ||
| 159 | } | ||
| 160 | // Non-simplicial faces have more than 3 vertices and contains a list of | ||
| 161 | // rigdes. Ridges are (3-1)D simplex (i.e. one edge). We mark the two | ||
| 162 | // vertices of each ridge as neighbors. | ||
| 163 | QhullRidgeSet f_ridges(facet.ridges()); | ||
| 164 | for (size_type j = 0; j < f_ridges.count(); ++j) { | ||
| 165 | assert(f_ridges[j].vertices().count() == 2); | ||
| 166 | int pi = pts_to_vertices[static_cast<size_t>( | ||
| 167 | f_ridges[j].vertices()[0].point().id())], | ||
| 168 | pj = pts_to_vertices[static_cast<size_t>( | ||
| 169 | f_ridges[j].vertices()[1].point().id())]; | ||
| 170 | // Update neighbors of pi and pj; | ||
| 171 | if (nneighbors[static_cast<size_t>(pj)] | ||
| 172 | .insert(static_cast<size_t>(pi)) | ||
| 173 | .second) | ||
| 174 | c_nneighbors++; | ||
| 175 | if (nneighbors[static_cast<size_t>(pi)] | ||
| 176 | .insert(static_cast<size_t>(pj)) | ||
| 177 | .second) | ||
| 178 | c_nneighbors++; | ||
| 179 | } | ||
| 180 | } | ||
| 181 | } | ||
| 182 | assert(!keepTriangles || static_cast<int>(i_polygon) == qh.facetCount()); | ||
| 183 | |||
| 184 | // Build the double representation (free in this case because qhull has | ||
| 185 | // alreday run) | ||
| 186 | convex->buildDoubleDescriptionFromQHullResult(qh); | ||
| 187 | |||
| 188 | // Fill the neighbor attribute of the returned object. | ||
| 189 | convex->nneighbors_.reset(new std::vector<unsigned int>(c_nneighbors)); | ||
| 190 | unsigned int* p_nneighbors = convex->nneighbors_->data(); | ||
| 191 | std::vector<Neighbors>& neighbors_ = *(convex->neighbors); | ||
| 192 | for (size_t i = 0; i < static_cast<size_t>(nvertex); ++i) { | ||
| 193 | Neighbors& n = neighbors_[i]; | ||
| 194 | if (nneighbors[i].size() >= (std::numeric_limits<unsigned char>::max)()) | ||
| 195 | COAL_THROW_PRETTY("Too many neighbors.", std::logic_error); | ||
| 196 | n.count_ = (unsigned char)nneighbors[i].size(); | ||
| 197 | n.n_ = p_nneighbors; | ||
| 198 | p_nneighbors = | ||
| 199 | std::copy(nneighbors[i].begin(), nneighbors[i].end(), p_nneighbors); | ||
| 200 | } | ||
| 201 | assert(p_nneighbors == convex->nneighbors_->data() + c_nneighbors); | ||
| 202 | |||
| 203 | // Now that the neighbors are computed, we can call the | ||
| 204 | // `buildSupportWarmStart` function. | ||
| 205 | convex->buildSupportWarmStart(); | ||
| 206 | return convex; | ||
| 207 | #else | ||
| 208 |
15/30✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 1 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 1 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 1 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 1 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 1 times.
✗ Branch 23 not taken.
✓ Branch 25 taken 1 times.
✗ Branch 26 not taken.
✓ Branch 28 taken 1 times.
✗ Branch 29 not taken.
✓ Branch 31 taken 1 times.
✗ Branch 32 not taken.
✓ Branch 34 taken 1 times.
✗ Branch 35 not taken.
✓ Branch 37 taken 1 times.
✗ Branch 38 not taken.
✓ Branch 41 taken 1 times.
✗ Branch 42 not taken.
✓ Branch 44 taken 1 times.
✗ Branch 45 not taken.
|
1 | COAL_THROW_PRETTY( |
| 209 | "Library built without qhull. Cannot build object of this type.", | ||
| 210 | std::logic_error); | ||
| 211 | COAL_UNUSED_VARIABLE(pts); | ||
| 212 | COAL_UNUSED_VARIABLE(num_points); | ||
| 213 | COAL_UNUSED_VARIABLE(keepTriangles); | ||
| 214 | COAL_UNUSED_VARIABLE(qhullCommand); | ||
| 215 | #endif | ||
| 216 | } | ||
| 217 | |||
| 218 | #ifdef COAL_HAS_QHULL | ||
| 219 | void ConvexBase::buildDoubleDescription() { | ||
| 220 | if (num_points <= 3) { | ||
| 221 | COAL_THROW_PRETTY( | ||
| 222 | "You shouldn't use this function with a convex less than" | ||
| 223 | " 4 points.", | ||
| 224 | std::invalid_argument); | ||
| 225 | } | ||
| 226 | |||
| 227 | Qhull qh; | ||
| 228 | const char* command = "Qt"; | ||
| 229 | using Vec3d = Eigen::Vector3d; | ||
| 230 | std::vector<Vec3d> qhull_pts; | ||
| 231 | qhull_pts.reserve(num_points); | ||
| 232 | for (size_t i = 0; i < num_points; ++i) { | ||
| 233 | qhull_pts.push_back((*points)[i].template cast<double>()); | ||
| 234 | } | ||
| 235 | qh.runQhull("", 3, static_cast<int>(num_points), qhull_pts[0].data(), | ||
| 236 | command); | ||
| 237 | |||
| 238 | if (qh.qhullStatus() != qh_ERRnone) { | ||
| 239 | if (qh.hasQhullMessage()) std::cerr << qh.qhullMessage() << std::endl; | ||
| 240 | COAL_THROW_PRETTY("Qhull failed", std::logic_error); | ||
| 241 | } | ||
| 242 | |||
| 243 | buildDoubleDescriptionFromQHullResult(qh); | ||
| 244 | } | ||
| 245 | |||
| 246 | void ConvexBase::buildDoubleDescriptionFromQHullResult(const Qhull& qh) { | ||
| 247 | num_normals_and_offsets = static_cast<unsigned int>(qh.facetCount()); | ||
| 248 | normals.reset(new std::vector<Vec3s>(num_normals_and_offsets)); | ||
| 249 | std::vector<Vec3s>& normals_ = *normals; | ||
| 250 | offsets.reset(new std::vector<Scalar>(num_normals_and_offsets)); | ||
| 251 | std::vector<Scalar>& offsets_ = *offsets; | ||
| 252 | unsigned int i_normal = 0; | ||
| 253 | for (QhullFacet facet = qh.beginFacet(); facet != qh.endFacet(); | ||
| 254 | facet = facet.next()) { | ||
| 255 | const orgQhull::QhullHyperplane& plane = facet.hyperplane(); | ||
| 256 | normals_[i_normal] = | ||
| 257 | Vec3s(Scalar(plane.coordinates()[0]), Scalar(plane.coordinates()[1]), | ||
| 258 | Scalar(plane.coordinates()[2])); | ||
| 259 | offsets_[i_normal] = Scalar(plane.offset()); | ||
| 260 | i_normal++; | ||
| 261 | } | ||
| 262 | assert(static_cast<int>(i_normal) == qh.facetCount()); | ||
| 263 | } | ||
| 264 | #endif | ||
| 265 | |||
| 266 | } // namespace coal | ||
| 267 |