Allocate memory for a given number of components
| 49 | |
| 50 | // Allocate memory for a given number of components |
| 51 | void HingeJointComponents::allocate(uint32 nbComponentsToAllocate) { |
| 52 | |
| 53 | assert(nbComponentsToAllocate > mNbAllocatedComponents); |
| 54 | |
| 55 | // Make sure capacity is an integral multiple of alignment |
| 56 | nbComponentsToAllocate = std::ceil(nbComponentsToAllocate / float(GLOBAL_ALIGNMENT)) * GLOBAL_ALIGNMENT; |
| 57 | |
| 58 | // Size for the data of a single component (in bytes) |
| 59 | const size_t totalSizeBytes = nbComponentsToAllocate * mComponentDataSize + mAlignmentMarginSize; |
| 60 | |
| 61 | // Allocate memory |
| 62 | void* newBuffer = mMemoryAllocator.allocate(totalSizeBytes); |
| 63 | assert(newBuffer != nullptr); |
| 64 | assert(reinterpret_cast<uintptr_t>(newBuffer) % GLOBAL_ALIGNMENT == 0); |
| 65 | |
| 66 | // New pointers to components data |
| 67 | Entity* newJointEntities = static_cast<Entity*>(newBuffer); |
| 68 | HingeJoint** newJoints = reinterpret_cast<HingeJoint**>(MemoryAllocator::alignAddress(newJointEntities + nbComponentsToAllocate, GLOBAL_ALIGNMENT)); |
| 69 | assert(reinterpret_cast<uintptr_t>(newJoints) % GLOBAL_ALIGNMENT == 0); |
| 70 | Vector3* newLocalAnchorPointBody1 = reinterpret_cast<Vector3*>(MemoryAllocator::alignAddress(newJoints + nbComponentsToAllocate, GLOBAL_ALIGNMENT)); |
| 71 | assert(reinterpret_cast<uintptr_t>(newLocalAnchorPointBody1) % GLOBAL_ALIGNMENT == 0); |
| 72 | Vector3* newLocalAnchorPointBody2 = reinterpret_cast<Vector3*>(MemoryAllocator::alignAddress(newLocalAnchorPointBody1 + nbComponentsToAllocate, GLOBAL_ALIGNMENT)); |
| 73 | assert(reinterpret_cast<uintptr_t>(newLocalAnchorPointBody2) % GLOBAL_ALIGNMENT == 0); |
| 74 | Vector3* newR1World = reinterpret_cast<Vector3*>(MemoryAllocator::alignAddress(newLocalAnchorPointBody2 + nbComponentsToAllocate, GLOBAL_ALIGNMENT)); |
| 75 | assert(reinterpret_cast<uintptr_t>(newR1World) % GLOBAL_ALIGNMENT == 0); |
| 76 | Vector3* newR2World = reinterpret_cast<Vector3*>(MemoryAllocator::alignAddress(newR1World + nbComponentsToAllocate, GLOBAL_ALIGNMENT)); |
| 77 | assert(reinterpret_cast<uintptr_t>(newR2World) % GLOBAL_ALIGNMENT == 0); |
| 78 | Matrix3x3* newI1 = reinterpret_cast<Matrix3x3*>(MemoryAllocator::alignAddress(newR2World + nbComponentsToAllocate, GLOBAL_ALIGNMENT)); |
| 79 | assert(reinterpret_cast<uintptr_t>(newI1) % GLOBAL_ALIGNMENT == 0); |
| 80 | Matrix3x3* newI2 = reinterpret_cast<Matrix3x3*>(MemoryAllocator::alignAddress(newI1 + nbComponentsToAllocate, GLOBAL_ALIGNMENT)); |
| 81 | assert(reinterpret_cast<uintptr_t>(newI2) % GLOBAL_ALIGNMENT == 0); |
| 82 | Vector3* newImpulseTranslation = reinterpret_cast<Vector3*>(MemoryAllocator::alignAddress(newI2 + nbComponentsToAllocate, GLOBAL_ALIGNMENT)); |
| 83 | assert(reinterpret_cast<uintptr_t>(newImpulseTranslation) % GLOBAL_ALIGNMENT == 0); |
| 84 | Vector2* newImpulseRotation = reinterpret_cast<Vector2*>(MemoryAllocator::alignAddress(newImpulseTranslation + nbComponentsToAllocate, GLOBAL_ALIGNMENT)); |
| 85 | assert(reinterpret_cast<uintptr_t>(newImpulseRotation) % GLOBAL_ALIGNMENT == 0); |
| 86 | Matrix3x3* newInverseMassMatrixTranslation = reinterpret_cast<Matrix3x3*>(MemoryAllocator::alignAddress(newImpulseRotation + nbComponentsToAllocate, GLOBAL_ALIGNMENT)); |
| 87 | assert(reinterpret_cast<uintptr_t>(newInverseMassMatrixTranslation) % GLOBAL_ALIGNMENT == 0); |
| 88 | Matrix2x2* newInverseMassMatrixRotation = reinterpret_cast<Matrix2x2*>(MemoryAllocator::alignAddress(newInverseMassMatrixTranslation + nbComponentsToAllocate, GLOBAL_ALIGNMENT)); |
| 89 | assert(reinterpret_cast<uintptr_t>(newInverseMassMatrixRotation) % GLOBAL_ALIGNMENT == 0); |
| 90 | Vector3* newBiasTranslation = reinterpret_cast<Vector3*>(MemoryAllocator::alignAddress(newInverseMassMatrixRotation + nbComponentsToAllocate, GLOBAL_ALIGNMENT)); |
| 91 | assert(reinterpret_cast<uintptr_t>(newBiasTranslation) % GLOBAL_ALIGNMENT == 0); |
| 92 | Vector2* newBiasRotation = reinterpret_cast<Vector2*>(MemoryAllocator::alignAddress(newBiasTranslation + nbComponentsToAllocate, GLOBAL_ALIGNMENT)); |
| 93 | assert(reinterpret_cast<uintptr_t>(newBiasRotation) % GLOBAL_ALIGNMENT == 0); |
| 94 | Quaternion* newInitOrientationDifferenceInv = reinterpret_cast<Quaternion*>(MemoryAllocator::alignAddress(newBiasRotation + nbComponentsToAllocate, GLOBAL_ALIGNMENT)); |
| 95 | assert(reinterpret_cast<uintptr_t>(newInitOrientationDifferenceInv) % GLOBAL_ALIGNMENT == 0); |
| 96 | Vector3* newHingeLocalAxisBody1 = reinterpret_cast<Vector3*>(MemoryAllocator::alignAddress(newInitOrientationDifferenceInv + nbComponentsToAllocate, GLOBAL_ALIGNMENT)); |
| 97 | assert(reinterpret_cast<uintptr_t>(newHingeLocalAxisBody1) % GLOBAL_ALIGNMENT == 0); |
| 98 | Vector3* newHingeLocalAxisBody2 = reinterpret_cast<Vector3*>(MemoryAllocator::alignAddress(newHingeLocalAxisBody1 + nbComponentsToAllocate, GLOBAL_ALIGNMENT)); |
| 99 | assert(reinterpret_cast<uintptr_t>(newHingeLocalAxisBody2) % GLOBAL_ALIGNMENT == 0); |
| 100 | Vector3* newA1 = reinterpret_cast<Vector3*>(MemoryAllocator::alignAddress(newHingeLocalAxisBody2 + nbComponentsToAllocate, GLOBAL_ALIGNMENT)); |
| 101 | assert(reinterpret_cast<uintptr_t>(newA1) % GLOBAL_ALIGNMENT == 0); |
| 102 | Vector3* newB2CrossA1 = reinterpret_cast<Vector3*>(MemoryAllocator::alignAddress(newA1 + nbComponentsToAllocate, GLOBAL_ALIGNMENT)); |
| 103 | assert(reinterpret_cast<uintptr_t>(newB2CrossA1) % GLOBAL_ALIGNMENT == 0); |
| 104 | Vector3* newC2CrossA1 = reinterpret_cast<Vector3*>(MemoryAllocator::alignAddress(newB2CrossA1 + nbComponentsToAllocate, GLOBAL_ALIGNMENT)); |
| 105 | assert(reinterpret_cast<uintptr_t>(newC2CrossA1) % GLOBAL_ALIGNMENT == 0); |
| 106 | decimal* newImpulseLowerLimit = reinterpret_cast<decimal*>(MemoryAllocator::alignAddress(newC2CrossA1 + nbComponentsToAllocate, GLOBAL_ALIGNMENT)); |
| 107 | assert(reinterpret_cast<uintptr_t>(newImpulseLowerLimit) % GLOBAL_ALIGNMENT == 0); |
| 108 | decimal* newImpulseUpperLimit = reinterpret_cast<decimal*>(MemoryAllocator::alignAddress(newImpulseLowerLimit + nbComponentsToAllocate, GLOBAL_ALIGNMENT)); |