public void InitializeNode (GraphNode node) {
//Get the index of the node
int ind = node.NodeIndex;
int bucketNumber = ind >> BucketSizeLog2;
int bucketIndex = ind & BucketIndexMask;
if (bucketNumber >= nodes.Length) {
// A resize is required
// At least increase the size to:
// Current size * 1.5
// Current size + 2 or
// bucketNumber+1
var newNodes = new PathNode[System.Math.Max(System.Math.Max(nodes.Length*3 / 2, bucketNumber+1), nodes.Length+2)][];
for (int i = 0; i < nodes.Length; i++) newNodes[i] = nodes[i];
var newBucketNew = new bool[newNodes.Length];
for (int i = 0; i < nodes.Length; i++) newBucketNew[i] = bucketNew[i];
var newBucketCreated = new bool[newNodes.Length];
for (int i = 0; i < nodes.Length; i++) newBucketCreated[i] = bucketCreated[i];
nodes = newNodes;
bucketNew = newBucketNew;
bucketCreated = newBucketCreated;
}
if (nodes[bucketNumber] == null) {
PathNode[] ns;
if (bucketCache.Count > 0) {
ns = bucketCache.Pop();
} else {
ns = new PathNode[BucketSize];
for (int i = 0; i < BucketSize; i++) ns[i] = new PathNode();
}
nodes[bucketNumber] = ns;
if (!bucketCreated[bucketNumber]) {
bucketNew[bucketNumber] = true;
bucketCreated[bucketNumber] = true;
}
filledBuckets++;
}
PathNode pn = nodes[bucketNumber][bucketIndex];
pn.node = node;
}