//Main Function int main() { i = 0; /* printf("Enter Terminal Colors\n"); for(i;i<4;i++) { printf("T%d : ",i); scanf("%d %d",&terminalColors[i][0],&terminalColors[i][1]); printf("\n"); } */ noOfSteps = 4; // Initial Cost Till BlackHole printCost("Initial Cost Till BlackHole"); noOfSteps = noOfSteps + 2 + 1; printCost("Going to Terminal One"); init_sort(); if (checkForCompletion1()) { printf("Sorted"); } else printf("Not Sorted"); while (1); return 1; }
void Pathfinder::printCost() { printCost(DIR_XP); printCost(DIR_XM); printCost(DIR_ZP); printCost(DIR_ZM); }
void gotoSort() { //forwardJaa(); noOfSteps++; printCost("Going From Terminal to Sort"); }
void nodeFront() { noOfSteps++; printCost("Going Forward"); //forwardJaa(); //printf("\nGoing Straight\n"); }
void goBack() { /* if(Center_white_line>40) { back(); set_color(); while(Center_white_line>40 && (Left_white_line>40 || Right_white_line>40)) { return;} } else { noNatak(); goBack(); } */ noOfSteps++; printCost("Going from Sort to Terminal"); }
void travel(unsigned char nxTerm) { //forwardJaa(); noOfSteps++; printCost("Goint To Sort"); //swapEncounterdAction swapMan(CT % 2, nxTerm); /* lcd_print(1,11,(CT==1 && (nxTerm == 3 || nxTerm== 4)),1);// || lcd_print(1,12,(CT==4 && (nxTerm == 1 || nxTerm== 2)),1); lcd_print(1,13,(CT==2 && (nxTerm == 3 || nxTerm== 4)),1); // || lcd_print(1,14,(CT==3 && (nxTerm == 1 || nxTerm== 2)),1); */ if ((CT == 1 && (nxTerm == 2 || nxTerm == 4)) || (CT == 4 && (nxTerm == 1 || nxTerm == 3))) { nodeLeft(); noOfSteps += 2; noOfSteps += 2; printCost("Going Opposite Side"); //forwardJaa(); //_delay_ms(1000); //forwardJaa(); CT = nxTerm; //changed for drop,pickup swapMan(nxTerm % 2, nxTerm); if (nxTerm == 1 || nxTerm == 4) nodeRight(); else nodeLeft(); } else if ((CT == 2 && (nxTerm == 1 || nxTerm == 3)) || (CT == 3 && (nxTerm == 2 || nxTerm == 4))) { nodeRight(); noOfSteps += 2; noOfSteps += 2; printCost("Going Opposite Side"); //forwardJaa(); //_delay_ms(500); //forwardJaa(); CT = nxTerm; //changed for drop,pickup swapMan(nxTerm % 2, nxTerm); if (nxTerm == 2 || nxTerm == 3) nodeLeft(); else nodeRight(); } else { //_delay_ms(1000); } //forwardJaa(); noOfSteps++; CT = nxTerm; printCost("Going To Terminal"); //stop(); //stop(); //buzzer(); }
std::vector<v3s16> Pathfinder::getPath(ServerEnvironment *env, v3s16 source, v3s16 destination, unsigned int searchdistance, unsigned int max_jump, unsigned int max_drop, PathAlgorithm algo) { #ifdef PATHFINDER_CALC_TIME timespec ts; clock_gettime(CLOCK_REALTIME, &ts); #endif std::vector<v3s16> retval; //check parameters if (env == 0) { ERROR_TARGET << "missing environment pointer" << std::endl; return retval; } m_searchdistance = searchdistance; m_env = env; m_maxjump = max_jump; m_maxdrop = max_drop; m_start = source; m_destination = destination; m_min_target_distance = -1; m_prefetch = true; if (algo == PA_PLAIN_NP) { m_prefetch = false; } int min_x = MYMIN(source.X, destination.X); int max_x = MYMAX(source.X, destination.X); int min_y = MYMIN(source.Y, destination.Y); int max_y = MYMAX(source.Y, destination.Y); int min_z = MYMIN(source.Z, destination.Z); int max_z = MYMAX(source.Z, destination.Z); m_limits.MinEdge.X = min_x - searchdistance; m_limits.MinEdge.Y = min_y - searchdistance; m_limits.MinEdge.Z = min_z - searchdistance; m_limits.MaxEdge.X = max_x + searchdistance; m_limits.MaxEdge.Y = max_y + searchdistance; m_limits.MaxEdge.Z = max_z + searchdistance; v3s16 diff = m_limits.MaxEdge - m_limits.MinEdge; m_max_index_x = diff.X; m_max_index_y = diff.Y; m_max_index_z = diff.Z; delete m_nodes_container; if (diff.getLength() > 5) { m_nodes_container = new MapGridNodeContainer(this); } else { m_nodes_container = new ArrayGridNodeContainer(this, diff); } #ifdef PATHFINDER_DEBUG printType(); printCost(); printYdir(); #endif //validate and mark start and end pos v3s16 StartIndex = getIndexPos(source); v3s16 EndIndex = getIndexPos(destination); PathGridnode &startpos = getIndexElement(StartIndex); PathGridnode &endpos = getIndexElement(EndIndex); if (!startpos.valid) { VERBOSE_TARGET << "invalid startpos" << "Index: " << PP(StartIndex) << "Realpos: " << PP(getRealPos(StartIndex)) << std::endl; return retval; } if (!endpos.valid) { VERBOSE_TARGET << "invalid stoppos" << "Index: " << PP(EndIndex) << "Realpos: " << PP(getRealPos(EndIndex)) << std::endl; return retval; } endpos.target = true; startpos.source = true; startpos.totalcost = 0; bool update_cost_retval = false; switch (algo) { case PA_DIJKSTRA: update_cost_retval = updateAllCosts(StartIndex, v3s16(0, 0, 0), 0, 0); break; case PA_PLAIN_NP: case PA_PLAIN: update_cost_retval = updateCostHeuristic(StartIndex, v3s16(0, 0, 0), 0, 0); break; default: ERROR_TARGET << "missing PathAlgorithm"<< std::endl; break; } if (update_cost_retval) { #ifdef PATHFINDER_DEBUG std::cout << "Path to target found!" << std::endl; printPathLen(); #endif //find path std::vector<v3s16> path; buildPath(path, EndIndex, 0); #ifdef PATHFINDER_DEBUG std::cout << "Full index path:" << std::endl; printPath(path); #endif //finalize path std::vector<v3s16> full_path; for (std::vector<v3s16>::iterator i = path.begin(); i != path.end(); ++i) { full_path.push_back(getIndexElement(*i).pos); } #ifdef PATHFINDER_DEBUG std::cout << "full path:" << std::endl; printPath(full_path); #endif #ifdef PATHFINDER_CALC_TIME timespec ts2; clock_gettime(CLOCK_REALTIME, &ts2); int ms = (ts2.tv_nsec - ts.tv_nsec)/(1000*1000); int us = ((ts2.tv_nsec - ts.tv_nsec) - (ms*1000*1000))/1000; int ns = ((ts2.tv_nsec - ts.tv_nsec) - ( (ms*1000*1000) + (us*1000))); std::cout << "Calculating path took: " << (ts2.tv_sec - ts.tv_sec) << "s " << ms << "ms " << us << "us " << ns << "ns " << std::endl; #endif return full_path; } else { #ifdef PATHFINDER_DEBUG printPathLen(); #endif ERROR_TARGET << "failed to update cost map"<< std::endl; } //return return retval; }