GCC Code Coverage Report


Directory: ./
File: src/distance-between-objects.cc
Date: 2024-12-13 16:14:03
Exec Total Coverage
Lines: 15 29 51.7%
Branches: 12 40 30.0%

Line Branch Exec Source
1 //
2 // Copyright (c) 2014 CNRS
3 // Authors: Florent Lamiraux
4 //
5
6 // Redistribution and use in source and binary forms, with or without
7 // modification, are permitted provided that the following conditions are
8 // met:
9 //
10 // 1. Redistributions of source code must retain the above copyright
11 // notice, this list of conditions and the following disclaimer.
12 //
13 // 2. Redistributions in binary form must reproduce the above copyright
14 // notice, this list of conditions and the following disclaimer in the
15 // documentation and/or other materials provided with the distribution.
16 //
17 // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
18 // "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
19 // LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
20 // A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
21 // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
22 // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
23 // LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
24 // DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
25 // THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
26 // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
27 // OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH
28 // DAMAGE.
29
30 #include <coal/distance.h>
31
32 #include <hpp/core/collision-pair.hh>
33 #include <hpp/core/distance-between-objects.hh>
34 #include <hpp/pinocchio/body.hh>
35 #include <hpp/pinocchio/collision-object.hh>
36 #include <hpp/pinocchio/device.hh>
37 #include <hpp/pinocchio/joint.hh>
38
39 namespace hpp {
40 namespace core {
41 7 void DistanceBetweenObjects::addObstacle(
42 const CollisionObjectConstPtr_t& object) {
43 using pinocchio::DISTANCE;
44
3/4
✓ Branch 2 taken 22 times.
✗ Branch 3 not taken.
✓ Branch 4 taken 15 times.
✓ Branch 5 taken 7 times.
22 for (size_type i = 0; i < robot_->nbJoints(); ++i) {
45
1/2
✓ Branch 2 taken 15 times.
✗ Branch 3 not taken.
15 JointConstPtr_t joint = robot_->jointAt(i);
46
1/2
✓ Branch 2 taken 15 times.
✗ Branch 3 not taken.
15 BodyPtr_t body = joint->linkedBody();
47
1/2
✓ Branch 1 taken 15 times.
✗ Branch 2 not taken.
15 if (body) {
48
3/4
✓ Branch 2 taken 30 times.
✗ Branch 3 not taken.
✓ Branch 4 taken 15 times.
✓ Branch 5 taken 15 times.
30 for (size_type j = 0; j < body->nbInnerObjects(); ++j) {
49
2/4
✓ Branch 2 taken 15 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 15 times.
✗ Branch 6 not taken.
15 collisionPairs_.emplace_back(body->innerObjectAt(j), object);
50 }
51 }
52 15 }
53 7 }
54
55 10 void DistanceBetweenObjects::obstacles(const ObjectStdVector_t& obstacles) {
56 10 for (ObjectStdVector_t::const_iterator itObj = obstacles.begin();
57
1/2
✗ Branch 2 not taken.
✓ Branch 3 taken 10 times.
10 itObj != obstacles.end(); ++itObj) {
58 addObstacle(*itObj);
59 }
60 10 }
61
62 void DistanceBetweenObjects::computeDistances() {
63 distanceResults_.resize(collisionPairs_.size());
64 std::size_t rank = 0;
65 coal::DistanceRequest distanceRequest(true, 0, 0);
66 for (CollisionPairs_t::const_iterator itCol = collisionPairs_.begin();
67 itCol != collisionPairs_.end(); ++itCol) {
68 const CollisionObjectConstPtr_t& obj1 = itCol->first;
69 const CollisionObjectConstPtr_t& obj2 = itCol->second;
70 distanceResults_[rank].clear();
71 coal::distance(obj1->geometry().get(), obj1->getFclTransform(),
72 obj2->geometry().get(), obj2->getFclTransform(),
73 distanceRequest, distanceResults_[rank]);
74 ++rank;
75 }
76 }
77
78 10 DistanceBetweenObjects::DistanceBetweenObjects(const DevicePtr_t& robot)
79 10 : robot_(robot) {}
80 } // namespace core
81 } // namespace hpp
82