b2BlockAllocator::~b2BlockAllocator()
{
for (int32 i = 0; i < m_chunkCount; ++i)
{
b2Free(m_chunks[i].blocks);
}
b2Free(m_chunks);
}
void* b2StackAllocator::Reallocate(void* p, int32 size)
{
b2Assert(m_entryCount > 0);
b2StackEntry* entry = m_entries + m_entryCount - 1;
b2Assert(p == entry->data);
B2_NOT_USED(p);
int32 incrementSize = size - entry->size;
if (incrementSize > 0)
{
if (entry->usedMalloc)
{
void* data = b2Alloc(size);
memcpy(data, entry->data, entry->size);
b2Free(entry->data);
entry->data = (char*)data;
}
else if (m_index + incrementSize > b2_stackSize)
{
void* data = b2Alloc(size);
memcpy(data, entry->data, entry->size);
m_index -= entry->size;
entry->data = (char*)data;
entry->usedMalloc = true;
}
else
{
m_index += incrementSize;
m_allocation += incrementSize;
m_maxAllocation = b2Max(m_maxAllocation, m_allocation);
}
entry->size = size;
}
return entry->data;
}
// 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.
b2DynamicTreeNode* oldNodes = m_nodes;
m_nodeCapacity *= 2;
m_nodes = (b2DynamicTreeNode*)b2Alloc(m_nodeCapacity * sizeof(b2DynamicTreeNode));
memcpy(m_nodes, oldNodes, m_nodeCount * sizeof(b2DynamicTreeNode));
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[m_nodeCapacity-1].next = b2_nullNode;
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_nodeCount;
return nodeId;
}
void* b2BlockAllocator::Allocate(int32 size)
{
if (size == 0)
return NULL;
b2Assert(0 < size);
if (size > b2_maxBlockSize)
{
return m_giants.Allocate(size);
}
int32 index = s_blockSizeLookup[size];
b2Assert(0 <= index && index < b2_blockSizes);
if (m_freeLists[index])
{
b2Block* block = m_freeLists[index];
m_freeLists[index] = block->next;
return block;
}
else
{
if (m_chunkCount == m_chunkSpace)
{
b2Chunk* oldChunks = m_chunks;
m_chunkSpace += b2_chunkArrayIncrement;
m_chunks = (b2Chunk*)b2Alloc(m_chunkSpace * sizeof(b2Chunk));
memcpy(m_chunks, oldChunks, m_chunkCount * sizeof(b2Chunk));
memset(m_chunks + m_chunkCount, 0, b2_chunkArrayIncrement * sizeof(b2Chunk));
b2Free(oldChunks);
}
b2Chunk* chunk = m_chunks + m_chunkCount;
chunk->blocks = (b2Block*)b2Alloc(b2_chunkSize);
#if DEBUG
memset(chunk->blocks, 0xcd, b2_chunkSize);
#endif
int32 blockSize = s_blockSizes[index];
chunk->blockSize = blockSize;
int32 blockCount = b2_chunkSize / blockSize;
b2Assert(blockCount * blockSize <= b2_chunkSize);
for (int32 i = 0; i < blockCount - 1; ++i)
{
b2Block* block = (b2Block*)((int8*)chunk->blocks + blockSize * i);
b2Block* next = (b2Block*)((int8*)chunk->blocks + blockSize * (i + 1));
block->next = next;
}
b2Block* last = (b2Block*)((int8*)chunk->blocks + blockSize * (blockCount - 1));
last->next = NULL;
m_freeLists[index] = chunk->blocks->next;
++m_chunkCount;
return chunk->blocks;
}
}
void b2BlockAllocator::Clear()
{
for (int32 i = 0; i < m_chunkCount; ++i)
{
b2Free(m_chunks[i].blocks);
}
m_chunkCount = 0;
memset(m_chunks, 0, m_chunkSpace * sizeof(b2Chunk));
memset(m_freeLists, 0, sizeof(m_freeLists));
}
void b2BroadPhase::BufferMove(int32 proxyId) {
if (m_moveCount == m_moveCapacity) {
int32* oldBuffer = m_moveBuffer;
m_moveCapacity *= 2;
m_moveBuffer = (int32*) b2Alloc(m_moveCapacity * sizeof(int32));
memcpy(m_moveBuffer, oldBuffer, m_moveCount * sizeof(int32));
b2Free(oldBuffer);
}
m_moveBuffer[m_moveCount] = proxyId;
++m_moveCount;
}
void b2StackAllocator::Free(void* p) {
b2Assert(m_entryCount > 0);
b2StackEntry* entry = m_entries + m_entryCount - 1;
b2Assert(p == entry->data);
if (entry->usedMalloc) {
b2Free(p);
} else {
m_index -= entry->size;
}
m_allocation -= entry->size;
--m_entryCount;
p = NULL;
}
b2Rope::~b2Rope()
{
b2Free(m_ps);
b2Free(m_p0s);
b2Free(m_vs);
b2Free(m_ims);
b2Free(m_Ls);
b2Free(m_as);
}
void b2BlockAllocator::Free(void* p, int32 size)
{
if (size == 0)
{
return;
}
b2Assert(0 < size);
if (size > b2_maxBlockSize)
{
b2Free(p);
return;
}
int32 index = s_blockSizeLookup[size];
b2Assert(0 <= index && index < b2_blockSizes);
#ifdef _DEBUG
// Verify the memory address and size is valid.
int32 blockSize = s_blockSizes[index];
bool found = false;
for (int32 i = 0; i < m_chunkCount; ++i)
{
b2Chunk* chunk = m_chunks + i;
if (chunk->blockSize != blockSize)
{
b2Assert( (int8*)p + blockSize <= (int8*)chunk->blocks ||
(int8*)chunk->blocks + b2_chunkSize <= (int8*)p);
}
else
{
if ((int8*)chunk->blocks <= (int8*)p && (int8*)p + blockSize <= (int8*)chunk->blocks + b2_chunkSize)
{
found = true;
}
}
}
b2Assert(found);
memset(p, 0xfd, blockSize);
#endif
b2Block* block = (b2Block*)p;
block->next = m_freeLists[index];
m_freeLists[index] = block;
}
// 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;
}
// 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;
}
void b2ChainShape::Clear()
{
b2Free(m_vertices);
m_vertices = NULL;
m_count = 0;
}
b2ChainShape::~b2ChainShape()
{
b2Free(m_vertices);
m_vertices = NULL;
m_count = 0;
}
b2BroadPhase::~b2BroadPhase()
{
b2Free(m_moveBuffer);
b2Free(m_pairBuffer);
}
b2World::~b2World()
{
DestroyBody(m_groundBody);
m_broadPhase->~b2BroadPhase();
b2Free(m_broadPhase);
}
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();
}
b2DynamicTree::~b2DynamicTree()
{
// This frees the entire tree in one shot.
b2Free(m_nodes);
}
/// Free a b2TrackedBlock.
void b2TrackedBlock::Free(b2TrackedBlock *block)
{
b2Assert(block);
block->~b2TrackedBlock();
b2Free(block);
}
