<commit_before>//
// Copyright (c) 2014 CNRS
// Authors: <NAME>
//
// This file is part of hpp-core
// hpp-core is free software: you can redistribute it
// and/or modify it under the terms of the GNU Lesser General Public
// License as published by the Free Software Foundation, either version
// 3 of the License, or (at your option) any later version.
//
// hpp-core is distributed in the hope that it will be
// useful, but WITHOUT ANY WARRANTY; without even the implied warranty
// of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
// General Lesser Public License for more details.  You should have
// received a copy of the GNU Lesser General Public License along with
// hpp-core  If not, see
// <http://www.gnu.org/licenses/>.

# include <hpp/util/debug.hh>
# include <hpp/core/distance.hh>
# include <hpp/core/node.hh>
# include <hpp/model/joint.hh>
# include <hpp/model/joint-configuration.hh>
# include <hpp/model/device.hh>
# include <hpp/core/weighed-distance.hh>
#include "k-d-tree.hh"
#include <iostream>
#include <fstream>

using namespace std;

namespace hpp {
  namespace core {

    // Constructor with the mother tree node (same bounds)
    KDTree::KDTree (const KDTreePtr_t mother, size_type splitDim) :
      robot_(mother->robot_),
      dim_(mother->dim_),
      distance_(mother->distance_),
      weights_ (mother->weights_),
      nodesMap_(),
      bucketSize_(mother->bucketSize_),
      bucket_(0),
      splitDim_(splitDim),
      upperBounds_(mother->upperBounds_),
      lowerBounds_(mother->lowerBounds_),
      supChild_(0x0),
      infChild_(0x0)
    {
    }

    KDTree::KDTree (const DevicePtr_t& robot, const DistancePtr_t& distance,
		    int bucketSize) :
      robot_(robot),
      dim_(),
      distance_(HPP_DYNAMIC_PTR_CAST (WeighedDistance, distance)),
      weights_ (robot->configSize ()),
      nodesMap_(),
      bucketSize_(bucketSize),
      bucket_(0),
      splitDim_(),
      upperBounds_(),
      lowerBounds_(),
      supChild_(),
      infChild_()
       {
	 JointVector_t jointVector = robot_->getJointVector ();
	 if (!distance_) {
	   // create a weighed distance with unit weighs.
	   distance_ = WeighedDistance::create
	     (robot_, std::vector <value_type> (jointVector.size (), 1.0));
	 }
	 size_type i=0;
	 // Fill vector of weights. index of vector is configuration coordinate
	 // and not joint rank in robot.
	 for (JointVector_t::const_iterator itJoint = jointVector.begin ();
	      itJoint != jointVector.end (); ++itJoint) {
	   weights_.segment ((*itJoint)->rankInConfiguration (),
			     (*itJoint)->configSize ()).setConstant
	     (distance_->getWeight (i));
	   ++i;
	 }
      this->findDeviceBounds();
      dim_ = lowerBounds_.size();
      splitDim_ = 0;
      supChild_ = NULL;
      infChild_ = NULL;
    }

    KDTree::~KDTree() {
      if (infChild_ != NULL ) { delete infChild_; }
      if (supChild_ != NULL ) { delete supChild_; }
    }

    // find the leaf node in the tree for the configuration of the node
    KDTreePtr_t KDTree::findLeaf (const NodePtr_t& node) {
      KDTreePtr_t CurrentTree = this;
      CurrentTree->nodesMap_[node->connectedComponent()];
      while ( CurrentTree->supChild_ != NULL && CurrentTree->infChild_ != NULL)
	{
	  if ( (*(node->configuration()))[CurrentTree->supChild_->splitDim_]
	       > CurrentTree->supChild_->lowerBounds_[CurrentTree->supChild_
						      ->splitDim_] )  {
	    CurrentTree = CurrentTree->supChild_;
	    CurrentTree->nodesMap_[node->connectedComponent()];
	  }
	  else {
	    CurrentTree = CurrentTree->infChild_;
	    CurrentTree->nodesMap_[node->connectedComponent()];
	  }
	}
      return CurrentTree;
    }


