// lの中にpの座標が存在してたら1を返す
int isExistPosition(PositionList* l, Position p) {
PositionNode* ptr = l->begin;
for(; ptr != NULL; ptr = ptr->next) {
if(isPositionEqual(ptr->pos, p)) return 1;
}
return 0;
}
// 動かすのに成功したら1、失敗したら0を返す
int movePlayer(Sugoroku *s, int player_id, enum Direction d, PositionList *plist, SugorokuStatus *ss) {
if(player_id >= s->player_num || player_id < 0) {
puts("player番号が頭おかしい");
return 0;
}
Position pos = s->player[player_id].pos;
Position mpos = pos; // 目的の場所
switch(d) {
case UP:
mpos.y--;
if(mpos.y < 0 || !canMove(&s->map, mpos)) return 0;
break;
case RIGHT:
mpos.x++;
if(mpos.x >= s->map.width || !canMove(&s->map, mpos)) return 0;
break;
case DOWN:
mpos.y++;
if(mpos.y >= s->map.height || !canMove(&s->map, mpos)) return 0;
break;
case LEFT:
mpos.x--;
if(mpos.x < 0 || !canMove(&s->map, mpos)) return 0;
break;
}
// 既に行ったことがある場所だったら行かない
// 一個前のとこに戻ることは出来る
if(isExistPosition(plist, mpos)) {
if(isPositionEqual(plist->end->pos, mpos)) {
// 進める数を一つ戻す
ss->move_num++;
s->player[player_id].pos = mpos;
popPosition(plist);
popPosition(&s->player[player_id].footmark);
} else {
return 0;
}
} else {
ss->move_num--;
s->player[player_id].pos = mpos;
pushPosition(plist, pos);
pushPosition(&s->player[player_id].footmark, pos);
}
return 1;
}
Node *Astar::makeChildrenNodes(Node *parent)
{
geometry_msgs::Pose P, sensorP;
Node *p, *q;
SearchSpaceNode *temp;
double start_angle,end_angle,angle,angle_difference,discrete_angle,robot_angle,child_angle,angle_resolution = DTOR(10);
bool collides = FALSE;
int direction;
P.position.x = parent->pose.p.position.x;
P.position.y = parent->pose.p.position.y;
P.position.z = parent->pose.p.position.z;
P.orientation.x = parent->pose.p.orientation.x;
P.orientation.y = parent->pose.p.orientation.y;
P.orientation.z = parent->pose.p.orientation.z;
P.orientation.w = parent->pose.p.orientation.w;
//not sure if it is necessary (Not necessary)
sensorP.position.x = parent->senPose.p.position.x;
sensorP.position.y = parent->senPose.p.position.y;
sensorP.position.z = parent->senPose.p.position.z;
sensorP.orientation.x = parent->senPose.p.orientation.x;
sensorP.orientation.y = parent->senPose.p.orientation.y;
sensorP.orientation.z = parent->senPose.p.orientation.z;
sensorP.orientation.w = parent->senPose.p.orientation.w;
// std::cout<<"parent #"<<"position x:"<<parent->pose.p.position.x<<" y:"<<parent->pose.p.position.y<<" z:"<<parent->pose.p.position.z<<"\n";
// std::cout<<"parent #"<<"orientation x:"<<parent->pose.p.orientation.x<<" y:"<<parent->pose.p.orientation.y<<" z:"<<parent->pose.p.orientation.z<<" w:"<<parent->pose.p.orientation.w<<"\n";
Tree t;
t.location = P;
if(!search_space)
return NULL;
temp = search_space;
// Locate the Cell in the Search Space, necessary to determine the neighbours
while(temp!=NULL)
{
//added the orientation since we have different
if (isPositionEqual(temp->location.position,P.position) && isOrientationEqual(temp->location.orientation,P.orientation))
break;
temp = temp->next;
}
if (!temp)
{
// LOG(Logger::Info," --->>> Node not found in the search Space ")
return NULL;
}
// qDebug("Node has %d children x=%f y=%f",temp->children.size(),temp->location.x(),temp->location.y());
q = NULL;
if (debug == true)
std::cout<<"\n\n\n#############children Size: "<< temp->children.size() <<" ##################\n\n\n";
// Check Each neighbour
for (int i=0;i<temp->children.size();i++)
{
/*
* Check what what as the Robot's direction of motion and see
* if we it's easier to go forward or backwards to the child
*/
// if (parent->direction == FORWARD)
// robot_angle = parent->pose.phi;
// else
// robot_angle = parent->pose.phi + M_PI;
// What will be our orientation when we go to this child node ?
//TODO:: Transfer this into a 3D Vector angle diff
//angle = ATAN2(temp->children[i]->location,P);
// angle = 0;
// How much it differs from our current orientations ?
// angle_difference = anglediff(angle,parent->pose.phi);
// Are we gonna turn too much ? if yes then why not go backwards ?
/*
if (angle_difference > DTOR(120))
{
//cout<<"\n Angle difference ="<<RTOD(angle_difference)<<" parent angle="<<RTOD(parent->angle)<<" destination angle="<<RTOD(angle);
direction = parent->direction * -1;
}
else
*/
// {
// direction = parent->direction;
// }
// collides = FALSE;
/* Discreatize the turning space and check for collison
* 1- Angle stored in the node is the direction of the PATH (NOT THE ROBOT)
* 2- If we were moving Forward then the Robot direction is the same as the Path
* 3- If we were moving BackWard then the Robot direction is Path + M_PI
* 4- Determine what will the Robot orientation will be at this step
* 5- Check for collision detection with a certain resolution
*/
// if (direction == FORWARD)
// child_angle = angle;
// else
// child_angle = angle + M_PI;
// if(robot_angle < 0 ) robot_angle += 2*M_PI;
// if(child_angle < 0 ) child_angle += 2*M_PI;
// Start from the largest angle and go down
// if (robot_angle > child_angle)
// {
// start_angle = robot_angle;
// end_angle = child_angle;
// }
// else
// {
// start_angle = child_angle;
// end_angle = robot_angle;
// }
// discrete_angle = start_angle;
//cout<<"\n Start is"<<RTOD(start_angle)<<" End angle="<<RTOD(end_angle);
// angle_difference = anglediff(start_angle,end_angle);
// for (int s=0 ; s <= ceil(angle_difference/angle_resolution); s++)
// {
// if (inObstacle(temp->children[i]->location,discrete_angle))
// {
// collides= true;
// break;
// }
// if(Abs(start_angle - end_angle) >= DTOR(180))
// {
// discrete_angle += angle_resolution;
// if (discrete_angle > 2*M_PI)
// discrete_angle-= 2*M_PI;
// if(discrete_angle > end_angle)
// discrete_angle = end_angle;
// }
// else
// {
// discrete_angle -= angle_resolution;
// if (discrete_angle < end_angle)
// discrete_angle = end_angle;
// }
// }
//if (!collides) // if after discretization the child still doens't collide then add it
//{
p = new Node;
p->pose.p.position.x = temp->children[i]->location.position.x;
p->pose.p.position.y = temp->children[i]->location.position.y;
p->pose.p.position.z = temp->children[i]->location.position.z;
p->pose.p.orientation.x = temp->children[i]->location.orientation.x;
p->pose.p.orientation.y = temp->children[i]->location.orientation.y;
p->pose.p.orientation.z = temp->children[i]->location.orientation.z;
p->pose.p.orientation.w = temp->children[i]->location.orientation.w;
// std::cout<<"child #"<<i<<"position x:"<<p->pose.p.position.x<<" y:"<<p->pose.p.position.y<<" z:"<<p->pose.p.position.z<<"\n";
// std::cout<<"child #"<<i<<"orientation x:"<<p->pose.p.orientation.x<<" y:"<<p->pose.p.orientation.y<<" z:"<<p->pose.p.orientation.z<<" w:"<<p->pose.p.orientation.w<<"\n";
p->senPose.p.position.x = temp->children[i]->sensorLocation.position.x;
p->senPose.p.position.y = temp->children[i]->sensorLocation.position.y;
p->senPose.p.position.z = temp->children[i]->sensorLocation.position.z;
p->senPose.p.orientation.x = temp->children[i]->sensorLocation.orientation.x;
p->senPose.p.orientation.y = temp->children[i]->sensorLocation.orientation.y;
p->senPose.p.orientation.z = temp->children[i]->sensorLocation.orientation.z;
p->senPose.p.orientation.w = temp->children[i]->sensorLocation.orientation.w;
p->id = temp->children[i]->id;
// p->direction = direction ;
t.children.push_back(p->pose.p);
// p->nearest_obstacle = temp->children[i]->obstacle_cost;
p->parent = parent;
// p->pose.phi = angle;
p->next = q;
q = p;
// }
}
// Save the search tree so that it can be displayed later
if (t.children.size() > 0)
tree.push_back(t);
std::cout<<" Making children nodes: "<<t.children.size()<<"\n";
return q;
}
