59bf2749dd
Refactored SoundChannel and SoundManager to use explicit locking for OpenAL operations and channel assignment, preventing race conditions during parallel sound playback. Added thread-local stacks in DynamicTree to ensure thread safety during parallel physics queries and raycasts. These changes address concurrency issues when sounds or physics queries are triggered from multiple threads.
1148 lines
39 KiB
C#
1148 lines
39 KiB
C#
// Copyright (c) 2018 Kastellanos Nikolaos
|
|
|
|
/* Original source Farseer Physics Engine:
|
|
* Copyright (c) 2014 Ian Qvist, http://farseerphysics.codeplex.com
|
|
* Microsoft Permissive License (Ms-PL) v1.1
|
|
*/
|
|
|
|
/*
|
|
* Farseer Physics Engine:
|
|
* Copyright (c) 2012 Ian Qvist
|
|
*
|
|
* Original source Box2D:
|
|
* Copyright (c) 2006-2011 Erin Catto http://www.box2d.org
|
|
*
|
|
* This software is provided 'as-is', without any express or implied
|
|
* warranty. In no event will the authors be held liable for any damages
|
|
* arising from the use of this software.
|
|
* Permission is granted to anyone to use this software for any purpose,
|
|
* including commercial applications, and to alter it and redistribute it
|
|
* freely, subject to the following restrictions:
|
|
* 1. The origin of this software must not be misrepresented; you must not
|
|
* claim that you wrote the original software. If you use this software
|
|
* in a product, an acknowledgment in the product documentation would be
|
|
* appreciated but is not required.
|
|
* 2. Altered source versions must be plainly marked as such, and must not be
|
|
* misrepresented as being the original software.
|
|
* 3. This notice may not be removed or altered from any source distribution.
|
|
*/
|
|
|
|
using System;
|
|
using System.Collections.Generic;
|
|
using System.Diagnostics;
|
|
using System.Threading;
|
|
using FarseerPhysics.Common;
|
|
using FarseerPhysics.Dynamics;
|
|
using Microsoft.Xna.Framework;
|
|
|
|
namespace FarseerPhysics.Collision
|
|
{
|
|
/// <summary>
|
|
/// A node in the dynamic tree. The client does not interact with this directly.
|
|
/// </summary>
|
|
internal struct TreeNode<T>
|
|
{
|
|
/// <summary>
|
|
/// Enlarged AABB
|
|
/// </summary>
|
|
internal AABB AABB;
|
|
|
|
internal int Child1;
|
|
internal int Child2;
|
|
|
|
// leaf = 0, free node = -1
|
|
internal int Height;
|
|
internal int ParentOrNext;
|
|
|
|
public Body Body;
|
|
|
|
internal T UserData;
|
|
|
|
internal bool IsLeaf()
|
|
{
|
|
return Child1 == DynamicTree<T>.NullNode;
|
|
}
|
|
}
|
|
|
|
/// <summary>
|
|
/// A dynamic tree arranges data in a binary tree to accelerate
|
|
/// queries such as volume queries and ray casts. Leafs are proxies
|
|
/// with an AABB. In the tree we expand the proxy AABB by Settings.b2_fatAABBFactor
|
|
/// so that the proxy AABB is bigger than the client object. This allows the client
|
|
/// object to move by small amounts without triggering a tree update.
|
|
///
|
|
/// Nodes are pooled and relocatable, so we use node indices rather than pointers.
|
|
/// </summary>
|
|
public class DynamicTree<T>
|
|
{
|
|
// Thread-local stacks to ensure thread safety during parallel queries/raycasts
|
|
[ThreadStatic]
|
|
private static Stack<int> _raycastStack;
|
|
[ThreadStatic]
|
|
private static Stack<int> _queryStack;
|
|
|
|
private static Stack<int> RaycastStack => _raycastStack ??= new Stack<int>(256);
|
|
private static Stack<int> QueryStack => _queryStack ??= new Stack<int>(256);
|
|
|
|
private int _freeList;
|
|
private int _nodeCapacity;
|
|
private int _nodeCount;
|
|
private TreeNode<T>[] _nodes;
|
|
private int _root;
|
|
internal const int NullNode = -1;
|
|
|
|
/// <summary>
|
|
/// Constructing the tree initializes the node pool.
|
|
/// </summary>
|
|
public DynamicTree()
|
|
{
|
|
_root = NullNode;
|
|
|
|
_nodeCapacity = 16;
|
|
_nodeCount = 0;
|
|
_nodes = new TreeNode<T>[_nodeCapacity];
|
|
|
|
// Build a linked list for the free list.
|
|
for (int i = 0; i < _nodeCapacity - 1; ++i)
|
|
{
|
|
_nodes[i].ParentOrNext = i + 1;
|
|
_nodes[i].Height = -1;
|
|
}
|
|
_nodes[_nodeCapacity - 1].ParentOrNext = NullNode;
|
|
_nodes[_nodeCapacity - 1].Height = -1;
|
|
_freeList = 0;
|
|
}
|
|
|
|
/// <summary>
|
|
/// Compute the height of the binary tree in O(N) time. Should not be called often.
|
|
/// </summary>
|
|
public int Height
|
|
{
|
|
get
|
|
{
|
|
if (_root == NullNode)
|
|
{
|
|
return 0;
|
|
}
|
|
|
|
return _nodes[_root].Height;
|
|
}
|
|
}
|
|
|
|
/// <summary>
|
|
/// Get the ratio of the sum of the node areas to the root area.
|
|
/// </summary>
|
|
public float AreaRatio
|
|
{
|
|
get
|
|
{
|
|
if (_root == NullNode)
|
|
{
|
|
return 0.0f;
|
|
}
|
|
|
|
//TreeNode<T>* root = &_nodes[_root];
|
|
float rootArea = _nodes[_root].AABB.Perimeter;
|
|
|
|
float totalArea = 0.0f;
|
|
for (int i = 0; i < _nodeCapacity; ++i)
|
|
{
|
|
//TreeNode<T>* node = &_nodes[i];
|
|
if (_nodes[i].Height < 0)
|
|
{
|
|
// Free node in pool
|
|
continue;
|
|
}
|
|
|
|
totalArea += _nodes[i].AABB.Perimeter;
|
|
}
|
|
|
|
return totalArea / rootArea;
|
|
}
|
|
}
|
|
|
|
/// <summary>
|
|
/// Get the maximum balance of an node in the tree. The balance is the difference
|
|
/// in height of the two children of a node.
|
|
/// </summary>
|
|
public int MaxBalance
|
|
{
|
|
get
|
|
{
|
|
int maxBalance = 0;
|
|
for (int i = 0; i < _nodeCapacity; ++i)
|
|
{
|
|
//TreeNode<T>* node = &_nodes[i];
|
|
if (_nodes[i].Height <= 1)
|
|
{
|
|
continue;
|
|
}
|
|
|
|
Debug.Assert(_nodes[i].IsLeaf() == false);
|
|
|
|
int child1 = _nodes[i].Child1;
|
|
int child2 = _nodes[i].Child2;
|
|
int balance = Math.Abs(_nodes[child2].Height - _nodes[child1].Height);
|
|
maxBalance = Math.Max(maxBalance, balance);
|
|
}
|
|
|
|
return maxBalance;
|
|
}
|
|
}
|
|
|
|
/// <summary>
|
|
/// Create a proxy in the tree as a leaf node. We return the index
|
|
/// of the node instead of a pointer so that we can grow
|
|
/// the node pool.
|
|
/// /// </summary>
|
|
/// <param name="aabb">The aabb.</param>
|
|
/// <param name="userData">The user data.</param>
|
|
/// <returns>Index of the created proxy</returns>
|
|
public int AddProxy(ref AABB aabb)
|
|
{
|
|
int proxyId = AllocateNode();
|
|
|
|
// Fatten the aabb.
|
|
Vector2 r = new Vector2(Settings.AABBExtension, Settings.AABBExtension);
|
|
_nodes[proxyId].AABB.LowerBound = aabb.LowerBound - r;
|
|
_nodes[proxyId].AABB.UpperBound = aabb.UpperBound + r;
|
|
_nodes[proxyId].Height = 0;
|
|
|
|
InsertLeaf(proxyId);
|
|
|
|
return proxyId;
|
|
}
|
|
|
|
/// <summary>
|
|
/// Destroy a proxy. This asserts if the id is invalid.
|
|
/// </summary>
|
|
/// <param name="proxyId">The proxy id.</param>
|
|
public void RemoveProxy(int proxyId)
|
|
{
|
|
Debug.Assert(0 <= proxyId && proxyId < _nodeCapacity);
|
|
Debug.Assert(_nodes[proxyId].IsLeaf());
|
|
|
|
RemoveLeaf(proxyId);
|
|
FreeNode(proxyId);
|
|
}
|
|
|
|
/// <summary>
|
|
/// Move a proxy with a swepted AABB. If the proxy has moved outside of its fattened AABB,
|
|
/// then the proxy is removed from the tree and re-inserted. Otherwise
|
|
/// the function returns immediately.
|
|
/// </summary>
|
|
/// <param name="proxyId">The proxy id.</param>
|
|
/// <param name="aabb">The aabb.</param>
|
|
/// <param name="displacement">The displacement.</param>
|
|
/// <returns>true if the proxy was re-inserted.</returns>
|
|
public bool MoveProxy(int proxyId, ref AABB aabb, Vector2 displacement)
|
|
{
|
|
Debug.Assert(0 <= proxyId && proxyId < _nodeCapacity);
|
|
|
|
Debug.Assert(_nodes[proxyId].IsLeaf());
|
|
|
|
if (_nodes[proxyId].AABB.Contains(ref aabb))
|
|
{
|
|
return false;
|
|
}
|
|
|
|
RemoveLeaf(proxyId);
|
|
|
|
// Extend AABB.
|
|
AABB b = aabb;
|
|
Vector2 r = new Vector2(Settings.AABBExtension, Settings.AABBExtension);
|
|
b.LowerBound = b.LowerBound - r;
|
|
b.UpperBound = b.UpperBound + r;
|
|
|
|
// Predict AABB displacement.
|
|
Vector2 d = Settings.AABBMultiplier * displacement;
|
|
|
|
if (d.X < 0.0f)
|
|
{
|
|
b.LowerBound.X += d.X;
|
|
}
|
|
else
|
|
{
|
|
b.UpperBound.X += d.X;
|
|
}
|
|
|
|
if (d.Y < 0.0f)
|
|
{
|
|
b.LowerBound.Y += d.Y;
|
|
}
|
|
else
|
|
{
|
|
b.UpperBound.Y += d.Y;
|
|
}
|
|
|
|
_nodes[proxyId].AABB = b;
|
|
|
|
InsertLeaf(proxyId);
|
|
return true;
|
|
}
|
|
|
|
/// <summary>
|
|
/// Set proxy user data.
|
|
/// </summary>
|
|
/// <typeparam name="T"></typeparam>
|
|
/// <param name="proxyId">The proxy id.</param>
|
|
/// <param name="userData">The proxy user data.</param>
|
|
public void SetUserData(int proxyId, T userData, Body body)
|
|
{
|
|
_nodes[proxyId].UserData = userData;
|
|
_nodes[proxyId].Body = body;
|
|
}
|
|
|
|
/// <summary>
|
|
/// Get proxy user data.
|
|
/// </summary>
|
|
/// <typeparam name="T"></typeparam>
|
|
/// <param name="proxyId">The proxy id.</param>
|
|
/// <returns>the proxy user data or 0 if the id is invalid.</returns>
|
|
public T GetUserData(int proxyId)
|
|
{
|
|
Debug.Assert(0 <= proxyId && proxyId < _nodeCapacity);
|
|
return _nodes[proxyId].UserData;
|
|
}
|
|
|
|
/// <summary>
|
|
/// Get the fat AABB for a proxy.
|
|
/// </summary>
|
|
/// <param name="proxyId">The proxy id.</param>
|
|
/// <param name="fatAABB">The fat AABB.</param>
|
|
public void GetFatAABB(int proxyId, out AABB fatAABB)
|
|
{
|
|
Debug.Assert(0 <= proxyId && proxyId < _nodeCapacity);
|
|
fatAABB = _nodes[proxyId].AABB;
|
|
}
|
|
|
|
/// <summary>
|
|
/// Get the fat AABB for a proxy.
|
|
/// </summary>
|
|
/// <param name="proxyId">The proxy id.</param>
|
|
/// <returns>The fat AABB.</returns>
|
|
public AABB GetFatAABB(int proxyId)
|
|
{
|
|
Debug.Assert(0 <= proxyId && proxyId < _nodeCapacity);
|
|
return _nodes[proxyId].AABB;
|
|
}
|
|
|
|
public Body GetBody(int proxyId)
|
|
{
|
|
Debug.Assert(0 <= proxyId && proxyId < _nodeCapacity);
|
|
return _nodes[proxyId].Body;
|
|
}
|
|
|
|
|
|
/// <summary>
|
|
/// Test overlap of fat AABBs.
|
|
/// </summary>
|
|
/// <param name="proxyIdA">The proxy id A.</param>
|
|
/// <param name="proxyIdB">The proxy id B.</param>
|
|
public bool TestFatAABBOverlap(int proxyIdA, int proxyIdB)
|
|
{
|
|
Debug.Assert(0 <= proxyIdA && proxyIdA < _nodeCapacity);
|
|
Debug.Assert(0 <= proxyIdB && proxyIdB < _nodeCapacity);
|
|
return AABB.TestOverlap(ref _nodes[proxyIdA].AABB, ref _nodes[proxyIdB].AABB);
|
|
}
|
|
|
|
/// <summary>
|
|
/// Query an AABB for overlapping proxies. The callback class
|
|
/// is called for each proxy that overlaps the supplied AABB.
|
|
/// </summary>
|
|
/// <param name="callback">The callback.</param>
|
|
/// <param name="aabb">The aabb.</param>
|
|
public void Query(Func<int, bool> callback, ref AABB aabb, ref Body body)
|
|
{
|
|
QueryStack.Clear();
|
|
QueryStack.Push(_root);
|
|
|
|
while (QueryStack.Count > 0)
|
|
{
|
|
int nodeId = QueryStack.Pop();
|
|
if (nodeId == NullNode)
|
|
{
|
|
continue;
|
|
}
|
|
|
|
TreeNode<T> node = _nodes[nodeId];
|
|
if (node.Body != body && AABB.TestOverlap(ref node.AABB, ref aabb))
|
|
{
|
|
if (node.IsLeaf())
|
|
{
|
|
if (node.Body.CollidesWithMatchesBetweenFixtures &&
|
|
body.CollisionCategoriesMatchBetweenFixtures)
|
|
{
|
|
//equivalent to
|
|
//collide = node.Body.CollidesWith.HasAnyFlag(body.CollisionCategories) && body.CollidesWith.HasAnyFlag(node.Body.CollisionCategories)
|
|
//same check as in ContactManager.ShouldCollide
|
|
//inlined here using binary operations because this is performance critical code
|
|
bool collide =
|
|
(node.Body.CollidesWith & body.CollisionCategories) != 0 &&
|
|
(body.CollidesWith & node.Body.CollisionCategories) != 0;
|
|
if (!collide)
|
|
{
|
|
continue;
|
|
}
|
|
}
|
|
bool proceed = callback(nodeId);
|
|
if (proceed == false)
|
|
{
|
|
return;
|
|
}
|
|
|
|
}
|
|
else
|
|
{
|
|
QueryStack.Push(node.Child1);
|
|
QueryStack.Push(node.Child2);
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
public void Query(Func<int, bool> callback, ref AABB aabb)
|
|
{
|
|
QueryStack.Clear();
|
|
QueryStack.Push(_root);
|
|
|
|
while (QueryStack.Count > 0)
|
|
{
|
|
int nodeId = QueryStack.Pop();
|
|
if (nodeId == NullNode)
|
|
{
|
|
continue;
|
|
}
|
|
|
|
//TreeNode<T>* node = &_nodes[nodeId];
|
|
if (AABB.TestOverlap(ref _nodes[nodeId].AABB, ref aabb))
|
|
{
|
|
if (_nodes[nodeId].IsLeaf())
|
|
{
|
|
bool proceed = callback(nodeId);
|
|
if (proceed == false)
|
|
{
|
|
return;
|
|
}
|
|
}
|
|
else
|
|
{
|
|
QueryStack.Push(_nodes[nodeId].Child1);
|
|
QueryStack.Push(_nodes[nodeId].Child2);
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
/// <summary>
|
|
/// Ray-cast against the proxies in the tree. This relies on the callback
|
|
/// to perform a exact ray-cast in the case were the proxy contains a Shape.
|
|
/// The callback also performs the any collision filtering. This has performance
|
|
/// roughly equal to k * log(n), where k is the number of collisions and n is the
|
|
/// number of proxies in the tree.
|
|
/// </summary>
|
|
/// <param name="callback">A callback class that is called for each proxy that is hit by the ray.</param>
|
|
/// <param name="input">The ray-cast input data. The ray extends from p1 to p1 + maxFraction * (p2 - p1).</param>
|
|
/// <param name="collisionCategory">The collision categories of the fixtures to raycast against.</param>
|
|
public void RayCast(IBroadPhase broadPhase, Func<RayCastInput, FixtureProxy, float> callback, ref RayCastInput input, Category collisionCategory = Category.All)
|
|
{
|
|
Vector2 p1 = input.Point1;
|
|
Vector2 p2 = input.Point2;
|
|
Vector2 r = p2 - p1;
|
|
Debug.Assert(r.LengthSquared() > 0.0f);
|
|
r.Normalize();
|
|
|
|
// v is perpendicular to the segment.
|
|
Vector2 absV = MathUtils.Abs(new Vector2(-r.Y, r.X)); //FPE: Inlined the 'v' variable
|
|
|
|
// Separating axis for segment (Gino, p80).
|
|
// |dot(v, p1 - c)| > dot(|v|, h)
|
|
|
|
float maxFraction = input.MaxFraction;
|
|
|
|
// Build a bounding box for the segment.
|
|
AABB segmentAABB = new AABB();
|
|
{
|
|
Vector2 t = p1 + maxFraction * (p2 - p1);
|
|
Vector2.Min(ref p1, ref t, out segmentAABB.LowerBound);
|
|
Vector2.Max(ref p1, ref t, out segmentAABB.UpperBound);
|
|
}
|
|
|
|
RaycastStack.Clear();
|
|
RaycastStack.Push(_root);
|
|
|
|
while (RaycastStack.Count > 0)
|
|
{
|
|
int nodeId = RaycastStack.Pop();
|
|
if (nodeId == NullNode)
|
|
{
|
|
continue;
|
|
}
|
|
|
|
//TreeNode<T>* node = &_nodes[nodeId];
|
|
|
|
if (AABB.TestOverlap(ref _nodes[nodeId].AABB, ref segmentAABB) == false)
|
|
{
|
|
continue;
|
|
}
|
|
|
|
// Separating axis for segment (Gino, p80).
|
|
// |dot(v, p1 - c)| > dot(|v|, h)
|
|
Vector2 c = _nodes[nodeId].AABB.Center;
|
|
Vector2 h = _nodes[nodeId].AABB.Extents;
|
|
float separation = Math.Abs(Vector2.Dot(new Vector2(-r.Y, r.X), p1 - c)) - Vector2.Dot(absV, h);
|
|
if (separation > 0.0f)
|
|
{
|
|
continue;
|
|
}
|
|
|
|
if (_nodes[nodeId].IsLeaf())
|
|
{
|
|
FixtureProxy proxy = broadPhase.GetProxy(nodeId);
|
|
if (collisionCategory != Category.All &&
|
|
//!collisionCategory.HasFlag(proxy.Fixture.CollisionCategories)
|
|
(collisionCategory & proxy.Fixture.CollisionCategories) == 0)
|
|
{
|
|
continue;
|
|
}
|
|
|
|
RayCastInput subInput;
|
|
subInput.Point1 = input.Point1;
|
|
subInput.Point2 = input.Point2;
|
|
subInput.MaxFraction = maxFraction;
|
|
|
|
float value = callback(subInput, proxy);
|
|
|
|
if (value == 0.0f)
|
|
{
|
|
// the client has terminated the raycast.
|
|
return;
|
|
}
|
|
|
|
if (value > 0.0f)
|
|
{
|
|
// Update segment bounding box.
|
|
maxFraction = value;
|
|
Vector2 t = p1 + maxFraction * (p2 - p1);
|
|
Vector2.Min(ref p1, ref t, out segmentAABB.LowerBound);
|
|
Vector2.Max(ref p1, ref t, out segmentAABB.UpperBound);
|
|
}
|
|
}
|
|
else
|
|
{
|
|
RaycastStack.Push(_nodes[nodeId].Child1);
|
|
RaycastStack.Push(_nodes[nodeId].Child2);
|
|
}
|
|
}
|
|
}
|
|
|
|
private int AllocateNode()
|
|
{
|
|
// Expand the node pool as needed.
|
|
if (_freeList == NullNode)
|
|
{
|
|
Debug.Assert(_nodeCount == _nodeCapacity);
|
|
|
|
// The free list is empty. Rebuild a bigger pool.
|
|
TreeNode<T>[] oldNodes = _nodes;
|
|
_nodeCapacity *= 2;
|
|
_nodes = new TreeNode<T>[_nodeCapacity];
|
|
Array.Copy(oldNodes, _nodes, _nodeCount);
|
|
|
|
// Build a linked list for the free list. The parent
|
|
// pointer becomes the "next" pointer.
|
|
for (int i = _nodeCount; i < _nodeCapacity - 1; ++i)
|
|
{
|
|
_nodes[i].ParentOrNext = i + 1;
|
|
_nodes[i].Height = -1;
|
|
}
|
|
_nodes[_nodeCapacity - 1].ParentOrNext = NullNode;
|
|
_nodes[_nodeCapacity - 1].Height = -1;
|
|
_freeList = _nodeCount;
|
|
}
|
|
|
|
// Peel a node off the free list.
|
|
int nodeId = _freeList;
|
|
_freeList = _nodes[nodeId].ParentOrNext;
|
|
_nodes[nodeId].ParentOrNext = NullNode;
|
|
_nodes[nodeId].Child1 = NullNode;
|
|
_nodes[nodeId].Child2 = NullNode;
|
|
_nodes[nodeId].Height = 0;
|
|
_nodes[nodeId].UserData = default(T);
|
|
++_nodeCount;
|
|
return nodeId;
|
|
}
|
|
|
|
private void FreeNode(int nodeId)
|
|
{
|
|
Debug.Assert(0 <= nodeId && nodeId < _nodeCapacity);
|
|
Debug.Assert(0 < _nodeCount);
|
|
_nodes[nodeId].ParentOrNext = _freeList;
|
|
_nodes[nodeId].Height = -1;
|
|
_freeList = nodeId;
|
|
--_nodeCount;
|
|
}
|
|
|
|
private void InsertLeaf(int leaf)
|
|
{
|
|
if (_root == NullNode)
|
|
{
|
|
_root = leaf;
|
|
_nodes[_root].ParentOrNext = NullNode;
|
|
return;
|
|
}
|
|
|
|
// Find the best sibling for this node
|
|
AABB leafAABB = _nodes[leaf].AABB;
|
|
int index = _root;
|
|
while (_nodes[index].IsLeaf() == false)
|
|
{
|
|
int child1 = _nodes[index].Child1;
|
|
int child2 = _nodes[index].Child2;
|
|
|
|
float area = _nodes[index].AABB.Perimeter;
|
|
|
|
AABB combinedAABB = new AABB();
|
|
combinedAABB.Combine(ref _nodes[index].AABB, ref leafAABB);
|
|
float combinedArea = combinedAABB.Perimeter;
|
|
|
|
// Cost of creating a new parent for this node and the new leaf
|
|
float cost = 2.0f * combinedArea;
|
|
|
|
// Minimum cost of pushing the leaf further down the tree
|
|
float inheritanceCost = 2.0f * (combinedArea - area);
|
|
|
|
// Cost of descending into child1
|
|
float cost1;
|
|
if (_nodes[child1].IsLeaf())
|
|
{
|
|
AABB aabb = new AABB();
|
|
aabb.Combine(ref leafAABB, ref _nodes[child1].AABB);
|
|
cost1 = aabb.Perimeter + inheritanceCost;
|
|
}
|
|
else
|
|
{
|
|
AABB aabb = new AABB();
|
|
aabb.Combine(ref leafAABB, ref _nodes[child1].AABB);
|
|
float oldArea = _nodes[child1].AABB.Perimeter;
|
|
float newArea = aabb.Perimeter;
|
|
cost1 = (newArea - oldArea) + inheritanceCost;
|
|
}
|
|
|
|
// Cost of descending into child2
|
|
float cost2;
|
|
if (_nodes[child2].IsLeaf())
|
|
{
|
|
AABB aabb = new AABB();
|
|
aabb.Combine(ref leafAABB, ref _nodes[child2].AABB);
|
|
cost2 = aabb.Perimeter + inheritanceCost;
|
|
}
|
|
else
|
|
{
|
|
AABB aabb = new AABB();
|
|
aabb.Combine(ref leafAABB, ref _nodes[child2].AABB);
|
|
float oldArea = _nodes[child2].AABB.Perimeter;
|
|
float newArea = aabb.Perimeter;
|
|
cost2 = newArea - oldArea + inheritanceCost;
|
|
}
|
|
|
|
// Descend according to the minimum cost.
|
|
if (cost < cost1 && cost1 < cost2)
|
|
{
|
|
break;
|
|
}
|
|
|
|
// Descend
|
|
if (cost1 < cost2)
|
|
{
|
|
index = child1;
|
|
}
|
|
else
|
|
{
|
|
index = child2;
|
|
}
|
|
}
|
|
|
|
int sibling = index;
|
|
|
|
// Create a new parent.
|
|
int oldParent = _nodes[sibling].ParentOrNext;
|
|
int newParent = AllocateNode();
|
|
_nodes[newParent].ParentOrNext = oldParent;
|
|
_nodes[newParent].UserData = default(T);
|
|
_nodes[newParent].AABB.Combine(ref leafAABB, ref _nodes[sibling].AABB);
|
|
_nodes[newParent].Height = _nodes[sibling].Height + 1;
|
|
|
|
if (oldParent != NullNode)
|
|
{
|
|
// The sibling was not the root.
|
|
if (_nodes[oldParent].Child1 == sibling)
|
|
{
|
|
_nodes[oldParent].Child1 = newParent;
|
|
}
|
|
else
|
|
{
|
|
_nodes[oldParent].Child2 = newParent;
|
|
}
|
|
|
|
_nodes[newParent].Child1 = sibling;
|
|
_nodes[newParent].Child2 = leaf;
|
|
_nodes[sibling].ParentOrNext = newParent;
|
|
_nodes[leaf].ParentOrNext = newParent;
|
|
}
|
|
else
|
|
{
|
|
// The sibling was the root.
|
|
_nodes[newParent].Child1 = sibling;
|
|
_nodes[newParent].Child2 = leaf;
|
|
_nodes[sibling].ParentOrNext = newParent;
|
|
_nodes[leaf].ParentOrNext = newParent;
|
|
_root = newParent;
|
|
}
|
|
|
|
// Walk back up the tree fixing heights and AABBs
|
|
index = _nodes[leaf].ParentOrNext;
|
|
while (index != NullNode)
|
|
{
|
|
index = Balance(index);
|
|
|
|
int child1 = _nodes[index].Child1;
|
|
int child2 = _nodes[index].Child2;
|
|
|
|
Debug.Assert(child1 != NullNode);
|
|
Debug.Assert(child2 != NullNode);
|
|
|
|
_nodes[index].Height = 1 + Math.Max(_nodes[child1].Height, _nodes[child2].Height);
|
|
_nodes[index].AABB.Combine(ref _nodes[child1].AABB, ref _nodes[child2].AABB);
|
|
|
|
index = _nodes[index].ParentOrNext;
|
|
}
|
|
|
|
//Validate();
|
|
}
|
|
|
|
private void RemoveLeaf(int leaf)
|
|
{
|
|
if (leaf == _root)
|
|
{
|
|
_root = NullNode;
|
|
return;
|
|
}
|
|
|
|
int parent = _nodes[leaf].ParentOrNext;
|
|
int grandParent = _nodes[parent].ParentOrNext;
|
|
int sibling;
|
|
if (_nodes[parent].Child1 == leaf)
|
|
{
|
|
sibling = _nodes[parent].Child2;
|
|
}
|
|
else
|
|
{
|
|
sibling = _nodes[parent].Child1;
|
|
}
|
|
|
|
if (grandParent != NullNode)
|
|
{
|
|
// Destroy parent and connect sibling to grandParent.
|
|
if (_nodes[grandParent].Child1 == parent)
|
|
{
|
|
_nodes[grandParent].Child1 = sibling;
|
|
}
|
|
else
|
|
{
|
|
_nodes[grandParent].Child2 = sibling;
|
|
}
|
|
_nodes[sibling].ParentOrNext = grandParent;
|
|
FreeNode(parent);
|
|
|
|
// Adjust ancestor bounds.
|
|
int index = grandParent;
|
|
while (index != NullNode)
|
|
{
|
|
index = Balance(index);
|
|
|
|
int child1 = _nodes[index].Child1;
|
|
int child2 = _nodes[index].Child2;
|
|
|
|
_nodes[index].AABB.Combine(ref _nodes[child1].AABB, ref _nodes[child2].AABB);
|
|
_nodes[index].Height = 1 + Math.Max(_nodes[child1].Height, _nodes[child2].Height);
|
|
|
|
index = _nodes[index].ParentOrNext;
|
|
}
|
|
}
|
|
else
|
|
{
|
|
_root = sibling;
|
|
_nodes[sibling].ParentOrNext = NullNode;
|
|
FreeNode(parent);
|
|
}
|
|
|
|
//Validate();
|
|
}
|
|
|
|
/// <summary>
|
|
/// Perform a left or right rotation if node A is imbalanced.
|
|
/// </summary>
|
|
/// <param name="iA"></param>
|
|
/// <returns>the new root index.</returns>
|
|
private int Balance(int iA)
|
|
{
|
|
Debug.Assert(iA != NullNode);
|
|
|
|
//TreeNode<T>* A = &_nodes[iA];
|
|
if (_nodes[iA].IsLeaf() || _nodes[iA].Height < 2)
|
|
{
|
|
return iA;
|
|
}
|
|
|
|
int iB = _nodes[iA].Child1;
|
|
int iC = _nodes[iA].Child2;
|
|
Debug.Assert(0 <= iB && iB < _nodeCapacity);
|
|
Debug.Assert(0 <= iC && iC < _nodeCapacity);
|
|
|
|
//TreeNode<T>* B = &_nodes[iB];
|
|
//TreeNode<T>* C = &_nodes[iC];
|
|
|
|
int balance = _nodes[iC].Height - _nodes[iB].Height;
|
|
|
|
// Rotate C up
|
|
if (balance > 1)
|
|
{
|
|
int iF = _nodes[iC].Child1;
|
|
int iG = _nodes[iC].Child2;
|
|
//TreeNode<T>* F = &_nodes[iF];
|
|
//TreeNode<T>* G = &_nodes[iG];
|
|
Debug.Assert(0 <= iF && iF < _nodeCapacity);
|
|
Debug.Assert(0 <= iG && iG < _nodeCapacity);
|
|
|
|
// Swap A and C
|
|
_nodes[iC].Child1 = iA;
|
|
_nodes[iC].ParentOrNext = _nodes[iA].ParentOrNext;
|
|
_nodes[iA].ParentOrNext = iC;
|
|
|
|
// A's old parent should point to C
|
|
if (_nodes[iC].ParentOrNext != NullNode)
|
|
{
|
|
if (_nodes[_nodes[iC].ParentOrNext].Child1 == iA)
|
|
{
|
|
_nodes[_nodes[iC].ParentOrNext].Child1 = iC;
|
|
}
|
|
else
|
|
{
|
|
Debug.Assert(_nodes[_nodes[iC].ParentOrNext].Child2 == iA);
|
|
_nodes[_nodes[iC].ParentOrNext].Child2 = iC;
|
|
}
|
|
}
|
|
else
|
|
{
|
|
_root = iC;
|
|
}
|
|
|
|
// Rotate
|
|
if (_nodes[iF].Height > _nodes[iG].Height)
|
|
{
|
|
_nodes[iC].Child2 = iF;
|
|
_nodes[iA].Child2 = iG;
|
|
_nodes[iG].ParentOrNext = iA;
|
|
_nodes[iA].AABB.Combine(ref _nodes[iB].AABB, ref _nodes[iG].AABB);
|
|
_nodes[iC].AABB.Combine(ref _nodes[iA].AABB, ref _nodes[iF].AABB);
|
|
|
|
_nodes[iA].Height = 1 + Math.Max(_nodes[iB].Height, _nodes[iG].Height);
|
|
_nodes[iC].Height = 1 + Math.Max(_nodes[iA].Height, _nodes[iF].Height);
|
|
}
|
|
else
|
|
{
|
|
_nodes[iC].Child2 = iG;
|
|
_nodes[iA].Child2 = iF;
|
|
_nodes[iF].ParentOrNext = iA;
|
|
_nodes[iA].AABB.Combine(ref _nodes[iB].AABB, ref _nodes[iF].AABB);
|
|
_nodes[iC].AABB.Combine(ref _nodes[iA].AABB, ref _nodes[iG].AABB);
|
|
|
|
_nodes[iA].Height = 1 + Math.Max(_nodes[iB].Height, _nodes[iF].Height);
|
|
_nodes[iC].Height = 1 + Math.Max(_nodes[iA].Height, _nodes[iG].Height);
|
|
}
|
|
|
|
return iC;
|
|
}
|
|
|
|
// Rotate B up
|
|
if (balance < -1)
|
|
{
|
|
int iD = _nodes[iB].Child1;
|
|
int iE = _nodes[iB].Child2;
|
|
//TreeNode<T>* D = &_nodes[iD];
|
|
//TreeNode<T>* E = &_nodes[iE];
|
|
Debug.Assert(0 <= iD && iD < _nodeCapacity);
|
|
Debug.Assert(0 <= iE && iE < _nodeCapacity);
|
|
|
|
// Swap A and B
|
|
_nodes[iB].Child1 = iA;
|
|
_nodes[iB].ParentOrNext = _nodes[iA].ParentOrNext;
|
|
_nodes[iA].ParentOrNext = iB;
|
|
|
|
// A's old parent should point to B
|
|
if (_nodes[iB].ParentOrNext != NullNode)
|
|
{
|
|
if (_nodes[_nodes[iB].ParentOrNext].Child1 == iA)
|
|
{
|
|
_nodes[_nodes[iB].ParentOrNext].Child1 = iB;
|
|
}
|
|
else
|
|
{
|
|
Debug.Assert(_nodes[_nodes[iB].ParentOrNext].Child2 == iA);
|
|
_nodes[_nodes[iB].ParentOrNext].Child2 = iB;
|
|
}
|
|
}
|
|
else
|
|
{
|
|
_root = iB;
|
|
}
|
|
|
|
// Rotate
|
|
if (_nodes[iD].Height > _nodes[iE].Height)
|
|
{
|
|
_nodes[iB].Child2 = iD;
|
|
_nodes[iA].Child1 = iE;
|
|
_nodes[iE].ParentOrNext = iA;
|
|
_nodes[iA].AABB.Combine(ref _nodes[iC].AABB, ref _nodes[iE].AABB);
|
|
_nodes[iB].AABB.Combine(ref _nodes[iA].AABB, ref _nodes[iD].AABB);
|
|
|
|
_nodes[iA].Height = 1 + Math.Max(_nodes[iC].Height, _nodes[iE].Height);
|
|
_nodes[iB].Height = 1 + Math.Max(_nodes[iA].Height, _nodes[iD].Height);
|
|
}
|
|
else
|
|
{
|
|
_nodes[iB].Child2 = iE;
|
|
_nodes[iA].Child1 = iD;
|
|
_nodes[iD].ParentOrNext = iA;
|
|
_nodes[iA].AABB.Combine(ref _nodes[iC].AABB, ref _nodes[iD].AABB);
|
|
_nodes[iB].AABB.Combine(ref _nodes[iA].AABB, ref _nodes[iE].AABB);
|
|
|
|
_nodes[iA].Height = 1 + Math.Max(_nodes[iC].Height, _nodes[iD].Height);
|
|
_nodes[iB].Height = 1 + Math.Max(_nodes[iA].Height, _nodes[iE].Height);
|
|
}
|
|
|
|
return iB;
|
|
}
|
|
|
|
return iA;
|
|
}
|
|
|
|
/// <summary>
|
|
/// Compute the height of a sub-tree.
|
|
/// </summary>
|
|
/// <param name="nodeId">The node id to use as parent.</param>
|
|
/// <returns>The height of the tree.</returns>
|
|
public int ComputeHeight(int nodeId)
|
|
{
|
|
Debug.Assert(0 <= nodeId && nodeId < _nodeCapacity);
|
|
//TreeNode<T>* node = &_nodes[nodeId];
|
|
|
|
if (_nodes[nodeId].IsLeaf())
|
|
{
|
|
return 0;
|
|
}
|
|
|
|
int height1 = ComputeHeight(_nodes[nodeId].Child1);
|
|
int height2 = ComputeHeight(_nodes[nodeId].Child2);
|
|
return 1 + Math.Max(height1, height2);
|
|
}
|
|
|
|
/// <summary>
|
|
/// Compute the height of the entire tree.
|
|
/// </summary>
|
|
/// <returns>The height of the tree.</returns>
|
|
public int ComputeHeight()
|
|
{
|
|
int height = ComputeHeight(_root);
|
|
return height;
|
|
}
|
|
|
|
public void ValidateStructure(int index)
|
|
{
|
|
if (index == NullNode)
|
|
{
|
|
return;
|
|
}
|
|
|
|
if (index == _root)
|
|
{
|
|
Debug.Assert(_nodes[index].ParentOrNext == NullNode);
|
|
}
|
|
|
|
//TreeNode<T>* node = &_nodes[index];
|
|
|
|
int child1 = _nodes[index].Child1;
|
|
int child2 = _nodes[index].Child2;
|
|
|
|
if (_nodes[index].IsLeaf())
|
|
{
|
|
Debug.Assert(child1 == NullNode);
|
|
Debug.Assert(child2 == NullNode);
|
|
Debug.Assert(_nodes[index].Height == 0);
|
|
return;
|
|
}
|
|
|
|
Debug.Assert(0 <= child1 && child1 < _nodeCapacity);
|
|
Debug.Assert(0 <= child2 && child2 < _nodeCapacity);
|
|
|
|
Debug.Assert(_nodes[child1].ParentOrNext == index);
|
|
Debug.Assert(_nodes[child2].ParentOrNext == index);
|
|
|
|
ValidateStructure(child1);
|
|
ValidateStructure(child2);
|
|
}
|
|
|
|
public void ValidateMetrics(int index)
|
|
{
|
|
if (index == NullNode)
|
|
{
|
|
return;
|
|
}
|
|
|
|
//TreeNode<T>* node = &_nodes[index];
|
|
|
|
int child1 = _nodes[index].Child1;
|
|
int child2 = _nodes[index].Child2;
|
|
|
|
if (_nodes[index].IsLeaf())
|
|
{
|
|
Debug.Assert(child1 == NullNode);
|
|
Debug.Assert(child2 == NullNode);
|
|
Debug.Assert(_nodes[index].Height == 0);
|
|
return;
|
|
}
|
|
|
|
Debug.Assert(0 <= child1 && child1 < _nodeCapacity);
|
|
Debug.Assert(0 <= child2 && child2 < _nodeCapacity);
|
|
|
|
int height1 = _nodes[child1].Height;
|
|
int height2 = _nodes[child2].Height;
|
|
int height = 1 + Math.Max(height1, height2);
|
|
Debug.Assert(_nodes[index].Height == height);
|
|
|
|
AABB AABB = new AABB();
|
|
AABB.Combine(ref _nodes[child1].AABB, ref _nodes[child2].AABB);
|
|
|
|
Debug.Assert(AABB.LowerBound == _nodes[index].AABB.LowerBound);
|
|
Debug.Assert(AABB.UpperBound == _nodes[index].AABB.UpperBound);
|
|
|
|
ValidateMetrics(child1);
|
|
ValidateMetrics(child2);
|
|
}
|
|
|
|
/// <summary>
|
|
/// Validate this tree. For testing.
|
|
/// </summary>
|
|
public void Validate()
|
|
{
|
|
ValidateStructure(_root);
|
|
ValidateMetrics(_root);
|
|
|
|
int freeCount = 0;
|
|
int freeIndex = _freeList;
|
|
while (freeIndex != NullNode)
|
|
{
|
|
Debug.Assert(0 <= freeIndex && freeIndex < _nodeCapacity);
|
|
freeIndex = _nodes[freeIndex].ParentOrNext;
|
|
++freeCount;
|
|
}
|
|
|
|
Debug.Assert(Height == ComputeHeight());
|
|
|
|
Debug.Assert(_nodeCount + freeCount == _nodeCapacity);
|
|
}
|
|
|
|
/// <summary>
|
|
/// Build an optimal tree. Very expensive. For testing.
|
|
/// </summary>
|
|
public void RebuildBottomUp()
|
|
{
|
|
int[] nodes = new int[_nodeCount];
|
|
int count = 0;
|
|
|
|
// Build array of leaves. Free the rest.
|
|
for (int i = 0; i < _nodeCapacity; ++i)
|
|
{
|
|
if (_nodes[i].Height < 0)
|
|
{
|
|
// free node in pool
|
|
continue;
|
|
}
|
|
|
|
if (_nodes[i].IsLeaf())
|
|
{
|
|
_nodes[i].ParentOrNext = NullNode;
|
|
nodes[count] = i;
|
|
++count;
|
|
}
|
|
else
|
|
{
|
|
FreeNode(i);
|
|
}
|
|
}
|
|
|
|
while (count > 1)
|
|
{
|
|
float minCost = Settings.MaxFloat;
|
|
int iMin = -1, jMin = -1;
|
|
for (int i = 0; i < count; ++i)
|
|
{
|
|
AABB AABBi = _nodes[nodes[i]].AABB;
|
|
|
|
for (int j = i + 1; j < count; ++j)
|
|
{
|
|
AABB AABBj = _nodes[nodes[j]].AABB;
|
|
AABB b = new AABB();
|
|
b.Combine(ref AABBi, ref AABBj);
|
|
float cost = b.Perimeter;
|
|
if (cost < minCost)
|
|
{
|
|
iMin = i;
|
|
jMin = j;
|
|
minCost = cost;
|
|
}
|
|
}
|
|
}
|
|
|
|
int index1 = nodes[iMin];
|
|
int index2 = nodes[jMin];
|
|
//TreeNode<T>* child1 = &_nodes[index1];
|
|
//TreeNode<T>* child2 = &_nodes[index2];
|
|
|
|
int parentIndex = AllocateNode();
|
|
//TreeNode<T>* parent = &_nodes[parentIndex];
|
|
_nodes[parentIndex].Child1 = index1;
|
|
_nodes[parentIndex].Child2 = index2;
|
|
_nodes[parentIndex].Height = 1 + Math.Max(_nodes[index1].Height, _nodes[index2].Height);
|
|
_nodes[parentIndex].AABB.Combine(ref _nodes[index1].AABB, ref _nodes[index2].AABB);
|
|
_nodes[parentIndex].ParentOrNext = NullNode;
|
|
|
|
_nodes[index1].ParentOrNext = parentIndex;
|
|
_nodes[index2].ParentOrNext = parentIndex;
|
|
|
|
nodes[jMin] = nodes[count - 1];
|
|
nodes[iMin] = parentIndex;
|
|
--count;
|
|
}
|
|
|
|
_root = nodes[0];
|
|
|
|
Validate();
|
|
}
|
|
|
|
/// <summary>
|
|
/// Shift the origin of the nodes
|
|
/// </summary>
|
|
/// <param name="newOrigin">The displacement to use.</param>
|
|
public void ShiftOrigin(Vector2 newOrigin)
|
|
{
|
|
// Build array of leaves. Free the rest.
|
|
for (int i = 0; i < _nodeCapacity; ++i)
|
|
{
|
|
_nodes[i].AABB.LowerBound -= newOrigin;
|
|
_nodes[i].AABB.UpperBound -= newOrigin;
|
|
}
|
|
}
|
|
}
|
|
} |