bool copySolution(solution_t solution, solution_t *newSolution, int maxSizeOfSolution) {
int i = 0;
*newSolution = createSolution(maxSizeOfSolution);
memcpy(*newSolution, solution, maxSizeOfSolution*sizeof(position_t) );
return true;
}
void MainWindow::rightClickMenu(const QPoint &pos){
QPoint PItem = ui->list_LevelSet->mapToGlobal(pos);
if(ui->list_LevelSet->indexAt(pos).row() >= 0){
QMenu submenu;
submenu.addAction("Regenerate Level");
submenu.addAction("Delete Level");
submenu.addAction("View Solution");
QAction* rightClickItem = submenu.exec(PItem);
if(rightClickItem && rightClickItem->text().contains("Delete Level")){
QListWidgetItem* item = ui->list_LevelSet->takeItem(ui->list_LevelSet->indexAt(pos).row());
if(item != NULL){
int levelNum = item->data(Qt::UserRole).toInt();
Generator.deleteLevel(levelNum);
vector<SokoGenerator::Level> levelSet = Generator.getLevels();
for(int i = levelNum; i < ui->list_LevelSet->count(); i++){
ui->list_LevelSet->item(i)->setData(Qt::UserRole, i);
int millis = levelSet[i].generationTime % 1000;
int seconds = ((int)levelSet[i].generationTime / 1000) % 60 ;
int minutes = ((int)levelSet[i].generationTime / (1000*60)) % 60;
QString padMillis = QString("%1").arg(millis, 3, 10, QChar('0'));
QString padSeconds = QString("%1").arg(seconds, 2, 10, QChar('0'));
QString padMinutes = QString("%1").arg(minutes, 2, 10, QChar('0'));
QString diff = levelSet[i].difficulty;
ui->list_LevelSet->item(i)->setText("Level " + QString::number(i+1) + " - " + padMinutes + ":" + padSeconds + ":" + padMillis + " - " + diff);
}
if(item->data(Qt::UserRole).toInt() == ui->list_LevelSet->count()){
ui->list_LevelSet->setCurrentRow(ui->list_LevelSet->count()-1);
}
else{
ui->list_LevelSet->setCurrentRow(item->data(Qt::UserRole).toInt());
}
delete item;
}
}
else if(rightClickItem && rightClickItem->text().contains("Regenerate Level")){
int row = ui->list_LevelSet->indexAt(pos).row();
if(row >= 0){
regenerateLevel(row);
displayLevel(row);
}
}
else if(rightClickItem && rightClickItem->text().contains("View Solution")){
int row = ui->list_LevelSet->indexAt(pos).row();
if(row >= 0){
QString solution = createSolution(row);
QMessageBox::about(this, "Level " + QString::number(row+1) + " Solution", solution);
}
}
}
}
int main( int argc, char *argv[]) {
solution_list_t solutions = NULL;
int numberOfSolutions = 0;
clock_t start, end;
unsigned long durationInMilliseconds;
int i;
int sizeOfBoard;
bool displayQueens = false;
if(argc == 2) {
sizeOfBoard = atoi(argv[1]);
} else if( argc == 3) {
if(strcmp(argv[1], "-d") == 0) {
sizeOfBoard = atoi(argv[2]);
displayQueens = true;
} else if(strcmp(argv[2], "-d") == 0) {
sizeOfBoard = atoi(argv[1]);
displayQueens = true;
} else {
printUsageAndExit();
}
} else {
printUsageAndExit();
}
// printf("%d\n", sizeOfBoard);
solution_t working = createSolution(sizeOfBoard);
start = clock();
solve( sizeOfBoard, &solutions, &numberOfSolutions, working, 0);
end = clock();
durationInMilliseconds = (end-start) * 1000 / CLOCKS_PER_SEC;
printf("N-Queens Found %d Solutions in %.6fs on a %dx%d board.\n", numberOfSolutions, durationInMilliseconds/1000.0, sizeOfBoard, sizeOfBoard);
if(displayQueens) {
displaySolutions(solutions, numberOfSolutions, sizeOfBoard);
}
deleteSolutions(solutions,numberOfSolutions);
deleteSolution(working);
return 0;
}
int main(int argc, char * argv[]) {
int i,j;
time(&initial);
srand(SEED);
/* Default values */
outFile = stdout;
maxAlpha = 2;
maxIter = 100;
maxTime = 30;
randomSeed = SEED;
simpleOutput = 0;
/* Read arguments */
if( argc > 7 )
argc = 7;
switch(argc) {
case 7:
simpleOutput = atoi(argv[6]);
case 6:
if( !(randomSeed = atoi(argv[5])) )
leave(argv[0]);
case 5:
if( !(maxTime = atoi(argv[4])) )
leave(argv[0]);
case 4:
if( !(maxIter = atoi(argv[3])) )
leave(argv[0]);
case 3:
if( !(maxAlpha = atoi(argv[2])) )
leave(argv[0]);
case 2:
if( simpleOutput ) {
if( !(outFile = fopen(argv[1],"a")) )
leave(argv[0]);
break;
}
if( !(outFile = fopen(argv[1],"w")) )
leave(argv[0]);
}
readInput(stdin);
/* Initiate positions */
for( i = 0 ; i < n ; ++i ) {
pOrd[i].ideal = planes[i].ideal;
pOrd[i].pos = i;
}
qsort (pOrd, n, sizeof(struct planeOrder), compIdealT);
for( i = 0 ; i < n ; ++i ) {
planes[pOrd[i].pos].pos = i;
}
/* Create lp instance */
glp_prob * Prob;
Prob = glp_create_prob();
glp_set_prob_name(Prob, "Airplane Landing Problem");
glp_set_obj_name(Prob, "Cost");
/* Create basic constraints */
for( i = 0 ; i < n ; ++i ) {
addBasicRestriction(Prob,i);
}
glp_create_index(Prob);
/* Create separation constraints and order variables (&ij) if necessary */
for( i = 0 ; i < n ; ++i ) {
for( j = i+1 ; j < n ; ++j ) {
if( planes[i].latest >= planes[j].earliest &&
planes[j].latest >= planes[i].earliest ) {
addOrderConstraint(Prob,i,j);
} else if ( planes[i].latest < planes[j].earliest &&
planes[i].latest + planes[i].sep[j] >= planes[j].earliest ) {
addSeparationConstraint(Prob, i, j);
} else if ( planes[j].latest < planes[i].earliest &&
planes[j].latest + planes[j].sep[i] >= planes[i].earliest ) {
addSeparationConstraint(Prob, j, i);
}
}
}
/* Write problem in MPS format so glpsol can (try to) solve it */
glp_write_mps(Prob, GLP_MPS_FILE, NULL,"mpsProblem.txt");
glp_delete_index(Prob);
glp_create_index(Prob);
/* GRASP */
/* Data to handle glp solving, time checking and solution generating */
glp_smcp * param = malloc(sizeof(glp_smcp));
glp_init_smcp(param);
param->msg_lev = GLP_MSG_ERR;
int solution[MAXSIZE], timeAux[MAXSIZE], t;
double currResult = DBL_MAX, bestResult = DBL_MAX;
alpha = 0;
time_t start, curr;
time(&start);
for( t = 0 ; t < maxIter ; ++t ) {
/* Greedy solution generation */
while(createSolution(solution,timeAux,0))
alpha = n;
/* Building the right constraints */
mapSolution(Prob,solution);
/* Solving with glpsol */
param->presolve = GLP_ON;
glp_simplex(Prob,param);
param->presolve = GLP_OFF;
currResult = glp_get_obj_val(Prob);
/* Local search using the first increase */
for( i = 0 ; i < n-1 ; ++i ) {
/* Swap two adjacent planes */
swapConstraint(Prob,i,solution,0);
glp_simplex(Prob,param);
/* Check for improvements */
if( GLP_OPT == glp_get_status(Prob) && glp_get_obj_val(Prob) < currResult ) {
currResult = glp_get_obj_val(Prob);
/* Changing the solution */
int swp;
swp = solution[i];
solution[i] = solution[i+1];
solution[i+1] = swp;
/* Restarting */
i = -1;
} else
swapConstraint(Prob,i,solution,1);
}
/* Checking improvements */
if( bestResult > currResult ) {
bestResult = currResult;
for( i = 0 ; i < n ; ++i )
planes[solution[i]].pos = i;
}
/* Choosing alpha */
alpha = rand()%(maxAlpha+1);
/* Is our time up? */
time(&curr);
if( difftime(curr,start) > maxTime )
break;
}
/* Print Answer */
printResult(Prob, stdout);
if( outFile ) {
printResult(Prob, outFile);
fclose(outFile);
}
return 0;
}
/* Creates a solution in a greedy randomized way */
int createSolution( int solution[], int time[], int pos ) {
int i,j;
int valid[MAXSIZE];
double cost[MAXSIZE];
/* Initialize valid[] */
for( i = 0 ; i < n ; ++i )
valid[i] = 1;
/* Find out who has been included */
for( i = 0 ; i < pos ; ++i )
valid[solution[i]] = 0;
/* For all planes not yet included, find the one
whose latest arrival is minimal. */
int arrivalLimit = INT_MAX;
for( i = 0 ; i < n ; ++i )
if( valid[i] && arrivalLimit > planes[i].latest )
arrivalLimit = planes[i].latest;
/* Find all the planes that can be placed next, their
minimum arrival times and respective costs */
int arrival = 0;
double posDiff;
for( i = 0 ; i < n ; ++i )
if( valid[i] ) {
/* Respect the time distance between planes */
for( j = pos - 1 ; j >= 0 ; --j )
if( arrival < planes[solution[j]].sep[i] + time[solution[j]] )
arrival = planes[solution[j]].sep[i] + time[solution[j]];
/* Respect own plane limits */
if( arrival > planes[i].latest )
return 1; /* Impossible */
if( arrival < planes[i].earliest )
arrival = planes[i].earliest;
/* Check if this plane has to arrive after another one */
if( arrival > arrivalLimit ) {
valid[i] = 0;
continue;
}
/* Calculate time and cost of arriving the plane now */
time[i] = arrival;
posDiff = (pos - planes[i].pos);
if( posDiff < 0 )
posDiff = -posDiff;
cost[i] = posDiff;
}
/* Find out the best option */
double bestOption;
tryAgain:
bestOption = DBL_MAX - 2* alpha;
for( i = 0 ; i < n ; ++i )
if( valid[i] && bestOption > cost[i] )
bestOption = cost[i];
/* Find out the number of options */
int numValid;
numValid = 0;
for( i = 0 ; i < n ; ++i )
if( valid[i] && cost[i] <= bestOption + alpha )
++numValid;
if( !numValid )
return 1;
/* Select an option close or equal to the best */
int option, counter;
option = rand()%numValid + 1;
counter = 0;
for( i = 0 ; i < n ; ++i )
if( valid[i] && cost[i] <= bestOption + alpha )
if( ++counter == option )
break;
solution[pos] = i;
/* Check if the solution is done */
if( ++pos == n )
return 0;
/* Check if the rest of the solution is feasible */
if( createSolution( solution, time, pos ) ) {
valid[i] = 0;
--pos;
goto tryAgain;
}
return 0;
}
bool BiRrt::plan(bool bQuiet)
{
if (!bQuiet)
{
SABA_INFO << "Starting BiRrt planner" << std::endl;
switch (rrtMode)
{
case eExtend:
cout << "-- ModeA: RRT-EXTEND" << endl;
break;
case eConnect:
cout << "-- ModeA: RRT-CONNECT" << endl;
break;
case eConnectCompletePath:
cout << "-- ModeA: RRT-CONNECT (only complete paths)" << endl;
break;
default:
break;
}
switch (rrtMode2)
{
case eExtend:
cout << "-- ModeB: RRT-EXTEND" << endl;
break;
case eConnect:
cout << "-- ModeB: RRT-CONNECT" << endl;
break;
case eConnectCompletePath:
cout << "-- ModeB: RRT-CONNECT (only complete paths)" << endl;
break;
default:
break;
}
}
if (!isInitialized())
{
SABA_ERROR << " planner: not initialized..." << std::endl;
return false;
}
cycles = 0;
int distChecksStart = cspace->performaceVars_distanceCheck;
int colChecksStart = cspace->performaceVars_collisionCheck;
bool found = false;
stopSearch = false;
clock_t startClock = clock();
solution.reset();
RobotPtr robot = cspace->getRobot();
bool bVisStatus = true;
if (robot)
{
bVisStatus = robot->getUpdateVisualizationStatus();
robot->setUpdateVisualization(false);
}
ExtensionResult extResultA;
ExtensionResult extResultB;
bool switched = false;
int *lastIDA = &lastAddedID;
int *lastIDB = &lastAddedID2;
CSpaceTreePtr treeA = tree;
CSpaceTreePtr treeB = tree2;
RrtMethod rrtModeA = rrtMode;
RrtMethod rrtModeB = rrtMode2;
// PLANNING LOOP
do
{
// CHOOSE A RANDOM CONFIGURATION (NOT GOAL DIRECTED, ONLY RANDOMLY)
cspace->getRandomConfig(tmpConfig, true);
LOCAL_DEBUG("----------------------------------------------------" << endl);
LOCAL_DEBUG("Random Conf:" << endl << tmpConfig << endl);
if (switched)
{
LOCAL_DEBUG ("SWITCHED" << endl);
lastIDA = &lastAddedID2;
lastIDB = &lastAddedID;
treeA = tree2;
treeB = tree;
rrtModeA = rrtMode2;
rrtModeB = rrtMode;
} else
{
lastIDA = &lastAddedID;
lastIDB = &lastAddedID2;
treeA = tree;
treeB = tree2;
rrtModeA = rrtMode;
rrtModeB = rrtMode2;
}
switch (rrtModeA)
{
case eExtend:
extResultA = extend(tmpConfig, treeA, *lastIDA);
break;
case eConnect:
extResultA = connectUntilCollision(tmpConfig, treeA, *lastIDA);
break;
case eConnectCompletePath:
extResultA = connectComplete(tmpConfig, treeA, *lastIDA);
break;
default:
break;
}
LOCAL_DEBUG ("ExtResultA:" << extResultA << endl);
if (extResultA==eError)
stopSearch = true;
if (extResultA==ePartial || extResultA==eSuccess)
{
// update config
LOCAL_DEBUG ("Last ID A:" << *lastIDA << endl);
CSpaceNodePtr n = treeA->getNode(*lastIDA);
tmpConfig = n->configuration;
LOCAL_DEBUG ("Tmp goal B:" << endl << tmpConfig << endl);
switch (rrtModeB)
{
case eExtend:
extResultB = extend(tmpConfig, treeB, *lastIDB);
break;
case eConnect:
extResultB = connectUntilCollision(tmpConfig, treeB, *lastIDB);
break;
case eConnectCompletePath:
extResultB = connectComplete(tmpConfig, treeB, *lastIDB);
break;
default:
break;
}
LOCAL_DEBUG ("Last ID B:" << *lastIDB << endl);
LOCAL_DEBUG ("ExtResultB:" << extResultB << endl);
if (extResultB==eError)
stopSearch = true;
if (extResultB==eSuccess)
{
goalNode = treeB->getNode(*lastIDB);
if (!goalNode)
{
SABA_ERROR << " no node for ID: " << lastIDB << endl;
stopSearch = true;
} else
{
found = true;
}
}
}
cycles++;
switched = !switched;
} while (!stopSearch && cycles<maxCycles && !found);
clock_t endClock = clock();
long diffClock = (long)(((float)(endClock - startClock) / (float)CLOCKS_PER_SEC) * 1000.0);
planningTime = (float)diffClock;
if (!bQuiet)
{
SABA_INFO << "Needed " << diffClock << " ms of processor time." << std::endl;
SABA_INFO << "Created " << tree->getNrOfNodes() << " + " << tree2->getNrOfNodes() << " = " << tree->getNrOfNodes()+tree2->getNrOfNodes() << " nodes." << std::endl;
SABA_INFO << "Collision Checks: " << (cspace->performaceVars_collisionCheck-colChecksStart) << std::endl;
SABA_INFO << "Distance Calculations: " << (cspace->performaceVars_distanceCheck-distChecksStart) << std::endl;
int nColChecks = (cspace->performaceVars_collisionCheck-colChecksStart);
if (diffClock>0)
{
float fPerf = (float)nColChecks / (float)diffClock * 1000.0f;
std::cout << "Performance: " << fPerf << " cps (collision-checks per second)." << std::endl;
}
}
if (robot && bVisStatus)
{
robot->setUpdateVisualization(bVisStatus);
}
if (found)
{
if (!bQuiet)
SABA_INFO << "Found RRT solution with " << cycles << " cycles."<< std::endl;
createSolution(bQuiet);
return true;
}
// something went wrong...
if (cycles>=maxCycles)
{
SABA_WARNING << " maxCycles exceeded..." << std::endl;
}
if (stopSearch)
{
SABA_WARNING << " search was stopped..." << std::endl;
}
return false;
}
