| Directory: | ./ |
|---|---|
| File: | src/distance.cpp |
| Date: | 2025-04-13 14:25:19 |
| Exec | Total | Coverage | |
|---|---|---|---|
| Lines: | 23 | 66 | 34.8% |
| Branches: | 19 | 208 | 9.1% |
| 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/distance.h" | ||
| 39 | #include "coal/collision_utility.h" | ||
| 40 | #include "coal/distance_func_matrix.h" | ||
| 41 | #include "coal/narrowphase/narrowphase.h" | ||
| 42 | |||
| 43 | #include "coal/tracy.hh" | ||
| 44 | |||
| 45 | namespace coal { | ||
| 46 | |||
| 47 | 45875 | DistanceFunctionMatrix& getDistanceFunctionLookTable() { | |
| 48 |
4/8✓ Branch 0 taken 10 times.
✓ Branch 1 taken 45865 times.
✓ Branch 3 taken 10 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 10 times.
✗ Branch 7 not taken.
✗ Branch 9 not taken.
✗ Branch 10 not taken.
|
45875 | static DistanceFunctionMatrix table; |
| 49 | 45875 | return table; | |
| 50 | } | ||
| 51 | |||
| 52 | 35223 | Scalar distance(const CollisionObject* o1, const CollisionObject* o2, | |
| 53 | const DistanceRequest& request, DistanceResult& result) { | ||
| 54 | 35223 | return distance(o1->collisionGeometryPtr(), o1->getTransform(), | |
| 55 | o2->collisionGeometryPtr(), o2->getTransform(), request, | ||
| 56 | 35223 | result); | |
| 57 | } | ||
| 58 | |||
| 59 | 45875 | Scalar distance(const CollisionGeometry* o1, const Transform3s& tf1, | |
| 60 | const CollisionGeometry* o2, const Transform3s& tf2, | ||
| 61 | const DistanceRequest& request, DistanceResult& result) { | ||
| 62 | COAL_TRACY_ZONE_SCOPED_N("coal::distance"); | ||
| 63 |
1/2✓ Branch 1 taken 45875 times.
✗ Branch 2 not taken.
|
45875 | GJKSolver solver(request); |
| 64 | |||
| 65 |
1/2✓ Branch 1 taken 45875 times.
✗ Branch 2 not taken.
|
45875 | const DistanceFunctionMatrix& looktable = getDistanceFunctionLookTable(); |
| 66 | |||
| 67 |
1/2✓ Branch 1 taken 45875 times.
✗ Branch 2 not taken.
|
45875 | OBJECT_TYPE object_type1 = o1->getObjectType(); |
| 68 |
1/2✓ Branch 1 taken 45875 times.
✗ Branch 2 not taken.
|
45875 | NODE_TYPE node_type1 = o1->getNodeType(); |
| 69 |
1/2✓ Branch 1 taken 45875 times.
✗ Branch 2 not taken.
|
45875 | OBJECT_TYPE object_type2 = o2->getObjectType(); |
| 70 |
1/2✓ Branch 1 taken 45875 times.
✗ Branch 2 not taken.
|
45875 | NODE_TYPE node_type2 = o2->getNodeType(); |
| 71 | |||
| 72 | 45875 | Scalar res = (std::numeric_limits<Scalar>::max)(); | |
| 73 | |||
| 74 |
3/4✓ Branch 0 taken 38632 times.
✓ Branch 1 taken 7243 times.
✓ Branch 2 taken 38632 times.
✗ Branch 3 not taken.
|
45875 | if (object_type1 == OT_GEOM && |
| 75 |
1/2✗ Branch 0 not taken.
✓ Branch 1 taken 38632 times.
|
38632 | (object_type2 == OT_BVH || object_type2 == OT_HFIELD)) { |
| 76 | ✗ | if (!looktable.distance_matrix[node_type2][node_type1]) { | |
| 77 | ✗ | COAL_THROW_PRETTY("Distance function between node type " | |
| 78 | << std::string(get_node_type_name(node_type1)) | ||
| 79 | << " and node type " | ||
| 80 | << std::string(get_node_type_name(node_type2)) | ||
| 81 | << " is not yet supported.", | ||
| 82 | std::invalid_argument); | ||
| 83 | } else { | ||
| 84 | ✗ | res = looktable.distance_matrix[node_type2][node_type1]( | |
| 85 | o2, tf2, o1, tf1, &solver, request, result); | ||
| 86 | ✗ | std::swap(result.o1, result.o2); | |
| 87 | ✗ | result.nearest_points[0].swap(result.nearest_points[1]); | |
| 88 | ✗ | result.normal *= -1; | |
| 89 | } | ||
| 90 | ✗ | } else { | |
| 91 |
1/2✗ Branch 0 not taken.
✓ Branch 1 taken 45875 times.
|
45875 | if (!looktable.distance_matrix[node_type1][node_type2]) { |
| 92 | ✗ | COAL_THROW_PRETTY("Distance function between node type " | |
| 93 | << std::string(get_node_type_name(node_type1)) | ||
| 94 | << " and node type " | ||
| 95 | << std::string(get_node_type_name(node_type2)) | ||
| 96 | << " is not yet supported.", | ||
| 97 | std::invalid_argument); | ||
| 98 | } else { | ||
| 99 |
1/2✓ Branch 1 taken 45875 times.
✗ Branch 2 not taken.
|
45875 | res = looktable.distance_matrix[node_type1][node_type2]( |
| 100 | o1, tf1, o2, tf2, &solver, request, result); | ||
| 101 | } | ||
| 102 | } | ||
| 103 | // Cache narrow phase solver result. If the option in the request is selected, | ||
| 104 | // also store the solver result in the request for the next call. | ||
| 105 |
1/2✓ Branch 1 taken 45875 times.
✗ Branch 2 not taken.
|
45875 | result.cached_gjk_guess = solver.cached_guess; |
| 106 |
1/2✓ Branch 1 taken 45875 times.
✗ Branch 2 not taken.
|
45875 | result.cached_support_func_guess = solver.support_func_cached_guess; |
| 107 |
1/2✓ Branch 1 taken 45875 times.
✗ Branch 2 not taken.
|
45875 | request.updateGuess(result); |
| 108 | 45875 | return res; | |
| 109 | 45875 | } | |
| 110 | |||
| 111 | ✗ | ComputeDistance::ComputeDistance(const CollisionGeometry* o1, | |
| 112 | ✗ | const CollisionGeometry* o2) | |
| 113 | ✗ | : o1(o1), o2(o2) { | |
| 114 | ✗ | const DistanceFunctionMatrix& looktable = getDistanceFunctionLookTable(); | |
| 115 | |||
| 116 | ✗ | OBJECT_TYPE object_type1 = o1->getObjectType(); | |
| 117 | ✗ | NODE_TYPE node_type1 = o1->getNodeType(); | |
| 118 | ✗ | OBJECT_TYPE object_type2 = o2->getObjectType(); | |
| 119 | ✗ | NODE_TYPE node_type2 = o2->getNodeType(); | |
| 120 | |||
| 121 | ✗ | swap_geoms = object_type1 == OT_GEOM && | |
| 122 | ✗ | (object_type2 == OT_BVH || object_type2 == OT_HFIELD); | |
| 123 | |||
| 124 | ✗ | if ((swap_geoms && !looktable.distance_matrix[node_type2][node_type1]) || | |
| 125 | ✗ | (!swap_geoms && !looktable.distance_matrix[node_type1][node_type2])) { | |
| 126 | ✗ | COAL_THROW_PRETTY("Distance function between node type " | |
| 127 | << std::string(get_node_type_name(node_type1)) | ||
| 128 | << " and node type " | ||
| 129 | << std::string(get_node_type_name(node_type2)) | ||
| 130 | << " is not yet supported.", | ||
| 131 | std::invalid_argument); | ||
| 132 | } | ||
| 133 | ✗ | if (swap_geoms) | |
| 134 | ✗ | func = looktable.distance_matrix[node_type2][node_type1]; | |
| 135 | else | ||
| 136 | ✗ | func = looktable.distance_matrix[node_type1][node_type2]; | |
| 137 | } | ||
| 138 | |||
| 139 | ✗ | Scalar ComputeDistance::run(const Transform3s& tf1, const Transform3s& tf2, | |
| 140 | const DistanceRequest& request, | ||
| 141 | DistanceResult& result) const { | ||
| 142 | COAL_TRACY_ZONE_SCOPED_N("coal::ComputeDistance::run"); | ||
| 143 | Scalar res; | ||
| 144 | |||
| 145 | ✗ | if (swap_geoms) { | |
| 146 | ✗ | res = func(o2, tf2, o1, tf1, &solver, request, result); | |
| 147 | ✗ | std::swap(result.o1, result.o2); | |
| 148 | ✗ | result.nearest_points[0].swap(result.nearest_points[1]); | |
| 149 | ✗ | result.normal *= -1; | |
| 150 | } else { | ||
| 151 | ✗ | res = func(o1, tf1, o2, tf2, &solver, request, result); | |
| 152 | } | ||
| 153 | // Cache narrow phase solver result. If the option in the request is selected, | ||
| 154 | // also store the solver result in the request for the next call. | ||
| 155 | ✗ | result.cached_gjk_guess = solver.cached_guess; | |
| 156 | ✗ | result.cached_support_func_guess = solver.support_func_cached_guess; | |
| 157 | ✗ | request.updateGuess(result); | |
| 158 | ✗ | return res; | |
| 159 | } | ||
| 160 | |||
| 161 | ✗ | Scalar ComputeDistance::operator()(const Transform3s& tf1, | |
| 162 | const Transform3s& tf2, | ||
| 163 | const DistanceRequest& request, | ||
| 164 | DistanceResult& result) const { | ||
| 165 | ✗ | solver.set(request); | |
| 166 | |||
| 167 | Scalar res; | ||
| 168 | ✗ | if (request.enable_timings) { | |
| 169 | ✗ | Timer timer; | |
| 170 | ✗ | res = run(tf1, tf2, request, result); | |
| 171 | ✗ | result.timings = timer.elapsed(); | |
| 172 | } else | ||
| 173 | ✗ | res = run(tf1, tf2, request, result); | |
| 174 | ✗ | return res; | |
| 175 | } | ||
| 176 | |||
| 177 | } // namespace coal | ||
| 178 |