Using std::remove_if with lambda predicate to delete multiple elements - c++

What would be the fastest and most efficient way of using std::remove_if with lambda predicate to delete multiple elements at the same time? At the moment I have a point struct with position and unique id. Inside an update loop we fill the points vector and we add points to be deleted at the end of the update loop. At the moment I have to call remove_if inside a loop to remove all the deleted points from the points vector. For example if we add 10 points per frame and after that we loop all the points to check if the point is outside screen bounds, if it is its added to deletedPoints_.
struct Point
{
/// Position.
Vector3 position_;
/// Unique id per point
int id_;
}
/// Current max id
int maxId_;
/// All points
std::vector<Point> points_;
/// Deleted points
std::vector<Point> deletedPoints_;
//Updates with 60fps
void App::Update()
{
/// Add 10 points per frame
for (int i = 0; i < 10; ++i)
{
Point newPoint;
/// Add position
newPoint.position_ = worldPosition;
/// Add id starts from 1
maxId_ += 1;
startPoint.id_ = maxId_;
/// Add new point in points
points_.push(newPoint);
}
/// If points outside of screen bounds add them to deletedPoints_
if (points_.size() > 0)
{
for (int i = 0; i < points_.size(); ++i)
{
/// Bounds
Vector2 min = Vector2(0.00,0.00);
Vector2 max = Vector2(1.00,1.00);
/// Check Bounds
if(points_[i].x < min.x || points_[i].y < min.y || points_[i].x > max.x || points_[i].y > max.y)
{
deletedPoints_.push(points_[i]);
}
}
/// Loop deleted points
for (int i = 0; i < deletedPoints_.size(); ++i)
{
int id = deletedPoints_[i].id_;
/// Remove by id
auto removeIt = std::remove_if(points_.begin(), points_.end(),
[id](const TrailPoint2& point)
{ return point.id_ == id; });
points_.erase(removeIt, points_.end());
}
}
}

Without changing your structures, the quickest fix is to invert the whole loop and check deletedPoints from inside the lambda instead.
Then, make deletedPoints a std::set<int> storing your unique IDs. Then it'll be relatively fast, because std::set<int>::find doesn't need to scan the entire container, though your final complexity will still not be quite linear-time.
std::vector<Point> points_;
std::set<int> deletedPointIds_;
/// Remove by id
auto removeIt = std::remove_if(points_.begin(), points_.end(),
[&](const TrailPoint2& point)
{ return deletedPointIds_.count(point.id_); });
points_.erase(removeIt, points_.end());
deletedPointIds_.clear();
That being said, whether the switch over to std::set will be actually faster depends on a few things; you lose memory locality and drop cache opportunities due to the way in which set's elements are stored.
An alternative might be to keep the vector (of IDs not points!), pre-sort it, then use std::binary_search to get the benefits of a quick search as well as the benefits of sequentially-stored data. However, performing this search may not be suitable for your application, depending on how much data you have and on how often you need to execute this algorithm.
You could also use a std::unordered_set<int> instead of a std::set; this has the same problems as a std::set but the hash-based lookup may be faster than a tree-based lookup. Again, this entirely depends on the size, form and distribution of your data.
Ultimately, the only way to know for sure, is to try a few things at simulated extents and measure it.

Related

Trying to scan a vector of vehicles an extract information for lane switching C++

