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); }