simulator.cpp

#include "simulator.h"

Simulator::Simulator() : globalTime_(0.0f), timeStep_(0.1f)
{
	srand(10);
	envMap_.init();
	std::cout << "setup scenario" << std::endl;
//	importAgents("myAgents.pos");
	setupScenario(15000);
	addPathsToAgents();
	for(unsigned int i = 0; i < agents_.size(); ++i)
	{
		agents_[i]->updateObstacleNeighbors();
		agents_[i]->updateAgentNeighbors();
	}
}

Simulator::~Simulator()
{
	for (unsigned int i = 0; i < agents_.size(); ++i)
		delete agents_[i];
}

void Simulator::doStep()
{
	envMap_.clearUpdateCandidates();

	envMap_.map.setCurrentLevel(0);
	for (unsigned int i = 0; i < agents_.size(); ++i)
	{
		agents_[i]->computePrefVelocity();
		agents_[i]->computeNewVelocity();
	}
	envMap_.map.setCurrentLevel(envMap_.map.getMaxLevel());

	for (unsigned int i = 0; i < agents_.size(); ++i)
	{
		Dart oldFace = agents_[i]->part_.d;
		agents_[i]->update();
//		if(envMap_.map.getEmbedding(oldFace, FACE_ORBIT) != envMap_.map.getEmbedding(agents_[i]->part_.d, FACE_ORBIT))
		if(agents_[i]->part_.crossCell != CGoGN::Algo::MovingObjects::NO_CROSS)
//		switch(agents_[i]->part_.crossCell)
		{
//			case CGoGN::Algo::MovingObjects::CROSS_EDGE :
//				envMap_.agentChangeFaceThroughEdge(agents_[i]);
//				envMap_.addCoarsenCandidate(oldFace);
//				envMap_.addRefineCandidate(agents_[i]->part_.d);
//				break;
//			case CGoGN::Algo::MovingObjects::CROSS_OTHER :
				envMap_.agentChangeFace(agents_[i], oldFace);
				envMap_.addCoarsenCandidate(oldFace);
				envMap_.addRefineCandidate(agents_[i]->part_.d);
//				break;
		}

		envMap_.map.setCurrentLevel(0);
		agents_[i]->updateObstacleNeighbors();
		envMap_.map.setCurrentLevel(envMap_.map.getMaxLevel());
		
		agents_[i]->updateAgentNeighbors();
	}

	envMap_.updateMap();

	globalTime_ += timeStep_;
}

void Simulator::setupScenario(unsigned int nbMaxAgent)
{
	/*
	* Add agents, specifying their start position, and store their goals on the
	* opposite side of the environment.
	*/
	Dart d = envMap_.map.begin();
	DartMarkerStore filled(envMap_.map);

	unsigned int nbx = 3;
	unsigned int nby = 3;

	unsigned int bMax = nbMaxAgent/(nbx*nby);

	for (unsigned int i = 0; i < bMax && d != envMap_.map.end(); ++i)
	{
		bool found = false;
		VEC3 pos;
		Dart dCell;
		while(!found && d != envMap_.map.end())
		{
			if(!filled.isMarked(d) && !envMap_.buildingMark.isMarked(d))
			{
				filled.markOrbit(FACE_ORBIT, d);
				pos = Algo::Geometry::faceCentroid<PFP>(envMap_.map, d, envMap_.position);
				pos[2] = 0;
				dCell = d;
				found = true;
			}
			envMap_.map.next(d);
		}

		if(found)
		{
			float ecart = 3.0f;
			VEC3 posinit = VEC3(pos[0] - (float(nbx)/2.0f * ecart), pos[1] - (float(nby)/2.0f * ecart), pos[2]);
			for(unsigned int curx = 0; curx < nbx; ++curx)
			{			
				for(unsigned int cury = 0; cury < nby; ++cury)
				{
					VEC3 posagent = posinit + VEC3(ecart * curx, ecart * cury, 0.0f);
					agents_.push_back(new Agent(this, posagent, dCell));
					agents_.back()->goals_.push_back(posagent);
					agents_.back()->goals_.push_back(-1.0f * posagent);
					agents_.back()->curGoal_ = 1;
				}
			}
		}
	}
	std::cout << "nb agents : " << agents_.size() << std::endl;

	swapAgentsGoals();

	for(unsigned int i = 0; i < agents_.size(); ++i)
		envMap_.pushAgentInCells(agents_[i], agents_[i]->part_.d);
}

