| Directory: | ./ |
|---|---|
| File: | src/narrowphase/minkowski_difference.cpp |
| Date: | 2025-05-02 10:16:21 |
| Exec | Total | Coverage | |
|---|---|---|---|
| Lines: | 124 | 163 | 76.1% |
| Branches: | 66 | 202 | 32.7% |
| 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 | * Copyright (c) 2021-2024, INRIA | ||
| 7 | * All rights reserved. | ||
| 8 | * | ||
| 9 | * Redistribution and use in source and binary forms, with or without | ||
| 10 | * modification, are permitted provided that the following conditions | ||
| 11 | * are met: | ||
| 12 | * | ||
| 13 | * * Redistributions of source code must retain the above copyright | ||
| 14 | * notice, this list of conditions and the following disclaimer. | ||
| 15 | * * Redistributions in binary form must reproduce the above | ||
| 16 | * copyright notice, this list of conditions and the following | ||
| 17 | * disclaimer in the documentation and/or other materials provided | ||
| 18 | * with the distribution. | ||
| 19 | * * Neither the name of Open Source Robotics Foundation nor the names of its | ||
| 20 | * contributors may be used to endorse or promote products derived | ||
| 21 | * from this software without specific prior written permission. | ||
| 22 | * | ||
| 23 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS | ||
| 24 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT | ||
| 25 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS | ||
| 26 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE | ||
| 27 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, | ||
| 28 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, | ||
| 29 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; | ||
| 30 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER | ||
| 31 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT | ||
| 32 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN | ||
| 33 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE | ||
| 34 | * POSSIBILITY OF SUCH DAMAGE. | ||
| 35 | */ | ||
| 36 | |||
| 37 | /** \authors Jia Pan, Florent Lamiraux, Josef Mirabel, Louis Montaut */ | ||
| 38 | |||
| 39 | #include "coal/narrowphase/minkowski_difference.h" | ||
| 40 | #include "coal/shape/geometric_shapes_traits.h" | ||
| 41 | |||
| 42 | namespace coal { | ||
| 43 | namespace details { | ||
| 44 | |||
| 45 | // ============================================================================ | ||
| 46 | template <typename Shape0, typename Shape1, bool TransformIsIdentity, | ||
| 47 | int _SupportOptions> | ||
| 48 | 144902734 | void getSupportTpl(const Shape0* s0, const Shape1* s1, const Matrix3s& oR1, | |
| 49 | const Vec3s& ot1, const Vec3s& dir, Vec3s& support0, | ||
| 50 | Vec3s& support1, support_func_guess_t& hint, | ||
| 51 | ShapeSupportData data[2]) { | ||
| 52 |
1/2✗ Branch 2 not taken.
✓ Branch 3 taken 72451367 times.
|
144902734 | assert(dir.norm() > Eigen::NumTraits<Scalar>::epsilon()); |
| 53 | 144902734 | getShapeSupport<_SupportOptions>(s0, dir, support0, hint[0], data[0]); | |
| 54 | |||
| 55 | if (TransformIsIdentity) { | ||
| 56 |
2/4✓ Branch 3 taken 68068880 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 68068880 times.
✗ Branch 7 not taken.
|
136137760 | getShapeSupport<_SupportOptions>(s1, -dir, support1, hint[1], data[1]); |
| 57 | } else { | ||
| 58 |
4/8✓ Branch 2 taken 4382487 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 4382487 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 4382487 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 4382487 times.
✗ Branch 12 not taken.
|
8764974 | getShapeSupport<_SupportOptions>(s1, -oR1.transpose() * dir, support1, |
| 59 | 8764974 | hint[1], data[1]); | |
| 60 |
2/4✓ Branch 2 taken 4382487 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 4382487 times.
✗ Branch 6 not taken.
|
8764974 | support1 = oR1 * support1 + ot1; |
| 61 | } | ||
| 62 | } | ||
| 63 | |||
| 64 | // ============================================================================ | ||
| 65 | template <typename Shape0, typename Shape1, bool TransformIsIdentity, | ||
| 66 | int _SupportOptions> | ||
| 67 | 144902734 | void getSupportFuncTpl(const MinkowskiDiff& md, const Vec3s& dir, | |
| 68 | Vec3s& support0, Vec3s& support1, | ||
| 69 | support_func_guess_t& hint, ShapeSupportData data[2]) { | ||
| 70 | 144902734 | getSupportTpl<Shape0, Shape1, TransformIsIdentity, _SupportOptions>( | |
| 71 | 144902734 | static_cast<const Shape0*>(md.shapes[0]), | |
| 72 | 144902734 | static_cast<const Shape1*>(md.shapes[1]), md.oR1, md.ot1, dir, support0, | |
| 73 | support1, hint, data); | ||
| 74 | } | ||
| 75 | |||
| 76 | // ============================================================================ | ||
| 77 | template <typename Shape0, int _SupportOptions> | ||
| 78 | 60066296 | MinkowskiDiff::GetSupportFunction makeGetSupportFunction1( | |
| 79 | const ShapeBase* s1, bool identity, | ||
| 80 | Eigen::Array<Scalar, 1, 2>& swept_sphere_radius, ShapeSupportData data[2]) { | ||
| 81 | if (_SupportOptions == SupportOptions::WithSweptSphere) { | ||
| 82 | // No need to store the information of swept sphere radius | ||
| 83 | 3200 | swept_sphere_radius[1] = 0; | |
| 84 | } else { | ||
| 85 | // We store the information of swept sphere radius. | ||
| 86 | // GJK and EPA will use this information to correct the solution they find. | ||
| 87 | 60063096 | swept_sphere_radius[1] = s1->getSweptSphereRadius(); | |
| 88 | } | ||
| 89 | |||
| 90 |
8/10✓ Branch 1 taken 29208521 times.
✓ Branch 2 taken 569321 times.
✓ Branch 3 taken 61311 times.
✓ Branch 4 taken 4385 times.
✓ Branch 5 taken 8483 times.
✓ Branch 6 taken 2336 times.
✓ Branch 7 taken 160394 times.
✗ Branch 8 not taken.
✓ Branch 9 taken 18397 times.
✗ Branch 10 not taken.
|
60066296 | switch (s1->getNodeType()) { |
| 91 | 58417042 | case GEOM_TRIANGLE: | |
| 92 |
2/2✓ Branch 0 taken 29208515 times.
✓ Branch 1 taken 6 times.
|
58417042 | if (identity) |
| 93 | 58417030 | return getSupportFuncTpl<Shape0, TriangleP, true, _SupportOptions>; | |
| 94 | else | ||
| 95 | 12 | return getSupportFuncTpl<Shape0, TriangleP, false, _SupportOptions>; | |
| 96 | 1138642 | case GEOM_BOX: | |
| 97 |
2/2✓ Branch 0 taken 25 times.
✓ Branch 1 taken 569296 times.
|
1138642 | if (identity) |
| 98 | 50 | return getSupportFuncTpl<Shape0, Box, true, _SupportOptions>; | |
| 99 | else | ||
| 100 | 1138592 | return getSupportFuncTpl<Shape0, Box, false, _SupportOptions>; | |
| 101 | 122622 | case GEOM_SPHERE: | |
| 102 | if (_SupportOptions == SupportOptions::NoSweptSphere) { | ||
| 103 | // Sphere can be considered a swept-sphere point. | ||
| 104 | 122622 | swept_sphere_radius[1] += static_cast<const Sphere*>(s1)->radius; | |
| 105 | } | ||
| 106 |
1/2✗ Branch 0 not taken.
✓ Branch 1 taken 61311 times.
|
122622 | if (identity) |
| 107 | ✗ | return getSupportFuncTpl<Shape0, Sphere, true, _SupportOptions>; | |
| 108 | else | ||
| 109 | 122622 | return getSupportFuncTpl<Shape0, Sphere, false, _SupportOptions>; | |
| 110 | 8770 | case GEOM_ELLIPSOID: | |
| 111 |
2/2✓ Branch 0 taken 1 times.
✓ Branch 1 taken 4384 times.
|
8770 | if (identity) |
| 112 | 2 | return getSupportFuncTpl<Shape0, Ellipsoid, true, _SupportOptions>; | |
| 113 | else | ||
| 114 | 8768 | return getSupportFuncTpl<Shape0, Ellipsoid, false, _SupportOptions>; | |
| 115 | 16966 | case GEOM_CAPSULE: | |
| 116 | if (_SupportOptions == SupportOptions::NoSweptSphere) { | ||
| 117 | // Sphere can be considered as a swept-sphere segment. | ||
| 118 | 16646 | swept_sphere_radius[1] += static_cast<const Capsule*>(s1)->radius; | |
| 119 | } | ||
| 120 |
1/2✗ Branch 0 not taken.
✓ Branch 1 taken 8483 times.
|
16966 | if (identity) |
| 121 | ✗ | return getSupportFuncTpl<Shape0, Capsule, true, _SupportOptions>; | |
| 122 | else | ||
| 123 | 16966 | return getSupportFuncTpl<Shape0, Capsule, false, _SupportOptions>; | |
| 124 | 4672 | case GEOM_CONE: | |
| 125 |
2/2✓ Branch 0 taken 16 times.
✓ Branch 1 taken 2320 times.
|
4672 | if (identity) |
| 126 | 32 | return getSupportFuncTpl<Shape0, Cone, true, _SupportOptions>; | |
| 127 | else | ||
| 128 | 4640 | return getSupportFuncTpl<Shape0, Cone, false, _SupportOptions>; | |
| 129 | 320788 | case GEOM_CYLINDER: | |
| 130 |
2/2✓ Branch 0 taken 8 times.
✓ Branch 1 taken 160386 times.
|
320788 | if (identity) |
| 131 | 16 | return getSupportFuncTpl<Shape0, Cylinder, true, _SupportOptions>; | |
| 132 | else | ||
| 133 | 320772 | return getSupportFuncTpl<Shape0, Cylinder, false, _SupportOptions>; | |
| 134 | ✗ | case GEOM_CONVEX16: { | |
| 135 | ✗ | const ConvexBase16* convex1 = static_cast<const ConvexBase16*>(s1); | |
| 136 | ✗ | if (static_cast<size_t>(convex1->num_points) > | |
| 137 | ConvexBase16::num_vertices_large_convex_threshold) { | ||
| 138 | ✗ | data[1].visited.assign(convex1->num_points, false); | |
| 139 | ✗ | data[1].last_dir.setZero(); | |
| 140 | ✗ | if (identity) | |
| 141 | ✗ | return getSupportFuncTpl<Shape0, LargeConvex16, true, | |
| 142 | _SupportOptions>; | ||
| 143 | else | ||
| 144 | ✗ | return getSupportFuncTpl<Shape0, LargeConvex16, false, | |
| 145 | _SupportOptions>; | ||
| 146 | } else { | ||
| 147 | ✗ | if (identity) | |
| 148 | ✗ | return getSupportFuncTpl<Shape0, SmallConvex16, true, | |
| 149 | _SupportOptions>; | ||
| 150 | else | ||
| 151 | ✗ | return getSupportFuncTpl<Shape0, SmallConvex16, false, | |
| 152 | _SupportOptions>; | ||
| 153 | } | ||
| 154 | } | ||
| 155 | 36794 | case GEOM_CONVEX32: { | |
| 156 | 36794 | const ConvexBase32* convex1 = static_cast<const ConvexBase32*>(s1); | |
| 157 |
1/2✗ Branch 0 not taken.
✓ Branch 1 taken 18397 times.
|
36794 | if (static_cast<size_t>(convex1->num_points) > |
| 158 | ConvexBase32::num_vertices_large_convex_threshold) { | ||
| 159 | ✗ | data[1].visited.assign(convex1->num_points, false); | |
| 160 | ✗ | data[1].last_dir.setZero(); | |
| 161 | ✗ | if (identity) | |
| 162 | ✗ | return getSupportFuncTpl<Shape0, LargeConvex32, true, | |
| 163 | _SupportOptions>; | ||
| 164 | else | ||
| 165 | ✗ | return getSupportFuncTpl<Shape0, LargeConvex32, false, | |
| 166 | _SupportOptions>; | ||
| 167 | } else { | ||
| 168 |
2/2✓ Branch 0 taken 10 times.
✓ Branch 1 taken 18387 times.
|
36794 | if (identity) |
| 169 | 20 | return getSupportFuncTpl<Shape0, SmallConvex32, true, | |
| 170 | _SupportOptions>; | ||
| 171 | else | ||
| 172 | 36774 | return getSupportFuncTpl<Shape0, SmallConvex32, false, | |
| 173 | _SupportOptions>; | ||
| 174 | } | ||
| 175 | } | ||
| 176 | ✗ | default: | |
| 177 | ✗ | COAL_THROW_PRETTY("Unsupported geometric shape.", std::logic_error); | |
| 178 | } | ||
| 179 | } | ||
| 180 | |||
| 181 | // ============================================================================ | ||
| 182 | template <int _SupportOptions> | ||
| 183 | 60066296 | MinkowskiDiff::GetSupportFunction makeGetSupportFunction0( | |
| 184 | const ShapeBase* s0, const ShapeBase* s1, bool identity, | ||
| 185 | Eigen::Array<Scalar, 1, 2>& swept_sphere_radius, ShapeSupportData data[2]) { | ||
| 186 | if (_SupportOptions == SupportOptions::WithSweptSphere) { | ||
| 187 | // No need to store the information of swept sphere radius | ||
| 188 | 3200 | swept_sphere_radius[0] = 0; | |
| 189 | } else { | ||
| 190 | // We store the information of swept sphere radius. | ||
| 191 | // GJK and EPA will use this information to correct the solution they find. | ||
| 192 | 60063096 | swept_sphere_radius[0] = s0->getSweptSphereRadius(); | |
| 193 | } | ||
| 194 | |||
| 195 |
8/10✓ Branch 1 taken 29205604 times.
✓ Branch 2 taken 419371 times.
✓ Branch 3 taken 230 times.
✓ Branch 4 taken 16066 times.
✓ Branch 5 taken 7836 times.
✓ Branch 6 taken 1316 times.
✓ Branch 7 taken 314823 times.
✗ Branch 8 not taken.
✓ Branch 9 taken 67902 times.
✗ Branch 10 not taken.
|
60066296 | switch (s0->getNodeType()) { |
| 196 | 58411208 | case GEOM_TRIANGLE: | |
| 197 | 58411208 | return makeGetSupportFunction1<TriangleP, _SupportOptions>( | |
| 198 | 58411208 | s1, identity, swept_sphere_radius, data); | |
| 199 | break; | ||
| 200 | 838742 | case GEOM_BOX: | |
| 201 | 838742 | return makeGetSupportFunction1<Box, _SupportOptions>( | |
| 202 | 838742 | s1, identity, swept_sphere_radius, data); | |
| 203 | break; | ||
| 204 | 460 | case GEOM_SPHERE: | |
| 205 | if (_SupportOptions == SupportOptions::NoSweptSphere) { | ||
| 206 | // Sphere can always be considered as a swept-sphere point. | ||
| 207 | 460 | swept_sphere_radius[0] += static_cast<const Sphere*>(s0)->radius; | |
| 208 | } | ||
| 209 | 460 | return makeGetSupportFunction1<Sphere, _SupportOptions>( | |
| 210 | 460 | s1, identity, swept_sphere_radius, data); | |
| 211 | break; | ||
| 212 | 32132 | case GEOM_ELLIPSOID: | |
| 213 | 32132 | return makeGetSupportFunction1<Ellipsoid, _SupportOptions>( | |
| 214 | 32132 | s1, identity, swept_sphere_radius, data); | |
| 215 | break; | ||
| 216 | 15672 | case GEOM_CAPSULE: | |
| 217 | if (_SupportOptions == SupportOptions::NoSweptSphere) { | ||
| 218 | // Capsule can always be considered as a swept-sphere segment. | ||
| 219 | 15032 | swept_sphere_radius[0] += static_cast<const Capsule*>(s0)->radius; | |
| 220 | } | ||
| 221 | 15672 | return makeGetSupportFunction1<Capsule, _SupportOptions>( | |
| 222 | 15672 | s1, identity, swept_sphere_radius, data); | |
| 223 | break; | ||
| 224 | 2632 | case GEOM_CONE: | |
| 225 | 2632 | return makeGetSupportFunction1<Cone, _SupportOptions>( | |
| 226 | 2632 | s1, identity, swept_sphere_radius, data); | |
| 227 | break; | ||
| 228 | 629646 | case GEOM_CYLINDER: | |
| 229 | 629646 | return makeGetSupportFunction1<Cylinder, _SupportOptions>( | |
| 230 | 629646 | s1, identity, swept_sphere_radius, data); | |
| 231 | break; | ||
| 232 | ✗ | case GEOM_CONVEX16: { | |
| 233 | ✗ | const ConvexBase16* convex0 = static_cast<const ConvexBase16*>(s0); | |
| 234 | ✗ | if (static_cast<size_t>(convex0->num_points) > | |
| 235 | ConvexBase16::num_vertices_large_convex_threshold) { | ||
| 236 | ✗ | data[0].visited.assign(convex0->num_points, false); | |
| 237 | ✗ | data[0].last_dir.setZero(); | |
| 238 | ✗ | return makeGetSupportFunction1<LargeConvex16, _SupportOptions>( | |
| 239 | ✗ | s1, identity, swept_sphere_radius, data); | |
| 240 | } else | ||
| 241 | ✗ | return makeGetSupportFunction1<SmallConvex16, _SupportOptions>( | |
| 242 | ✗ | s1, identity, swept_sphere_radius, data); | |
| 243 | break; | ||
| 244 | } | ||
| 245 | 135804 | case GEOM_CONVEX32: { | |
| 246 | 135804 | const ConvexBase32* convex0 = static_cast<const ConvexBase32*>(s0); | |
| 247 |
1/2✗ Branch 0 not taken.
✓ Branch 1 taken 67902 times.
|
135804 | if (static_cast<size_t>(convex0->num_points) > |
| 248 | ConvexBase32::num_vertices_large_convex_threshold) { | ||
| 249 | ✗ | data[0].visited.assign(convex0->num_points, false); | |
| 250 | ✗ | data[0].last_dir.setZero(); | |
| 251 | ✗ | return makeGetSupportFunction1<LargeConvex32, _SupportOptions>( | |
| 252 | ✗ | s1, identity, swept_sphere_radius, data); | |
| 253 | } else | ||
| 254 | 135804 | return makeGetSupportFunction1<SmallConvex32, _SupportOptions>( | |
| 255 | 135804 | s1, identity, swept_sphere_radius, data); | |
| 256 | break; | ||
| 257 | } | ||
| 258 | ✗ | default: | |
| 259 | ✗ | COAL_THROW_PRETTY("Unsupported geometric shape", std::logic_error); | |
| 260 | } | ||
| 261 | } | ||
| 262 | |||
| 263 | // ============================================================================ | ||
| 264 | 30101050 | bool getNormalizeSupportDirection(const ShapeBase* shape) { | |
| 265 |
8/10✓ Branch 1 taken 29205604 times.
✓ Branch 2 taken 419507 times.
✓ Branch 3 taken 61383 times.
✓ Branch 4 taken 16818 times.
✓ Branch 5 taken 7868 times.
✓ Branch 6 taken 1348 times.
✓ Branch 7 taken 314855 times.
✗ Branch 8 not taken.
✓ Branch 9 taken 73667 times.
✗ Branch 10 not taken.
|
30101050 | switch (shape->getNodeType()) { |
| 266 | 29205604 | case GEOM_TRIANGLE: | |
| 267 | 29205604 | return (bool)shape_traits<TriangleP>::NeedNesterovNormalizeHeuristic; | |
| 268 | break; | ||
| 269 | 419507 | case GEOM_BOX: | |
| 270 | 419507 | return (bool)shape_traits<Box>::NeedNesterovNormalizeHeuristic; | |
| 271 | break; | ||
| 272 | 61383 | case GEOM_SPHERE: | |
| 273 | 61383 | return (bool)shape_traits<Sphere>::NeedNesterovNormalizeHeuristic; | |
| 274 | break; | ||
| 275 | 16818 | case GEOM_ELLIPSOID: | |
| 276 | 16818 | return (bool)shape_traits<Ellipsoid>::NeedNesterovNormalizeHeuristic; | |
| 277 | break; | ||
| 278 | 7868 | case GEOM_CAPSULE: | |
| 279 | 7868 | return (bool)shape_traits<Capsule>::NeedNesterovNormalizeHeuristic; | |
| 280 | break; | ||
| 281 | 1348 | case GEOM_CONE: | |
| 282 | 1348 | return (bool)shape_traits<Cone>::NeedNesterovNormalizeHeuristic; | |
| 283 | break; | ||
| 284 | 314855 | case GEOM_CYLINDER: | |
| 285 | 314855 | return (bool)shape_traits<Cylinder>::NeedNesterovNormalizeHeuristic; | |
| 286 | break; | ||
| 287 | ✗ | case GEOM_CONVEX16: | |
| 288 | ✗ | return (bool)shape_traits<ConvexBase16>::NeedNesterovNormalizeHeuristic; | |
| 289 | break; | ||
| 290 | 73667 | case GEOM_CONVEX32: | |
| 291 | 73667 | return (bool)shape_traits<ConvexBase32>::NeedNesterovNormalizeHeuristic; | |
| 292 | break; | ||
| 293 | ✗ | default: | |
| 294 | ✗ | COAL_THROW_PRETTY("Unsupported geometric shape", std::logic_error); | |
| 295 | } | ||
| 296 | } | ||
| 297 | |||
| 298 | // ============================================================================ | ||
| 299 | 30033148 | void getNormalizeSupportDirectionFromShapes(const ShapeBase* shape0, | |
| 300 | const ShapeBase* shape1, | ||
| 301 | bool& normalize_support_direction) { | ||
| 302 |
2/2✓ Branch 1 taken 67902 times.
✓ Branch 2 taken 29965246 times.
|
30101050 | normalize_support_direction = getNormalizeSupportDirection(shape0) && |
| 303 |
2/2✓ Branch 1 taken 5765 times.
✓ Branch 2 taken 62137 times.
|
67902 | getNormalizeSupportDirection(shape1); |
| 304 | 30033148 | } | |
| 305 | |||
| 306 | // ============================================================================ | ||
| 307 | template <int _SupportOptions> | ||
| 308 | 1649260 | void MinkowskiDiff::set(const ShapeBase* shape0, const ShapeBase* shape1, | |
| 309 | const Transform3s& tf0, const Transform3s& tf1) { | ||
| 310 | 1649260 | shapes[0] = shape0; | |
| 311 | 1649260 | shapes[1] = shape1; | |
| 312 | 1649260 | getNormalizeSupportDirectionFromShapes(shape0, shape1, | |
| 313 | 1649260 | normalize_support_direction); | |
| 314 | |||
| 315 |
3/6✓ Branch 4 taken 824630 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 824630 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 824630 times.
✗ Branch 11 not taken.
|
1649260 | oR1.noalias() = tf0.getRotation().transpose() * tf1.getRotation(); |
| 316 |
4/8✓ Branch 3 taken 824630 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 824630 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 824630 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 824630 times.
✗ Branch 13 not taken.
|
1649260 | ot1.noalias() = tf0.getRotation().transpose() * |
| 317 | 1649260 | (tf1.getTranslation() - tf0.getTranslation()); | |
| 318 | |||
| 319 |
6/8✓ Branch 2 taken 824630 times.
✗ Branch 3 not taken.
✓ Branch 4 taken 63795 times.
✓ Branch 5 taken 760835 times.
✓ Branch 8 taken 63795 times.
✗ Branch 9 not taken.
✓ Branch 10 taken 57 times.
✓ Branch 11 taken 63738 times.
|
1649260 | bool identity = (oR1.isIdentity() && ot1.isZero()); |
| 320 | |||
| 321 | 3298520 | getSupportFunc = makeGetSupportFunction0<_SupportOptions>( | |
| 322 | 1649260 | shape0, shape1, identity, swept_sphere_radius, data); | |
| 323 | 1649260 | } | |
| 324 | // clang-format off | ||
| 325 | template void COAL_DLLAPI MinkowskiDiff::set<SupportOptions::NoSweptSphere>(const ShapeBase*, const ShapeBase*); | ||
| 326 | |||
| 327 | template void COAL_DLLAPI MinkowskiDiff::set<SupportOptions::WithSweptSphere>(const ShapeBase*, const ShapeBase*); | ||
| 328 | // clang-format on | ||
| 329 | |||
| 330 | // ============================================================================ | ||
| 331 | template <int _SupportOptions> | ||
| 332 | 58417036 | void MinkowskiDiff::set(const ShapeBase* shape0, const ShapeBase* shape1) { | |
| 333 | 58417036 | shapes[0] = shape0; | |
| 334 | 58417036 | shapes[1] = shape1; | |
| 335 | 58417036 | getNormalizeSupportDirectionFromShapes(shape0, shape1, | |
| 336 | 58417036 | normalize_support_direction); | |
| 337 | |||
| 338 | 58417036 | oR1.setIdentity(); | |
| 339 | 58417036 | ot1.setZero(); | |
| 340 | |||
| 341 | 116834072 | getSupportFunc = makeGetSupportFunction0<_SupportOptions>( | |
| 342 | 58417036 | shape0, shape1, true, swept_sphere_radius, data); | |
| 343 | } | ||
| 344 | // clang-format off | ||
| 345 | template void COAL_DLLAPI MinkowskiDiff::set<SupportOptions::NoSweptSphere>(const ShapeBase*, const ShapeBase*, const Transform3s&, const Transform3s&); | ||
| 346 | |||
| 347 | template void COAL_DLLAPI MinkowskiDiff::set<SupportOptions::WithSweptSphere>(const ShapeBase*, const ShapeBase*, const Transform3s&, const Transform3s&); | ||
| 348 | // clang-format on | ||
| 349 | |||
| 350 | // ============================================================================ | ||
| 351 | // clang-format off | ||
| 352 | template Vec3s COAL_DLLAPI MinkowskiDiff::support0<SupportOptions::NoSweptSphere>(const Vec3s&, int&) const; | ||
| 353 | |||
| 354 | template Vec3s COAL_DLLAPI MinkowskiDiff::support0<SupportOptions::WithSweptSphere>(const Vec3s&, int&) const; | ||
| 355 | |||
| 356 | template Vec3s COAL_DLLAPI MinkowskiDiff::support1<SupportOptions::NoSweptSphere>(const Vec3s&, int&) const; | ||
| 357 | |||
| 358 | template Vec3s COAL_DLLAPI MinkowskiDiff::support1<SupportOptions::WithSweptSphere>(const Vec3s&, int&) const; | ||
| 359 | // clang-format on | ||
| 360 | |||
| 361 | } // namespace details | ||
| 362 | } // namespace coal | ||
| 363 |