public void ApplyForces(ArborNode m, double repulsion)
{
try {
Queue <object> queue = new Queue <object>();
queue.Enqueue(fRoot);
while (queue.Count > 0)
{
object obj = queue.Dequeue();
if (obj == null || obj == m)
{
continue;
}
ArborPoint ptx, k;
double l, kMag, massx;
if (obj is ArborNode)
{
ArborNode node = (ArborNode)obj;
massx = node.Mass;
ptx = node.Pt;
k = m.Pt.Sub(ptx);
kMag = k.Magnitude();
l = Math.Max(1.0f, kMag);
m.ApplyForce(k.Normalize().Mul(repulsion * massx).Div(l * l));
}
else
{
Branch branch = (Branch)obj;
massx = branch.Mass;
ptx = branch.Pt.Div(massx);
k = m.Pt.Sub(ptx);
kMag = k.Magnitude();
double h = Math.Sqrt(branch.Size.X * branch.Size.Y);
if (h / kMag > fDist)
{
queue.Enqueue(branch.Q[QNe]);
queue.Enqueue(branch.Q[QNw]);
queue.Enqueue(branch.Q[QSe]);
queue.Enqueue(branch.Q[QSw]);
}
else
{
l = Math.Max(1.0f, kMag);
m.ApplyForce(k.Normalize().Mul(repulsion * massx).Div(l * l));
}
}
}
} catch (Exception ex) {
Debug.WriteLine("BarnesHutTree.ApplyForces(): " + ex.Message);
}
}
public void Test_ApplyForce()
{
var node = new ArborNode("x");
Assert.IsNotNull(node);
#if DEBUG
node.V = new ArborPoint(1.0f, 1.0f);
node.F = new ArborPoint(3.0f, 4.0f);
node.Mass = 2.0f;
node.ApplyForce(new ArborPoint(3.0f, 3.0f));
Assert.AreEqual(new ArborPoint(4.5f, 5.5f), node.F);
node.Mass = 0.0d;
node.ApplyForce(new ArborPoint(3.0f, 3.0f));
Assert.AreEqual(new ArborPoint(double.PositiveInfinity, double.PositiveInfinity), node.F);
#endif
}
private void UpdateVelocityAndPosition(double dt)
{
int nodesCount = fNodes.Count;
if (nodesCount == 0)
{
fEnergyMax = 0.0f;
fEnergyMean = 0.0f;
fEnergySum = 0.0f;
return;
}
double eMax = 0.0f;
double eSum = 0.0f;
// calc center drift
ArborPoint rr = ArborPoint.Zero;
for (int i = 0; i < nodesCount; i++)
{
ArborNode node = fNodes[i];
rr = rr.Sub(node.Pt);
}
ArborPoint drift = rr.Div(nodesCount);
// main updates loop
for (int i = 0; i < nodesCount; i++)
{
ArborNode node = fNodes[i];
// apply center drift
node.ApplyForce(drift);
// apply center gravity
if (fGravity)
{
ArborPoint q = node.Pt.Mul(-1.0f);
node.ApplyForce(q.Mul(fRepulsion / 100.0f));
}
// update velocities
if (node.Fixed)
{
node.V = ArborPoint.Zero;
}
else
{
node.V = node.V.Add(node.F.Mul(dt));
node.V = node.V.Mul(1.0f - fFriction);
double r = node.V.MagnitudeSquare();
if (r > 1000000.0f)
{
node.V = node.V.Div(r);
}
}
node.F = ArborPoint.Zero;
// update positions
node.Pt = node.Pt.Add(node.V.Mul(dt));
// update energy
double energy = node.V.MagnitudeSquare();
eSum += energy;
eMax = Math.Max(energy, eMax);
}
fEnergyMax = eMax;
fEnergyMean = eSum / nodesCount;
fEnergySum = eSum;
}
