GridGraphSolver::SolveStateT MyBFSGridGraphSolver::step() {
if (m_state == STEPPING) {
//pop all elements in current list
//if they are the finish node, add them, youre done
if (m_openList.size() == 0) {
m_state = UNSOLVED;
return m_state;
}
std::list<PathNodeT *> children;
while (m_openList.size()) {
PathNodeT* next = m_openList.back();
incrementStepCount();
m_closedList.push_back(next); //add to closed list
m_openList.pop_back(); //pop the just considered element
if (next->m_node == m_finish) {
m_state = SOLVED;
computePath(next);
return m_state;
}
for (int dir = 0; dir < GridNode::NUM_DIRS; dir++) {
if (next->m_node->containsEdge(dir)
&& !next->m_node->getNeighbor(dir)->isVisited()) {
next->m_node->getNeighbor(dir)->setVisited(true);
children.push_back(
new PathNodeT(next,
next->m_node->getNeighbor(dir), next->m_generation+1));
}
}
}
m_openList = children; //new children list
}
return m_state;
}
/**
This is the base class for all AIs. At this stage there are two similar
AIs: one is the SkiddingAI, which is the AI used in lap based races
(including follow-the-leader mode), the other one is the end controller,
I.e. the controller that takes over from a player (or AI) when the race is
finished.
This base class defines some basic operations:
- It takes care on which part of the QuadGraph the AI currently is.
- It determines which path the AI should take (in case of shortcuts
or forks in the road).
At race start and every time a new lap is started, the AI will compute the
path the kart is taking this lap (computePath). At this stage the decision
which road in case of shortcut to take is purely random. It stores the
information in two arrays:
m_successor_index[i] stores which successor to take from node i.
The successor is a number between 0 and number_of_successors - 1.
m_next_node_index[i] stores the actual index of the graph node that
follows after node i.
Depending on operation one of the other data is more useful, so this
class stores both information to avoid looking it up over and over.
Once this is done (still in computePath), the array m_all_look_aheads is
computed. This array stores for each quad a list of the next (atm) 10 quads.
This is used when the AI is selecting where to drive next, and it will just
pass the list of next quads to findRoadSector.
Note that the quad graph information is stored for every quad in the quad
graph, even if the quad is not on the path chosen. This is necessary since
it can happen that a kart ends up on a path not choses (e.g. perhaps it was
pushed on that part, or couldn't get a sharp corner).
In update(), which gets called one per frame per AI, this object will
determine the quad the kart is currently on (which is then used to determine
where the kart will be driving to). This uses the m_all_look_aheads to
speed up this process (since the kart is likely to be either on the same
quad as it was before, or the next quad in the m_all_look_aheads list).
It will also check if the kart is stuck:
this is done by maintaining a list of times when the kart hits the track. If
(atm) more than 3 collisions happen in 1.5 seconds, the kart is considered
stuck and will trigger a rescue (due to the pushback from the track it will
take some time if a kart is really stuck before it will hit the track again).
This base class also contains some convenience functions which are useful
in all AIs, e.g.:
- steerToPoint: determine the steering angle to use depending on the
current location and the point the kart is driving to.
- normalizeAngle: To normalise the steering angle to be in [-PI,PI].
- setSteering: Converts the steering angle into a steering fraction
in [-1,1].
*/
AIBaseLapController::AIBaseLapController(AbstractKart *kart,
StateManager::ActivePlayer *player)
: AIBaseController(kart, player)
{
if (race_manager->getMinorMode()!=RaceManager::MINOR_MODE_3_STRIKES &&
race_manager->getMinorMode()!=RaceManager::MINOR_MODE_SOCCER)
{
m_world = dynamic_cast<LinearWorld*>(World::getWorld());
m_track = m_world->getTrack();
computePath();
}
else
{
// Those variables are not defined in a battle mode (m_world is
// a linear world, since it assumes the existance of drivelines)
m_world = NULL;
m_track = NULL;
m_next_node_index.clear();
m_all_look_aheads.clear();
m_successor_index.clear();
} // if battle mode
// Don't call our own setControllerName, since this will add a
// billboard showing 'AIBaseLapController' to the kar.
Controller::setControllerName("AIBaseLapController");
} // AIBaseLapController
/** Triggers a recomputation of the path to use, so that the AI does not
* always use the same way.
*/
void AIBaseLapController::newLap(int lap)
{
if(lap>0)
{
computePath();
}
} // newLap
void GameEntityPath::beginPathing()
{
if(pathtoplayer == "")
{
pathtoplayer = computePath();
//logHelperMessage(INFO,2,"Path:'",pathtoplayer.c_str());
}
}
void Lord::showPath( bool state )
{
if( state ) {
computePath( _dest );
} else {
if(_showPath){
_showPath->clear();
}
}
}
bool PathConnectPlugin::run(ScribusDoc* doc, QString)
{
currDoc = doc;
firstUpdate = true;
if (currDoc == 0)
currDoc = ScCore->primaryMainWindow()->doc;
if (currDoc->m_Selection->count() > 1)
{
Item1 = currDoc->m_Selection->itemAt(0);
Item2 = currDoc->m_Selection->itemAt(1);
originalPath1 = Item1->PoLine.copy();
originalPath2 = Item2->PoLine.copy();
originalXPos = Item1->xPos();
originalYPos = Item1->yPos();
PathConnectDialog *dia = new PathConnectDialog(currDoc->scMW());
connect(dia, SIGNAL(updateValues(int, int, int, int)), this, SLOT(updateEffect(int, int, int, int)));
if (dia->exec())
{
int pointOne = dia->getFirstLinePoint();
int pointTwo = dia->getSecondLinePoint();
int mode = dia->getMode();
Item1->PoLine = computePath(pointOne, pointTwo, mode, originalPath1, originalPath2);
Item1->Frame = false;
Item1->ClipEdited = true;
Item1->FrameType = 3;
currDoc->AdjustItemSize(Item1);
Item1->OldB2 = Item1->width();
Item1->OldH2 = Item1->height();
Item1->updateClip();
Item1->ContourLine = Item1->PoLine.copy();
currDoc->m_Selection->removeItem(Item1);
currDoc->itemSelection_DeleteItem();
currDoc->changed();
}
else
{
Item1->PoLine = originalPath1.copy();
Item1->Frame = false;
Item1->ClipEdited = true;
Item1->FrameType = 3;
Item1->setXYPos(originalXPos, originalYPos);
currDoc->AdjustItemSize(Item1);
Item1->OldB2 = Item1->width();
Item1->OldH2 = Item1->height();
Item1->updateClip();
Item1->ContourLine = Item1->PoLine.copy();
}
currDoc->view()->DrawNew();
delete dia;
}
QList< QPair<Vec, Vec> >
GiLightObject::getPointAndNormalPath()
{
computePath(m_points);
QList< QPair<Vec, Vec> > pathNormal;
if (m_path.count() == 0)
return pathNormal;
for (int i=0; i<m_path.count(); i++)
pathNormal.append(qMakePair(m_path[i], m_pathX[i]));
return pathNormal;
}
PathComponent::PathPool computePath(const Ndk::EntityHandle& e, micropather::MicroPather* pather)
{
auto& path = e->GetComponent<PathComponent>().path;
auto& pos = e->GetComponent<PositionComponent>();
auto& move = e->GetComponent<MoveComponent>();
TealAssert(pather, "Pather is null, cannot compute path !");
if (move.tile == toVector2(Def::StandStillPos))
return PathComponent::PathPool {}; // This entity doesn't want to move.
if (isEntityMoving(e) && !isPositionValid(pos.xy))
return PathComponent::PathPool {}; // Invalid position, can't stop it
return computePath(pos.xy, move.tile, pather);
}
bool PathConnectPlugin::run(ScribusDoc* doc, QString)
{
m_doc = doc;
firstUpdate = true;
if (m_doc == 0)
m_doc = ScCore->primaryMainWindow()->doc;
if (m_doc->m_Selection->count() > 1)
{
m_item1 = m_doc->m_Selection->itemAt(0);
m_item2 = m_doc->m_Selection->itemAt(1);
originalPath1 = m_item1->PoLine.copy();
originalPath2 = m_item2->PoLine.copy();
originalXPos = m_item1->xPos();
originalYPos = m_item1->yPos();
PathConnectDialog *dia = new PathConnectDialog(m_doc->scMW());
connect(dia, SIGNAL(updateValues(int, int, int, int)), this, SLOT(updateEffect(int, int, int, int)));
if (dia->exec())
{
int pointOne = dia->getFirstLinePoint();
int pointTwo = dia->getSecondLinePoint();
int mode = dia->getMode();
UndoTransaction *trans = NULL;
if(UndoManager::undoEnabled())
trans = new UndoTransaction(UndoManager::instance()->beginTransaction(Um::BezierCurve,Um::ILine,Um::ConnectPath,"",Um::ILine));
m_item1->PoLine = computePath(pointOne, pointTwo, mode, originalPath1, originalPath2);
m_item1->ClipEdited = true;
m_item1->FrameType = 3;
m_doc->AdjustItemSize(m_item1);
m_item1->OldB2 = m_item1->width();
m_item1->OldH2 = m_item1->height();
if(UndoManager::undoEnabled())
{
ScItemState<QPair<FPointArray,FPointArray> > *is = new ScItemState<QPair<FPointArray,FPointArray> >(Um::ConnectPath);
is->set("CONNECT_PATH","connect_path");
is->set("OLDX", originalXPos);
is->set("OLDY", originalYPos);
is->set("NEWX", m_item1->xPos());
is->set("NEWY", m_item1->yPos());
is->setItem(qMakePair(originalPath1, m_item1->PoLine));
UndoManager::instance()->action(m_item1, is);
}
m_item1->updateClip();
m_item1->ContourLine = m_item1->PoLine.copy();
m_doc->m_Selection->removeItem(m_item1);
m_doc->itemSelection_DeleteItem();
m_doc->changed();
if (trans)
{
trans->commit();
delete trans;
trans = NULL;
}
}
else
{
m_item1->PoLine = originalPath1.copy();
m_item1->ClipEdited = true;
m_item1->FrameType = 3;
m_item1->setXYPos(originalXPos, originalYPos);
m_doc->AdjustItemSize(m_item1);
m_item1->OldB2 = m_item1->width();
m_item1->OldH2 = m_item1->height();
m_item1->updateClip();
m_item1->ContourLine = m_item1->PoLine.copy();
}
m_doc->view()->DrawNew();
delete dia;
}
int main(int argc, char **argv)
{
int ch=1;
int ret;
while (ch != 0) {
ch = get_input();
LOG("Your choice = %d\n", ch);
switch (ch) {
case 1:
{
char dataFile[25] = { 0 };
LOG("Please load original routing table data file\n");
scanf("%s", dataFile);
ort = loadOrigRoutingTable(dataFile);
if (!ort){
DEBUG("%s:%d error in constructing original routing table\n",
__FUNCTION__, __LINE__);
return -1;
}
DEBUG("%s:%d ort=%p &lsp=%p\n",
__FUNCTION__, __LINE__, ort, &lsp);
ret = constructLSP(ort, &lsp);
if (ret != 0) {
DEBUG("Link State Packets construction failure\n");
} else {
LOG("Link State Packets construction success\n");
printLSP(lsp, ort->numOfRouters);
}
allocRoutingTables(ort->numOfRouters);
}
break;
case 2:
{
int routerId;
LOG("Please select a router\n");
scanf("%d", &routerId);
routerId--;
buildRoutingTable2(routerId);
LOG("The routing table for router %d is\n", routerId);
printRoutingTable(routerId);
}
break;
case 3:
{
int srcRouterId;
int dstRouterId;
LOG("Please input the source and the destination router\n");
scanf("%d", &srcRouterId);
scanf("%d", &dstRouterId);
computePath(srcRouterId, dstRouterId, &path);
LOG("The shrtest path from %d to %d is %d", srcRouterId, dstRouterId, srcRouterId);
printRoutePath(&path);
LOG("%d\n", dstRouterId);
LOG ("Num of Hops = %d\n",path.numOfHops);
free(path.rt);
break;
}
}
}
}
LayoutRect HTMLAreaElement::computeRect(RenderObject* obj) const
{
return enclosingLayoutRect(computePath(obj).boundingRect());
}
void
GiLightObject::computePathLength()
{
computePath(m_points);
}
bool VoronoiPlanner::computePlan(costmap_2d::Costmap2D* costmap_, DynamicVoronoi * voronoi_,
const geometry_msgs::PoseStamped& start,
const geometry_msgs::PoseStamped& goal, double tolerance,
std::vector<geometry_msgs::PoseStamped>& plan) {
//Sure that plan is clear
plan.clear();
ros::NodeHandle n;
double wx = start.pose.position.x;
double wy = start.pose.position.y;
unsigned int start_x_i, start_y_i, goal_x_i, goal_y_i;
double start_x, start_y, goal_x, goal_y;
if (!costmap_->worldToMap(wx, wy, start_x_i, start_y_i)) {
ROS_WARN(
"The robot's start position is off the global costmap. Planning will always fail, are you sure the robot has been properly localized?");
return false;
}
worldToMap(costmap_, wx, wy, start_x, start_y);
wx = goal.pose.position.x;
wy = goal.pose.position.y;
if (!costmap_->worldToMap(wx, wy, goal_x_i, goal_y_i)) {
ROS_WARN_THROTTLE(1.0,
"The goal sent to the global planner is off the global costmap. Planning will always fail to this goal.");
return false;
}
worldToMap(costmap_, wx, wy, goal_x, goal_y);
clearRobotCell(costmap_, start_x_i, start_y_i);
bool ** map;
computeVoronoi(voronoi_, costmap_, map);
ros::Time t_b = ros::Time::now();
ros::Time t = ros::Time::now();
std::vector<std::pair<float, float> > path1;
std::vector<std::pair<float, float> > path2;
std::vector<std::pair<float, float> > path3;
ROS_INFO("start_x %f, start_y %f", start_x, start_y);
bool res1 = false, res2 = false, res3 = false;
// If goal not are in cell of the vornoi diagram, we have that best findPath of goal to voronoi diagram without have a cell occupancie
if (!voronoi_->isVoronoi(goal_x, goal_y)) {
res3 = computePath(&path3, goal_x, goal_y, start_x, start_y, voronoi_, 0,
1);
std::cout << "computePath goal to VD " << res3 << std::endl;
goal_x = std::get < 0 > (path3[path3.size() - 1]);
goal_y = std::get < 1 > (path3[path3.size() - 1]);
ROS_INFO("Is voronoi goal compute %d", voronoi_->isVoronoi(goal_x, goal_y));
std::reverse(path3.begin(), path3.end());
}
if (!voronoi_->isVoronoi(start_x, start_y)) {
res1 = computePath(&path1, start_x, start_y, goal_x, goal_y, voronoi_, 0,
1);
std::cout << "computePath start to VD " << res1 << std::endl;
start_x = std::get < 0 > (path1[path1.size() - 1]);
start_y = std::get < 1 > (path1[path1.size() - 1]);
ROS_INFO("Is voronoi start compute %d", voronoi_->isVoronoi(start_x, start_y));
}
res2 = computePath(&path2, start_x, start_y, goal_x, goal_y, voronoi_, 1, 0);
ROS_INFO("computePath %d", res2);
if (!(res1 && res2 && res3)) {
ROS_INFO("Failed to compute full path");
}
path1.insert(path1.end(), path2.begin(), path2.end());
path1.insert(path1.end(), path3.begin(), path3.end());
/*for (int i = 0; i < path1.size(); i++) {
int x = std::get < 0 > (path1[i]);
int y = std::get < 1 > (path1[i]);
if (x > 0 && y > 0)
map[x][y] = 1;
}*/
smoothPath(&path1);
for (int i = 0; i < path1.size(); i++) {
geometry_msgs::PoseStamped new_goal = goal;
tf::Quaternion goal_quat = tf::createQuaternionFromYaw(1.54);
new_goal.pose.position.x = std::get < 0 > (path1[i]);
new_goal.pose.position.y = std::get < 1 > (path1[i]);
mapToWorld(costmap_, new_goal.pose.position.x, new_goal.pose.position.y,
new_goal.pose.position.x, new_goal.pose.position.y);
new_goal.pose.orientation.x = goal_quat.x();
new_goal.pose.orientation.y = goal_quat.y();
new_goal.pose.orientation.z = goal_quat.z();
new_goal.pose.orientation.w = goal_quat.w();
plan.push_back(new_goal);
}
ROS_INFO("\nTime to get plan: %f sec\n", (ros::Time::now() - t_b).toSec());
return !plan.empty();
}
