Improve velocity obstacle calculation algorithm/performance

Question

My goal

My goal is to calculate the (velocity) obstacle that is imposed by unit B onto unit A. So I want to calculate the velocities from the center of unit A (circle) that will lead to a collision with unit B assuming unit B is not moving. The illegal velocities would be all velocities that are between the two tangents from the center of unit A to unit B (circle).
Thus I actually only need the angle of the two tangents relative to some other velocity.

Here is a simple drawing to illustrate my approach:

The "other vector" (=green) is the "optimal velocity" for unit A, that is calculated through higher level pathfinding. My goal is then to choose the velocity that is closest to the optimal velocity, but not within the blocked/illegal velocities.

My approach

I already have a working demo, but it is too slow.

Note: In my implementation I extruded the radius of unit B by the radius of unit A.

My (primitive) approach to calculating the angles is:
1. Calculate the line from the center of unit A to Unit B
2. Calculate the first tangent point using trigonometry
3. Calculate the closest point from the first tangent point to the line from (1) (i call it the mirrorPoint)
4. Mirror the first tangent point along the mirrorPoint
5. Calculate angle1 and angle2 using Vector.SignedAngle

After wards i still have to do some work to find the closest legal vector to the optimal velocity, but that is not too time consuming.

Here is an image taken from the editor:

The purple line is (1)
The green lines are the lines from the center of Unit B to the tangent points.
The black line (in unit B) are the lines from the mirrorPoint to the tangent points
The blue and the red line are the lines from the center of unit A to the tangent points
The orange line is the optimal velocity
The black line (starting at the center of Unit A) is the legal velocity that is closest to optimal velocity

Here is the code that i use to calculate the angles:

public Vector2 GetDesiredSteering(VelocityObstacleSteeringInput input)
    {
        TestUnit unit = input.Unit;
        List<TestUnit> neighbors = GetNeighbors(unit);
        foreach (TestUnit neighbor in neighbors)
        {
            Vector2 tangent1;
            Vector2 tangent2;
            CalculateTangent(unit.Pos, unit.Scale.x, neighbor.Pos, neighbor.Scale.x, out tangent1, out tangent2);

            Vector2 relativeTangent1 = tangent1 - unit.Pos;
            Vector2 relativeTangent2 = tangent2 - unit.Pos;
            float angle1 = Vector2.SignedAngle(relativeTangent1, input.OptimalVelocity);
            float angle2 = Vector2.SignedAngle(relativeTangent2, input.OptimalVelocity);
            Debug.Log("angle1: " + angle1 + " angle2: " + angle2);
        }

        //return dummy for this stripped down version
        return Vector2.up;
    }

 bool CalculateTangent(Vector2 unitPos, float unitRadius, Vector2 otherPos, float otherRadius, out Vector2 tangentPoint1, out Vector2 tangentPoint2)
    {
        Vector2 hypo = unitPos - otherPos;
        float hypoLen = hypo.magnitude;
        float combinedRadius = (unitRadius + otherRadius) / 2.0f;
        float ratio = combinedRadius / hypoLen;
        float alpha = Mathf.Acos(ratio);
        hypo.Normalize();
        Vector2 rotated = GeometryHelper.RotateRad(hypo, -alpha);
        Vector2 centerToTangent = rotated * combinedRadius;
        tangentPoint1 = otherPos + centerToTangent;
        tangentPoint2 = GeometryHelper.CenterToOtherTangentPoint(otherPos, hypo, tangentPoint1, out Vector2 curMirrorPoint);
        return true;
    }


 public static Vector2 RotateRad(Vector2 aPoint, float rad)
    {
        float s = Mathf.Sin(rad);
        float c = Mathf.Cos(rad);
        return new Vector2(
            aPoint.x * c - aPoint.y * s,
            aPoint.y * c + aPoint.x * s);
    }

