private void AppendNewHead <T>(Dimension <T> dimension, SpacePoint <T> sp, float position)
{
var newHead = new DimensionPoint <T>(dimension)
{
Position = position
};
newHead.AddPoint(sp);
newHead.HeadLink = new DimensionLink <T>(0, newHead);
var oldHead = dimension.HeadDimPoint;
dimension.HeadDimPoint = newHead;
newHead.HeadLink.AssignNext(oldHead.HeadLink);
var upper = oldHead.HeadLink.Upper;
newHead.HeadLink.AssignUpper(upper);
oldHead.HeadLink.Upper = null;
newHead.TailLink = oldHead.TailLink.Level == 0 ? newHead.HeadLink : oldHead.TailLink;
oldHead.TailLink = oldHead.HeadLink;
_spaceManager.TryAddLevel(dimension);
var toExtendUp = dimension.HeadDimPoint.HeadLink.NextUntil((n, i) => i == 1);
while (_spaceManager.TryExtendUp(toExtendUp, out toExtendUp))
{
_spaceManager.TryAddLevel(dimension);
}
}
private static bool RemoveDimensionPoint <T>(DimensionPoint <T> dPoint)
{
var dimension = dPoint.Dimension;
if (dimension == null || dimension.HeadDimPoint == null)
{
throw new InvalidOperationException("Invalid tree: point was requested to be removed but the tree is already empty");
}
if (dimension.Count == 1)
{
if (dimension.HeadDimPoint == dPoint)
{
dimension.Count = 0;
dimension.HeadDimPoint = dimension.TailDimPoint = null;
return(true);
}
throw new InvalidOperationException("Invalid tree: point requested to be removed does not seem to belong to the tree");
}
if (dimension.HeadDimPoint == dPoint)
{
RemoveHead(dPoint.Dimension);
}
else if (dimension.TailDimPoint == dPoint)
{
RemoveTail(dPoint.Dimension);
}
else
{
RemoveMiddlePoint(dPoint);
}
dimension.Count--;
return(true);
}
private static void RemoveMiddlePoint <T>(DimensionPoint <T> dPoint)
{
var link = dPoint.HeadLink;
RemoveOrMoveColumn(dPoint);
var prev = link.Prev;
var next = link.Next;
prev.AssignNext(next);
TryRemoveLevelInternal(dPoint.Dimension);
}
private static IEnumerable <SpacePoint <T> > EnumerateSpacePoints <T>(
DimensionPoint <T> curr, HashSet <SpacePoint <T> > spacePoints,
HashSet <DimensionPoint <T> > tempDimPoints, float within,
IDimensionSelector dimSelector, params float[] dimensionPositions)
{
if (curr == null)
{
yield break;
}
tempDimPoints.Add(curr);
foreach (var sp in curr.SpaPoints)
{
if (!spacePoints.Contains(sp))
{
var dist = sp.DistanceTo(dimensionPositions, dimSelector);
if (dist <= within)
{
spacePoints.Add(sp);
yield return(sp);
}
}
}
}
bool ISpaceManager.TryFindDimensionPoint <T>(Dimension <T> dimension, float position, out DimensionPoint <T> left, out DimensionPoint <T> right)
{
if (dimension.HeadDimPoint == null)
{
left = null;
right = null;
return(false);
}
var candidateLeft = dimension.HeadDimPoint.TailLink;
var candidateRight = dimension.TailDimPoint.TailLink;
var leftLimit = candidateLeft.DimPoint.Position;
var rightLimit = candidateRight.DimPoint.Position;
if (position > (leftLimit - dimension.Epsillon) && position < (rightLimit + dimension.Epsillon))
{
while (true)
{
while (candidateLeft.Next != null && candidateLeft.Next != candidateRight)
{
if ((candidateLeft.Next.DimPoint.Position - dimension.Epsillon) < position)
{
candidateLeft = candidateLeft.Next;
}
else
{
candidateRight = candidateLeft.Next;
break;
}
}
if (candidateLeft.Level > 0)
{
candidateLeft = candidateLeft.Lower;
candidateRight = candidateRight.Lower;
continue;
}
if (dimension.Eq(position, candidateLeft.DimPoint.Position))
{
left = right = candidateLeft.DimPoint;
return(true);
}
if (dimension.Eq(position, candidateRight.DimPoint.Position))
{
left = right = candidateRight.DimPoint;
return(true);
}
break;
}
left = candidateLeft.DimPoint;
right = candidateRight.DimPoint;
return(false);
}
else if (position < leftLimit)
{
left = null;
right = candidateLeft.DimPoint;
return(false);
}
else if (position > rightLimit)
{
left = candidateRight.DimPoint;
right = null;
return(false);
}
left = null;
right = null;
return(false);
}
bool ISpaceManager.AddPoint <T>(Dimension <T> dimension, SpacePoint <T> sp, float position)
{
if (dimension == null || sp == null)
{
return(false);
}
MustBe.Null(sp.Dimensions[dimension.Index], () => "space point already has assigned the dimension with index=" + dimension.Index);
if (dimension.HeadDimPoint == null)
{
var dp = new DimensionPoint <T>(dimension)
{
Position = position
};
dp.HeadLink = dp.TailLink = new DimensionLink <T>(0, dp);
dp.AddPoint(sp);
dimension.HeadDimPoint = dimension.TailDimPoint = dp;
dimension.Count = 1;
return(true);
}
// try to find existing dimension point
DimensionPoint <T> left;
DimensionPoint <T> right;
if (_spaceManager.TryFindDimensionPoint(dimension, position, out left, out right))
{
// if found add the space point to it
return(left.AddPoint(sp));
}
// new head
if (left == null)
{
AppendNewHead(dimension, sp, position);
dimension.Count++;
return(true);
}
// new tail
if (right == null)
{
AppendNewTail(dimension, sp, position);
dimension.Count++;
return(true);
}
// new in between
var newPoint = new DimensionPoint <T>(dimension)
{
Position = position
};
newPoint.AddPoint(sp);
var newLink = new DimensionLink <T>(0, newPoint);
newPoint.HeadLink = newLink;
newPoint.TailLink = newLink;
left.HeadLink.AssignNext(newLink);
newLink.AssignNext(right.HeadLink);
// for fast movers do not normalize the tree
if (!sp.IsFastMover)
{
_spaceManager.TryAddLevel(dimension);
var toExtendUp = newLink.GetSiblingExtensionCandidate();
while (_spaceManager.TryExtendUp(toExtendUp, out toExtendUp))
{
_spaceManager.TryAddLevel(dimension);
}
}
dimension.Count++;
return(true);
}
private static bool RemoveOrMoveColumn <T>(DimensionPoint <T> dPoint)
{
if (dPoint == null || dPoint.HeadLink == null)
{
return(false);
}
var dimension = dPoint.Dimension;
if (dimension.HeadDimPoint == dPoint || dimension.TailDimPoint == dPoint)
{
return(false);
}
var head = dPoint.HeadLink;
var tail = dPoint.TailLink;
// try move right to direct neighbour that has only 1 level
if (HasBottomLevelNeighbourToTheRight(head))
{
var destination = head.Next;
destination.AssignUpper(head.Upper);
destination.DimPoint.TailLink = tail;
head.AssignUpper(null);
head.DimPoint.TailLink = head.DimPoint.HeadLink;
}
// try move left to direct neighbour that has only 1 level
else if (HasBottomLevelNeighbourToTheLeft(head))
{
var destination = head.Prev;
destination.AssignUpper(head.Upper);
destination.DimPoint.TailLink = tail;
head.AssignUpper(null);
head.DimPoint.TailLink = head.DimPoint.HeadLink;
}
// try move the tip right to current level neighbour
else if (HasTopLevelNeighbourToTheRight(tail))
{
var toDelete = tail.Lower.DimPoint;
var target = tail.Next.Lower.Prev.Prev;
tail.Lower.AssignUpper(null);
target.AssignUpper(tail);
target.DimPoint.TailLink = tail;
DeleteColumn(toDelete.HeadLink);
}
// try move the tip left to current level neighbour
else if (HasTopLevelNeighbourToTheLeft(tail))
{
var toDelete = tail.Lower.DimPoint;
var target = tail.Prev.Lower.Next.Next;
tail.Lower.AssignUpper(null);
target.AssignUpper(tail);
target.DimPoint.TailLink = tail;
DeleteColumn(toDelete.HeadLink);
}
// delete the column
else
{
DeleteColumn(head);
}
return(true);
}
private bool PopulateDimension <T>(Dimension <T> dimension, List <SpacePointSource <T> > points)
{
if (points.Count == 0)
{
return(false);
}
points.Sort(new SpacePointComparer <T>(dimension.Index));
//points.Sort((a, b) => a.InitialPositions[dimension.Index].CompareTo(b.InitialPositions[dimension.Index]));
DimensionPoint <T> head = null;
DimensionPoint <T> curr = null;
DimensionPoint <T> prev = null;
for (var i = 0; i < points.Count; ++i)
{
var sp = points[i];
if (sp == null)
{
continue;
}
var position = sp.InitialPositions[dimension.Index];
if (prev != null)
{
if (dimension.Eq(prev.Position, position))
{
prev.AddPoint(sp);
continue;
}
}
curr = new DimensionPoint <T>(dimension)
{
Position = position
};
curr.HeadLink = new DimensionLink <T>(0, curr);
curr.TailLink = curr.HeadLink;
curr.AddPoint(sp);
if (prev != null)
{
prev.HeadLink.AssignNext(curr.HeadLink);
}
else
{
head = curr;
}
prev = curr;
}
dimension.HeadDimPoint = head;
dimension.TailDimPoint = curr;
dimension.Count += points.Count;
// ReSharper disable PossibleNullReferenceException
DimensionLink <T> link = dimension.HeadDimPoint.HeadLink;
// ReSharper restore PossibleNullReferenceException
// if this is the top level
if (link.Next == null || link.Next.Next == null)
{
return(true);
}
DimensionLink <T> prevUpper = null;
DimensionLink <T> firstUpper = null;
while (link != null)
{
var upper = new DimensionLink <T>((byte)(link.Level + 1));
if (firstUpper == null)
{
firstUpper = upper;
}
link.AssignUpper(upper);
link.DimPoint.TailLink = upper;
if (prevUpper != null)
{
prevUpper.AssignNext(upper);
}
// if this is the end
if (link.Next == null)
{
link = firstUpper;
firstUpper = null;
prevUpper = null;
// if this is the top level
if (link.Next == null || link.Next.Next == null)
{
return(true);
}
continue;
}
link = link.Next.Next;
// the next is the end and it is two links away
if (link.Next != null && link.Next.Next == null)
{
link = link.Next;
}
prevUpper = upper;
}
return(true);
}
