public override void MarkSubtree(cpBBTree tree, Node staticRoot, Func<object, object, ulong, object, ulong> func)
{
if (this.stamp == tree.GetStamp())
{
if (staticRoot != null) staticRoot.MarkLeafQuery(this, false, tree, func);
for (Node node = this; node?.parent != null; node = node.parent)
{
if (node == node.parent.A)
{
node.parent.B.MarkLeafQuery(this, true, tree, func);
}
else
{
node.parent.A.MarkLeafQuery(this, false, tree, func);
}
}
}
else
{
var pair = this.pairs;
while (pair != null)
{
if (this == pair.b.leaf)
{
if (func != null)
func(pair.a.leaf.obj, this.obj, pair.id, null);
pair = pair.b.next;
}
else
{
pair = pair.a.next;
}
}
}
}
public static bool bbTreeIntersectsNode(Node a, Node b)
{
return (a.bb.l <= b.bb.r && b.bb.l <= a.bb.r && a.bb.b <= b.bb.t && b.bb.b <= a.bb.t);
}
public void SubtreeQuery(Node subtree, object obj, cpBB bb, Func<object, object, ulong, object, ulong> func, ref object data)
{
//if(bbIntersectsBB(subtree.bb, bb)){
if (subtree.bb.Intersects(bb))
{
if (subtree.isLeaf)
{
func(obj, subtree.obj, 0, data);
}
else
{
SubtreeQuery(subtree.A, obj, bb, func, ref data);
SubtreeQuery(subtree.B, obj, bb, func, ref data);
}
}
}
public Node SubtreeRemove(Node subtree, Leaf leaf)
{
if (leaf == subtree)
{
return null;
}
else
{
var parent = leaf.parent;
if (parent == subtree)
{
var other = subtree.Other(leaf);
other.parent = subtree.parent;
//NodeRecycle();
subtree.Recycle(this);
return other;
}
else
{
if (parent == null)
return null;
parent.parent.ReplaceChild(parent, parent.Other(leaf), this);
return subtree;
}
}
}
public float NodeSegmentQuery(Node node, cpVect a, cpVect b)
{
float idx = 1 / (b.x - a.x);
float tx1 = (node.bb.l == a.x ? -cp.Infinity : (node.bb.l - a.x) * idx);
float tx2 = (node.bb.r == a.x ? cp.Infinity : (node.bb.r - a.x) * idx);
float txmin = cp.cpfmin(tx1, tx2);
float txmax = cp.cpfmax(tx1, tx2);
float idy = 1 / (b.y - a.y);
float ty1 = (node.bb.b == a.y ? -cp.Infinity : (node.bb.b - a.y) * idy);
float ty2 = (node.bb.t == a.y ? cp.Infinity : (node.bb.t - a.y) * idy);
float tymin = cp.cpfmin(ty1, ty2);
float tymax = cp.cpfmax(ty1, ty2);
if (tymin <= txmax && txmin <= tymax)
{
float min_ = cp.cpfmax(txmin, tymin);
float max_ = cp.cpfmin(txmax, tymax);
if (0.0 <= max_ && min_ <= 1.0f) return cp.cpfmax(min_, 0.0f);
}
return cp.Infinity;
}
public void PairsClear(Node leaf)
{
Pair pair = leaf.pairs;
Pair next;
leaf.PAIRS = null;
while (pair != null)
{
if (pair.a.leaf == leaf)
{
next = pair.a.next;
pair.b.Unlink();
PairRecycle(pair);
pair = next;
}
else
{
next = pair.b.next;
pair.a.Unlink();// ThreadUnlink();
PairRecycle(pair);
pair = next;
}
}
}
public virtual void MarkSubtree(cpBBTree tree, Node staticRoot, Func<object, object, ulong, object, ulong> func)
{
this.a.MarkSubtree(tree, staticRoot, func);
this.b.MarkSubtree(tree, staticRoot, func);
}
public Pair MakePair(Node leafA, Pair nextA, Node leafB, Pair nextB)
{
var pair = this.pooledPairs;
if (pair != null)
{
this.pooledPairs = pair.a.prev;
pair.a.prev = null;
pair.a.leaf = leafA;
pair.a.next = nextA;
pair.b.prev = null;
pair.b.leaf = leafB;
pair.b.next = nextB;
return pair;
}
else
{
cp.numPairs++;
return new Pair(leafA, nextA, leafB, nextB);
}
}
public Node(Node a, Node b, cpBBTree tree)
{
this.obj = null;
bb = cpBB.Merge(a.bb, b.bb);
parent = null;
this.SetA(a);
this.SetB(b);
}
public MarkContext(cpBBTree tree, Node staticRoot, Func<object, object, ulong, object, ulong> func, object data)
{
this.tree = tree;
this.staticRoot = staticRoot;
this.func = func;
this.data = data;
}
public static void nodeRender(Node node, int depth)
{
if (!node.isLeaf && depth <= 10)
{
nodeRender(node.A, depth + 1);
nodeRender(node.B, depth + 1);
}
var str = "";
for (var i = 0; i < depth; i++)
{
str += " ";
}
Trace(str + node.bb.b + " " + node.bb.t);
}
/// ///////////////////////////////////////////////////////////////////
public static float bbTreeMergedArea2(Node node, float l, float b, float r, float t)
{
return (cp.cpfmax(node.bb.r, r) - cp.cpfmin(node.bb.l, l)) * (cp.cpfmax(node.bb.t, t) - cp.cpfmin(node.bb.b, b));
}
public static float bbTreeMergedArea(Node a, Node b)
{
return (cp.cpfmax(a.bb.r, b.bb.r) - cp.cpfmin(a.bb.l, b.bb.l)) * (cp.cpfmax(a.bb.t, b.bb.t) - cp.cpfmin(a.bb.b, b.bb.b));
}
public static void Unlink(Pair prev, Node leaf, Pair next)
{
if (next != null)
{
if (next.a.leaf == leaf)
next.a.prev = prev;
else next.b.prev = prev;
}
if (prev != null)
{
if (prev.a.leaf == leaf) prev.a.next = next;
else prev.b.next = next;
}
else
{
leaf.pairs = next;
}
}
public Node Other(Node child)
{
return (this.A == child ? this.B : this.A);
}
public Node MakeNode(Node a, Node b)
{
var node = this.pooledNodes;
if (node != null)
{
this.pooledNodes = node.parent;
node = new Node(a, b, this);
return node;
}
else
{
cp.numNodes++;
return new Node(a, b, this);
}
}
public virtual void Recycle(cpBBTree tree)
{
this.parent = tree.pooledNodes;
tree.pooledNodes = this;
}
//MARK: Node Functions
public virtual void NodeRecycle(Node node)
{
node.parent = this.pooledNodes;
this.pooledNodes = node;
}
public void ReplaceChild(Node child, Node value, cpBBTree tree)
{
cp.AssertSoft(child == this.A || child == this.B, "Node is not a child of parent.");
if (this.A == child)
{
tree.NodeRecycle(this.A);//.Recycle(tree);
this.SetA(value);
}
else
{
tree.NodeRecycle(this.B);
this.SetB(value);
}
for (var node = this; node != null; node = node.parent)
{
node.bb = node.A.bb.Merge(node.B.bb);
}
}
public void PairInsert(Node a, Node b)
{
Pair nextA = a.PAIRS, nextB = b.PAIRS;
Pair pair = MakePair(a, nextA, b, nextB);
a.pairs = b.pairs = pair;
if (nextA != null)
{
if (nextA.a.leaf == a) nextA.a.prev = pair; else nextA.b.prev = pair;
}
if (nextB != null)
{
if (nextB.a.leaf == b) nextB.a.prev = pair; else nextB.b.prev = pair;
}
}
public void SetA(Node value)
{
this.a = value;
value.parent = this;
}
public Node SubtreeInsert(Node subtree, Leaf leaf)
{
if (subtree == null)
{
return leaf;
}
else if (subtree.isLeaf)
{
return MakeNode(leaf, subtree);
}
else
{
var cost_a = subtree.B.bbArea() + subtree.A.bb.MergedArea(leaf.bb);
var cost_b = subtree.A.bbArea() + subtree.B.bb.MergedArea(leaf.bb);
if (cost_a == cost_b)
{
cost_a = subtree.A.bb.Proximity(leaf.bb);
cost_b = subtree.B.bb.Proximity(leaf.bb);
}
if (cost_b < cost_a)
{
subtree.SetB(SubtreeInsert(subtree.B, leaf));
}
else
{
subtree.SetA(SubtreeInsert(subtree.A, leaf));
}
// subtree.bb = bbMerge(subtree.bb, leaf.bb);
subtree.bb.l = Math.Min(subtree.bb.l, leaf.bb.l);
subtree.bb.b = Math.Min(subtree.bb.b, leaf.bb.b);
subtree.bb.r = Math.Max(subtree.bb.r, leaf.bb.r);
subtree.bb.t = Math.Max(subtree.bb.t, leaf.bb.t);
return subtree;
}
}
public void SetB(Node value)
{
this.b = value;
value.parent = this;
}
public void SubtreeRecycle(Node node)
{
if (!node.isLeaf)
{
SubtreeRecycle(node.A);
SubtreeRecycle(node.B);
NodeRecycle(node);
}
}
// Objects created with constructors are faster than object literals. :(
public Pair(Node leafA, Pair nextA, Node leafB, Pair nextB)
{
a = new Thread(null, leafA, nextA);
b = new Thread(null, leafB, nextB);
id = 0;
}
public float SubtreeSegmentQuery(Node subtree, object obj, cpVect a, cpVect b, float t_exit, Func<object, object, object, float> func, object data)
{
if (subtree.isLeaf)
{
return func(obj, subtree.obj, data);
}
else
{
float t_a = subtree.A.bb.SegmentQuery(a, b);
float t_b = subtree.B.bb.SegmentQuery(a, b);
if (t_a < t_b)
{
if (t_a < t_exit) t_exit = cp.cpfmin(t_exit, SubtreeSegmentQuery(subtree.A, obj, a, b, t_exit, func, data));
if (t_b < t_exit) t_exit = cp.cpfmin(t_exit, SubtreeSegmentQuery(subtree.B, obj, a, b, t_exit, func, data));
}
else
{
if (t_b < t_exit) t_exit = cp.cpfmin(t_exit, SubtreeSegmentQuery(subtree.B, obj, a, b, t_exit, func, data));
if (t_a < t_exit) t_exit = cp.cpfmin(t_exit, SubtreeSegmentQuery(subtree.A, obj, a, b, t_exit, func, data));
}
return t_exit;
}
}
public Thread(Pair prev, Node leaf, Pair next)
{
this.prev = prev;
this.leaf = leaf;
this.next = next;
}
public static float Proximity(Node a, Leaf b)
{
return Proximity(a.bb, b.bb);
}
public static float bbProximity(Node a, Node b)
{
return cp.cpfabs(a.bb.l + a.bb.r - b.bb.l - b.bb.r) + cp.cpfabs(a.bb.b + a.bb.t - b.bb.b - b.bb.t);
}
