merging

Debug/CMakeLists.txt

@@ -25,6 +25,7 @@ add_executable( socialAgentsD
 	../src/env_map.cpp
 	../src/agent.cpp
 	../src/moving_obstacle.cpp
+	../src/articulated_obstacle.cpp
 	../src/simulator.cpp
 	../src/moving_mesh.cpp
 	../src/gl2ps.c

Release/CMakeLists.txt

@@ -23,6 +23,7 @@ add_executable( socialAgents
 	../src/env_map.cpp
 	../src/agent.cpp
 	../src/moving_obstacle.cpp
+	../src/articulated_obstacle.cpp
 	../src/simulator.cpp
 	../src/moving_mesh.cpp
 	../src/gl2ps.c

include/agent.h

@@ -55,7 +55,12 @@ public:
 	VEC3 finalGoal ;
 	Dart finalDart ;
 
+
+	MovingObstacle **movingObstacles_;
+	int nb_mos;
+
 	static unsigned int maxNeighbors_ ;
+	static unsigned int maxMovingObstacles_;
 	static float averageMaxSpeed_ ;
 	static float neighborDist_ ;
 	static float neighborDistSq_ ;

include/env_generator.h

@@ -15,7 +15,7 @@ bool notDiagonalAdjacentToAnObstacle(typename PFP::MAP& map, Dart d, CellMarker<
 
 template <typename PFP> Algo::Modelisation::Polyhedron<PFP> generateGrid(EnvMap& envMap) ;
 
-template <typename PFP> void generateCity(EnvMap& envMap) ;
+template <typename PFP> void generateCity(EnvMap& envMap, unsigned int nbBuildings) ;
 
 template <typename PFP>
 Algo::Modelisation::Polyhedron<PFP> generateTrianGrid(typename PFP::MAP& map,

include/env_generator.hpp

@@ -63,9 +63,9 @@ Algo::Modelisation::Polyhedron<PFP> generateGrid(EnvMap& envMap)
 }
 
 template <typename PFP>
-void generateCity(EnvMap& envMap)
+void generateCity(EnvMap& envMap, unsigned int nbBuildings)
 {
-	unsigned int nbBuilding = 1000 ;
+	unsigned int nbBuilding = nbBuildings ;
 	std::cout << " - Generate City : " << nbBuilding << " buildings" << std::endl ;
 
 	generateGrid<PFP>(envMap) ;
@@ -76,7 +76,7 @@ void generateCity(EnvMap& envMap)
 		if (!envMap.buildingMark.isMarked(d) && (rand() % 12 == 0)
 		    && notDiagonalAdjacentToAnObstacle<PFP>(envMap.map, d, envMap.buildingMark))
 		{
-			generateBuilding<PFP>(envMap, d, (1 + (rand() % 3)) * 2.0f, rand() % 4) ;
+			generateBuilding<PFP>(envMap, d, (1 + (rand() % 3)) * 50.0f, rand() % 4) ;
 			--nbBuilding ;
 		}
 	}

include/env_map.h

@@ -28,6 +28,7 @@ using namespace CGoGN ;
 class Agent;
 class Obstacle;
 class MovingObstacle;
+class ArticulatedObstacle;
 
 #include "pfp.h"
 
@@ -402,6 +403,7 @@ inline void EnvMap::find_next(Obstacle* o,Dart * ddd, CellMarkerMemo<FACE>& cms)
 inline void EnvMap::pushObstNeighborInCells(Obstacle* o, Dart d)
 {
 	assert(map.getCurrentLevel() == map.getMaxLevel());
+	assert(std::find(neighborObstvect[d].begin(), neighborObstvect[d].end(), o) == neighborObstvect[d].end());
 
 	addElementToVector<Obstacle*>(neighborObstvect[d],o);
 }

include/moving_obstacle.h

@@ -10,13 +10,13 @@
 using namespace std;
 PFP::VEC3 rotate (PFP::VEC3 pos1, PFP::VEC3 center, float angle);
 float get_angle (PFP::VEC3 v1, PFP::VEC3 v2);
-
+PFP::VEC3 get_center (ArticulatedObstacle * art, int index);
 class Simulator ;
 
 class MovingObstacle
 {
 public:
-	MovingObstacle(Simulator* sim, int index, std::vector<PFP::VEC3> pos, std::vector<VEC3> goals,bool spin);
+	MovingObstacle(Simulator* sim, int index, std::vector<PFP::VEC3> pos, std::vector<VEC3> goals,bool spin, ArticulatedObstacle * art=NULL, int ind2=-1);
 	bool test_opposition(VEC3 o, VEC3 p1, VEC3 p2);
 //	void contournerBatiment();
 	void updateAgentNeighbors() ;
@@ -44,6 +44,8 @@ public:
 	std::vector<Dart> * neighbor_cells;
 	std::set<Dart> general_belonging;
 	VEC3 front;
+	VEC3 focus1, focus2;
+	double length, width, sum_dist_foci, sum_dist_foci_rest;
 
 	VEC3 finalGoal;
 	float angle;
@@ -61,11 +63,17 @@ public:
 	float obstacle_range;
 	static float timeHorizonObst_;
 	float velocity_factor;
+	float color1;
+	float color2;
+	float color3;
+	bool seen;
 	VEC3 velocity_;
 	VEC3 newVelocity_;
 	VEC3 prefVelocity_;
 	Simulator* sim_;
 	bool spinning;
+	ArticulatedObstacle * parent;
+	int index_parent;
 };
 
 #endif

include/simulator.h

@@ -7,7 +7,7 @@
 #include "env_map.h"
 #include "agent.h"
 #include "obstacle.h"
-#include "moving_obstacle.h"
+#include "articulated_obstacle.h"
 #include "moving_mesh.h"
 #include "path_finder.h"
 
@@ -84,7 +84,7 @@ public:
 
 	~Simulator() ;
 
-	void init(unsigned int config, float dimension, bool enablePathFinding = false) ;
+	void init(float dimension, bool enablePathFinding = false) ;
 
 	void doStep() ;
 
@@ -92,7 +92,7 @@ public:
 
 	void setupCircleScenario(unsigned int nbAgents, unsigned int nbObstacles) ;
 	void setupCorridorScenario(unsigned int nbAgents, unsigned int nbObstacles) ;
-	void setupCorridor2Scenario(unsigned int nbAgents, unsigned int nbObstacles) ;
+	void setupSnakeCorridorScenario(unsigned int nbAgents, unsigned int nbSnakes, int snakeSize) ;
 	void setupCityScenario(int nbLines, int nbRank) ;
 	void setupScenario(unsigned int nbMaxAgent) ;
 
@@ -121,6 +121,7 @@ public:
 	std::vector<Agent*> agents_ ;
 	std::vector<MovingObstacle*> movingObstacles_;
 	int minSize;
+	unsigned int config;
 	float timeStep_ ;
 	float globalTime_ ;
 	unsigned int nbSteps_ ;

include/socialAgents.ui

@@ -129,6 +129,13 @@
     <item>
      <widget class="QSpinBox" name="check_slide"/>
     </item>
+    <item>
+     <widget class="QCheckBox" name="check_elipse">
+      <property name="text">
+       <string>draw elipse</string>
+      </property>
+     </widget>
+    </item>
     <item>
      <spacer name="verticalSpacer">
       <property name="orientation">

include/viewer.h

@@ -103,6 +103,7 @@ public:
 	bool drawObstPredictionTri ;
 	bool drawObstPath ;
 	bool draw_dart;
+	bool draw_elipse;
 	unsigned int dartSlider;
 
 public slots:
@@ -182,4 +183,9 @@ public slots:
 		dartSlider = i;
 		updateGL();
 	}
+	void slot_elipse(bool b)
+		{
+			draw_elipse = b;
+			updateGL();
+		}
 } ;

src/env_map.cpp

@@ -73,17 +73,18 @@ void EnvMap::init(unsigned int config, REAL width, REAL height, REAL minSize, RE
 	switch (config)
 	{
 		case 0 :
-			CityGenerator::generateGrid<PFP>(*this) ;
+//			CityGenerator::generateGrid<PFP>(*this) ;
+			CityGenerator::generateCity<PFP>(*this,1) ;
 			break ;
 		case 1 :
 			CityGenerator::generateGrid<PFP>(*this) ;
 			break ;
 		case 2 :
-			CityGenerator::generateCity<PFP>(*this) ;
+			CityGenerator::generateGrid<PFP>(*this) ;
 			// CityGenerator::generateMall<PFP>(map, position, obstacleMark, buildingMark, sideSize);
 			break ;
 		case 3 :
-			CityGenerator::generateCity<PFP>(*this) ;
+			CityGenerator::generateCity<PFP>(*this,10) ;
 			break ;
 		case 4 :
 			CityGenerator::generatePlanet<PFP>(*this) ;

src/moving_obstacle.cpp

@@ -70,11 +70,17 @@ VEC3 rotate(VEC3 pos1, VEC3 center, float angle) // renvoie le déplacement nece
 	return pos2;
 }
 
-MovingObstacle::MovingObstacle(Simulator* sim, int ind, std::vector<VEC3> pos, std::vector<VEC3> goals, bool spin) :
+MovingObstacle::MovingObstacle(Simulator* sim, int ind, std::vector<VEC3> pos, std::vector<VEC3> goals, bool spin, ArticulatedObstacle * art, int ind2) :
 		index(ind),
 		newVelocity_(0),
-		sim_(sim)
+		sim_(sim),
+		parent(art),
+		index_parent(ind2)
 {
+	seen = false;
+	color1=1.0f;
+	color2=1.0f;
+	color3=1.0f;
 	assert(pos.size() > 2);
 
 //	movingObstNeighbors_.reserve(maxNeighbors_);
@@ -102,7 +108,18 @@ MovingObstacle::MovingObstacle(Simulator* sim, int ind, std::vector<VEC3> pos, s
 	}
 	center = sum / nbVertices;
 	front=(vertices[1] + vertices[2]) / 2;
-	if (spinning) //départ face à la cible en cas d'obstacles pouvant effectuer des rotations
+
+	//-------- code ajoute par Arash pour les obstacles rectangulaires --------------
+
+	length = (vertices[0]-vertices[1]).norm();
+	width  = (vertices[1]-vertices[2]).norm();
+	sum_dist_foci_rest = 2*(length + width*(sqrt(2)-0.5));
+	// M appartient à l'ellipse ssi MF1 + MF2 = sum_dist_foci est une constante
+	// où F1 et F2 sont les deux foyers.
+
+	//-------- fin du code ajoute par Arash pour les obstacles rectangulaires -------
+
+	if (spinning && parent==NULL) //départ face à la cible en cas d'obstacles pouvant effectuer des rotations
 	{
 		angle = get_angle(goals_[curGoal_] - center,front  - center);
 		for (unsigned int i = 0; i < nbVertices; ++i)
@@ -319,6 +336,13 @@ void MovingObstacle::update()
 	if(sim_->detect_agent_collision)
 		general_belonging.clear();
 
+	if(!seen)
+	{
+		color1=1.0f;
+		color2=1.0f;
+		color3=1.0f;
+	}
+	seen=false;
 	PFP::VEC3 bary = 0;
 
 	Dart d;
@@ -327,7 +351,15 @@ void MovingObstacle::update()
 
 	// MAJ des particules
 	float abs_angle= angle > 0 ? 1 : -1;
-	float rotor = abs_angle*angle > 0.01f ? 0.01f : abs_angle*angle ;
+	float rotor=0;
+	if (index_parent==0)
+	{
+		rotor = abs_angle*angle > 0.04f ? 0.04f : abs_angle*angle ;
+	}
+	else
+	{
+		rotor = abs_angle*angle ;
+	}
 //	CGoGNout << "Obstacle "<< index << CGoGNendl;
 //	CGoGNout << "vitesse : "<< velocity_ << CGoGNendl;
 	//	on fait tourner l'obstacle
@@ -354,6 +386,25 @@ void MovingObstacle::update()
 
 	center = bary / nbVertices;
 
+	//-------- code ajoute par Arash pour les obstacles rectangulaires --------------
+
+	VEC3 P0_P1 = vertices[1] - vertices[0];
+	float velocity_coef = 10.0;
+
+	if(P0_P1 * velocity_ > 0) 	// P0_P1 est dans le sens de la vitesse
+	{
+		focus1  = center - P0_P1*(1-(width/length)*(sqrt(2)-0.5));
+		focus2 = center + P0_P1*(1-(width/length)*(sqrt(2)-0.5)) + (velocity_coef*velocity_);
+	}
+	else
+	{
+		focus1  = center - P0_P1*(1-(width/length)*(sqrt(2)-0.5)) + (velocity_coef*velocity_);
+		focus2 = center + P0_P1*(1-(width/length)*(sqrt(2)-0.5));
+	}
+	sum_dist_foci = sum_dist_foci_rest + velocity_coef*velocity_.norm();
+
+	//-------- fin du code ajoute par Arash pour les obstacles rectangulaires -------
+
 	// MAJ des obstacles
 	for (unsigned int i = 0; i < nbVertices; ++i)
 	{
@@ -386,7 +437,7 @@ void MovingObstacle::update()
 			std::vector<Agent*> vector =sim_->envMap_.agentvect[(*it2)];
 			for(std::vector<Agent*>::iterator it=vector.begin();it!=vector.end(); ++it)
 			{
-				if (this->is_inside((*it)->getPosition()))
+				if (this->is_inside((*it)->part_.getPosition()))
 				{
 					(*it)->alive=false;
 					(*it)->color1=0.0f;
@@ -401,6 +452,7 @@ void MovingObstacle::update()
 
 }
 
+
 std::vector<Dart> MovingObstacle::getMemoCross(const VEC3& pos, const VEC3& dest, Dart& d1)
 {
 	registering_part->move(pos);
@@ -417,7 +469,8 @@ void resetPartSubdiv(Obstacle* o)
 	if (mo != NULL)
 	{
 		VEC3 pos =mo->registering_part->getPosition();
-		mo->registering_part->ParticleBase<PFP>::move(Algo::Geometry::faceCentroid<PFP>(mo->sim_->envMap_.map, mo->registering_part->d, mo->sim_->envMap_.position)) ;
+		mo->registering_part->CGoGN::Algo::MovingObjects::ParticleBase<PFP>::move(Algo::Geometry::faceCentroid<PFP>(mo->sim_->envMap_.map, mo->registering_part->d, mo->sim_->envMap_.position)) ;
+
 		mo->registering_part->setState(FACE) ;
 		mo->registering_part->move(pos) ;
 	}
@@ -459,22 +512,38 @@ void displayMO(Obstacle * o)
 // TODO Check position
 void MovingObstacle::computePrefVelocity() //calcul du vecteur optimal pour atteindre l'objectif // changer pour 2.5 ?
 {
-	VEC3 goalVector = goals_[curGoal_] - center ;
-	float goalDist2 = goalVector.norm2() ;
-
-	if (goalDist2 < 2.0f)
+	VEC3 goalVector;
+	if (index_parent<1)
 	{
-		curGoal_ = (curGoal_ + 1) % goals_.size() ;
 		goalVector = goals_[curGoal_] - center ;
-		goalDist2 = goalVector.norm2() ;
-	}
 
-	if (goalDist2 > maxSpeed_)
+		float goalDist2 = goalVector.norm2() ;
+
+		if (goalDist2 < 5.0f)
+		{
+			curGoal_ = (curGoal_ + 1) % goals_.size() ;
+			goalVector = goals_[curGoal_] - center ;
+			goalDist2 = goalVector.norm2() ;
+		}
+
+		if (goalDist2 > maxSpeed_)
+		{
+			goalVector.normalize() ;
+			goalVector *= maxSpeed_;
+		}
+	}
+	else
 	{
-		goalVector.normalize() ;
-		goalVector *= maxSpeed_;
+		goalVector = get_center(parent,index_parent-1) -center;
+		float goalDist2 = goalVector.norm2() ;
+		if (goalDist2 > maxSpeed_)
+		{
+			goalVector.normalize() ;
+			goalVector *= maxSpeed_;
+		}
 	}
 	if (spinning) angle =get_angle(goalVector,front-center);
+
 	prefVelocity_ = goalVector ;
 
 }
@@ -490,6 +559,7 @@ void MovingObstacle::computeNewVelocity()  //comportement des obstacles en tenan
 				forward.normalize() ;
 				forward *= objective;
 			}
+		if(index_parent==0 && angle>0.01f) forward/=10;
 		newVelocity_=forward;
 	}
 	else

src/simulator.cpp

@@ -14,11 +14,12 @@ Simulator::Simulator(unsigned int config, unsigned int minS) :
 	nb_dead(0)
 {
 	minSize=minS;
-	multires=false;
-	detect_agent_collision=false;
+	multires=true;
+	detect_agent_collision=true;
 	srand(10) ;
 	nbStepsPerUnit_ = 1 / timeStep_ ;
-	init(config, minSize, 2.0f) ;
+	this->config=config;
+	init(2.0f) ;
 }
 
 Simulator::~Simulator()
@@ -27,27 +28,21 @@ Simulator::~Simulator()
 		delete agents_[i] ;
 }
 
-void Simulator::init(unsigned int config, float dimension, bool enablePathFinding)
+void Simulator::init( float dimension, bool enablePathFinding)
 {
 	std::cout << "Setup scenario" << std::endl ;
 
 	switch (config)
 	{
 		case 0 :
-			envMap_.init(config, 2000.0f, 2000.0f, minSize, 400.0f) ;
-			setupCircleScenario(1000, 35) ;
+			setupCircleScenario(1000,20) ;
 			break ;
 		case 1 :
-			envMap_.init(config, 1600.0f, 1200.0f, minSize, 400.0f) ;
-			setupCorridorScenario(1000, 40) ;
+			setupCorridorScenario(1000,40) ;
 			break ;
 		case 2 :
-			envMap_.init(config, 1600.0f, 1200.0f, minSize, 400.0f) ;
-			setupCorridor2Scenario(200, 4) ;
+			setupSnakeCorridorScenario(1000,5,10) ;
 			break ;
-//		case 2 :
-//			setupScenario(1000) ;
-//			break ;
 //		case 3 :
 //			setupCityScenario(20, 20) ;
 ////			setupCityScenario(-1.0f * (12 * (70.0f / 2.0f) - 10),
@@ -206,6 +201,16 @@ void Simulator::addAgent(const VEC3& start, const VEC3& goal, Dart d)
 
 void Simulator::setupCircleScenario(unsigned int nbAgents , unsigned int nbObstacles)
 {
+	if (multires)
+	{
+		envMap_.init(config, 2000.0f, 2000.0f, minSize, 400.0f) ; //grosses cases
+	}
+	else
+	{
+
+		envMap_.init(config, 1600.0f, 960.0f, minSize, 400.0f) ; //cases fines
+	}
+
 	std::cout << " - Setup Circle Scenario : " << nbAgents << " agents" << std::endl ;
 
 	// Bordure à éviter autour de la scène (10% de sa taille)
@@ -245,7 +250,7 @@ void Simulator::setupCircleScenario(unsigned int nbAgents , unsigned int nbObsta
 		}
 		else
 		{
-			v=VEC3 (std::cos(angle) * (radius/3), std::sin(angle) * (radius/3), 0) ;
+			v=VEC3 (std::cos(angle) * (radius/2), std::sin(angle) * (radius/2), 0) ;
 		}
 		start = center + v ;
 		positions.push_back(start);
@@ -296,10 +301,23 @@ void Simulator::setupCircleScenario(unsigned int nbAgents , unsigned int nbObsta
 	envMap_.clearUpdateCandidates() ;
 	envMap_.map.setCurrentLevel(0) ;
 #endif
+
+	std::cout << "nb agents : " << agents_.size() << std::endl ;
 }
 
 void Simulator::setupCorridorScenario(unsigned int nbAgents, unsigned int nbObstacles)
 {
+	if (multires)
+	{
+		envMap_.init(config, 1600.0f, 960.0f, minSize, 320.0f) ; //grosses cases
+	}
+	else
+	{
+
+		envMap_.init(config, 1600.0f, 960.0f, minSize, 320.0f) ; //cases fines
+	}
+
+
 	std::cout << " - Setup Corridor Scenario : " << nbAgents << " agents et " << nbObstacles << " obstacles" << std::endl ;
 
 	// Bordure à éviter autour de la scène (10% de sa taille)
@@ -398,13 +416,24 @@ void Simulator::setupCorridorScenario(unsigned int nbAgents, unsigned int nbObst
 	}
 }
 
-void Simulator::setupCorridor2Scenario(unsigned int nbAgents, unsigned int nbObstacles)
+void Simulator::setupSnakeCorridorScenario(unsigned int nbAgents, unsigned int nbSnakes, int snakeSize)
 {
-	std::cout << " - Setup Corridor Scenario : " << nbAgents << " agents et " << nbObstacles << " obstacles" << std::endl ;
+	if (multires)
+	{
+		envMap_.init(config, 1600.0f, 960.0f, minSize, 160.0f) ; //grosses cases
+	}
+	else
+	{
+
+		envMap_.init(config, 1600.0f, 960.0f, minSize, 160.0f) ; //cases fines
+	}
+
+
+	std::cout << " - Setup Snake Corridor Scenario : " << nbAgents << " agents et " << nbSnakes*snakeSize << " obstacles" << std::endl ;
 
 	// Bordure à éviter autour de la scène (10% de sa taille)
-	int xBorder = envMap_.geometry.size(0) / 10 ;
-	int yBorder = envMap_.geometry.size(1) / 10 ;
+	int xBorder = envMap_.geometry.size(0) / 5 ;
+	int yBorder = envMap_.geometry.size(1) / 5 ;
 
 	// Les coordonnées sont comprises entre xMin et xMin+xDelta
 
@@ -426,75 +455,86 @@ void Simulator::setupCorridor2Scenario(unsigned int nbAgents, unsigned int nbObs
 		VEC3  goal(xGoalMin  + rand() % xGoalDelta,  yGoalMin  + rand() % yGoalDelta,  0) ;
 
 		// Un agent sur 2 va de droite à gauche
+		VEC3 tmp ;
 		if (i % 2 == 1)
-			addAgent(start, goal) ;
-		else
-			addAgent(goal, start) ;
+		{
+			tmp = goal ;
+			goal = start ;
+			start = tmp ;
+		}
+		addAgent(start, goal) ;
 	}
 
 	// Départ des obstacles du quart haut sur toute une demi-largeur
 	xStartMin = envMap_.geometry.min()[0] + envMap_.geometry.size(0) / 4 ;
 	xStartDelta = envMap_.geometry.size(0) / 2 ;
 	yStartMin = envMap_.geometry.min()[1] + yBorder ;
-	yStartDelta = envMap_.geometry.size(1) / 5 ;
+	yStartDelta = envMap_.geometry.size(1) / 20 ;
 
 	// Arrivée des obstacles à l'opposée
-	yGoalDelta = envMap_.geometry.size(1) / 5 ;
+	yGoalDelta = 3* envMap_.geometry.size(1) / 5 ;
 	yGoalMin = envMap_.geometry.max()[1] - yBorder - yGoalDelta ;
 
 	VEC3 xSide (5.0f,0.0f,0.0f);
 	VEC3 ySide (0.0f,10.0f,0.0f);
-	std::vector<VEC3> vPos;
-	MovingObstacle* mo4;
 
-	for (unsigned int i = 0 ; i < nbObstacles ; i++)
+
+	int sumObstacles=0;
+	for(unsigned int j = 0; j<nbSnakes; j++)
 	{
-		float x = xStartMin + ((int)i*30) % xStartDelta;
-//		std::cout << "x ? " << x << " xStartDelta " << xStartDelta << std::endl;
+		std::vector<PFP::VEC3> positions [snakeSize] ;
+		float x = xStartMin + rand() % xStartDelta;
+		VEC3 goal;
+
 		VEC3 start(x, yStartMin + rand() % yStartDelta, 0) ;
-		VEC3  goal(x, yGoalMin  + rand() % yGoalDelta,  0) ;
 
-		vPos.clear();
-		// Un obstacle sur deux va vers le haut
-		VEC3 tmp ;
-		if (i % 2 == 1)
+
+		std::vector<VEC3> vPos;
+
+
+		vPos.push_back(start+xSide-ySide);
+		vPos.push_back(start+xSide+ySide);
+		vPos.push_back(start-xSide+ySide);
+		vPos.push_back(start-xSide-ySide);
+
+		std::vector<VEC3> goals;
+		goals.push_back(start);
+		int r=0;
+		while (r<40)
 		{
-			tmp = goal ;
-			goal = start ;
-			start = tmp ;
-			vPos.push_back(start-xSide+ySide);
-			vPos.push_back(start-xSide-ySide);
-			vPos.push_back(start+xSide-ySide);
-			vPos.push_back(start+xSide+ySide);
+			x = xStartMin + rand() % xStartDelta;
+			goal=VEC3 (x, yGoalMin  + rand() % yGoalDelta,  0) ;
+			if ((goal-goals[r]).norm2()>1000){
+				goals.push_back(goal);
+							r++;
+			}
+
 		}
-		else {
+
+		positions[0]=vPos;
+
+		for (int i = 1 ; i < snakeSize ; i++)
+
+		{
+
+			start=start-ySide-ySide;
+
+			vPos.clear();
+
+
 			vPos.push_back(start+xSide-ySide);
 			vPos.push_back(start+xSide+ySide);
 			vPos.push_back(start-xSide+ySide);
 			vPos.push_back(start-xSide-ySide);
+