    void KDTree::addNode (const NodePtr_t& node) {

      KDTreePtr_t Leaf = this->findLeaf(node);
      if ( Leaf->bucket_ < bucketSize_ ) {
	Leaf->nodesMap_[node->connectedComponent()].push_front(node);
	Leaf->bucket_++;
      }
      else {
	Leaf->split();
	for ( NodesMap_t::iterator map = Leaf->nodesMap_.begin();
	      map != Leaf->nodesMap_.end() ; map++ ) {
	  for (Nodes_t::iterator it = map->second.begin ();
	       it != map->second.end (); it++ ) {
	    Leaf->addNode(*it);
	  }
	  map->second.clear();
	}
	Leaf->addNode(node);
      }
    }


    void KDTree::clear() {
      nodesMap_.clear();
      if ( infChild_ != NULL ) { infChild_ = NULL; }
      if ( supChild_ != NULL ) { supChild_ = NULL; }
    }


    void KDTree::split() {
      if ( infChild_ != NULL || supChild_ != NULL ) {
	// Error, you're triing to split a non leaf part of the KDTree
	throw std::runtime_error
	  ("Attempt to split the KDTree in a non leaf part");
      }
      // Compute actual bounds of node configurations
      vector_t actualLower (robot_->configSize ());
      actualLower.setConstant (+std::numeric_limits <value_type>::infinity ());
      vector_t actualUpper (robot_->configSize ());
      actualUpper.setConstant (-std::numeric_limits <value_type>::infinity ());
      std::vector <value_type> values (bucket_);<commit_msg>Fix memory deallocation when deleting KDTree.<commit_after>//
// Copyright (c) 2014 CNRS
// Authors: <NAME>
//
// This file is part of hpp-core
// hpp-core is free software: you can redistribute it
// and/or modify it under the terms of the GNU Lesser General Public
// License as published by the Free Software Foundation, either version
// 3 of the License, or (at your option) any later version.
//
// hpp-core is distributed in the hope that it will be
// useful, but WITHOUT ANY WARRANTY; without even the implied warranty
// of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
// General Lesser Public License for more details.  You should have
// received a copy of the GNU Lesser General Public License along with
// hpp-core  If not, see
// <http://www.gnu.org/licenses/>.

# include <hpp/util/debug.hh>
# include <hpp/core/distance.hh>
# include <hpp/core/node.hh>
# include <hpp/model/joint.hh>
# include <hpp/model/joint-configuration.hh>
# include <hpp/model/device.hh>
# include <hpp/core/weighed-distance.hh>
#include "k-d-tree.hh"
#include <iostream>
#include <fstream>

using namespace std;

namespace hpp {
  namespace core {

    // Constructor with the mother tree node (same bounds)
    KDTree::KDTree (const KDTreePtr_t mother, size_type splitDim) :
      robot_(mother->robot_),
      dim_(mother->dim_),
      distance_(mother->distance_),
      weights_ (mother->weights_),
      nodesMap_(),
      bucketSize_(mother->bucketSize_),
      bucket_(0),
      splitDim_(splitDim),
      upperBounds_(mother->upperBounds_),
      lowerBounds_(mother->lowerBounds_),
      supChild_(0x0),
      infChild_(0x0)
    {
    }

    KDTree::KDTree (const DevicePtr_t& robot, const DistancePtr_t& distance,
		    int bucketSize) :
      robot_(robot),
      dim_(),
      distance_(HPP_DYNAMIC_PTR_CAST (WeighedDistance, distance)),
      weights_ (robot->configSize ()),
      nodesMap_(),
      bucketSize_(bucketSize),
      bucket_(0),
      splitDim_(),
      upperBounds_(),
      lowerBounds_(),
      supChild_(),
      infChild_()
       {
	 JointVector_t jointVector = robot_->getJointVector ();
	 if (!distance_) {
	   // create a weighed distance with unit weighs.
	   distance_ = WeighedDistance::create
	     (robot_, std::vector <value_type> (jointVector.size (), 1.0));
	 }
	 size_type i=0;
	 // Fill vector of weights. index of vector is configuration coordinate
	 // and not joint rank in robot.
	 for (JointVector_t::const_iterator itJoint = jointVector.begin ();
	      itJoint != jointVector.end (); ++itJoint) {
	   weights_.segment ((*itJoint)->rankInConfiguration (),
			     (*itJoint)->configSize ()).setConstant
	     (distance_->getWeight (i));
	   ++i;
	 }
      this->findDeviceBounds();
      dim_ = lowerBounds_.size();
      splitDim_ = 0;
      supChild_ = NULL;
      infChild_ = NULL;
    }

    KDTree::~KDTree() {
      clear ();
    }

    // find the leaf node in the tree for the configuration of the node
    KDTreePtr_t KDTree::findLeaf (const NodePtr_t& node) {
      KDTreePtr_t CurrentTree = this;
      CurrentTree->nodesMap_[node->connectedComponent()];
      while ( CurrentTree->supChild_ != NULL && CurrentTree->infChild_ != NULL)
	{
	  if ( (*(node->configuration()))[CurrentTree->supChild_->splitDim_]
	       > CurrentTree->supChild_->lowerBounds_[CurrentTree->supChild_
						      ->splitDim_] )  {
	    CurrentTree = CurrentTree->supChild_;
	    CurrentTree->nodesMap_[node->connectedComponent()];
	  }
	  else {
	    CurrentTree = CurrentTree->infChild_;
	    CurrentTree->nodesMap_[node->connectedComponent()];
	  }
	}
      return CurrentTree;
    }


    void KDTree::addNode (const NodePtr_t& node) {

      KDTreePtr_t Leaf = this->findLeaf(node);
      if ( Leaf->bucket_ < bucketSize_ ) {
	Leaf->nodesMap_[node->connectedComponent()].push_front(node);
	Leaf->bucket_++;
      }
      else {
	Leaf->split();
	for ( NodesMap_t::iterator map = Leaf->nodesMap_.begin();
	      map != Leaf->nodesMap_.end() ; map++ ) {
	  for (Nodes_t::iterator it = map->second.begin ();
	       it != map->second.end (); it++ ) {
	    Leaf->addNode(*it);
	  }
	  map->second.clear();
	}
	Leaf->addNode(node);
      }
    }


    void KDTree::clear() {
      nodesMap_.clear();
      if (infChild_ != NULL ) {
	delete infChild_;
	infChild_ = NULL;
      }
      if (supChild_ != NULL ) {
	delete supChild_;
	supChild_ = NULL;
      }
    }


    void KDTree::split() {
      if ( infChild_ != NULL || supChild_ != NULL ) {
	// Error, you're triing to split a non leaf part of the KDTree
	throw std::runtime_error
	  ("Attempt to split the KDTree in a non leaf part");
      }
      // Compute actual bounds of node configurations
      vector_t actualLower (robot_->configSize ());
      actualLower.setConstant (+std::numeric_limits <value_type>::infinity ());
      vector_t actualUpper (robot_->configSize ());
      actualUpper.setConstant (-std::numeric_limits <value_type>::infinity ());
      std::vector <value_type> values (bucket_);
{'fraction_non_alphanumeric': 0.13209802749551702, 'fraction_numerical': 0.002988643156007173, 'mean_word_length': 3.0365902694008846, 'pattern_counts': {'":': 0, '<': 35, '<?xml version=': 0, '>': 125, 'https://': 0, 'lorem ipsum': 0, 'www.': 2, 'xml': 0}, 'pii_count': 0, 'substrings_counts': 0, 'word_list_counts': {'cursed_substrings.json': 0, 'profanity_word_list.json': 0, 'sexual_word_list.json': 0, 'zh_pornsignals.json': 0}}
{'dir': 'git-commits-cleaned', 'id': '7438127', 'n_tokens_mistral': 3211, 'n_tokens_neox': 3010, 'n_words': 797}