private void subdivide()
{
var oldValue = value;
if (this.value != null)
{
propagateEvent(EventType.Removing);
value = null;
}
quads = new RegionQuadtree <T> [4];
quads[0] = new RegionQuadtree <T>(resolution, depth + 1, oldValue, this, new AABB2i(
aabb.LowerBound,
aabb.LowerBound + new Point2i(aabb.Width / 2, aabb.Height / 2)));
quads[1] = new RegionQuadtree <T>(resolution, depth + 1, oldValue, this, new AABB2i(
aabb.LowerBound + new Point2i(aabb.Width / 2, 0),
aabb.LowerBound + new Point2i(aabb.Width, aabb.Height / 2)));
quads[2] = new RegionQuadtree <T>(resolution, depth + 1, oldValue, this, new AABB2i(
aabb.LowerBound + new Point2i(aabb.Width / 2, aabb.Height / 2),
aabb.LowerBound + new Point2i(aabb.Width, aabb.Height)));
quads[3] = new RegionQuadtree <T>(resolution, depth + 1, oldValue, this, new AABB2i(
aabb.LowerBound + new Point2i(0, aabb.Height / 2),
aabb.LowerBound + new Point2i(aabb.Width / 2, aabb.Height)));
}
private bool removeQuadtreeInternal(RegionQuadtree <T> other)
{
if (other == this)
{
var unset = unsetInternal();
if (unset)
{
if (parent != null)
{
parent.unsubdivide();
}
else
{
unsubdivide();
}
}
return(true);
}
else
{
for (int i = 0; i < 4; i++)
{
if (this.quads[i].removeQuadtreeInternal(other))
{
return(true);
}
}
}
return(false);
}
public void MoveTo(RegionQuadtree <T> other)
{
/*
* var foundPosition = false;
* var myPosition = 0;
* for (int i = 0; i < parent.quads.Length; i++)
* {
* if (parent.quads[i] == this)
* {
* myPosition = i;
* foundPosition = true;
* break;
* }
* }
*
* if (!foundPosition)
* {
* throw new InvalidOperationException("Parent doesn't have this quadtree as child!");
* }
*/
foreach (var quadtree in (IEnumerable <RegionQuadtree <T> >) this)
{
other.SetAABB(quadtree.AABB, quadtree.value.Value);
}
Unset();
}
private static RegionQuadtree <T> soni(RegionQuadtree <T> p, QuadDirection q)
{
if (p != null && p.Type == QuadType.Grey)
{
return(p[q]);
}
return(p);
}
private RegionQuadtree(int resolution, int depth, T?value, RegionQuadtree <T> parent, AABB2i aabb)
{
this.resolution = resolution;
this.depth = depth;
this.value = value;
this.parent = parent;
this.aabb = aabb;
if (value != null)
{
propagateEvent(EventType.Added);
}
}
private void getAllWhoShareEdgeInternal(QuadType type, RegionQuadtree <T> original, List <RegionQuadtree <T> > share, QuadDirection[] dir)
{
if (this.Type == type && this.aabb.Intersects(ref original.aabb))
{
share.Add(this);
}
else if (this.Type == QuadType.Grey)
{
for (int i = 0; i < dir.Length; i++)
{
this[dir[i]].getAllWhoShareEdgeInternal(type, original, share, dir);
}
}
}
public override bool Equals(object obj)
{
if (obj == null)
{
return(false);
}
if (!GetType().Equals(obj.GetType()))
{
return(false);
}
RegionQuadtree <T> otherQuadtree = (RegionQuadtree <T>)obj;
return(points.Equals(otherQuadtree.points) &&
data.Equals(otherQuadtree.data) &&
depth == otherQuadtree.depth &&
region.Equals(otherQuadtree.region));
}
public void Subdivide()
{
QRegion child0Region = CalculateChildRegion((QQuadrant)0);
children[0] = new RegionQuadtree <T> (
depth + 1,
maximumDepth,
child0Region,
CalculateChildData(child0Region)
);
QRegion child1Region = CalculateChildRegion((QQuadrant)1);
children[1] = new RegionQuadtree <T> (
depth + 1,
maximumDepth,
child1Region,
CalculateChildData(child1Region)
);
QRegion child2Region = CalculateChildRegion((QQuadrant)2);
children[2] = new RegionQuadtree <T> (
depth + 1,
maximumDepth,
child2Region,
CalculateChildData(child2Region)
);
QRegion child3Region = CalculateChildRegion((QQuadrant)3);
children[3] = new RegionQuadtree <T> (
depth + 1,
maximumDepth,
child3Region,
CalculateChildData(child3Region)
);
foreach (QVector2D point in points)
{
InsertInChild(point, data);
}
points.Clear();
InsertInChild(subdivisionPoint, subdivisionPointData);
}
private IEnumerable <RegionQuadtree <T> > traverseInternal(RegionQuadtree <T>[] a)
{
var sides = QDO.Sides;
var t = new RegionQuadtree <T> [8];
if (Type == QuadType.Grey)
{
for (int i = 0; i < 4; i++)
{
var d = sides[i];
t[(int)d] = soni(a[(int)d], QDO.Quad(QDO.OpSide(d), QDO.CSide(d)));
t[(int)QDO.Quad(d, QDO.CSide(d))] = soni(a[(int)QDO.Quad(d, QDO.CSide(d))], QDO.Quad(QDO.OpSide(d), QDO.CCSide(d)));
t[(int)QDO.CSide(d)] = soni(a[(int)QDO.CSide(d)], QDO.Quad(d, QDO.CCSide(d)));
t[(int)QDO.Quad(QDO.OpSide(d), QDO.CSide(d))] = soni(a[(int)QDO.CSide(d)], QDO.Quad(QDO.OpSide(d), QDO.CCSide(d)));
t[(int)QDO.OpSide(d)] = this[QDO.Quad(QDO.OpSide(d), QDO.CSide(d))];
t[(int)QDO.Quad(QDO.OpSide(d), QDO.CCSide(d))] = this[QDO.Quad(QDO.OpSide(d), QDO.CCSide(d))];
t[(int)QDO.CCSide(d)] = this[QDO.Quad(d, QDO.CCSide(d))];
t[(int)QDO.Quad(d, QDO.CCSide(d))] = soni(a[(int)d], QDO.Quad(QDO.OpSide(d), QDO.CCSide(d)));
foreach (var item in this[QDO.Quad(d, QDO.CSide(d))].traverseInternal(t))
{
yield return(item);
}
}
}
else if (Type == QuadType.Black)
{
for (int i = 0; i < 8; i++)
{
if (a[i] != null && a[i].Type == QuadType.White)
{
a[i] = null;
}
}
yield return(this);
}
}
private void propagateEvent(EventType eventType, RegionQuadtree <T> start = null, T?oldValue = null)
{
var always = start ?? this;
var par = start ?? this;
while (par != null)
{
if (eventType == EventType.Added)
{
if (par.OnQuadAdded != null)
{
par.OnQuadAdded(par, new QuadEventArgs <T>(always));
}
}
else if (eventType == EventType.Removing)
{
if (par.OnQuadRemoving != null)
{
par.OnQuadRemoving(par, new QuadEventArgs <T>(always));
}
}
else
{
if (par.OnQuadChanged != null)
{
if (oldValue == null)
{
throw new ArgumentException("Old value shouldn't be null");
}
par.OnQuadChanged(par, new QuadChangedEventArgs <T>(always, oldValue.Value));
}
}
par = par.parent;
}
}
public RegionQuadtree <T> ExpandFromCenter()
{
if (Type != QuadType.Grey)
{
subdivide();
}
Action <RegionQuadtree <T>, Action <RegionQuadtree <T> > > doForEach = null;
doForEach = (qt, f) =>
{
f(qt);
if (qt.Type == QuadType.Grey)
{
foreach (var dir in QDO.Quadrants)
{
doForEach(qt[dir], f);
}
}
};
doForEach(this, (qt) =>
{
if (qt.Type == QuadType.Black)
{
qt.propagateEvent(EventType.Removing);
}
});
var newRoot = new RegionQuadtree <T>(resolution + 1);
newRoot.AutoExpand = AutoExpand;
newRoot.subdivide();
foreach (var child in newRoot.quads)
{
child.subdivide();
}
var offset = new Point2i(newRoot.aabb.Width / 4, newRoot.aabb.Height / 4);
var offsets = new Dictionary <QuadDirection, Point2i>
{
{ QuadDirection.NorthWest, new Point2i(quads[0].aabb.Width / 2, quads[0].aabb.Height / 2) },
{ QuadDirection.SouthEast, new Point2i(quads[0].aabb.Width / 2, quads[0].aabb.Height / 2) },
{ QuadDirection.NorthEast, new Point2i(quads[0].aabb.Width / 2, quads[0].aabb.Height / 2) },
{ QuadDirection.SouthWest, new Point2i(quads[0].aabb.Width / 2, quads[0].aabb.Height / 2) },
};
// Set children
Action <RegionQuadtree <T>, RegionQuadtree <T> > expandProcessor = null;
expandProcessor = (qt, parent) =>
{
// Do this
qt.parent = parent;
qt.depth = qt.parent.depth + 1;
qt.resolution = qt.parent.resolution;
// Do children
if (qt.Type == QuadType.Grey)
{
foreach (var dir in QDO.Quadrants)
{
switch (dir)
{
case QuadDirection.NorthWest:
qt[dir].aabb.LowerBound = qt.aabb.LowerBound;
qt[dir].aabb.UpperBound = qt.aabb.LowerBound + new Point2i(qt.aabb.Width / 2, qt.aabb.Height / 2);
break;
case QuadDirection.NorthEast:
qt[dir].aabb.LowerBound = qt.aabb.LowerBound + new Point2i(qt.aabb.Width / 2, 0);
qt[dir].aabb.UpperBound = qt.aabb.LowerBound + new Point2i(qt.aabb.Width, qt.aabb.Height / 2);
break;
case QuadDirection.SouthEast:
qt[dir].aabb.LowerBound = qt.aabb.LowerBound + new Point2i(qt.aabb.Width / 2, qt.aabb.Height / 2);
qt[dir].aabb.UpperBound = qt.aabb.LowerBound + new Point2i(qt.aabb.Width, qt.aabb.Height);
break;
case QuadDirection.SouthWest:
qt[dir].aabb.LowerBound = qt.aabb.LowerBound + new Point2i(0, qt.aabb.Height / 2);
qt[dir].aabb.UpperBound = qt.aabb.LowerBound + new Point2i(qt.aabb.Width / 2, qt.aabb.Height);
break;
default:
throw new ArgumentException("Invalid direction");
}
expandProcessor(qt[dir], qt);
}
}
};
foreach (var quadDirection in QDO.Quadrants)
{
var oldQuad = newRoot[quadDirection][QDO.OpSide(quadDirection)];
newRoot[quadDirection][QDO.OpSide(quadDirection)] = this[quadDirection];
var newQuad = newRoot[quadDirection][QDO.OpSide(quadDirection)];
newQuad.aabb = oldQuad.aabb;
expandProcessor(newQuad, oldQuad.parent);
}
// Call the event
if (OnExpand != null)
{
OnExpand(this, new QuadExpandEventArgs <T>(newRoot, offset));
}
// Move events
// TODO: Test this works properly
newRoot.OnQuadAdded = OnQuadAdded;
OnQuadAdded = null;
newRoot.OnQuadRemoving = OnQuadRemoving;
OnQuadRemoving = null;
newRoot.OnQuadChanged = OnQuadChanged;
OnQuadChanged = null;
doForEach(newRoot, (qt) =>
{
if (qt.Type == QuadType.Black)
{
qt.propagateEvent(EventType.Added);
}
});
newRoot.OnExpand = OnExpand;
return(newRoot);
}
public bool RemoveQuadtree(RegionQuadtree <T> other)
{
return(removeQuadtreeInternal(other));
}
private void cclInternal(RegionQuadtree <T>[] a, List <DisjointSet <int> > linked, Dictionary <RegionQuadtree <T>, int> labels, QuadDirection[] sides, bool quadrants)
{
var t = new RegionQuadtree <T> [8];
if (Type == QuadType.Grey)
{
for (int i = 0; i < 4; i++)
{
var d = sides[i];
t[(int)d] = soni(a[(int)d], QDO.Quad(QDO.OpSide(d), QDO.CSide(d)));
t[(int)QDO.CSide(d)] = soni(a[(int)QDO.CSide(d)], QDO.Quad(d, QDO.CCSide(d)));
t[(int)QDO.OpSide(d)] = this[QDO.Quad(QDO.OpSide(d), QDO.CSide(d))];
t[(int)QDO.CCSide(d)] = this[QDO.Quad(d, QDO.CCSide(d))];
if (quadrants)
{
t[(int)QDO.Quad(d, QDO.CSide(d))] = soni(a[(int)QDO.Quad(d, QDO.CSide(d))], QDO.Quad(QDO.OpSide(d), QDO.CCSide(d)));
t[(int)QDO.Quad(QDO.OpSide(d), QDO.CSide(d))] = soni(a[(int)QDO.CSide(d)], QDO.Quad(QDO.OpSide(d), QDO.CCSide(d)));
t[(int)QDO.Quad(QDO.OpSide(d), QDO.CCSide(d))] = this[QDO.Quad(QDO.OpSide(d), QDO.CCSide(d))];
t[(int)QDO.Quad(d, QDO.CCSide(d))] = soni(a[(int)d], QDO.Quad(QDO.OpSide(d), QDO.CCSide(d)));
}
this[QDO.Quad(d, QDO.CSide(d))].cclInternal(t, linked, labels, sides, quadrants);
}
}
else if (Type == QuadType.Black)
{
bool noNeighbors = true;
var neighborLabels = new List <int>();
for (int i = 0; i < 8; i++)
{
if (a[i] != null && a[i].Type == QuadType.White)
{
a[i] = null;
}
if (a[i] != null)
{
if (a[i].Type == QuadType.Black && labels.ContainsKey(a[i]))
{
neighborLabels.Add(labels[a[i]]);
noNeighbors = false;
}
else if (a[i].Type == QuadType.Grey)
{
var share = getAllWhoShareEdge((QuadDirection)i, QuadType.Black);
foreach (var shareBlack in share)
{
if (labels.ContainsKey(shareBlack))
{
neighborLabels.Add(labels[shareBlack]);
noNeighbors = false;
}
}
}
}
}
if (noNeighbors)
{
var nextLabel = linked.Count;
linked.Add(new DisjointSet <int>(nextLabel));
labels[this] = nextLabel;
}
else
{
labels[this] = neighborLabels.Min();
for (int i = 0; i < neighborLabels.Count; i++)
{
for (int j = 0; j < neighborLabels.Count; j++)
{
linked[neighborLabels[i]].Union(linked[neighborLabels[j]]);
}
}
}
}
}
public QuadEventArgs(RegionQuadtree <T> quadTree)
{
this.quadTree = quadTree;
}
/*
* IEnumerator<RegionQuadtree<T>> IEnumerable<RegionQuadtree<T>>.GetEnumerator()
* {
* switch (Type)
* {
* case QuadType.Black:
* yield return this;
* break;
* case QuadType.Grey:
* foreach (var item in quads)
* {
* foreach (var item2 in ((IEnumerable<RegionQuadtree<T>>)item))
* {
* yield return item2;
* }
* }
* break;
* case QuadType.White:
* default:
* break;
* }
* }
*/
public IEnumerable <RegionQuadtree <T> > Traverse()
{
var a = new RegionQuadtree <T> [8];
return(traverseInternal(a));
}
public QuadChangedEventArgs(RegionQuadtree <T> quadTree, T oldValue)
: base(quadTree)
{
this.oldValue = oldValue;
}
public QuadExpandEventArgs(RegionQuadtree <T> newRoot, Point2i offset)
{
this.newRoot = newRoot;
this.offset = offset;
}
public bool SetCircle(Point2i point, int radius, T value)
{
var rectSize = (int)(radius / Math.Sqrt(2));
var testAABB = new AABB2i(point - new Point2i(rectSize, rectSize), point + new Point2i(rectSize, rectSize));
for (int i = point.X - radius; i <= point.X + radius; i++)
{
for (int j = point.Y - radius; j <= point.Y + radius; j++)
{
var currentPoint = new Point2i(i, j);
if (testAABB.Contains(currentPoint))
{
continue;
}
if ((point - currentPoint).Length() < radius)
{
if (isPointOutside(currentPoint))
{
if (findRoot() == this && AutoExpand)
{
RegionQuadtree <T> newRoot = this;
if (newRoot.isPointOutside(currentPoint))
{
newRoot = newRoot.ExpandFromCenter();
point += new Point2i(newRoot.aabb.Width / 4, newRoot.aabb.Height / 4);
}
return(newRoot.SetCircle(point, radius, value));
}
}
}
}
}
bool anyOuterSet = false;
for (int i = point.X - radius; i <= point.X + radius; i++)
{
for (int j = point.Y - radius; j <= point.Y + radius; j++)
{
var currentPoint = new Point2i(i, j);
if (testAABB.Contains(currentPoint))
{
continue;
}
if ((point - currentPoint).Length() < radius)
{
anyOuterSet |= setInternal(ref currentPoint, ref value);
}
}
}
var anyAABBSet = setAABBInternal(ref testAABB, ref value);
if (anyOuterSet || anyAABBSet)
{
unsubdivide();
}
return(anyOuterSet || anyAABBSet);
}