// Get all neighbouring directions
RoadTile::Directions RoadTile::getNeighbouringDirections() {
Directions dir;
for (Tile* t : getNeighbours()) {
dir.insert(Tile::calculateDirection(getTilePosition(), t->getTilePosition()));
}
return dir;
}
void BFS::initializeAnimation(){
setMenu(false);
int r=255, g=255, b=0;
int color_step= 25;
m_operations.push_back([=,this](){ getNode(_currentNode)->setColor(QColor(r, g, b));colorNode(_currentNode); });
r-=color_step;
g-=color_step;
QQueue<int> queue;
queue.append(_currentNode);
getNode(_currentNode)->setVisited(true);
while(!queue.empty()){
int curr = queue.at(0);
queue.pop_front();
NodeList list = getNeighbours(curr);
for (auto iter : list){
if(!getNode(iter)->visited()){
getNode(iter)->setVisited(true);
m_operations.push_back([=,this](){getEdge(curr, iter)->setColor(QColor(r,g,b)); colorEdge(curr, iter); emit highlightLine(10);});
m_operations.push_back([=,this](){getNode(iter)->setColor(QColor(r,g,b)); colorNode(iter);});
queue.append(iter);
}
}
r-=color_step;
g-=color_step;
}
m_animationInitialized= true;
m_currentStepInAnimation= 0;
m_numberOfStepsInAnimation= m_operations.size();
}
double hillClimbing(double x[10], double bsf[10]){
double currentNode[10];
memcpy(currentNode, x,sizeof(double)*10);
double neighbors[10][10];
int maxRange = 0;
double nextEval = -9999;
double nextNode[10];
double currentEval = evaluar(currentNode);
double temp;
int i =0;
while(numeval <= NUMEVAL){
getNeighbours(currentNode,neighbors);
nextEval = -99999;
int i = 0;
for(i = 0; i < 10; i++){
numeval++;
temp = evaluar(neighbors[i]);
if (temp > nextEval){
memcpy(nextNode,neighbors[i],sizeof(double)*10);
nextEval = temp;
}
}
if (nextEval <= currentEval){
memcpy(bsf,currentNode,sizeof(double)*10);
return currentEval;
}
memcpy(currentNode,nextNode,sizeof(double)*10);
currentEval = nextEval;
}
}
void GameBoard::revealCell(Cell *_cell) {
if (!_cell->isRevealed() && !_cell->isMine()) {
m_UnrevealedCells -= 1;
}
_cell->reveal();
if (_cell->isMine()) {
if (!m_PlayerHasLost) {
revealAllMines();
m_PlayerHasLost = true;
Common::InstanceCollection::getInstance<NotificationManager>().NotifyLoss();
}
return;
}
if (_cell->getNeighbouringMines() == 0) {
auto& neighbours = getNeighbours(_cell);
for (auto& n : neighbours) {
if (!n->isRevealed()) {
revealCell(n);
}
}
}
handlePotentialWin();
}
void Grid::fillNeighboursOf(Point position, Block block, std::set<Point>& visitedPositions) const
{
std::vector<Point> neighbours = getNeighbours(position, block);
for (auto neighbour : neighbours)
{
auto result = visitedPositions.insert(neighbour);
if (result.second)
{
fillNeighboursOf(neighbour, block, visitedPositions);
}
}
}
int main(int argc, char *argv[]){
int x, y, i, j;
FILE *file;
char iniBoard[MAX_SIZE][MAX_SIZE];
int status[MAX_SIZE][MAX_SIZE];
SDL_Simplewin sw;
SDL_Rect rectangle;
file = NULL;
Neill_SDL_Init(&sw);
initial(iniBoard, status);
file = fopen(argv[1], "r");
/* fopen returns NULL pointer on failure */
if (file == NULL){
printf("Could not open file. \n");
}
else {
printf("File (%s) opened. \n", argv[1]);
fscanf(file, "%d %d", &x, &y);
printf("Row: %d Column: %d \n", x, y);
char board[y][x];
if(fgetc(file) != EOF){
for(j = 0; j < y; j++){
for(i = 0; i < x + 1; i++){
board[j][i] = fgetc(file);
}
}
}
/* Closing file */
fclose(file);
/* Input the board which read from the txt file to a 50x50 board*/
inputBoard(x, y, board, iniBoard);
do{
/* Sleep for a short time */
SDL_Delay(MILLISECONDDELAY);
SDL_RenderClear(sw.renderer);
/* Draw the actual board */
drawBoard(iniBoard, sw, rectangle, x, y);
/* Update window */
SDL_RenderPresent(sw.renderer);
SDL_UpdateWindowSurface(sw.win);
/* Get the neighbours' status */
getNeighbours(x, y, iniBoard, status);
/* Calculate next generation */
nextGen(x, y, iniBoard, status);
Neill_SDL_Events(&sw);
}while(!sw.finished);
/* Clear up graphics subsystems */
atexit(SDL_Quit);
}
return 0;
}
struct Matrix
iterateMatrix (struct Matrix plat)
{
for (int row = 1; row < plat.row-1; row++){
for(int column = 1; column < plat.column-1; column++){
tempCoord.row = row;
tempCoord.column = column;
nextTo = getNeighbours(tempCoord);
char x = followRules(nextTo);
plat.inc[row][column] = x;
}
}
return plat;
}
// Return the border of the given object containing the given point.
vector<Point> extractBorderFromPoint(const Domain domain, const DigitalSet object, Point start) {
vector<Point> border ;
border.push_back(start) ;
vector<Point> neigh = getNeighbours(domain, start) ;
for(unsigned i = 0 ; i <= neigh.size() ; i++) {
if(!object(neigh[mod(i-1, neigh.size())]) && object(neigh[mod(i, neigh.size())])) {
border.push_back(neigh[mod(i, neigh.size())]) ;
break ;
}
}
if(border.size() == 2) {
while(border.back() != start) {
border.push_back(nextPoint(object, border.end()[-2], border.end()[-1])) ;
}
}
return border ;
}
//Updates the current map.
void updateMap()
{
int i, j;
for ( i = 0; i < 30; i++ )
{
for ( j = 0; j < 30; j++ )
{
int n = getNeighbours(i, j);
//If Cell Is Alive
if(currentMap[i][j] == 1)
{
//Sustainable amount of neighbours.
if(n == 2 || n == 3)
{
newMap[i][j] = 1;
}
//Underpopulation or Overcrowding
else
{
newMap[i][j] = 0;
}
}
//If Cell Is Dead.
else if(currentMap[i][j] == 0)
{
//Reproduction.
if(n == 3)
{
newMap[i][j] = 1;
}
}
}
}
//Update the current map with the updated map.
for ( i = 0; i < 30; i++ )
{
for ( j = 0; j < 30; j++ )
{
currentMap[i][j] = newMap[i][j];
}
}
}
void QuadEquationTranformationGraph::extend( )
{
typedef IntLabeledGraph::edge_type edge_type;
typedef IntLabeledGraph::vertex_type vertex_type;
// cout << "Extend" << endl;
// A. choose any non-processed equation
if( isDone( ) )
return;
Word eq = (*equationsInProcess.begin( )).first;
int eq_num = (*equationsInProcess.begin( )).second;
equationsInProcess.erase( equationsInProcess.begin( ) );
processedEquations[eq] = eq_num;
// B. Find the neighbours of the current equation and add them into the graph
set< Word > n = getNeighbours( eq );
for( set< Word >::const_iterator n_it=n.begin( ) ; n_it!=n.end( ) ; ++n_it ) {
Word new_eq = *n_it;
int target;
map< Word , int >::iterator e_it;
if( ( e_it = processedEquations.find(new_eq) ) !=processedEquations.end( ) ) {
target = (*e_it).second;
} else if( ( e_it = equationsInProcess.find(new_eq) ) !=equationsInProcess.end( ) ) {
target = (*e_it).second;
} else {
target = equationsInProcess[ new_eq ] = theGraph.newVertex( );
if( isTrivialSolution(new_eq) )
hasSolution = true;
}
theGraph.newEdge( eq_num , edge_type( target , 1 ) );
}
}
GridWorld::TilePair GridWorld::getMinSuccessor(GridWorld::Tile*& tile){
Tile* minTile = 0;
double minCost = PF_INFINITY;
std::vector<Tile*> neighbours(getNeighbours(tile));
for (int i = 0; i < neighbours.size(); i++){
double cost = calculateC(tile, neighbours[i]);
double g = neighbours[i]->g;
if (cost == PF_INFINITY || g == PF_INFINITY){
continue;
}
if (cost + g < minCost){ //potential overflow?
minTile = neighbours[i];
minCost = cost + g;
}
}
return TilePair(minTile, minCost);
}
// Assign cluster numbers in a spectrum
void clusterSpectrum(Spectrum *spec, int *flags) {
Neighbour curr_pt, nbbuff[NBBUFLEN];
int curr_flag = 1;
for(int a=0; a<spec->len; ++a) {
curr_pt.scan = &(spec->scans[a]);
for(int b=0; b<spec->scans[a].len; ++b) {
curr_pt.point = &(curr_pt.scan->points[b]);
if (curr_pt.point->cluster_flag == NULL) {
int nbs = getNeighbours(curr_pt, nbbuff);
if (nbs >= min_nb) {
// Cluster point
} else {
// Noise point
curr_pt.point->cluster_flag = &(flags[0]);
}
}
}
}
}
void GameOfLifeModel::setUpGameOfLife(int aRows, int aColumns, int aAnimFrequency){
m_isRunning = false;
m_rows = aRows;
m_columns = aColumns;
m_total = m_rows * m_columns;
m_animFrequency= aAnimFrequency;
m_cells.clear();
m_cells.reserve(m_total);
for(int i = 0; i < m_total; ++i) {
Cell* newCell = new Cell();
//getting all the neighbours of this cell and storing int the cell struct itself
newCell->m_neighbours = getNeighbours(i);
//putting this cell into the world
m_cells.append(newCell);
}
generateRandom();
m_timer.setSingleShot(true);
//m_timer.setInterval(m_animFrequency);
//connect(&m_timer, SIGNAL(timeout()), this, SLOT(calculateNextStep()));
}
//----------------------------------------------------------------------------
void PrNestedTriangulation::getParents(int i, int jlev, int& p1, int& p2)
//-----------------------------------------------------------------------------
// Assuming that node i belongs to V^j\V^{j-1} and that
// jlev >= 1, find i's two parent nodes in V^{j-1} p1 and p2.
{
vector<int> neighbours;
getNeighbours(i,jlev,neighbours);
if(isBoundary(i))
{
// Then there are exactly four neighbours. We want the
// first and last ones.
p1 = neighbours[0];
p2 = neighbours[3];
return;
}
else
{
// Then there are exactly six neighbours. Find the first
// coarse one.
if(neighbours[0] < getNumNodes(jlev-1))
{
p1 = neighbours[0];
p2 = neighbours[3];
return;
}
else if(neighbours[1] < getNumNodes(jlev-1))
{
p1 = neighbours[1];
p2 = neighbours[4];
return;
}
else
{
p1 = neighbours[2];
p2 = neighbours[5];
return;
}
}
}
double hillclimbing(double x[30], double resultA[30])
{
double currentNode[30];
memcpy(currentNode, x, sizeof(double)*30);
double neighbors[60][30];
double nextEval = 99999;
double nextNode[30];
double currentEval = evaluate(currentNode);
printf("RandomEval: %lf\n", currentEval);
double tmp;
int i;
//printf("Test %lf\n", currentEval);
while(1)//currentEval > 0.0001)
{
getNeighbours(currentNode,neighbors);
nextEval = 99999;
for(i = 0; i < 60; i++)
{
tmp = evaluate(neighbors[i]);
if(tmp < nextEval){
memcpy(nextNode, neighbors[i],sizeof(double)*30);
nextEval = tmp;
}
counter++;
}
if (nextEval >= currentEval)
{
memcpy(resultA,currentNode,sizeof(double)*30);
return currentEval;
}
memcpy(currentNode,nextNode,sizeof(double)*30);
currentEval = nextEval;
//printf("currentEval[%d]: %lf\n", counter, currentEval );
}
}
void Field::setRevealState(unsigned int i, bool state, bool check_neighbours) {
if (i < 0 || i > field.size()) {
cout << "Error in setRevealState(): Argument out of bounds\n";
return;
}
field[i].is_revealed = state;
field[i].update_vertex = true;
if (is_debugging)
cout << "Tile (" << i%size.x << ", " << (int)((float)(i/size.x)) << ") = " << i << " is set to revealed\n";
if (!check_neighbours) {
return;
}
if (field[i].type == Tile::EMPTY) {
uint pos[9];
getNeighbours(i, pos);
for (uint j = 0; j < 9; ++j) {
if (!field[pos[j]].update_vertex) {
setRevealState(pos[j],state);
}
}
}
}
void Graph::bfs_transversal(Vertex *start, std::vector< std::pair<Vertex*, Vertex*> > &transversal)
{
Vertex* current;
std::queue< Vertex* > q;
std::vector< Vertex* > adj;
unsigned int i;
q.push(start);
start->set_visited(true);
while(!q.empty()) {
current = q.front();
q.pop();
adj = getNeighbours(current);
for(i = 0; i < adj.size(); i++) {
if(!adj[i]->is_visited()) {
adj[i]->set_visited(true);
transversal.push_back(std::pair<Vertex*, Vertex*>(current, adj[i]));
q.push(adj[i]);
}
}
}
}
void IndexColorPalette::mergeMostReduantColors()
{
QVector<ColorString> colorHood;
colorHood.resize(numColors());
for(int i = 0; i < numColors(); ++i)
{
colorHood[i].color = i;
colorHood[i].neighbours = getNeighbours(i);
float lSimilarity = 0.05f, rSimilarity = 0.05f;
// There will be exactly 2 colors that have only 1 neighbour, the darkest and the brightest, we don't want to remove those
if(colorHood[i].neighbours.first != -1)
lSimilarity = similarity(colors[colorHood[i].neighbours.first], colors[i]);
if(colorHood[i].neighbours.second != -1)
rSimilarity = similarity(colors[colorHood[i].neighbours.second], colors[i]);
colorHood[i].similarity = (lSimilarity + rSimilarity) / 2;
}
int mostSimilarColor = 0;
for(int i = 0; i < numColors(); ++i)
if(colorHood[i].similarity > colorHood[mostSimilarColor].similarity)
mostSimilarColor = i;
int darkerIndex = colorHood[mostSimilarColor].neighbours.first;
int brighterIndex = colorHood[mostSimilarColor].neighbours.second;
if(darkerIndex != -1 &&
brighterIndex != -1)
{
LabColor clrA = colors[darkerIndex];
LabColor clrB = colors[mostSimilarColor];
LabColor clrC = colors[brighterIndex];
// Remove two, add one = 1 color less
colors.remove(darkerIndex);
colors.remove(mostSimilarColor);
//colors.remove(brighterIndex);
insertShades(clrA, clrB, 1);
//insertShades(clrB, clrC, 1);
}
}
void dtCrowd::update(const float dt, dtCrowdAgentDebugInfo* debug)
{
m_velocitySampleCount = 0;
const int debugIdx = debug ? debug->idx : -1;
dtCrowdAgent** agents = m_activeAgents;
int nagents = getActiveAgents(agents, m_maxAgents);
// Check that all agents still have valid paths.
checkPathValidity(agents, nagents, dt);
// Update async move request and path finder.
updateMoveRequest(dt);
// Optimize path topology.
updateTopologyOptimization(agents, nagents, dt);
// Register agents to proximity grid.
m_grid->clear();
for (int i = 0; i < nagents; ++i)
{
dtCrowdAgent* ag = agents[i];
const float* p = ag->npos;
const float r = ag->params.radius;
m_grid->addItem((unsigned short)i, p[0]-r, p[2]-r, p[0]+r, p[2]+r);
}
// Get nearby navmesh segments and agents to collide with.
for (int i = 0; i < nagents; ++i)
{
dtCrowdAgent* ag = agents[i];
if (ag->state != DT_CROWDAGENT_STATE_WALKING)
continue;
// Update the collision boundary after certain distance has been passed or
// if it has become invalid.
const float updateThr = ag->params.collisionQueryRange*0.25f;
if (dtVdist2DSqr(ag->npos, ag->boundary.getCenter()) > dtSqr(updateThr) ||
!ag->boundary.isValid(m_navquery, &m_filter))
{
ag->boundary.update(ag->corridor.getFirstPoly(), ag->npos, ag->params.collisionQueryRange,
m_navquery, &m_filter);
}
// Query neighbour agents
ag->nneis = getNeighbours(ag->npos, ag->params.height, ag->params.collisionQueryRange,
ag, ag->neis, DT_CROWDAGENT_MAX_NEIGHBOURS,
agents, nagents, m_grid);
for (int j = 0; j < ag->nneis; j++)
ag->neis[j].idx = getAgentIndex(agents[ag->neis[j].idx]);
}
// Find next corner to steer to.
for (int i = 0; i < nagents; ++i)
{
dtCrowdAgent* ag = agents[i];
if (ag->state != DT_CROWDAGENT_STATE_WALKING)
continue;
if (ag->targetState == DT_CROWDAGENT_TARGET_NONE || ag->targetState == DT_CROWDAGENT_TARGET_VELOCITY)
continue;
// Find corners for steering
ag->ncorners = ag->corridor.findCorners(ag->cornerVerts, ag->cornerFlags, ag->cornerPolys,
DT_CROWDAGENT_MAX_CORNERS, m_navquery, &m_filter);
// Check to see if the corner after the next corner is directly visible,
// and short cut to there.
if ((ag->params.updateFlags & DT_CROWD_OPTIMIZE_VIS) && ag->ncorners > 0)
{
const float* target = &ag->cornerVerts[dtMin(1,ag->ncorners-1)*3];
ag->corridor.optimizePathVisibility(target, ag->params.pathOptimizationRange, m_navquery, &m_filter);
// Copy data for debug purposes.
if (debugIdx == i)
{
dtVcopy(debug->optStart, ag->corridor.getPos());
dtVcopy(debug->optEnd, target);
}
}
else
{
// Copy data for debug purposes.
if (debugIdx == i)
{
dtVset(debug->optStart, 0,0,0);
dtVset(debug->optEnd, 0,0,0);
}
}
}
// Trigger off-mesh connections (depends on corners).
for (int i = 0; i < nagents; ++i)
{
dtCrowdAgent* ag = agents[i];
if (ag->state != DT_CROWDAGENT_STATE_WALKING)
continue;
if (ag->targetState == DT_CROWDAGENT_TARGET_NONE || ag->targetState == DT_CROWDAGENT_TARGET_VELOCITY)
continue;
// Check
const float triggerRadius = ag->params.radius*2.25f;
if (overOffmeshConnection(ag, triggerRadius))
{
// Prepare to off-mesh connection.
const int idx = (int)(ag - m_agents);
dtCrowdAgentAnimation* anim = &m_agentAnims[idx];
// Adjust the path over the off-mesh connection.
dtPolyRef refs[2];
if (ag->corridor.moveOverOffmeshConnection(ag->cornerPolys[ag->ncorners-1], refs,
anim->startPos, anim->endPos, m_navquery))
{
dtVcopy(anim->initPos, ag->npos);
anim->polyRef = refs[1];
anim->active = 1;
anim->t = 0.0f;
anim->tmax = (dtVdist2D(anim->startPos, anim->endPos) / ag->params.maxSpeed) * 0.5f;
ag->state = DT_CROWDAGENT_STATE_OFFMESH;
ag->ncorners = 0;
ag->nneis = 0;
continue;
}
else
{
// Path validity check will ensure that bad/blocked connections will be replanned.
}
}
}
// Calculate steering.
for (int i = 0; i < nagents; ++i)
{
dtCrowdAgent* ag = agents[i];
if (ag->state != DT_CROWDAGENT_STATE_WALKING)
continue;
if (ag->targetState == DT_CROWDAGENT_TARGET_NONE)
continue;
float dvel[3] = {0,0,0};
if (ag->targetState == DT_CROWDAGENT_TARGET_VELOCITY)
{
dtVcopy(dvel, ag->targetPos);
ag->desiredSpeed = dtVlen(ag->targetPos);
}
else
{
// Calculate steering direction.
if (ag->params.updateFlags & DT_CROWD_ANTICIPATE_TURNS)
calcSmoothSteerDirection(ag, dvel);
else
calcStraightSteerDirection(ag, dvel);
// Calculate speed scale, which tells the agent to slowdown at the end of the path.
const float slowDownRadius = ag->params.radius*2; // TODO: make less hacky.
const float speedScale = getDistanceToGoal(ag, slowDownRadius) / slowDownRadius;
ag->desiredSpeed = ag->params.maxSpeed;
dtVscale(dvel, dvel, ag->desiredSpeed * speedScale);
}
// Separation
if (ag->params.updateFlags & DT_CROWD_SEPARATION)
{
const float separationDist = ag->params.collisionQueryRange;
const float invSeparationDist = 1.0f / separationDist;
const float separationWeight = ag->params.separationWeight;
float w = 0;
float disp[3] = {0,0,0};
for (int j = 0; j < ag->nneis; ++j)
{
const dtCrowdAgent* nei = &m_agents[ag->neis[j].idx];
float diff[3];
dtVsub(diff, ag->npos, nei->npos);
diff[1] = 0;
const float distSqr = dtVlenSqr(diff);
if (distSqr < 0.00001f)
continue;
if (distSqr > dtSqr(separationDist))
continue;
const float dist = dtMathSqrtf(distSqr);
const float weight = separationWeight * (1.0f - dtSqr(dist*invSeparationDist));
dtVmad(disp, disp, diff, weight/dist);
w += 1.0f;
}
if (w > 0.0001f)
{
// Adjust desired velocity.
dtVmad(dvel, dvel, disp, 1.0f/w);
// Clamp desired velocity to desired speed.
const float speedSqr = dtVlenSqr(dvel);
const float desiredSqr = dtSqr(ag->desiredSpeed);
if (speedSqr > desiredSqr)
dtVscale(dvel, dvel, desiredSqr/speedSqr);
}
}
// Set the desired velocity.
dtVcopy(ag->dvel, dvel);
}
// Velocity planning.
for (int i = 0; i < nagents; ++i)
{
dtCrowdAgent* ag = agents[i];
if (ag->state != DT_CROWDAGENT_STATE_WALKING)
continue;
if (ag->params.updateFlags & DT_CROWD_OBSTACLE_AVOIDANCE)
{
m_obstacleQuery->reset();
// Add neighbours as obstacles.
for (int j = 0; j < ag->nneis; ++j)
{
const dtCrowdAgent* nei = &m_agents[ag->neis[j].idx];
m_obstacleQuery->addCircle(nei->npos, nei->params.radius, nei->vel, nei->dvel);
}
// Append neighbour segments as obstacles.
for (int j = 0; j < ag->boundary.getSegmentCount(); ++j)
{
const float* s = ag->boundary.getSegment(j);
if (dtTriArea2D(ag->npos, s, s+3) < 0.0f)
continue;
m_obstacleQuery->addSegment(s, s+3);
}
dtObstacleAvoidanceDebugData* vod = 0;
if (debugIdx == i)
vod = debug->vod;
// Sample new safe velocity.
bool adaptive = true;
int ns = 0;
const dtObstacleAvoidanceParams* params = &m_obstacleQueryParams[ag->params.obstacleAvoidanceType];
if (adaptive)
{
ns = m_obstacleQuery->sampleVelocityAdaptive(ag->npos, ag->params.radius, ag->desiredSpeed,
ag->vel, ag->dvel, ag->nvel, params, vod);
}
else
{
ns = m_obstacleQuery->sampleVelocityGrid(ag->npos, ag->params.radius, ag->desiredSpeed,
ag->vel, ag->dvel, ag->nvel, params, vod);
}
m_velocitySampleCount += ns;
}
else
{
// If not using velocity planning, new velocity is directly the desired velocity.
dtVcopy(ag->nvel, ag->dvel);
}
}
// Integrate.
for (int i = 0; i < nagents; ++i)
{
dtCrowdAgent* ag = agents[i];
if (ag->state != DT_CROWDAGENT_STATE_WALKING)
continue;
integrate(ag, dt);
}
// Handle collisions.
static const float COLLISION_RESOLVE_FACTOR = 0.7f;
for (int iter = 0; iter < 4; ++iter)
{
for (int i = 0; i < nagents; ++i)
{
dtCrowdAgent* ag = agents[i];
const int idx0 = getAgentIndex(ag);
if (ag->state != DT_CROWDAGENT_STATE_WALKING)
continue;
dtVset(ag->disp, 0,0,0);
float w = 0;
for (int j = 0; j < ag->nneis; ++j)
{
const dtCrowdAgent* nei = &m_agents[ag->neis[j].idx];
const int idx1 = getAgentIndex(nei);
float diff[3];
dtVsub(diff, ag->npos, nei->npos);
diff[1] = 0;
float dist = dtVlenSqr(diff);
if (dist > dtSqr(ag->params.radius + nei->params.radius))
continue;
dist = dtMathSqrtf(dist);
float pen = (ag->params.radius + nei->params.radius) - dist;
if (dist < 0.0001f)
{
// Agents on top of each other, try to choose diverging separation directions.
if (idx0 > idx1)
dtVset(diff, -ag->dvel[2],0,ag->dvel[0]);
else
dtVset(diff, ag->dvel[2],0,-ag->dvel[0]);
pen = 0.01f;
}
else
{
pen = (1.0f/dist) * (pen*0.5f) * COLLISION_RESOLVE_FACTOR;
}
dtVmad(ag->disp, ag->disp, diff, pen);
w += 1.0f;
}
if (w > 0.0001f)
{
const float iw = 1.0f / w;
dtVscale(ag->disp, ag->disp, iw);
}
}
for (int i = 0; i < nagents; ++i)
{
dtCrowdAgent* ag = agents[i];
if (ag->state != DT_CROWDAGENT_STATE_WALKING)
continue;
dtVadd(ag->npos, ag->npos, ag->disp);
}
}
for (int i = 0; i < nagents; ++i)
{
dtCrowdAgent* ag = agents[i];
if (ag->state != DT_CROWDAGENT_STATE_WALKING)
continue;
// Move along navmesh.
ag->corridor.movePosition(ag->npos, m_navquery, &m_filter);
// Get valid constrained position back.
dtVcopy(ag->npos, ag->corridor.getPos());
// If not using path, truncate the corridor to just one poly.
if (ag->targetState == DT_CROWDAGENT_TARGET_NONE || ag->targetState == DT_CROWDAGENT_TARGET_VELOCITY)
{
ag->corridor.reset(ag->corridor.getFirstPoly(), ag->npos);
}
}
// Update agents using off-mesh connection.
for (int i = 0; i < m_maxAgents; ++i)
{
dtCrowdAgentAnimation* anim = &m_agentAnims[i];
if (!anim->active)
continue;
dtCrowdAgent* ag = agents[i];
anim->t += dt;
if (anim->t > anim->tmax)
{
// Reset animation
anim->active = 0;
// Prepare agent for walking.
ag->state = DT_CROWDAGENT_STATE_WALKING;
continue;
}
// Update position
const float ta = anim->tmax*0.15f;
const float tb = anim->tmax;
if (anim->t < ta)
{
const float u = tween(anim->t, 0.0, ta);
dtVlerp(ag->npos, anim->initPos, anim->startPos, u);
}
else
{
const float u = tween(anim->t, ta, tb);
dtVlerp(ag->npos, anim->startPos, anim->endPos, u);
}
// Update velocity.
dtVset(ag->vel, 0,0,0);
dtVset(ag->dvel, 0,0,0);
}
}
/*
This method does the same thing as the pseudo-code's updateVertex(),
except for grids instead of graphs.
Pathfinding algorimths tend to be demonstraited with a graph rather than a grid,
in order to update the cost between two tiles we must update both the tile and its neighbour.
*/
void GridWorld::updateCost(unsigned int x, unsigned int y, double newCost){
static int count = 1;
count++;
Tile* tile = getTileAt(x, y);
printf("Updating <%d, %d> from %2.0lf to %2.0lf - Update: %d\n", x, y, tile->cost, newCost, count);
km += calculateH(previous);
previous = start;
//I am aware that the following code below could be refactored by 50%
//since it's repeating itself with only a few changes
double oldCost = tile->cost;
double oldCostToTile, newCostToTile;
double currentRHS, otherG;
//Update CURRENT by finding its new minimum RHS-value from NEIGHBOURS
std::vector<Tile*> neighbours(getNeighbours(tile));
for (int i = 0; i < neighbours.size(); i++){
tile->cost = oldCost;
oldCostToTile = calculateC(tile, neighbours[i]);
tile->cost = newCost;
newCostToTile = calculateC(tile, neighbours[i]);
currentRHS = tile->rhs;
otherG = neighbours[i]->g;
if (oldCostToTile > newCostToTile){
if (tile != goal){
tile->rhs = std::min(currentRHS, (newCostToTile + otherG));
}
}
else if (currentRHS == (oldCostToTile + otherG)){
if (tile != goal){
tile->rhs = getMinSuccessor(tile).second;
}
}
}
updateVertex(tile);
//Update all NEIGHBOURING cells by finding their new min RHS-values from CURRENT
for (int i = 0; i < neighbours.size(); i++){
tile->cost = oldCost;
oldCostToTile = calculateC(tile, neighbours[i]);
tile->cost = newCost;
newCostToTile = calculateC(tile, neighbours[i]);
currentRHS = neighbours[i]->rhs;
otherG = tile->g;
if (oldCostToTile > newCostToTile){
if (neighbours[i] != goal){
neighbours[i]->rhs = std::min(currentRHS, (newCostToTile + otherG));
}
}
else if (currentRHS == (oldCostToTile + otherG)){
if (neighbours[i] != goal){
neighbours[i]->rhs = getMinSuccessor(neighbours[i]).second;
}
}
updateVertex(neighbours[i]);
}
computeShortestPath();
}
bool AStar::getShortestPath(Node _start, Node _end){
Node goal = _end;
Node start =_start;
came_from_map[start.id] = -1;
addNodeToSet(openSet,start);
while(!(openSet.size() == 0)){
#if DEBUG
printf("Size of open list is %d \n", openSet.size());
#endif
Node current = getMinimumNode(openSet);
#if DEBUG
printf("Expanding node:id= %lld\n", current.id);
#endif
if(current.data == goal.data){
cout<<"\nPath found.Reconstructing the path.\n";
reconstructPath(current.id,0);
return true;
}
removeMinimum(openSet,current);
addNodeToSet(closedSet,current);
#if DEBUG
printf ("Adding node= %lld to Closed multiset\n", current.id);
#endif
vector<Node> nodeNbrs = getNeighbours(current);
for(int i=0;i<nodeNbrs.size();i++){
//nbr is a neighbouring node
Node nbr = nodeNbrs[i];
if(findInSet(closedSet,nbr)){
//TODO: remove the node from the closed and shift in open
#if DEBUG
cout<<"Node found in closed multiset "<< nbr.id<<endl;
#endif
Node nodeInClosedList = getNodeFromSet(closedSet,nbr);
int temp_g_score = current.g_score + distance(current,nbr);
if(temp_g_score < nodeInClosedList.g_score){
printf(" Removing a Node from closed list id %lld in previous %lld \n", nodeInClosedList.id,came_from_map[nodeInClosedList.id]);
printf(" New gscore is %d , old gscore is %d new wannabe parent %lld\n",temp_g_score, nodeInClosedList.g_score,current.id );
printf(" hscore for %lld wannabe parent %d \n" ,current.id , H(current,goal));
//printf(" hscore for %lld previous parent \n" ,current.id , H(current,goal));
// reconstructPath(nodeInClosedList.id],0);
//exit(0);
removeNodeFromSet(closedSet,nodeInClosedList);
}
else{
continue;
}
}
int tentative_g_score = current.g_score + distance(current,nbr);
#if DEBUG
nbr.printData();
#endif
//TODO update this with the follwing logic.
// update if it is found in open multiset with higher g score
// or else it is not in open multiset.
pair<Node,bool> check = findInOpenSet(nbr);
if(check.second && tentative_g_score < check.first.g_score){
removeNodeFromSet(openSet,nbr);
came_from_map[check.first.id] = current.id;
#if DEBUG
printf("Setting parent of %lld to %lld\n", check.first.id, current.id);
#endif
//check.first.came_from = ¤t;
check.first.g_score = tentative_g_score;
check.first.f_score = tentative_g_score + H(check.first,goal);
addNodeToSet(openSet,check.first);
#if DEBUG
printf("Updating node in open multiset %lld with new parent = %lld\n",check.first.id,current.id);
#endif
}
else if(check.second==false){
came_from_map[check.first.id] = current.id;
#if DEBUG
printf("Setting parent of %lld to %lld\n", check.first.id, current.id);
#endif
//nbr.came_from = ¤t;
nbr.g_score = tentative_g_score;
nbr.f_score = tentative_g_score + H(nbr,goal);
addNodeToSet(openSet,nbr);
#if DEBUG
printf("Adding node= %lld to Open multiset with f_score %d \n", nbr.id,nbr.f_score);
#endif
}
}
#if DEBUG
cout<<"------------------------------------------------------------------------------------"<<endl;
#endif
}
printf("Solution Not Found. Goal is not reachable from start\n");
return false;
}
// Perform clustering
vector<Cluster*> DBScan::performClustering(vector<DataPoint> &dataPoints)
{
// Initliase clustering
char neighbors[dataPoints.size()]; // Store neighor points
char visited[dataPoints.size()]; // Store visited
numClusters = 0;
clusters.clear();
// Get pointer to underlying data points array
DataPoint *points = reinterpret_cast<DataPoint *>(&dataPoints[0]);
// Set neighbors and visited to 0
memset(neighbors, 0, dataPoints.size() * sizeof(char));
memset(visited, 0, dataPoints.size() * sizeof(char));
// Loop over all data point
for(unsigned i = 0; i < dataPoints.size(); i++)
{
// Check if data point has already been processed
if (visited[i])
continue;
// Set as visitied
visited[i] = 1;
// Get list of neghbours
unsigned found = getNeighbours(points, dataPoints.size(), i, neighbors, visited);
// if not enough points to create a cluster mark as noise
if (found < min_points)
{
dataPoints[i].cluster = -1;
// Not enough point in neighborhood, reset neighor list
memset(neighbors, 0, dataPoints.size());
continue;
}
Cluster *cluster = new Cluster(this -> survey, clusters.size() + 1, dataPoints);
clusters.push_back(cluster);
// Assign point to new cluster
dataPoints[i].cluster = cluster -> ClusterId();
cluster -> addPoint(i);
// Process all the found point
while (found != 0)
{
// Get index of next neighboring point
unsigned j = 0;
while (neighbors[j] == 0)
{
j++;
if (j >= dataPoints.size()) j = 0;
}
// Check if data point has already been processed
if (!visited[j])
{
// Mark data point as visited
visited[j] = 1;
// Add point's neighborhood to current list
found += getNeighbours(points, dataPoints.size(), j, neighbors, visited);
}
// If not already assigned to a cluster
if (dataPoints[j].cluster == 0)
{
dataPoints[j].cluster = cluster -> ClusterId();
cluster -> addPoint(j);
}
// Mark this neighbor as processed
neighbors[j] = 0;
found--;
}
}
this -> numClusters = clusters.size();
return clusters;
}
void dtCrowd::updateStepProximityData(const float dt, dtCrowdAgentDebugInfo*)
{
// Register agents to proximity grid.
m_grid->clear();
for (int i = 0; i < m_numActiveAgents; ++i)
{
dtCrowdAgent* ag = m_activeAgents[i];
const float* p = ag->npos;
const float r = ag->params.radius;
m_grid->addItem((unsigned short)i, p[0] - r, p[2] - r, p[0] + r, p[2] + r);
}
// Get nearby navmesh segments and agents to collide with.
for (int i = 0; i < m_numActiveAgents; ++i)
{
dtCrowdAgent* ag = m_activeAgents[i];
if (ag->state != DT_CROWDAGENT_STATE_WALKING)
continue;
m_navquery->updateLinkFilter(ag->params.linkFilter);
// Update the collision boundary after certain distance has been passed or
// if it has become invalid.
const float updateThr = ag->params.collisionQueryRange*0.25f;
if (dtVdist2DSqr(ag->npos, ag->boundary.getCenter()) > dtSqr(updateThr) ||
!ag->boundary.isValid(m_navquery, &m_filters[ag->params.filter]))
{
// UE4: force removing segments too close to offmesh links
const bool useForcedRemove = m_linkRemovalRadius > 0.0f && ag->ncorners &&
(ag->cornerFlags[ag->ncorners - 1] & DT_STRAIGHTPATH_OFFMESH_CONNECTION);
const float* removePos = useForcedRemove ? &ag->cornerVerts[(ag->ncorners - 1) * 3] : 0;
const float removeRadius = m_linkRemovalRadius;
// UE4: prepare filter containing only current area
// boundary update will take all corner polys and include them in local neighborhood
unsigned char allowedArea = DT_WALKABLE_AREA;
if (m_raycastSingleArea)
{
m_navquery->getAttachedNavMesh()->getPolyArea(ag->corridor.getFirstPoly(), &allowedArea);
m_raycastFilter.setAreaCost(allowedArea, 1.0f);
}
// UE4: move dir for segment scoring
float moveDir[3] = { 0.0f };
if (ag->ncorners)
{
dtVsub(moveDir, &ag->cornerVerts[2], &ag->cornerVerts[0]);
}
else
{
dtVcopy(moveDir, ag->vel);
}
dtVnormalize(moveDir);
ag->boundary.update(ag->corridor.getFirstPoly(), ag->npos, ag->params.collisionQueryRange,
removePos, removeRadius, useForcedRemove,
ag->corridor.getPath(), ag->corridor.getPathCount(),
moveDir,
m_navquery, m_raycastSingleArea ? &m_raycastFilter : &m_filters[ag->params.filter]);
m_raycastFilter.setAreaCost(allowedArea, DT_UNWALKABLE_POLY_COST);
}
// Query neighbour agents
ag->nneis = getNeighbours(ag->npos, ag->params.height, ag->params.collisionQueryRange,
ag, ag->neis, DT_CROWDAGENT_MAX_NEIGHBOURS,
m_activeAgents, m_numActiveAgents, m_grid);
for (int j = 0; j < ag->nneis; j++)
ag->neis[j].idx = getAgentIndex(m_activeAgents[ag->neis[j].idx]);
}
}
/*
This method does the same thing as the pseudo-code's updateVertex(),
except for grids instead of graphs.
Pathfinding algorimths tend to be demonstraited with a graph rather than a grid,
in order to update the cost between two tiles we must update both the tile and its neighbour.
*/
void GridWorld::updateCost(unsigned int x, unsigned int y, double newCost){
static int count = 1;
count++;
Tile* tile = getTileAt(x, y);
printf("Updating <%d, %d> from %2.0lf to %2.0lf - Update: %d\n", x, y, tile->cost, newCost, count);
km += calculateH(previous);
previous = goal;
//I am aware that the following code below could be refactored by 50%
//since it's repeating itself with only a few changes
double oldCost = tile->cost;
double oldCostToTile, newCostToTile;
//Update CURRENT by finding its new minimum RHS-value from NEIGHBOURS
std::vector<Tile*> neighbours(getNeighbours(tile));
for (int i = 0; i < neighbours.size(); i++){
tile->cost = oldCost;
oldCostToTile = calculateC(tile, neighbours[i]);
tile->cost = newCost;
newCostToTile = calculateC(tile, neighbours[i]);
if (oldCostToTile > newCostToTile){
if (tile != start && tile->rhs > neighbours[i]->g + newCostToTile){
tile->successor = neighbours[i];
tile->rhs = neighbours[i]->g + newCostToTile;
}
}
else if (tile != start && tile->successor == neighbours[i]){
TilePair minSucc(getMinSuccessor(tile));
tile->rhs = minSucc.second;
tile->successor = (tile->rhs == PF_INFINITY ? 0 : minSucc.first);
}
}
updateVertex(tile);
//Update all NEIGHBOURING cells by finding their new min RHS-values from CURRENT
for (int i = 0; i < neighbours.size(); i++){
tile->cost = oldCost;
oldCostToTile = calculateC(tile, neighbours[i]);
tile->cost = newCost;
newCostToTile = calculateC(tile, neighbours[i]);
if (oldCostToTile > newCostToTile){
if (neighbours[i] != start && neighbours[i]->rhs > tile->g + newCostToTile){
neighbours[i]->successor = tile;
neighbours[i]->rhs = tile->g + newCostToTile;
updateVertex(neighbours[i]);
}
}
else if (neighbours[i] != start && neighbours[i]->successor == tile){
TilePair minSucc(getMinSuccessor(neighbours[i]));
neighbours[i]->rhs = minSucc.second;
neighbours[i]->successor = (neighbours[i]->rhs == PF_INFINITY ? 0 : minSucc.first);
updateVertex(neighbours[i]);
}
}
computeShortestPath();
}
void AStar::update(Node current, Node goal){
removeMinimum(openSet,current);
addNodeToSet(closedSet,current);
#if DEBUG
printf("Adding node= %lld to Closed multiset\n", current.id);
#endif
vector<Node> nodeNbrs = getNeighbours(current);
for(int i=0;i<nodeNbrs.size();i++){
//nbr is a neighbouring node
Node nbr = nodeNbrs[i];
if(findInSet(closedSet,nbr)){
//TODO: remove the node from the closed and shift in open
#if DEBUG
cout<<"Node found in closed multiset "<< nbr.id<<endl;
#endif
continue;
}
int tentative_g_score = current.g_score + distance(current,nbr);
#if DEBUG
nbr.printData();
#endif
//TODO update this with the follwing logic.
// update if it is found in open multiset with higher g score
// or else it is not in open multiset.
pair<Node,bool> check = findInOpenSet(nbr);
if(check.second && tentative_g_score < check.first.g_score){
removeNodeFromSet(openSet,nbr);
came_from_map[check.first.id] = current.id;
#if DEBUG
printf("Setting parent of %lld to %lld\n", check.first.id, current.id);
#endif
//check.first.came_from = ¤t;
check.first.g_score = tentative_g_score;
check.first.f_score = tentative_g_score + H(check.first,goal);
addNodeToSet(openSet,check.first);
#if DEBUG
printf("Updating node in open multiset %lld with new parent = %lld\n",check.first.id,current.id);
#endif
}
else if(check.second==false){
came_from_map[check.first.id] = current.id;
#if DEBUG
printf("Setting parent of %lld to %lld\n", check.first.id, current.id);
#endif
//nbr.came_from = ¤t;
nbr.g_score = tentative_g_score;
nbr.f_score = tentative_g_score + H(nbr,goal);
addNodeToSet(openSet,nbr);
#if DEBUG
printf("Adding node= %lld to Open multiset with f_score %d \n", nbr.id,nbr.f_score);
#endif
}
}
#if DEBUG
cout<<"------------------------------------------------------------------------------------"<<endl;
#endif
return;
}
bool GridWorld::computeShortestPath(){
if (open.empty()){
std::cout << "ERROR: No tiles to expand on... can't do anything" << std::endl;
return false;
}
while (!open.empty() && (compareKeys(open.front()->key, goal->key = calculateKey(goal)) || goal->rhs != goal->g)){
Tile* current = open.front();
//Notice that CURRENT wasn't pop/removed yet
//std::cout << "Expanding:";
//current->info();
//std::cout << std::endl;
KeyPair k_new = calculateKey(current);
if (compareKeys(current->key, k_new)){
//Tiles under this branch will have to be updated as incremental search has happened
current->key = k_new;
make_heap(open.begin(), open.end(), GridWorld::compareTiles);
}
else if (current->g > current->rhs){
//Majority of the tiles will fall under this conditional branch as
//it undergoes normal A* pathfinding
current->g = current->rhs;
open.erase(open.begin());
make_heap(open.begin(), open.end(), GridWorld::compareTiles);
current->isOpen = false;
std::vector<Tile*> neighbours(getNeighbours(current));
for (int i = 0; i < neighbours.size(); i++){
if (neighbours[i] != start && neighbours[i]->rhs > current->g + calculateC(current, neighbours[i])){
neighbours[i]->successor = current;
neighbours[i]->rhs = current->g + calculateC(current, neighbours[i]);
updateVertex(neighbours[i]);
}
}
}
else {
//Tiles under this branch will need to be updated during incremental search
current->g = PF_INFINITY;
//Update CURRENT
if (current != start && current->successor == current){
TilePair minSucc(getMinSuccessor(current));
current->rhs = minSucc.second;
current->successor = current->rhs == PF_INFINITY ? 0 : minSucc.first;
}
updateVertex(current);
//Update NEIGHBOURS
std::vector<Tile*> neighbours(getNeighbours(current));
for (int i = 0; i < neighbours.size(); i++){
if (neighbours[i] != start && neighbours[i]->successor == current){
TilePair minSucc(getMinSuccessor(neighbours[i]));
neighbours[i]->rhs = minSucc.second;
neighbours[i]->successor = neighbours[i]->rhs == PF_INFINITY ? 0 : minSucc.first;
}
updateVertex(neighbours[i]);
}
}
//Uncomment this to see CURRENT'S new values
//std::cout << "Expanded:";
//current->info();
//std::cout << std::endl;
}
return true;
}
bool GridWorld::computeShortestPath(){
if (open.empty()){
std::cout << "ERROR: No tiles to expand on... can't do anything" << std::endl;
return false;
}
double currentRHS, otherG, previousG;
while (!open.empty() && (compareKeys(open.front()->key, start->key = calculateKey(start)) || start->rhs != start->g)){
Tile* current = open.front();
//Notice that CURRENT wasn't pop/removed yet..
KeyPair k_old = current->key;
KeyPair k_new = calculateKey(current);
currentRHS = current->rhs;
otherG = current->g;
/*std::cout << "Expanding:";
current->info();
std::cout << std::endl;*/
if (compareKeys(k_old, k_new)){
//This branch updates tile that were already in the OPEN list originally
//This branch tends to execute AFTER the else branch
current->key = k_new;
make_heap(open.begin(), open.end(), GridWorld::compareTiles);
}
else if (otherG > currentRHS){
//Majority of the execution will fall under this conditional branch as
//it is undergoing normal A* pathfinding
current->g = current->rhs;
otherG = currentRHS;
open.erase(open.begin());
make_heap(open.begin(), open.end(), GridWorld::compareTiles);
current->isOpen = false;
std::vector<Tile*> neighbours(getNeighbours(current));
for (int i = 0; i < neighbours.size(); i++){
if (neighbours[i] != 0){
if (neighbours[i] != goal){
neighbours[i]->rhs = std::min(neighbours[i]->rhs, calculateC(current, neighbours[i]) + otherG);
}
updateVertex(neighbours[i]);
}
}
}
else {
//Execution of this branch updates the tile during incremental search
previousG = otherG;
current->g = PF_INFINITY;
//Update CURRENT'S RHS
if (current != goal){
current->rhs = getMinSuccessor(current).second;
}
updateVertex(current);
//Update NEIGHBOUR'S RHS to their minimum successor
std::vector<Tile*> neighbours(getNeighbours(current));
for (int i = 0; i < neighbours.size(); i++){
if (neighbours[i] != 0){
if (neighbours[i]->rhs == (calculateC(current, neighbours[i]) + previousG) && neighbours[i] != goal){
neighbours[i]->rhs = getMinSuccessor(neighbours[i]).second;
}
updateVertex(neighbours[i]);
}
}
}
//Uncomment this to see CURRENT'S new values
//std::cout << "Expanded:";
//current->info();
//std::cout << std::endl;
}
return true;
}
bool PathPlanner::aStar(Eigen::Vector2i start, Eigen::Vector2i end, std::vector<
Eigen::Vector2i> & path)
{
std::map<std::pair<int, int>, std::pair<int, int> > cameFrom;
bool ** closedSet = new bool *[image.cols];
for(int i = 0; i < image.cols; i++)
{
closedSet[i] = new bool[image.rows];
for(int j = 0; j < image.rows; j++)
{
closedSet[i][j] = false;
}
}
bool ** openSet = new bool *[image.cols];
for(int i = 0; i < image.cols; i++)
{
openSet[i] = new bool[image.rows];
for(int j = 0; j < image.rows; j++)
{
openSet[i][j] = false;
}
}
float ** gScore = new float *[image.cols];
for(int i = 0; i < image.cols; i++)
{
gScore[i] = new float[image.rows];
for(int j = 0; j < image.rows; j++)
{
gScore[i][j] = 0;
}
}
float ** fScore = new float *[image.cols];
for(int i = 0; i < image.cols; i++)
{
fScore[i] = new float[image.rows];
for(int j = 0; j < image.rows; j++)
{
fScore[i][j] = 0;
}
}
openSet[start(0)][start(1)] = true;
gScore[start(0)][start(1)] = 0;
fScore[start(0)][start(1)] = gScore[start(0)][start(1)]
+ heuristic(start, end);
std::priority_queue<std::pair<float, Eigen::Vector2i>,
std::vector<std::pair<int, Eigen::Vector2i> >,
PathPlanner::Comparator> queue;
queue.push(std::pair<float, Eigen::Vector2i>(0, start));
while(!queue.empty())
{
Eigen::Vector2i current = queue.top().second;
queue.pop();
int x = current(0);
int y = current(1);
if(current == end)
{
path =
reconstructPath(cameFrom, std::pair<int, int>(end(0), end(1)));
for(int i = 0; i < image.cols; i++)
{
delete[] closedSet[i];
}
delete[] closedSet;
for(int i = 0; i < image.cols; i++)
{
delete[] openSet[i];
}
delete[] openSet;
for(int i = 0; i < image.cols; i++)
{
delete[] gScore[i];
}
delete[] gScore;
for(int i = 0; i < image.cols; i++)
{
delete[] fScore[i];
}
delete[] fScore;
return true;
}
openSet[x][y] = false;
closedSet[x][y] = true;
std::vector<Eigen::Vector2i> neighbours = getNeighbours(current);
for(size_t i = 0; i < neighbours.size(); i++)
{
int nx = neighbours.at(i)(0);
int ny = neighbours.at(i)(1);
if(closedSet[nx][ny])
continue;
float gTentative = gScore[x][y]
+ heuristic(current, neighbours.at(i));
if(!openSet[nx][ny] || gTentative < gScore[nx][ny])
{
cameFrom[std::pair<int, int>(neighbours.at(i)(0), neighbours.at(i)(1))] =
std::pair<int, int>(current(0), current(1));
gScore[nx][ny] = gTentative;
fScore[nx][ny] = gScore[nx][ny]
+ heuristic(neighbours.at(i), end);
openSet[nx][ny] = true;
queue.push(std::pair<float, Eigen::Vector2i>(fScore[nx][ny], neighbours.at(i)));
}
}
}
for(int i = 0; i < image.cols; i++)
{
delete[] closedSet[i];
}
delete[] closedSet;
for(int i = 0; i < image.cols; i++)
{
delete[] openSet[i];
}
delete[] openSet;
for(int i = 0; i < image.cols; i++)
{
delete[] gScore[i];
}
delete[] gScore;
for(int i = 0; i < image.cols; i++)
{
delete[] fScore[i];
}
delete[] fScore;
return false;
}
