model_manager.hh
a178054c2fc0old-master

model_manager.hh
View Options

			/**
			* @file model_manager.hh
			* @author Alejandro M. Aragón <alejandro.aragon@epfl.ch>
			* @date Wed Aug 22 12:14:00 2012
			*
			* @brief higher order object that deals with collections of models
			*
			* @section LICENSE
			*
			* Copyright (©) 2010-2011 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_MODEL_MANAGER_HH__
			#define __AKANTU_MODEL_MANAGER_HH__

			#include <cstring>
			#include <queue>
			#include <unordered_set>
			#include <array/expr.hpp>

			#include "aka_common.hh"
			#include "mesh.hh"
			#include "mesh_io_abaqus.hh"
			#include "model.hh"
			#include "aka_tree.hh"
			#include "solid_mechanics_model.hh"
			#include "aka_plane.hh"
			#include "aka_geometry.hh"
			#include "solid_mechanics_model_element.hh"
			#include "aka_timer.hh"



			#define DEBUG_MANAGER 1


			__BEGIN_AKANTU__


			typedef array::vector_type<Real> vector_type;
			typedef array::matrix_type<Real> matrix_type;
			using array::transpose;


			enum Discretization_type { Node_to_node, Node_to_segment };


			template <class Model_policy>
			class Model_manager {

			public:

			typedef Model_policy model_type;
			typedef model_type* model_pointer;
			typedef model_type& model_reference;

			typedef std::list<model_type*> model_container;
			typedef typename model_container::iterator model_iterator;
			typedef typename model_container::const_iterator const_model_iterator;

			protected:

			model_container models_; //!< Models


			public:

			//! Default constructor
			Model_manager() : models_() {}

			virtual void add_model(model_reference m)
			{ models_.push_back(&m); }

			virtual void add_model(model_pointer m)
			{ models_.push_back(m); }

			model_iterator models_begin()
			{ return models_.begin(); }

			model_iterator models_end()
			{ return models_.end(); }

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

			os<<"Model manager info:"<<endl;
			os<<" models: "<<mm.models_.size()<<endl;
			size_t i=0;
			for (const_model_iterator it = mm.models_.begin(); it != mm.models_.end(); ++it) {
			os<<"\tmodel "<<++i<<" memory address: "<<*it<<endl;
			os<<"\tmodel "<<**it<<endl;
			}

			return os;
			}

			};



			enum Kinematic_type { static_object_t, dynamic_object_t};
			enum Consider_acceleration { Consider_t, Neglect_t };


			template <Consider_acceleration>
			class Kinematic_traits;



			template<>
			class Kinematic_traits<Consider_t> {

			protected:

			typedef Real time_type;

			// check collision considering acceleration
			// the equation to solve is
			//
			// alpha t^4 + beta t^3 + gamma t^2 + delta t + epsilon = 0
			//
			// where alpha = (a.a)/4, beta = (a.v), gamma = a.s + v.v, delta = 2(s.v),
			// epsilon = s.s - r^2, a = a1-a2, v = v1-v2, s = c1-c2, r = r1-r2
			//
			template <class iterator>
			time_type resolve_time(iterator it1, iterator it2) {

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

			const point_type& v1 = it1->velocity_;
			const point_type& v2 = it2->velocity_;

			point_type a1 = it1->acceleration_;
			point_type a2 = it2->acceleration_;

			Real r = it1->radius() + it2->radius();
			point_type s = it2->center() - it1->center();
			point_type v = v2 - v1;
			point_type a = a2 - a1;

			// get coefficients
			Real alpha = (a*a)/4;
			Real beta = a*v;
			Real gamma = as + vv;
			Real delta = 2(sv);
			Real epsilon = ss - rr;

			// obtain roots from quartic equation by calling the function such that
			// the coefficient for the quartic term is equal to one
			std::vector<Real> x(4, inf);


			uint32_t roots = solve_quartic(beta/alpha, gamma/alpha, delta/alpha, epsilon/alpha,
			&x[0], &x[1], &x[2], &x[3]);

			Real tmin = inf;

			// if there are roots, take the first one as an indication of the collision
			if (roots > 0) {

			for (size_t i=0; i<x.size(); ++i)
			tmin = std::min(tmin, x[i]);

			if (tmin > 0) {
			#ifdef DEBUG_MANAGER
			cout<<" Solve quartic with coefficients ";
			cout<<(beta/alpha)<<", "<<(gamma/alpha)<<", "<<(delta/alpha)<<", "<<(epsilon/alpha)<<endl;
			cout<<" Approximate collision time -> tmin = "<<tmin<<" = "<<(tmin)<<endl;
			cout<<" Roots:";
			for (size_t i=0; i<x.size(); ++i)
			cout<<" "<<x[i];
			cout<<endl;
			#endif
			for (size_t i=0; i<x.size(); ++i)
			tmin = std::min(tmin, x[i]);

			}
			} // if roots
			return tmin > 0 ? tmin : inf;
			}

			private:

			/! \brief Solve quartic equation x^4 + ax^3 + bx^2 + cx + d = 0.
			*
			* Solves the quartic equation. Returns the number of roots
			* found. Roots are filled in ascending order.
			* Code taken from the Ion Beam Simulator, which is distrubuted under
			* the terms of the GNU General Public License as published by the Free
			* Software Foundation; either version 2 of the License, or (at your
			* option) any later version.
			*/
			uint32_t solve_quartic(Real a, Real b, Real c, Real d,
			Real x0, Real x1, Real x2, Real x3 );

			};



			template<>
			class Kinematic_traits<Neglect_t> {

			protected:

			typedef Real time_type;

			// check collision neglecting acceleration
			template <class iterator>
			time_type resolve_time(iterator it1, iterator it2) {

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

			const volume_type& s1 = *it1;
			const volume_type& s2 = *it2;

			const point_type& v1 = s1.velocity_;
			const point_type& v2 = s2.velocity_;

			// vector between spheress
			point_type s = s2.center() - s1.center();
			// relative motion of s1 with respect to stationary s0
			point_type v = v2 - v1;
			// sum of radii
			value_type r = s1.radius() + s2.radius();
			value_type c = ss - rr;

			#ifdef DEBUG_MANAGER
			cout<<"Checking collisiong between:"<<endl;
			cout<<" "<<s1<<", velocity: "<<v1<<endl;
			cout<<" "<<s2<<", velocity: "<<v2<<endl;
			cout<<" Relative velocity: "<<v<<endl;
			#endif

			value_type epsilon = 1e-8;

			// already intersecting, call recursively on branches
			if (c < -epsilon) {

			#ifdef DEBUG_MANAGER
			cout<<" Intersecting bounding volumes"<<endl;
			#endif
			// should not get to this point
			assert(false);
			}

			value_type a = v*v;
			// if spheres not moving relative to each other
			if (a < epsilon) {
			#ifdef DEBUG_MANAGER
			cout<<" Objects not moving relative to each other"<<endl;
			cout<<" a: "<<a<<endl;
			#endif
			return inf;
			}

			value_type b = v*s;
			// if spheres not moving towards each other
			if (b >= 0.) {
			#ifdef DEBUG_MANAGER
			cout<<" Objects not moving towards each other"<<endl;
			cout<<" b: "<<b<<endl;
			#endif
			return inf;
			}

			value_type d = bb - ac;
			// if no real-valued root (d < 0), spheres do not intersect
			// otherwise add time to timer
			if (d >= 0.) {
			time_type ts = (-b - sqrt(d))/a;
			if (ts > -epsilon) {
			#ifdef DEBUG_MANAGER
			cout<<" Objects intersects at time "<<(ts)<<endl;
			#endif
			return ts;
			}
			#ifdef DEBUG_MANAGER
			else {
			cout<<" ts negative: "<<ts<<endl;
			}
			#endif

			}
			#ifdef DEBUG_MANAGER
			else {
			cout<<" Objects do not intersect"<<endl;
			cout<<" discriminant: "<<d<<endl;
			}
			#endif

			return inf;
			}
			};



			template <class VolumeType, class DataPolicy, template <class> class CostPolicy = Cost_functor>
			class DataTree :
			public Tree<VolumeType, CostPolicy> {

			public:

			typedef DataPolicy data_type;
			typedef VolumeType volume_type;
			typedef CostPolicy<volume_type> cost_functor;
			typedef Tree<volume_type, CostPolicy> tree_type;
			typedef typename tree_type::iterator tree_iterator;
			typedef typename tree_type::const_iterator const_tree_iterator;

			// leaf information
			typedef std::map<tree_iterator, data_type> leaves_data;
			typedef typename leaves_data::iterator leaves_data_iterator;


			bool add_data(tree_iterator it, data_type& data) {
			auto i = data_.insert(std::make_pair(it, data));
			return i.second;
			}


			leaves_data_iterator leaves_data_begin()
			{ return data_.begin(); }

			leaves_data_iterator leaves_data_end()
			{ return data_.end(); }

			leaves_data_iterator find_data(tree_iterator it)
			{ return data_.find(it); }

			private:

			leaves_data data_;

			};

			template <class U, class T>
			std::pair<U, T> minmax(const U& u, const T& t) {
			return u < t ? std::make_pair(u,t) : std::make_pair(t,u);
			}

			template <Discretization_type, class, class>
			class ContactElement;


			template <class point_type, class element_type>
			class ContactElement<Node_to_node, point_type, element_type> {

			public:

			typedef Real time_type;
			typedef typename vector_type::value_type value_type;
			typedef std::tuple<time_type, point_type> impact_tuple;
			typedef typename element_type::model_type model_type;

			struct Comparator {

			bool operator()(const ContactElement c1, const ContactElement c2) const {

			auto p1 = minmax(c1->el1_, c1->el2_);
			auto p2 = minmax(c2->el1_, c2->el2_);

			if (p1.first < p2.first \|\| p1.second < p2.second)
			return true;

			auto i1 = minmax(c1->id1_, c1->id2_);
			auto i2 = minmax(c2->id1_, c2->id2_);

			if (i1.first < i2.first \|\| i1.second < i2.second)
			return true;

			return false;
			}
			};

			typedef Comparator comparator_type;

			//! Parameter constructor
			template <class parameter_type>
			ContactElement(const parameter_type& p) :
			id1_(std::get<0>(p)), id2_(std::get<1>(p)),
			el1_(std::get<2>(p)), el2_(std::get<3>(p)),
			impact_(std::get<4>(p)), m1_(), m2_(), linked_(), release_(true) {}


			//! Resolve impact
			/*! At the moment of impact, obtain velocities after impact from
			* contacting nodes, and join masses (nodes behave as one)
			*/
			void resolve_impact(time_type t, time_type Dt) {

			// if at moment of impact
			if (equal(t, std::get<0>(impact_)) && !linked_) {


			#ifdef DEBUG_MANAGER
			cout<<"Linking nodes"<<endl;
			#endif

			// get models
			model_type &model1 = el1_->model();
			model_type &model2 = el2_->model();

			// get global node ids
			UInt n1 = el1_->node(id1_);
			UInt n2 = el2_->node(id2_);

			// get references to mass and velocity vectors
			Array<Real> &mass1 = model1.getMass();
			Array<Real> &mass2 = model2.getMass();
			Array<Real> &velocity1 = model1.getVelocity();
			Array<Real> &velocity2 = model2.getVelocity();

			// get references to masses and velocities involved in the collision
			Real &m1 = mass1(n1);
			Real &m2 = mass2(n2);
			value_type &v1 = velocity1(n1);
			value_type &v2 = velocity2(n2);

			// set correct velocity
			Real vc = (m1v1 + m2v2) / (m1+m2);
			velocity1(n1) = vc;
			velocity2(n2) = vc;

			// save mass values and add masses to treat them as a single node
			m1_ = m1;
			m2_ = m2;
			mass1(n1) += m2_;
			mass2(n2) += m1_;

			// set link flag
			linked_ = true;
			}
			}

			//! Treat linked nodes
			/*! During the time that the nodes are in contact, treat them as a single
			* node of joint mass and synchronize residual values
			*/
			bool resolve(time_type t, time_type Dt) {

			// get models
			model_type &model1 = el1_->model();
			model_type &model2 = el2_->model();

			// get global node ids
			UInt n1 = el1_->node(id1_);
			UInt n2 = el2_->node(id2_);

			// get references to residual vectors
			Array<Real> & r1 = model1.getResidual();
			Array<Real> & r2 = model2.getResidual();

			// check condition for delinking of nodes
			vector_type vec = el2_->barycenter() - el1_->barycenter();

			if (std::signbit(vec[0]) != std::signbit(r1(n1))) {

			#ifdef DEBUG_MANAGER
			cout<<"Unlinking nodes"<<endl;
			#endif

			if (release_) {

			// get masses
			Array<Real> &mass1 = model1.getMass();
			Array<Real> &mass2 = model2.getMass();

			mass1(id1_) = m1_;
			mass2(id2_) = m2_;

			release_ = false;
			return true;
			}
			}

			// synchronize if necessary
			if (linked_ && release_) {
			Real tmp = r1(n1);

			r1(n1) += r2(n2);
			r2(n2) += tmp;
			}
			return false;
			}

			friend std::ostream& operator<<(std::ostream& os, const ContactElement& ce) {
			cout<<"Contact element info:\n linking elements ("<<ce.el1_<<" - "<<ce.el2_<<")"<<endl;
			cout<<" colliding nodes ("<<ce.id1_<<" - "<<ce.id2_<<")"<<endl;
			cout<<" first impact at location "<<std::get<1>(ce.impact_)<<" at time "<<std::get<0>(ce.impact_)<<endl;
			if (ce.m1_ > 0. && ce.m2_ > 0)
			cout<<" saved masses: ("<<ce.m1_<<","<<ce.m2_<<")"<<endl;
			if (ce.linked_)
			cout<<" linked state (treating contacting nodes as a single node)"<<endl;

			return os;
			}

			private:
			size_t id1_, id2_; //!< Ids of element nodes involved in the collision
			element_type el1_, el2_; //!< Pointers to elements involved in teh collision
			impact_tuple impact_; //!< Impact information, tuple containing the time and point of contact

			value_type m1_, m2_; //!< Mass values stored after linking of the nodes
			bool linked_, release_; //!< Flags used to specify the state of the linking
			};


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


			using Kinematic_traits<accel>::resolve_time;

			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;
			typedef ContactElement<DT, point_type, element_type> contact_element_type;
			typedef std::set<contact_element_type*, typename contact_element_type::comparator_type > contact_element_container;
			typedef typename contact_element_container::iterator contact_element_iterator;
			typedef typename contact_element_type::time_type time_type;
			typedef typename contact_element_type::impact_tuple impact_tuple;

			// 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;

			// 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."; }
			};

			struct Contactor : public std::exception {

			impact_tuple data_;

			Contactor(const impact_tuple& c) : data_(c) {}

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

			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
			contact_element_container celems_; //!< Contact elements

			public:

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

			//! Destructor
			~Contact_model_manager() {

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

			// delete contact element
			for (contact_element_iterator it = celems_.begin(); it != celems_.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_));
			}

			void resolve(time_type t, time_type Dt) {

			// loop over contact elements
			contact_element_iterator elit = celems_.begin();
			while (elit != celems_.end()) {

			cout<<**elit<<endl;
			if ((*elit)->resolve(t, Dt)) {
			celems_.erase(elit++);

			last_ = t + Dt;

			} else
			++elit;
			}
			}


			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<<", 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;

			// 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 (true) {

			#ifdef DEBUG_MANAGER
			cout<<"______________________________________________"<<endl;
			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) {

			tuple_type& best = e.best_;
			time_type& best_time = std::get<0>(best);
			Dt = best_time;
			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);

			try {
			// set time step if required
			cout<<"calling set_time_set in Continuator"<<endl;

			set_time_step(t, Dt, Dt1);
			} catch (Time_exception& e) {
			cout<<"catching inner Time_exception"<<endl;

			}


			}
			catch (Contactor& c) {

			// get collision impact
			Dt = std::get<0>(c.data_);

			try {

			cout<<"calling set_time_set in Contactor"<<endl;

			// set time step if required
			set_time_step(t, Dt, Dt1);
			// last_ = t + Dt;

			} catch (Time_exception& e) {
			// if too small a time step, do nothing, next time step
			// will carry out DCR

			cout<<"catching outter Time_exception"<<endl;

			// last_ = t + Dt;

			Dt = Dt1;
			}
			cout<<"last -> "<<last_<<endl;
			}
			}

			} // 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


			// resolve collision if necessary
			for (contact_element_iterator elit = celems_.begin(); elit != celems_.end(); ++elit)
			(*elit)->resolve_impact(t, Dt);


			}


			private:

			void set_time_step(time_type t, time_type& Dt, time_type Dt1) {

			if (Dt > Dt1)
			Dt = Dt1;

			if (Dt < 1e-10) {
			cout<<"* INFO * New time step is too small. Throwing exception..."<<endl;
			throw Time_exception();
			}
			#ifdef DEBUG_MANAGER
			cout<<" Leaves found that collide at time "<<(t + Dt)<<endl;
			cout<<" Setting new time step: "<<Dt<<endl;
			#endif
			for (model_iterator mit = models_.begin(); mit != models_.end(); ++mit)
			(*mit)->setTimeStep(Dt);
			}

			// check time of collision between points
			impact_tuple check_points(time_type t, leaves_data_iterator it1, leaves_data_iterator it2) {

			const volume_type& v1 = *it1->first;
			const volume_type& v2 = *it2->first;

			std::vector<const Real*> c1 = it1->second.coordinates();
			std::vector<const Real*> c2 = it2->second.coordinates();

			// compute intersection
			volume_type vint = v1 && v2;

			// get indices of colliding nodes
			size_t ii=0, jj=0;
			for (size_t i=1; i<c1.size(); ++i) {

			if (vint & point_type(c1[i]))
			ii = i;
			if (vint & point_type(c2[i]))
			jj = i;
			}

			impact_tuple impact =
			moving_point_against_point(point_type(c1[ii]), point_type(c2[jj]), it1->first->velocity_, it2->first->velocity_);

			time_type &timpact = std::get<0>(impact);
			timpact += t;

			celems_.insert(new contact_element_type(std::make_tuple(ii, jj, &it1->second, &it2->second, impact)));

			return impact;

			}


			// check time of collision between triangles
			impact_tuple check_triangles(leaves_data_iterator it1, leaves_data_iterator it2) {

			std::vector<const Real*> c1 = it1->second.coordinates();
			std::vector<const Real*> c2 = it2->second.coordinates();

			assert(c1.size() == 3);
			assert(c1.size() == c2.size());
			impact_tuple min = std::make_tuple(inf,point_type());

			for (size_t i=0; i<c1.size(); ++i) {

			point_type r(c1[i]);

			// form plane from three points
			point_type o(c2[0]);
			point_type p(c2[1]);
			point_type q(c2[2]);

			Plane pi(o,p,q);

			// relative velocity
			point_type v = it1->first->velocity_ - it2->first->velocity_;

			// intersect point r with velocity v with plane pi
			// the function returns collision time and point of contact
			impact_tuple impact = moving_point_against_plane(r, v, pi);

			// make sure intersection point lies within the triangle
			if (is_point_in_triangle2(std::get<1>(impact), o, p, q))
			if (std::get<0>(impact) < std::get<0>(min))
			min = impact;
			}
			return min;
			}

			impact_tuple fine_collision_time(time_type t, leaves_data_iterator it1, leaves_data_iterator it2, Int2Type<1>) {

			// check nodes of first segment against second segment
			return check_points(t, it1, it2);
			}

			impact_tuple fine_collision_time(time_type t, leaves_data_iterator it1, leaves_data_iterator it2, Int2Type<2>) {

			assert(false);
			return impact_tuple();
			}

			impact_tuple fine_collision_time(time_type t, leaves_data_iterator it1, leaves_data_iterator it2, Int2Type<3>) {

			// check nodes of first triangle against second triangle
			impact_tuple impact1 = check_triangles(it1, it2);
			impact_tuple impact2 = check_triangles(it2, it1);

			// check nodes of second triangle against first triangle
			return std::get<0>(impact1) < std::get<0>(impact2) ? impact1 : impact2;
			}

			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);

			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;

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

			}

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

			// determine collision time at the lowest level of detection
			impact_tuple impact = fine_collision_time(t, lit1, lit2, Int2Type<volume_type::dim()>());

			#ifdef DEBUG_MANAGER
			cout<<" Fine intersection time: "<<std::get<0>(impact)<<endl;
			#endif
			throw Contactor(impact);
			}
			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;
			cout<<" Modifying timer and time step..."<<endl;
			}
			#endif

			if (tstar == inf) {
			cout<<" Leaves found that DO NOT collide"<<endl;
			return;
			}
			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 Contact_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_MODEL_MANAGER_HH__ */

rAKA/src/model/model_manager.hha178054c2fc0old-master

model_manager.hhView Options

rAKA/src/model/model_manager.hh
a178054c2fc0old-master

model_manager.hh
View Options