3D collision avoidance: finding the updated velocity vector (outside the "collision-velocities" cones)

Question

I am trying to understand and implement the mechanism of a fully 3D collision avoidance (steering behavior) system for flight movement (six degrees of freedom), currently focusing on circumventing static obstacles (all with the shape of a sphere).

However, I don't quite get how to figure it out the new velocity vector of the moving agent. The figure below illustrates the scene. The moving agent (green) has to steer three static objects (blue). The red line represents the initial ahead velocity vector.

Notice that there are also three white/semi-transparent cones. These represent the "forbidden velocity vectors" regarding each obstacle. It means, the set of velocity vectors that, if used as the new ahead vectors of the agent, would make the agent collide with one or more of the obstacles (also note that the radius of each cone is equal the radius of the given obstacle plus the radius of agent, so to allow an offset for the player to maneuver around).

In order to find out the new ahead vector of the moving agent in such 3D environment, considering the three obstacles, a naïve approach would be to simply port to 3D the classic solution explained in this often cited article and exemplified by the following 2D image:

There, a new velocity (orange arrow) is simply calculated by normalizing the minimum distance (black arrow) between the original velocity and the center of the obstacle and then multiplying such normal by the sum between the radius of the obstacle and the radius of the moving agent. Then, an average of the new velocities calculated for each of the obstacles would give the total final velocity.

In many cases, that is sufficient. However, take a look at the cases below (exemplified in 2D to ease visualization):

In all of them, the naïve approach will result in a collision. In a and b, the final new velocity will coincide with the original velocity (red arrow) and the moving agent will move forward despite being partially or fully blocked. In c) and d), the new velocity (orange arrow) will still result in the same consequence.

So, my question is: what is the most computationally efficient way to find out the new ahead vector of the moving agent in such 3D environment, considering the three obstacles, in a way that avoids collision? Or, in other words, the new ahead vector that:

1) is not inside any of the cones;

2) is the closest possible to the original ahead vector (red line in the picture).

PS: Preferably, I am not looking for a library, I am looking to learn how to that.

Answer 1

I don't think there's an efficient way of solving the problem exactly, but here's how I'd try to tackle it.

First, I'd use bounding volumes around each object, instead of the objects themselves. Each object can be approximated by the union of more than one bounding volume, though.

The simplest solution would be to compute a single bounding volume that contains all the objects that you need to avoid and compute the cone from that volume.

This might not be good enough if objects are not relatively close to each other. You might then want to do some clustering in a way that two objects belong to the same cluster if it's not possible, or at least not trivial to pass between them. Compute the cluster of objects considering their bounding volumes plus the size of the player's bounding volume plus some extra margin. You might use something like this:

http://lab.polygonal.de/?p=120

After you have the clusters, find the closest one and compute the cone to avoid colliding with it. Because of the way the clusters were created, if you steer just enough to avoid hitting one cluster, you won't hit another.

Furthermore, you can create a recursive structure when computing the clusters which will help you find the closest cluster .

There are a few things you can play with. For example, instead of picking the closest cluster, pick the two closest clusters and compute a single cone that avoids them both. Also, you could try other bounding volumes other than spheres.

Answer 2

There are two ways to address this question that I can see. First, if you simply want to find a way to steer, then you just need to implement pathfinding (I find this quite helpful). That would be the end of that (and is the correct practical answer to this question), but I think you are more curious about a mathematical solution to your problem.

To address that, let's look at an equivalent problem. Take a 2D slice of your scene "ahead" of your pilot of a plane that is normal to the original vector. What you will get is a point that represents your original vector and a series of ellipses that are the 2D projections of your occlusion cones. Now what you want to do is to find The closest point to your original point (let's call it P) that lies on the exterior of non-overlapping ellipses. It turns out that this is a pretty difficult problem to solve. There are 3 steps:

• Figure out the intersection points of all your ellipses
• Find any holes in the union of all your ellipses
• Figure out the closest point to P on all your ellipses outside the union (which can be inside a hole)

Okay, all of this requires Lagrange multipliers and angle checking and some other really complicated stuff to solve. Let's look at other options. If we transform our problem instead into angular space, we see that what we actually want to do is find the minumum distance between a point and multiple circles projected onto the surface of a sphere. In differential geometry, this is often called a 2-sphere and is useful here. So first, we need to find the distance between two points on the surface of the sphere, which we will use the 2-sphere metric to find. Luckily, it's pretty easy: ds^2 = (R^2)*(dth^2) + (R^2)*(sin(th)^2)*(dph^2), where we hold R constant as the radius of our 2-sphere projected into 3-space. Coming from physics, I take th to be the angle from positive z and ph to be the angle from positive x with s being the distance.

What does doing this accomplish? It allows us to remove the use of Lagrange multipliers to minimize distance, and allows us to work in "native" coordinate (since we define our cones in terms of an angle of occlusion). So now we need to find the radius of our cone-projections. Let's take one cone for now, and call the angle that defines it a. The radius of the projection is then simply R*a. That was easy enough -- what other quantity do we need to know about the cones? We need to know the center of the projection, which is defined by the look vector from the actor. First we convert x, y, and z into spherical coordinates where we know we are at distance R, and solve for th and ph, which we can do by simply plugging into our conversion formulas: th = acos(z/R) and ph = atan2(y/x) (use atan2 here to account for arctangent's quadrant ambigities). The process is the same to find the position of your original vector in spherical coordinates.

So the problem has been simplified. However, the problem is still quite difficult to solve, but now you only need to worry about circles and points instead of ellipses and points or angles and vectors. The rest of the process is rather procedural. You essentially need to find a bounding area formed by your circles, for which there are multiple solutions. I'll present one method which may not be the best, but will work.

I would first compare the sum of the radii of all circle pairs and the distance between the centers, and then if they sum is larger than the center distance, set them equal and solve for th and ph to find the intersections. You know that each pair of intersections describes "forbidden angles" for the circle in question, which you would store as an array of intersection point angles (from the center of the circle) for each circle that intersects this one -- importantly you must "merge" any ranges that overlap as you add them to the array for the next part to work correctly. Then you find the closest point on the edge of the circle to the original point (which is as simple as drawing a line through the center point of the circle and the original point, then finding the location on the line at the radius of the circle away from the center). Then loop through your stored array and test if that angle is in the range of each forbidden angle. If so, then select the closest edge. That is the closest point to the outside described inside this circle. Repeat the process for all circles (some optimization can be done in the selection process -- you don't actually need to calculate this for every circle). Now compare all your shortest distances, find the smallest, and that is your answer, described in the angles th and ph. Remember that distance is described with the 2-sphere metric while performing this calculation. Since you don't care about the sphere that all of this is on, only the angles, you can set R=1 and do these calculations on the unit sphere.

This is the simplest way I could think to do this. I'm not sure it's the absolute simplest way, but it would work pretty well. Practically for a game, however, you just want to implement pathfinding.