void
CFG< MdesType>::toStream(ostream& os)
{
Numeration num = makeTopologicalNumeration();
std::vector< CFNode< MdesType> *> nodes( this->numNodes());
/* Fill array of node pointers */
for ( CFNode< MdesType> *node = this->firstNode();
isNotNullP( node);
node = node->nextNode() )
{
IR_ASSERTD( node->isNumbered( num));
nodes[ node->number( num)] = node;
}
/* Print reachable nodes */
for ( UInt32 i = startNode()->number( num);
i < this->numNodes();
++i)
{
CFNode< MdesType> *node = nodes[ i];
os << *node << endl;
}
for ( UInt32 i = 0, end = startNode()->number( num);
i < end;
++i)
{
CFNode< MdesType> *node = nodes[ i];
os << "unreachable " << *node << endl;
}
this->freeNum( num);
}
Graph Scene::buildGraph() const
{
Graph g;
for (auto item : items()) {
auto node = dynamic_cast<Node *>(item);
if (node) {
GraphNode graphNode;
graphNode.index = node->index();
graphNode.weight = node->number();
g.addNode(graphNode);
}
}
for (auto item : items()) {
auto edge = dynamic_cast<Edge *>(item);
if (edge) {
auto &startGraphNode = g[edge->startNode()->index()];
auto &endGraphNode = g[edge->endNode()->index()];
if (startGraphNode.index != 0 && endGraphNode.index != 0) {
endGraphNode.addParent(startGraphNode.index);
}
}
}
return g;
}
void CViewSourceHTML::WriteTextInElement(const nsAString& tagName,
eHTMLTags tagType, const nsAString& text,
nsTokenAllocator* allocator,
const nsAString& attrName,
const nsAString& attrValue) {
// Open the element, supplying the attribute, if any.
nsTokenAllocator* theAllocator = mTokenizer->GetTokenAllocator();
if (!theAllocator) {
return;
}
CStartToken* startToken =
static_cast<CStartToken*>
(theAllocator->CreateTokenOfType(eToken_start, tagType, tagName));
if (!startToken) {
return;
}
nsCParserStartNode startNode(startToken, theAllocator);
if (!attrName.IsEmpty()) {
AddAttrToNode(startNode, allocator, attrName, attrValue);
}
mSink->OpenContainer(startNode);
IF_FREE(startToken, theAllocator);
// Add the text node.
CTextToken textToken(text);
nsCParserNode textNode(&textToken, 0/*stack token*/);
mSink->AddLeaf(textNode);
// Close the element.
mSink->CloseContainer(tagType);
}
/**
* Call this to start a new PRE block. See bug 86355 for why this
* makes some pages much faster.
*/
void CViewSourceHTML::StartNewPreBlock(void){
CEndToken endToken(eHTMLTag_pre);
nsCParserNode endNode(&endToken, 0/*stack token*/);
mSink->CloseContainer(eHTMLTag_pre);
nsTokenAllocator* theAllocator = mTokenizer->GetTokenAllocator();
if (!theAllocator) {
return;
}
CStartToken* theToken =
static_cast<CStartToken*>
(theAllocator->CreateTokenOfType(eToken_start,
eHTMLTag_pre,
NS_LITERAL_STRING("PRE")));
if (!theToken) {
return;
}
nsCParserStartNode startNode(theToken, theAllocator);
AddAttrToNode(startNode, theAllocator,
NS_LITERAL_STRING("id"),
NS_ConvertASCIItoUTF16(nsPrintfCString("line%d", mLineNumber)));
mSink->OpenContainer(startNode);
IF_FREE(theToken, theAllocator);
#ifdef DUMP_TO_FILE
if (gDumpFile) {
fprintf(gDumpFile, "</pre>\n");
fprintf(gDumpFile, "<pre id=\"line%d\">\n", mLineNumber);
}
#endif // DUMP_TO_FILE
mTokenCount = 0;
}
bool Connection::areInspectorsValid() const
{
return areNodesValid() &&
gscene()->getInspector(startNode()) &&
(drag_state != CONNECTED ||
gscene()->getInspector(endNode()));
}
std::vector<LineStringEntity*> LineStringEntity::getLineNeighboursDegree2()
{
std::vector<LineStringEntity*> vNeighbours;
int StartDegree=startNode()->getDegree();
int EndDegree=endNode()->getDegree();
computeNeighbours();
if (StartDegree<=2)
{
std::vector<LineStringEntity*> vUpNeighbours;
vUpNeighbours= getLineOrientUpNeighbours();
vNeighbours.insert (vNeighbours.end(),vUpNeighbours.begin(),vUpNeighbours.end());
}
if (EndDegree<=2)
{
std::vector<LineStringEntity*> vDownNeighbours;
vDownNeighbours=getLineOrientDownNeighbours();
vNeighbours.insert (vNeighbours.end(),vDownNeighbours.begin(),vDownNeighbours.end());
}
return vNeighbours;
}
void LineStringEntity::computeNeighbours()
{
delete mp_Neighbours;
mp_Neighbours = new std::set<LandREntity*>;
geos::planargraph::DirectedEdgeStar* UpStar = startNode()->getOutEdges();
geos::planargraph::DirectedEdgeStar* DownStar = endNode()->getOutEdges();
std::vector<geos::planargraph::DirectedEdge*>::iterator it=UpStar->iterator();
std::vector<geos::planargraph::DirectedEdge*>::iterator ite=UpStar->end();
for (; it != ite; ++it)
{
LandREntity* Ent = dynamic_cast<LandREntity*>((*it)->getEdge());
if (Ent != this)
mp_Neighbours->insert(Ent);
}
it = DownStar->iterator();
ite = DownStar->end();
for (; it != ite; ++it)
{
LandREntity* Ent = dynamic_cast<LandREntity*>((*it)->getEdge());
if (Ent != this)
mp_Neighbours->insert(Ent);
}
}
void CoreXmlReaderClient::startHierarchy(const DocumentsReader::ContentStructuredDocument &contentDocument)
{
m_handler->m_parentlastOpenedNode = 0;
m_handler->set_LastStructureId(m_handler->get_LastStructureId() + 1);
m_handler->set_lastNodeId(1);
m_handler->m_openedNodes.clear();
m_handler->m_openedNodes.push_back(0);
startNode(contentDocument, false);
}
bool PorscheSteeringWheel::homeWheel()
{
bool run;
bool success = true;
unsigned char pdodata[8];
//std::cerr << "Starting node... ";
if (!startNode())
success = false;
//std::cerr << "Enabling operation... ";
if (!enableOp())
success = false;
//std::cerr << "Setting operation mode... ";
if (!setOpMode(6))
success = false;
//std::cerr << "Setting up homing mode... ";
if (!setupHoming(0, 33, 100000, 10))
success = false;
;
//std::cerr << "Starting homing... ";
if (!enableHoming())
success = false;
//std::cerr << "Waiting till homing is finished... ";
run = true;
unsigned long starttime = getTimeInSeconds();
while (run)
{
bus->recvPDO(1, 1, pdodata);
if ((pdodata[1] & 0x14) == 0x14)
run = false;
if ((getTimeInSeconds() - starttime) > 10)
{
run = false;
success = false;
}
std::cerr << "Elapsed time: " << (getTimeInSeconds() - starttime) << std::endl;
}
//std::cerr << "homing finished!" << std::endl;
//std::cerr << "Shutting down... ";
if (!shutdown())
success = false;
//std::cerr << "Stopping node... ";
if (!stopNode())
success = false;
return success;
}
void LineStringEntity::computeLineOrientUpNeighbours()
{
mp_LOUpNeighbours = new std::vector<LineStringEntity*>();
geos::planargraph::DirectedEdgeStar* UpStar = startNode()->getOutEdges();
geos::geom::Coordinate UpNodeCoo = startNode()->getCoordinate();
std::vector<geos::planargraph::DirectedEdge*>::iterator it =
UpStar->iterator();
std::vector<geos::planargraph::DirectedEdge*>::iterator ite =
UpStar->end();
for (; it != ite; ++it)
{
LineStringEntity* Unit = dynamic_cast<LineStringEntity*>((*it)->getEdge());
if (Unit->endNode()->getCoordinate().equals(UpNodeCoo))
mp_LOUpNeighbours->push_back(Unit);
}
}
bool PorscheSteeringWheel::startAngleTorqueMode()
{
if (startNode())
{
bus->recvPDO();
bus->recvPDO();
return true;
}
else
return false;
}
// -------------------------------------------------------------------------
double Pgr_trspHandler::construct_path(int64_t ed_id, Position pos) {
pgassert(pos != ILLEGAL);
if (m_parent[ed_id].isIllegal(pos)) {
Path_t pelement;
auto cur_edge = &m_edges[ed_id];
if (pos == RC_EDGE) {
pelement.node = cur_edge->startNode();
pelement.cost = cur_edge->cost();
} else {
pelement.node = cur_edge->endNode();
pelement.cost = cur_edge->r_cost();
}
pelement.edge = cur_edge->edgeID();
m_path.push_back(pelement);
pgassert(m_path.start_id() == m_start_vertex);
return pelement.cost;
}
double ret = construct_path(m_parent[ed_id].e_idx[pos],
m_parent[ed_id].v_pos[pos]);
Path_t pelement;
auto cur_edge = &m_edges[ed_id];
if (pos == RC_EDGE) {
pelement.node = cur_edge->startNode();
pelement.cost = m_dCost[ed_id].endCost - ret;
ret = m_dCost[ed_id].endCost;
} else {
pelement.node = cur_edge->endNode();
pelement.cost = m_dCost[ed_id].startCost - ret;
ret = m_dCost[ed_id].startCost;
}
pelement.edge = cur_edge->edgeID();
m_path.push_back(pelement);
return ret;
}
// -------------------------------------------------------------------------
void Pgr_trspHandler::explore(
int64_t cur_node,
const EdgeInfo cur_edge,
bool isStart) {
double extra_cost = 0.0;
double totalCost;
auto vecIndex = cur_edge.get_idx(isStart);
for (const auto &index : vecIndex) {
auto edge = m_edges[index];
extra_cost = getRestrictionCost(
cur_edge.idx(),
edge, isStart);
if ((edge.startNode() == cur_node) && (edge.cost() >= 0.0)) {
totalCost = get_tot_cost(
edge.cost() + extra_cost,
cur_edge.idx(),
isStart);
if (totalCost < m_dCost[index].endCost) {
m_dCost[index].endCost = totalCost;
m_parent[edge.idx()].v_pos[RC_EDGE] = (isStart? C_EDGE : RC_EDGE);
m_parent[edge.idx()].e_idx[RC_EDGE] =
cur_edge.idx();
add_to_que(totalCost, edge.idx(), true);
}
}
if ((edge.endNode() == cur_node) && (edge.r_cost() >= 0.0)) {
totalCost = get_tot_cost(
edge.r_cost() + extra_cost,
cur_edge.idx(),
isStart);
if (totalCost < m_dCost[index].startCost) {
m_dCost[index].startCost = totalCost;
m_parent[edge.idx()].v_pos[C_EDGE] = (isStart? C_EDGE : RC_EDGE);
m_parent[edge.idx()].e_idx[C_EDGE] = cur_edge.idx();
add_to_que(totalCost, edge.idx(), false);
}
}
} // for
}
/*! This will automatically start and finish a node to load the data */
void loadBinaryValue( void * data, size_t size )
{
startNode();
std::string encoded;
loadValue( encoded );
auto decoded = base64::decode( encoded );
if( size != decoded.size() )
throw Exception("Decoded binary data size does not match specified size");
std::memcpy( data, decoded.data(), decoded.size() );
finishNode();
};
forceinline void
LayoutCursor::processCurrentNode() {
VisualNode* currentNode = node();
if (currentNode->isDirty()) {
if (currentNode->isHidden()) {
// do nothing
} else if (currentNode->getNumberOfChildren() < 1) {
currentNode->setShape(Shape::leaf);
} else {
currentNode->computeShape(na,startNode());
}
currentNode->setDirty(false);
}
if (currentNode->getNumberOfChildren() >= 1)
currentNode->setChildrenLayoutDone(true);
}
Path
Pgr_trspHandler::process_trsp(
size_t edge_count) {
pgassert(m_path.start_id() == m_start_vertex);
pgassert(m_path.end_id() == m_end_vertex);
pgassert(m_parent.empty());
m_parent.resize(edge_count + 1);
m_dCost.resize(edge_count + 1);
initialize_que();
current_node = m_start_vertex;
pgassert(m_path.start_id() == m_start_vertex);
auto cur_edge = dijkstra_exploration();
pgassert(m_path.start_id() == m_start_vertex);
if (current_node != m_end_vertex) {
Path result(m_start_vertex, m_end_vertex);
return result.renumber_vertices(m_min_id);;
}
pgassert(m_path.start_id() == m_start_vertex);
if (current_node == cur_edge.startNode()) {
construct_path(cur_edge.idx(), C_EDGE);
} else {
construct_path(cur_edge.idx(), RC_EDGE);
}
Path_t pelement;
pelement.node = m_end_vertex;
pelement.edge = -1;
pelement.cost = 0.0;
m_path.push_back(pelement);
m_path.Path::recalculate_agg_cost();
return m_path.renumber_vertices(m_min_id);
}
OutputBinaryTree::OutputBinaryTree(const FileStreamPtr& fin)
: m_fin(fin)
{
startNode(0);
}
void GUILevelPathing::CustomUpdate(float elapsedTime, Vector2f relativeMouse)
{
LevelInfo& lvl = editor.LevelData;
if (!UpdatePathing)
{
return;
}
//If rooms need to have their pathing info updated, pick one of them and update it.
if (roomsToNav > 0)
{
#pragma region Calculate "Average Length" for a room
assert(roomsToNav <= lvl.Rooms.size());
//Figure out the average path length to pass through the room.
LevelInfo::RoomData& room = lvl.Rooms[roomsToNav - 1];
LevelInfo::UIntBox rmBnds = lvl.GetBounds(roomsToNav - 1);
Array2D<BlockTypes> tempRoom(room.Walls.GetWidth(), room.Walls.GetHeight());
tempRoom.Fill(levelGrid, -ToV2i(room.MinCornerPos));
//Get a list of all open end-points of the block.
std::vector<Vector2u> openSpaces;
openSpaces.reserve((2 * tempRoom.GetWidth()) + (2 * tempRoom.GetHeight()));
for (unsigned int x = 0; x < tempRoom.GetWidth(); ++x)
{
if (tempRoom[Vector2u(x, 0)] == BT_NONE)
{
openSpaces.push_back(Vector2u(x, 0));
}
if (tempRoom[Vector2u(x, tempRoom.GetHeight() - 1)] == BT_NONE)
{
openSpaces.push_back(Vector2u(x, tempRoom.GetHeight() - 1));
}
}
for (unsigned int y = 0; y < tempRoom.GetHeight(); ++y)
{
if (tempRoom[Vector2u(0, y)] == BT_NONE)
{
openSpaces.push_back(Vector2u(0, y));
}
if (tempRoom[Vector2u(tempRoom.GetWidth() - 1, y)] == BT_NONE)
{
openSpaces.push_back(Vector2u(tempRoom.GetWidth() - 1, y));
}
}
//Get all combinations of endpoints along the edge of the room.
std::vector<Vector4u> openSpacePairs;
openSpacePairs.reserve(openSpacePairs.size() * openSpacePairs.size());
for (unsigned int i = 0; i < openSpaces.size(); ++i)
{
for (unsigned int j = i + 1; j < openSpaces.size(); ++j)
{
openSpacePairs.push_back(Vector4u(openSpaces[i].x, openSpaces[i].y,
openSpaces[j].x, openSpaces[j].y));
}
}
std::random_shuffle(openSpacePairs.begin(), openSpacePairs.end());
//Trim down the number of pairs to a reasonable size.
const unsigned int MAX_PAIRS = 30;
if (openSpacePairs.size() > MAX_PAIRS)
{
openSpacePairs.resize(MAX_PAIRS);
}
//Now go through every pair and get the average path length.
unsigned int totalSteps = 0;
unsigned int nPaths = 0;
LevelGraph graph(tempRoom);
LevelGraphPather pather(&graph);
GraphSearchGoal<LevelNode> goal = GraphSearchGoal<LevelNode>(LevelNode(Vector2u()));
std::vector<LevelNode> dummyPath;
float dummyTraverseCost, dummySearchCost;
for (unsigned int i = 0; i < openSpacePairs.size(); ++i)
{
Vector2u start(openSpacePairs[i].x, openSpacePairs[i].y),
end(openSpacePairs[i].z, openSpacePairs[i].w);
dummyPath.clear();
goal.SpecificEnd.SetValue(end);
if (pather.Search(start, goal, dummyTraverseCost, dummySearchCost, dummyPath))
{
totalSteps += dummyPath.size();
nPaths += 1;
}
}
if (nPaths == 0)
{
room.AverageLength = 0.0f;
}
else
{
room.AverageLength = (float)totalSteps / (float)nPaths;
}
#pragma endregion
roomsToNav -= 1;
}
else if (roomsToPath > 0)
{
assert(lvl.Rooms.size() >= roomsToPath);
//Path from the room to the team bases.
path.clear();
for (unsigned int i = 0; i < 2; ++i)
{
unsigned int teamIndex = (i == 0 ? lvl.Team1Base : lvl.Team2Base);
LevelInfo::RoomData& room = lvl.Rooms[roomsToPath - 1];
LevelInfo::UIntBox rmBnds = lvl.GetBounds(roomsToPath - 1);
RoomNode startNode(&room);
LevelInfo::RoomData& goalRm = lvl.Rooms[teamIndex];
LevelInfo::UIntBox goalBnds = lvl.GetBounds(teamIndex);
GraphSearchGoal<RoomNode> goal = GraphSearchGoal<RoomNode>(RoomNode(&goalRm));
//The pather may fail if a room isn't connected to anything.
float& nSteps = nStepsFromRoomToTeamBases[roomsToPath - 1][i];
float dummyTraverseCost, dummySearchCost;
if (pather.Search(startNode, goal, dummyTraverseCost, dummySearchCost, path))
{
nSteps = 0.0f;
for (unsigned int j = 0; j < path.size(); ++j)
{
nSteps += path[j].Room->AverageLength;
}
}
else
{
nSteps = Mathf::NaN;
}
path.clear();
}
roomsToPath -= 1;
//If we've finally calculated pathing info for all the rooms, finalize pathing data.
if (roomsToPath == 0)
{
//Get the max possible distance from any room to each team base.
float maxDists[2] = { -1.0f, -1.0f };
for (unsigned int i = 0; i < nStepsFromRoomToTeamBases.size(); ++i)
{
const std::array<float, 2>& vals = nStepsFromRoomToTeamBases[i];
if (!Mathf::IsNaN(vals[0]))
{
maxDists[0] = Mathf::Max(maxDists[0], vals[0]);
}
if (!Mathf::IsNaN(vals[1]))
{
maxDists[1] = Mathf::Max(maxDists[1], vals[1]);
}
}
//Get each room's distance to each base, from 0 to 1.
for (unsigned int i = 0; i < nStepsFromRoomToTeamBases.size(); ++i)
{
roomNormalizedDistsToTeamBases[i][0] =
Mathf::Min(1.0f, nStepsFromRoomToTeamBases[i][0] / maxDists[0]);
roomNormalizedDistsToTeamBases[i][1] =
Mathf::Min(1.0f, nStepsFromRoomToTeamBases[i][1] / maxDists[1]);
}
}
}
}
forceinline bool
NextSolCursor::mayMoveDownwards(void) {
return notOnSol() && !(back && node() == startNode())
&& node()->hasSolvedChildren()
&& NodeCursor<VisualNode>::mayMoveDownwards();
}
forceinline bool
NextSolCursor::notOnSol(void) {
return node() == startNode() || node()->getStatus() != SOLVED;
}
void Algorithm::findPath(const Location& start, const Location& end,
PathVector& output) {
NodeMap l_open;
NodeMap l_closed;
std::priority_queue<Node> l_priority;
// Create Start Node
Location origin;
Node startNode(start, 0, 0, origin);
// Add start to priority queue and open set
l_priority.push(startNode);
l_open[start] = startNode;
while (!l_priority.empty()) {
Node l_current = l_priority.top();
l_priority.pop();
l_open.erase(l_current.location);
l_closed[l_current.location] = l_current;
// Check whether this is the target node
if (l_current.location == end) {
Location current = l_current.location;
while (current != start) {
// output.push_back (index);
output.insert(output.begin(), current);
NodeMapIter open_iter = l_closed.find(current);
current = open_iter->second.origin;
if (current.x == UINT_MAX)
break;
}
return;
}
// Process neighbours
Location* neighbours = new Location[m_numNeighbours];
unsigned int validNeighbours =
m_neighbourFunction(l_current.location, neighbours, m_customData);
for (size_t ii = 0; ii < validNeighbours; ii++) {
// TODO: not needed? if (neighbours[ii] < 0) continue; // Not a
// valid neighbour
int cost = m_costFunction(neighbours[ii], m_customData);
// If cost is -ve, we will close the node
unsigned int path = l_current.distance + cost;
unsigned int priority =
path + m_distanceFunction(neighbours[ii], end, m_customData) +
1;
Node neighbour(neighbours[ii], path, priority, l_current.location);
// Is it closed?
NodeMapIter closed_iter = l_closed.find(neighbours[ii]);
if (closed_iter != l_closed.end()) {
// But dow we now have a better path?
if (cost > 0 && path < closed_iter->second.distance) {
l_closed.erase(closed_iter);
} else {
continue; // It's closed and there's no better path
}
}
NodeMapIter open_iter = l_open.find(neighbours[ii]);
if (open_iter != l_open.end()) {
// Remove it from open if there's a better path now
l_open.erase(open_iter);
} else if (cost >= 0) {
// Neighbour not in open
l_open[neighbours[ii]] = neighbour;
l_priority.push(neighbour);
}
}
delete[] neighbours;
}
}
SPpts Pathfinding::FindPath(const Vec2& currentWorldPos, const Vec2& targetWorldPos)
{
Vec2 currentGridPos = ToGridCoords(currentWorldPos);
Vec2 targetGridPos = ToGridCoords(targetWorldPos);
if(!TheWorld::Instance()->IsPassable((UINT)targetGridPos.x, (UINT)targetGridPos.y))
return SPpts();
PathNodeSPtr startNode(new PathNode());
PathNodeSPtr endNode(new PathNode());
startNode->Initialize((int)currentGridPos.x, (int)currentGridPos.y, nullptr);
endNode->Initialize((int)targetGridPos.x, (int)targetGridPos.y, nullptr);
PathNodeVec openList;
PathNodeVec closedList;
SPpts path(new std::vector<Vec2>);
startNode->Heuristic(endNode);
openList.push_back(startNode);
while(!openList.empty())
{
PathNodeSPtr currentNode = GetNextNode(&openList, &closedList);
if(currentNode->GetID() == endNode->GetID())
{
endNode->SetParent(currentNode->GetParent());
PathNodeSPtr getPath = endNode;
// reversing
while(getPath->GetParent())
{
path->push_back(Engine::Vec2((float)getPath->GetX(), (float)getPath->GetY()));
getPath = getPath->GetParent();
} // ---> while parent
// ---> swap the path
unsigned int size = path->size();
for(unsigned int n = 0; n < size; n++)
{
if(n == (size - 1 - n))
break;
std::swap(path->at(n), path->at(size - 1 - n));
}
break;
} // ---> if curr == end
else
{
//right
AddNode(currentNode->GetX() + 1, currentNode->GetY(), currentNode->GetG() + 1, currentNode, false,
&openList, &closedList, endNode);
//left
AddNode(currentNode->GetX() - 1, currentNode->GetY(), currentNode->GetG() + 1, currentNode, false,
&openList, &closedList, endNode);
//up
AddNode(currentNode->GetX(), currentNode->GetY() - 1, currentNode->GetG() + 1, currentNode, false,
&openList, &closedList, endNode);
//down
AddNode(currentNode->GetX(), currentNode->GetY() + 1, currentNode->GetG() + 1, currentNode, false,
&openList, &closedList, endNode);
//left up diag
AddNode(currentNode->GetX() - 1, currentNode->GetY() - 1, currentNode->GetG() + 1.414f, currentNode, true,
&openList, &closedList, endNode);
//right up diag
AddNode(currentNode->GetX() + 1, currentNode->GetY() - 1, currentNode->GetG() + 1.414f, currentNode, true,
&openList, &closedList, endNode);
//left down diag
AddNode(currentNode->GetX() - 1, currentNode->GetY() + 1, currentNode->GetG() + 1.414f, currentNode, true,
&openList, &closedList, endNode);
//right down diag
AddNode(currentNode->GetX() + 1, currentNode->GetY() + 1, currentNode->GetG() + 1.414f, currentNode, true,
&openList, &closedList, endNode);
}
} // ---> while(){}
// ---> converting points to world coords from grid coords
unsigned int size = path->size();
for(unsigned int i = 0; i < size; i++)
{
Vec2 temp = ToWorldCoords(path->at(i));
path->at(i) = temp;
}
return path;
}
NodeInspector* Connection::startInspector() const
{
NodeInspector* i = gscene()->getInspector(startNode());
Q_ASSERT(i);
return i;
}
bool AStarPlanner::computePath(std::vector<Util::Point>& agent_path, Util::Point start, Util::Point goal, SteerLib::GridDatabase2D * _gSpatialDatabase, bool append_to_path)
{
gSpatialDatabase = _gSpatialDatabase;
int startIndex = gSpatialDatabase->getCellIndexFromLocation(start);
SearchNodePtr startNode(new SearchNode(startIndex, 0, 0));
int goalIndex = gSpatialDatabase->getCellIndexFromLocation(goal);
std::vector<SearchNodePtr> open;
std::vector<SearchNodePtr> closed;
SearchNodePtr goalNode(nullptr);
open.push_back(startNode);
while (!open.empty()) {
// Get the node with the minimum f, breaking ties on g.
std::vector<SearchNodePtr>::iterator minIter = std::min_element(open.begin(), open.end());
SearchNodePtr minNode = *minIter;
open.erase(minIter);
// If we're visiting the goal, we're finished.
if (minNode->index() == goalIndex) {
goalNode = minNode;
break;
}
// Expand this node.
std::vector<SearchNodePtr> expandedList = _expand(minNode, goal);
for (SearchNodePtr& expandedNode : expandedList) {
// If this cell is already in the open set, check if this is a cheaper path.
std::vector<SearchNodePtr>::iterator iter = std::find(open.begin(), open.end(), expandedNode);
if (iter != open.end()) {
if (expandedNode->g() < (*iter)->g()) {
(*iter)->g(expandedNode->g());
(*iter)->prev(minNode);
}
} else {
if (std::find(closed.begin(), closed.end(), expandedNode) == closed.end()) {
expandedNode->prev(minNode);
open.push_back(expandedNode);
}
}
}
closed.push_back(minNode);
}
// Check if a path to the goal was not found.
if (goalNode == nullptr) {
return false;
}
// Go back from goal node to start.
SearchNodePtr ptr = goalNode;
float totalPathCost = 0;
while (ptr != nullptr) {
Util::Point point;
gSpatialDatabase->getLocationFromIndex(ptr->index(), point);
agent_path.insert(agent_path.begin(), point);
SearchNodePtr prev = ptr->prev();
if (prev == nullptr) break;
Util::Point prevPoint;
gSpatialDatabase->getLocationFromIndex(prev->index(), prevPoint);
totalPathCost += distanceBetween(point, prevPoint);
ptr = prev;
}
std::cout << "\nTotal path cost is " << totalPathCost << std::endl;
std::cout << "\nThe number of expanded nodes is " << numExpanded << std::endl;
return true;
}
void CoreXmlReaderClient::startIndexing(const DocumentsReader::ContentStructuredDocument &contentDocument)
{
startNode(contentDocument, true);
}
std::list<Tile *> TileMap::findShortestPath(Vec3<int> origin, Vec3<int> destination,
unsigned int iterationLimit,
const CanEnterTileHelper &canEnterTile, float)
{
TRACE_FN;
PathNodeComparer c;
std::unordered_map<Tile *, PathNode> visitedTiles;
std::set<PathNode, PathNodeComparer> fringe(c);
Vec3<float> goalPosition;
unsigned int iterationCount = 0;
LogInfo("Trying to route from {%d,%d,%d} to {%d,%d,%d}", origin.x, origin.y, origin.z,
destination.x, destination.y, destination.z);
if (origin.x < 0 || origin.x >= this->size.x || origin.y < 0 || origin.y >= this->size.y ||
origin.z < 0 || origin.z >= this->size.z)
{
LogError("Bad origin {%d,%d,%d}", origin.x, origin.y, origin.z);
return {};
}
if (destination.x < 0 || destination.x >= this->size.x || destination.y < 0 ||
destination.y >= this->size.y || destination.z < 0 || destination.z >= this->size.z)
{
LogError("Bad destination {%d,%d,%d}", destination.x, destination.y, destination.z);
return {};
}
goalPosition = {destination.x, destination.y, destination.z};
Tile *goalTile = this->getTile(destination);
if (!goalTile)
{
LogError("Failed to get destination tile at {%d,%d,%d}", destination.x, destination.y,
destination.z);
return {};
}
Tile *startTile = this->getTile(origin);
if (!startTile)
{
LogError("Failed to get origin tile at {%d,%d,%d}", origin.x, origin.y, origin.z);
return {};
}
if (origin == destination)
{
LogInfo("Destination == origin {%d,%d,%d}", destination.x, destination.y, destination.z);
return {goalTile};
}
PathNode startNode(0.0f, nullptr, startTile, goalPosition);
fringe.emplace(startNode);
visitedTiles.emplace(startTile, startNode);
auto closestNodeSoFar = *fringe.begin();
while (iterationCount++ < iterationLimit)
{
auto first = fringe.begin();
if (first == fringe.end())
{
LogInfo("No more tiles to expand after %d iterations", iterationCount);
return {};
}
auto nodeToExpand = *first;
fringe.erase(first);
// Make it so we always try to move at least one tile
if (closestNodeSoFar.parentTile == nullptr)
closestNodeSoFar = nodeToExpand;
Vec3<int> currentPosition = nodeToExpand.thisTile->position;
if (currentPosition == destination)
return getPathToNode(visitedTiles, nodeToExpand);
if (nodeToExpand.distanceToGoal < closestNodeSoFar.distanceToGoal)
{
closestNodeSoFar = nodeToExpand;
}
for (int z = -1; z <= 1; z++)
{
for (int y = -1; y <= 1; y++)
{
for (int x = -1; x <= 1; x++)
{
if (x == 0 && y == 0 && z == 0)
{
continue;
}
auto nextPosition = currentPosition;
nextPosition.x += x;
nextPosition.y += y;
nextPosition.z += z;
if (!tileIsValid(nextPosition))
continue;
Tile *tile = this->getTile(nextPosition);
// If Skip if we've already expanded this, as in a 3d-grid we know the first
// expansion will be the shortest route
if (visitedTiles.find(tile) != visitedTiles.end())
continue;
// FIXME: Make 'blocked' tiles cleverer (e.g. don't plan around objects that
// will
// move anyway?)
if (!canEnterTile.canEnterTile(nodeToExpand.thisTile, tile))
continue;
// FIXME: The old code *tried* to disallow diagonal paths that would clip past
// scenery but it didn't seem to work, no we should re-add that here
float newNodeCost = nodeToExpand.costToGetHere;
newNodeCost +=
glm::length(Vec3<float>{nextPosition} - Vec3<float>{currentPosition});
// make pathfinder biased towards vehicle's altitude preference
newNodeCost += canEnterTile.adjustCost(nextPosition, z);
PathNode newNode(newNodeCost, nodeToExpand.thisTile, tile, goalPosition);
visitedTiles.emplace(tile, newNode);
fringe.emplace(newNode);
}
}
}
}
LogInfo("No route found after %d iterations, returning closest path {%d,%d,%d}", iterationCount,
closestNodeSoFar.thisTile->position.x, closestNodeSoFar.thisTile->position.y,
closestNodeSoFar.thisTile->position.z);
return getPathToNode(visitedTiles, closestNodeSoFar);
}
void
DrawingCursor::processCurrentNode(void) {
Gist::VisualNode* n = node();
double parentX = x - static_cast<double>(n->getOffset());
double parentY = y - static_cast<double>(Layout::dist_y) + nodeWidth;
if (!n->isRoot() &&
(n->getParent(na)->getStatus() == STOP ||
n->getParent(na)->getStatus() == UNSTOP) )
parentY -= (nodeWidth-failedWidth)/2;
double myx = x;
double myy = y;
if (n->getStatus() == STOP || n->getStatus() == UNSTOP)
myy += (nodeWidth-failedWidth)/2;
if (n != startNode()) {
if (n->isOnPath())
painter.setPen(Qt::red);
else
painter.setPen(Qt::black);
// Here we use drawPath instead of drawLine in order to
// workaround a strange redraw artefact on Windows
QPainterPath path;
path.moveTo(myx,myy);
path.lineTo(parentX,parentY);
painter.drawPath(path);
}
// draw shadow
if (n->isMarked()) {
painter.setBrush(Qt::gray);
painter.setPen(Qt::NoPen);
if (n->isHidden()) {
QPointF points[3] = {QPointF(myx+shadowOffset,myy+shadowOffset),
QPointF(myx+nodeWidth+shadowOffset,
myy+hiddenDepth+shadowOffset),
QPointF(myx-nodeWidth+shadowOffset,
myy+hiddenDepth+shadowOffset),
};
painter.drawConvexPolygon(points, 3);
} else {
switch (n->getStatus()) {
case Gist::SOLVED:
{
QPointF points[4] = {QPointF(myx+shadowOffset,myy+shadowOffset),
QPointF(myx+halfNodeWidth+shadowOffset,
myy+halfNodeWidth+shadowOffset),
QPointF(myx+shadowOffset,
myy+nodeWidth+shadowOffset),
QPointF(myx-halfNodeWidth+shadowOffset,
myy+halfNodeWidth+shadowOffset)
};
painter.drawConvexPolygon(points, 4);
}
break;
case Gist::FAILED:
painter.drawRect(myx-halfFailedWidth+shadowOffset,
myy+shadowOffset, failedWidth, failedWidth);
break;
case Gist::UNSTOP:
case Gist::STOP:
{
QPointF points[8] = {QPointF(myx+shadowOffset-quarterFailedWidthF,
myy+shadowOffset),
QPointF(myx+shadowOffset+quarterFailedWidthF,
myy+shadowOffset),
QPointF(myx+shadowOffset+halfFailedWidth,
myy+shadowOffset
+quarterFailedWidthF),
QPointF(myx+shadowOffset+halfFailedWidth,
myy+shadowOffset+halfFailedWidth+
quarterFailedWidthF),
QPointF(myx+shadowOffset+quarterFailedWidthF,
myy+shadowOffset+failedWidth),
QPointF(myx+shadowOffset-quarterFailedWidthF,
myy+shadowOffset+failedWidth),
QPointF(myx+shadowOffset-halfFailedWidth,
myy+shadowOffset+halfFailedWidth+
quarterFailedWidthF),
QPointF(myx+shadowOffset-halfFailedWidth,
myy+shadowOffset
+quarterFailedWidthF),
};
painter.drawConvexPolygon(points, 8);
}
break;
case Gist::BRANCH:
painter.drawEllipse(myx-halfNodeWidth+shadowOffset,
myy+shadowOffset, nodeWidth, nodeWidth);
break;
case Gist::UNDETERMINED:
painter.drawEllipse(myx-halfNodeWidth+shadowOffset,
myy+shadowOffset, nodeWidth, nodeWidth);
break;
}
}
}
painter.setPen(Qt::SolidLine);
if (n->isHidden()) {
if (n->hasOpenChildren()) {
QLinearGradient gradient(myx-nodeWidth,myy,
myx+nodeWidth*1.3,myy+hiddenDepth*1.3);
if (n->hasSolvedChildren()) {
gradient.setColorAt(0, white);
gradient.setColorAt(1, green);
} else if (n->hasFailedChildren()) {
gradient.setColorAt(0, white);
gradient.setColorAt(1, red);
} else {
gradient.setColorAt(0, white);
gradient.setColorAt(1, QColor(0,0,0));
}
painter.setBrush(gradient);
} else {
if (n->hasSolvedChildren())
painter.setBrush(QBrush(green));
else
painter.setBrush(QBrush(red));
}
QPointF points[3] = {QPointF(myx,myy),
QPointF(myx+nodeWidth,myy+hiddenDepth),
QPointF(myx-nodeWidth,myy+hiddenDepth),
};
painter.drawConvexPolygon(points, 3);
} else {
switch (n->getStatus()) {
case Gist::SOLVED:
{
if (n->isCurrentBest(curBest)) {
painter.setBrush(QBrush(orange));
} else {
painter.setBrush(QBrush(green));
}
QPointF points[4] = {QPointF(myx,myy),
QPointF(myx+halfNodeWidth,myy+halfNodeWidth),
QPointF(myx,myy+nodeWidth),
QPointF(myx-halfNodeWidth,myy+halfNodeWidth)
};
painter.drawConvexPolygon(points, 4);
}
break;
case Gist::FAILED:
painter.setBrush(QBrush(red));
painter.drawRect(myx-halfFailedWidth, myy, failedWidth, failedWidth);
break;
case Gist::UNSTOP:
case Gist::STOP:
{
painter.setBrush(n->getStatus() == STOP ?
QBrush(red) : QBrush(green));
QPointF points[8] = {QPointF(myx-quarterFailedWidthF,myy),
QPointF(myx+quarterFailedWidthF,myy),
QPointF(myx+halfFailedWidth,
myy+quarterFailedWidthF),
QPointF(myx+halfFailedWidth,
myy+halfFailedWidth+
quarterFailedWidthF),
QPointF(myx+quarterFailedWidthF,
myy+failedWidth),
QPointF(myx-quarterFailedWidthF,
myy+failedWidth),
QPointF(myx-halfFailedWidth,
myy+halfFailedWidth+
quarterFailedWidthF),
QPointF(myx-halfFailedWidth,
myy+quarterFailedWidthF),
};
painter.drawConvexPolygon(points, 8);
}
break;
case Gist::BRANCH:
painter.setBrush(n->childrenLayoutIsDone() ? QBrush(blue) :
QBrush(white));
painter.drawEllipse(myx-halfNodeWidth, myy, nodeWidth, nodeWidth);
break;
case Gist::UNDETERMINED:
painter.setBrush(Qt::white);
painter.drawEllipse(myx-halfNodeWidth, myy, nodeWidth, nodeWidth);
break;
}
}
if (copies && (n->hasCopy() && !n->hasWorkingSpace())) {
painter.setBrush(Qt::darkRed);
painter.drawEllipse(myx, myy, 10.0, 10.0);
}
if (copies && n->hasWorkingSpace()) {
painter.setBrush(Qt::darkYellow);
painter.drawEllipse(myx, myy + 10.0, 10.0, 10.0);
}
if (n->isBookmarked()) {
painter.setBrush(Qt::black);
painter.drawEllipse(myx-10-0, myy, 10.0, 10.0);
}
}
/** Parse GCC dump */
void TestParser::parseGCCUnit( DumpUnitInfo *unit)
{
total_lines_num = 0;
DumpPos beg = unit->pos();
DumpPos end = unit->end();
if ( !file.isOpen() && !file.open( QIODevice::ReadOnly))
{
return;
}
file.reset();
/** Read file line by line */
QTextStream in( &file);
QString line;
unit_beg = 0;
unit_id = 0;
do
{
line = in.readLine();
if ( total_lines_num >= beg)
{
break;
}
total_lines_num++;
} while ( !line.isNull());
cur_line_num = 0;
/** Init state */
setStateInit();
#ifdef _DEBUG
out( "Started parsing");
#endif
do
{
curr_line = in.readLine();
cur_line_num++;
if ( !nextLineGCC( curr_line))
{
line.append( curr_line);
} else
{
if ( !line.isNull())
{
if ( nodeStopGCC( line))
{
endNode();
setStateDefault();
}
if ( nodeStartGCC( line))
{
setStateNode();
startNode();
}
parseLineGCC( line);
}
line = curr_line;
}
if ( total_lines_num >= end)
break;
total_lines_num++;
} while ( !curr_line.isNull());
if ( isStateNode())
{
endNode();
}
#ifdef _DEBUG
out( "Finished parsing");
#endif
}
PathPlanner::PathPlanner(int startX, int startY, int goalX, int goalY, Map* map) {
_aStarAlgorithm = new AStarSearch<MapSearchNode>(MAX_NODES_COUNT);
MapSearchNode startNode(startX, startY, map);
MapSearchNode goalNode(goalX, goalY, map);
_aStarAlgorithm->SetStartAndGoalStates(startNode, goalNode);
}
