How do I optimize 2D visibility cone calculations?

Question

I'm creating a simple 2D game and got a little bit stuck with a visibility cone algorithm, influenced by walls and other obstacles on the map.

The only issue I'm currently having, is that it's very slow.

On a map with ~100 square obstacles, an UpdateVision call takes up to 60ms without compiler optimizations (up to 15ms with, so still very long), though depending on how many obstacles are in the cone. According to the profiler, most of the time is spent in find_intersection.

I know one of the main causes: the algorithm actually checks all the obstacles in the 120 degree cone, ignoring how far away they are. The small cone is actually a masked version of this:

So ignoring far points early should help a lot. Except I can't simply ignore them, because then stuff like this happens:

How can I improve on this?

---

Here is the commented relevant part of my code:

struct IntersectionData {
    float T1, T2;
};

/*
* For two lines, (p11, p12) and (p21, p22)
* Finds their intersection point.
* The point coordinate ca be calculated with: p11 + T1 * (p12 - p11) OR p21 + T2 * (p22 - p21)
*
* Formula from: http://ncase.me/sight-and-light/
*/
IntersectionData find_intersection(const sf::Vector2f &p11, const sf::Vector2f &p12, const sf::Vector2f &p21, const sf::Vector2f &p22)
{
    float r_px = p11.x, r_py = p11.y;
    float r_dx = p12.x-p11.x, r_dy = p12.y-p11.y;

    float s_px = p21.x, s_py = p21.y;
    float s_dx = p22.x-p21.x, s_dy = p22.y-p21.y;

    float T1, T2;

    T2 = (r_dx*(s_py-r_py) + r_dy*(r_px-s_px))/(s_dx*r_dy - s_dy*r_dx);

    if (r_dx != 0)
        T1 = (s_px+s_dx*T2-r_px)/r_dx;
    else
        T1 = (s_py+s_dy*T2-r_py)/r_dy;

    return IntersectionData{T1, T2};
}

/*
* Finds the closest intersection of a ray with origin (x, y) and angle a
* With a wall
* A wall contains a vector of points, making a convex shape.
*/
sf::Vector2f Enemy::CastRay(float a, const std::vector<std::shared_ptr<Wall>> &walls) {
    float best_T1 = 1000000;
    const float cos_a = cosf(a), sin_a = sinf(a);
    for(const auto &wall : walls){
        for(size_t i = 0; i < wall->points.size(); ++i){
            const auto &p1 = wall->points[i];
            const auto &p2 = wall->points[(i+1)%4];
            const auto intersection = find_intersection({x, y}, {x+cos_a, y+sin_a}, p1, p2);
            const float &T1 = intersection.T1, &T2 = intersection.T2;

            if ((T1 > 0) && (T2 > 0) && (T2 < 1))
                if(T1 < best_T1)
                    best_T1 = T1;
        }
    }
    return {x + best_T1 * cos_a, y + best_T1 * sin_a};
}

/*
* Algorithm from: http://ncase.me/sight-and-light/
*/
void Enemy::UpdateVision(const std::vector<std::shared_ptr<Wall>> &walls)
{
    // first, let's get all the vertices of all the walls

    std::vector<sf::Vector2f> points;
    for(const auto &wall : walls)
        for(const auto &p : wall->points)
            points.push_back(p);

    std::vector<sf::Vector2f> visPoints;

    for(const auto& point : points) {
        float angle = atan2(point.y-y, point.x-x);
        float normalized = NormalizeAngle(angle);

        // if the point is outside of the cone of vision, ignore it

        if(normalized > visibilityAngle || normalized < -visibilityAngle)
            continue;

        // analyze two slightly off angles, to see both a wall the point belongs to and possibly a wall just behind it

        float a1 = angle + 0.0001, a2 = angle - 0.0001;

        auto p1 = CastRay(a1, walls);
        auto p2 = CastRay(a2, walls);

        visPoints.push_back(p1);
        visPoints.push_back(p2);
    }

    // also add two rays just on the edges of cone of vision

    visPoints.push_back(CastRay(direction - visibilityAngle, walls));
    visPoints.push_back(CastRay(direction + visibilityAngle, walls));

    // sort the points based on their angle, so they can be correctly drawn with OpenGL TriangleFan mode

    std::sort(visPoints.begin(), visPoints.end(),
        [&](const sf::Vector2f &p1, const sf::Vector2f &p2) {
            return NormalizeAngle(atan2(p2.y-y, p2.x-x)) < NormalizeAngle(atan2(p1.y-y, p1.x-x));
        });

    float mid = texture.getSize().x / 2;

    // vision is a vector of sf::Vertex and is used for actual rendering
    // the third argument is the UV coordinate on the texture

    vision.clear();
    vision.push_back(sf::Vertex({x, y}, sf::Color::Green, {mid, mid}));

    for(auto &p : visPoints)
        vision.push_back(sf::Vertex(p, sf::Color::Green, {p.x - x + mid, p.y - y + mid}));
}

Answer 1

I have a couple of suggestions

Avoid Allocations and Copies

A lot of your code has unnecessary allocations and copies. Instead of filling a vector with push_back, try pre-allocating the vector and setting the data there. You have a fixed maximum number of intersection points, so pre-allocate a vector of that size.

Do a Simple Broadphase

Before you do anything at all, find out which shapes could possibly intersect the camera. You already do this here:

    if(normalized > visibilityAngle || normalized < -visibilityAngle)
        continue;

Avoid doing it again here:

for(const auto &wall : walls){
    for(size_t i = 0; i < wall->points.size(); ++i){

Instead, you should only check the walls which potentially intersect the camera when raycasting.

Example: Try Frustum Culling + Quad Tree

Convert your algorithm so that it uses a frustum rather than a cone with a radius. Then, do the following:

• Create a quadtree. All of the points in all of the rectangles should get added to this tree.
• Intersect the quadtree with the bounding box of the camera frustum. This gives you a list of potentially intersecting rectangles.
• Continue as normal, using only these intersecting rectangles.

You can then make your frustum longer or shorter to keep from having to check so many rectangles.

Example 2: Try Bresenham's Algorithm

You can make a loose, coarse grid over your whole scene. When a shape intersects a grid cell, add it to a list associated with that cell. Then, when you cast a ray, use Bresenham's Algorithm to search for the cells that the ray could intersect. Only check the shapes contained in these cells, rather than all of the shapes. If your grid is coarse enough, Bresenham's algorithm will run extremely quickly.

Answer 2

Whenever possible, use vectors, not angles. Arithmetic operations are more efficient than trigonometric ones, and vectors enable spatial indexing.

1. Use vector cross products for cone intersection (not angles)

Your current method wastes cycles on the costly atan, sin and cos functions for every point upfront, even points that get discarded.

Instead, consider this:

The sign of the cross product of 2 vectors determines which side (left or right) of the second the first is on. Example implementation on StackOverflow here. With this, you can filter points by whether they are both—

• to the right of the vision left border (in red), and
• to the left of the vision's right border (in green):

All points that satisfy the conditions (i.e. fall within the red-green section on the right) are within the vision cone.

This requires only arithmetic operations in the inner loop.

2. Precompute a spatial index

This is an illustration of @mklingen's observation that you can optimise spatial queries easily, by precomputing a spatial index and using it for broad-phase pruning.

For example, if you precompute a quadtree containing all of your environment's corner points, you can do an initial O(log n) quadtree walk to prune points down to the vision region's AABB:

For a large or detailed map, this gives a huge performance boost.