void GraphGenerator::operator()() {
mFinished = false;
mCurrentPath.clear();
mCurrentPath.push_back(1);
mActivatorsPath.clear();
mEndPath.clear();
genGraph_Handler(0, 1, 2, Perso::All);
addObstacles();
}
void Ocean::initCells(void) {
addEmptyCells();
addObstacles();
addPredators();
addPrey();
displayStats(-1);
displayBorder();
Ocean1 = this;
}
void AILevelFiller::createObstaclesList(Level *_level)
{
level = _level;
int currentX = (int)(((level->getWidth() / 3.0f) - 1) / 2);
int currentY = 0;
int currentZ = 7;
addObstacles(currentX, currentY, currentZ);
}
bool addObstacles(Key<2> k, int depth, int size, Tree<2>* t){
if(depth==t->getMaxDepth()){
Ocount++;
if(Ocount%((int)pow(2,8))==0){
std::cout << "\r" << "Obstacle creation " << std::setw(10) << Ocount*100.0/pow(2,2*depth) << "\% done, ";
time_t timerNow=time(NULL);
int seconds = (int)difftime(timerNow,timerStart);
seconds=(int)(seconds*(1-( Ocount/pow(2,2*depth)))/( Ocount/pow(2,2*depth)));
int hours = seconds/3600;
int minutes= (seconds/60)%60;
seconds=seconds%60;
std::cout << "time left: " << std::setw(4) << hours << ":"
<< std::setw(2) << minutes << ":"
<< std::setw(2) << seconds;
}
//finest resolution: update obstacle presence
//if obstacles
double p=obstacleProbability(t->getState(k));
if(p!=0.0){
//add obstacle to the tree
t->addObstacle(k);
Node<2>* n=t->getNode(k);
n->setValue(p);
//indicate that the tree was updated
return true;
}else{
//indicate that not changes were performed on the tree
return false;
}
}else{
bool update=false;
//update children
int size2=size>>1;
for(const Key<2>& dir: *(t->getDirections())){
update=addObstacles(k+dir*size2,depth+1,size2,t) || update;
}
//if any children created, get node
if(update){
Node<2>* cur=t->getNode(k);
//prune and update val (single stage, no recurrence (children are up to date))
cur->update(false);
}
//indicate if updates were performed on the tree
return update;
}
}
/////////////////////////////////////////////////////////////////
/// @brief Enable with lighting and texture for behind the scene.
/////////////////////////////////////////////////////////////////
void Physicsworld::init()
{
if (m_isInit) return;
// Enable texturing
glDisable(GL_TEXTURE_2D);
// Enable counter clockwise face ordering
glFrontFace(GL_CCW);
//glEnable(GL_LIGHTING);
glEnable(GL_NORMALIZE);
glEnable( GL_POINT_SMOOTH );
glEnable( GL_MULTISAMPLE_ARB);
glEnable(GL_DEPTH_TEST);
m_isInit = true;
//initalise the obstacles
addObstacles();
}
void MSPP_planning(){
ROS_INFO("MSPP planning");
if(mapChanged){
//Set Search Space Bounds
State<2> minState;
minState[0]=minX(local_map->info);
minState[1]=minY(local_map->info);
State<2> maxState;
maxState[0]=maxX(local_map->info);
maxState[1]=maxY(local_map->info);
t->setStateBounds(minState,maxState);
//std::cout << "min bound : " << minState <<std::endl;
//std::cout << "max bound : " << maxState <<std::endl;
// Set Tree Max Depth
t->setMaxDepth(depth);
//Depth First Obstacle Creation
//*
std::cout << "Obstacle creation " << std::setw(10) << 0.0 << "\% done.";
Ocount=0;
timerStart=time(NULL);
addObstacles(t->getRootKey(),0,t->getRootKey()[0],t);
std::cout << std::endl;
time_t timerNow=time(NULL);
int seconds = (int)difftime(timerNow,timerStart);
int hours = seconds/3600;
int minutes= (seconds/60)%60;
seconds=seconds%60;
std::cout << "Obstacles created in " << std::setw(4) << hours << ":"
<< std::setw(2) << minutes << ":"
<< std::setw(2) << seconds
<< std::endl;
//*/
mapChanged=false;
}
MSP<2> algo(t);
//Set algo parameters
algo.setNewNeighboorCheck(true);
//algo.setMapLearning(true,nb_obstacle_check,isObstacle);
algo.setSpeedUp(true);
algo.setAlpha(2*sqrt(2));
algo.setEpsilon(epsilon);
algo.setLambda1(lambda1);
//algo.setMinRGcalc(true);
bool initAlgo=algo.init(startState,goalState);
std::cout << "start : " << startState <<std::endl;
std::cout << "goal : " << goalState <<std::endl;
std::cout << "init : " << initAlgo <<std::endl;
//Run algo
if(initAlgo && algo.run()){
ROS_INFO_STREAM("Algo init "<<initAlgo<<", planning in progress ...");
current_path_raw=algo.getPath();
/*std::cout << "Path length: " << current_path_raw.size() << std::endl;
std::cout << "Path cost: " << algo.getPathCost() << std::endl;
std::cout << "Path :" << std::endl;
for(std::deque<State<2>>::iterator it=current_path_raw.begin(),end=current_path_raw.end();it!=end;++it){
std::cout << (*it) << " -- ";
}
std::cout << std::endl;*/
current_path_raw.push_back(goalState);
planned=true;
}else{
ROS_INFO("Planning failed");
planned=false;
}
algo.clear();
}
void AILevelFiller::addObstacles(int _currentX,int _currentY,int _currentZ)
{
int levelLength = (int)(level->getLength() / 3.0f) - 1;
if (_currentZ >= levelLength)
return;
int levelWidth = (int)(level->getWidth() / 3.0f);
int currentTempX = _currentX;
int currentTempY = _currentY;
int currentTempZ = _currentZ;
int zGap = updateZGap();
int nextObstacleZ = currentTempZ + zGap + 1;
int difficultyPercent = (int)(difficulty * 100);
checkXState(currentTempX);
if (difficulty <= 1.0f)
difficulty += 0.02f;
if (qrand() % 100 < difficultyPercent)
{
if ( xState & X_STATE_LEFT_EDGE )
{
if ( xState & X_STATE_RIGHT_EDGE )
{
addObstacles(currentTempX, currentTempY, nextObstacleZ);
}
else
{
for (int i = 2; i < levelWidth; i++)
for (int j = currentTempZ; j < nextObstacleZ; j++)
if ((qrand() % 100 < difficultyPercent) && (j <= levelLength))
createAndAddObstacle(i, currentTempY, j);
if (nextObstacleZ < levelLength)
createAndAddObstacle(currentTempX, currentTempY, nextObstacleZ);
addObstacles((++currentTempX), currentTempY, nextObstacleZ);
}
}
else if ( xState & X_STATE_RIGHT_EDGE )
{
for (int i = 0;i < levelWidth -2; i++)
for (int j = currentTempZ; j < nextObstacleZ; j++)
if ((qrand() % 100 < difficultyPercent) && (j <= levelLength))
createAndAddObstacle(i, currentTempY, j);
if (nextObstacleZ < levelLength)
createAndAddObstacle(currentTempX, currentTempY, nextObstacleZ);
addObstacles((--currentTempX), currentTempY, nextObstacleZ);
}
else
{
for (int i = 0; i < currentTempX -2; i++)
for (int j = currentTempZ; j < nextObstacleZ; j++)
if ((qrand() % 100 < difficultyPercent) && (j <= levelLength))
createAndAddObstacle(i, currentTempY, j);
for (int i = currentTempX +2; i < levelWidth; i++)
for (int j = currentTempZ; j < nextObstacleZ; j++)
if ((qrand() % 100 < difficultyPercent) && (j <= levelLength))
createAndAddObstacle(i, currentTempY, j);
if (nextObstacleZ < levelLength)
createAndAddObstacle(currentTempX, currentTempY, (currentTempZ + zGap +1));
qrand() % 100 < 50 ? addObstacles((++currentTempX), currentTempY, nextObstacleZ):
addObstacles((--currentTempX), currentTempY, nextObstacleZ);
}
}
else
{
addObstacles(currentTempX, currentTempY, currentTempZ + 3);
}
}
