Calculating angles and distances

I am running a simulation with 250 interacting agents and have a few functions that are called over and over again. Even with precomputing all distances between agents before the N2 (250x250) interaction loop, my simulation is still very slow. Are there any C++ optimization tricks that I could use to speed these up?

This is the most-used function in my simulation. It calculates the distance2 between two agents in a continuous space. I have a feeling there isn't much that can be done to further optimize this, but you guys have surprised me with some tricks before:

double tGame::calcDistanceSquared(double fromX, double fromY, double toX, double toY)
{
double diffX = fromX - toX;
double diffY = fromY - toY;

return ( diffX * diffX ) + ( diffY * diffY );
}


Here's another expensive function in my simulation. It calculates the angle from one agent to another agent relative to the 'from' agent's heading. As you can see, I already did a little precomputing with the atan2() function (and that DOES speed things up a bit, despite what I've read in other posts).

double tGame::calcAngle(double fromX, double fromY, double fromAngle, double toX, double toY)
{
double Ux = 0.0, Uy = 0.0, Vx = 0.0, Vy = 0.0;

Ux = (toX - fromX);
Uy = (toY - fromY);

Vx = cosLookup[(int)fromAngle];
Vy = sinLookup[(int)fromAngle];

int firstTerm = (int)((Ux * Vy) - (Uy * Vx));
int secondTerm = (int)((Ux * Vx) + (Uy * Vy));

if (fabs(firstTerm) < 1000 && fabs(secondTerm) < 1000)
{
return atan2Lookup[firstTerm + 1000][secondTerm + 1000];
}

else
{
return atan2(firstTerm, secondTerm) * 180.0 / cPI;
}
}


Finally, here's the monster function that uses the calcDistanceSquared() function so much. This is run every simulation time step, and there's 2,000 time steps per simulation (and MANY simulations). The most expensive part is the calcDistanceSquared() in the N2 loop.

void tGame::recalcPredAndPreyDistTable(double preyX[], double preyY[], bool preyDead[],
double predX, double predY,
double predDists[250], double preyDists[250][250])
{
for (int i = 0; i < 250; ++i)
{
{
predDists[i] = calcDistanceSquared(predX, predY, preyX[i], preyY[i]);
preyDists[i][i] = 0.0;

for (int j = i + 1; j < 250; ++j)
{
{
preyDists[i][j] = preyDists[j][i] = calcDistanceSquared(preyX[i], preyY[i], preyX[j], preyY[j]);
}
}
}
}
}

-
Use a space partitioning algorithm to avoid the N^2 loop. –  GManNickG Jan 12 '13 at 20:12
Just my 2 cents: is it really necessary for each of the 250 agents to interact with all other 249? For swarm AI, usually only the n nearest are used, which basically makes the N^2 a N. –  Constantinius Jan 12 '13 at 20:16
@Constantinius I am simulating a retina for each prey, thus I have to at least know if each prey is within a visible range. –  Randy Olson Jan 12 '13 at 20:18
@GManNickG Excuse my ignorance (I've never used a space partitioning algorithm before), but would it take a large overhaul to implement a space partitioning algorithm in a continuous space like this? –  Randy Olson Jan 12 '13 at 20:19
@RandyOlson: Large overhaul or not, you have to do it. Speed up and optimization is rarely about the little nanosecond improvements you might spend (waste) time finding. It's about structuring your data efficiently for your use. Anything beyond the smallest toy games need to organize their entities in space. –  GManNickG Jan 12 '13 at 20:26

It looks good to me. I would change

    if (!preyDead[i])
{


To

    if (preyDead[i])
continue;


Just so its less nested. Same with preyDead[j]. But everything looks fine to me. A tip I once saw on SO is, if your list order doesn't matter you can sort the preyDead so all the dead would be at the beginning or end and that will help branch prediction. However thats assuming its not really expensive to sort it and that its a very erratic bool and it isnt true/false 90% of the time already.

Thats a uber optimize that may not help, it shouldn't be done less you really really want to

-

I don't believe you will not get maximum benefits from code changes, but see my answer here:
Optimization Coding Practices

One issue that is screaming out is why are you making so many iterations?
For example, do all the positions need to be recalculated?

Can you cache the calculations?
You only need to perform the calculations if something moves. You can quickly detect if something moves by comparing the coordinates (don't need to calculate distances here).

Another observation is that you are currently polling, that is, looping until something moves. You may want to change your paradigm to event driven: when something changes, it sends out a message or notification. With the Event Driven paradigm, your reduce a lot of computation for things that don't change.

-

It is being said that compilers can vectorize stuff better if there are no ifs in loops. So, in that spirit, try to remove the if from the inner loop of recalcPredAndPreyDistTable() to see if it does not vectorize better and thus generate better code and thus is not a performance win despite the recomputation of distances between the dead prey.

Second thought, if you have a compiler that supports OpenMP, try to parallelize the outer loop, like this:

#pragma omp parallel for
for (int i = 0; i < 250; ++i)
{

-

Almost certainly whatever you try to achieve with "fromAngle" and atan2, can be accomplished with pure vector math.

Re-arrange the matrix (with extra level of indirection in other parts of the code) so that the prayDead[i]==true are in the beginning of the array (and prayDead[i]==false) are at the end.

It does not only reduce branch prediction, but decreases from N*N to (N-n)*(N-n).

Also it may not be optimal to try to reduce the operations from N*N to N*N/2 by computing an upper triangle. It's better to make a simple loop that the compiler can parallelize (that's 4x gain compared to max 2x gain from reducing the symmetry.)

Further, it's often better to split 256x256 operations to 32x32 x 8x8 operations or so. Then each block will benefit from the array being in cache -- however in this case it may again be beneficial to permute upper triangle blocks:

a b c <-- here the diagonal blocks (a,d,f) are calculated as usual
B d e     but B,C,E can be calculated by rotating the results of (b,c,e)
C E f

-

If this is possible - try to rewrite methods header like this. Once I get valuable speed-up. This is because you have no need to copy arrays, you can give to your method only addresses.

void tGame::recalcPredAndPreyDistTable(double &preyX[], double &preyY[], bool &preyDead[],
double predX, double predY,
double &predDists[250], double &preyDists[250][250])


EDIT: will this work?

void tGame::recalcPredAndPreyDistTable(double *preyX, double *preyY, bool *preyDead,
double predX, double predY,
double *predDists, double **preyDists)

-
If I remember right, arrays are passed by reference by default in C++. –  Randy Olson Jan 12 '13 at 21:18
Randy is kind of correct. C arrays are just pointers, no more, no less... So, this adds nothing. –  Alex Chamberlain Jan 12 '13 at 21:26
@AlexChamberlain: Please, do not maintain the false assertion that arrays are pointers alive!!! Arrays are not pointers, they decay into pointers easily, but they are not pointers. –  David Rodríguez - dribeas Jan 12 '13 at 22:48
@cupidon4uk: The code in your answer will not compile, double &preyX[] means array of unknown number of references to double which is not legal in C++. Even if you added parenthesis as in double (&preyX)[] meaning reference to an array... it would still not compile as you cannot have a reference to an array of an unknown number of elements. –  David Rodríguez - dribeas Jan 12 '13 at 22:51
@dribeas Really? C style arrays are pretty much just pointers once you pass then to another function. Of course, in a function they reserve space on the stack. Now, in C++, you shouldn't use C arrays any more. Use std::array if you can, otherwise tr1 or boost. Pass them by const reference and all is good with the world. Furthermore, if you want to return an array, just return an array. Compilers will optimise out the copy. –  Alex Chamberlain Jan 13 '13 at 8:19