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