GCC Code Coverage Report


Directory: ./
File: src/nearest-neighbor/basic.cc
Date: 2024-12-13 16:14:03
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