contact_manager0.hh
No OneTemporary
Actions

Subscribers

None

File Metadata

Created: Sun, Jun 2, 13:43

contact_manager0.hh
View Options

	/**
	* @file contact_manager0.hh
	*
	* @author Alejandro M. Aragón <alejandro.aragon@epfl.ch>
	*
	* @date creation: Tue May 13 2014
	* @date last modification: Tue May 13 2014
	*
	* @brief zeroth level of contact (simplest implementation)
	*
	* @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_CONTACT_MANAGER_0_HH__
	#define __AKANTU_CONTACT_MANAGER_0_HH__

	#include <unordered_set>


	//#include "aka_common.hh"
	#include "model_manager.hh"

	#define DEBUG_MANAGER 1


	__BEGIN_AKANTU__


	template <class pair_type>
	class PairComp {

	public:
	bool operator()(pair_type const &a, pair_type const &b) {
	return a.first->first < b.first->first
	\|\| (!(b.first->first < a.first->first) && a.second->first < b.second->first);
	}
	};







	template <class Bounding_policy, Discretization_type DT, Consider_acceleration accel = Consider_t, template <class> class Cost_policy = Cost_functor>
	class Contact0_model_manager : public Model_manager<SolidMechanicsModel>, public Kinematic_traits<accel> {


	using Kinematic_traits<accel>::resolve_time;

	typedef Real time_type;
	typedef ctimer chronograph_type;

	// model type
	typedef SolidMechanicsModel model_type;
	typedef model_type* model_pointer;
	typedef model_type& model_reference;

	// geometric types
	typedef Bounding_policy volume_type;
	typedef typename volume_type::point_type point_type;
	typedef typename point_type::value_type value_type;
	typedef typename volume_type::aabb_type aabb_type;

	// element type
	typedef ModelElement<model_type> element_type;

	// Bounding volume hierarchy related types
	typedef Cost_policy<volume_type> cost_functor;
	typedef DataTree<volume_type, element_type, Cost_policy > tree_type;
	typedef typename tree_type::leaves_data_iterator leaves_data_iterator;
	typedef typename tree_type::leaf_iterator tree_leaf_iterator;
	typedef typename tree_type::iterator tree_iterator;
	typedef typename tree_type::const_iterator const_tree_iterator;
	typedef std::list<tree_type*> forest_container;
	typedef typename forest_container::iterator forest_iterator;
	typedef typename forest_container::const_iterator const_forest_iterator;

	// data structure for detailed check
	typedef std::pair<leaves_data_iterator, leaves_data_iterator> check_type;


	// typedef std::unordered_set<check_type, PairComp2<check_type> > check_container;
	typedef std::set<check_type, PairComp<check_type> > check_container;
	typedef typename check_container::iterator check_iterator;

	// kinematic types
	typedef point_type velocity_type;
	typedef std::list<velocity_type> velocity_container;
	typedef typename velocity_container::iterator velocity_iterator;

	typedef unsigned long mask_size;

	// timer type
	typedef std::priority_queue<time_type, std::vector<time_type>, std::greater<time_type> > timer_type;

	// tuple type
	typedef std::tuple<time_type, tree_iterator, tree_iterator> tuple_type;

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

	typedef typename std::priority_queue<tuple_type, std::vector<tuple_type>, Tuple_compare> hierarchy_timer;


	//! Structure used to do a postorder update of tree hierarchies
	struct Updater {

	tree_type &t_; //!< Reference to hierarchy

	Updater(tree_type& t) : t_(t) {}

	void operator()(tree_iterator it) {
	if (!it.is_leaf()) {
	volume_type& v = *it;
	volume_type& lv = *it.left();
	volume_type& rv = *it.right();
	v = lv + rv;
	assert(lv.last_time_ == rv.last_time_);
	v.last_time_ = lv.last_time_;
	v.velocity_ = 0.5 * (lv.velocity_ + rv.velocity_);
	v.acceleration_ = 0.5 * (lv.acceleration_ + rv.acceleration_);
	}
	}
	};

	struct Printer {
	void operator()(tree_iterator it)
	{ cout<<*it<<", "; }
	};

	class Time_exception : public std::exception {
	virtual const char* what() const throw()
	{ return "* EXCEPTION * Zero time increment."; }
	};

	struct Continuator : public std::exception {

	tuple_type best_;

	Continuator(const tuple_type& b) : best_(b) {}

	virtual const char* what() const throw()
	{ return "* EXCEPTION * Continue."; }
	};

	template <bool flag>
	struct Bool2Type {
	enum { value = flag };
	};

	private:

	forest_container forest_; //!< Bounding volume hierarchies
	hierarchy_timer timer_; //!< Priority queue of times
	mask_size masks_; //!< Variable used for static objects
	tree_iterator null_;
	time_type last_; //!< Keep time of last detection engine reset
	check_container detailed_;
	bool detect_;

	//! Data structure that holds contact data
	struct ContactData {

	typedef SolidMechanicsModel model_type;
	typedef ModelElement<model_type> element_type;

	typedef std::set<UInt> node_set;

	struct Comparator {
	template <class iterator>
	bool operator()(iterator const &a, iterator const &b)
	{ return a->first < b->first; }
	};

	typedef std::set<leaves_data_iterator, Comparator> elem_set;

	leaves_data_iterator left_;
	leaves_data_iterator right_;

	mutable elem_set ssurface_;
	mutable elem_set msurface_;
	mutable node_set slaves_;

	mutable tree_type *stree_;
	mutable tree_type *mtree_;

	ContactData(leaves_data_iterator l, leaves_data_iterator r) : left_(l), right_(r), ssurface_(), msurface_(), slaves_(), stree_(nullptr), mtree_(nullptr) {}

	template <class neighbor_type>
	void initialize(neighbor_type& ln, neighbor_type& rn) const {

	stree_ = ln.tree_;
	mtree_ = rn.tree_;

	// insert slave and master elements
	for (auto it = ln.elems_.begin(); it != ln.elems_.end(); ++it)
	ssurface_.insert(*it);
	for (auto it = rn.elems_.begin(); it != rn.elems_.end(); ++it)
	msurface_.insert(*it);
	}



	bool operator<(const ContactData& cd) const {

	return this->left_->first < cd.left_->first
	\|\| (!(cd.left_->first < this->left_->first) && this->right_->first < cd.right_->first);
	}


	friend std::ostream& operator<<(std::ostream& os, const ContactData& cd) {

	os<<"Contact data info:"<<endl;
	os<<" "<<cd.slaves_.size()<<" slave nodes";
	// for (std::set<UInt>::const_iterator it = cd.slaves_.begin(); it!=cd.slaves_.end(); ++it)
	// os<<" "<<*it<<endl;
	// os<<" "<<*it;
	return os;
	}

	};

	std::set<ContactData> contact_;
	typedef typename std::set<ContactData>::iterator contact_iterator;

	public:

	//! Default constructor
	Contact0_model_manager()
	: Model_manager(), forest_(), timer_(), masks_(), null_(tree_iterator(nullptr)), last_(), detect_(true) {}

	//! Destructor
	~Contact0_model_manager() {

	// delete trees
	for (forest_iterator it = forest_.begin(); it != forest_.end(); ++it)
	delete *it;
	}

	virtual void add_model(model_pointer m, Kinematic_type k = dynamic_object_t) {

	m->initializeUpdateResidualData();
	models_.push_back(m);

	if (models_.size() > 8*sizeof(mask_size)) {
	cout<<"* ERROR * Type used for masks is too small to handle all models."<<endl;
	cout<<"Aborting..."<<endl;
	exit(1);
	}

	// create tree
	tree_type* tp = construct_tree_bottom_up<tree_type, model_type, element_type>(*m);
	forest_.push_back(tp);


	#ifdef DEBUG_MANAGER
	// cout<<"tree "<<*tp<<endl;
	// print_mathematica(*tp);
	#endif

	// mask model as dynamic or static
	masks_ \|= (k << (models_.size()-1));
	}


	void update_forest(time_type t) {

	int k = 0;
	for (forest_iterator fit = forest_.begin(); fit != forest_.end(); ++fit) {

	// check if the object is dynamic to update
	if (!(masks_ & (1 << k++)))
	continue;

	// loop over leaves
	for (leaves_data_iterator lit = (*fit)->leaves_data_begin();
	lit != (*fit)->leaves_data_end(); ++lit) {

	Real t_old = lit->first->last_time_;

	std::vector<const Real*> c = lit->second.coordinates();
	volume_type v = Volume_creator<volume_type>::create(c);

	// get positions
	const point_type& p0 = lit->first->center();
	const point_type& p1 = v.center();

	// get velocities
	const point_type& v0 = lit->first->velocity_;
	const point_type& v1 = v.velocity_;

	// new velocity and acceleration
	v.velocity_ = 1/(t - t_old) * (p1-p0);
	v.acceleration_ = 1/(t - t_old) * (v1-v0);

	v.last_time_ = t;

	// set new volume
	*lit->first = v;
	}

	tree_type &t = **fit;
	Updater u(t);
	postorder(t.root(),u);
	}
	}


	void reset() {

	// clear queue
	while (!timer_.empty())
	timer_.pop();

	timer_.push(std::make_tuple(last_, null_, null_));
	timer_.push(std::make_tuple(inf, null_, null_));
	}


	template <class queue_type>
	void print_queue(queue_type copy) {

	cout<<"Printing queue values:";
	while (!copy.empty()) {
	const tuple_type& tuple = copy.top();
	cout<<" "<<std::get<0>(tuple);
	copy.pop();
	}
	cout<<endl;
	}



	/*! \param t - Current elapsed time
	* \param Dt - Time step
	*/
	void intersect(time_type t, time_type Dt) {

	#ifdef DEBUG_MANAGER
	cout<<"t = "<<t<<endl;
	// cout<<"t = "<<t<<", Dt = "<<Dt<<", timer:";

	// hierarchy_timer copy = timer_;
	// while (!copy.empty()) {
	// const tuple_type& tuple = copy.top();
	// cout<<" "<<std::get<0>(tuple);
	// copy.pop();
	// }
	// cout<<endl;

	#endif

	static time_type Dt1 = Dt;

	// reset if first enter the function
	if (t == last_ \|\| t == Dt1) {
	Dt = Dt1;
	reset();
	}

	const tuple_type& tuple = timer_.top();
	time_type top = std::get<0>(tuple);

	// update hierarchies and get positions
	// note that the updating starts before the next intetersection check
	if (models_.size() > 1 && top <= t + 3*Dt1) {
	update_forest(t);
	//#ifdef DEBUG_MANAGER
	// cout<<"Updating forest"<<endl;
	//#endif
	}

	// check if detection is shut off
	if (t + Dt < top)
	return;

	if (detect_) {

	// get iterators from priority element
	tree_iterator it1 = std::get<1>(tuple);
	tree_iterator it2 = std::get<2>(tuple);

	// remove time from timer
	timer_.pop();

	// check for intersection becase:
	// 1. there are enough models
	// 2. intersection happens before the next increment
	if (models_.size() > 1 && top <= t + Dt1) {

	// if first step, add next time to timer and return because there
	// is not enough information for the computation of intersection times
	if (t <= last_ + (accel == Consider_t ? 2 : 1)*Dt) {
	// remove time from timer
	timer_.push(std::make_tuple(t, null_, null_));
	#ifdef DEBUG_MANAGER
	cout<<"Early out"<<endl;
	#endif
	return;
	}

	// check if iterators are null to compute O(n^2) collision times
	if (it1 == null_ \|\| it2 == null_) {

	// do O(n^2) operation to obtain next time of intersections
	for (forest_iterator it1 = forest_.begin(); it1 != --forest_.end(); ++it1) {

	forest_iterator it2 = it1;
	for (++it2; it2 != forest_.end(); ++it2) {

	// get collision time
	#ifdef DEBUG_MANAGER
	cout<<"Calling check_collision in O(n^2) branch"<<endl;
	#endif
	time_type tstar = resolve_time((it1)->root(), (it2)->root());
	if (tstar != inf)
	timer_.push(std::make_tuple(t+tstar, (it1)->root(), (it2)->root()));
	#ifdef DEBUG_MANAGER
	else
	cout<<"* INFO * Objects do not intersect:\n "<<(it1)->root()<<"\n "<<(it2)->root()<<endl;
	#endif
	} // inner hierarchy loop
	} // outer hierarchy loop

	}

	// else use collision information previously computed (avoids O(n^2) operation above)
	else {

	#ifdef DEBUG_MANAGER
	cout<<"Calling check_collision in non-O(n^2) branch"<<endl;
	#endif

	// temporary queue for tree traversal
	hierarchy_timer pq;
	pq.push(std::make_tuple(top, it1, it2));

	try {

	// enter infinite loop
	while (!pq.empty()) {

	#ifdef DEBUG_MANAGER
	cout<<"______________________________________________"<<endl;
	cout<<"queue empty? -> "<<pq.empty()<<endl;
	if (!pq.empty())
	print_queue(pq);
	#endif

	const tuple_type& tuple = pq.top();

	time_type tstar = std::get<0>(tuple);
	tree_iterator left = std::get<1>(tuple);
	tree_iterator right = std::get<2>(tuple);

	#ifdef DEBUG_MANAGER
	cout<<"Queue time "<<tstar<<", items: "<<left<<", "<<right<<endl;
	#endif

	pq.pop();

	check_collision(t, left, right, pq);

	} // infinite loop

	}
	catch (Continuator& e) {

	cout<<"In continuator"<<endl;

	tuple_type& best = e.best_;
	time_type& best_time = std::get<0>(best);
	best_time += t;

	// clean hierarchy timer until best time
	while (std::get<0>(timer_.top()) <= best_time)
	timer_.pop();

	timer_.push(best);
	timer_.push(best);

	}

	}

	} // if statement on enough models
	// else do nothing as there are not enough models to carry out intersection
	// or the check engine is shut down until the next time in timer
	}

	// detailed check
	detailed_check(t, Dt);
	}


	struct Neighbor_finder {

	int count_;
	element_type *el_;
	tree_type *tree_;

	class Comparator {

	public:
	template <class iterator>
	bool operator()(iterator const &a, iterator const &b) {
	return a->first < b->first;
	}
	};

	std::set<leaves_data_iterator, Comparator > elems_;

	Neighbor_finder() : count_(), el_(nullptr), tree_(nullptr) {}

	UInt size() const
	{ return elems_.size(); }

	template <class container>
	void add_slaves(container& c) {

	for (auto it = elems_.begin(); it != elems_.end(); ++it) {
	auto elem = (*it)->second;
	for (size_t i=0; i<elem.numNodes(); ++i)
	c.insert(elem.node(i));
	}
	}

	template <class container>
	void add_masters(container& c) {

	for (auto it = elems_.begin(); it != elems_.end(); ++it)
	c.insert(&(*it)->second);
	}

	bool operator()() { assert(el_ != nullptr); return elems_.size() == el_->numNodes()+1; }

	void operator()(tree_iterator it) {

	assert(tree_ != nullptr);
	assert(el_ != nullptr);
	auto eit = tree_->find_data(it);
	if (eit != tree_->leaves_data_end())
	if (el_->shareNodes(eit->second))
	elems_.insert(eit);
	++count_;
	}

	friend std::ostream& operator<<(std::ostream& os, const Neighbor_finder& nf) {

	os<<"Neighbor finder info:"<<endl;
	os<<" found neighbors in "<<nf.count_<<" evaluations"<<endl;

	if (nf.el_)
	os<<" reference element "<<*nf.el_<<endl;

	cout<<" neighbors: "<<nf.size()<<endl;
	for (auto n:nf.elems_)
	os<<'\t'<<n->second<<'\n';
	return os;
	}

	};


	void detailed_check(time_type t, time_type Dt) {

	constexpr int dim = point_type::dim();

	if (contact_.size() > 0) {

	// loop over contact structures
	for (contact_iterator cit = contact_.begin(); cit != contact_.end(); ++cit) {

	ContactData& c = const_cast<ContactData&>(*cit);

	typedef std::set<element_type> elem_list;
	typedef std::pair<point_type, vector_type> closest_type;
	typedef std::tuple<UInt, Real, element_type, closest_type, elem_list> test_type;

	std::map<UInt, test_type > map;
	std::set<UInt> checked;

	typename ContactData::elem_set snew, mnew;

	// loop over slave elements to determine slave nodes
	for (auto sit:c.ssurface_) {

	// get slave element
	auto sel = sit->second;
	auto sbb = sel.template boundingBox<dim>();

	// loop over master elements
	for (auto mit:c.msurface_) {

	auto mel = mit->second;

	// tighter check with AABBs
	auto mbb = mel.template boundingBox<dim>();
	if (!(sbb & mbb))
	continue;

	// loop over slave nodes
	for (UInt i=0; i<sel.numNodes(); ++i) {

	UInt s = sel.node(i);

	// treat first element as slave
	auto coord = sel.coordinates();

	// create point
	point_type p(coord[i]);

	if (!penetrates(p,mel)) {
	continue;
	} else {

	// find node in contact data structure
	auto fslave = c.slaves_.find(s);
	// if not found, add it to container and search for new contact elements
	if (fslave == c.slaves_.end()) {

	c.slaves_.insert(s);

	Neighbor_finder sc, mc;
	sc.tree_ = c.stree_;
	mc.tree_ = c.mtree_;
	sc.el_ = &sit->second;
	mc.el_ = &mit->second;
	c.stree_->collect_neighbors(sit->first, sc);
	c.mtree_->collect_neighbors(mit->first, mc);

	for (auto i:sc.elems_)
	snew.insert(i);
	for (auto i:mc.elems_)
	mnew.insert(i);
	}

	// compute closest point
	closest_type r = closest_point_to_element(p, mel);

	Real nd = (p-r.first).sq_norm();

	auto elit = map.find(s);
	if (elit == map.end()) {
	map[s] = test_type(i, nd, sel, r, elem_list());

	test_type &tuple = map[s];
	std::get<4>(tuple).insert(mel);
	}
	else {

	auto tuple = map[s];
	Real dist = std::get<1>(tuple);
	element_type sel = std::get<2>(tuple);

	if (std::abs(nd - dist) < 1.0e-6) {

	test_type &tuple = map[s];

	// edit closest point
	elem_list &els = std::get<4>(tuple);
	els.insert(mel);

	if (els.size() == 2) {

	closest_type &p = std::get<3>(tuple);

	p = commonPonit<point_type>(els);
	}


	} else if (nd < dist) {

	map[s] = test_type(i, nd, sel, r, elem_list());

	test_type &tuple = map[s];
	std::get<4>(tuple).insert(mel);

	}

	}

	}

	} // loop over master elements


	} // loop over slave nodes
	} // loop over slave elements to determine slave nodes

	// add new elements if found
	if (!snew.empty() \|\| !mnew.empty()) {
	for (auto s:snew)
	c.ssurface_.insert(s);
	for (auto m:mnew)
	c.msurface_.insert(m);
	}


	// loop over map to balance slave nodes with closest element
	for (auto it = map.begin(); it != map.end(); ++it) {

	UInt id = std::get<0>(it->second);
	element_type sel = std::get<2>(it->second);
	auto cp = std::get<3>(it->second);
	auto mel = std::get<4>(it->second);


	// process slave
	auto el = *mel.begin();
	balance<point_type>(Dt, id, cp, sel, const_cast<element_type&>(el));

	}
	} // loop over contact structures
	} // contact_ > 0

	// WORKING CODE FOR SINGLE PASS
	// if (contact_.size() > 0) {
	//
	// // loop over contact structures
	// for (auto c: contact_) {
	//
	// typedef std::tuple<UInt, Real, element_type, element_type> test_type;
	//
	// std::map<UInt, test_type > map;
	// std::set<UInt> checked;
	//
	// typename ContactData::elem_set snew, mnew;
	//
	// cout<<"------------------------------------------------------------------"<<endl;
	// cout<<"slaves -> "<<c.ssurface_.size()<<endl;
	// cout<<"masters -> "<<c.msurface_.size()<<endl;
	//
	// // loop over slave elements to determine slave nodes
	// for (auto sit:c.ssurface_) {
	//
	// // get slave element
	// auto sel = sit->second;
	// auto sbb = sel.template boundingBox<dim>();
	//
	// cout<<"sel -> "<<sel<<endl;
	//
	// // loop over master elements
	// for (auto mit:c.msurface_) {
	//
	// auto mel = mit->second;
	// cout<<"mel -> "<<mel<<endl;
	//
	// // tighter check with AABBs
	// auto mbb = mel.template boundingBox<dim>();
	// if (!(sbb & mbb)) {
	// cout<<"AABBS"<<endl;
	// continue;
	// }
	//
	// // loop over slave nodes
	// for (UInt i=0; i<sel.numNodes(); ++i) {
	//
	// UInt s = sel.node(i);
	//
	// cout<<"***NODE "<<s<<endl;
	// cout<<"***MBB -> "<<mbb<<endl;
	// cout<<"***MN -> "<<mel.normal()<<endl;
	//
	// // treat first element as slave
	// auto coord = sel.coordinates();
	//
	// // create point
	// point_type p(coord[i]);
	//
	// if (!penetrates(p,mel)) {
	// cout<<">SLAVE GETOUT"<<endl;
	// continue;
	// } else {
	//
	//
	// // find node in contact data structure
	// auto fslave = c.slaves_.find(s);
	// // if not found, add it to container and search for new contact elements
	// if (fslave == c.slaves_.end()) {
	// c.slaves_.insert(s);
	//
	// cout<<"type of sit -> "<<typeid(sit).name()<<endl;
	//
	// Neighbor_finder sc, mc;
	// sc.tree_ = c.stree_;
	// mc.tree_ = c.mtree_;
	// sc.el_ = &sit->second;
	// mc.el_ = &mit->second;
	// c.stree_->collect_neighbors(sit->first, sc);
	// c.mtree_->collect_neighbors(mit->first, mc);
	//
	// for (auto i:sc.elems_)
	// snew.insert(i);
	// for (auto i:mc.elems_)
	// mnew.insert(i);
	//
	// for (auto i:sc.elems_)
	// cout<<i->second;
	// for (auto i:mc.elems_)
	// cout<<i->second;
	// }
	//
	// cout<<">SLAVE "<<s<<": "<<p<<", with id "<<i<<", belonging to "<<sel<<" PENETRATES"<<endl;
	// cout<<"master element "<<mel<<", master bb -> "<<mbb<<endl;
	//
	// // compute closest point
	// std::pair<point_type, vector_type> r = closest_point_to_element(p, mel);
	//
	// cout<<"r -> "<<r.first<<endl;
	//
	// Real nd = (p-r.first).sq_norm();
	//
	// auto elit = map.find(s);
	// if (elit == map.end()) {
	// cout<<"NO ELEMENT IN MAP"<<endl;
	// map[s] = test_type(i, nd, sel, mel);
	// }
	// else {
	// cout<<"ELEMENT IN MAP"<<endl;
	// cout<<"stored info in map: ";
	//
	// auto tuple = map[s];
	// UInt id = std::get<0>(tuple);
	// Real dist = std::get<1>(tuple);
	// element_type sel = std::get<2>(tuple);
	// element_type mel = std::get<3>(tuple);
	//
	// cout<<"id "<<id<<", dist "<<dist<<"slave element "<<sel<<", master el "<<mel<<endl;
	//
	// cout<<"comparing new distance "<<nd<<" with stored value "<<dist<<endl;
	// if (nd < dist) {
	// cout<<"new distance smaller, inserting new element"<<endl;
	// map[s] = test_type(i, nd, sel, mel);
	// } else
	// cout<<"new distance is bigger!"<<endl;
	// }
	//
	// }
	//
	// } // loop over master elements
	//
	//
	// } // loop over slave nodes
	// } // loop over slave elements to determine slave nodes
	//
	// // add new elements if found
	// if (!snew.empty() \|\| !mnew.empty()) {
	// for (auto s:snew)
	// c.ssurface_.insert(s);
	// for (auto m:mnew)
	// c.msurface_.insert(m);
	// }
	//
	//
	// // loop over map to balance slave nodes with closest element
	// for (auto it = map.begin(); it != map.end(); ++it) {
	//
	// UInt id = std::get<0>(it->second);
	// element_type sel = std::get<2>(it->second);
	// element_type mel = std::get<3>(it->second);
	//
	// cout<<"balance node with id -> "<<id<<" in slave element "<<sel<<endl;
	// cout<<"master el -> "<<mel<<endl;
	//
	// // process slave
	// balance<point_type>(Dt, id, sel, mel);
	//
	// }
	// } // loop over contact structures
	// } // contact_ > 0
	}


	template <class contact_type, class bbox_type, class slave_container>
	void solve_contact(time_type Dt, contact_type& c1, contact_type& c2, const bbox_type& bb, slave_container& slaves) {

	// treat first element as slave
	auto coord = c1.coordinates();

	int slave = 0;
	for (const double* c:coord) {

	// create point
	point_type p(c);

	// if point lies outside of collision zone, continue
	if (!(bb & p)) {
	++slave;
	continue;
	}

	UInt id = c1.node(slave);
	auto it = slaves.find(id);
	if (it != slaves.end())
	continue;
	slaves.insert(id);

	// else find closest distance from p to contacting element c2
	std::pair<point_type, vector_type> r = closest_point_to_element(p, c2);
	const point_type& q = r.first;
	const vector_type& n = r.second;

	// get distance from current position
	Real delta = sqrt((q-p).sq_norm());

	auto mass = c1.getMass(slave)[0];

	// compute force at slave node
	vector_type N = 2 * delta * mass / pow(Dt,2.) * n;

	// compute forces in master element balancing linear and angular momenta
	balance(Dt, slave, r, N, c1, c2);

	++slave;
	}

	}


	private:



	template <class iterator>
	void traverse_right(iterator it1, iterator it2, hierarchy_timer& pq) {

	#ifdef DEBUG_MANAGER
	cout<<" traversing right hierarchy"<<endl;
	#endif

	iterator lit = it2.left();
	iterator rit = it2.right();
	assert(lit != null_);
	assert(rit != null_);

	time_type tstar1 = resolve_time(it1, lit);
	if (tstar1 != inf) {
	#ifdef DEBUG_MANAGER
	cout<<" Adding queue time "<<tstar1<<endl;
	#endif
	pq.push(std::make_tuple(tstar1, it1, lit));
	}
	time_type tstar2 = resolve_time(it1, rit);
	if (tstar2 != inf) {
	#ifdef DEBUG_MANAGER
	cout<<" Adding queue time "<<tstar2<<endl;
	#endif
	pq.push(std::make_tuple(tstar2, it1, rit));
	}
	}

	template <class iterator>
	void traverse_left(iterator it1, iterator it2, hierarchy_timer& pq) {

	#ifdef DEBUG_MANAGER
	cout<<" traversing left hierarchy"<<endl;
	#endif

	iterator lit = it1.left();
	iterator rit = it1.right();
	assert(lit != null_);
	assert(rit != null_);

	time_type tstar1 = resolve_time(lit, it2);
	if (tstar1 != inf) {
	#ifdef DEBUG_MANAGER
	cout<<" Adding queue time "<<tstar1<<endl;
	#endif
	pq.push(std::make_tuple(tstar1, lit, it2));
	}
	time_type tstar2 = resolve_time(rit, it2);
	if (tstar2 != inf) {
	#ifdef DEBUG_MANAGER
	cout<<" Adding queue time "<<tstar2<<endl;
	#endif
	pq.push(std::make_tuple(tstar2, rit, it2));
	}
	}


	template <class iterator>
	void check_collision(time_type t, iterator it1, iterator it2, hierarchy_timer& pq) {

	// if volumes are leaves, change the timer and time step
	if (it1.is_leaf() && it2.is_leaf()) {

	cout<<"* FOUND LEAVES *"<<endl;

	// case where objects intersect
	if (it1 & it2) {

	cout<<"* BOUNDING SPHERE INTERSECTION *"<<endl;

	// add leaves to carry out penetration tests
	leaves_data_iterator lit1(nullptr), lit2(nullptr);

	Neighbor_finder lc, rc;

	for (forest_iterator it = forest_.begin(); it != forest_.end(); ++it) {

	leaves_data_iterator lit = (*it)->find_data(it1);
	if (lit != (*it)->leaves_data_end()) {
	lit1 = lit;

	lc.tree_ = *it;
	lc.el_ = &lit->second;
	(*it)->collect_neighbors(lit->first, lc);
	}

	lit = (*it)->find_data(it2);
	if (lit != (*it)->leaves_data_end()) {
	lit2 = lit;

	rc.tree_ = *it;
	rc.el_ = &lit->second;
	(*it)->collect_neighbors(lit->first, rc);
	}
	}

	assert (lit1 != leaves_data_iterator(nullptr));
	assert (lit2 != leaves_data_iterator(nullptr));

	// check for new ContactData
	auto ins = contact_.insert(ContactData(lit1, lit2));

	if (ins.second) {
	auto cd = ins.first;
	cd->initialize(lc,rc);
	cout<<"* INFO * Adding contact data."<<endl;
	} else {
	cout<<"* WARNING * Contact data not inserted."<<endl;
	}

	// add to data structure for detailed check
	detailed_.insert(std::make_pair(lit1, lit2));

	detect_ = false;

	cout<<"***DETAILED SIZE -> "<<detailed_.size()<<endl;

	}
	else
	cout<<"* NO INTERSECTION BETWEEN BOUNDING SPHERES *"<<endl;


	time_type tstar = resolve_time(it1, it2);

	#ifdef DEBUG_MANAGER

	if (tstar == inf)
	cout<<" Leaves found that DO NOT collide"<<endl;
	else {
	cout<<" Leaves found that collide at time "<<(tstar)<<endl;
	}
	#endif

	if (tstar == inf) {
	cout<<" Leaves found that DO NOT collide"<<endl;
	}

	throw Continuator(std::make_tuple(tstar, it1, it2));
	}

	// found left leaf, traverse right hierarchy
	else if (it1.is_leaf() && !it2.is_leaf()) {

	#ifdef DEBUG_MANAGER
	cout<<" s1 is leaf"<<endl;
	#endif
	traverse_right(it1, it2, pq);
	}

	// found right leaf, traverse left hierarchy
	else if (!it1.is_leaf() && it2.is_leaf()) {

	#ifdef DEBUG_MANAGER
	cout<<" s2 is leaf"<<endl;
	#endif
	traverse_left(it1, it2, pq);
	}

	// else non-leaf case found, check volume sizes
	else {

	value_type m1 = it1->measure();
	value_type m2 = it2->measure();

	// volumes are equal to numerical error, traverse both hierarchies
	if (equal(m1, m2)) {

	#ifdef DEBUG_MANAGER
	cout<<" "<<m1<<" == "<<m2<<endl;
	#endif
	traverse_right(it1, it2, pq);
	traverse_left(it1, it2, pq);
	}
	// left volume is bigger, traverse right hierarchy
	else if (m1 > m2) {

	#ifdef DEBUG_MANAGER
	cout<<" "<<m1<<" > "<<m2<<endl;
	#endif
	traverse_left(it1, it2, pq);
	}
	// right volume is bigger, traverse left hierarchy
	else if (m1 < m2) {

	#ifdef DEBUG_MANAGER
	cout<<" "<<m1<<" < "<<m2<<endl;
	#endif
	traverse_right(it1, it2, pq);
	}
	} // non-leaf case

	}


	friend std::ostream& operator<<(std::ostream& os, const Contact0_model_manager& mm) {

	os<<"Contact model manager info:"<<endl;
	os<<" models: "<<mm.models_.size()<<endl;
	size_t i=0;
	const_forest_iterator tit = mm.forest_.begin();
	for (const_model_iterator it = mm.models_.begin(); it != mm.models_.end(); ++it) {
	os<<"\tmodel "<<++i<<" memory address: "<<*it<<endl;
	os<<"\tmodel: "<<**it<<endl;
	os<<"\ttree: ";
	print_mathematica(**tit++);
	}
	return os;
	}
	};

	__END_AKANTU__

	#endif /* __AKANTU_CONTACT_MANAGER_0_HH__ */

contact_manager0.hhNo OneTemporaryActions

File Metadata

contact_manager0.hhView Options

Event Timeline

contact_manager0.hh
No OneTemporary
Actions

contact_manager0.hh
View Options