Use Matrices to program agents which avoid collision with each other in Unity
Recently I needed to program agents that avoid collision with each other. I decided to go down the analytical way and use simultaneous equations.
So here is what this article contains:
1. A link to the Git hub page where the code for this project is posted. The project contains an Equation solver for C# and a custom Matrix2x2 class.
2. Explanation on how to use matrices to solve simultaneous equations and program agents which avoid each other.
As humans, we not only check if there is an object within our direction of movement, but also in case of dynamic objects, we guess if we are going to be colliding with them based on their velocity and ours in the coming few seconds. Here is a summary of one way to imitate this behaviour:
Each dynamic agent has a velocity. This is a vector which points at the direction which they are moving towards. Its magnitude determines how fast the objects are moving. If you extrapolate this vector in both sides, you get a line through space. This lines represents all the possible positions which the agent can have, with this given velocity. The object is at any given time in only one of these position. Mathematically we can express this as the vector form of a line equation, lets call a position on this this line r:
r = objectCurrentPosition + time * velocity.normalized;
As stated, for variable time going between negative infinity and positive infinity, this creates a line through space. Now if we consider that each dynamic object has a line of its own, all we need to do is to find A) where these lines cross and B) at which time are both agents going to be in the cross section. If they are both going to be there at the same time, a collision will happen, and action needs to be taken. If you think back to high school maths, you can use simultaneous equation to find the cross section between two lines. Typically a method like Gaussian elimination is used to find the solution. As early as 2300 years ago in China, people were using similar ways to find solutions to their daily problems. What we are going to use is matrices. By writing the equations as a matrix (one where the coefficients of the x and y are column entries and each equation a row entry), finding the inverse of this matrix and multiplying it with the right hand side, we get the x,y coordinate of the cross section.
First thing which needs to be done is to convert the equation from its vector form to a cartesian form. This means getting the equation to a form like this:
a*x +b*y = c
Where a, b and c are the constants in the equations. Then the same is done for the other agent, the coefficient can be put in a matrix M and the following equation is found. We can solve this equation for x and y using the inverse of M.
M* [x,y] = [c1, c2]
[x,y] = M_Inverse * [c1, c2]
I won’t go into the details of how each step is achieved. The code I wrote for this is quite self explanatory.
Now that we have the position where the two lines meet (x and y) we know of a possible position where the agents could meet. If they will or not depends on their current position and their speed. Note that the equation is not always solvable. You know this if the matrix M does not have an inverse. If there is no inverse, it is either that the two agents are moving parallel to each other, or are moving on the same line.
To find out if they are actually going to meet or not, you need to substitute the cross section position in the vector form of the line equation and solve for time.
time = ([x, y]- objectCurrentPosition)/velocity ;
if the two agents are going to be at the cross section point at similar time, a collision is guaranteed and actions need to be taken.
I think it is worth mentioning here why we rarely see matrices used like these in game dev within Unity scripts. For one Unity doesn’t have a native Matrix class beside a 4x4 matrices. Even then there are no such functions such as GetDeterminant, Solve or even to know if a system of equations are solvable or not. In the Github page I linked, you find an implementation I did for a 2x2 matrix class. There are C# libraries which have a fully implemented NxN matrix, which have all stated functionality, though linking them with your Unity project might be challenging.
Another point is that matrices are quite unintuitive at first, if you are not used to them. A matrix can be understood in so many different ways, and each way of defining a matrix is valid and relevant.
Last but not least, analytical solutions for real time are not usually the most performant. Their advantage is that you can guarantee a certain behaviour. In our example, the performance is fine if we are talking about a hundred agents or so. For more we might want to consider a k-d tree, and solving only for those agents which are close enough.
As usual, thanks for reading. You can follow me on Twitter and in case there is factually incorrect information in the article, please contact me and let me know.