// doesn't this always return iteration? // This does always return iteration. Sorry. I don't know the hint of this commet. int PathFinder::shortestLength(int iteration=1){ std::vector<Point> neighbors; std::set<Point>::iterator it; // Why allocate newClosed on the heap? make it stack allocated, and you get destruction for free //yest the swap is better choice std::set<Point> newClosed; for(it=closed->begin(); it != closed->end(); it++){ neighbors=getNeighbors(*it); for(int i=0; i< neighbors.size(); i++){ Point neighbor = neighbors[i]; // magic numbers like -1 aren't good here //don't quite get the idea of this comment if(validPoint(neighbor) && (*open)[neighbor] == -1) return iteration; if(validPoint(neighbor) && (*open)[neighbor] == 0 && (*pointStatus)[neighbor] !=1){ (*open)[neighbor]=iteration +1; newClosed.insert(neighbor); } } } closed->swap(newClosed); iteration=shortestLength(iteration+1); return iteration; }
// takes a char (L, R, B), point, and initial direction and gets // the next point static point charToPoint(char letter, int difference, point currentPoint) { if (difference == COL_DEC) { if (letter == 'L') { currentPoint.y++; } else if (letter == 'R') { currentPoint.y--; } else if (letter == 'B') { currentPoint.x++; } } else if (difference == COL_INC) { if (letter == 'L') { currentPoint.y--; } else if (letter == 'R') { currentPoint.y++; } else if (letter == 'B') { currentPoint.x--; } } else if (difference == ROW_DEC) { if (currentPoint.x%2 == currentPoint.y%2) { if (letter == 'L') { currentPoint.y--; } else if (letter == 'R') { currentPoint.x++; } else if (letter == 'B') { currentPoint.y++; } } else { if (letter == 'L') { currentPoint.x--; } else if (letter == 'R') { currentPoint.y--; } else if (letter == 'B') { currentPoint.y++; } } } else if (difference == ROW_INC) { if (currentPoint.x%2 == currentPoint.y%2) { if (letter == 'L') { currentPoint.x++; } else if (letter == 'R') { currentPoint.y++; } else if (letter == 'B') { currentPoint.y--; } } else { if (letter == 'L') { currentPoint.y++; } else if (letter == 'R') { currentPoint.x--; } else if (letter == 'B') { currentPoint.y--; } } } if (!validPoint(currentPoint.x, currentPoint.y)) { currentPoint.x = -1; } return currentPoint; }
// Create the board static void createBoard(Game g, int discipline[], int dice[]) { g->gameBoard = malloc (sizeof (struct _board)); assert(g->gameBoard != NULL); // Create the points that exist int x = 0; while (x < NUM_POINTS_X) { int y = 0; while (y < NUM_POINTS_Y) { if (validPoint(x, y)) { createPoint(g, x, y); } y++; } x++; } // Create the regions that exist x = 0; int i = 0; while (x < NUM_REGIONS_X) { int y = 0; while (y < NUM_REGIONS_Y) { if (validRegion(x, y)) { createRegion(g, x, y, i, dice[i], discipline[i]); // Add the region to the points around it addRegionToPoints(g, x, y); i++; } y++; } x++; } }
// free all the memory malloced for points static void disposePoints(Game g) { int x = 0; int y = 0; while (x < NUM_POINTS_X) { y = 0; while (y < NUM_POINTS_Y) { if (validPoint(x, y)) { free(g->gameBoard->points[x][y]); } y++; } x++; } }
std::vector<Point> PathFinder::getPath(Point point, int iteration) { //reverst the direction to the start --> end if(iteration==0){ std::reverse(path.begin(), path.end()); return path; } std::vector<Point> neighbors = getNeighbors(point); for(int i=0; i<neighbors.size(); i++){ Point neighbor=neighbors[i]; if(validPoint(neighbor) && (*open)[neighbor]==iteration){ path.push_back(neighbor); return getPath(neighbor, iteration-1); } } //If there is no available path, return the empty std::vector<Point> result; return result; }
std::vector<Eigen::Vector3f> PathPlanner::getPath(Eigen::Vector3f start, Eigen::Vector3f end, cv::Mat3b * image) { std::vector<Eigen::Vector3f> path; std::vector<Eigen::Vector2i> pathImg; //Objects are outside the configuration space, so get the closest point Eigen::Vector2i lastPoint; if(!validPoint(worldToImg(end))) { if(!findClosestPoint(start, end, bot_in_pixels_)) { return path; } } //Check if we can just go straight there unobstructed if(traceLine(worldToImg(start), worldToImg(end))) { path.push_back(start); path.push_back(end); } else if(aStar(worldToImg(start), worldToImg(end), pathImg)) { pathImg = smoothPath(pathImg); for(size_t i = 0; i < pathImg.size(); i++) { path.push_back(imgToWorld(pathImg.at(i))); } } if(image) { drawPath(path, *image); } return path; }
// moving average on vector nPoints a,(b,(c)) of given sensor, thread safe // container: unique class object // xma: average type // pv: use xma to smooth position or velocity // nPoints: window length, longer window equals smoother results, but slower end-point convergence // estimate: nPoints = freq * w_time; [s = 1/s * s] // initEndEff: user set start point void moving_average(DataContainer* container, int sensor_in) { // xma average type int xma = container->getInt(1); int pv = container->getInt(2); int nPoints = container->getInt(3); // tell container in getVec which flag to strike int accessor; // to smooth vector entries int a = 0; int b = 0; int c = 0; // interactive marker menu 1:notchecked 2:checked double ui_validjumps = 1; // iteration counter int count = 1; // invalid iteration counter int invalidCount = 0; // time between last saved window-entry double t_delta = 0.; // smoothed entries double x = 0.; double y = 0.; double z = 0.; // velocity is calculated by the last two smoothed entries divided by their t_delta // Suffix _delta indicates incremental steps. double v_x = 0.; double v_y = 0.; double v_z = 0.; double x_delta = 0.; double y_delta = 0.; double z_delta = 0.; double v_x_delta = 0.; double v_y_delta = 0.; double v_z_delta = 0.; // EMA adjusting factor double alpha; // XMA weights std::vector<double> weights; // temporary for setVec, p:3,4,5 v:6,7,8 std::vector<double> vec_out(9, 0); // first t_delta will therefore be huge vec_out[0] = ros::Time::now().toNSec(); // info on how many points were involved vec_out[2] = nPoints; // temporary vector for getVec std::vector<double> tmp(6, 0); // intital end-effector position std::vector<double> initEndEff(6, 0); // window holds past nPoints, w[length-1] is the oldest entry std::vector<std::vector<double> > w; // timespan before rechecking Flag struct timespec req, rem; req.tv_sec = 0; req.tv_nsec = 30000000; // 33Hz if (sensor_in == 15) { // 3D point accessor = 1; a = 3; b = 4; c = 5; initEndEff[0] = ros::Time::now().toNSec(); initEndEff[1] = sensor_in; initEndEff[2] = nPoints; initEndEff[3] = container->getInt(5); initEndEff[4] = container->getInt(6); initEndEff[5] = container->getInt(7); for (int i = 0; i < nPoints; ++i) { w.push_back(initEndEff); } vec_out[1] = 16; } else { ROS_ERROR(NNAME": xma for Sensor_in %d not set up.", sensor_in); return; } // calculate weights weights.resize(nPoints, 1. / nPoints); // SMA if (xma == 1) { // LMA double sum = 0.; for (int i = 1; i <= nPoints; ++i) { weights[nPoints - i] = 1. / nPoints * i; sum += 1. / nPoints * i; } // normalize, division gets superfluous for (int i = 0; i < nPoints; ++i) weights[i] /= sum; } else if (xma == 2) { // EMA // choose alpha (0;1), close to 1: aggressive response // 0: stay on init value // 1: output the input alpha = nPoints/100; } while (!ros::isShuttingDown() && container->getInt(14) == 0) { // do until valid vector bool valid = false; while (!valid) { if (container->checkVecFlag(accessor, sensor_in) == 1) { container->getVec(accessor, sensor_in, tmp); if (validPoint(container, tmp, w[0])) { valid = true; w.insert(w.begin(), 1, tmp); w.pop_back(); container->setInt(12, 0); invalidCount = 0; } else if (invalidCount > 90) { container->setInt(12, 1); invalidCount = 0; } else { invalidCount++; } } else { nanosleep(&req, &rem); if (ros::isShuttingDown()) { return; } } } if (xma < 2) { // SMA LMA x = 0.; y = 0.; z = 0.; for (int i = 0; i < nPoints; ++i) { x += w[i][a] * weights[i]; y += w[i][b] * weights[i]; z += w[i][c] * weights[i]; } } else { // EMA x = alpha * w[0][a] + (1. - alpha) * vec_out[3]; y = alpha * w[0][b] + (1. - alpha) * vec_out[4]; z = alpha * w[0][c] + (1. - alpha) * vec_out[5]; } // calculate deltas and (velocity @EXPERIMENTAL), change! if (accessor == 1) t_delta = (w[0][0] - vec_out[0]); // kinect uses secs else t_delta = (w[0][0] - vec_out[0]) * 1e-9; // android nsces to secs if (pv > 0) { x_delta = x - vec_out[3]; y_delta = y - vec_out[4]; z_delta = z - vec_out[5]; if (pv > 1) { v_x = x_delta / t_delta; v_y = y_delta / t_delta; v_z = z_delta / t_delta; if (pv > 2 && count > 0) { v_x_delta = (v_x - vec_out[6]); v_y_delta = (v_y - vec_out[7]); v_z_delta = (v_z - vec_out[8]); } } } // refresh outgoing vector vec_out[0] = w[0][0]; if (pv == 0) { vec_out[3] = x; vec_out[4] = y; vec_out[5] = z; } else if (pv == 1) { vec_out[3] = x_delta; vec_out[4] = y_delta; vec_out[5] = z_delta; } else if (pv == 2) { vec_out[3] = v_x; vec_out[4] = v_y; vec_out[5] = v_z; } else if (pv == 3 && count > 0) { vec_out[3] = v_x_delta; vec_out[4] = v_y_delta; vec_out[5] = v_z_delta; } container->setVec(vec_out); // hold on to data for next loop if (pv > 0) { vec_out[3] = x; vec_out[4] = y; vec_out[5] = z; if (pv > 2) { vec_out[6] = v_x; vec_out[7] = v_y; vec_out[8] = v_z; } } count++; } }
// returns TRUE if it is legal for the current // player to make the specified action, FALSE otherwise-> // // "legal" means everything is legal: // * that the action code is a valid action code which is legal to // be made at this time // * that any path is well formed and legal ie consisting only of // the legal direction characters and of a legal length, // and which does not leave the island into the sea at any stage-> // * that disciplines mentioned in any retraining actions are valid // discipline numbers, and that the university has sufficient // students of the correct type to perform the retraining // // eg when placing a campus consider such things as: // * is the path a well formed legal path // * does it lead to a vacent vertex? // * under the rules of the game are they allowed to place a // campus at that vertex? (eg is it adjacent to one of their ARCs?) // * does the player have the 4 specific students required to pay for // that campus? // It is not legal to make any action during Terra Nullis ie // before the game has started-> // It is not legal for a player to make the moves OBTAIN_PUBLICATION // or OBTAIN_IP_PATENT (they can make the move START_SPINOFF) // you can assume that any pths passed in are NULL terminated strings-> int isLegalAction(Game g, action a) { int isLegal = TRUE; printf("Checking if an action is legal\n"); int player = getWhoseTurn(g); int flag = 1; // Protect from stupid shit if ((a.actionCode < PASS) || (a.actionCode > RETRAIN_STUDENTS)) { printf("Invalid Action Code\n"); flag = 0; } else if (a.actionCode == RETRAIN_STUDENTS) { printf("You've Called a retrain, checking if discipline"); printf("within the bounds\n"); // If it's a retrain action make sure the to // and from are in a nice range. if ((a.disciplineTo < STUDENT_THD) || (a.disciplineTo > STUDENT_MMONEY)) { printf("Invalid Discpline Code\n"); flag = 0; } if ((a.disciplineFrom < STUDENT_THD) || (a.disciplineFrom > STUDENT_MMONEY)) { printf("Invalid Discpline Code\n"); flag = 0; } } printf("The Flag when testing is %d\n", flag); if (flag == 1) { // Check all the basic stuff if (getTurnNumber(g) == TERRA_NULLIS) { isLegal = FALSE; } else if ((a.actionCode < 0) || (a.actionCode > MAX_ACTION)) { isLegal = FALSE; } else if ((getWhoseTurn(g) < 0) || (getWhoseTurn(g) > NUM_UNIS)) { isLegal = FALSE; } else if (a.actionCode == OBTAIN_IP_PATENT || a.actionCode == OBTAIN_PUBLICATION) { isLegal = FALSE; } if (a.actionCode <= 3 && a.actionCode > 0 && isLegal) { if (validString(a.destination) == FALSE) { isLegal = FALSE; } else if (validPoint(pathToPoint(a.destination).x, pathToPoint(a.destination).y) == FALSE) { isLegal = FALSE; } } // edge actionEdge = pathToEdgeF (a.destination); // Check that someone can get an arc if (a.actionCode == OBTAIN_ARC && isLegal) { printf("Checking if the arc is legal. Arc:%s\n", a.destination); if (validNewEdge(g, pathToEdgeF(a.destination), player) == FALSE) { printf("Geographically Valid Edge\n"); isLegal = FALSE; } else if (getStudents(g, player, STUDENT_BQN) < 1) { isLegal = FALSE; } else if (getStudents(g, player, STUDENT_BPS) < 1) { isLegal = FALSE; } } // Check that someone can get a Campus if (a.actionCode == BUILD_CAMPUS && isLegal) { if (validNewContents(g, a.destination, player) == FALSE) { isLegal = FALSE; } else if (getStudents(g, player, STUDENT_BQN) < 1) { isLegal = FALSE; } else if (getStudents(g, player, STUDENT_BPS) < 1) { isLegal = FALSE; } else if (getStudents(g, player, STUDENT_MJ) < 1) { isLegal = FALSE; } else if (getStudents(g, player, STUDENT_MTV) < 1) { isLegal = FALSE; } } if (a.actionCode == BUILD_GO8 && isLegal) { point actionPoint = pathToPoint(a.destination); if (g->gameBoard->points[actionPoint.x][actionPoint.y]->contents != player) { isLegal = FALSE; } else if (getStudents(g, player, STUDENT_MJ) < 2) { isLegal = FALSE; } else if (getStudents(g, player, STUDENT_MMONEY) < 3) { isLegal = FALSE; } else if (getTotalGO8s(g) == 8) { isLegal = FALSE; } } if (a.actionCode == START_SPINOFF && isLegal) { if (getStudents(g, player, STUDENT_MJ) < 1) { isLegal = FALSE; } else if (getStudents(g, player, STUDENT_MTV) < 1) { isLegal = FALSE; } else if (getStudents(g, player, STUDENT_MMONEY) < 1) { isLegal = FALSE; } } if (a.actionCode == RETRAIN_STUDENTS && isLegal) { int exchange = getExchangeRate(g, player, a.disciplineFrom, a.disciplineTo); if (getStudents(g, player, a.disciplineFrom) < exchange) { isLegal = FALSE; } else if (a.disciplineFrom == STUDENT_THD) { isLegal = FALSE; } } } else if (flag == 0) { isLegal = FALSE; } return isLegal; }
std::vector<Eigen::Vector2i> PathPlanner::getNeighbours(Eigen::Vector2i p) { Eigen::Vector2i p0(p(0) + 1, p(1)); Eigen::Vector2i p1(p(0) - 1, p(1)); Eigen::Vector2i p2(p(0), p(1) + 1); Eigen::Vector2i p3(p(0), p(1) - 1); Eigen::Vector2i p4(p(0) + 1, p(1) + 1); Eigen::Vector2i p5(p(0) - 1, p(1) - 1); Eigen::Vector2i p6(p(0) - 1, p(1) + 1); Eigen::Vector2i p7(p(0) + 1, p(1) - 1); std::vector<Eigen::Vector2i> points; if(validPoint(p0)) points.push_back(p0); if(validPoint(p1)) points.push_back(p1); if(validPoint(p2)) points.push_back(p2); if(validPoint(p3)) points.push_back(p3); //Disallow walking through diagonal cracks if(validPoint(p4) && (validPoint(p0) || validPoint(p2))) points.push_back(p4); if(validPoint(p5) && (validPoint(p1) || validPoint(p3))) points.push_back(p5); if(validPoint(p6) && (validPoint(p1) || validPoint(p2))) points.push_back(p6); if(validPoint(p7) && (validPoint(p0) || validPoint(p3))) points.push_back(p7); return points; }
bool PathPlanner::findClosestPoint(Eigen::Vector3f start, Eigen::Vector3f & end, int radius) { Eigen::Vector2i imgPoint = worldToImg(end); std::vector<Eigen::Vector2i> points, validPoints; while(radius-- > 0) { //Collect valid pixels on the Bresenham circle int f = 1 - radius; int ddFx = 0; int ddFy = -2 * radius; int x = 0; int y = radius; points.push_back(Eigen::Vector2i(imgPoint(0), imgPoint(1) + radius)); points.push_back(Eigen::Vector2i(imgPoint(0), imgPoint(1) - radius)); points.push_back(Eigen::Vector2i(imgPoint(0) + radius, imgPoint(1))); points.push_back(Eigen::Vector2i(imgPoint(0) - radius, imgPoint(1))); while(x < y) { if(f >= 0) { y--; ddFy += 2; f += ddFy; } x++; ddFx += 2; f += ddFx + 1; points.push_back(Eigen::Vector2i(imgPoint(0) + x, imgPoint(1) + y)); points.push_back(Eigen::Vector2i(imgPoint(0) - x, imgPoint(1) + y)); points.push_back(Eigen::Vector2i(imgPoint(0) + x, imgPoint(1) - y)); points.push_back(Eigen::Vector2i(imgPoint(0) - x, imgPoint(1) - y)); points.push_back(Eigen::Vector2i(imgPoint(0) + y, imgPoint(1) + x)); points.push_back(Eigen::Vector2i(imgPoint(0) - y, imgPoint(1) + x)); points.push_back(Eigen::Vector2i(imgPoint(0) + y, imgPoint(1) - x)); points.push_back(Eigen::Vector2i(imgPoint(0) - y, imgPoint(1) - x)); } } for(size_t i = 0; i < points.size(); i++) { if(image.at<cv::Vec<unsigned char, 3> >(points.at(i)(1), points.at(i)(0))[0] == 0) { validPoints.push_back(points.at(i)); } } Eigen::Vector2i startPoint = worldToImg(start); float bestDistance = std::numeric_limits<float>::max(); for(size_t i = 0; i < validPoints.size(); i++) { float distance = (startPoint.cast<float>() - validPoints.at(i).cast<float>()).norm(); if(distance < bestDistance) { bestDistance = distance; end = imgToWorld(validPoints.at(i)); } } return validPoint(worldToImg(end)); }