public BoundingSphere(BoundingSphere one, BoundingSphere two)
{
Vector3 centreOffset = two.Center - one.Center;
float distance = centreOffset.SquaredMagnitude;
float radiusDiff = two.Radius - one.Radius;
if (radiusDiff * radiusDiff >= distance)
{
if (one.Radius > two.Radius)
{
_center = one.Center;
_radius = one.Radius;
}
else
{
_center = two.Center;
_radius = two.Radius;
}
}
else
{
distance = MathHelper.Sqrt(distance);
_radius = (distance + one.Radius + two.Radius) * ((float)0.5);
_center = one.Center;
if (distance > 0)
{
_center += centreOffset * ((_radius - one.Radius) / distance);
}
}
}
public HNode(HNode parent, BoundingSphere volume, RigidBody body = null)
{
Parent = parent;
Volume = volume;
Body = body;
Children = new HNode[2];
}
public void Insert(RigidBody body, BoundingSphere volume)
{
if (IsLeaf)
{
Children[0] = new HNode(this, Volume, Body);
Children[1] = new HNode(this, volume, body);
Body = null;
recalculateboundingVolume();
}
else
{
if (Children[0].Volume.GetGrowth(volume) < Children[1].Volume.GetGrowth(volume))
Children[0].Insert(body, volume);
else
Children[1].Insert(body, volume);
}
}
public float GetGrowth(BoundingSphere other)
{
BoundingSphere newSphere = new BoundingSphere(this, other);
return newSphere.Radius * newSphere.Radius - _radius * _radius;
}
public bool Overlaps(BoundingSphere other)
{
float distanceSquared = (_center - other.Center).SquaredMagnitude;
return distanceSquared < (_radius + other._radius) * (_radius + other.Radius);
}
void recalculateboundingVolume(bool recurse = true)
{
if (IsLeaf) return;
Volume = new BoundingSphere(Children[0].Volume, Children[1].Volume);
if (Parent != null) Parent.recalculateboundingVolume(true);
}
