Allocate memory for a given number of components
| 44 | |
| 45 | // Allocate memory for a given number of components |
| 46 | void FixedJointComponents::allocate(uint32 nbComponentsToAllocate) { |
| 47 | |
| 48 | assert(nbComponentsToAllocate > mNbAllocatedComponents); |
| 49 | |
| 50 | // Make sure capacity is an integral multiple of alignment |
| 51 | nbComponentsToAllocate = std::ceil(nbComponentsToAllocate / float(GLOBAL_ALIGNMENT)) * GLOBAL_ALIGNMENT; |
| 52 | |
| 53 | // Size for the data of a single component (in bytes) |
| 54 | const size_t totalSizeBytes = nbComponentsToAllocate * mComponentDataSize + mAlignmentMarginSize; |
| 55 | |
| 56 | // Allocate memory |
| 57 | void* newBuffer = mMemoryAllocator.allocate(totalSizeBytes); |
| 58 | assert(newBuffer != nullptr); |
| 59 | assert(reinterpret_cast<uintptr_t>(newBuffer) % GLOBAL_ALIGNMENT == 0); |
| 60 | |
| 61 | // New pointers to components data |
| 62 | Entity* newJointEntities = static_cast<Entity*>(newBuffer); |
| 63 | FixedJoint** newJoints = reinterpret_cast<FixedJoint**>(MemoryAllocator::alignAddress(newJointEntities + nbComponentsToAllocate, GLOBAL_ALIGNMENT)); |
| 64 | assert(reinterpret_cast<uintptr_t>(newJoints) % GLOBAL_ALIGNMENT == 0); |
| 65 | Vector3* newLocalAnchorPointBody1 = reinterpret_cast<Vector3*>(MemoryAllocator::alignAddress(newJoints + nbComponentsToAllocate, GLOBAL_ALIGNMENT)); |
| 66 | assert(reinterpret_cast<uintptr_t>(newLocalAnchorPointBody1) % GLOBAL_ALIGNMENT == 0); |
| 67 | Vector3* newLocalAnchorPointBody2 = reinterpret_cast<Vector3*>(MemoryAllocator::alignAddress(newLocalAnchorPointBody1 + nbComponentsToAllocate, GLOBAL_ALIGNMENT)); |
| 68 | assert(reinterpret_cast<uintptr_t>(newLocalAnchorPointBody2) % GLOBAL_ALIGNMENT == 0); |
| 69 | Vector3* newR1World = reinterpret_cast<Vector3*>(MemoryAllocator::alignAddress(newLocalAnchorPointBody2 + nbComponentsToAllocate, GLOBAL_ALIGNMENT)); |
| 70 | assert(reinterpret_cast<uintptr_t>(newR1World) % GLOBAL_ALIGNMENT == 0); |
| 71 | Vector3* newR2World = reinterpret_cast<Vector3*>(MemoryAllocator::alignAddress(newR1World + nbComponentsToAllocate, GLOBAL_ALIGNMENT)); |
| 72 | assert(reinterpret_cast<uintptr_t>(newR2World) % GLOBAL_ALIGNMENT == 0); |
| 73 | Matrix3x3* newI1 = reinterpret_cast<Matrix3x3*>(MemoryAllocator::alignAddress(newR2World + nbComponentsToAllocate, GLOBAL_ALIGNMENT)); |
| 74 | assert(reinterpret_cast<uintptr_t>(newI1) % GLOBAL_ALIGNMENT == 0); |
| 75 | Matrix3x3* newI2 = reinterpret_cast<Matrix3x3*>(MemoryAllocator::alignAddress(newI1 + nbComponentsToAllocate, GLOBAL_ALIGNMENT)); |
| 76 | assert(reinterpret_cast<uintptr_t>(newI1) % GLOBAL_ALIGNMENT == 0); |
| 77 | Vector3* newImpulseTranslation = reinterpret_cast<Vector3*>(MemoryAllocator::alignAddress(newI2 + nbComponentsToAllocate, GLOBAL_ALIGNMENT)); |
| 78 | assert(reinterpret_cast<uintptr_t>(newImpulseTranslation) % GLOBAL_ALIGNMENT == 0); |
| 79 | Vector3* newImpulseRotation = reinterpret_cast<Vector3*>(MemoryAllocator::alignAddress(newImpulseTranslation + nbComponentsToAllocate, GLOBAL_ALIGNMENT)); |
| 80 | assert(reinterpret_cast<uintptr_t>(newImpulseRotation) % GLOBAL_ALIGNMENT == 0); |
| 81 | Matrix3x3* newInverseMassMatrixTranslation = reinterpret_cast<Matrix3x3*>(MemoryAllocator::alignAddress(newImpulseRotation + nbComponentsToAllocate, GLOBAL_ALIGNMENT)); |
| 82 | assert(reinterpret_cast<uintptr_t>(newInverseMassMatrixTranslation) % GLOBAL_ALIGNMENT == 0); |
| 83 | Matrix3x3* newInverseMassMatrixRotation = reinterpret_cast<Matrix3x3*>(MemoryAllocator::alignAddress(newInverseMassMatrixTranslation + nbComponentsToAllocate, GLOBAL_ALIGNMENT)); |
| 84 | assert(reinterpret_cast<uintptr_t>(newInverseMassMatrixRotation) % GLOBAL_ALIGNMENT == 0); |
| 85 | Vector3* newBiasTranslation = reinterpret_cast<Vector3*>(MemoryAllocator::alignAddress(newInverseMassMatrixRotation + nbComponentsToAllocate, GLOBAL_ALIGNMENT)); |
| 86 | assert(reinterpret_cast<uintptr_t>(newBiasTranslation) % GLOBAL_ALIGNMENT == 0); |
| 87 | Vector3* newBiasRotation = reinterpret_cast<Vector3*>(MemoryAllocator::alignAddress(newBiasTranslation + nbComponentsToAllocate, GLOBAL_ALIGNMENT)); |
| 88 | assert(reinterpret_cast<uintptr_t>(newBiasRotation) % GLOBAL_ALIGNMENT == 0); |
| 89 | Quaternion* newInitOrientationDifferenceInv = reinterpret_cast<Quaternion*>(MemoryAllocator::alignAddress(newBiasRotation + nbComponentsToAllocate, GLOBAL_ALIGNMENT)); |
| 90 | assert(reinterpret_cast<uintptr_t>(newInitOrientationDifferenceInv) % GLOBAL_ALIGNMENT == 0); |
| 91 | assert(reinterpret_cast<uintptr_t>(newInitOrientationDifferenceInv + nbComponentsToAllocate) <= reinterpret_cast<uintptr_t>(newBuffer) + totalSizeBytes); |
| 92 | |
| 93 | // If there was already components before |
| 94 | if (mNbComponents > 0) { |
| 95 | |
| 96 | // Copy component data from the previous buffer to the new one |
| 97 | memcpy(newJointEntities, mJointEntities, mNbComponents * sizeof(Entity)); |
| 98 | memcpy(newJoints, mJoints, mNbComponents * sizeof(FixedJoint*)); |
| 99 | memcpy(newLocalAnchorPointBody1, mLocalAnchorPointBody1, mNbComponents * sizeof(Vector3)); |
| 100 | memcpy(newLocalAnchorPointBody2, mLocalAnchorPointBody2, mNbComponents * sizeof(Vector3)); |
| 101 | memcpy(newR1World, mR1World, mNbComponents * sizeof(Vector3)); |
| 102 | memcpy(newR2World, mR2World, mNbComponents * sizeof(Vector3)); |
| 103 | memcpy(newI1, mI1, mNbComponents * sizeof(Matrix3x3)); |