//linePointhas to be center of unit
 public static Vector2 CenterToOtherTangentPoint(Vector2 linePoint, Vector2 lineDirection, Vector2 tangentPoint1, out Vector2 returnMirrorPoint)
    {
        Vector2 mirrorPoint = FindNearestPointOnLine(linePoint, lineDirection, tangentPoint1);
        returnMirrorPoint = mirrorPoint;
        Vector2 tangentPointToMirrorPoint = mirrorPoint - tangentPoint1;
        return mirrorPoint + tangentPointToMirrorPoint;
    }

 public static Vector2 FindNearestPointOnLine(Vector2 origin, Vector2 direction, Vector2 point)
    {
        direction.Normalize();
        Vector2 lhs = point - origin;

        //t = how far to move from the origin along the direction to reach the closest point
        float t = Vector2.Dot(lhs, direction);
        return origin + direction * t;
    }

My Problem

I need to be able to scale this approach to at the very minmum 100 units (but I would very much prefer 200 units), which currently is not possible, because the algorithm/implementation is too slow.

So I am wondering if somebody can think of an improvement to this approach. Either mathematically(i.e. some better method/shortcut to calculate the angles) or other tips how to improve my code.

Answer 1

The first and best optimization you can make is to cut down on the number of times you need to do this calculation at all. You may already be doing this, but since we can't tell from your question, here's a quick intro to the idea:

If every moving object needs to compute steering angles vs every obstacle object, then your total cost is:

[# moving objects] * ([# obstacles] * [cost per angle range calc]
                      + [cost of velocity selection])

This grows quadratically as you add more stuff to your world (assuming the ratio of moving objects to obstacles remains similar)

But, if each moving object only needs to consider the obstacles "close" to it, and there's on average ~3 obstacles in that range, then your expected cost drops to:

[# moving objects] * (3 * [cost per angle range calc] 
                      + [cost of velocity selection]
                      + [cost to find close obstacles])

Now this grows linearly as your scene expands (with suitable implementation choices). You might even be able to skip obstacle computations for some moving objects entirely, if they're in open space.

Of course, this depends on having a fast way to gather the few relevant obstacles without iterating over the whole collection. You can use a spatial partition data structure for this, so you only need to search the partition cells within your avoidance radius. If you're using physics colliders, then Physics2D.OverlapCircleNonAlloc gives you a quick way to query this using the built-in physics engine's spatial partition, rather than maintaining your own. Or you could use a trigger collider to detect when new obstacles come into range and prune them as they leave your range.

---

Okay, so let's assume we've done everything in our power to limit the number of obstacles we're considering for each mover, and we're still finding the [cost per angle range calc] is too much. What can we do to bring that down?

As others have pointed out, it's really the transcendentals that are the slowest here. (That's the trig functions like sin, asin, atan2 - used inside the Angle() / SignedAngle() methods even when you don't write them explicitly)

Estimates vary depending on the processor, but you can generally expect addition/subtraction/multiplication to have extremely high throughput and low latency. Costs for division can be ~10 times more than the other arithmetical operators, and square roots ~20 times more. Trig operations however can be more like ~100 times more costly.

So what if we tried to put off that trigonometry for as long as possible?

Instead of computing absolute angles, let's compute tangent ratios of angles relative to our current velocity. These increase monotonically with angle over the 180-degree arc in front of our object, so the overlapping ranges of "tangent ratio obstacles" we calculate this way will correspond 1:1 with the velocity obstacles we want to find. Once we've identified a range of tangent ratios that we want to steer toward, we can convert just the two endpoints of that range back to angles, rather than doing it for every intermediate calculation.

