| Line | Branch | Exec | Source |
|---|---|---|---|
| 1 | /* | ||
| 2 | * Software License Agreement (BSD License) | ||
| 3 | * | ||
| 4 | * Copyright (c) 2011-2014, Willow Garage, Inc. | ||
| 5 | * Copyright (c) 2014-2015, Open Source Robotics Foundation | ||
| 6 | * All rights reserved. | ||
| 7 | * | ||
| 8 | * Redistribution and use in source and binary forms, with or without | ||
| 9 | * modification, are permitted provided that the following conditions | ||
| 10 | * are met: | ||
| 11 | * | ||
| 12 | * * Redistributions of source code must retain the above copyright | ||
| 13 | * notice, this list of conditions and the following disclaimer. | ||
| 14 | * * Redistributions in binary form must reproduce the above | ||
| 15 | * copyright notice, this list of conditions and the following | ||
| 16 | * disclaimer in the documentation and/or other materials provided | ||
| 17 | * with the distribution. | ||
| 18 | * * Neither the name of Open Source Robotics Foundation nor the names of its | ||
| 19 | * contributors may be used to endorse or promote products derived | ||
| 20 | * from this software without specific prior written permission. | ||
| 21 | * | ||
| 22 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS | ||
| 23 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT | ||
| 24 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS | ||
| 25 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE | ||
| 26 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, | ||
| 27 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, | ||
| 28 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; | ||
| 29 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER | ||
| 30 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT | ||
| 31 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN | ||
| 32 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE | ||
| 33 | * POSSIBILITY OF SUCH DAMAGE. | ||
| 34 | */ | ||
| 35 | |||
| 36 | /** \author Jia Pan */ | ||
| 37 | |||
| 38 | #include "coal/shape/geometric_shapes.h" | ||
| 39 | #include "coal/shape/geometric_shapes_utility.h" | ||
| 40 | |||
| 41 | #ifdef COAL_HAS_QHULL | ||
| 42 | #include <libqhullcpp/QhullError.h> | ||
| 43 | #include <libqhullcpp/QhullFacet.h> | ||
| 44 | #include <libqhullcpp/QhullLinkedList.h> | ||
| 45 | #include <libqhullcpp/QhullVertex.h> | ||
| 46 | #include <libqhullcpp/QhullVertexSet.h> | ||
| 47 | #include <libqhullcpp/QhullRidge.h> | ||
| 48 | #include <libqhullcpp/Qhull.h> | ||
| 49 | |||
| 50 | using orgQhull::Qhull; | ||
| 51 | using orgQhull::QhullFacet; | ||
| 52 | using orgQhull::QhullPoint; | ||
| 53 | using orgQhull::QhullRidgeSet; | ||
| 54 | using orgQhull::QhullVertexList; | ||
| 55 | using orgQhull::QhullVertexSet; | ||
| 56 | #endif | ||
| 57 | |||
| 58 | namespace coal { | ||
| 59 | |||
| 60 | template <typename IndexType> | ||
| 61 | ✗ | ConvexBaseTpl<IndexType>* ConvexBaseTpl<IndexType>::convexHull( | |
| 62 | std::shared_ptr<std::vector<Vec3s>>& pts, unsigned int num_points, | ||
| 63 | bool keepTriangles, const char* qhullCommand) { | ||
| 64 | COAL_COMPILER_DIAGNOSTIC_PUSH | ||
| 65 | COAL_COMPILER_DIAGNOSTIC_IGNORED_DEPRECECATED_DECLARATIONS | ||
| 66 | ✗ | return ConvexBaseTpl<IndexType>::convexHull(pts->data(), num_points, | |
| 67 | ✗ | keepTriangles, qhullCommand); | |
| 68 | COAL_COMPILER_DIAGNOSTIC_POP | ||
| 69 | } | ||
| 70 | template COAL_DLLAPI ConvexBaseTpl<Triangle16::IndexType>* | ||
| 71 | ConvexBaseTpl<Triangle16::IndexType>::convexHull( | ||
| 72 | std::shared_ptr<std::vector<Vec3s>>& pts, unsigned int num_points, | ||
| 73 | bool keepTriangles, const char* qhullCommand); | ||
| 74 | template COAL_DLLAPI ConvexBaseTpl<Triangle32::IndexType>* | ||
| 75 | ConvexBaseTpl<Triangle32::IndexType>::convexHull( | ||
| 76 | std::shared_ptr<std::vector<Vec3s>>& pts, unsigned int num_points, | ||
| 77 | bool keepTriangles, const char* qhullCommand); | ||
| 78 | |||
| 79 | template <typename IndexType> | ||
| 80 | 2 | ConvexBaseTpl<IndexType>* ConvexBaseTpl<IndexType>::convexHull( | |
| 81 | const Vec3s* pts, unsigned int num_points, bool keepTriangles, | ||
| 82 | const char* qhullCommand) { | ||
| 83 | #ifdef COAL_HAS_QHULL | ||
| 84 | if (num_points <= 3) { | ||
| 85 | COAL_THROW_PRETTY( | ||
| 86 | "You shouldn't use this function with less than" | ||
| 87 | " 4 points.", | ||
| 88 | std::invalid_argument); | ||
| 89 | } | ||
| 90 | assert(pts[0].data() + 3 == pts[1].data()); | ||
| 91 | |||
| 92 | Qhull qh; | ||
| 93 | const char* command = | ||
| 94 | qhullCommand ? qhullCommand : (keepTriangles ? "Qt" : ""); | ||
| 95 | |||
| 96 | // TODO: add a ifdef not double precision here | ||
| 97 | using Vec3d = Eigen::Vector3d; | ||
| 98 | std::vector<Vec3d> qhull_pts; | ||
| 99 | qhull_pts.reserve(num_points); | ||
| 100 | for (size_t i = 0; i < num_points; ++i) { | ||
| 101 | qhull_pts.push_back(pts[i].template cast<double>()); | ||
| 102 | } | ||
| 103 | qh.runQhull("", 3, static_cast<int>(num_points), qhull_pts[0].data(), | ||
| 104 | command); | ||
| 105 | |||
| 106 | if (qh.qhullStatus() != qh_ERRnone) { | ||
| 107 | if (qh.hasQhullMessage()) std::cerr << qh.qhullMessage() << std::endl; | ||
| 108 | COAL_THROW_PRETTY("Qhull failed", std::logic_error); | ||
| 109 | } | ||
| 110 | |||
| 111 | typedef int size_type; | ||
| 112 | |||
| 113 | // Map index in pts to index in vertices. -1 means not used | ||
| 114 | std::vector<int> pts_to_vertices(num_points, -1); | ||
| 115 | |||
| 116 | // Initialize the vertices | ||
| 117 | size_t nvertex = static_cast<size_t>(qh.vertexCount()); | ||
| 118 | if (nvertex >= size_t(std::numeric_limits<IndexType>::max())) { | ||
| 119 | COAL_THROW_PRETTY("nvertex >= std::numeric_limits<IndexType>::max()", | ||
| 120 | std::runtime_error); | ||
| 121 | } | ||
| 122 | std::shared_ptr<std::vector<Vec3s>> vertices( | ||
| 123 | new std::vector<Vec3s>(size_t(nvertex))); | ||
| 124 | QhullVertexList vertexList(qh.vertexList()); | ||
| 125 | size_t i_vertex = 0; | ||
| 126 | for (QhullVertexList::const_iterator v = vertexList.begin(); | ||
| 127 | v != vertexList.end(); ++v) { | ||
| 128 | QhullPoint pt((*v).point()); | ||
| 129 | pts_to_vertices[(size_t)pt.id()] = (int)i_vertex; | ||
| 130 | (*vertices)[i_vertex] = Vec3s(Scalar(pt[0]), Scalar(pt[1]), Scalar(pt[2])); | ||
| 131 | ++i_vertex; | ||
| 132 | } | ||
| 133 | assert(i_vertex == nvertex); | ||
| 134 | |||
| 135 | ConvexTpl<TriangleTpl<IndexType>>* convex_tri(NULL); | ||
| 136 | ConvexBaseTpl<IndexType>* convex(NULL); | ||
| 137 | if (keepTriangles) | ||
| 138 | convex = convex_tri = new ConvexTpl<TriangleTpl<IndexType>>(); | ||
| 139 | else | ||
| 140 | convex = new ConvexBaseTpl<IndexType>; | ||
| 141 | convex->initialize(vertices, static_cast<unsigned int>(nvertex)); | ||
| 142 | |||
| 143 | // Build the neighbors | ||
| 144 | convex->neighbors.reset(new std::vector<Neighbors>(size_t(nvertex))); | ||
| 145 | std::vector<std::set<IndexType>> nneighbors(static_cast<size_t>(nvertex)); | ||
| 146 | if (keepTriangles) { | ||
| 147 | convex_tri->num_polygons = static_cast<unsigned int>(qh.facetCount()); | ||
| 148 | convex_tri->polygons.reset( | ||
| 149 | new std::vector<TriangleTpl<IndexType>>(convex_tri->num_polygons)); | ||
| 150 | convex_tri->computeCenter(); | ||
| 151 | } | ||
| 152 | |||
| 153 | unsigned int c_nneighbors = 0; | ||
| 154 | unsigned int i_polygon = 0; | ||
| 155 | |||
| 156 | // TODO: make sure number of vertices < size of IndexType | ||
| 157 | |||
| 158 | // Compute the neighbors from the edges of the faces. | ||
| 159 | for (QhullFacet facet = qh.beginFacet(); facet != qh.endFacet(); | ||
| 160 | facet = facet.next()) { | ||
| 161 | if (facet.isSimplicial()) { | ||
| 162 | // In 3D, simplicial faces have 3 vertices. We mark them as neighbors. | ||
| 163 | QhullVertexSet f_vertices(facet.vertices()); | ||
| 164 | IndexType n = static_cast<IndexType>(f_vertices.count()); | ||
| 165 | assert(n == 3); | ||
| 166 | TriangleTpl<IndexType> tri( | ||
| 167 | static_cast<IndexType>( | ||
| 168 | pts_to_vertices[static_cast<size_t>(f_vertices[0].point().id())]), | ||
| 169 | static_cast<IndexType>( | ||
| 170 | pts_to_vertices[static_cast<size_t>(f_vertices[1].point().id())]), | ||
| 171 | static_cast<IndexType>(pts_to_vertices[static_cast<size_t>( | ||
| 172 | f_vertices[2].point().id())])); | ||
| 173 | if (keepTriangles) { | ||
| 174 | reorderTriangle(convex_tri, tri); | ||
| 175 | (*convex_tri->polygons)[i_polygon++] = tri; | ||
| 176 | } | ||
| 177 | for (IndexType j = 0; j < static_cast<IndexType>(n); ++j) { | ||
| 178 | IndexType i = (j == 0) ? n - 1 : j - 1; | ||
| 179 | IndexType k = (j == n - 1) ? 0 : j + 1; | ||
| 180 | // Update neighbors of pj; | ||
| 181 | if (nneighbors[tri[j]].insert(tri[i]).second) c_nneighbors++; | ||
| 182 | if (nneighbors[tri[j]].insert(tri[k]).second) c_nneighbors++; | ||
| 183 | } | ||
| 184 | } else { | ||
| 185 | if (keepTriangles) { // TODO I think there is a memory leak here. | ||
| 186 | COAL_THROW_PRETTY( | ||
| 187 | "You requested to keep triangles so you " | ||
| 188 | "must pass option \"Qt\" to qhull via the qhull command argument.", | ||
| 189 | std::invalid_argument); | ||
| 190 | } | ||
| 191 | // Non-simplicial faces have more than 3 vertices and contains a list of | ||
| 192 | // rigdes. Ridges are (3-1)D simplex (i.e. one edge). We mark the two | ||
| 193 | // vertices of each ridge as neighbors. | ||
| 194 | QhullRidgeSet f_ridges(facet.ridges()); | ||
| 195 | for (size_type j = 0; j < f_ridges.count(); ++j) { | ||
| 196 | assert(f_ridges[j].vertices().count() == 2); | ||
| 197 | int pi = pts_to_vertices[static_cast<size_t>( | ||
| 198 | f_ridges[j].vertices()[0].point().id())], | ||
| 199 | pj = pts_to_vertices[static_cast<size_t>( | ||
| 200 | f_ridges[j].vertices()[1].point().id())]; | ||
| 201 | // Update neighbors of pi and pj; | ||
| 202 | if (nneighbors[static_cast<size_t>(pj)] | ||
| 203 | .insert(static_cast<IndexType>(pi)) | ||
| 204 | .second) | ||
| 205 | c_nneighbors++; | ||
| 206 | if (nneighbors[static_cast<size_t>(pi)] | ||
| 207 | .insert(static_cast<IndexType>(pj)) | ||
| 208 | .second) | ||
| 209 | c_nneighbors++; | ||
| 210 | } | ||
| 211 | } | ||
| 212 | } | ||
| 213 | assert(!keepTriangles || static_cast<int>(i_polygon) == qh.facetCount()); | ||
| 214 | |||
| 215 | // Build the double representation (free in this case because qhull has | ||
| 216 | // alreday run) | ||
| 217 | convex->buildDoubleDescriptionFromQHullResult(qh); | ||
| 218 | |||
| 219 | // Fill the neighbor attribute of the returned object. | ||
| 220 | convex->nneighbors_.reset(new std::vector<IndexType>(c_nneighbors)); | ||
| 221 | std::vector<Neighbors>& neighbors_ = *(convex->neighbors); | ||
| 222 | std::vector<IndexType>& nneighbors_ = *(convex->nneighbors_); | ||
| 223 | IndexType begin_id = 0; | ||
| 224 | for (size_t i = 0; i < static_cast<size_t>(nvertex); ++i) { | ||
| 225 | Neighbors& n = neighbors_[i]; | ||
| 226 | if (nneighbors[i].size() >= (std::numeric_limits<unsigned char>::max)()) | ||
| 227 | COAL_THROW_PRETTY("Too many neighbors.", std::logic_error); | ||
| 228 | n.count = (unsigned char)nneighbors[i].size(); | ||
| 229 | n.begin_id = begin_id; | ||
| 230 | IndexType j = 0; | ||
| 231 | for (IndexType idx : nneighbors[i]) { | ||
| 232 | nneighbors_[n.begin_id + j] = idx; | ||
| 233 | j++; | ||
| 234 | } | ||
| 235 | begin_id += n.count; | ||
| 236 | } | ||
| 237 | |||
| 238 | // Now that the neighbors are computed, we can call the | ||
| 239 | // `buildSupportWarmStart` function. | ||
| 240 | convex->buildSupportWarmStart(); | ||
| 241 | return convex; | ||
| 242 | #else | ||
| 243 |
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.
|
2 | COAL_THROW_PRETTY( |
| 244 | "Library built without qhull. Cannot build object of this type.", | ||
| 245 | std::logic_error); | ||
| 246 | COAL_UNUSED_VARIABLE(pts); | ||
| 247 | COAL_UNUSED_VARIABLE(num_points); | ||
| 248 | COAL_UNUSED_VARIABLE(keepTriangles); | ||
| 249 | COAL_UNUSED_VARIABLE(qhullCommand); | ||
| 250 | #endif | ||
| 251 | } | ||
| 252 | template COAL_DLLAPI ConvexBaseTpl<Triangle16::IndexType>* | ||
| 253 | ConvexBaseTpl<Triangle16::IndexType>::convexHull(const Vec3s* pts, | ||
| 254 | unsigned int num_points, | ||
| 255 | bool keepTriangles, | ||
| 256 | const char* qhullCommand); | ||
| 257 | template COAL_DLLAPI ConvexBaseTpl<Triangle32::IndexType>* | ||
| 258 | ConvexBaseTpl<Triangle32::IndexType>::convexHull(const Vec3s* pts, | ||
| 259 | unsigned int num_points, | ||
| 260 | bool keepTriangles, | ||
| 261 | const char* qhullCommand); | ||
| 262 | |||
| 263 | #ifdef COAL_HAS_QHULL | ||
| 264 | template <typename IndexType> | ||
| 265 | void ConvexBaseTpl<IndexType>::buildDoubleDescription() { | ||
| 266 | if (num_points <= 3) { | ||
| 267 | COAL_THROW_PRETTY( | ||
| 268 | "You shouldn't use this function with a convex less than" | ||
| 269 | " 4 points.", | ||
| 270 | std::invalid_argument); | ||
| 271 | } | ||
| 272 | |||
| 273 | Qhull qh; | ||
| 274 | const char* command = "Qt"; | ||
| 275 | using Vec3d = Eigen::Vector3d; | ||
| 276 | std::vector<Vec3d> qhull_pts; | ||
| 277 | qhull_pts.reserve(num_points); | ||
| 278 | for (size_t i = 0; i < num_points; ++i) { | ||
| 279 | qhull_pts.push_back((*points)[i].template cast<double>()); | ||
| 280 | } | ||
| 281 | qh.runQhull("", 3, static_cast<int>(num_points), qhull_pts[0].data(), | ||
| 282 | command); | ||
| 283 | |||
| 284 | if (qh.qhullStatus() != qh_ERRnone) { | ||
| 285 | if (qh.hasQhullMessage()) std::cerr << qh.qhullMessage() << std::endl; | ||
| 286 | COAL_THROW_PRETTY("Qhull failed", std::logic_error); | ||
| 287 | } | ||
| 288 | |||
| 289 | buildDoubleDescriptionFromQHullResult(qh); | ||
| 290 | } | ||
| 291 | template void COAL_DLLAPI | ||
| 292 | ConvexBaseTpl<Triangle16::IndexType>::buildDoubleDescription(); | ||
| 293 | template void COAL_DLLAPI | ||
| 294 | ConvexBaseTpl<Triangle32::IndexType>::buildDoubleDescription(); | ||
| 295 | |||
| 296 | template <typename IndexType> | ||
| 297 | void ConvexBaseTpl<IndexType>::buildDoubleDescriptionFromQHullResult( | ||
| 298 | const Qhull& qh) { | ||
| 299 | num_normals_and_offsets = static_cast<unsigned int>(qh.facetCount()); | ||
| 300 | normals.reset(new std::vector<Vec3s>(num_normals_and_offsets)); | ||
| 301 | std::vector<Vec3s>& normals_ = *normals; | ||
| 302 | offsets.reset(new std::vector<Scalar>(num_normals_and_offsets)); | ||
| 303 | std::vector<Scalar>& offsets_ = *offsets; | ||
| 304 | unsigned int i_normal = 0; | ||
| 305 | for (QhullFacet facet = qh.beginFacet(); facet != qh.endFacet(); | ||
| 306 | facet = facet.next()) { | ||
| 307 | const orgQhull::QhullHyperplane& plane = facet.hyperplane(); | ||
| 308 | normals_[i_normal] = | ||
| 309 | Vec3s(Scalar(plane.coordinates()[0]), Scalar(plane.coordinates()[1]), | ||
| 310 | Scalar(plane.coordinates()[2])); | ||
| 311 | offsets_[i_normal] = Scalar(plane.offset()); | ||
| 312 | i_normal++; | ||
| 313 | } | ||
| 314 | assert(static_cast<int>(i_normal) == qh.facetCount()); | ||
| 315 | } | ||
| 316 | template void COAL_DLLAPI | ||
| 317 | ConvexBaseTpl<Triangle16::IndexType>::buildDoubleDescriptionFromQHullResult( | ||
| 318 | const Qhull& qh); | ||
| 319 | template void COAL_DLLAPI | ||
| 320 | ConvexBaseTpl<Triangle32::IndexType>::buildDoubleDescriptionFromQHullResult( | ||
| 321 | const Qhull& qh); | ||
| 322 | #endif | ||
| 323 | |||
| 324 | 17640 | void Halfspace::unitNormalTest() { | |
| 325 | 17640 | Scalar l = n.norm(); | |
| 326 |
2/2✓ Branch 0 taken 17638 times.
✓ Branch 1 taken 2 times.
|
17640 | if (l > 0) { |
| 327 | 17638 | Scalar inv_l = Scalar(1) / l; | |
| 328 |
1/2✓ Branch 1 taken 17638 times.
✗ Branch 2 not taken.
|
17638 | n *= inv_l; |
| 329 | 17638 | d *= inv_l; | |
| 330 | } else { | ||
| 331 |
3/6✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 2 times.
✗ Branch 8 not taken.
|
2 | n << 1, 0, 0; |
| 332 | 2 | d = 0; | |
| 333 | } | ||
| 334 | 17640 | } | |
| 335 | |||
| 336 | 255 | void Plane::unitNormalTest() { | |
| 337 | 255 | Scalar l = n.norm(); | |
| 338 |
2/2✓ Branch 0 taken 253 times.
✓ Branch 1 taken 2 times.
|
255 | if (l > 0) { |
| 339 | 253 | Scalar inv_l = Scalar(1) / l; | |
| 340 |
1/2✓ Branch 1 taken 253 times.
✗ Branch 2 not taken.
|
253 | n *= inv_l; |
| 341 | 253 | d *= inv_l; | |
| 342 | } else { | ||
| 343 |
3/6✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 2 times.
✗ Branch 8 not taken.
|
2 | n << 1, 0, 0; |
| 344 | 2 | d = 0; | |
| 345 | } | ||
| 346 | 255 | } | |
| 347 | |||
| 348 | 14112 | void Box::computeLocalAABB() { | |
| 349 |
2/4✓ Branch 1 taken 14112 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 14112 times.
✗ Branch 5 not taken.
|
14112 | computeBV<AABB>(*this, Transform3s(), aabb_local); |
| 350 | 14112 | const Scalar ssr = this->getSweptSphereRadius(); | |
| 351 |
2/2✓ Branch 0 taken 1 times.
✓ Branch 1 taken 14111 times.
|
14112 | if (ssr > 0) { |
| 352 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
1 | aabb_local.min_ -= Vec3s::Constant(ssr); |
| 353 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
1 | aabb_local.max_ += Vec3s::Constant(ssr); |
| 354 | } | ||
| 355 |
1/2✓ Branch 1 taken 14112 times.
✗ Branch 2 not taken.
|
14112 | aabb_center = aabb_local.center(); |
| 356 |
2/4✓ Branch 1 taken 14112 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 14112 times.
✗ Branch 5 not taken.
|
14112 | aabb_radius = (aabb_local.min_ - aabb_center).norm(); |
| 357 | 14112 | } | |
| 358 | |||
| 359 | 16918 | void Sphere::computeLocalAABB() { | |
| 360 |
2/4✓ Branch 1 taken 16918 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 16918 times.
✗ Branch 5 not taken.
|
16918 | computeBV<AABB>(*this, Transform3s(), aabb_local); |
| 361 | 16918 | const Scalar ssr = this->getSweptSphereRadius(); | |
| 362 |
2/2✓ Branch 0 taken 1 times.
✓ Branch 1 taken 16917 times.
|
16918 | if (ssr > 0) { |
| 363 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
1 | aabb_local.min_ -= Vec3s::Constant(ssr); |
| 364 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
1 | aabb_local.max_ += Vec3s::Constant(ssr); |
| 365 | } | ||
| 366 |
1/2✓ Branch 1 taken 16918 times.
✗ Branch 2 not taken.
|
16918 | aabb_center = aabb_local.center(); |
| 367 | 16918 | aabb_radius = radius; | |
| 368 | 16918 | } | |
| 369 | |||
| 370 | 1 | void Ellipsoid::computeLocalAABB() { | |
| 371 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
1 | computeBV<AABB>(*this, Transform3s(), aabb_local); |
| 372 | 1 | const Scalar ssr = this->getSweptSphereRadius(); | |
| 373 |
1/2✓ Branch 0 taken 1 times.
✗ Branch 1 not taken.
|
1 | if (ssr > 0) { |
| 374 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
1 | aabb_local.min_ -= Vec3s::Constant(ssr); |
| 375 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
1 | aabb_local.max_ += Vec3s::Constant(ssr); |
| 376 | } | ||
| 377 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | aabb_center = aabb_local.center(); |
| 378 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
1 | aabb_radius = (aabb_local.min_ - aabb_center).norm(); |
| 379 | 1 | } | |
| 380 | |||
| 381 | 10011 | void Capsule::computeLocalAABB() { | |
| 382 |
2/4✓ Branch 1 taken 10011 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 10011 times.
✗ Branch 5 not taken.
|
10011 | computeBV<AABB>(*this, Transform3s(), aabb_local); |
| 383 | 10011 | const Scalar ssr = this->getSweptSphereRadius(); | |
| 384 |
2/2✓ Branch 0 taken 1 times.
✓ Branch 1 taken 10010 times.
|
10011 | if (ssr > 0) { |
| 385 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
1 | aabb_local.min_ -= Vec3s::Constant(ssr); |
| 386 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
1 | aabb_local.max_ += Vec3s::Constant(ssr); |
| 387 | } | ||
| 388 |
1/2✓ Branch 1 taken 10011 times.
✗ Branch 2 not taken.
|
10011 | aabb_center = aabb_local.center(); |
| 389 |
2/4✓ Branch 1 taken 10011 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 10011 times.
✗ Branch 5 not taken.
|
10011 | aabb_radius = (aabb_local.min_ - aabb_center).norm(); |
| 390 | 10011 | } | |
| 391 | |||
| 392 | 1 | void Cone::computeLocalAABB() { | |
| 393 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
1 | computeBV<AABB>(*this, Transform3s(), aabb_local); |
| 394 | 1 | const Scalar ssr = this->getSweptSphereRadius(); | |
| 395 |
1/2✓ Branch 0 taken 1 times.
✗ Branch 1 not taken.
|
1 | if (ssr > 0) { |
| 396 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
1 | aabb_local.min_ -= Vec3s::Constant(ssr); |
| 397 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
1 | aabb_local.max_ += Vec3s::Constant(ssr); |
| 398 | } | ||
| 399 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | aabb_center = aabb_local.center(); |
| 400 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
1 | aabb_radius = (aabb_local.min_ - aabb_center).norm(); |
| 401 | 1 | } | |
| 402 | |||
| 403 | 8913 | void Cylinder::computeLocalAABB() { | |
| 404 |
2/4✓ Branch 1 taken 8913 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 8913 times.
✗ Branch 5 not taken.
|
8913 | computeBV<AABB>(*this, Transform3s(), aabb_local); |
| 405 | 8913 | const Scalar ssr = this->getSweptSphereRadius(); | |
| 406 |
2/2✓ Branch 0 taken 1 times.
✓ Branch 1 taken 8912 times.
|
8913 | if (ssr > 0) { |
| 407 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
1 | aabb_local.min_ -= Vec3s::Constant(ssr); |
| 408 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
1 | aabb_local.max_ += Vec3s::Constant(ssr); |
| 409 | } | ||
| 410 |
1/2✓ Branch 1 taken 8913 times.
✗ Branch 2 not taken.
|
8913 | aabb_center = aabb_local.center(); |
| 411 |
2/4✓ Branch 1 taken 8913 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 8913 times.
✗ Branch 5 not taken.
|
8913 | aabb_radius = (aabb_local.min_ - aabb_center).norm(); |
| 412 | 8913 | } | |
| 413 | |||
| 414 | 1 | void Halfspace::computeLocalAABB() { | |
| 415 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
1 | computeBV<AABB>(*this, Transform3s(), aabb_local); |
| 416 | 1 | const Scalar ssr = this->getSweptSphereRadius(); | |
| 417 |
1/2✓ Branch 0 taken 1 times.
✗ Branch 1 not taken.
|
1 | if (ssr > 0) { |
| 418 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
1 | aabb_local.min_ -= Vec3s::Constant(ssr); |
| 419 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
1 | aabb_local.max_ += Vec3s::Constant(ssr); |
| 420 | } | ||
| 421 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | aabb_center = aabb_local.center(); |
| 422 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
1 | aabb_radius = (aabb_local.min_ - aabb_center).norm(); |
| 423 | 1 | } | |
| 424 | |||
| 425 | 1 | void Plane::computeLocalAABB() { | |
| 426 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
1 | computeBV<AABB>(*this, Transform3s(), aabb_local); |
| 427 | 1 | const Scalar ssr = this->getSweptSphereRadius(); | |
| 428 |
1/2✓ Branch 0 taken 1 times.
✗ Branch 1 not taken.
|
1 | if (ssr > 0) { |
| 429 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
1 | aabb_local.min_ -= Vec3s::Constant(ssr); |
| 430 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
1 | aabb_local.max_ += Vec3s::Constant(ssr); |
| 431 | } | ||
| 432 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | aabb_center = aabb_local.center(); |
| 433 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
1 | aabb_radius = (aabb_local.min_ - aabb_center).norm(); |
| 434 | 1 | } | |
| 435 | |||
| 436 | 1 | void TriangleP::computeLocalAABB() { | |
| 437 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
1 | computeBV<AABB>(*this, Transform3s(), aabb_local); |
| 438 | 1 | const Scalar ssr = this->getSweptSphereRadius(); | |
| 439 |
1/2✓ Branch 0 taken 1 times.
✗ Branch 1 not taken.
|
1 | if (ssr > 0) { |
| 440 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
1 | aabb_local.min_ -= Vec3s::Constant(ssr); |
| 441 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
1 | aabb_local.max_ += Vec3s::Constant(ssr); |
| 442 | } | ||
| 443 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | aabb_center = aabb_local.center(); |
| 444 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
1 | aabb_radius = (aabb_local.min_ - aabb_center).norm(); |
| 445 | 1 | } | |
| 446 | |||
| 447 | } // namespace coal | ||
| 448 |