GCC Code Coverage Report

Directory: ./
File: src/nearest-neighbor/basic.cc
Date: 2025-03-10 11:18:21
Exec Total Coverage
Lines: 37 95 38.9%
Branches: 32 124 25.8%
Line Branch Exec Source
1 // Copyright (c) 2015 CNRS
2 // Authors: Joseph Mirabel
3 //
4
5 // Redistribution and use in source and binary forms, with or without
6 // modification, are permitted provided that the following conditions are
7 // met:
8 //
9 // 1. Redistributions of source code must retain the above copyright
10 // notice, this list of conditions and the following disclaimer.
11 //
12 // 2. Redistributions in binary form must reproduce the above copyright
13 // notice, this list of conditions and the following disclaimer in the
14 // documentation and/or other materials provided with the distribution.
15 //
16 // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
17 // "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
18 // LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
19 // A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
20 // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
21 // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
22 // LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
23 // DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
24 // THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
25 // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
26 // OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH
27 // DAMAGE.
28
29 #include "../src/nearest-neighbor/basic.hh"
30
31 #include <hpp/core/connected-component.hh>
32 #include <hpp/core/distance.hh>
33 #include <hpp/core/roadmap.hh>
34 #include <limits>
35 #include <queue>
36
37 namespace hpp {
38 namespace core {
39 namespace nearestNeighbor {
40 namespace {
41 typedef std::pair<value_type, NodePtr_t> DistAndNode_t;
42 struct DistAndNodeComp_t {
43 5 bool operator()(const DistAndNode_t& r, const DistAndNode_t& l) {
44 5 return r.first < l.first;
45 }
46 };
47 typedef std::priority_queue<DistAndNode_t, std::vector<DistAndNode_t>,
48 DistAndNodeComp_t>
49 Queue_t;
50 } // namespace
51
52 208 NodePtr_t Basic::search(ConfigurationIn_t configuration,
53 const ConnectedComponentPtr_t& connectedComponent,
54 value_type& distance, bool reverse) {
55 208 NodePtr_t result = NULL;
56 208 distance = std::numeric_limits<value_type>::infinity();
57 208 const Distance& dist = *distance_;
58 value_type d;
59 208 for (NodeVector_t::const_iterator itNode =
60 208 connectedComponent->nodes().begin();
61
2/2
✓ Branch 5 taken 942 times.
✓ Branch 6 taken 208 times.
 1150 itNode != connectedComponent->nodes().end(); ++itNode) {
62
2/2
✓ Branch 0 taken 7 times.
✓ Branch 1 taken 935 times.
 942 if (reverse)
63
4/8
✓ Branch 2 taken 7 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 7 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 7 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 7 times.
✗ Branch 12 not taken.
 7 d = dist(configuration, (*itNode)->configuration());
64 else
65
4/8
✓ Branch 1 taken 935 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 935 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 935 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 935 times.
✗ Branch 12 not taken.
 935 d = dist((*itNode)->configuration(), configuration);
66
2/2
✓ Branch 0 taken 363 times.
✓ Branch 1 taken 579 times.
 942 if (d < distance) {
67 363 distance = d;
68 363 result = *itNode;
69 }
70 }
71
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 208 times.
 208 assert(result);
72 208 return result;
73 }
74
75 ✗ NodePtr_t Basic::search(const NodePtr_t& node,
76 const ConnectedComponentPtr_t& connectedComponent,
77 value_type& distance) {
78 ✗ NodePtr_t result = NULL;
79 ✗ distance = std::numeric_limits<value_type>::infinity();
80 ✗ const Distance& dist = *distance_;
81 ✗ for (NodeVector_t::const_iterator itNode =
82 ✗ connectedComponent->nodes().begin();
83 ✗ itNode != connectedComponent->nodes().end(); ++itNode) {
84 ✗ value_type d = dist(*itNode, node);
85 ✗ if (d < distance) {
86 ✗ distance = d;
87 ✗ result = *itNode;
88 }
89 }
90 ✗ assert(result);
91 ✗ return result;
92 }
93
94 1 Nodes_t Basic::KnearestSearch(ConfigurationIn_t q,
95 const ConnectedComponentPtr_t& connectedComponent,
96 const std::size_t K, value_type& distance) {
97 1 Queue_t ns;
98 1 distance = std::numeric_limits<value_type>::infinity();
99 1 const Distance& dist = *distance_;
100 1 for (NodeVector_t::const_iterator itNode =
101 1 connectedComponent->nodes().begin();
102
2/2
✓ Branch 5 taken 4 times.
✓ Branch 6 taken 1 times.
 5 itNode != connectedComponent->nodes().end(); ++itNode) {
103
4/8
✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 4 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 4 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 4 times.
✗ Branch 12 not taken.
 4 value_type d = dist((*itNode)->configuration(), q);
104
2/2
✓ Branch 1 taken 3 times.
✓ Branch 2 taken 1 times.
 4 if (ns.size() < K)
105
1/2
✓ Branch 3 taken 3 times.
✗ Branch 4 not taken.
 3 ns.push(DistAndNode_t(d, (*itNode)));
106
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 else if (ns.top().first > d) {
107
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 ns.pop();
108
1/2
✓ Branch 3 taken 1 times.
✗ Branch 4 not taken.
 1 ns.push(DistAndNode_t(d, (*itNode)));
109 }
110 }
111 1 Nodes_t nodes;
112
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 if (ns.size() > 0) distance = ns.top().first;
113
2/2
✓ Branch 1 taken 3 times.
✓ Branch 2 taken 1 times.
 4 while (ns.size() > 0) {
114
1/2
✓ Branch 2 taken 3 times.
✗ Branch 3 not taken.
 3 nodes.push_front(ns.top().second);
115
1/2
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
 3 ns.pop();
116 }
117 2 return nodes;
118 1 }
119
120 ✗ Nodes_t Basic::KnearestSearch(const NodePtr_t& node,
121 const ConnectedComponentPtr_t& connectedComponent,
122 const std::size_t K, value_type& distance) {
123 ✗ Queue_t ns;
124 ✗ distance = std::numeric_limits<value_type>::infinity();
125 ✗ const Distance& dist = *distance_;
126 ✗ for (NodeVector_t::const_iterator itNode =
127 ✗ connectedComponent->nodes().begin();
128 ✗ itNode != connectedComponent->nodes().end(); ++itNode) {
129 ✗ value_type d = dist(*itNode, node);
130 ✗ if (ns.size() < K)
131 ✗ ns.push(DistAndNode_t(d, (*itNode)));
132 ✗ else if (ns.top().first > d) {
133 ✗ ns.pop();
134 ✗ ns.push(DistAndNode_t(d, (*itNode)));
135 }
136 }
137 ✗ Nodes_t nodes;
138 ✗ if (ns.size() > 0) distance = ns.top().first;
139 ✗ while (ns.size() > 0) {
140 ✗ nodes.push_front(ns.top().second);
141 ✗ ns.pop();
142 }
143 ✗ return nodes;
144 }
145
146 ✗ Nodes_t Basic::KnearestSearch(ConfigurationIn_t q, const RoadmapPtr_t& roadmap,
147 const std::size_t K, value_type& distance) {
148 ✗ Queue_t ns;
149 ✗ distance = std::numeric_limits<value_type>::infinity();
150 ✗ const Distance& dist = *distance_;
151 ✗ for (Nodes_t::const_iterator itNode = roadmap->nodes().begin();
152 ✗ itNode != roadmap->nodes().end(); ++itNode) {
153 ✗ value_type d = dist((*itNode)->configuration(), q);
154 ✗ if (ns.size() < K)
155 ✗ ns.push(DistAndNode_t(d, (*itNode)));
156 ✗ else if (ns.top().first > d) {
157 ✗ ns.pop();
158 ✗ ns.push(DistAndNode_t(d, (*itNode)));
159 }
160 }
161 ✗ Nodes_t nodes;
162 ✗ if (ns.size() > 0) distance = ns.top().first;
163 ✗ while (ns.size() > 0) {
164 ✗ nodes.push_front(ns.top().second);
165 ✗ ns.pop();
166 }
167 ✗ return nodes;
168 }
169
170 ✗ NodeVector_t Basic::withinBall(ConfigurationIn_t q,
171 const ConnectedComponentPtr_t& cc,
172 value_type maxDistance) {
173 ✗ const Distance& dist = *distance_;
174 ✗ NodeVector_t nodes;
175 ✗ for (NodeVector_t::const_iterator itNode = cc->nodes().begin();
176 ✗ itNode != cc->nodes().end(); ++itNode) {
177 ✗ NodePtr_t n = *itNode;
178 ✗ if (dist(n->configuration(), q) < maxDistance) nodes.push_back(n);
179 }
180 ✗ return nodes;
181 }
182 } // namespace nearestNeighbor
183 } // namespace core
184 } // namespace hpp
185