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 BarnesHutTree ComputeBarnesHutTree()
{
float minX = float.MaxValue;
float maxX = float.MinValue;
float minY = float.MaxValue;
float maxY = float.MinValue;
foreach (Body body in _bodies)
{
if (body.Position.X < minX)
{
minX = body.Position.X;
}
if (body.Position.X > maxX)
{
maxX = body.Position.X;
}
if (body.Position.Y < minY)
{
minY = body.Position.Y;
}
if (body.Position.Y > maxY)
{
maxY = body.Position.Y;
}
}
float width = Math.Abs(maxX - minX);
float height = Math.Abs(maxY - minY);
float size = (float)Math.Ceiling(Math.Max(width, height) + 0.5);
var middle = new Vector2((minX + maxX) / 2, (minY + maxY) / 2);
var origin = new Vector2(middle.X - size / 2, middle.Y - size / 2);
var tree = new BarnesHutTree(new Rectangle(origin, size, size), 4);
foreach (Body body in _bodies)
{
tree.Insert(body);
}
return(tree);
}
private void Subdivide()
{
double width = Boundary.Width / 2;
double height = Boundary.Height / 2;
Rectangle[] boundaries = new Rectangle[]
{
new Rectangle(new Vector2(Boundary.Origin.X, Boundary.Origin.Y), width, height),
new Rectangle(new Vector2((float)(Boundary.Origin.X + width), Boundary.Origin.Y), width, height),
new Rectangle(new Vector2(Boundary.Origin.X, (float)(Boundary.Origin.Y + height)), width, height),
new Rectangle(new Vector2((float)(Boundary.Origin.X + width), (float)(Boundary.Origin.Y + height)), width, height),
};
Nodes = new BarnesHutTree[4];
for (int i = 0; i < Nodes.Length; i++)
{
Nodes[i] = new BarnesHutTree(boundaries[i], LeafCapacity);
}
foreach (Body body in Bodies)
{
bool pointInserted = false;
foreach (BarnesHutTree tree in Nodes)
{
if (tree.Insert(body))
{
pointInserted = true;
break;
}
}
Debug.Assert(pointInserted == true, "Point were not inserted during subdivision.");
}
Bodies = null;
}
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);
}
