Related
I'm learning C++ and am doing something I'm comfortable with in java to start out. Particle simulation and flocking using a quadtree to cheaply find particles in a region. Everything is working but when I use the quadtree to get the particles from a region it's really slow (about 1s for 5000 calls).
I tried replacing the vector with an array and measuring the execution time of various parts of the function.
Am I making any rooky mistakes like unnecessarily copying objects etc.? I'm using 5000 particles, I can't imagine 1fps is the fastest it can go.
Full code for minimal reproducable example as per request:
main.cpp
#include <string>
#include <iostream>
#include <random>
#include <chrono>
#include <thread>
#include <cmath>
#include "Particle.h"
#include "Quadtree.h"
// Clock
using namespace std::chrono;
using namespace std::this_thread;
// Global constants
const int SCREEN_WIDTH = 640;
const int SCREEN_HEIGHT = 480;
const int desiredFPS = 30;
const int frameTimeMS = int(1000 / (double)desiredFPS);
const int numberOfParticles = 5000;
// Random number generation
std::random_device dev;
std::mt19937 rng(dev());
std::uniform_real_distribution<> dist(0, 1);
Particle particles[numberOfParticles];
Quadtree quadtree = Quadtree(0, 0, SCREEN_WIDTH, SCREEN_HEIGHT);
int main(int argc, char* args[])
{
for (int i = 0; i < numberOfParticles; i++)
{
particles[i] = Particle(dist(rng) * SCREEN_WIDTH, dist(rng) * SCREEN_HEIGHT);
}
// Clock for making all frames equally long and achieving the desired framerate when possible
auto lapStartTime = system_clock::now();
// Main loop
for (int i = 0; i < 1; i++)
{
// Insert the particles into the quadtree
quadtree = Quadtree(0, 0, SCREEN_WIDTH, SCREEN_HEIGHT);
for (int i = 0; i < numberOfParticles; i++)
{
quadtree.insert(&particles[i]);
}
double neighbourhoodRadius = 40;
for (int i = 0; i < numberOfParticles; i++)
{
// THIS IS THE PART THAT IS SLOW
std::vector<Particle*> neighbours = quadtree.getCircle(
particles[i].x,
particles[i].y,
neighbourhoodRadius
);
}
// Update clocks
auto nextFrameTime = lapStartTime + milliseconds(frameTimeMS);
sleep_until(nextFrameTime);
lapStartTime = nextFrameTime;
}
return 0;
}
Quadtree.h
#pragma once
#include <vector>
#include "Particle.h"
#include "Rect.h"
class Quadtree
{
public:
const static int capacity = 10; // Capacity of any section
Quadtree(double px, double py, double width, double height);
Quadtree(Rect r);
bool insert(Particle* p); // Add a particle to the tree
std::vector<Particle*> getCircle(double px, double py, double r);
int numberOfItems(); // Total amount in the quadtree
private:
std::vector<Particle*> particles; // Particles stored by this section
std::vector<Quadtree> sections; // Branches (only if split)
Rect area; // Region occupied by the quadtree
bool isSplit() { return sections.size() > 0; }
void split(); // Split the quadtree into 4 branches
};
Quadtree.cpp
#include <iostream>
#include "Quadtree.h"
Quadtree::Quadtree(double px, double py, double width, double height)
{
area = Rect(px, py, width, height);
sections = {};
particles = {};
}
Quadtree::Quadtree(Rect r)
{
area = r;
sections = {};
particles = {};
}
bool Quadtree::insert(Particle* p)
{
if (area.intersectPoint(p->x, p->y))
{
if (!isSplit() && particles.size() < capacity)
{
particles.push_back(p);
}
else
{
if (!isSplit()) // Capacity is reached and tree is not split yet
{
split();
}
// That this is a reference is very important!
// Otherwise a copy of the tree will be modified
for (Quadtree& s : sections)
{
if (s.insert(p))
{
return true;
}
}
}
return true;
}
else
{
return false;
}
}
std::vector<Particle*> Quadtree::getCircle(double px, double py, double r)
{
std::vector<Particle*> selection = {};
if (!isSplit())
{
// Add all particles from this section that lie within the circle
for (Particle* p : particles)
{
double a = px - p->x;
double b = py - p->y;
if (a * a + b * b <= r * r)
{
selection.push_back(p);
}
}
}
else
{
// The section is split so add all the particles from the
// branches together
for (Quadtree& s : sections)
{
// Check if the branch and the circle even have any intersection
if (s.area.intersectRect(Rect(px - r, py - r, 2 * r, 2 * r)))
{
// Get the particles from the branch and add them to selection
std::vector<Particle*> branchSelection = s.getCircle(px, py, r);
selection.insert(selection.end(), branchSelection.begin(), branchSelection.end());
}
}
}
return selection;
}
void Quadtree::split()
{
sections.push_back(Quadtree(area.getSection(2, 2, 0, 0)));
sections.push_back(Quadtree(area.getSection(2, 2, 0, 1)));
sections.push_back(Quadtree(area.getSection(2, 2, 1, 0)));
sections.push_back(Quadtree(area.getSection(2, 2, 1, 1)));
std::vector<Particle*> oldParticles{ particles };
particles.clear();
for (Particle* p : oldParticles)
{
bool success = insert(p);
}
}
int Quadtree::numberOfItems()
{
if (!isSplit())
{
return particles.size();
}
else
{
int result = 0;
for (Quadtree& q : sections)
{
result += q.numberOfItems();
}
return result;
}
}
Particle.h
#pragma once
class Particle {
public:
double x;
double y;
Particle(double px, double py) : x(px), y(py) {}
Particle() = default;
};
Rect.h
#pragma once
class Rect
{
public:
double x;
double y;
double w;
double h;
Rect(double px, double py, double width, double height);
Rect() : x(0), y(0), w(0), h(0) {}
bool intersectPoint(double px, double py);
bool intersectRect(Rect r);
Rect getSection(int rows, int cols, int ix, int iy);
};
Rect.cpp
#include "Rect.h"
Rect::Rect(double px, double py, double width, double height)
{
x = px;
y = py;
w = width;
h = height;
}
bool Rect::intersectPoint(double px, double py)
{
return px >= x && px < x + w && py >= y && py < y + h;
}
bool Rect::intersectRect(Rect r)
{
return x + w >= r.x && y + h >= r.y && x <= r.x + r.w && y <= r.y + r.w;
}
Rect Rect::getSection(int cols, int rows, int ix, int iy)
{
return Rect(x + ix * w / cols, y + iy * h / rows, w / cols, h / rows);
}
So... In the original code creating the quadtree takes about 0.001s (relatively insignificant), and the neighbor search takes about 0.06s - here is our culprit (as mentioned by the OP).
Passing the std::vector<Particle*> neighbours as a reference to the getCircle function, gets rid of the insert call at the end of the function as well as new vector allocations (hi to everyone saying "oh, it will be optimized away automatically"). The time is reduced to 0.011s.
The nieghbours vector can be taken out of the main loop, and cleared after use, so that it only resizes on the first frame.
I do not see any more immediately obvious targets (without doing a complete rewrite). Maybe I will add something later.
I decided to approach this more systematically: I added an #if switch for every change I made and actually recorded some statistics, instead of eyeballing it. (Evey change is added incrementally, times include tree construction).
original
by reference
out of loop
min time:
0.0638s
0.0127s
0.0094s
avg time:
0.0664s
0.0136s
0.0104s
max time:
0.0713s
0.0157s
0.0137s
All measurements were done on my machine, with optimized build, using QueryPerfoemanceCounter.
I did end up rewriting the whole thing...
Got rid of vectors.
The Quadtree::particles is now Particle* particles[capacity] with a count.
sections is a pointer; isSplit just checks if sections is 0.
Since the total (or maximum) number of particles is known, the number of particles that can be returned by getCircle can't be more than that. So I allocate that much outside of the main loop to store neighbours. Adding another result involves just bumping a pointer (without even a check in release). And resetting it after use is done by setting the count to 0 (see arena or bump allocator).
The maximum number of quadtree nodes can be inferred from the number of particles. So, similarly, splitting just bumps the pointer by 4.
Trying to precompute the Rect in getCircle, or put px, py, r (and/or that rect as well) in a struct (passed as value or reference) does not yield any improvement (or is detremental). (was suggested by Goswin von Brederlow).
Then I flipped the recursion (was suggested by Ted Lyngmo). The temporary stack is, again, preallocated. And then I did the same thing for insert.
rewrite
non-recursive
insert as well
min_time:
0.0077
0.0069
0.0068
avg_time:
0.0089
0.0073
0.0070
max_time:
0.0084
0.0078
0.0074
So in the end the most impactful thing was the very first - not inserting and not creating unnecessary vectors every call, but instead passing the same one by reference.
One last thing - might want to store the quadtree particles separately, since most of the time getCircle is traversing nodes, where particles are not stored.
Otherwise, I do not see how to improve this any more. At this point it would require someone actually smart or crazy...
I am attempting an online coding challenge wherein I am to implement a pathfinding algorithm that finds the shortest path between two points on a 2D grid. The code that is submitted is tested against a number of test cases that I, unfortunately, am unable to see, but it will however tell me if my answer for shortest distance is correct or not. My implementation of the A* algorithm returns a correct answer on 2/3 test cases and I cannot seem to figure out what scenario might create an incorrect answer on the third?
I have tried several of my own test cases and have gotten correct answers for all of those and at this point am feeling a little bit lost. There must be something small in my code that I am not seeing that is causing this third case to fail.
More details
The grid is w by h and contains only 1's (passable) and 0's (impassable) with every edge having a cost of 1 and the pathway cannot move diagonally
It all starts with the FindPath function which is to return the length of the shortest path, or -1 if no path is available
pOutBuffer is used to contain the path taken from beginning to end (excluding the starting point). If multiple paths are available then any will be accepted. So it isnt looking for one path in particular
I know the issue is not the result of time or memory inefficiency. I has to be either the distance returned is incorrect, or the values in pOutBuffer are incorrect.
Any help would be greatly appreciated as I am just about out of ideas as to what could possibly be wrong here. Thank you.
#include <set>
#include <vector>
#include <tuple>
#include <queue>
#include <unordered_map>
inline int PositionToIndex(const int x, const int y, const int w, const int h)
{
return x >= 0 && y >= 0 && x < w && y < h? x + y * w : -1;
}
inline std::pair<int, int> IndexToPosition(const int i, const int w)
{
return std::make_pair<int, int>(i % w, i / w);
}
inline int Heuristic(const int xa, const int ya, const int xb, const int yb)
{
return std::abs(xa - xb) + std::abs(ya - yb);
}
class Map
{
public:
const unsigned char* mapData;
int width, height;
const std::vector<std::pair<int, int>> directions = { {1,0}, {0,1}, {-1,0}, {0,-1} };
Map(const unsigned char* pMap, const int nMapWidth, const int nMapHeight)
{
mapData = pMap;
width = nMapWidth;
height = nMapHeight;
}
inline bool IsWithinBounds(const int x, const int y)
{
return x >= 0 && y >= 0 && x < width && y < height;
}
inline bool IsPassable(const int i)
{
return mapData[i] == char(1);
}
std::vector<int> GetNeighbours(const int i)
{
std::vector<int> ret;
int x, y, neighbourIndex;
std::tie(x, y) = IndexToPosition(i, width);
for (auto pair : directions)
{
neighbourIndex = PositionToIndex(x + pair.first, y + pair.second, width, height);
if (neighbourIndex >= 0 && IsWithinBounds(x + pair.first, y + pair.second) && IsPassable(neighbourIndex))
ret.push_back(neighbourIndex);
}
return ret;
}
};
int FindPath(const int nStartX, const int nStartY,
const int nTargetX, const int nTargetY,
const unsigned char* pMap, const int nMapWidth, const int nMapHeight,
int* pOutBuffer, const int nOutBufferSize)
{
int ret = -1;
// create the map
Map map(pMap, nMapWidth, nMapHeight);
// get start and end indecies
int targetIndex = PositionToIndex(nTargetX, nTargetY, nMapWidth, nMapHeight);
int startIndex = PositionToIndex(nStartX, nStartY, nMapWidth, nMapHeight);
// if start and end are same exit
if (targetIndex == startIndex) return 0;
std::unordered_map<int, int> pathway = { {startIndex, startIndex} };
std::unordered_map<int, int> distances = { {startIndex, 0} };
// queue for indecies to process
typedef std::pair<int, int> WeightedLocation;
std::priority_queue<WeightedLocation, std::vector<WeightedLocation>, std::greater<WeightedLocation>> queue;
queue.emplace(0, startIndex);
while (!queue.empty())
{
int currentWeight, currentIndex;
std::tie(currentWeight, currentIndex) = queue.top();
queue.pop();
if (currentIndex == targetIndex)
break;
int newDistance = distances[currentIndex] + 1;
for (int n : map.GetNeighbours(currentIndex))
{
if (distances.find(n) == distances.end() || newDistance < distances[n])
{
distances[n] = newDistance;
int weight = newDistance + Heuristic(n % nMapWidth, n / nMapWidth, nTargetX, nTargetY);
queue.emplace(weight, n);
pathway[n] = currentIndex;
}
}
}
if (pathway.find(targetIndex) != pathway.end())
{
int current = targetIndex;
while (current != startIndex)
{
int outIndex = distances[current] - 1;
pOutBuffer[distances[current] - 1] = current;
current = pathway[current];
}
ret = distances[targetIndex];
}
return ret;
}
Seems there is no way to compute line line intersection using boost::geometry, but I wonder what is the most common way to do it in C++?
I need intersection algorithms for two infinite lines in 2D, if it will be faster it can be two different functions like:
bool line_intersection(line,line);
point line_intersetion(line,line);
P.S. I really try to avoid a wheel invention, so incline to use some library.
The best algorithms that I've found for finding the intersection of lines are in: Real Time Collision Detection by Christer Ericson, a copy of the book can be found here.
Chapter 5 from page 146 onwards describes how to find the closest point of 3D lines which is also the crossing point of 2D lines... with example code in C.
Note: beware of parallel lines, they can cause divide by zero errors.
Express one of the lines in parametric form and the other in implicit form:
X = X0 + t (X1 - X0), Y= Y0 + t (Y1 - Y0)
S(X, Y) = (X - X2) (Y3 - Y2) - (Y - Y2) (X3 - X2) = 0
By linearity of the relations, you have
S(X, Y) = S(X0, Y0) + t (S(X1, Y1) - S(X0, Y0)) = S0 + t (S1 - S0) = 0
From this you get t, and from t the coordinates of the intersection.
It takes a total of 15 adds, 6 multiplies and a single divide.
Degeneracy is indicated by S1 == S0, meaning that the lines are parallel. In practice, the coordinates may not be exact because of truncation errors or others, so that test for equality to 0 can fail. A workaround is to consider the test
|S0 - S1| <= µ |S0|
for small µ.
You can try my code, I'm using boost::geometry and I put a small test case in the main function.
I define a class Line with two points as attributes.
Cross product is very a simple way to know if two lines intersect. In 2D, you can compute the perp dot product (see perp function) that is a projection of cross product on the normal vector of 2D plane. To compute it, you need to get a direction vector of each line (see getVector method).
In 2D, you can get the intersection point of two lines using perp dot product and parametric equation of line. I found an explanation here.
The intersect function returns a boolean to check if two lines intersect. If they intersect, it computes the intersection point by reference.
#include <cmath>
#include <iostream>
#include <boost/geometry/geometry.hpp>
#include <boost/geometry/geometries/point_xy.hpp>
namespace bg = boost::geometry;
// Define two types Point and Vector for a better understanding
// (even if they derive from the same class)
typedef bg::model::d2::point_xy<double> Point;
typedef bg::model::d2::point_xy<double> Vector;
// Class to define a line with two points
class Line
{
public:
Line(const Point& point1,const Point& point2): p1(point1), p2(point2) {}
~Line() {}
// Extract a direction vector
Vector getVector() const
{
Vector v(p2);
bg::subtract_point(v,p1);
return v;
}
Point p1;
Point p2;
};
// Compute the perp dot product of vectors v1 and v2
double perp(const Vector& v1, const Vector& v2)
{
return bg::get<0>(v1)*bg::get<1>(v2)-bg::get<1>(v1)*bg::get<0>(v2);
}
// Check if lines l1 and l2 intersect
// Provide intersection point by reference if true
bool intersect(const Line& l1, const Line& l2, Point& inter)
{
Vector v1 = l1.getVector();
Vector v2 = l2.getVector();
if(std::abs(perp(v1,v2))>0.)
{
// Use parametric equation of lines to find intersection point
Line l(l1.p1,l2.p1);
Vector v = l.getVector();
double t = perp(v,v2)/perp(v1,v2);
inter = v1;
bg::multiply_value(inter,t);
bg::add_point(inter,l.p1);
return true;
}
else return false;
}
int main(int argc, char** argv)
{
Point p1(0.,0.);
Point p2(1.,0.);
Point p3(0.,1.);
Point p4(0.,2.);
Line l1(p1,p2);
Line l2(p3,p4);
Point inter;
if( intersect(l1,l2,inter) )
{
std::cout<<"Coordinates of intersection: "<<inter.x()<<" "<<inter.y()<<std::endl;
}
return 0;
}
EDIT: more detail on cross product and perp dot product + delete tol argument (off topic)
This code should work for you. You may be able to optimize it a bit:
template <class Tpoint>
Tpoint line<Tpoint>::intersect(const line& other) const{
Tpoint x = other.first - first;
Tpoint d1 = second - first;
Tpoint d2 = other.second - other.first;
auto cross = d1.x*d2.y - d1.y*d2.x;
auto t1 = (x.x * d2.y - x.y * d2.x) / static_cast<float>(cross);
return first + d1 * t1;
}
Perhaps a common way is to approximate the infinity? From my library using boost::geometry:
// prev and next are segments and RAY_LENGTH is a very large constant
// create 'lines'
auto prev_extended = extendSeg(prev, -RAY_LENGTH, RAY_LENGTH);
auto next_extended = extendSeg(next, -RAY_LENGTH, RAY_LENGTH);
// intersect!
Points_t isection_howmany;
bg::intersection(prev_extended, next_extended, isection_howmany);
then you could test whether the 'lines' intersect like this:
if (isection_howmany.empty())
cout << "parallel";
else if (isection_howmany.size() == 2)
cout << "collinear";
extendSeg() simply extends the segment in both directions by the given amounts.
Also bear in mind - to support an infinite line arithmetic the point type should also support an infinite value. However here the assumption is that you are looking for a numerical solution!
To solve this problem, I pieced together the following function, but unexpectedly found that it cannot calculate the intersection of line segments, but the intersection of lines.
class Solution {
typedef complex<double> point;
#define x real()
#define y imag()
struct LinePara
{
double k;
double b;
};
LinePara getLinePara(float x1, float y1, float x2, float y2)
{
LinePara ret;
double m = x2 - x1;
if (m == 0)
{
ret.k = 1000.0;
ret.b = y1 - ret.k * x1;
}
else
{
ret.k = (y2 - y1) / (x2 - x1);
ret.b = y1 - ret.k * x1;
}
return ret;
}
struct line {
double a, b, c;
};
const double EPS = 1e-6;
double det(double a, double b, double c, double d) {
return a * d - b * c;
}
line convertLineParaToLine(LinePara s)
{
return line{ s.k,-1,s.b };
}
bool intersect(line m, line n, point& res) {
double zn = det(m.a, m.b, n.a, n.b);
if (abs(zn) < EPS)
return false;
res.real(-det(m.c, m.b, n.c, n.b) / zn);
res.imag(-det(m.a, m.c, n.a, n.c) / zn);
return true;
}
bool parallel(line m, line n) {
return abs(det(m.a, m.b, n.a, n.b)) < EPS;
}
bool equivalent(line m, line n) {
return abs(det(m.a, m.b, n.a, n.b)) < EPS
&& abs(det(m.a, m.c, n.a, n.c)) < EPS
&& abs(det(m.b, m.c, n.b, n.c)) < EPS;
}
vector<double> mian(vector<vector<double>> line1, vector<vector<double>> line2)
{
vector<point> points;
points.push_back(point(line1[0][0], line1[0][1]));
points.push_back(point(line1[1][0], line1[1][1]));
points.push_back(point(line2[0][0], line2[0][1]));
points.push_back(point(line2[1][0], line2[1][1]));
line li1 = convertLineParaToLine(getLinePara(line1[0][0], line1[0][1], line1[1][0], line1[1][1]));
line li2 = convertLineParaToLine(getLinePara(line2[0][0], line2[0][1], line2[1][0], line2[1][1]));
point pos;
if (intersect(li1, li2, pos))
{
return{ pos.x ,pos.y };
}
else
{
if (equivalent(li1, li2)) {
if (points[1].x < points[2].x)
{
return vector<double>{ points[1].x, points[1].y };
}
else if (points[1].x > points[2].x)
{
return vector<double>{ points[2].x, points[2].y };
}
else if (points[1].x == points[2].x)
{
if (points[1].y < points[2].y)
{
return vector<double>{ points[1].x, points[1].y };
}
else if (points[1].y > points[2].y)
{
return vector<double>{ points[2].x, points[2].y };
}
}
else
{
return vector<double>{ points[2].x, points[2].y };
}
}
else
{
return {}/* << "平行！"*/;
}
return {};
}
}
public:
vector<double> intersection(vector<int>& start1, vector<int>& end1, vector<int>& start2, vector<int>& end2) {
vector<vector<double>> line1{ {(double)start1[0],(double)start1[1]},{(double)end1[0],(double)end1[1] } };
vector<vector<double>> line2{ {(double)start2[0],(double)start2[1]},{(double)end2[0],(double)end2[1] } };
return mian(line1, line2);
}
};
From there
Could anybody help me to understand what is wrong with my A* search implementation?
I implemented a basic A* search based on this incredible helpful site: http://www.redblobgames.com/pathfinding/a-star/implementation.html#cplusplus
(big thanks here to the author, Amit!).
I am using a grid, eight directions and the Octile distance as heuristic.
Unfortunately my path, going from start(0,h/2) to end(w-1,h/2), is not the expected straight line but looks like this:
My code (should be compilable as is but requires OpenCv):
struct PriorityQueue
{
typedef pair<int, cv::Point> PQElement;
struct SortPairPoints
{
bool operator()(const PQElement & l, const PQElement & r)
{
return (l.first > r.first);
}
};
priority_queue<PQElement, vector<PQElement>, SortPairPoints> elements;
inline bool empty() { return elements.empty(); }
inline void put(int priority,cv::Point item)
{
elements.emplace(priority, item);
}
inline cv::Point get()
{
cv::Point best_item = elements.top().second;
elements.pop();
return best_item;
}
};
template <class T>
inline void hash_combine(std::size_t& seed, const T& v)
{
std::hash<T> hasher;
seed ^= hasher(v) + 0x9e3779b9 + (seed<<6) + (seed>>2);
}
namespace std
{
template <>
struct hash<cv::Point>
{
size_t operator()(const cv::Point & p) const
{
size_t seed = 0;
hash_combine(seed,p.x);
hash_combine(seed,p.y);
return seed;
}
};
}
int heuristic(cv::Point next, cv::Point goal)
{
// int D = 1;
// int dx = abs(next.x - goal.x);
// int dy = abs(next.y - goal.y);
// return D * (dx + dy);
// return sqrt(dx * dx + dy * dy);
// int D = 1;
// int D2 = 1;
int D = 1;
int D2 = sqrt(2);
int dx = abs(next.x - goal.x);
int dy = abs(next.y - goal.y);
return D * (dx + dy) + (D2 - 2 * D) * min(dx, dy);
}
int w = 250;
int h = 250;
std::vector<cv::Point> pathDirs({cv::Point(1, 0),cv::Point(0, -1),cv::Point(0, 1),cv::Point(-1, 0), cv::Point(1, 1), cv::Point(-1, 1),cv::Point(-1, -1),cv::Point(1, -1)});
//std::vector<cv::Point> pathDirs({cv::Point(1, 0),cv::Point(0, -1),cv::Point(-1, 0),cv::Point(0, 1)});
cv::Rect scenebox(0,0,w,h);
void search(
cv::Mat map,
cv::Point start,
cv::Point goal,
unordered_map<cv::Point, cv::Point>& came_from,
unordered_map<cv::Point, int>& cost_so_far
)
{
PriorityQueue frontier;
frontier.put(0,start);
came_from[start] = start;
cost_so_far[start] = 0;
while (!frontier.empty()) {
auto current = frontier.get();
if (current == goal) {
break;
}
for (auto dir : pathDirs)
{
cv::Point next(current.x + dir.x, current.y + dir.y);
if (scenebox.contains(next) && (map.at<uchar>(next) == 255))
{
int new_cost = cost_so_far[current] + 1;
if (!cost_so_far.count(next) || new_cost < cost_so_far[next])
{
cost_so_far[next] = new_cost;
int priority = new_cost + heuristic(next, goal);
frontier.put(priority,next);
came_from[next] = current;
}
}
}
}
}
vector<cv::Point> reconstruct_path(
cv::Point start,
cv::Point goal,
unordered_map<cv::Point, cv::Point>& came_from
)
{
vector<cv::Point> path;
cv::Point current = goal;
path.push_back(current);
while (current != start) {
current = came_from[current];
path.push_back(current);
}
std::reverse(path.begin(), path.end());
return path;
}
int main(int argc, const char * argv[])
{
cv::Mat tracemap = cv::Mat(w,h, CV_8UC1, cvScalar(255) );
cv::Point start(0,h/2);
cv::Point end(w-1,h/2);
// cv::Point start(0,0);
// cv::Point end(w-1,h-1);
// cv::line(tracemap,
// cv::Point (75,125),
// cv::Point (125,75),
// cvScalar(150),50);
unordered_map<cv::Point, cv::Point> came_from;
unordered_map<cv::Point, int> cost_so_far;
search(tracemap, start, end, came_from, cost_so_far);
vector<cv::Point> path = reconstruct_path(start, end, came_from);
for(int i = 0; i < path.size(); i++)
{
tracemap.at<uchar>(path[i]) = 0;
}
imshow("tracemap", tracemap);
cv::waitKey();
return 0;
}
Any insights or tips on how to get to the root of the problem are highly appreciated!
UPDATE: With Amit's suggestions I get the following paths now:
FOLLOW-UP (highly related that is why I am adding it here and don't open a new post):
If I use only four directions with a Manhattan distance as heuristic and a movement cost of 1 for all four steps, I get a jittery diagonal. Of course the algorithm has to take 'stairs" like this, but I would still expect something more straight - am I missing something obvious?
Your movement cost for diagonals is the same as for orthogonal steps.
A path going southeast, southeast, northeast, northeast is just as short as a path going east, east, east, east. Both have cost 4.
When there are multiple shortest paths, A* is giving you one of them, but it's not the one you want.
If you set diagonals to have a higher movement cost (sqrt(2) is what your heuristic states), then A* would prefer east, east, east, east. Change
int new_cost = cost_so_far[current] + 1;
to either use 1 or sqrt(2) depending on whether it's an orthogonal or diagonal step. You'll also need to make the costs into floats/doubles instead of ints, and make the priority queue do the same. (Alternatively, if you want to keep using ints, some people will use 14 and 10 as the costs, and scale the heuristic up to use 14 and 10 for D2 and D.)
How would you solve the problem of finding the points of a (integer) grid within a circle centered on the origin of the axis, with the results ordered by norm, as in distance from the centre, in C++?
I wrote an implementation that works (yeah, I know, it is extremely inefficient, but for my problem anything more would be overkill). I'm extremely new to C++, so my biggest problem was finding a data structure capable of
being sort-able;
being able to save an array in one of its elements,
rather than the implementation of the algorithm. My code is as follows. Thanks in advance, everyone!
typedef std::pair<int, int[2]> norm_vec2d;
bool norm_vec2d_cmp (norm_vec2d a, norm_vec2d b)
{
bool bo;
bo = (a.first < b.first ? true: false);
return bo;
}
int energy_to_momenta_2D (int energy, std::list<norm_vec2d> *momenta)
{
int i, j, norm, n=0;
int energy_root = (int) std::sqrt(energy);
norm_vec2d temp;
for (i=-energy_root; i<=energy_root; i++)
{
for (j =-energy_root; j<=energy_root; j++)
{
norm = i*i + j*j;
if (norm <= energy)
{
temp.first = norm;
temp.second[0] = i;
temp.second[1] = j;
(*momenta).push_back (temp);
n++;
}
}
}
(*momenta).sort(norm_vec2d_cmp);
return n;
}
How would you solve the problem of finding the points of a (integer) grid within a circle centered on the origin of the axis, with the results ordered by norm, as in distance from the centre, in C++?
I wouldn't use a std::pair to hold the points. I'd create my own more descriptive type.
struct Point {
int x;
int y;
int square() const { return x*x + y*y; }
Point(int x = 0, int y = 0)
: x(x), y(y) {}
bool operator<(const Point& pt) const {
if( square() < pt.square() )
return true;
if( pt.square() < square() )
return false;
if( x < pt.x )
return true;
if( pt.x < x)
return false;
return y < pt.y;
}
friend std::ostream& operator<<(std::ostream& os, const Point& pt) {
return os << "(" << pt.x << "," << pt.y << ")";
}
};
This data structure is (probably) exactly the same size as two ints, it is less-than comparable, it is assignable, and it is easily printable.
The algorithm walks through all of the valid points that satisfy x=[0,radius] && y=[0,x] && (x,y) inside circle:
std::set<Point>
GetListOfPointsInsideCircle(double radius = 1) {
std::set<Point> result;
// Only examine bottom half of quadrant 1, then
// apply symmetry 8 ways
for(Point pt(0,0); pt.x <= radius; pt.x++, pt.y = 0) {
for(; pt.y <= pt.x && pt.square()<=radius*radius; pt.y++) {
result.insert(pt);
result.insert(Point(-pt.x, pt.y));
result.insert(Point(pt.x, -pt.y));
result.insert(Point(-pt.x, -pt.y));
result.insert(Point(pt.y, pt.x));
result.insert(Point(-pt.y, pt.x));
result.insert(Point(pt.y, -pt.x));
result.insert(Point(-pt.y, -pt.x));
}
}
return result;
}
I chose a std::set to hold the data for two reasons:
It is stored is sorted order, so I don't have to std::sort it, and
It rejects duplicates, so I don't have to worry about points whose reflection are identical
Finally, using this algorithm is dead simple:
int main () {
std::set<Point> vp = GetListOfPointsInsideCircle(2);
std::copy(vp.begin(), vp.end(),
std::ostream_iterator<Point>(std::cout, "\n"));
}
It's always worth it to add a point class for such geometric problem, since usually you have more than one to solve. But I don't think it's a good idea to overload the 'less' operator to satisfy the first need encountered. Because:
Specifying the comparator where you sort will make it clear what order you want there.
Specifying the comparator will allow to easily change it without affecting your generic point class.
Distance to origin is not a bad order, but for a grid but it's probably better to use row and columns (sort by x first then y).
Such comparator is slower and will thus slow any other set of points where you don't even care about norm.
Anyway, here is a simple solution using a specific comparator and trying to optimize a bit:
struct v2i{
int x,y;
v2i(int px, int py) : x(px), y(py) {}
int norm() const {return x*x+y*y;}
};
bool r_comp(const v2i& a, const v2i& b)
{ return a.norm() < b.norm(); }
std::vector<v2i> result;
for(int x = -r; x <= r; ++x) {
int my = r*r - x*x;
for(int y = 0; y*y <= my; ++y) {
result.push_back(v2i(x,y));
if(y > 0)
result.push_back(v2i(x,-y));
}
}
std::sort(result.begin(), result.end(), r_comp);