I am trying to build a simulation which contains certain objects. I have vehicles and lanes. I have an engine which allows vehicles to advance, based on their velocity and acceleration.
bool Lane::allowedOvertake(double pos, double mindist)
{
for (unsigned int iV = 0; iV < getNVehiclesinLane() - 1; iV++)
{
if ((fVehicles[iV]->getPosition() > pos - mindist) // If inside rear safety distance.
|| fVehicles[iV]->getPosition() < pos + mindist) // If inside front safety distance.
{}//continue
//else {return false;}
}
return true;
}
I would like this for loop to scan over all the vehicles in a lane, so that a vehicle from a neighbouring lane can check whether it can move into this scanned lane. As a note, the pos and mindist parameters are the positions and the minimum distance the lane seeking vehicle needs to safely switch lanes. Also, fVehicles is a vector of vehicles. If the result is true, I then use an if statement in my 'master' object, the road, which allows for an actual switch to take place (using vector.insert()).
I currently get vehicles switching lanes without regard. At first glance, I would suspect it is the above function's logic which is incorrect. Any help in providing a fix, or even a better solution, would be appreciated.
Note: I have a vector of vehicles, and a vector of lanes. However, the vehicles are not ordered in the vector by their positions. I have been advised to re-design this so that the order of the vehicles in the vector are more significant and one can benefit from this when developing the code. However, for now, I would like to fix the design I currently have. Then I will look into redesigning the simulation to make the order more significant. Besides this, my problem above would still exist, just in a slightly different form.
Given an unsorted vector, you have to check if all of them are distant enough from the passed position or, in other words, if none of them is too close:
#include <algorithm>
bool Lane::allowedOvertake(double pos, double mindist)
{
return std::none_of(
fVehicles.begin(), fVehicles.end(), [pos, mindist] (auto & v) {
return v->getPosition() <= pos + mindist
and v->getPosition() >= pos - mindist;
}
);
}

How to use multi-threading within a loop that iterates through a point cloud in C++?

I have made a function that estimates the normal vectors of a 3D Point Cloud and it takes a lot of time to run on a cloud of size 2 million. I want to multi-thread by calling the same function on two different points at the same time but it didn't work (it was creating hundreds of threads). Here is what I tried:
// kd-tree used for finding neighbours
pcl::KdTreeFLANN<pcl::PointXYZRGB> kdt;
// cloud iterators
pcl::PointCloud<pcl::PointXYZRGB>::iterator cloud_it = pt_cl->points.begin();
pcl::PointCloud<pcl::PointXYZRGB>::iterator cloud_it1;
pcl::PointCloud<pcl::PointXYZRGB>::iterator cloud_it2;
pcl::PointCloud<pcl::PointXYZRGB>::iterator cloud_it3;
pcl::PointCloud<pcl::PointXYZRGB>::iterator cloud_it4;
// initializing tree
kdt.setInputCloud(pt_cl);
// loop exit condition
bool it_completed = false;
while (!it_completed)
{
// initializing cloud iterators
cloud_it1 = cloud_it;
cloud_it2 = cloud_it++;
cloud_it3 = cloud_it++;
if (cloud_it3 != pt_cl->points.end())
{
// attaching threads
boost::thread thread_1 = boost::thread(geom::vectors::find_normal, pt_cl, cloud_it1, kdt, radius, max_neighbs);
boost::thread thread_2 = boost::thread(geom::vectors::find_normal, pt_cl, cloud_it2, kdt, radius, max_neighbs);
boost::thread thread_3 = boost::thread(geom::vectors::find_normal, pt_cl, cloud_it3, kdt, radius, max_neighbs);
// joining threads
thread_1.join();
thread_2.join();
thread_3.join();
cloud_it++;
}
else
{
it_completed = true;
}
}
As you can see I am trying to call the same function on 3 different points at the same time. Any suggestions for how to make this work? Sorry for the poor code, I'm tired and thank you in advance.
EDIT: here is the find_normal function
Here are the parameters:
#param pt_cl is a pointer to the point cloud to be treated (pcl::PointCloud<PointXYZRGB>::Ptr)
#param cloud_it is an iterator of this cloud (pcl::PointCloud<PointXYZRGB>::iterator)
#param kdt is the kd_tree used to find the closest neighbours of a point
#param radius defines the range in which to search for the neighbours of a point
#param max_neighbs is the maximum number of neighbours to be returned by the radius search
// auxilliary vectors for the k-tree nearest search
std::vector<int> pointIdxRadiusSearch; // neighbours ids
std::vector<float> pointRadiusSquaredDistance; // distances from the source to the neighbours
// the vectors of which the cross product calculates the normal
geom::vectors::vector3 *vect1;
geom::vectors::vector3 *vect2;
geom::vectors::vector3 *cross_prod;
geom::vectors::vector3 *abs_cross_prod;
geom::vectors::vector3 *normal;
geom::vectors::vector3 *normalized_normal;
// vectors to average
std::vector<geom::vectors::vector3> vct_toavg;
// if there are neighbours left
if (kdt.radiusSearch(*cloud_it, radius, pointIdxRadiusSearch, pointRadiusSquaredDistance, max_neighbs) > 0)
{
for (int pt_index = 0; pt_index < (pointIdxRadiusSearch.size() - 1); pt_index++)
{
// defining the first vector
vect1 = geom::vectors::create_vect2p((*cloud_it), pt_cl->points[pointIdxRadiusSearch[pt_index + 1]]);
// defining the second vector; making sure there is no 'out of bounds' error
if (pt_index == pointIdxRadiusSearch.size() - 2)
vect2 = geom::vectors::create_vect2p((*cloud_it), pt_cl->points[pointIdxRadiusSearch[1]]);
else
vect2 = geom::vectors::create_vect2p((*cloud_it), pt_cl->points[pointIdxRadiusSearch[pt_index + 2]]);
// adding the cross product of the two previous vectors to our list
cross_prod = geom::vectors::cross_product(*vect1, *vect2);
abs_cross_prod = geom::aux::abs_vector(*cross_prod);
vct_toavg.push_back(*abs_cross_prod);
// freeing memory
delete vect1;
delete vect2;
delete cross_prod;
delete abs_cross_prod;
}
// calculating the normal
normal = geom::vectors::vect_avg(vct_toavg);
// calculating the normalized normal
normalized_normal = geom::vectors::normalize_normal(*normal);
// coloring the point
geom::aux::norm_toPtRGB(&(*cloud_it), *normalized_normal);
// freeing memory
delete normal;
delete normalized_normal;
// clearing vectors
vct_toavg.clear();
pointIdxRadiusSearch.clear();
pointRadiusSquaredDistance.clear();
// shrinking vectors
vct_toavg.shrink_to_fit();
pointIdxRadiusSearch.shrink_to_fit();
pointRadiusSquaredDistance.shrink_to_fit();
}
Since I don't quite get it how the result data is being stored, I'm going to suggest a solution based on OpenMP that matches the code you've posted.
// kd-tree used for finding neighbours
pcl::KdTreeFLANN<pcl::PointXYZRGB> kdt;
#pragma openmp parallel for schedule(static)
for (pcl::PointCloud<pcl::PointXYZRGB>::iterator cloud_it = pt_cl->points.begin();
cloud_it < pt_cl.end();
++cloud_it) {
geom::vectors::find_normal, pt_cl, cloud_it, kdt, radius, max_neighbs);
}
Note that you should be using the < comparison, and not the != one, -that's how OpenMP works (it wants random access iterators). I'm using the static schedule since every element should take more or less identical time to process. If that's not the case, try using schedule(dynamic) instead.
This solution uses OpenMP, and you may investigate e.g. TBB as well, though it has a higher entrance barrier than OpenMP and uses an OOP-style API.
Also, repeating what I've said in the comments already: OpenMP as well as TBB are going to handle thread management and load distribution for you. You only pass them hints (such as schedule(static)) on how to do it to so as to better suit your needs.
Other than that, please, do get in the habit of repeating as little code as you can; ideally, no code should be duplicated. E.g. when you declare many variables of the same type, or call a certain function a few times in a row with a similar pattern, etc. I also see excessive commenting in the code, with an unclear reason behind it.

Compute the "lower contour" of a set of segments in the plane in `O(n log n)`

Suppose you've a set s of horizontal line segments in the plane described by a starting point p, an end point q and a y-value.
We can assume that all values of p and qare pairwise distinct and no two segments overlap.
I want to compute the "lower contour" of the segment.
We can sort s by p and iterate through each segment j. If i is the "active" segment and j->y < i->y we "switch to" j (and output the corresponding contour element).
However, what can we do, when no such j exists and we find a j with i->q < j->p. Then, we would need to switch to the "next higher segment". But how do we know that segment? I can't find a way such that the resulting algorithm would have a running time of O(n log n). Any ideas?
A sweep line algorithm is an efficient way to solve your problem. As explained previously by Brian, we can sort all the endpoints by the x-coordinate and process them in order. An important distinction to make here is that we are sorting the endpoints of the segment and not the segments in order of increasing starting point.
If you imagine a vertical line sweeping from left to right across your segments, you will notice two things:
At any position, the vertical line either intersects a set of segments or nothing. Let's call this set the active set. The lower contour is the segment within the active set with the smallest y-coordinate.
The only x-coordinates where the lower contour can change are the segment endpoints.
This immediately brings one observation: the lower contour should be a list of segments. A list of points does not provide sufficient information to define the contour, which can be undefined at certain x-coordinates (where there are no segments).
We can model the active set with an std::set ordered by the y position of the segment. Processing the endpoints in order of increasing x-coordinate. When encountering a left endpoint, insert the segment. When encountering a right endpoint, erase the segment. We can find the active segment with the lowest y-coordinate with set::begin() in constant time thanks to the ordering. Since each segment is only ever inserted once and erased once, maintaining the active set takes O(n log n) time in total.
In fact, it is possible to maintain a std::multiset of only the y-coordinates for each segment that intersects the sweep line, if it is easier.
The assumption that the segments are non-overlapping and have distinct endpoints is not entirely necessary. Overlapping segments are handled both by the ordered set of segments and the multiset of y-coordinates. Coinciding endpoints can be handled by considering all endpoints with the same x-coordinate at one go.
Here, I assume that there are no zero-length segments (i.e. points) to simplify things, although they can also be handled with some additional logic.
std::list<segment> lower_contour(std::list<segment> segments)
{
enum event_type { OPEN, CLOSE };
struct event {
event_type type;
const segment &s;
inline int position() const {
return type == OPEN ? s.sp : s.ep;
}
};
struct order_by_position {
bool operator()(const event& first, const event& second) {
return first.position() < second.position();
}
};
std::list<event> events;
for (auto s = segments.cbegin(); s != segments.cend(); ++s)
{
events.push_back( event { OPEN, *s } );
events.push_back( event { CLOSE, *s } );
}
events.sort(order_by_position());
// maintain a (multi)set of the y-positions for each segment that intersects the sweep line
// the ordering allows querying for the lowest segment in O(log N) time
// the multiset also allows overlapping segments to be handled correctly
std::multiset<int> active_segments;
bool contour_is_active = false;
int contour_y;
int contour_sp;
// the resulting lower contour
std::list<segment> contour;
for (auto i = events.cbegin(); i != events.cend();)
{
auto j = i;
int current_position = i->position();
while (j != events.cend() && j->position() == current_position)
{
switch (j->type)
{
case OPEN: active_segments.insert(j->s.y); break;
case CLOSE: active_segments.erase(j->s.y); break;
}
++j;
}
i = j;
if (contour_is_active)
{
if (active_segments.empty())
{
// the active segment ends here
contour_is_active = false;
contour.push_back( segment { contour_sp, current_position, contour_y } );
}
else
{
// if the current lowest position is different from the previous one,
// the old active segment ends here and a new active segment begins
int current_y = *active_segments.cbegin();
if (current_y != contour_y)
{
contour.push_back( segment { contour_sp, current_position, contour_y } );
contour_y = current_y;
contour_sp = current_position;
}
}
}
else
{
if (!active_segments.empty())
{
// a new contour segment begins here
int current_y = *active_segments.cbegin();
contour_is_active = true;
contour_y = current_y;
contour_sp = current_position;
}
}
}
return contour;
}
As Brian also mentioned, a binary heap like std::priority_queue can also be used to maintain the active set and tends to outperform std::set, even if it does not allow arbitrary elements to be deleted. You can work around this by flagging a segment as removed instead of erasing it. Then, repeatedly remove the top() of the priority_queue if it is a flagged segment. This might end up being faster, but it may or may not matter for your use case.
First sort all the endpoints by x-coordinate (both starting and ending points). Iterate through the endpoints and keep a std::set of all the y-coordinates of active segments. When you reach a starting point, add its y-coordinate to the set and "switch" to it if it's the lowest; when you reach an ending point, remove its y-coordinate from the set and recalculate the lowest y-coordinate using the set. This gives an O(n log n) solution overall.
A balanced binary search tree such as that used to implement std::set generally has a large constant factor. You can speed up this approach by using a binary heap (std::priority_queue) instead of a set, with the lowest y-coordinate at the root. In this case, you can't remove a non-root node, but when you reach such an ending point, just mark the segment inactive in an array. When the root node is popped, continue popping until there is a new root node that hasn't been marked inactive already. I think this will be about twice as fast as the set-based approach, but you'll have to code it yourself and see, if that's a concern.

Optimizing the Dijkstra's algorithm

I need a graph-search algorithm that is enough in our application of robot navigation and I chose Dijkstra's algorithm.
We are given the gridmap which contains free, occupied and unknown cells where the robot is only permitted to pass through the free cells. The user will input the starting position and the goal position. In return, I will retrieve the sequence of free cells leading the robot from starting position to the goal position which corresponds to the path.
Since executing the dijkstra's algorithm from start to goal would give us a reverse path coming from goal to start, I decided to execute the dijkstra's algorithm backwards such that I would retrieve the path from start to goal.
Starting from the goal cell, I would have 8 neighbors whose cost horizontally and vertically is 1 while diagonally would be sqrt(2) only if the cells are reachable (i.e. not out-of-bounds and free cell).
Here are the rules that should be observe in updating the neighboring cells, the current cell can only assume 8 neighboring cells to be reachable (e.g. distance of 1 or sqrt(2)) with the following conditions:
The neighboring cell is not out of bounds
The neighboring cell is unvisited.
The neighboring cell is a free cell which can be checked via the 2-D grid map.
Here is my implementation:
#include <opencv2/opencv.hpp>
#include <algorithm>
#include "Timer.h"
/// CONSTANTS
static const int UNKNOWN_CELL = 197;
static const int FREE_CELL = 255;
static const int OCCUPIED_CELL = 0;
/// STRUCTURES for easier management.
struct vertex {
cv::Point2i id_;
cv::Point2i from_;
vertex(cv::Point2i id, cv::Point2i from)
{
id_ = id;
from_ = from;
}
};
/// To be used for finding an element in std::multimap STL.
struct CompareID
{
CompareID(cv::Point2i val) : val_(val) {}
bool operator()(const std::pair<double, vertex> & elem) const {
return val_ == elem.second.id_;
}
private:
cv::Point2i val_;
};
/// Some helper functions for dijkstra's algorithm.
uint8_t get_cell_at(const cv::Mat & image, int x, int y)
{
assert(x < image.rows);
assert(y < image.cols);
return image.data[x * image.cols + y];
}
/// Some helper functions for dijkstra's algorithm.
bool checkIfNotOutOfBounds(cv::Point2i current, int rows, int cols)
{
return (current.x >= 0 && current.y >= 0 &&
current.x < cols && current.y < rows);
}
/// Brief: Finds the shortest possible path from starting position to the goal position
/// Param gridMap: The stage where the tracing of the shortest possible path will be performed.
/// Param start: The starting position in the gridMap. It is assumed that start cell is a free cell.
/// Param goal: The goal position in the gridMap. It is assumed that the goal cell is a free cell.
/// Param path: Returns the sequence of free cells leading to the goal starting from the starting cell.
bool findPathViaDijkstra(const cv::Mat& gridMap, cv::Point2i start, cv::Point2i goal, std::vector<cv::Point2i>& path)
{
// Clear the path just in case
path.clear();
// Create working and visited set.
std::multimap<double,vertex> working, visited;
// Initialize working set. We are going to perform the djikstra's
// backwards in order to get the actual path without reversing the path.
working.insert(std::make_pair(0, vertex(goal, goal)));
// Conditions in continuing
// 1.) Working is empty implies all nodes are visited.
// 2.) If the start is still not found in the working visited set.
// The Dijkstra's algorithm
while(!working.empty() && std::find_if(visited.begin(), visited.end(), CompareID(start)) == visited.end())
{
// Get the top of the STL.
// It is already given that the top of the multimap has the lowest cost.
std::pair<double, vertex> currentPair = *working.begin();
cv::Point2i current = currentPair.second.id_;
visited.insert(currentPair);
working.erase(working.begin());
// Check all arcs
// Only insert the cells into working under these 3 conditions:
// 1. The cell is not in visited cell
// 2. The cell is not out of bounds
// 3. The cell is free
for (int x = current.x-1; x <= current.x+1; x++)
for (int y = current.y-1; y <= current.y+1; y++)
{
if (checkIfNotOutOfBounds(cv::Point2i(x, y), gridMap.rows, gridMap.cols) &&
get_cell_at(gridMap, x, y) == FREE_CELL &&
std::find_if(visited.begin(), visited.end(), CompareID(cv::Point2i(x, y))) == visited.end())
{
vertex newVertex = vertex(cv::Point2i(x,y), current);
double cost = currentPair.first + sqrt(2);
// Cost is 1
if (x == current.x || y == current.y)
cost = currentPair.first + 1;
std::multimap<double, vertex>::iterator it =
std::find_if(working.begin(), working.end(), CompareID(cv::Point2i(x, y)));
if (it == working.end())
working.insert(std::make_pair(cost, newVertex));
else if(cost < (*it).first)
{
working.erase(it);
working.insert(std::make_pair(cost, newVertex));
}
}
}
}
// Now, recover the path.
// Path is valid!
if (std::find_if(visited.begin(), visited.end(), CompareID(start)) != visited.end())
{
std::pair <double, vertex> currentPair = *std::find_if(visited.begin(), visited.end(), CompareID(start));
path.push_back(currentPair.second.id_);
do
{
currentPair = *std::find_if(visited.begin(), visited.end(), CompareID(currentPair.second.from_));
path.push_back(currentPair.second.id_);
} while(currentPair.second.id_.x != goal.x || currentPair.second.id_.y != goal.y);
return true;
}
// Path is invalid!
else
return false;
}
int main()
{
// cv::Mat image = cv::imread("filteredmap1.jpg", CV_LOAD_IMAGE_GRAYSCALE);
cv::Mat image = cv::Mat(100,100,CV_8UC1);
std::vector<cv::Point2i> path;
for (int i = 0; i < image.rows; i++)
for(int j = 0; j < image.cols; j++)
{
image.data[i*image.cols+j] = FREE_CELL;
if (j == image.cols/2 && (i > 3 && i < image.rows - 3))
image.data[i*image.cols+j] = OCCUPIED_CELL;
// if (image.data[i*image.cols+j] > 215)
// image.data[i*image.cols+j] = FREE_CELL;
// else if(image.data[i*image.cols+j] < 100)
// image.data[i*image.cols+j] = OCCUPIED_CELL;
// else
// image.data[i*image.cols+j] = UNKNOWN_CELL;
}
// Start top right
cv::Point2i goal(image.cols-1, 0);
// Goal bottom left
cv::Point2i start(0, image.rows-1);
// Time the algorithm.
Timer timer;
timer.start();
findPathViaDijkstra(image, start, goal, path);
std::cerr << "Time elapsed: " << timer.getElapsedTimeInMilliSec() << " ms";
// Add the path in the image for visualization purpose.
cv::cvtColor(image, image, CV_GRAY2BGRA);
int cn = image.channels();
for (int i = 0; i < path.size(); i++)
{
image.data[path[i].x*cn*image.cols+path[i].y*cn+0] = 0;
image.data[path[i].x*cn*image.cols+path[i].y*cn+1] = 255;
image.data[path[i].x*cn*image.cols+path[i].y*cn+2] = 0;
}
cv::imshow("Map with path", image);
cv::waitKey();
return 0;
}
For the algorithm implementation, I decided to have two sets namely the visited and working set whose each elements contain:
The location of itself in the 2D grid map.
The accumulated cost
Through what cell did it get its accumulated cost (for path recovery)
And here is the result:
The black pixels represent obstacles, the white pixels represent free space and the green line represents the path computed.
On this implementation, I would only search within the current working set for the minimum value and DO NOT need to scan throughout the cost matrix (where initially, the initially cost of all cells are set to infinity and the starting point 0). Maintaining a separate vector of the working set I think promises a better code performance because all the cells that have cost of infinity is surely to be not included in the working set but only those cells that have been touched.
I also took advantage of the STL which C++ provides. I decided to use the std::multimap since it can store duplicating keys (which is the cost) and it sorts the lists automatically. However, I was forced to use std::find_if() to find the id (which is the row,col of the current cell in the set) in the visited set to check if the current cell is on it which promises linear complexity. I really think this is the bottleneck of the Dijkstra's algorithm.
I am well aware that A* algorithm is much faster than Dijkstra's algorithm but what I wanted to ask is my implementation of Dijkstra's algorithm optimal? Even if I implemented A* algorithm using my current implementation in Dijkstra's which is I believe suboptimal, then consequently A* algorithm will also be suboptimal.
What improvement can I perform? What STL is the most appropriate for this algorithm? Particularly, how do I improve the bottleneck?
You're using a std::multimap for 'working' and 'visited'. That's not great.
The first thing you should do is change visited into a per-vertex flag so you can do your find_if in constant time instead of linear times and also so that operations on the list of visited vertices take constant instead of logarithmic time. You know what all the vertices are and you can map them to small integers trivially, so you can use either a std::vector or a std::bitset.
The second thing you should do is turn working into a priority queue, rather than a balanced binary tree structure, so that operations are a (largish) constant factor faster. std::priority_queue is a barebones binary heap. A higher-radix heap---say quaternary for concreteness---will probably be faster on modern computers due to its reduced depth. Andrew Goldberg suggests some bucket-based data structures; I can dig up references for you if you get to that stage. (They're not too complicated.)
Once you've taken care of these two things, you might look at A* or meet-in-the-middle tricks to speed things up even more.
Your performance is several orders of magnitude worse than it could be because you're using graph search algorithms for what looks like geometry. This geometry is much simpler and less general than the problems that graph search algorithms can solve. Also, with a vertex for every pixel your graph is huge even though it contains basically no information.
I heard you asking "how can I make this better without changing what I'm thinking" but nevertheless I'll tell you a completely different and better approach.
It looks like your robot can only go horizontally, vertically or diagonally. Is that for real or just a side effect of you choosing graph search algorithms? I'll assume the latter and let it go in any direction.
The algorithm goes like this:
(0) Represent your obstacles as polygons by listing the corners. Work in real numbers so you can make them as thin as you like.
(1) Try for a straight line between the end points.
(2) Check if that line goes through an obstacle or not. To do that for any line, show that all corners of any particular obstacle lie on the same side of the line. To do that, translate all points by (-X,-Y) of one end of the line so that that point is at the origin, then rotate until the other point is on the X axis. Now all corners should have the same sign of Y if there's no obstruction. There might be a quicker way just using gradients.
(3) If there's an obstruction, propose N two-segment paths going via the N corners of the obstacle.
(4) Recurse for all segments, culling any paths with segments that go out of bounds. That won't be a problem unless you have obstacles that go out of bounds.
(5) When it stops recursing, you should have a list of locally optimised paths from which you can choose the shortest.
(6) If you really want to restrict bearings to multiples of 45 degrees, then you can do this algorithm first and then replace each segment by any 45-only wiggly version that avoids obstacles. We know that such a version exists because you can stay extremely close to the original line by wiggling very often. We also know that all such wiggly paths have the same length.

Why Is My Spatial Hash So Slow?

Why is my spatial hash so slow? I am working on a code that uses smooth particle hydrodynamics to model the movement of landslides. In smooth particle hydrodynamics each particle influences the particles that are within a distance of 3 "smoothing lengths". I am trying to implement a spatial hash function in order to have a fast look up of the neighboring particles.
For my implementation I made use of the "set" datatype from the stl. At each time step the particles are hashed into their bucket using the function below. "bucket" is a vector of sets, with one set for each grid cell (the spatial domain is limited). Each particle is identified by an integer.
To look for collisions the function below entitled "getSurroundingParticles" is used which takes an integer (corresponding to a particle) and returns a set that contains all the grid cells that are within 3 support lengths of the particle.
The problem is that this implementation is really slow, slower even than just checking each particle against every other particles, when the number of particles is 2000. I was hoping that someone could spot a glaring problem in my implementation that I'm not seeing.
//put each particle into its bucket(s)
void hashParticles()
{
int grid_cell0;
cellsWithParticles.clear();
for(int i = 0; i<N; i++)
{
//determine the four grid cells that surround the particle, as well as the grid cell that the particle occupies
//using the hash function int grid_cell = ( floor(x/cell size) ) + ( floor(y/cell size) )*width
grid_cell0 = ( floor( (Xnew[i])/cell_spacing) ) + ( floor(Ynew[i]/cell_spacing) )*cell_width;
//keep track of cells with particles, cellsWithParticles is an unordered set so duplicates will automatically be deleted
cellsWithParticles.insert(grid_cell0);
//since each of the hash buckets is an unordered set any duplicates will be deleted
buckets[grid_cell0].insert(i);
}
}
set<int> getSurroundingParticles(int particleOfInterest)
{
set<int> surroundingParticles;
int numSurrounding;
float divisor = (support_length/cell_spacing);
numSurrounding = ceil( divisor );
int grid_cell;
for(int i = -numSurrounding; i <= numSurrounding; i++)
{
for(int j = -numSurrounding; j <= numSurrounding; j++)
{
grid_cell = (int)( floor( ((Xnew[particleOfInterest])+j*cell_spacing)/cell_spacing) ) + ( floor((Ynew[particleOfInterest]+i*cell_spacing)/cell_spacing) )*cell_width;
surroundingParticles.insert(buckets[grid_cell].begin(),buckets[grid_cell].end());
}
}
return surroundingParticles;
}
The code that looks calls getSurroundingParticles:
set<int> nearbyParticles;
//for each bucket with particles in it
for ( int i = 0; i < N; i++ )
{
nearbyParticles = getSurroundingParticles(i);
//for each particle in the bucket
for ( std::set<int>::iterator ipoint = nearbyParticles.begin(); ipoint != nearbyParticles.end(); ++ipoint )
{
//do stuff to check if the smaller subset of particles collide
}
}
Thanks a lot!
Your performance is getting eaten alive by the sheer amount of stl heap allocations caused by repeatedly creating and populating all those Sets. If you profiled the code (say with a quick and easy non-instrumenting tool like Sleepy), I'm certain you'd find that to be the case. You're using Sets to avoid having a given particle added to a bucket more than once - I get that. If Duck's suggestion doesn't give you what you need, I think you could dramatically improve performance by using preallocated arrays or vectors, and getting uniqueness in those containers by adding an "added" flag to the particle that gets set when the item is added. Then just check that flag before adding, and be sure to clear the flags before the next cycle. (If the number of particles is constant, you can do this extremely efficiently with a preallocated array dedicated to storing the flags, then memsetting to 0 at the end of the frame.)
That's the basic idea. If you decide to go this route and get stuck somewhere, I'll help you work out the details.