private void Avoidance(float elapsedSeconds)
{
const float k = 25f;
const float l = 0.6f;
const int cellRadius = (int) (GridMultiplier*l);
for (var i = 0; i < ParticleCount; ++i)
{
var ix = (int)(Particles[i].X * GridMultiplier);
var iy = (int)(Particles[i].Y * GridMultiplier);
var neighbors = new List<int>();
for (var dx = Math.Max(-cellRadius,-ix); dx < Math.Min(cellRadius,_width*GridMultiplier-ix-1); ++dx)
for (var dy = Math.Max(-cellRadius,-iy); dy < Math.Min(cellRadius,_height*GridMultiplier-iy-1); ++dy)
neighbors.AddRange(_grid[ix + dx, iy + dy]);
_velocityBuffer[i] = Velocities[i];
foreach (var j in neighbors)
{
if (i == j)
continue;
var dist = Vector2.Distance(Particles[i], Particles[j]);
if (dist > l)
continue;
var n = Particles[j] - Particles[i];
if (n.Length() == 0)
continue;
n.Normalize();
_velocityBuffer[i] += elapsedSeconds * k * (dist-l) * n;
}
const int dir = 4;
for (var j = 0; j < dir; ++j)
{
var wall = Particles[i] + Vector2.UnitX.Rotate((float) (2f*Math.PI*j/dir))*l;
if (!_physics.IsEmpty(wall))
{
var res = _physics.Raycast(Particles[i], wall);
if (!res.HasHit)
continue;
var n = res.Position - Particles[i];
var dist = Vector2.Distance(Particles[i], res.Position);
if (n.Length() == 0)
continue;
n.Normalize();
_velocityBuffer[i] += elapsedSeconds*k*(dist - l)*n;
}
}
}
var tmp = _velocityBuffer;
_velocityBuffer = Velocities;
Velocities = tmp;
}