Commit 5fae1110 authored by Joseph Mirabel's avatar Joseph Mirabel
Browse files

[Refactoring] Remove usage of ConfigurationPtr_t in NearestNeighbor

parent 8d9f088c
......@@ -39,7 +39,7 @@ namespace hpp {
* if false from nodes in roadmap to given configuration
* @return
*/
virtual NodePtr_t search (const ConfigurationPtr_t& configuration,
virtual NodePtr_t search (const Configuration_t& configuration,
const ConnectedComponentPtr_t&
connectedComponent,
value_type& distance,bool reverse = false) = 0;
......@@ -52,7 +52,7 @@ namespace hpp {
/// \param[out] distance to the Kth closest neighbor
/// \return the K nearest neighbors
virtual Nodes_t KnearestSearch (const ConfigurationPtr_t& configuration,
virtual Nodes_t KnearestSearch (const Configuration_t& configuration,
const ConnectedComponentPtr_t&
connectedComponent,
const std::size_t K,
......@@ -71,7 +71,7 @@ namespace hpp {
/// \param K the number of nearest neighbors to return
/// \retval distance to the Kth closest neighbor
/// \return the K nearest neighbors
virtual Nodes_t KnearestSearch (const ConfigurationPtr_t& configuration,
virtual Nodes_t KnearestSearch (const Configuration_t& configuration,
const RoadmapPtr_t& roadmap,
const std::size_t K,
value_type& distance) = 0;
......
......@@ -39,7 +39,7 @@ namespace hpp {
DistAndNodeComp_t > Queue_t;
}
NodePtr_t Basic::search (const ConfigurationPtr_t& configuration,
NodePtr_t Basic::search (const Configuration_t& configuration,
const ConnectedComponentPtr_t&
connectedComponent,
value_type& distance, bool reverse)
......@@ -52,9 +52,9 @@ namespace hpp {
connectedComponent->nodes ().begin ();
itNode != connectedComponent->nodes ().end (); ++itNode) {
if(reverse)
d = dist ( *configuration, *(*itNode)->configuration ());
d = dist ( configuration, *(*itNode)->configuration ());
else
d = dist ( *(*itNode)->configuration (), *configuration);
d = dist ( *(*itNode)->configuration (), configuration);
if (d < distance) {
distance = d;
result = *itNode;
......@@ -85,7 +85,7 @@ namespace hpp {
return result;
}
Nodes_t Basic::KnearestSearch (const ConfigurationPtr_t& configuration,
Nodes_t Basic::KnearestSearch (const Configuration_t& q,
const ConnectedComponentPtr_t&
connectedComponent,
const std::size_t K,
......@@ -94,7 +94,6 @@ namespace hpp {
Queue_t ns;
distance = std::numeric_limits <value_type>::infinity ();
const Distance& dist = *distance_;
const Configuration_t& q = *configuration;
for (NodeVector_t::const_iterator itNode =
connectedComponent->nodes ().begin ();
itNode != connectedComponent->nodes ().end (); ++itNode) {
......@@ -142,14 +141,13 @@ namespace hpp {
return nodes;
}
Nodes_t Basic::KnearestSearch (const ConfigurationPtr_t& configuration,
Nodes_t Basic::KnearestSearch (const Configuration_t& q,
const RoadmapPtr_t& roadmap,
const std::size_t K, value_type& distance)
{
Queue_t ns;
distance = std::numeric_limits <value_type>::infinity ();
const Distance& dist = *distance_;
const Configuration_t& q = *configuration;
for (Nodes_t::const_iterator itNode =
roadmap->nodes ().begin ();
itNode != roadmap->nodes ().end (); ++itNode) {
......
......@@ -51,12 +51,12 @@ namespace hpp {
connectedComponent,
value_type& distance);
virtual NodePtr_t search (const ConfigurationPtr_t& configuration,
virtual NodePtr_t search (const Configuration_t& configuration,
const ConnectedComponentPtr_t&
connectedComponent,
value_type& distance, bool reverse = false);
virtual Nodes_t KnearestSearch (const ConfigurationPtr_t& configuration,
virtual Nodes_t KnearestSearch (const Configuration_t& configuration,
const ConnectedComponentPtr_t&
connectedComponent,
const std::size_t K,
......@@ -74,7 +74,7 @@ namespace hpp {
/// \param K the number of nearest neighbors to return
/// \retval distance to the Kth closest neighbor
/// \return the K nearest neighbors
virtual Nodes_t KnearestSearch (const ConfigurationPtr_t& configuration,
virtual Nodes_t KnearestSearch (const Configuration_t& configuration,
const RoadmapPtr_t& roadmap,
const std::size_t K,
value_type& distance);
......
......@@ -219,34 +219,34 @@ namespace hpp {
}
value_type KDTree::distanceToBox (const ConfigurationPtr_t& configuration) {
value_type KDTree::distanceToBox (const Configuration_t& configuration) {
value_type minDistance;
value_type DistanceToUpperBound;
value_type DistanceToLowerBound;
// Projection of the configuration on the box
Configuration_t confbox = *configuration;
Configuration_t confbox = configuration;
DistanceToLowerBound = fabs (lowerBounds_[splitDim_] -
(*configuration) [splitDim_])
configuration [splitDim_])
* weights_ [splitDim_];
DistanceToUpperBound = fabs (upperBounds_[splitDim_] -
(*configuration)[splitDim_])
configuration[splitDim_])
* weights_ [splitDim_];
minDistance = std::min (DistanceToLowerBound, DistanceToUpperBound);
return minDistance;
}
NodePtr_t KDTree::search (const ConfigurationPtr_t& configuration,
NodePtr_t KDTree::search (const Configuration_t& configuration,
const ConnectedComponentPtr_t& connectedComponent,
value_type& minDistance, bool) {
// Test if the configuration is in the root box
for ( std::size_t i=0 ; i<dim_ ; i++ ) {
if ( (*configuration)[i] < lowerBounds_[i] || (*configuration)[i]
if ( configuration[i] < lowerBounds_[i] || configuration[i]
> upperBounds_[i] ) {
std::ostringstream oss ("The Configuration isn't in the root box: \n"
" i = ");
oss << i << ", lower = " << lowerBounds_[i] << ", config = "
<< (*configuration)[i] << ", upper = " << upperBounds_[i]
<< configuration[i] << ", upper = " << upperBounds_[i]
<< ".";
throw std::runtime_error (oss.str ());
}
......@@ -263,7 +263,7 @@ namespace hpp {
NodePtr_t KDTree::search (const NodePtr_t& node,
const ConnectedComponentPtr_t& connectedComponent,
value_type& minDistance) {
return search (node->configuration (), connectedComponent, minDistance);
return search (*node->configuration (), connectedComponent, minDistance);
}
Nodes_t KDTree::KnearestSearch (const NodePtr_t&,
......@@ -273,14 +273,14 @@ namespace hpp {
assert (false && "K-nearest neighbor in KD-tree: unimplemented features");
}
Nodes_t KDTree::KnearestSearch (const ConfigurationPtr_t&,
Nodes_t KDTree::KnearestSearch (const Configuration_t&,
const ConnectedComponentPtr_t&, const std::size_t,
value_type&)
{
assert (false && "K-nearest neighbor in KD-tree: unimplemented features");
}
Nodes_t KDTree::KnearestSearch (const ConfigurationPtr_t& configuration,
Nodes_t KDTree::KnearestSearch (const Configuration_t& configuration,
const RoadmapPtr_t& roadmap,
const std::size_t K, value_type& distance)
{
......@@ -288,7 +288,7 @@ namespace hpp {
}
void KDTree::search (value_type boxDistance, value_type& minDistance,
const ConfigurationPtr_t& configuration,
const Configuration_t& configuration,
const ConnectedComponentPtr_t& connectedComponent,
NodePtr_t& nearest,bool reverse) {
if ( boxDistance < minDistance*minDistance
......@@ -300,9 +300,9 @@ namespace hpp {
nodesMap_[connectedComponent].begin ();
itNode != nodesMap_[connectedComponent].end (); ++itNode) {
if(reverse)
distance = (*distance_) (*configuration,*((*itNode)->configuration ()));
distance = (*distance_) (configuration,*((*itNode)->configuration ()));
else
distance = (*distance_) (*((*itNode)->configuration ()),*configuration);
distance = (*distance_) (*((*itNode)->configuration ()),configuration);
if (distance < minDistance) {
minDistance = distance;
nearest = (*itNode);
......@@ -314,7 +314,7 @@ namespace hpp {
value_type distanceToInfChild;
value_type distanceToSupChild;
if ( boxDistance == 0. ) {
if ( (*configuration) [supChild_->splitDim_]
if ( configuration [supChild_->splitDim_]
> supChild_->lowerBounds_[supChild_->splitDim_]) {
distanceToSupChild = 0.;
distanceToInfChild = infChild_->distanceToBox(configuration);
......
......@@ -48,7 +48,7 @@ namespace hpp {
virtual void clear();
// search nearest node
virtual NodePtr_t search (const ConfigurationPtr_t& configuration,
virtual NodePtr_t search (const Configuration_t& configuration,
const ConnectedComponentPtr_t&
connectedComponent,
value_type& minDistance, bool reverse = false);
......@@ -65,7 +65,7 @@ namespace hpp {
const std::size_t K,
value_type& distance);
virtual Nodes_t KnearestSearch (const ConfigurationPtr_t& configuration,
virtual Nodes_t KnearestSearch (const Configuration_t& configuration,
const ConnectedComponentPtr_t&
connectedComponent,
const std::size_t K,
......@@ -77,7 +77,7 @@ namespace hpp {
/// \param K the number of nearest neighbors to return
/// \retval distance to the Kth closest neighbor
/// \return the K nearest neighbors
virtual Nodes_t KnearestSearch (const ConfigurationPtr_t& configuration,
virtual Nodes_t KnearestSearch (const Configuration_t& configuration,
const RoadmapPtr_t& roadmap,
const std::size_t K,
value_type& distance);
......@@ -127,11 +127,11 @@ namespace hpp {
void findDeviceBounds();
// distance to the nearest bound on the splited dimention
value_type distanceToBox(const ConfigurationPtr_t& configuration);
value_type distanceToBox(const Configuration_t& configuration);
// search nearest node
void search(value_type boxDistance, value_type& minDistance,
const ConfigurationPtr_t& configuration,
const Configuration_t& configuration,
const ConnectedComponentPtr_t& connectedComponent,
NodePtr_t& nearest, bool reverse = false);
......
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment