// This is called from b2DynamicTree::Query when we are gathering pairs.
bool b2BroadPhase::QueryCallback(int32 proxyId) {
// A proxy cannot form a pair with itself.
if (proxyId == m_queryProxyId) {
return true;
}
// Grow the pair buffer as needed.
if (m_pairCount == m_pairCapacity) {
b2Pair* oldBuffer = m_pairBuffer;
m_pairCapacity *= 2;
m_pairBuffer = (b2Pair*) b2Alloc(m_pairCapacity * sizeof(b2Pair));
memcpy(m_pairBuffer, oldBuffer, m_pairCount * sizeof(b2Pair));
b2Free(oldBuffer);
}
m_pairBuffer[m_pairCount].proxyIdA = b2Min(proxyId, m_queryProxyId);
m_pairBuffer[m_pairCount].proxyIdB = b2Max(proxyId, m_queryProxyId);
++m_pairCount;
return true;
}
void b2ChainShape::CreateLoop(const b2Vec2* vertices, int32 count)
{
b2Assert(m_vertices == NULL && m_count == 0);
b2Assert(count >= 3);
for (int32 i = 1; i < count; ++i)
{
b2Vec2 v1 = vertices[i-1];
b2Vec2 v2 = vertices[i];
// If the code crashes here, it means your vertices are too close together.
b2Assert(b2DistanceSquared(v1, v2) > b2_linearSlop * b2_linearSlop);
}
m_count = count + 1;
m_vertices = (b2Vec2*)b2Alloc(m_count * sizeof(b2Vec2));
memcpy(m_vertices, vertices, count * sizeof(b2Vec2));
m_vertices[count] = m_vertices[0];
m_prevVertex = m_vertices[m_count - 2];
m_nextVertex = m_vertices[1];
m_hasPrevVertex = true;
m_hasNextVertex = true;
}
b2DynamicTree::b2DynamicTree()
{
m_root = b2_nullNode;
m_nodeCapacity = 16;
m_nodeCount = 0;
m_nodes = (b2DynamicTreeNode*)b2Alloc(m_nodeCapacity * sizeof(b2DynamicTreeNode));
memset(m_nodes, 0, m_nodeCapacity * sizeof(b2DynamicTreeNode));
// Build a linked list for the free list.
for (int32 i = 0; i < m_nodeCapacity - 1; ++i)
{
m_nodes[i].next = i + 1;
}
m_nodes[m_nodeCapacity-1].next = b2_nullNode;
m_freeList = 0;
m_path = 0;
m_insertionCount = 0;
}
// Allocate a node from the pool. Grow the pool if necessary.
uint16 b2DynamicTree::AllocateNode()
{
// Peel a node off the free list.
if (m_freeList != b2_nullNode)
{
uint16 node = m_freeList;
m_freeList = m_nodes[node].parent;
m_nodes[node].parent = b2_nullNode;
m_nodes[node].child1 = b2_nullNode;
m_nodes[node].child2 = b2_nullNode;
return node;
}
// The free list is empty. Rebuild a bigger pool.
int32 newPoolCount = b2Min(2 * m_nodeCount, USHRT_MAX - 1);
b2Assert(newPoolCount > m_nodeCount);
b2DynamicTreeNode* newPool = (b2DynamicTreeNode*)b2Alloc(newPoolCount * sizeof(b2DynamicTreeNode));
memcpy(newPool, m_nodes, m_nodeCount * sizeof(b2DynamicTreeNode));
memset(newPool + m_nodeCount, 0, (newPoolCount - m_nodeCount) * sizeof(b2DynamicTreeNode));
// Build a linked list for the free list. The parent
// pointer becomes the "next" pointer.
for (int32 i = m_nodeCount; i < newPoolCount - 1; ++i)
{
newPool[i].parent = uint16(i + 1);
}
newPool[newPoolCount-1].parent = b2_nullNode;
m_freeList = uint16(m_nodeCount);
b2Free(m_nodes);
m_nodes = newPool;
m_nodeCount = newPoolCount;
// Finally peel a node off the new free list.
uint16 node = m_freeList;
m_freeList = m_nodes[node].parent;
return node;
}
// Allocate a node from the pool. Grow the pool if necessary.
int32 b2DynamicTree::AllocateNode()
{
// Expand the node pool as needed.
if (m_freeList == b2_nullNode)
{
b2Assert(m_nodeCount == m_nodeCapacity);
// The free list is empty. Rebuild a bigger pool.
b2TreeNode* oldNodes = m_nodes;
m_nodeCapacity *= 2;
m_nodes = (b2TreeNode*)b2Alloc(m_nodeCapacity * sizeof(b2TreeNode));
memcpy(m_nodes, oldNodes, m_nodeCount * sizeof(b2TreeNode));
b2Free(oldNodes);
// Build a linked list for the free list. The parent
// pointer becomes the "next" pointer.
for (int32 i = m_nodeCount; i < m_nodeCapacity - 1; ++i)
{
m_nodes[i].next = i + 1;
m_nodes[i].height = -1;
}
m_nodes[m_nodeCapacity-1].next = b2_nullNode;
m_nodes[m_nodeCapacity-1].height = -1;
m_freeList = m_nodeCount;
}
// Peel a node off the free list.
int32 nodeId = m_freeList;
m_freeList = m_nodes[nodeId].next;
m_nodes[nodeId].parent = b2_nullNode;
m_nodes[nodeId].child1 = b2_nullNode;
m_nodes[nodeId].child2 = b2_nullNode;
m_nodes[nodeId].height = 0;
m_nodes[nodeId].userData = NULL;
++m_nodeCount;
return nodeId;
}
void* b2StackAllocator::Allocate(int32 size)
{
b2Assert(m_entryCount < b2_maxStackEntries);
b2StackEntry* entry = m_entries + m_entryCount;
entry->size = size;
if (m_index + size > b2_stackSize)
{
entry->data = (char*)b2Alloc(size);
entry->usedMalloc = true;
}
else
{
entry->data = m_data + m_index;
entry->usedMalloc = false;
m_index += size;
}
m_allocation += size;
m_maxAllocation = b2Max(m_maxAllocation, m_allocation);
++m_entryCount;
return entry->data;
}
void b2ChainShape::CreateChain(const b2Vec2* vertices, int32 count)
{
b2Assert(m_vertices == NULL && m_count == 0);
b2Assert(count >= 2);
for (int32 i = 1; i < count; ++i)
{
#if B2_ASSERT_ENABLED
b2Vec2 v1 = vertices[i-1];
b2Vec2 v2 = vertices[i];
// If the code crashes here, it means your vertices are too close together.
b2Assert(b2DistanceSquared(v1, v2) > b2_linearSlop * b2_linearSlop);
#endif // B2_ASSERT_ENABLED
}
m_count = count;
m_vertices = (b2Vec2*)b2Alloc(count * sizeof(b2Vec2));
memcpy(m_vertices, vertices, m_count * sizeof(b2Vec2));
m_hasPrevVertex = false;
m_hasNextVertex = false;
m_prevVertex.SetZero();
m_nextVertex.SetZero();
}
void b2DynamicTree::RebuildBottomUp()
{
int32* nodes = (int32*)b2Alloc(m_nodeCount * sizeof(int32));
int32 count = 0;
// Build array of leaves. Free the rest.
for (int32 i = 0; i < m_nodeCapacity; ++i)
{
if (m_nodes[i].height < 0)
{
// free node in pool
continue;
}
if (m_nodes[i].IsLeaf())
{
m_nodes[i].parent = b2_nullNode;
nodes[count] = i;
++count;
}
else
{
FreeNode(i);
}
}
while (count > 1)
{
float32 minCost = b2_maxFloat;
int32 iMin = -1, jMin = -1;
for (int32 i = 0; i < count; ++i)
{
b2AABB aabbi = m_nodes[nodes[i]].aabb;
for (int32 j = i + 1; j < count; ++j)
{
b2AABB aabbj = m_nodes[nodes[j]].aabb;
b2AABB b;
b.Combine(aabbi, aabbj);
float32 cost = b.GetPerimeter();
if (cost < minCost)
{
iMin = i;
jMin = j;
minCost = cost;
}
}
}
int32 index1 = nodes[iMin];
int32 index2 = nodes[jMin];
b2TreeNode* child1 = m_nodes + index1;
b2TreeNode* child2 = m_nodes + index2;
int32 parentIndex = AllocateNode();
b2TreeNode* parent = m_nodes + parentIndex;
parent->child1 = index1;
parent->child2 = index2;
parent->height = 1 + b2Max(child1->height, child2->height);
parent->aabb.Combine(child1->aabb, child2->aabb);
parent->parent = b2_nullNode;
child1->parent = parentIndex;
child2->parent = parentIndex;
nodes[jMin] = nodes[count-1];
nodes[iMin] = parentIndex;
--count;
}
m_root = nodes[0];
b2Free(nodes);
Validate();
}