public List <VirtualBody> Query(Body body, float theta)
{
var virtualBodies = new List <VirtualBody>();
if (Bodies != null && Bodies.Count > 0)
{
foreach (Body b in Bodies)
{
if (b != body)
{
virtualBodies.Add(VirtualBody.FromBody(b));
}
}
}
else if (Nodes != null)
{
Vector2?centerOfMass;
foreach (BarnesHutTree tree in Nodes)
{
centerOfMass = tree.GetCenterOfMass(body);
if (centerOfMass.HasValue == true)
{
float distance = Vector2.Distance(centerOfMass.Value, body.Position);
float localTheta = (float)(tree.Boundary.Width / distance);
if (localTheta >= theta)
{
virtualBodies.AddRange(tree.Query(body, theta));
}
else
{
virtualBodies.Add(new VirtualBody()
{
Mass = tree.GetTotalMass(body),
Position = centerOfMass.Value,
});
}
}
}
}
return(virtualBodies);
}
private Vector2?GetCenterOfMass(Body exclude)
{
// Checking if there is no bodies or no sub-trees.
if (Bodies != null && Bodies.Count == 0 && Nodes == null)
{
return(null);
}
// Checking if we can use the cached value (if it has already been computed).
if (_didComputeCenterOfMass == true && Boundary.Contains(exclude.Position) == false)
{
return(_centerOfMass);
}
// Getting the bodies from which to compute the center of mass.
var bodies = new List <VirtualBody>();
if (Bodies != null)
{
foreach (Body body in Bodies)
{
if (body != exclude)
{
bodies.Add(VirtualBody.FromBody(body));
}
}
}
else if (Nodes != null)
{
Vector2? nodesCenterOfMass;
VirtualBody newBody;
float treesTotalMass;
foreach (BarnesHutTree tree in Nodes)
{
nodesCenterOfMass = tree.GetCenterOfMass(exclude);
if (nodesCenterOfMass.HasValue == true)
{
treesTotalMass = tree.GetTotalMass(exclude);
newBody = new VirtualBody()
{
Mass = treesTotalMass,
Position = nodesCenterOfMass.Value,
};
bodies.Add(newBody);
if (treesTotalMass == 0)
{
Debugger.Break();
}
}
}
}
if (bodies.Count == 0)
{
return(null);
}
// Actually computing the center of mass.
float totalMass = 0;
float allX = 0;
float allY = 0;
foreach (VirtualBody body in bodies)
{
totalMass += body.Mass;
allX += body.Position.X * body.Mass;
allY += body.Position.Y * body.Mass;
}
var centerOfMass = new Vector2(allX / totalMass, allY / totalMass);
if (float.IsNaN(centerOfMass.X) || float.IsNaN(centerOfMass.Y))
{
Debugger.Break();
}
// Caching for later use (only if there is no body to exclude).
if (Boundary.Contains(exclude.Position) == false)
{
_didComputeCenterOfMass = true;
_centerOfMass = centerOfMass;
}
return(centerOfMass);
}