void Astar::begin(){
//let's create a new state from the start node using the start node
//double check the naming it's confusing
start.create_state(start);
//push to the front of the openlist list, this should be the openlist
openlist.push_front(start);
//let's create a node called current
node current_node;
//let's check there is nothing on the open list
while( !openlist.empty() ){
//let's print the open list
print_list(openlist, "Open");
//store the front of the open list on the current_node
current_node = openlist.front();
//delete it from the open list queue
openlist.pop_front();
//add current_node to the closed list
closedlist.push_front(current_node);
//check if the node just popped actually the goal
//if it is we just print it here but something else needs to be done.
if ( isGoal(current_node,goal) ){
cout << "We have reached the goal" << endl;
cout << current_node << endl;
break;
} else {
//else let's expand the currnent node
cout << "Not yet, let's expand the current_node node." << endl;
cout << current_node << endl;
expand(current_node);
openlist.sort();
}
}
}
/**
* Test whether the "now" state is valid
*/
int validState(int dx, int dy, State &now, const vector<string> &ground)
{
// boundary check
if (now.person.x >= ground.size() || now.person.x < 0)
return 0;
if (now.person.y >= ground[now.person.x].size() || now.person.y < 0)
return 0;
// is wall?
if (isWall(ground[now.person.x][now.person.y]))
return 0;
// check boxes overlap with person
int personOverlapBox = 1;
for (int i = 0; i < now.box.size(); ++i) {
if (now.box[i] == now.person) {
personOverlapBox = 2;
now.box[i].x += dx;
now.box[i].y += dy;
if (!isValidBox(now.box[i], now, ground))
return 0;
}
}
// terminal state?
for (int i = 0; i < now.box.size(); ++i) {
if (!isGoal(ground[now.box[i].x][now.box[i].y])) {
return personOverlapBox;
}
}
return -1;
}
void calc4ee_(struct exp ea, enum opr opr, struct exp eb)
{
float r,a,b;
a = ea.r;
b = eb.r;
switch (opr)
{
case op_add:
r = a + b;
break;
case op_sub:
r = a - b;
break;
case op_mul:
r = a * b;
break;
case op_div:
if (b != 0.0)
r = (float)a / (float)b;
else
r = 0;
break;
default:
r = 0;
break;
}
if (isGoal(r, 24))
{
printf("(%s)%c(%s)\t",ea.str,oprStr[opr],eb.str);
}
}
void print()
{
std::cout << toString();
std::cout << "Dimension "<< dimension()<< '\n';
std::cout << "is Goal "<< isGoal() << '\n';
std::cout << "hamming "<< hamming() << '\n';
std::cout << "manhattan " << manhattan() << '\n';
}
//更新処理
void update(const float elapsed_time)
{
if (!isGoal())
{
const float now_time = m_time * m_t + elapsed_time;//時間を進める
m_t = std::min(now_time / m_time, 1.f);//進行度を算出
m_now = m_begin * (1.f - m_t) + m_end * m_t;//現在の値を更新（線形補間）
}
}
/**
* get the goal positions
*/
void getGoalPosition(const vector<string> &ground, vector<Position> &goal)
{
for (int i = 0; i < ground.size(); ++i) {
for (int j = 0; j < ground[i].size(); ++j) {
if (isGoal(ground[i][j])) {
goal.push_back(Position(i, j));
}
}
}
}
main() {
// 0 if no mine, 1 if mine
int gameOver = 0, winGame = 0;
int i, j;
char x[1],y[1];
int row,col;
for(i = 0; i < SIZE ; i ++){
for(j = 0; j < SIZE ; j ++){
boardState[i][j] = 0;
boardActivated[i][j] = 0;
}
}
initializeBoard();
/**********************************/
while(1){
printf("Choose index of the tile [row]<space>[col]:");
fgets(x,2,stdin);
row = x[0] - '0';
getchar();
fgets(y,2,stdin);
col = y[0] - '0';
getchar();
boardActivated[row][col] = 1;
printBoard();
if(boardState[row][col] == 100){
gameOver = 1;
break;
}
// else checkNeighbors(x,y);
if (isGoal(boardState, boardActivated)== 100){
winGame = 1;
break;
}
}
if(winGame){
printf("You win the game!\n");
}
else if(gameOver){
printf("BOOOM! \n");
}
}
int searchPQS (PriQ q, State s, int size) {
//returns index in PQ, uses linear search.
int success = FALSE;
int i = 0;
State temp = q->heap[1]->s;
while (i < q->arraySize && temp != NULL &&
!isGoal(temp,s,size)) {
i++;
if (q->heap[i] != NULL) {
temp = q->heap[i]->s;
if (isGoal(temp,s,size)) {
success = TRUE;
}
}
}
if (success == FALSE) {
i = NOTHING;
}
return i;
}
/**
* judge if the current state is freeze deadlock
*/
int isFreezeDeadlock(State &now, const vector<string> &ground)
{
vector<int>hfreeze(now.box.size(), -2);
vector<int>vfreeze(now.box.size(), -2);
for (int i = 0; i < now.box.size(); ++i) {
horizontalFreeze(i, hfreeze, vfreeze, now, ground);
verticalFreeze(i, hfreeze, vfreeze, now, ground);
}
for (int i = 0; i < hfreeze.size(); ++i) {
if (hfreeze[i] == 0 && vfreeze[i] == 0 && !isGoal(ground[now.box[i].x][now.box[i].y])) {
return 1;
}
}
return 0;
}
void calc4enn_(struct exp ea, enum opr opr, int b)
{
float r,a;
a = ea.r;
switch (opr)
{
case op_add:
r = a + b;
break;
case op_sub:
r = a - b;
break;
case op_mul:
r = a * b;
break;
case op_div:
r = (float)a / (float)b;
break;
case op_rsub:
r = b - a;
break;
case op_rdiv:
if (a != 0.0)
r = b / a;
break;
default:
break;
}
if (isGoal(r, 24))
{
if (opr == op_rdiv || opr == op_rsub)
printf("%d%c(%s)\t", b, oprStr[opr], ea.str);
else
printf("(%s)%c%d\t", ea.str, oprStr[opr], b);
}
}
void DataWrapper::publish(const VisionFieldObject* visual_object)
{
if( isGoal(visual_object->getID()) )
{
digitalWrite(18, HIGH);
}
else if( visual_object->getID() == BALL )
{
digitalWrite(17, HIGH);
}
else if( visual_object->getID() == FIELDLINE )
{
digitalWrite(23, HIGH);
}
else if( visual_object->getID() == OBSTACLE )
{
digitalWrite(22, HIGH);
}
#if VISION_WRAPPER_VERBOSITY > 0
visual_object->printLabel(debug);
debug << std::endl;
#endif
}
/*
* Function: aStart
*
* @Parameters: Grid_Container grid ~ grid_container to find neighboors
* Point start ~ starting point
* Point goal ~ goal point
*
* Uses an pseudocode I found of wikipedia as guidance on my implementation.
*
* @returns an array of points which contains the path to the goal from the start point
*
*
*/
ArrayList* aStar( Grid_Container *grid, Point start, Point goal)
{
int i = 0;
ArrayList *closed_set = NULL;
PQ *open_set = NULL;
ArrayList *map = NULL;
start.g_score = 0;
start.f_score = start.g_score + heuristic_estimate( start, goal);
open_set = makePQ();
closed_set = makeArrayList();
map = makeArrayList();
insert( open_set, start);
while ( !isEmpty( open_set))
{
Point current = pop(open_set);
//printf("Current x: %d y: %d\n", current.x, current.y);
if ( isGoal( current))
{
ArrayList* rtrnList = NULL;
free_PQ( open_set);
free_ArrayList(closed_set);
rtrnList = reconstruct_Path(map, current);
free_ArrayList( map);
return rtrnList;
}
add(closed_set, current);
ArrayList *neighbors = NULL;
neighbors = getNeighborPoints(current, grid);
//printf("Printing neighbors:\n");
//printArrayList(neighbors);
for ( i = 0; i < getSize( neighbors); i++)
{
Point neighbor = get( neighbors, i);
if ( !contains( closed_set, neighbor))
{
int tentative_g_score = calculate_g_score(current, neighbor);
if (!containsPoint( open_set, neighbor) || tentative_g_score < neighbor.g_score)
{
neighbor.parent_x = current.x;
neighbor.parent_y = current.y;
add( map, current);
neighbor.g_score = tentative_g_score;
if (!isGoal(neighbor))
{
neighbor.f_score = neighbor.g_score + heuristic_estimate( current, neighbor);
}
else
{
neighbor.f_score = 0;
}
if (!containsPoint( open_set, neighbor))
{
insert( open_set, neighbor);
}
}
}
}
free_ArrayList(neighbors);
}
return NULL;
}
bool WorldObject::isGoalPost(){
return (isGoal() && !isGoalCenter());
}
//現在の状態を表示
void debugPrint(const int flame)
{
//printf("%2d: now=%.3f, begin=%.3f, end=%.3f, diff=%.3f, time=%.3f, gradient=%.3f, t=%.3f, remain_t=%.3f, progress_time=%.3f, remain_time=%.3f, isGoal()=%s\n", flame, now(), begin(), end(), difference(), time(), gradient(), t(), remain_t(), progress_time(), remain_time(), isGoal() ? "true" : "false");
printf("%2d: now=%.3f, begin=%.3f, end=%.3f, time=%.3f, t=%.3f, isGoal()=%s\n", flame, now(), begin(), end(), time(), t(), isGoal() ? "true" : "false");
}
/**
* @function getShortestPath
*/
bool BFS3D::getShortestPath(std::vector<short unsigned int> start, std::vector<std::vector<int> > &path)
{
int val = 0, cntr = 0, min_val = INFINITE_COST;
std::vector<int> state(3,0);
std::vector<int> next_state(3,0);
int newx,newy,newz;
//make sure the while loop eventually stops
int max_path_length = dimX_*dimY_;
int dx[DIRECTIONS3D] = { 1, 1, 1, 0, 0, 0, -1, -1, -1, 1, 1, 1, 0, 0, -1, -1, -1, 1, 1, 1, 0, 0, 0, -1, -1, -1};
int dy[DIRECTIONS3D] = { 1, 0, -1, 1, 0, -1, -1, 0, 1, 1, 0, -1, 1, -1, -1, 0, 1, 1, 0, -1, 1, 0, -1, -1, 0, 1};
int dz[DIRECTIONS3D] = {-1, -1, -1, -1, -1, -1, -1, -1, -1, 0, 0, 0, 0, 0, 0, 0, 0, 1, 1, 1, 1, 1, 1, 1, 1, 1};
path.resize(0);
next_state[0] = start[0];
next_state[1] = start[1];
next_state[2] = start[2];
while( !isGoal(next_state) || cntr > max_path_length )
{
state = next_state;
min_val = INFINITE_COST;
//iterate through neighbors
for( int d = 0; d < DIRECTIONS3D; d++ )
{
newx = state[0] + dx[d];
newy = state[1] + dy[d];
newz = state[2] + dz[d];
//make sure it is inside the map and has no obstacle
if( 0 > newx || newx >= dimX_ || 0 > newy || newy >= dimY_ || 0 > newz || newz >= dimZ_ )
continue;
val = dist_[xyzToIndex(newx,newy,newz)];
if( val >= INFINITE_COST )
continue;
if ( state[0] != newx && state[1] != newy && state[2] != newz )
val += cost_sqrt3_move_;
else if ( (state[1] != newy && state[2] != newz) ||
(state[0] != newx && state[2] != newz) ||
(state[0] != newx && state[1] != newy) )
val += cost_sqrt2_move_;
else
val += cost_1_move_;
if( val < min_val )
{
min_val = val;
next_state[0] = newx;
next_state[1] = newy;
next_state[2] = newz;
}
}
path.push_back(next_state);
cntr++;
}
//unable to find path
if(cntr > max_path_length)
{
printf("--(!) [BFS3D] Unable to find path to goal. Exiting.\n");
path.clear();
return false;
}
return true;
}
void findWaypoints() {
int position[3]; // Current position
int i;
double path_length = 0; // Total length of the path
int path_steps = 0; // Total length of the path in steps
logStart("findWaypoints");
position[0] = getPosition()[0];
position[1] = getPosition()[1];
position[2] = getPosition()[2];
for (i = 0; i < 1024; i++) {
int a, b, c;
double best_value = world[position[0]][position[1]][position[2]]; // So far best weigt == current weight
int next[3] = { -1, -1, -1 }; // Next position
for (a = -1; a <= 1; a++) {
if (position[0] + a >= 0 && position[0] + a < WORLD_SIZE_X) {
for (b = -1; b <= 1; b++) {
if (position[1] + b >= 0 && position[1] + b < WORLD_SIZE_Y) {
for (c = -1; c <= 1; c++) {
if (position[2] + c >= 0 && position[2] + c < WORLD_SIZE_Z) {
if (!isObstacle(position[0] + a, position[1] + b, position[2] + c)) {
double new_value = world[position[0] + a][position[1] + b][position[2] + c];
/* if (LL_DEBUG || 1)
printf(
"Surrounding point [%i, %i, %i] has value %2.12f (%e) - best know has value %2.12f (%e)\n",
position[0] + a, position[1]
+ b, position[2] + c,
new_value, best_value); */
if (new_value > best_value) {
best_value = new_value;
next[0] = position[0] + a;
next[1] = position[1] + b;
next[2] = position[2] + c;
}
}
}
}
}
}
}
}
printf("Next waypoint: [%i, %i, %i] with value %2.12f (%e)\n", next[0], next[1], next[2], best_value,
best_value);
// Calculate path length
int del_0 = position[0] - next[0];
int del_1 = position[1] - next[1];
int del_2 = position[2] - next[2];
del_0 *= del_0;
del_1 *= del_1;
del_2 *= del_2;
if (del_0 + del_1 + del_2 == 1)
path_length += 1;
if (del_0 + del_1 + del_2 == 2)
path_length += 1.414; // SQRT(2)
if (del_0 + del_1 + del_2 == 3)
path_length += 1.732; // SQRT(3)
path_steps++;
if (isGoal(next[0], next[1], next[2]))
break;
else {
// Set found waypoint as next position
position[0] = next[0];
position[1] = next[1];
position[2] = next[2];
}
}
printf("Total path length: %f\n", path_length);
printf("Total path length in steps: %i\n", path_steps);
logEnd("findWaypoints");
}
void demo_7_main_core_0() {
int threads = TOTAL_PROC_NUM;
// Initialize obstacle map from heightmap
if (1) {
logStart("initialize obstacle map");
// Recode heightmap to global_obstacles
int a, b, c;
for (a = 0; a < WORLD_SIZE_X; a++) {
for (b = 0; b < WORLD_SIZE_Y; b++) {
int height = getHeightmapValue(a, b);
// some regions cannot be flown over
if (height > 174)
height = 255;
for (c = 0; c < height; c++) {
global_obstacles[a][b][c] = 1;
}
for (c = height; c < WORLD_SIZE_Z; c++) {
global_obstacles[a][b][c] = 0;
}
}
}
logEnd("initialize obstacle map");
}
// Set goal
setGoal(WORLD_SIZE_X - 3, WORLD_SIZE_Y - 3, getHeightmapValue(WORLD_SIZE_X - 3, WORLD_SIZE_Y - 3) + 2);
// Set current position
setPosition(3, 3, getHeightmapValue(3, 3) + 2);
// Init weightmap from obstacle map
if (1) {
int a, b, c;
logStart("initialize weightmap from obstacle map");
for (a = 0; a < WORLD_SIZE_X; a++) {
for (b = 0; b < WORLD_SIZE_Y; b++) {
for (c = 0; c < WORLD_SIZE_Z; c++) {
if (isGoal(a, b, c)) {
world[a][b][c] = WEIGHT_SINK;
} else if (isObstacle(a, b, c)) {
world[a][b][c] = WEIGHT_OBSTACLE;
} else {
world[a][b][c] = WEIGHT_INIT;
}
}
}
}
logEnd("initialize weightmap from obstacle map");
}
// Propagate weights with selected algorithm
promote_world(ITERATIONS, threads);
// Find waypoints to goal
findWaypoints();
printf("Iterations: %i\n", ITERATIONS);
return;
}
bool AstarSearch::search()
{
// -- Start Astar search ----------
// If the openlist is empty, search failed
while (!openlist_.empty()) {
// Terminate the search if the count reaches a certain value
static int search_count = 0;
search_count++;
if (search_count > 300000) {
ROS_WARN("Exceed time limit");
search_count = 0;
return false;
}
// Pop minimum cost node from openlist
SimpleNode sn;
sn = openlist_.top();
openlist_.pop();
nodes_[sn.index_y][sn.index_x][sn.index_theta].status = STATUS::CLOSED;
// Expand nodes from this node
AstarNode *current_node = &nodes_[sn.index_y][sn.index_x][sn.index_theta];
// for each update
for (const auto &state : state_update_table_[sn.index_theta]) {
// Next state
double next_x = current_node->x + state.shift_x;
double next_y = current_node->y + state.shift_y;
double next_theta = astar::modifyTheta(current_node->theta + state.rotation);
double move_cost = state.step;
#if DEBUG
// Display search process
geometry_msgs::Pose p;
p.position.x = next_x;
p.position.y = next_y;
tf::quaternionTFToMsg(tf::createQuaternionFromYaw(next_theta), p.orientation);
p = astar::transformPose(p, map2ogm_);
debug_pose_array_.poses.push_back(p);
#endif
// Increase curve cost
if (state.curve)
move_cost *= curve_weight_;
// Increase reverse cost
if (current_node->back != state.back)
move_cost *= reverse_weight_;
// Calculate index of the next state
SimpleNode next;
next.index_x = next_x / map_info_.resolution;
next.index_y = next_y / map_info_.resolution;
next.index_theta = sn.index_theta + state.index_theta;
// Avoid invalid index
next.index_theta = (next.index_theta + angle_size_) % angle_size_;
// Check if the index is valid
if (isOutOfRange(next.index_x, next.index_y) || detectCollision(next))
continue;
AstarNode *next_node = &nodes_[next.index_y][next.index_x][next.index_theta];
double next_hc = nodes_[next.index_y][next.index_x][0].hc;
// Calculate euclid distance heuristic cost
if (!use_wavefront_heuristic_)
next_hc = astar::calcDistance(next_x, next_y, goal_pose_local_.pose.position.x, goal_pose_local_.pose.position.y);
// GOAL CHECK
if (isGoal(next_x, next_y, next_theta)) {
search_count = 0;
next_node->status = STATUS::OPEN;
next_node->x = next_x;
next_node->y = next_y;
next_node->theta = next_theta;
next_node->gc = current_node->gc + move_cost;
next_node->hc = next_hc;
next_node->back = state.back;
next_node->parent = current_node;
setPath(next);
return true;
}
// NONE
if (next_node->status == STATUS::NONE) {
next_node->status = STATUS::OPEN;
next_node->x = next_x;
next_node->y = next_y;
next_node->theta = next_theta;
next_node->gc = current_node->gc + move_cost;
next_node->hc = next_hc;
next_node->back = state.back;
next_node->parent = current_node;
next.cost = next_node->gc + next_node->hc;
openlist_.push(next);
continue;
}
// OPEN or CLOSED
if (next_node->status == STATUS::OPEN || next_node->status == STATUS::CLOSED) {
if (current_node->gc + move_cost + next_hc < next_node->gc + next_hc) {
next_node->status = STATUS::OPEN;
next_node->x = next_x;
next_node->y = next_y;
next_node->theta = next_theta;
next_node->gc = current_node->gc + move_cost;
next_node->hc = next_hc;
next_node->back = state.back;
next_node->parent = current_node;
next.cost = next_node->gc + next_node->hc;
openlist_.push(next);
continue;
}
}
if (search_count == 0)
break;
} // state update
}
// Failed to find path
ROS_WARN("Openlist is Empty!");
return false;
}
Node* AIManager::astar(Vector3f end, Vector3f begin)
{
bool firstGo = true;
Node* start = new Node(begin);
Node* goal = new Node(end);
Node* current;
std::vector<Node*> openlist;
std::vector<Node*> closelist;
Vector3f goalvf = end;
openlist.push_back(start);
std::make_heap(openlist.begin(), openlist.end());
while(!openlist.empty())
{
current = openlist.front();
std::pop_heap(openlist.begin(), openlist.end());
openlist.pop_back();
if(isGoal(current, goal))
{
return current;
}
// getSuccessors(current, openlist, closelist, end);
//If adjacent position is not obstacle and not on closed
//add adjacent positions to list
Node* temp = new Node;
bool valid = true;
//if(!Overlay::isObstacle(current->_x+STEP, current->_y, current->_z))
if(!checkObstacle(Vector3f(current->_x+STEP, current->_y, current->_z)))
{
temp->_x = current->_x+STEP;
temp->_z = current->_z;
for(unsigned int i = 0; i < closelist.size() && valid; ++i)
{
if(temp->isSamePosition(closelist[i]))
valid = false;
}
if(valid)
{
temp->_g = current->_g+GSCORE;
temp->_h = calculateHn(Vector3f(float(temp->_x), float(temp->_y), float(temp->_z)), goalvf);
temp->calculateFn();
temp->setParent(current);
openlist.push_back(temp);
std::push_heap(openlist.begin(), openlist.end());
}
}
//if(!Overlay::isObstacle(current->_x-STEP, current->_y, current->_z))
if(!checkObstacle(Vector3f(current->_x-STEP, current->_y, current->_z)))
{
temp->_x = current->_x-STEP;
temp->_z = current->_z;
for(unsigned int i = 0; i < closelist.size() && valid; ++i)
{
if(temp->isSamePosition(closelist[i]))
valid = false;
}
if(valid)
{
temp->_g = current->_g+GSCORE;
temp->_h = calculateHn(Vector3f(float(temp->_x), float(temp->_y), float(temp->_z)), goalvf);
temp->calculateFn();
temp->setParent(current);
openlist.push_back(temp);
std::push_heap(openlist.begin(), openlist.end());
}
valid = true;
}
//if(!Overlay::isObstacle(current->_x, current->_y, current->_z+STEP))
if(!checkObstacle(Vector3f(current->_x, current->_y, current->_z+STEP)))
{
temp->_x = current->_x;
temp->_z = current->_z+STEP;
for(unsigned int i = 0; i < closelist.size() && valid; ++i)
{
if(temp->isSamePosition(closelist[i]))
valid = false;
}
if(valid)
{
temp->_g = current->_g+GSCORE;
temp->_h = calculateHn(Vector3f(float(temp->_x), float(temp->_y), float(temp->_z)), goalvf);
temp->calculateFn();
temp->setParent(current);
openlist.push_back(temp);
std::push_heap(openlist.begin(), openlist.end());
}
valid = true;
}
//if(!Overlay::isObstacle(current->_x, current->_y, current->_z-STEP))
if(!checkObstacle(Vector3f(current->_x, current->_y, current->_z-STEP)))
{
temp->_x = current->_x;
temp->_z = current->_z-STEP;
for(unsigned int i = 0; i < closelist.size() && valid; ++i)
{
if(temp->isSamePosition(closelist[i]))
valid = false;
}
if(valid)
{
temp->_g = current->_g+GSCORE;
temp->_h = calculateHn(Vector3f(float(temp->_x), float(temp->_y), float(temp->_z)), goalvf);
temp->calculateFn();
temp->setParent(current);
openlist.push_back(temp);
std::push_heap(openlist.begin(), openlist.end());
}
valid = true;
}
if(firstGo && !openlist.empty())
{
std::make_heap(openlist.begin(), openlist.end());
firstGo = false;
}
closelist.push_back(current);
return current;
}
return NULL;
}
