aka_tree.hh
No OneTemporary
Actions

Subscribers

None

File Metadata

Created: Mon, Nov 4, 12:44

aka_tree.hh
View Options

	/**
	* @file aka_tree.hh
	*
	* @author Alejandro M. Aragón <alejandro.aragon@epfl.ch>
	*
	* @date creation: Fri Jan 04 2013
	* @date last modification: Tue May 13 2014
	*
	* @brief bounding volume hierarchies
	*
	* @section LICENSE
	*
	* Copyright (©) 2014 EPFL (Ecole Polytechnique Fédérale de Lausanne)
	* Laboratory (LSMS - Laboratoire de Simulation en Mécanique des Solides)
	*
	* Akantu 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.
	*
	* Akantu 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 Lesser General Public License for more
	* details.
	*
	* You should have received a copy of the GNU Lesser General Public License
	* along with Akantu. If not, see <http://www.gnu.org/licenses/>.
	*
	*/

	/* -------------------------------------------------------------------------- */

	#ifndef __AKANTU_TREE_HH__
	#define __AKANTU_TREE_HH__

	#include <cassert>
	#include <queue>
	#include <set>
	#include <iomanip>
	//#include <limits>

	#include "aka_ball.hh"
	#include "aka_bounding_box.hh"
	#include "aka_array.hh"

	//#define DEBUG_TREE 1


	__BEGIN_AKANTU__


	using std::cout;
	using std::endl;


	struct Tree_node_base
	{
	typedef Tree_node_base* base_ptr;
	typedef const Tree_node_base* const_base_ptr;

	base_ptr parent_;
	base_ptr left_;
	base_ptr right_;

	static base_ptr _minimum(base_ptr x) {
	while (x->left_ != 0) x = x->left_;
	return x;
	}

	static const_base_ptr _minimum(const_base_ptr x) {
	while (x->left_ != 0) x = x->left_;
	return x;
	}

	static base_ptr _maximum(base_ptr x) {
	while (x->right_ != 0) x = x->right_;
	return x;
	}

	static const_base_ptr _maximum(const_base_ptr x) {
	while (x->right_ != 0) x = x->right_;
	return x;
	}
	};

	template<typename T>
	struct Tree_node : public Tree_node_base
	{
	typedef T value_tpe;
	typedef Tree_node<value_tpe>* link_type;

	value_tpe value_;

	template<typename... Args>
	Tree_node(Args&&... args)
	: Tree_node_base(), value_(std::forward<Args>(args)...) { }
	};


	// iterators

	static Tree_node_base* local_leaf_increment(Tree_node_base* x) throw () {

	Tree_node_base* y = x->parent_;
	if (y->parent_ == x)
	return y;
	while (x == y->right_) {
	x = y;
	y = y->parent_;
	}
	if (y->parent_ == x)
	return y;
	if (x->right_ != y)
	x = y;
	if (x->right_ != 0) {
	x = x->right_;
	while (x->left_ != 0)
	x = x->left_;
	}
	return x;
	}

	template <class tree_pointer>
	tree_pointer* _tree_leaf_increment(tree_pointer* x) throw ()
	{ return local_leaf_increment(x); }

	template <class tree_pointer>
	const tree_pointer* _tree_leaf_increment(const tree_pointer* x) throw ()
	{ return local_leaf_increment(const_cast<tree_pointer*>(x)); }


	template<typename _Tp>
	struct Tree_leaf_iterator
	{
	typedef _Tp value_type;
	typedef _Tp& reference;
	typedef _Tp* pointer;

	typedef std::bidirectional_iterator_tag iterator_category;
	typedef std::ptrdiff_t difference_type;

	typedef Tree_leaf_iterator<_Tp> self_type;
	typedef Tree_node_base::base_ptr base_ptr;
	typedef Tree_node<_Tp>* link_type;

	Tree_leaf_iterator() : node_() { }

	explicit Tree_leaf_iterator(link_type x) : node_(x) { }

	reference operator*() const
	{ return static_cast<link_type>(node_)->value_; }

	pointer operator->() const
	{ return std::__addressof(static_cast<link_type>(node_)->value_); }

	self_type& operator++() {
	node_ = _tree_leaf_increment(node_);
	return *this;
	}

	self_type operator++(int) {
	self_type tmp = *this;
	node_ = _tree_leaf_increment(node_);
	return tmp;
	}

	bool operator==(const self_type& x) const
	{ return node_ == x.node_; }

	bool operator!=(const self_type& x) const
	{ return node_ != x.node_; }

	bool operator<(const self_type& x) const
	{ return node_ < x.node_; }

	base_ptr node_;
	};


	template<typename _Tp>
	struct Tree_leaf_const_iterator
	{
	typedef _Tp value_type;
	typedef const _Tp& reference;
	typedef const _Tp* pointer;

	typedef Tree_leaf_iterator<_Tp> iterator;

	typedef std::bidirectional_iterator_tag iterator_category;
	typedef std::ptrdiff_t difference_type;

	typedef Tree_leaf_const_iterator<_Tp> self_type;
	typedef Tree_node_base::const_base_ptr base_ptr;
	typedef const Tree_node<_Tp>* link_type;

	Tree_leaf_const_iterator() : node_() { }

	explicit Tree_leaf_const_iterator(link_type x) : node_(x) { }

	Tree_leaf_const_iterator(const iterator& it) : node_(it.node_) { }

	iterator _const_cast() const
	{ return iterator(static_cast<typename iterator::link_type>(const_cast<typename iterator::base_ptr>(node_))); }

	reference operator*() const
	{ return static_cast<link_type>(node_)->value_; }

	pointer operator->() const
	{ return std::__addressof(static_cast<link_type>
	(node_)->value_); }

	self_type& operator++() {
	node_ = _tree_leaf_increment(node_);
	return *this;
	}

	self_type operator++(int) {
	self_type tmp = *this;
	node_ = _tree_leaf_increment(node_);
	return tmp;
	}

	bool operator==(const self_type& x) const
	{ return node_ == x.node_; }

	bool operator!=(const self_type& x) const
	{ return node_ != x.node_; }

	bool operator<(const self_type& x) const
	{ return node_ < x.node_; }

	base_ptr node_;
	};


	template<typename _Val>
	inline bool operator==(const Tree_leaf_iterator<_Val>& x,
	const Tree_leaf_const_iterator<_Val>& y)
	{ return x.node_ == y.node_; }

	template<typename _Val>
	inline bool operator!=(const Tree_leaf_iterator<_Val>& x,
	const Tree_leaf_const_iterator<_Val>& y)
	{ return x.node_ != y.node_; }



	template <class tree_pointer>
	static tree_pointer*
	local_tree_increment(tree_pointer* x) throw ()
	{
	if (x->right_ != 0)
	{
	x = x->right_;
	while (x->left_ != 0)
	x = x->left_;
	}
	else
	{
	tree_pointer* y = x->parent_;
	while (x == y->right_)
	{
	x = y;
	y = y->parent_;
	}
	if (x->right_ != y)
	x = y;
	}
	return x;
	}


	template <class tree_pointer>
	tree_pointer* _tree_increment(tree_pointer* x) throw ()
	{ return local_tree_increment(x); }

	template <class tree_pointer>
	const tree_pointer*
	_tree_increment(const tree_pointer* x) throw ()
	{ return local_tree_increment(const_cast<tree_pointer*>(x)); }

	template <class tree_pointer>
	static tree_pointer*
	local_tree_decrement(tree_pointer* x) throw () {
	if (x->parent_->parent_ == x)
	x = x->right_;
	else if (x->left_ != 0) {
	Tree_node_base* y = x->left_;
	while (y->right_ != 0)
	y = y->right_;
	x = y;
	}
	else {
	tree_pointer* y = x->parent_;
	while (x == y->left_) {
	x = y;
	y = y->parent_;
	}
	x = y;
	}
	return x;
	}

	template <class tree_pointer>
	tree_pointer* _tree_decrement(tree_pointer* x) throw ()
	{ return local_tree_decrement(x); }

	template <class tree_pointer>
	const tree_pointer* _tree_decrement(const tree_pointer* x) throw ()
	{ return local_tree_decrement(const_cast<tree_pointer*>(x)); }


	// iterators

	template<typename _Tp>
	struct Tree_iterator
	{
	typedef _Tp value_type;
	typedef _Tp& reference;
	typedef _Tp* pointer;

	typedef std::bidirectional_iterator_tag iterator_category;
	typedef std::ptrdiff_t difference_type;

	typedef Tree_iterator<_Tp> self_type;
	typedef Tree_node_base::base_ptr base_ptr;
	typedef Tree_node<_Tp>* link_type;

	typedef Tree_leaf_iterator<value_type> leaf_iterator;


	Tree_iterator() : node_() { }

	explicit Tree_iterator(link_type x) : node_(x) { }

	Tree_iterator(const leaf_iterator& it) : node_(it.node_) { }

	reference operator*() const
	{ return static_cast<link_type>(node_)->value_; }

	pointer operator->() const
	{ return std::__addressof(static_cast<link_type>(node_)->value_); }

	self_type& operator++() {
	node_ = _tree_increment(node_);
	return *this;
	}

	self_type operator++(int) {
	self_type tmp = *this;
	node_ = _tree_increment(node_);
	return tmp;
	}

	self_type& operator--() {
	node_ = _tree_decrement(node_);
	return *this;
	}

	self_type operator--(int) {
	self_type tmp = *this;
	node_ = _tree_decrement(node_);
	return tmp;
	}


	self_type left()
	{ return self_type(static_cast<link_type>(node_->left_)); }

	self_type right()
	{ return self_type(static_cast<link_type>(node_->right_)); }

	self_type parent()
	{ return self_type(static_cast<link_type>(node_->parent_)); }

	bool is_leaf() {
	assert(node_ != nullptr);
	return node_->left_ == nullptr && node_->right_ == nullptr;
	}


	bool operator==(const self_type& x) const
	{ return node_ == x.node_; }

	bool operator!=(const self_type& x) const
	{ return node_ != x.node_; }

	bool operator<(const self_type& x) const
	{ return node_ < x.node_; }

	base_ptr node_;
	};

	template<typename _Tp>
	struct Tree_const_iterator
	{
	typedef _Tp value_type;
	typedef const _Tp& reference;
	typedef const _Tp* pointer;

	typedef Tree_iterator<_Tp> iterator;

	typedef std::bidirectional_iterator_tag iterator_category;
	typedef std::ptrdiff_t difference_type;

	typedef Tree_const_iterator<_Tp> self_type;
	typedef Tree_node_base::const_base_ptr base_ptr;
	typedef const Tree_node<_Tp>* link_type;

	Tree_const_iterator() : node_() { }

	explicit Tree_const_iterator(link_type x) : node_(x) { }

	Tree_const_iterator(const iterator& it) : node_(it.node_) { }

	iterator _const_cast() const
	{ return iterator(static_cast<typename iterator::link_type>(const_cast<typename iterator::base_ptr>(node_))); }

	reference operator*() const
	{ return static_cast<link_type>(node_)->value_; }

	pointer operator->() const
	{ return std::__addressof(static_cast<link_type>
	(node_)->value_); }

	self_type& operator++() {
	node_ = _tree_increment(node_);
	return *this;
	}

	self_type operator++(int) {
	self_type tmp = *this;
	node_ = _tree_increment(node_);
	return tmp;
	}

	self_type& operator--() {
	node_ = _tree_decrement(node_);
	return *this;
	}

	self_type operator--(int) {
	self_type tmp = *this;
	node_ = _tree_decrement(node_);
	return tmp;
	}

	self_type left()
	{ return self_type(static_cast<link_type>(node_->left_)); }

	self_type right()
	{ return self_type(static_cast<link_type>(node_->right_)); }

	self_type parent()
	{ return self_type(static_cast<link_type>(node_->parent_)); }

	bool is_leaf() {
	assert(node_ != nullptr);
	return node_->left_ == nullptr && node_->right_ == nullptr;
	}


	bool operator==(const self_type& x) const
	{ return node_ == x.node_; }

	bool operator!=(const self_type& x) const
	{ return node_ != x.node_; }

	bool operator<(const self_type& x) const
	{ return node_ < x.node_; }

	base_ptr node_;
	};

	template<typename _Val>
	inline bool operator==(const Tree_iterator<_Val>& x,
	const Tree_const_iterator<_Val>& y)
	{ return x.node_ == y.node_; }

	template<typename _Val>
	inline bool operator!=(const Tree_iterator<_Val>& x,
	const Tree_const_iterator<_Val>& y)
	{ return x.node_ != y.node_; }




	// traversal
	template <class iterator_type, class functor_type>
	void inorder(iterator_type it, functor_type& fn) {
	if (it.node_ == nullptr)
	return;
	inorder(it.left(), fn);
	fn(it);
	inorder(it.right(), fn);
	}

	template <class iterator_type, class functor_type>
	void preorder(iterator_type it, functor_type& fn) {
	if (it.node_ == nullptr)
	return;
	fn(it);
	preorder(it.left(), fn);
	preorder(it.right(), fn);
	}

	template <class iterator_type, class functor_type>
	void postorder(iterator_type it, functor_type& fn) {
	if (it.node_ == nullptr)
	return;
	postorder(it.left(), fn);
	postorder(it.right(), fn);
	fn(it);
	}



	//! Tree class
	template<typename T, template <class> class _Cost, typename _Alloc = std::allocator<T> >
	class Tree
	{
	typedef typename _Alloc::template rebind<Tree_node<T> >::other
	_Node_allocator;

	protected:
	typedef Tree_node_base* base_ptr;
	typedef const Tree_node_base* const_base_ptr;

	public:

	typedef T value_type;
	typedef value_type* pointer;
	typedef const value_type* const_pointer;
	typedef value_type& reference;
	typedef const value_type& const_reference;
	typedef Tree_node<T>* link_type;
	typedef const Tree_node<T>* const_link_type;
	typedef size_t size_type;
	typedef std::ptrdiff_t difference_type;
	typedef _Cost<value_type> cost_functor;
	typedef _Alloc allocator_type;

	_Node_allocator& _get_Node_allocator()
	{ return static_cast<_Node_allocator>(&this->impl_); }

	const _Node_allocator& _get_Node_allocator() const
	{ return static_cast<const _Node_allocator>(&this->impl_); }

	allocator_type
	get_allocator() const
	{ return allocator_type(_get_Node_allocator()); }

	private:

	link_type _get_node()
	{ return impl_._Node_allocator::allocate(1); }

	void _put_node(link_type p)
	{ impl_._Node_allocator::deallocate(p, 1); }

	void _destroy_node(link_type p) {
	_get_Node_allocator().destroy(p);
	_put_node(p);
	}

	link_type _clone_node(const_link_type x) {
	link_type tmp = _create_node(x->value_);
	tmp->left_ = 0;
	tmp->right_ = 0;
	return tmp;
	}

	template<typename... _Args>
	link_type _create_node(_Args&&... args) {

	link_type tmp = _get_node();
	try {
	_get_Node_allocator().construct(tmp, std::forward<_Args>(args)...);
	tmp->left_ = 0;
	tmp->right_ = 0;
	tmp->parent_ = 0;
	}
	catch(...) {
	_put_node(tmp);
	throw;
	}
	return tmp;
	}

	private:

	template<typename _Cost_functor>
	struct Tree_impl : public _Node_allocator
	{
	_Cost_functor cost_;
	Tree_node_base header_;
	size_type node_count_; // Keeps track of size of tree.

	Tree_impl()
	: _Node_allocator(), cost_(), header_(),
	node_count_(0)
	{ _M_initialize(); }

	Tree_impl(const _Cost_functor& comp, const _Node_allocator& a)
	: _Node_allocator(a), cost_(comp), header_(),
	node_count_(0)
	{ _M_initialize(); }

	Tree_impl(const _Cost_functor& comp, _Node_allocator&& a)
	: _Node_allocator(std::move(a)), cost_(comp),
	header_(), node_count_(0)
	{ _M_initialize(); }

	private:
	void
	_M_initialize()
	{
	this->header_.parent_ = 0;
	this->header_.left_ = &this->header_;
	this->header_.right_ = &this->header_;
	}
	};

	private:

	Tree_impl<cost_functor> impl_; //!< Tree implementation instance

	base_ptr& _root()
	{ return this->impl_.header_.parent_; }

	const_base_ptr _root() const
	{ return this->impl_.header_.parent_; }

	base_ptr& _leftmost()
	{ return this->impl_.header_.left_; }

	const_base_ptr _leftmost() const
	{ return this->impl_.header_.left_; }

	base_ptr& _rightmost()
	{ return this->impl_.header_.right_; }

	const_base_ptr _rightmost() const
	{ return this->impl_.header_.right_; }

	link_type _begin()
	{ return static_cast<link_type>(this->impl_.header_.parent_); }

	const_link_type _begin() const
	{ return static_cast<const_link_type>(this->impl_.header_.parent_); }

	link_type _end()
	{ return static_cast<link_type>(&this->impl_.header_); }

	const_link_type _end() const
	{ return static_cast<const_link_type>(&this->impl_.header_); }

	static reference _value(link_type x)
	{ return x->value_; }

	static const_reference _value(const_link_type x)
	{ return x->value_; }

	static reference _value(base_ptr x)
	{ return static_cast<link_type>(x)->value_; }

	static const_reference _value(const_base_ptr x)
	{ return static_cast<const_link_type>(x)->value_; }

	static link_type _left(base_ptr x)
	{ return static_cast<link_type>(x->left_); }

	static const_link_type _left(const_base_ptr x)
	{ return static_cast<const_link_type>(x->left_); }

	static link_type _right(base_ptr x)
	{ return static_cast<link_type>(x->right_); }

	static const_link_type _right(const_base_ptr x)
	{ return static_cast<const_link_type>(x->right_); }

	static link_type _parent(base_ptr x)
	{ return static_cast<link_type>(x->parent_); }

	static const_link_type _parent(const_base_ptr x)
	{ return static_cast<const_link_type>(x->parent_); }


	void _erase(link_type x) {
	while (x != 0)
	{
	#ifdef DEBUG_TREE
	cout<<"pointer x -> "<<x<<endl;
	cout<<"calling erase on right -> "<<_right(x)<<endl;
	cout<<"destroying node -> "<<_value(x)<<endl;
	cout<<"setting x to left -> "<<_left(x)<<endl;
	#endif
	_erase(_right(x));
	link_type y = _left(x);
	_destroy_node(x);
	--impl_.node_count_; // decrement nodes
	x = y;
	}
	}

	link_type _copy(const_link_type x, link_type p) {

	link_type top = _clone_node(x);
	top->parent_ = p;

	try
	{
	if (x->right_)
	top->right_ = _copy(_right(x), top);
	p = top;
	x = _left(x);

	while (x != 0)
	{
	link_type y = _clone_node(x);
	p->left_ = y;
	y->parent_ = p;
	if (x->right_)
	y->right_ = _copy(_right(x), y);
	p = y;
	x = _left(x);
	}
	}
	catch(...)
	{
	_erase(top);
	throw;
	}
	return top;
	}

	void _check_integrity(link_type x) {

	while (x != 0) {

	link_type r = _right(x);
	if (r)
	if (r->parent_ != x) {
	cout<<"* ERROR * Integrity check failed"<<endl;
	cout<<"Right child parent mismatch"<<endl;
	exit(1);
	}
	link_type l = _left(x);
	if (l)
	if (l->parent_ != x) {
	cout<<"* ERROR * Integrity check failed"<<endl;
	cout<<"Left child parent mismatch"<<endl;
	exit(1);
	}
	_check_integrity(r);
	x = _left(x);
	}
	}


	public:

	typedef Tree_iterator<value_type> iterator;
	typedef Tree_const_iterator<value_type> const_iterator;

	typedef std::reverse_iterator<iterator> reverse_iterator;
	typedef std::reverse_iterator<const_iterator> const_reverse_iterator;

	typedef Tree_leaf_iterator<value_type> leaf_iterator;
	typedef Tree_leaf_const_iterator<value_type> const_leaf_iterator;



	// allocation/deallocation
	Tree() {}

	Tree(const cost_functor& cost, const allocator_type& a = allocator_type())
	: impl_(cost, _Node_allocator(a)) { }

	Tree(const Tree& x) : impl_(x.impl_.cost_, x._get_Node_allocator())
	{
	if (x._root() != 0)
	{
	_root() = _copy(x._begin(), _end());
	_leftmost() = Tree_node_base::_minimum(_root());
	_rightmost() = Tree_node_base::_maximum(_root());
	impl_.node_count_ = x.impl_.node_count_;
	}
	}

	//! construct a tree from link type, defined by the user in any way
	Tree(link_type x) {

	// integrity check
	_check_integrity(x);
	if (x) _insert(0,x);
	}


	~Tree() noexcept
	{ _erase(_begin()); }

	const cost_functor& cost() const
	{ return impl_.cost_; }

	void clear() noexcept
	{
	_erase(_begin());
	_leftmost() = _end();
	_root() = 0;
	_rightmost() = _end();
	// assert count == 0, _erase should take care of decrementing the node count
	assert(impl_.node_count_ == 0);
	// impl_.node_count_ = 0;
	}

	iterator begin()
	{ return iterator(static_cast<link_type>(this->impl_.header_.left_)); }

	const_iterator begin() const
	{ return const_iterator(static_cast<const_link_type>(this->impl_.header_.left_)); }

	iterator end()
	{ return iterator(static_cast<link_type>(&this->impl_.header_)); }

	const_iterator end() const
	{ return const_iterator(static_cast<const_link_type>(&this->impl_.header_)); }


	reverse_iterator rbegin()
	{ return reverse_iterator(end()); }

	const_reverse_iterator rbegin() const
	{ return const_reverse_iterator(end()); }

	reverse_iterator rend()
	{ return reverse_iterator(begin()); }

	const_reverse_iterator rend() const
	{ return const_reverse_iterator(begin()); }


	leaf_iterator leaves_begin()
	{ return leaf_iterator(static_cast<link_type>(impl_.header_.left_)); }

	const_leaf_iterator leaves_begin() const
	{ return const_leaf_iterator(static_cast<const_link_type>(impl_.header_.left_)); }

	leaf_iterator leaves_end()
	{ return leaf_iterator(static_cast<link_type>(&impl_.header_)); }

	const_leaf_iterator leaves_end() const
	{ return const_leaf_iterator(static_cast<const_link_type>(&impl_.header_)); }

	iterator root()
	{ return iterator(static_cast<link_type>(impl_.header_.parent_)); }

	const_iterator root() const
	{ return const_iterator(static_cast<const_link_type>(impl_.header_.parent_)); }

	bool empty() const
	{ return impl_.node_count_ == 0; }

	size_type size() const
	{ return impl_.node_count_; }

	size_type max_size() const
	{ return _get_Node_allocator().max_size(); }


	std::tuple<size_t, size_t, size_t> height() const {

	size_t count = 0;
	size_t min = std::numeric_limits<size_t>::max();
	size_t max = 0;
	size_t h = 0;
	for (const_leaf_iterator it = leaves_begin(); it != leaves_end(); ++it) {
	const_base_ptr x = it.node_;
	size_t h1 = 0;
	while (x->parent_ != _end()) {
	++h1;
	x = x->parent_;
	}
	min = std::min(min, h1);
	max = std::max(max, h1);
	++count;
	h += h1;
	}

	return std::make_tuple(h/count, min, max);
	}


	private:

	void _add_level(std::queue<link_type>& q, base_ptr x, size_t h, size_t level) {

	if (h < level) {
	if (x->left_ != 0) _add_level(q, x->left_, h+1, level);
	if (x->right_ != 0) _add_level(q, x->right_, h+1, level);
	}
	else if (h == level)
	q.push(static_cast<link_type>(x));
	else
	return;
	}



	iterator _insert(base_ptr l, base_ptr r, bool count = true) {

	#ifdef DEBUG_TREE
	cout<<"inside inserting two for iterators"<<endl;
	cout<<"left pointer "<<l<<endl;
	if (l)
	cout<<"left "<<_value(l)<<endl;
	cout<<"right pointer "<<r<<endl;
	cout<<"right "<<_value(r)<<endl;
	#endif

	// set parent to header to count nodes
	link_type p = _end();
	r->parent_ = p;

	// get left most
	base_ptr lt = r;
	while (lt->left_ != 0)
	lt = lt->left_;

	#ifdef DEBUG_TREE
	cout<<"left most -> "<<_value(lt)<<endl;
	#endif

	// count nodes
	if (count)
	for (iterator it(static_cast<link_type>(lt)); it != end(); ++it)
	++impl_.node_count_;

	// get right most
	base_ptr rt = r;
	while (rt->right_ != 0)
	rt = rt->right_;

	// if l is not null, create a new node
	link_type lr = static_cast<link_type>(r);
	if (l) {

	auto vp = impl_.cost_(_value(l),_value(r));
	lr = _create_node(vp);
	++impl_.node_count_;

	p = _parent(l);
	lr->parent_ = p;
	lr->right_ = r;
	lr->left_ = l;

	r->parent_ = lr;
	l->parent_ = lr;

	} else
	_leftmost() = lt;

	// fix header
	if (lr->parent_ == _end()) {

	_root() = lr;
	_rightmost() = rt;

	} else {
	// fix parent pointers
	if (p->right_ == l)
	p->right_ = lr;
	else if (p->left_ == l)
	p->left_ = lr;
	}

	// recursively fix parents
	auto fix = _value(lr);
	while (lr->parent_ != _end()) {
	p = _parent(lr);
	fix = impl_.cost_(_value(p), fix);
	p->value_ = fix;
	lr = p;
	}

	return iterator(static_cast<link_type>(r));
	}

	bool _is_leaf(const_base_ptr x) const
	{ assert(x != nullptr); return (_right(x) == nullptr) && (_left(x) == nullptr); }



	public:


	struct _Queue_compare {

	// pointer - ancestor expansion - best cost tuples
	typedef std::tuple<link_type, Real, Real> tuple_type;

	bool operator()(const tuple_type& t1, const tuple_type& t2) const
	{ return std::get<1>(t1) > std::get<1>(t2); }
	};

	//! Adapted from Omohundro:89 routine best_sib
	template <typename Arg>
	iterator best_sibling(Arg&& v) {

	#ifdef DEBUG_TREE
	cout<<"--------------------"<<endl;
	cout<<"inserting "<<v<<endl;
	#endif

	typedef typename _Queue_compare::tuple_type tuple_type;
	typedef typename std::priority_queue<tuple_type, std::vector<tuple_type>, _Queue_compare> queue_type;

	// set result to root
	link_type result = _begin();

	// check for empty tree
	if (!result)
	return iterator(nullptr);

	// check for leaf node
	else if (_is_leaf(result))
	return iterator(result);

	// compute combined cost
	Real bcost = impl_.cost_(impl_.cost_(v, _value(result)));

	#ifdef DEBUG_TREE
	cout<<"root -> "<<_value(result)<<endl;
	cout<<"bcost -> "<<bcost<<endl;
	#endif

	// start the queue
	queue_type q;
	decltype(impl_.cost_(v)) e, c;

	q.push(std::make_tuple(result, 0., bcost));

	// while queue is not empty
	while (!q.empty()) {

	// best candidate
	auto t = q.top();
	auto nd = std::get<0>(t);
	auto aexp = std::get<1>(t);
	auto ndvol = std::get<2>(t);
	// remove top most element
	q.pop();

	#ifdef DEBUG_TREE
	cout<<" inside while"<<endl;
	cout<<" top queue item "<<_value(nd)<<endl;
	#endif

	// no way to get better than bnd
	if (aexp >= bcost) {
	#ifdef DEBUG_TREE
	cout<<"*** BREAKING"<<endl;
	#endif
	break;
	}

	// new ancestor expansion
	e = aexp + ndvol - impl_.cost_(_value(nd));

	#ifdef DEBUG_TREE
	cout<<" a exp "<<e<<endl;
	cout<<" bcost "<<bcost<<endl;
	cout<<" cost x "<<impl_.cost_(_value(nd))<<endl;
	cout<<" e -> "<<e<<endl;
	#endif

	// do left node
	c = impl_.cost_(impl_.cost_(v, _value(nd->left_)));
	#ifdef DEBUG_TREE
	cout<<" c left -> "<<c<<endl;
	#endif
	if (c+e <= bcost) {
	bcost = c+e;
	result = _left(nd);
	#ifdef DEBUG_TREE
	cout<<" c+e < bcost!!! result set to left "<<_value(result)<<endl;
	#endif
	}
	if(!_is_leaf(nd->left_))
	q.push(std::make_tuple(_left(nd), e, c));

	// do right node
	c = impl_.cost_(impl_.cost_(v, _value(nd->right_)));
	#ifdef DEBUG_TREE
	cout<<" c right -> "<<c<<endl;
	#endif
	if (c+e <= bcost) {
	bcost = c+e;
	result = _right(nd);
	#ifdef DEBUG_TREE
	cout<<" c+e < bcost!!! result set to right "<<_value(result)<<endl;
	#endif
	}
	if(!_is_leaf(nd->right_))
	q.push(std::make_tuple(_right(nd), e, c));

	} // while queue is not empty

	#ifdef DEBUG_TREE
	cout<<"best sibling -> "<<_value(result)<<endl;
	#endif
	return iterator(result);
	}

	//! Adapted from Omohundro:89 routine cheap_best_sib
	template <typename Arg>
	iterator cheap_best_sibling(Arg&& v) {

	#ifdef DEBUG_TREE
	cout<<"--------------------"<<endl;
	cout<<"inserting "<<v<<endl;
	#endif

	link_type x = _begin();

	if (!x)
	return iterator(nullptr);

	#ifdef DEBUG_TREE
	cout<<"root -> "<<_value(x)<<endl;
	#endif

	link_type result = x;

	// compute combined cost
	Real wv = impl_.cost_(v, _value(x));
	// node vol + ancestor expansion
	Real bcost = wv;
	// ancestor expansion starts at zero
	Real ae = 0;

	#ifdef DEBUG_TREE
	cout<<"bcost -> "<<bcost<<endl;
	#endif

	// while not a leaf node
	while (x->left_ && x->right_) {

	#ifdef DEBUG_TREE
	cout<<" inside while"<<endl;
	#endif
	auto nd = impl_.cost_(_value(x));

	// correct for both children
	ae += wv - nd;

	#ifdef DEBUG_TREE
	cout<<" top queue item "<<_value(x)<<endl;
	cout<<" a exp "<<ae<<endl;
	cout<<" bcost "<<bcost<<endl;
	cout<<" cost x "<<nd<<endl;
	cout<<" e -> "<<ae<<endl;
	#endif

	// cannot do any better, insert the volumes in the tree
	if (ae >= bcost)
	break;
	else {

	auto lv = impl_.cost_(v, _value(x->left_));
	auto rv = impl_.cost_(v, _value(x->right_));

	#ifdef DEBUG_TREE
	cout<<" c left -> "<<lv<<endl;
	#endif
	if (ae + lv <= bcost) {
	bcost = ae + lv;
	result = _left(x);
	#ifdef DEBUG_TREE
	cout<<" c+e < bcost!!! result set to left "<<_value(_left(x))<<endl;
	#endif
	}

	#ifdef DEBUG_TREE
	cout<<" c right -> "<<rv<<endl;
	#endif

	if (ae + rv <= bcost) {
	bcost = ae + rv;
	result = _right(x);
	#ifdef DEBUG_TREE
	cout<<" c+e < bcost!!! result set to right "<<_value(_right(x))<<endl;
	#endif
	}

	// if left branch expands less
	if (lv - impl_.cost_(_value(x->left_)) <=
	rv - impl_.cost_(_value(x->right_))) {
	wv = lv;
	x = _left(x);
	} else {
	wv = rv;
	x = _right(x);
	}
	}
	} // while loop

	#ifdef DEBUG_TREE
	cout<<"best sibling -> "<<_value(result)<<endl;
	#endif
	return iterator(result);
	}

	private:

	template <class functor_type>
	void _execute_at_leaves(iterator x, std::set<base_ptr>& visited, functor_type& fn) {

	auto it = visited.find(x.node_);
	if (it != visited.end())
	return;

	visited.insert(x.node_);

	if (x.is_leaf())
	fn(x);
	else {
	if (x.node_->left_ != 0) _execute_at_leaves(iterator(static_cast<link_type>(x.node_->left_)), visited, fn);
	if (x.node_->right_ != 0) _execute_at_leaves(iterator(static_cast<link_type>(x.node_->right_)), visited, fn);
	}
	}

	public:

	//! Collect neighbors
	/*! This function is used to collect neighbors from a tree given a functor.
	* The functor should implement a parameterless and a one-parameter operator() overloads.
	* The first load is used as the predicate to finish the search of neighbors, whereas
	* the second one is executed at leaves.
	*/
	template <class functor_type>
	void collect_neighbors(iterator it, functor_type& fn) {

	// check for null ptr
	if (it.node_ == nullptr)
	return;

	std::set<base_ptr> visited;

	base_ptr p = it.node_;

	// execute while predicate is not true
	while (!fn()) {

	p = p->parent_;

	// root node
	if (p == _root()->parent_)
	break;

	_execute_at_leaves(iterator(static_cast<link_type>(p)), visited, fn);
	}
	}

	template <typename Arg>
	std::pair<iterator,bool> cheap_insert(Arg&& v) {

	iterator it = cheap_best_sibling(v);
	link_type r = _create_node(v);

	return std::pair<iterator, bool>(_insert(it.node_, r), true);
	}


	template <typename Arg>
	std::pair<iterator,bool> insert(Arg&& v) {

	iterator it = best_sibling(v);
	link_type r = _create_node(v);

	return std::pair<iterator, bool>(_insert(it.node_, r), true);
	}


	std::pair<iterator,bool> insert(link_type x)
	{ return insert(iterator(x)); }

	std::pair<iterator,bool> insert(iterator vit) {

	iterator it = best_sibling(_value(vit.node_));
	return std::pair<iterator, bool>(_insert(it.node_, vit.node_), true);
	}


	iterator sibling(iterator it) {

	base_ptr x = it.node_;
	if (!x)
	return iterator(nullptr);
	if (x == _root())
	return end();

	base_ptr p = x->parent_;
	assert (p != 0);
	if (p->left_ == x)
	return iterator(_right(p));
	return iterator(_left(p));
	}

	public:

	iterator relocate_parent(iterator it) {

	#if DEBUG_TREE
	if (it.node_)
	cout<<"relocating node "<<_value(it.node_)<<endl;
	#endif

	link_type x = static_cast<link_type>(it.node_);


	// null pointer
	if (!x)
	return end();

	// get parent
	link_type p = _parent(x);

	// if parent is header return root
	if (p == _end())
	return iterator(_begin());

	// get grand-parent
	link_type gp = _parent(p);

	// no need to recompute volume as the parent has
	// the enclosing volume of its children

	// sibling
	link_type s = x == p->left_ ? _right(p) : _left(p);

	#ifdef DEBUG_TREE
	cout<<"relocating x -> "<<_value(x)<<endl;
	cout<<"parent of node -> "<<_value(p)<<endl;
	cout<<"sibling -> "<<_value(s)<<endl;
	#endif

	if (gp == _end()) {
	#ifdef DEBUG_TREE
	cout<<"* WARNING * sibling new parent is the header!"<<endl;
	#endif
	_root() = s;
	} else if (gp->left_ == p) {
	gp->left_ = s;
	#ifdef DEBUG_TREE
	cout<<"setting parent->parent->left to sibling"<<endl;
	#endif
	}
	else if (gp->right_ == p) {
	gp->right_ = s;
	#ifdef DEBUG_TREE
	cout<<"setting parent->parent->right to sibling"<<endl;
	#endif
	} else
	assert(false);

	// set parent of sibling
	s->parent_ = p->parent_;

	// recursively fix parents
	link_type r = _parent(s);
	while (r != _end()) {
	r->value_ = impl_.cost_(_value(r->right_), _value(r->left_));
	r = _parent(r);
	}

	// destroy parent node p and reinsert x into the tree
	x->parent_ = 0;
	p->parent_ = 0;
	p->left_ = 0;
	p->right_ = 0;
	_erase(p);

	// get the best sibling
	iterator bit = best_sibling(_value(x));

	#ifdef DEBUG_TREE
	cout<<"best sibling for relocation -> "<<bit.node_<<endl;
	if (it.node_)
	cout<<" best sibling value -> "<<_value(bit.node_)<<endl;
	#endif

	iterator result = _insert(bit.node_, x, false);

	// set leftmost and rightmost
	base_ptr lt = _root();
	while (lt->left_ != 0)
	lt = lt->left_;
	_leftmost() = lt;

	base_ptr rt = _root();
	while (rt->right_ != 0)
	rt = rt->right_;
	_rightmost() = rt;

	return result;
	}


	iterator erase(iterator it) {

	link_type x = static_cast<link_type>(it.node_);

	// null pointer
	if (!x)
	return end();

	// root node
	else if (x == _root()) {
	clear();
	return end();
	}

	link_type p = _parent(x);

	link_type s; // sibling
	if (x == p->left_) {
	s = _right(p);
	p->right_ = 0; // set to zero to use erase
	} else {
	s = _left(p);
	p->left_ = 0;
	}

	#ifdef DEBUG_TREE
	cout<<"erasing x -> "<<_value(x)<<endl;
	cout<<"parent of node -> "<<_value(p)<<endl;
	cout<<"sibling -> "<<_value(s)<<endl;
	#endif

	if (_leftmost() == x) {
	base_ptr l = s;
	while (l->left_ != 0)
	l = l->left_;
	_leftmost() = l;
	#ifdef DEBUG_TREE
	cout<<"x is leftmost, so setting leftmost to "<<_value(l)<<endl;
	#endif
	} if (_rightmost() == x) {
	base_ptr r = s;
	while (r->right_ != 0)
	r = r->right_;
	_rightmost() = r;
	#ifdef DEBUG_TREE
	cout<<"x is rightmost, so setting rightmost to "<<_value(r)<<endl;
	#endif
	}

	s->parent_ = p->parent_;
	if (p->parent_ == _end()) {
	#ifdef DEBUG_TREE
	cout<<"* WARNING * sibling new parent is the header!"<<endl;
	#endif
	_root() = s;
	} else if (p->parent_->left_ == p) {
	p->parent_->left_ = s;
	#ifdef DEBUG_TREE
	cout<<"setting parent->parent->left to sibling"<<endl;
	#endif
	}
	else {
	p->parent_->right_ = s;
	#ifdef DEBUG_TREE
	cout<<"setting parent->parent->right to sibling"<<endl;
	#endif
	}


	#ifdef DEBUG_TREE
	cout<<"p right -> "<<p->right_<<endl;
	cout<<"p left -> "<<p->left_<<endl;
	if (p->right_)
	cout<<"p right value -> "<<_value(p->right_)<<endl;
	if (p->left_)
	cout<<"p left value -> "<<_value(p->left_)<<endl;
	#endif

	// recursively fix parents
	link_type r = _parent(s);
	while (r != _end()) {
	r->value_ = _value(r->right_) + _value(r->left_);
	r = _parent(r);
	}

	// erase subtree rooted at p
	_erase(p);

	return iterator(s);
	}



	Real volume() const {
	Real v = 0;
	for (const_iterator it = begin(); it != end(); ++it)
	v += it->measure();
	return v;
	}


	friend std::ostream& operator<<(std::ostream& os, const Tree& t) {

	os<<"Tree: count "<<t.impl_.node_count_<<endl;
	os<<" header: "<<&t.impl_.header_<<endl;
	if (t.impl_.header_.parent_)
	os<<" ROOT: "<<t.impl_.header_.parent_<<", value "<<_value(t.impl_.header_.parent_)<<endl;
	os<<" left: "<<t.impl_.header_.left_<<endl;
	os<<" right: "<<t.impl_.header_.right_<<endl;
	size_t c = 0;
	Real v = 0;
	os<<endl;
	for (Tree::const_iterator it = t.begin(); it != t.end(); ++it, ++c) {

	Real cost = t.impl_.cost_(*it);

	if (it.node_ == t.impl_.header_.parent_) {
	os<<" ROOT "<<it.node_;
	if (it.node_ == t._leftmost())
	os<<" [LEFTMOST]";
	if (it.node_ == t._rightmost())
	os<<" [RIGHTMOST]";
	os<<"\n value: "<<*it<<endl;
	os<<" parent: "<<it.node_->parent_<<endl;
	} else {
	os<<" pointer "<<it.node_;
	if (it.node_ == t._leftmost())
	os<<" [LEFTMOST]";
	if (it.node_ == t._rightmost())
	os<<" [RIGHTMOST]";
	os<<"\n value: "<<*it<<endl;
	os<<" parent: "<<it.node_->parent_<<", value "<<_value(it.node_->parent_)<<endl;
	}

	if (it.node_->left_)
	os<<" left: "<<it.node_->left_<<", value "<<_value(it.node_->left_)<<endl;
	if (it.node_->right_)
	os<<" right: "<<it.node_->right_<<", value "<<_value(it.node_->right_)<<endl;
	if (!it.node_->left_ && !it.node_->right_)
	os<<" leaf"<<endl;
	os<<" cost: "<<cost<<endl;
	os<<endl;

	v += cost;
	}
	cout<<"total cost -> "<<v<<endl;

	if (c != t.impl_.node_count_)
	cout<<"* WARNING * Tree node count mismatch "<<t.impl_.node_count_<<" != "<<c<<endl;
	return os;
	}

	template <class functor_type>
	functor_type execute_at_level(size_t level, functor_type fn) {

	link_type x = _begin();
	if (x == nullptr)
	return fn;

	std::queue<link_type> q;
	_add_level(q, x, 0, level);

	while (!q.empty()) {
	link_type l = q.front();
	assert(l != 0);
	fn(iterator(l));
	q.pop();
	}
	return fn;
	}
	};





	template <class object_type>
	struct Cost_functor {

	auto operator()(const object_type& o) const -> decltype(o.measure())
	{ return o.measure(); }

	auto operator()(const object_type& o1, const object_type& o2) const -> decltype(o1+o2)
	{ return (o1+o2); }

	};


	template <class link_type>
	struct Tuple_compare {

	typedef std::tuple<link_type, link_type, Real> tuple_type;

	bool operator()(const tuple_type& t1, const tuple_type& t2) const
	{ return std::get<2>(t1) > std::get<2>(t2); }
	};

	// bottom up tree construction
	template <class tree_type, class volume_container>
	tree_type* construct_tree_bottom_up(const volume_container& volumes)
	{
	// online tree type definitions
	typedef typename tree_type::value_type volume_type;
	typedef typename tree_type::cost_functor cost_functor;
	typedef typename tree_type::iterator iterator_type;

	// queue type definitions
	typedef std::tuple<iterator_type, iterator_type, Real> tuple_type;
	typedef typename std::priority_queue<tuple_type, std::vector<tuple_type>, Tuple_compare<iterator_type> > queue_type;

	// already paired container type definitions
	typedef std::set<iterator_type> paired_set;
	typedef typename paired_set::iterator paired_iterator;

	size_t numVolumes = volumes.size();
	assert(numVolumes != 0);

	tree_type* tp = new tree_type();
	const cost_functor& cost = tp->cost();
	tree_type& t = *tp;
	queue_type q;
	paired_set s;

	std::vector<iterator_type> links(numVolumes);

	// insert volumes into online tree
	for (size_t i=0; i<numVolumes; ++i) {
	std::pair<iterator_type, bool> p = t.insert(volumes[i]);
	assert(p.second);
	links[i] = p.first;
	}

	iterator_type ii,jj;
	for (size_t i=0; i<numVolumes-1; ++i) {

	Real merged = std::numeric_limits<Real>::infinity();

	for (size_t j=i+1; j<numVolumes; ++j) {

	volume_type v = cost(links[i], links[j]);
	Real measure = cost(v);
	if (measure < merged) {
	ii = links[i];
	jj = links[j];
	merged = measure;
	}
	}
	q.push(std::make_tuple(ii,jj,merged));
	}

	while (!q.empty()) {

	// best candidate
	auto tuple = q.top();
	iterator_type node = std::get<0>(tuple);
	iterator_type pair = std::get<1>(tuple);

	q.pop();

	paired_iterator it = s.find(node);
	if (it != s.end())
	continue;

	it = s.find(pair);
	if (it != s.end())
	continue;

	#if DEBUG_TREE
	cout<<t<<endl;
	cout<<"node -> "<<node.node_<<endl;
	cout<<"node value -> "<<*node<<endl;
	cout<<"pair -> "<<pair.node_<<endl;
	cout<<"pair value -> "<<*pair<<endl;
	cout<<"volume -> "<<std::get<2>(tuple)<<endl;

	#endif

	// obtain best pair from online tree
	iterator_type best = t.sibling(node);
	#if DEBUG_TREE
	cout<<"best sibling -> "<<*best<<endl;
	#endif
	if (pair == best) {

	iterator_type parent = t.relocate_parent(node);

	assert(parent != t.end());

	#if DEBUG_TREE
	cout<<"BEST SIBLING MATCHES! relocating..."<<endl;
	cout<<" parent -> "<<*parent<<endl;
	#endif

	// add paired nodes to container
	s.insert(node);
	s.insert(pair);

	// compute best pair for the parent
	best = t.sibling(parent);

	if (best != t.end()) {

	tuple_type tuple = std::make_tuple(parent, best, cost(cost(parent, best)));
	q.push(tuple);

	iterator_type node = std::get<0>(tuple);
	iterator_type pair = std::get<1>(tuple);

	#if DEBUG_TREE
	cout<<" pushing new tuple "<<endl;
	cout<<" node -> "<<*node<<endl;
	cout<<" pair -> "<<*pair<<endl;
	cout<<" volume -> "<<std::get<2>(tuple)<<endl;
	#endif
	}
	}
	// else enqueue new pair
	else {

	if (best != t.end()) {

	tuple_type tuple = std::make_tuple(node,best, cost(cost(node, best)));
	q.push(tuple);

	iterator_type node = std::get<0>(tuple);
	iterator_type pair = std::get<1>(tuple);

	#if DEBUG_TREE
	cout<<"BEST SIBLING DOES NOT MATCH!"<<endl;
	cout<<" pushing new tuple "<<endl;
	cout<<" node -> "<<*node<<endl;
	cout<<" pair -> "<<*pair<<endl;
	cout<<" volume -> "<<std::get<2>(tuple)<<endl;
	#endif
	}

	}
	}

	return tp;
	}



	template <class volume_type>
	class Volume_creator;

	template<int d>
	struct Volume_creator<Ball<d> > {

	typedef typename Ball<d>::point_type point_type;

	template <class coord_array>
	static Ball<d> create(const coord_array& coord) {

	std::vector<point_type> pts;
	size_t nnodes = coord.size();
	pts.reserve(nnodes);
	for (size_t i=0; i<nnodes; ++i)
	pts.push_back(point_type(coord[i]));
	return bounding_ball<d>(pts);
	}
	};

	template <int d>
	struct Volume_creator<BoundingBox<d> > {

	typedef typename BoundingBox<d>::point_type point_type;

	template <class coord_array>
	static Sphere create(const coord_array& coord) {

	std::vector<point_type> pts;
	size_t nnodes = coord.size();
	pts.reserve(nnodes);
	for (size_t i=0; i<nnodes; ++i)
	pts.push_back(point_type(coord[i]));
	return BoundingBox<d>(pts.begin(), pts.end());
	}
	};



	// bottom up tree construction
	template <class tree_type, class model_type, class element_type>
	tree_type* construct_tree_bottom_up(model_type& model) {

	// online tree type definitions
	typedef typename tree_type::value_type volume_type;
	typedef typename tree_type::cost_functor cost_functor;
	typedef typename tree_type::iterator iterator_type;

	// queue type definitions
	typedef std::tuple<iterator_type, iterator_type, Real> tuple_type;
	typedef typename std::priority_queue<tuple_type, std::vector<tuple_type>, Tuple_compare<iterator_type> > queue_type;

	// already paired container type definitions
	typedef std::set<iterator_type> paired_set;
	typedef typename paired_set::iterator paired_iterator;

	tree_type* tp = new tree_type();
	tree_type& t = *tp;
	const cost_functor& cost = t.cost();
	std::vector<iterator_type> links;

	typedef typename model_type::mesh_type mesh_type;

	mesh_type& mesh = model.getMesh();
	int dim = mesh.getSpatialDimension();

	// iterate over elements of lower dimension
	typename mesh_type::type_iterator it = mesh.firstType(dim-1);
	typename mesh_type::type_iterator end = mesh.lastType(dim-1);

	size_t numVolumes = 0;
	for(; it != end; ++it) {

	// add elements to corresponding surface
	for(UInt e = 0; e < mesh.getNbElement(*it); ++e) {

	// create solid mechanics element
	element_type el(model, *it, e);

	// vector of coordinates for update
	std::vector<const Real*> coord = el.coordinates();

	// create leaf volume
	volume_type v = Volume_creator<volume_type>::create(coord);

	// do not add zero measure volumes
	if (v.measure() == 0 && dim != 1)
	continue;

	std::pair<iterator_type, bool> p = t.insert(v);
	assert(p.second);
	links.push_back(p.first);
	++numVolumes;

	// save leaf data
	if (!t.add_data(p.first,el)) {
	cout<<"* ERROR * Could not add data. Aborting..."<<endl;
	exit(1);
	}


	} // loop over elements
	} // loop over types of element

	if (numVolumes == 0) {
	cout<<"* ERROR * No volumes were created. Aborting..."<<endl;
	exit(1);
	}

	queue_type q;
	paired_set s;

	iterator_type ii,jj;
	for (size_t i=0; i<numVolumes-1; ++i) {

	Real merged = std::numeric_limits<Real>::infinity();

	for (size_t j=i+1; j<numVolumes; ++j) {

	volume_type v = cost(links[i], links[j]);
	Real measure = v.measure();
	if (measure < merged) {
	ii = links[i];
	jj = links[j];
	merged = measure;
	}
	}
	q.push(std::make_tuple(ii,jj,merged));
	}

	while (!q.empty()) {

	// best candidate
	auto tuple = q.top();
	iterator_type node = std::get<0>(tuple);
	iterator_type pair = std::get<1>(tuple);

	q.pop();

	paired_iterator it = s.find(node);
	if (it != s.end())
	continue;

	it = s.find(pair);
	if (it != s.end())
	continue;

	#if DEBUG_TREE
	cout<<t<<endl;
	cout<<"node -> "<<node.node_<<endl;
	cout<<"node value -> "<<*node<<endl;
	cout<<"pair -> "<<pair.node_<<endl;
	cout<<"pair value -> "<<*pair<<endl;
	cout<<"volume -> "<<std::get<2>(tuple)<<endl;

	#endif

	// obtain best pair from online tree
	iterator_type best = t.sibling(node);
	#if DEBUG_TREE
	cout<<"best sibling -> "<<*best<<endl;
	#endif
	if (pair == best) {

	iterator_type parent = t.relocate_parent(node);

	assert(parent != t.end());

	#if DEBUG_TREE
	cout<<"BEST SIBLING MATCHES! relocating..."<<endl;
	cout<<" parent -> "<<*parent<<endl;
	#endif

	// add paired nodes to container
	s.insert(node);
	s.insert(pair);

	// compute best pair for the parent
	best = t.sibling(parent);

	if (best != t.end()) {

	tuple_type tuple = std::make_tuple(parent, best, cost(cost(parent,best)));
	q.push(tuple);

	iterator_type node = std::get<0>(tuple);
	iterator_type pair = std::get<1>(tuple);

	#if DEBUG_TREE
	cout<<" pushing new tuple "<<endl;
	cout<<" node -> "<<*node<<endl;
	cout<<" pair -> "<<*pair<<endl;
	cout<<" volume -> "<<std::get<2>(tuple)<<endl;
	#endif
	}
	}
	// else enqueue new pair
	else {

	if (best != t.end()) {

	tuple_type tuple = std::make_tuple(node,best, cost(cost(node, best)));
	q.push(tuple);

	iterator_type node = std::get<0>(tuple);
	iterator_type pair = std::get<1>(tuple);

	#if DEBUG_TREE
	cout<<"BEST SIBLING DOES NOT MATCH!"<<endl;
	cout<<" pushing new tuple "<<endl;
	cout<<" node -> "<<*node<<endl;
	cout<<" pair -> "<<*pair<<endl;
	cout<<" volume -> "<<std::get<2>(tuple)<<endl;
	#endif
	}

	}
	}

	return tp;
	}


	// bottom up tree construction
	template <class tree_type, class element_container>
	tree_type* construct_tree_bottom_up(element_container& elems) {

	// element type
	typedef typename element_container::value_type element_type;

	// online tree type definitions
	typedef typename tree_type::value_type volume_type;
	typedef typename tree_type::cost_functor cost_functor;
	typedef typename tree_type::iterator iterator_type;

	// queue type definitions
	typedef std::tuple<iterator_type, iterator_type, Real> tuple_type;
	typedef typename std::priority_queue<tuple_type, std::vector<tuple_type>, Tuple_compare<iterator_type> > queue_type;

	// already paired container type definitions
	typedef std::set<iterator_type> paired_set;
	typedef typename paired_set::iterator paired_iterator;

	tree_type* tp = new tree_type();
	tree_type& t = *tp;
	const cost_functor& cost = t.cost();
	std::vector<iterator_type> links;

	size_t numVolumes = 0;

	for (typename element_container::iterator it = elems.begin();
	it != elems.end(); ++it) {

	element_type &el = *it;

	// vector of coordinates for update
	std::vector<const Real*> coord = el.coordinates();

	// create leaf volume
	volume_type v = Volume_creator<volume_type>::create(coord);

	// do not add zero measure volumes
	if (v.measure() == 0)
	continue;

	std::pair<iterator_type, bool> p = t.insert(v);
	assert(p.second);
	links.push_back(p.first);
	++numVolumes;

	// save leaf data
	if (!t.add_data(p.first,el)) {
	cout<<"* ERROR * Could not add data. Aborting..."<<endl;
	exit(1);
	}

	} // loop over elements in container

	if (numVolumes == 0) {
	cout<<"* ERROR * No volumes were created. Aborting..."<<endl;
	exit(1);
	}

	queue_type q;
	paired_set s;

	iterator_type ii,jj;
	for (size_t i=0; i<numVolumes-1; ++i) {

	Real merged = std::numeric_limits<Real>::infinity();

	for (size_t j=i+1; j<numVolumes; ++j) {

	volume_type v = cost(links[i], links[j]);
	Real measure = v.measure();
	if (measure < merged) {
	ii = links[i];
	jj = links[j];
	merged = measure;
	}
	}
	q.push(std::make_tuple(ii,jj,merged));
	}

	while (!q.empty()) {

	// best candidate
	auto tuple = q.top();
	iterator_type node = std::get<0>(tuple);
	iterator_type pair = std::get<1>(tuple);

	q.pop();

	paired_iterator it = s.find(node);
	if (it != s.end())
	continue;

	it = s.find(pair);
	if (it != s.end())
	continue;

	#if DEBUG_TREE
	cout<<t<<endl;
	cout<<"node -> "<<node.node_<<endl;
	cout<<"node value -> "<<*node<<endl;
	cout<<"pair -> "<<pair.node_<<endl;
	cout<<"pair value -> "<<*pair<<endl;
	cout<<"volume -> "<<std::get<2>(tuple)<<endl;

	#endif

	// obtain best pair from online tree
	iterator_type best = t.sibling(node);
	#if DEBUG_TREE
	cout<<"best sibling -> "<<*best<<endl;
	#endif
	if (pair == best) {

	iterator_type parent = t.relocate_parent(node);

	assert(parent != t.end());

	#if DEBUG_TREE
	cout<<"BEST SIBLING MATCHES! relocating..."<<endl;
	cout<<" parent -> "<<*parent<<endl;
	#endif

	// add paired nodes to container
	s.insert(node);
	s.insert(pair);

	// compute best pair for the parent
	best = t.sibling(parent);

	if (best != t.end()) {

	tuple_type tuple = std::make_tuple(parent, best, cost(cost(parent,best)));
	q.push(tuple);

	iterator_type node = std::get<0>(tuple);
	iterator_type pair = std::get<1>(tuple);

	#if DEBUG_TREE
	cout<<" pushing new tuple "<<endl;
	cout<<" node -> "<<*node<<endl;
	cout<<" pair -> "<<*pair<<endl;
	cout<<" volume -> "<<std::get<2>(tuple)<<endl;
	#endif
	}
	}
	// else enqueue new pair
	else {

	if (best != t.end()) {

	tuple_type tuple = std::make_tuple(node,best, cost(cost(node, best)));
	q.push(tuple);

	iterator_type node = std::get<0>(tuple);
	iterator_type pair = std::get<1>(tuple);

	#if DEBUG_TREE
	cout<<"BEST SIBLING DOES NOT MATCH!"<<endl;
	cout<<" pushing new tuple "<<endl;
	cout<<" node -> "<<*node<<endl;
	cout<<" pair -> "<<*pair<<endl;
	cout<<" volume -> "<<std::get<2>(tuple)<<endl;
	#endif
	}

	}
	}

	return tp;
	}




	template <class tree_type>
	void print_mathematica(tree_type& t) {

	typedef typename tree_type::value_type volume_type;

	int d = volume_type::dim();

	cout<<std::fixed;
	cout.precision(2);

	if (d == 2)
	cout<<"Graphics[{";
	else
	cout<<"Graphics3D[{";

	for (typename tree_type::const_leaf_iterator it = t.leaves_begin(); it != t.leaves_end(); ++it)
	cout<<*it<<", ";

	cout<<"}];"<<endl;
	}


	template <typename T>
	std::string stringify(const T& x) {
	std::stringstream o;
	if (!(o << x)) {
	cout<<"* ERROR * Bad argument to stringity function"<<endl;
	exit(1);
	}
	return o.str();
	}

	template <class tree>
	void export_video(tree& t, Real duration, Real min_opacity, std::string pre = "", std::string post = "") {

	typedef typename tree::value_type volume_type;
	typedef typename volume_type::aabb_type aabb_type;
	typedef typename aabb_type::point_type point_type;
	typedef typename point_type::value_type value_type;
	typedef typename tree::iterator iterator;

	int d = volume_type::dim();
	cout<<std::fixed;
	cout.precision(2);

	std::tuple<size_t, size_t, size_t> h = t.height();

	typename tree::iterator it = t.begin();

	aabb_type bb = it->bounding_box();

	// get bounding box
	for (; it != t.end(); ++it)
	bb += it->bounding_box();

	std::string figs;
	int H = std::get<2>(h);
	int k = 0;
	for (int l = H; l>=0; --l) {

	std::string g = "g";
	g += stringify(k);
	++k;

	// compute opacity for figure
	Real opacity = (1 - min_opacity)*static_cast<double>(l)/H + min_opacity;

	if (d == 2)
	cout<<g<<" = Graphics[{Opacity["<<stringify(opacity)<<"],"<<pre;
	else
	cout<<g<<" = Graphics3D[{Opacity["<<stringify(opacity)<<"],"<<pre;

	t.execute_at_level(l, [](iterator it){ cout<<*it<<", ";});

	figs += figs.empty() ? g : (','+g);

	cout<<"}];\nShow["<<figs<<", PlotRange-> {{";
	const point_type &m = bb.min();
	const point_type &M = bb.max();
	for (int i=0; i<d; ++i) {
	value_type l = 0.05*(M[i] - m[i]);
	cout<<(m[i]-l)<<','<<(M[i]+l)<<(i < d-1 ? "},{" : "}}");
	}
	cout<<post<<"];\n";
	cout<<"Export[\""<<g<<".png\", %, ImageResolution -> 300];"<<endl;
	}

	cout<<"files = Last /@ Sort[{Characters@#, #} & /@ FileNames[\"*.png\"]]"<<endl;
	cout<<"images = Map[Import, files];"<<endl;
	cout<<"Export[\"video.mov\", images, \"FrameRate\" -> "<<stringify(static_cast<double>(k)/duration)<<"];"<<endl;
	}


	//// bottom up tree construction
	//template <class volume_container>
	//Tree<typename volume_container::value_type, Cost_functor<typename volume_container::value_type> >
	//construct_tree_bottom_up2(const volume_container& volumes)
	//{
	//
	// typedef typename volume_container::value_type volume_type;
	//
	// // online tree type definitions
	// typedef Tree<volume_type, Cost_functor<volume_type> > tree_type;
	//
	// typedef Tree_node<volume_type> node_type;
	// typedef typename tree_type::link_type link_type;
	//
	// size_t numVolumes = volumes.size();
	// assert(numVolumes != 0);
	//
	// // allocate leafs of the tree
	// link_type* leafs = new link_type[numVolumes];
	//
	// for (size_t i=0; i<numVolumes; ++i)
	// leafs[i] = new node_type(volumes[i]);
	//
	// while (numVolumes > 1) {
	//
	// // find indices of the two "nearest" nodes, based on some criterion
	// size_t ii,jj;
	// Real merged = std::numeric_limits<Real>::infinity();
	// for (size_t i=0; i<numVolumes; ++i)
	// for (size_t j=i+1; j<numVolumes; ++j) {
	//
	// volume_type v = leafs[i]->value_ + leafs[j]->value_;
	// if (v.measure() < merged) {
	// ii = i;
	// jj = j;
	// merged = v.measure();
	// }
	// }
	//
	// volume_type v = leafs[ii]->value_ + leafs[jj]->value_;
	// link_type parent = new node_type(v);
	//
	//#if DEBUG_TREE
	// cout<<"best volume found -> "<<v<<endl;
	// cout<<"measure -> "<<v.measure()<<endl;
	// cout<<"parent -> "<<parent->value_<<endl;
	// cout<<"left -> "<<leafs[ii]->value_<<endl;
	// cout<<"right -> "<<leafs[jj]->value_<<endl;
	//#endif
	//
	// parent->left_ = leafs[ii];
	// parent->right_ = leafs[jj];
	// leafs[ii]->parent_ = parent;
	// leafs[jj]->parent_ = parent;
	//
	// // remove the two nodes from the active set and add in the new node.
	// leafs[ii] = parent;
	// leafs[jj] = leafs[numVolumes - 1];
	// --numVolumes;
	// }
	//
	// link_type root = leafs[0];
	//
	// tree_type t(root);
	//
	// delete leafs;
	//
	// return t;
	//}

	__END_AKANTU__

	#endif /* __AKANTU_TREE_HH__ */

aka_tree.hhNo OneTemporaryActions

File Metadata

aka_tree.hhView Options

Event Timeline

aka_tree.hh
No OneTemporary
Actions

aka_tree.hh
View Options