// 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));
}
