GCC Code Coverage Report

Directory: ./
File: include/pinocchio/collision/distance.hxx
Date: 2025-02-12 21:03:38
Exec Total Coverage
Lines: 33 39 84.6%
Branches: 18 70 25.7%
Line Branch Exec Source
1 //
2 // Copyright (c) 2015-2021 CNRS INRIA
3 //
4
5 #ifndef __pinocchio_collision_distance_hxx__
6 #define __pinocchio_collision_distance_hxx__
7
8 #include "pinocchio/collision/distance.hpp"
9 #include "pinocchio/algorithm/geometry.hpp"
10 #include "pinocchio/multibody/geometry.hpp"
11
12 namespace pinocchio
13 {
14
15 template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl>
16 inline std::size_t computeDistances(
17 const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
18 const DataTpl<Scalar, Options, JointCollectionTpl> & data,
19 const GeometryModel & geom_model,
20 GeometryData & geom_data)
21 {
22 assert(model.check(data) && "data is not consistent with model.");
23 updateGeometryPlacements(model, data, geom_model, geom_data);
24 return computeDistances(geom_model, geom_data);
25 }
26
27 template<
28 typename Scalar,
29 int Options,
30 template<typename, int> class JointCollectionTpl,
31 typename ConfigVectorType>
32 1 inline std::size_t computeDistances(
33 const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
34 DataTpl<Scalar, Options, JointCollectionTpl> & data,
35 const GeometryModel & geom_model,
36 GeometryData & geom_data,
37 const Eigen::MatrixBase<ConfigVectorType> & q)
38 {
39
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 assert(model.check(data) && "data is not consistent with model.");
40 1 updateGeometryPlacements(model, data, geom_model, geom_data, q);
41 1 return computeDistances(geom_model, geom_data);
42 }
43
44 141 inline fcl::DistanceResult & computeDistance(
45 const GeometryModel & geom_model, GeometryData & geom_data, const PairIndex pair_id)
46 {
47
1/4
✗ Branch 1 not taken.
✓ Branch 2 taken 141 times.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
 141 PINOCCHIO_CHECK_INPUT_ARGUMENT(pair_id < geom_model.collisionPairs.size());
48
1/4
✗ Branch 2 not taken.
✓ Branch 3 taken 141 times.
✗ Branch 6 not taken.
✗ Branch 7 not taken.
 141 PINOCCHIO_CHECK_INPUT_ARGUMENT(
49 geom_model.collisionPairs.size() == geom_data.collisionResults.size());
50 141 const CollisionPair & pair = geom_model.collisionPairs[pair_id];
51
52
1/4
✗ Branch 0 not taken.
✓ Branch 1 taken 141 times.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
 141 PINOCCHIO_CHECK_INPUT_ARGUMENT(pair.first < geom_model.ngeoms);
53
1/4
✗ Branch 0 not taken.
✓ Branch 1 taken 141 times.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
 141 PINOCCHIO_CHECK_INPUT_ARGUMENT(pair.second < geom_model.ngeoms);
54
55 141 fcl::DistanceRequest & distance_request = geom_data.distanceRequests[pair_id];
56 141 fcl::DistanceResult & distance_result = geom_data.distanceResults[pair_id];
57
1/2
✓ Branch 1 taken 141 times.
✗ Branch 2 not taken.
 141 distance_result.clear();
58
59
1/2
✓ Branch 2 taken 141 times.
✗ Branch 3 not taken.
 141 fcl::Transform3f oM1(toFclTransform3f(geom_data.oMg[pair.first])),
60
1/2
✓ Branch 2 taken 141 times.
✗ Branch 3 not taken.
 141 oM2(toFclTransform3f(geom_data.oMg[pair.second]));
61
62 try
63 {
64 141 GeometryData::ComputeDistance & do_computations = geom_data.distance_functors[pair_id];
65
1/2
✓ Branch 1 taken 141 times.
✗ Branch 2 not taken.
 141 do_computations(oM1, oM2, distance_request, distance_result);
66 }
67 ✗ catch (std::invalid_argument & e)
68 {
69 ✗ std::stringstream ss;
70 ✗ ss << "Problem when trying to compute the distance of collision pair #" << pair_id << " ("
71 ✗ << pair.first << "," << pair.second << ")" << std::endl;
72 ✗ ss << "hpp-fcl original error:\n" << e.what() << std::endl;
73 ✗ throw std::invalid_argument(ss.str());
74 }
75
76 282 return geom_data.distanceResults[pair_id];
77 }
78
79 2 inline std::size_t computeDistances(const GeometryModel & geom_model, GeometryData & geom_data)
80 {
81 2 std::size_t min_index = geom_model.collisionPairs.size();
82 2 double min_dist = std::numeric_limits<double>::infinity();
83
84
2/2
✓ Branch 1 taken 140 times.
✓ Branch 2 taken 2 times.
 142 for (std::size_t cp_index = 0; cp_index < geom_model.collisionPairs.size(); ++cp_index)
85 {
86 140 const CollisionPair & cp = geom_model.collisionPairs[cp_index];
87
88 140 if (
89
1/2
✓ Branch 1 taken 140 times.
✗ Branch 2 not taken.
 140 geom_data.activeCollisionPairs[cp_index]
90
2/4
✓ Branch 0 taken 140 times.
✗ Branch 1 not taken.
✓ Branch 2 taken 140 times.
✗ Branch 3 not taken.
 280 && !(
91
1/2
✓ Branch 1 taken 140 times.
✗ Branch 2 not taken.
 140 geom_model.geometryObjects[cp.first].disableCollision
92
1/2
✓ Branch 2 taken 140 times.
✗ Branch 3 not taken.
 280 || geom_model.geometryObjects[cp.second].disableCollision))
93 {
94 140 computeDistance(geom_model, geom_data, cp_index);
95
2/2
✓ Branch 1 taken 10 times.
✓ Branch 2 taken 130 times.
 140 if (geom_data.distanceResults[cp_index].min_distance < min_dist)
96 {
97 10 min_index = cp_index;
98 10 min_dist = geom_data.distanceResults[cp_index].min_distance;
99 }
100 }
101 }
102
103 2 return min_index;
104 }
105
106 } // namespace pinocchio
107
108 #endif // ifndef __pinocchio_collision_distance_hxx__
109