Directory: | ./ |
---|---|
File: | src/nearest-neighbor/basic.cc |
Date: | 2024-08-10 11:29:48 |
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 | 206 | NodePtr_t Basic::search(ConfigurationIn_t configuration, | |
53 | const ConnectedComponentPtr_t& connectedComponent, | ||
54 | value_type& distance, bool reverse) { | ||
55 | 206 | NodePtr_t result = NULL; | |
56 | 206 | distance = std::numeric_limits<value_type>::infinity(); | |
57 | 206 | const Distance& dist = *distance_; | |
58 | value_type d; | ||
59 | 206 | for (NodeVector_t::const_iterator itNode = | |
60 | 206 | connectedComponent->nodes().begin(); | |
61 |
2/2✓ Branch 5 taken 930 times.
✓ Branch 6 taken 206 times.
|
1136 | itNode != connectedComponent->nodes().end(); ++itNode) { |
62 |
2/2✓ Branch 0 taken 7 times.
✓ Branch 1 taken 923 times.
|
930 | 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 923 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 923 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 923 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 923 times.
✗ Branch 12 not taken.
|
923 | d = dist((*itNode)->configuration(), configuration); |
66 |
2/2✓ Branch 0 taken 358 times.
✓ Branch 1 taken 572 times.
|
930 | if (d < distance) { |
67 | 358 | distance = d; | |
68 | 358 | result = *itNode; | |
69 | } | ||
70 | } | ||
71 |
1/2✗ Branch 0 not taken.
✓ Branch 1 taken 206 times.
|
206 | assert(result); |
72 | 206 | 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 |