(This method does not scale gracefully to the full circle, unfortunately, but we can work around this: if we find an object that's completely blocked from 90° left to 90° right, we can check the arc behind us as a fallback; hopefully this case is rare)

To do this, we'll exploit some trig identities....

$$\begin{align} \tan(\theta + \alpha) &= \frac {\tan\theta + \tan\alpha} {1 - \tan\theta \tan\alpha} = \frac { \frac y x + \frac r c} {1 - \frac y x \frac r c} = \frac { \frac {yc + xr} {xc} } {\frac { xc - yr} {xc}} = \frac {yc + xr} {xc - yr} \\ \tan(\theta - \alpha) &= \frac {\tan\theta - \tan\alpha} {1 + \tan\theta \tan\alpha} = \frac { \frac y x - \frac r c} {1 + \frac y x \frac r c} = \frac { \frac {yc - xr} {xc} } {\frac { xc + yr} {xc}} = \frac {yc - xr} {xc + yr} \end{align}$$

First, for each moving object we can pre-compute the following:

object.forward = object.velocity.normalized;
object.left = new Vector2(-object.forward.y, object.forward.x);

(Or, if you're already rotating your objects to face their velocity, their transform components will already pre-compute this for you)

Now, when considering each obstacle, we can compute:

Vector2 offset = objectB.position - objectA.position;
Vector2 localOffset = new Vector2(
                          Vector2.Dot(forward, offset),
                          Vector2.Dot(left, offset)
                      );
// Or localOffset = transform.inverseTransformPoint(objectB.position);


float radiusComplement = Mathf.Sqrt(localOffset.sqrMagnitude - objectB.radius * objectB.radius);


float xr = localOffset.x * objectB.radius;
float xc = localOffset.x * radiusComplement;
float yr = localOffset.y * objectB.radius;
float yc = localOffset.y * radiusComplement;

float minTan = (yc - xr)/(xc + yr);
float maxTan = (yc + xr)/(xc - yr);

// If we've crossed the y-axis, clamp to 90° left / right.
if(minTan * localOffset.y < 0f) minTan = minTan * float.NegativeInfinity;
if(maxTan * localOffset.y < 0f) maxTan = maxTan * float.NegativeInfinity;

Note that in the above we needed just one square root per moving object / obstacle pair (to compute \$c\$ AKA radiusComplement), comparable to the magnitude operations in the other solutions. Everything else is multiplication, addition, and subtraction, which are very cheap, and two divisions, which are more costly but still not as much so as a square root or trig function.

After we've computed the minTan and maxTan for all obstacles in range and found the best viable gap, we can convert it back to an absolute velocity, again without trig functions, like so:

Vector2 newDirection = (forward + chosenTangent * left).normalized;
Vector2 newVelocity = newDirection * speed;

So the total count of expensive ops is:

• per moving object:

  • 2 square roots
  • 4 divisions (dividing each of x & y by length in the two .normalized calls, though it's possible these will be vectorized so we pay more like the cost of 2)
  • 0 trig functions

• per mover/obstacle pair:

  • 1 square root
  • 2 divisions
  • 0 trig functions

---

Edit:

Come to think of it, we might be able to do this better using a (modified) sine ratio instead of the tangent ratio, to cover the full 360° in one pass:

float inverseSquare = 1f / localOffset.sqrMagnitude;

float maxSine = inverseSquare * (yc + xr);
float minSine = inverseSquare * (yc - xr);

if (xc - yr < 0) maxSine = Mathf.Sign(maxSine) * 2f - maxSine;
if (xc + yr < 0) minSine = Mathf.Sign(minSine) * 2f - minSine;

if(minSine < maxSine) {
    AddObstacleRange(minSine, maxSine);        
} else {
    AddObstacleRange(-2f, maxSine);
    AddObstacleRange(minSine, 2f);
}

These values will sit in the range -2...2, with the values between -1...1 corresponding to the sine of the angle within the 180° arc in front of the moving object. Values outside this range correspond to the 180° behind the mover, "unfolded" so that we get a non-decreasing function over angles from -180° to +180°.

---

With either solution, you can merge obstacle ranges as you find them, like so:

void AddObstacleRange(float min, float max) {
    for(int i = 0; i < obstacleRanges.Count; i++) {
        // Leave any ranges that don't intersect this one as-is.
        if(obstacleRanges[i].min > max || obstacleRanges[i].max < min)
           continue;

        // Expand the range to encompass any other ranges it rouches.
        min = Mathf.Min(min, obstacleRanges[i].min);
        max = Mathf.Max(max, obstacleRanges[i].max);

        // Remove any ranges subsumed in this way.
        UnorderedRemove(obstacleRanges, i--);
    }    
    // Add our (possibly expanded) obstacle range to the list.
    obstacleRanges.Add(new ObstacleRange(min, max));
}

Then once you've got all the obstacles accounted for, you can scan through them looking for the window edge closest to zero, or zero itself if nothing blocks it:

float GetLeastDeviation() {
    // Find the obstacle range that covers zero, and return is nearest edge.
    foreach(var range in obstacleRanges) {
        if(range.min < 0f && range.max > 0f)
            return range.max > -range.min ? range.min : range.max;
    }

    // If there's no obstacle covering zero, then our ideal trajectory
    // isn't blocked, and we can continue without evasive maneuvers.
    return 0f;
}

Then you can convert that closest unblocked (pseudo) ratio back into an angle or velocity as needed.

Answer 2

---

New Answer

Frame challenge: If we are checking if the sphere will collide with another, ee do not need angles. We need to know if the velocity vector we use for reference will lead to a collision of the spheres.

That is, we need to know if the distance from the second sphere to the line defined by the position of the first sphere and the velocity vector passes closer than the sum of the radius of the first sphere and the radius of the second sphere.

We, start by working with the center of the first sphere as origin. Then project the position of the second sphere (respect to the origin, a.k.a. the center of the first sphere) to the velocity vector. Then we take the distance of the projection to it, and see if it is too close.

Thus:

bool isOutOfRange(sphere1, sphere2, velocity)
{
    var L = sphere2.position - sphere1.position;
    var projected = velocity* dot(L, velocity) / dot (velocity, velocity);
    var check = L - projected; // order of operands is irrelevant
    var r = sphere1.radius + sphere2.radius;
    return length(check) > r;
}

Now, we have no angles, and no trigonometry. We have an sqrt hidding there still... we can get rid of it by comparing the square of the length:

bool isOutOfRange(sphere1, sphere2, velocity)
{
    var L = sphere2.position - sphere1.position;
    var projected = velocity* dot(L, velocity) / dot (velocity, velocity);
    var check = L - projected;
    var r = sphere1.radius + sphere2.radius;
    return (check.x * check.x + check.y * check.y) > (r * r); // 2D, right?
}

Et voilà, no trigonometry, and no sqrt.

---

Addendum

OP says:

Do you also have an idea how I could efficiently select the closest angle to the optimal velocity that is not within one of the angle-ranges that are blocked by the neighbors/velocity obstacles (obviously that could also be the optimal velocity if it is not blocked)?

For a single obstacle, assuming the velocity vector does not match the vector the center of the other sphere, then this is easy: Having the vector that goes from the center of the second sphere to its projection on the velocity vector, we can get a point that is just outside the range. Then if we scale it to the velocity we have our new velocity.

vector getFixedVelocity(sphere1, sphere2, velocity)
{
    var L = sphere2.position - sphere1.position;
    var projected = velocity* dot(L, velocity) / dot (velocity, velocity);
    var check = projected - L; // order is important now
    var r = sphere1.radius + sphere2.radius;
    if ((check.x * check.x + check.y * check.y) > (r * r))
    {
        return velocity;
    }
    var just = sphere2.position + r * normalize(check);
    return length(velocity) * normalize(just);
}

This is the vector that has the same length as the input velocity, does not hit the obstacle and is closer to the input velocity. To get the other velocity (the one that is further away) you can reflect the just vector.

Also Notice I introduced three sqrt. If you can pass the unit velocity, you can get rid of one. The other two are actually 1/sqrt for which there are well known optimizations (which hopefully normalize already uses).

This is the code to get both vectors:

(vector, vector) getFixedVelocity(sphere1, sphere2, velocity)
{
    var L = sphere2.position - sphere1.position;
    var projected = velocity* dot(L, velocity) / dot (velocity, velocity);
    var check = projected - L;
    var r = sphere1.radius + sphere2.radius;
    if ((check.x * check.x + check.y * check.y) > (r * r))
    {
        return (velocity, null);
    }
    var just = sphere2.position + r * normalize(check);
    var reflected = (2 * dot(just, L) / dot(L, L)) - just;
    return (length(velocity) * normalize(just), length(velocity) * normalize(reflected));
}

Note: We can solve the problem velocity and L are vectors on the same direction by adding a small offset to velocity. Dirty, I know.

---

Then to find a velocity direction with no obstacles, we can do a Breadth-first search. If bychecking a velocity vector against the obstacles we get the same vector back always, that is success. Otherwise we got a list of new candidate velocity vectors to check. They should be moving away from the original velocity vector.

If you consider the original velocity vector a node, and the new candidate vectors you get after checking new nodes. You have a tree, and you should do breadth-first search.

If a candidate vector is closer to the original velocity vector than one that has already been discarted, you can discart it too... and to check that, you can compare the dot product of their unit vectors (no need to take acos). In fact, work only with unit velocity vectors so you can avoid unnecesary sqrts.

You might think that you need to sort the vectors by their dot product with the original velocity vector. However, that is not true, all you need to take care of is make sure you are doing breadth-first search.

However, it is useful to sort the obstacles by that criteria.

If the dot product with any candidate reaches -1, then you searched the whole circunference. You can stop the search. If at that point no candidate is clear from obstacles, then you are surrounded, there is no path.

---

Old Answer

If I understand correctly, you need to solve this:

That is a triangle with one side L being the segment from the center of one sphere to the other, another side r is the sum of the radius of the spheres.

We know L, and r. We know that the triangle has a right angle (because one side is a tangent and another side is a radius). And we want to find θ.

---

We are working on a plane, that would be defined by the two centers of the sphere and whatever velocity vector you are using as reference. And we actually want two angles, one is the angle between the reference velocity vector and vector that goes from the first sphere to the second plus θ, and the other is the same angle but minus θ.

---

We can find θ like this:

r/sin(θ) = L/sin(τ/4) where τ = one turn
=>
r/sin(θ) = L/1
=>
r/sin(θ) = L
=>
r = L*sin(θ)
=>
r/L = sin(θ)      
=>
asin(r/L) = θ     

---

Then your angles are

var referenceAngle = angle(velocity, L);
var angle1 = referenceAngle - θ;
var angle2 = referenceAngle + θ;

---

Complete pseudo code:

(float angle1, float angle2) getAngles(sphere1, sphere2, velocity)
{
    var L = sphere2.position - sphere1.position;
    var r = sphere1.radius + sphere2.radius;
    var θ = asin(r/L);
    var referenceAngle = angle(velocity, L);
    return (referenceAngle - θ, referenceAngle + θ);
}

If you only want to check if the velocity is out of the range, then:

bool isOutOfRange(sphere1, sphere2, velocity)
{
    var L = sphere2.position - sphere1.position;
    var r = sphere1.radius + sphere2.radius;
    var θ = asin(r/L);
    return angle(velocity, L) > θ;
}

---

Sure, you can use table lookup for the trigonometry if that is a problem.

Answer 3

Not a complete answer, but this might help.

The slowest parts of your code are the Mathf.Sin, Mathf.Cos and Mathf.Acos commands.They are usually evaluated by libraries at runtime using mathematical series and are accurate to last bit of float. (up to 1.5 * 10^(-45)) That is way to much precision required just for a game.

I suggest substituting them with a look up tables of precalculated values for each degree, decydegree or centydegree depending on how much memory you can spare. You can also make it more precise, but much more complited by using a nonuniform partition.

Having such tables you can ether simply use the nearest value from the table, or if more precision is required use linear interpolation between each two rows in the look up table. Both algorithms will be much faster then using a series, and if you experiment with table sizes the imperfection will be unnoticeable.

This should dramatically increase performance.

I used similar technique to quickly calculate Sqrt when I needed to calculate gravity between 300 simulated planets in real time at 60 fps.