GCC Code Coverage Report

Directory: ./
File: include/pinocchio/algorithm/geometry.hxx
Date: 2025-02-12 21:03:38
Exec Total Coverage
Lines: 31 33 93.9%
Branches: 18 28 64.3%
Line Branch Exec Source
1 //
2 // Copyright (c) 2015-2022 CNRS INRIA
3 //
4
5 #ifndef __pinocchio_algo_geometry_hxx__
6 #define __pinocchio_algo_geometry_hxx__
7
8 #include <boost/foreach.hpp>
9 #include <sstream>
10
11 namespace pinocchio
12 {
13 /* --- GEOMETRY PLACEMENTS -------------------------------------------------------- */
14 /* --- GEOMETRY PLACEMENTS -------------------------------------------------------- */
15 /* --- GEOMETRY PLACEMENTS -------------------------------------------------------- */
16 template<
17 typename Scalar,
18 int Options,
19 template<typename, int> class JointCollectionTpl,
20 typename ConfigVectorType>
21 4024 inline void updateGeometryPlacements(
22 const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
23 DataTpl<Scalar, Options, JointCollectionTpl> & data,
24 const GeometryModel & geom_model,
25 GeometryData & geom_data,
26 const Eigen::MatrixBase<ConfigVectorType> & q)
27 {
28
1/2
✓ Branch 1 taken 4024 times.
✗ Branch 2 not taken.
 4024 assert(model.check(data) && "data is not consistent with model.");
29
30 4024 forwardKinematics(model, data, q);
31 4024 updateGeometryPlacements(model, data, geom_model, geom_data);
32 4024 }
33
34 template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl>
35 4031 inline void updateGeometryPlacements(
36 const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
37 const DataTpl<Scalar, Options, JointCollectionTpl> & data,
38 const GeometryModel & geom_model,
39 GeometryData & geom_data)
40 {
41 PINOCCHIO_UNUSED_VARIABLE(model);
42
1/2
✓ Branch 1 taken 4031 times.
✗ Branch 2 not taken.
 4031 assert(model.check(data) && "data is not consistent with model.");
43
44
2/2
✓ Branch 0 taken 80630 times.
✓ Branch 1 taken 4031 times.
 84661 for (GeomIndex i = 0; i < (GeomIndex)geom_model.ngeoms; ++i)
45 {
46 80630 const Model::JointIndex joint_id = geom_model.geometryObjects[i].parentJoint;
47
2/2
✓ Branch 0 taken 80615 times.
✓ Branch 1 taken 15 times.
 80630 if (joint_id > 0)
48
1/2
✓ Branch 5 taken 80615 times.
✗ Branch 6 not taken.
 80615 geom_data.oMg[i] = (data.oMi[joint_id] * geom_model.geometryObjects[i].placement);
49 else
50 15 geom_data.oMg[i] = geom_model.geometryObjects[i].placement;
51 }
52 4031 }
53
54 /* --- APPEND GEOMETRY MODEL ----------------------------------------------------------- */
55
56 2 inline void appendGeometryModel(GeometryModel & geom_model1, const GeometryModel & geom_model2)
57 {
58
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 2 times.
 2 assert(geom_model1.ngeoms == geom_model1.geometryObjects.size());
59 2 Index nGeom1 = geom_model1.geometryObjects.size();
60 2 Index nColPairs1 = geom_model1.collisionPairs.size();
61
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 2 times.
 2 assert(geom_model2.ngeoms == geom_model2.geometryObjects.size());
62 2 Index nGeom2 = geom_model2.geometryObjects.size();
63 2 Index nColPairs2 = geom_model2.collisionPairs.size();
64
65 /// Append the geometry objects and geometry positions
66
1/2
✓ Branch 4 taken 2 times.
✗ Branch 5 not taken.
 4 geom_model1.geometryObjects.insert(
67 2 geom_model1.geometryObjects.end(), geom_model2.geometryObjects.begin(),
68 geom_model2.geometryObjects.end());
69 2 geom_model1.ngeoms += nGeom2;
70
71 /// 1. copy the collision pairs and update geom_data1 accordingly.
72 2 geom_model1.collisionPairs.reserve(nColPairs1 + nColPairs2 + nGeom1 * nGeom2);
73
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 2 times.
 2 for (Index i = 0; i < nColPairs2; ++i)
74 {
75 ✗ const CollisionPair & cp = geom_model2.collisionPairs[i];
76 ✗ geom_model1.collisionPairs.push_back(CollisionPair(cp.first + nGeom1, cp.second + nGeom1));
77 }
78
79 /// 2. add the collision pairs between geom_model1 and geom_model2.
80
2/2
✓ Branch 0 taken 20 times.
✓ Branch 1 taken 2 times.
 22 for (Index i = 0; i < nGeom1; ++i)
81 {
82 20 Index parent_i = geom_model1.geometryObjects[i].parentJoint;
83
2/2
✓ Branch 0 taken 400 times.
✓ Branch 1 taken 20 times.
 420 for (Index j = nGeom1; j < nGeom1 + nGeom2; ++j)
84 {
85
2/2
✓ Branch 1 taken 380 times.
✓ Branch 2 taken 20 times.
 400 if (parent_i != geom_model1.geometryObjects[j].parentJoint)
86
1/2
✓ Branch 2 taken 380 times.
✗ Branch 3 not taken.
 380 geom_model1.collisionPairs.push_back(CollisionPair(i, j));
87 }
88 }
89 2 }
90
91 } // namespace pinocchio
92
93 #endif // ifnded __pinocchio_algo_geometry_hxx__
94