void Simulator::addPathsToAgents()
{
	//city
	unsigned int dartDistForPath=50;
	for(unsigned int i = 0; i < agents_.size(); ++i)
	{
		agents_[i]->goals_.clear();

		Dart dStart = agents_[i]->part_.d;
		Dart dStop = dStart;
		for(unsigned int j = 0; envMap_.buildingMark.isMarked(dStop) || j < dartDistForPath+rand()*20 || envMap_.map.sameOrientedFace(dStop,dStart); ++j)
		{
			envMap_.map.next(dStop);
			if(dStop == envMap_.map.end())
				dStop = envMap_.map.begin();
		}

		std::vector<Dart> path = CGoGN::PathFinder::pathFindAStar<PFP>(envMap_.map, envMap_.position, dStart, dStop, envMap_.buildingMark);

		for(std::vector<Dart>::iterator it = path.begin(); it!=path.end();++it)
			agents_[i]->goals_.push_back(Algo::Geometry::faceCentroid<PFP>(envMap_.map, *it, envMap_.position));

		Dart dStop2 = dStop;
		for(unsigned int j = 0; envMap_.buildingMark.isMarked(dStop2) || j < dartDistForPath+rand()*20 || envMap_.map.sameOrientedFace(dStop,dStop2) || envMap_.map.sameOrientedFace(dStop2,dStart); ++j)
		{
			envMap_.map.next(dStop2);
			if(dStop2 == envMap_.map.end())
				dStop2 = envMap_.map.begin();
		}

		path = CGoGN::PathFinder::pathFindAStar<PFP>(envMap_.map, envMap_.position, dStop, dStop2, envMap_.buildingMark);

		for(std::vector<Dart>::iterator it = path.begin(); it!=path.end();++it)
			agents_[i]->goals_.push_back(Algo::Geometry::faceCentroid<PFP>(envMap_.map, *it, envMap_.position));

		path = CGoGN::PathFinder::pathFindAStar<PFP>(envMap_.map, envMap_.position, dStop2, dStart, envMap_.buildingMark);

		for(std::vector<Dart>::iterator it = path.begin(); it!=path.end();++it)
			agents_[i]->goals_.push_back(Algo::Geometry::faceCentroid<PFP>(envMap_.map, *it, envMap_.position));
	}
}


bool Simulator::importAgents(std::string filename)
{
	std::ifstream myfile(filename.c_str(), std::ios::in);

	if (!myfile.good())
	{
		std::cerr << "(export) Unable to open file " << filename << std::endl;
		return false;
	}

	std::string line,token;
	while ( myfile.good() )
	{
		getline(myfile,line);

		if(line.size() > 1)
		{
			std::istringstream iss(line);
			float x,y,z;

			iss >> x;
			iss >> y;
			iss >> z;

			VEC3 pos(x, y, z);
			Dart dCell = envMap_.getBelongingCell(pos);

			agents_.push_back(new Agent(this, pos, dCell));
			agents_.back()->goals_.push_back(pos);
			agents_.back()->goals_.push_back(-1.0f * pos);
			agents_.back()->curGoal_ = 1;
		}
	}

	swapAgentsGoals();

	for (unsigned int i = 0; i < agents_.size(); ++i)
		envMap_.pushAgentInCells(agents_[i], agents_[i]->part_.d);

	myfile.close();
	return true;
}

bool Simulator::exportAgents(std::string filename)
{
	std::ofstream out(filename.c_str(), std::ios::out);
	if (!out.good()) {
		std::cerr << "(export) Unable to open file " << filename << std::endl;
		return false;
	}

	for (unsigned int i = 0; i < agents_.size(); ++i) {
		out << agents_[i]->part_.m_position << std::endl;
	}

	out.close();
	return true;
}

void Simulator::swapAgentsGoals()
{
	unsigned int nbAgents = agents_.size();
	for(unsigned int i = 0; i < nbAgents; ++i)
	{
		unsigned int r = rand() % nbAgents;

		if(agents_[i]->goals_[1][0]!=agents_[r]->goals_[1][0] || agents_[i]->goals_[1][1]!=agents_[r]->goals_[1][1]) {
			VEC3 g = agents_[i]->goals_[1];
			agents_[i]->goals_[1] = agents_[r]->goals_[1];
			agents_[r]->goals_[1] = g;
		}
	}
}