private void CollideBodies(BarnesHutTree tree, float maxRadius)
{
var collisions = new Dictionary <Body, HashSet <Body> >();
foreach (Body a in _bodies)
{
Vector2 origin = new Vector2(a.Position.X - 2 * maxRadius, a.Position.Y - 2 * maxRadius);
Rectangle range = new Rectangle(origin, 4 * maxRadius, 4 * maxRadius);
List <Body> closeBodies = tree.Query(range);
foreach (Body b in closeBodies)
{
if (a == b)
{
continue;
}
float distance = Vector2.Distance(a.Position, b.Position);
if (distance < (a.Radius + b.Radius))
{
// If there is A, there must not be B inside of it.
if (collisions.ContainsKey(a) && collisions.GetValueOrDefault(a).Contains(b) == false)
{
collisions.GetValueOrDefault(a).Add(b);
}
// Either there is no B, or if there is, there must not be A inside of B.
else if (collisions.ContainsKey(b) == false || (collisions.ContainsKey(b) && collisions.GetValueOrDefault(b).Contains(a) == false))
{
collisions.Add(a, new HashSet <Body>()
{
b
});
}
}
}
}
foreach (var collision in collisions)
{
ComputeCollision(collision.Key, collision.Value);
foreach (Body body in collision.Value)
{
_bodies.Remove(body);
}
}
}
private List <Vector2> ComputeForces(BarnesHutTree tree)
{
var forces = new List <Vector2>();
foreach (Body a in _bodies)
{
var forcesOnBody = Vector2.Zero;
List <VirtualBody> virtualBodies = tree.Query(a, Theta);
foreach (VirtualBody b in virtualBodies)
{
float distance = Vector2.Distance(a.Position, b.Position);
float force = GravitationalConstant * ((a.Mass * b.Mass) / (float)Math.Pow(distance, 2));
// if (distance < float.Epsilon)
// {
// Console.WriteLine("d {0} f {1} a {2} b {3}", distance, force, a, b);
// throw new Exception("Epsylon");
// }
// if (float.IsNaN(force))
// {
// Console.WriteLine("d {0} f {1} a {2} b {3}", distance, force, a, b);
// throw new Exception("NaN");
// }
float angle = (float)Math.Atan2(b.Position.Y - a.Position.Y, b.Position.X - a.Position.X);
forcesOnBody.X += (float)Math.Cos(angle) * force;
forcesOnBody.Y += (float)Math.Sin(angle) * force;
}
forces.Add(forcesOnBody);
}
return(forces);
}