From 0d1861029880e2273cd7a9cf9a54bba56e9a2018 Mon Sep 17 00:00:00 2001 From: Daniel Chappuis Date: Sun, 27 Mar 2022 13:26:45 +0200 Subject: [PATCH] Make sure the allocated memory for components is properly aligned --- .../reactphysics3d/components/Components.h | 8 +- .../BallAndSocketJointComponents.cpp | 61 ++++++--- src/components/ColliderComponents.cpp | 44 +++--- src/components/CollisionBodyComponents.cpp | 21 +-- src/components/Components.cpp | 11 +- src/components/FixedJointComponents.cpp | 50 ++++--- src/components/HingeJointComponents.cpp | 110 +++++++++------ src/components/JointComponents.cpp | 29 ++-- src/components/RigidBodyComponents.cpp | 98 +++++++++----- src/components/SliderJointComponents.cpp | 125 ++++++++++++------ src/components/TransformComponents.cpp | 16 ++- src/engine/PhysicsWorld.cpp | 10 ++ 12 files changed, 384 insertions(+), 199 deletions(-) diff --git a/include/reactphysics3d/components/Components.h b/include/reactphysics3d/components/Components.h index 17dfa001..80fc2e4a 100644 --- a/include/reactphysics3d/components/Components.h +++ b/include/reactphysics3d/components/Components.h @@ -63,6 +63,9 @@ class Components { /// Size (in bytes) of a single component size_t mComponentDataSize; + /// Size (in bytes) to allocate to make sure we can offset the components array to keep alignment + size_t mAlignmentMarginSize; + /// Number of allocated components uint32 mNbAllocatedComponents; @@ -96,11 +99,14 @@ class Components { // -------------------- Methods -------------------- // /// Constructor - Components(MemoryAllocator& allocator, size_t componentDataSize); + Components(MemoryAllocator& allocator, size_t componentDataSize, size_t alignmentMarginSize); /// Destructor virtual ~Components(); + /// Initialize the components: + void init(); + /// Remove a component void removeComponent(Entity entity); diff --git a/src/components/BallAndSocketJointComponents.cpp b/src/components/BallAndSocketJointComponents.cpp index 6f843e78..4a429bb7 100644 --- a/src/components/BallAndSocketJointComponents.cpp +++ b/src/components/BallAndSocketJointComponents.cpp @@ -38,10 +38,9 @@ BallAndSocketJointComponents::BallAndSocketJointComponents(MemoryAllocator& allo sizeof(Vector3) + sizeof(Vector3) + sizeof(Vector3) + sizeof(Matrix3x3) + sizeof(Matrix3x3) + sizeof(Vector3) + sizeof(Matrix3x3) + sizeof(Vector3) + sizeof(bool) + sizeof(decimal) + - sizeof(decimal) + sizeof(decimal) + sizeof(decimal) + sizeof(bool) + sizeof(Vector3)) { + sizeof(decimal) + sizeof(decimal) + sizeof(decimal) + sizeof(bool) + sizeof(Vector3), + 18 * GLOBAL_ALIGNMENT) { - // Allocate memory for the components data - allocate(INIT_NB_ALLOCATED_COMPONENTS); } // Allocate memory for a given number of components @@ -50,31 +49,51 @@ void BallAndSocketJointComponents::allocate(uint32 nbComponentsToAllocate) { assert(nbComponentsToAllocate > mNbAllocatedComponents); // Size for the data of a single component (in bytes) - const size_t totalSizeBytes = nbComponentsToAllocate * mComponentDataSize; + const size_t totalSizeBytes = nbComponentsToAllocate * mComponentDataSize + mAlignmentMarginSize; // Allocate memory void* newBuffer = mMemoryAllocator.allocate(totalSizeBytes); assert(newBuffer != nullptr); + assert(reinterpret_cast(newBuffer) % GLOBAL_ALIGNMENT == 0); // New pointers to components data Entity* newJointEntities = static_cast(newBuffer); - BallAndSocketJoint** newJoints = reinterpret_cast(newJointEntities + nbComponentsToAllocate); - Vector3* newLocalAnchorPointBody1 = reinterpret_cast(newJoints + nbComponentsToAllocate); - Vector3* newLocalAnchorPointBody2 = reinterpret_cast(newLocalAnchorPointBody1 + nbComponentsToAllocate); - Vector3* newR1World = reinterpret_cast(newLocalAnchorPointBody2 + nbComponentsToAllocate); - Vector3* newR2World = reinterpret_cast(newR1World + nbComponentsToAllocate); - Matrix3x3* newI1 = reinterpret_cast(newR2World + nbComponentsToAllocate); - Matrix3x3* newI2 = reinterpret_cast(newI1 + nbComponentsToAllocate); - Vector3* newBiasVector = reinterpret_cast(newI2 + nbComponentsToAllocate); - Matrix3x3* newInverseMassMatrix = reinterpret_cast(newBiasVector + nbComponentsToAllocate); - Vector3* newImpulse = reinterpret_cast(newInverseMassMatrix + nbComponentsToAllocate); - bool* newIsConeLimitEnabled = reinterpret_cast(newImpulse + nbComponentsToAllocate); - decimal* newConeLimitImpulse = reinterpret_cast(newIsConeLimitEnabled + nbComponentsToAllocate); - decimal* newConeLimitHalfAngle = reinterpret_cast(newConeLimitImpulse + nbComponentsToAllocate); - decimal* newInverseMassMatrixConeLimit = reinterpret_cast(newConeLimitHalfAngle + nbComponentsToAllocate); - decimal* newBConeLimit = reinterpret_cast(newInverseMassMatrixConeLimit + nbComponentsToAllocate); - bool* newIsConeLimitViolated = reinterpret_cast(newBConeLimit + nbComponentsToAllocate); - Vector3* newConeLimitACrossB = reinterpret_cast(newIsConeLimitViolated + nbComponentsToAllocate); + assert(reinterpret_cast(newJointEntities) % GLOBAL_ALIGNMENT == 0); + BallAndSocketJoint** newJoints = reinterpret_cast(MemoryAllocator::alignAddress(newJointEntities + nbComponentsToAllocate, GLOBAL_ALIGNMENT)); + assert(reinterpret_cast(newJoints) % GLOBAL_ALIGNMENT == 0); + Vector3* newLocalAnchorPointBody1 = reinterpret_cast(MemoryAllocator::alignAddress(newJoints + nbComponentsToAllocate, GLOBAL_ALIGNMENT)); + assert(reinterpret_cast(newLocalAnchorPointBody1) % GLOBAL_ALIGNMENT == 0); + Vector3* newLocalAnchorPointBody2 = reinterpret_cast(MemoryAllocator::alignAddress(newLocalAnchorPointBody1 + nbComponentsToAllocate, GLOBAL_ALIGNMENT)); + assert(reinterpret_cast(newLocalAnchorPointBody2) % GLOBAL_ALIGNMENT == 0); + Vector3* newR1World = reinterpret_cast(MemoryAllocator::alignAddress(newLocalAnchorPointBody2 + nbComponentsToAllocate, GLOBAL_ALIGNMENT)); + assert(reinterpret_cast(newR1World) % GLOBAL_ALIGNMENT == 0); + Vector3* newR2World = reinterpret_cast(MemoryAllocator::alignAddress(newR1World + nbComponentsToAllocate, GLOBAL_ALIGNMENT)); + assert(reinterpret_cast(newR2World) % GLOBAL_ALIGNMENT == 0); + Matrix3x3* newI1 = reinterpret_cast(MemoryAllocator::alignAddress(newR2World + nbComponentsToAllocate, GLOBAL_ALIGNMENT)); + assert(reinterpret_cast(newI1) % GLOBAL_ALIGNMENT == 0); + Matrix3x3* newI2 = reinterpret_cast(MemoryAllocator::alignAddress(newI1 + nbComponentsToAllocate, GLOBAL_ALIGNMENT)); + assert(reinterpret_cast(newI2) % GLOBAL_ALIGNMENT == 0); + Vector3* newBiasVector = reinterpret_cast(MemoryAllocator::alignAddress(newI2 + nbComponentsToAllocate, GLOBAL_ALIGNMENT)); + assert(reinterpret_cast(newBiasVector) % GLOBAL_ALIGNMENT == 0); + Matrix3x3* newInverseMassMatrix = reinterpret_cast(MemoryAllocator::alignAddress(newBiasVector + nbComponentsToAllocate, GLOBAL_ALIGNMENT)); + assert(reinterpret_cast(newInverseMassMatrix) % GLOBAL_ALIGNMENT == 0); + Vector3* newImpulse = reinterpret_cast(MemoryAllocator::alignAddress(newInverseMassMatrix + nbComponentsToAllocate, GLOBAL_ALIGNMENT)); + assert(reinterpret_cast(newImpulse) % GLOBAL_ALIGNMENT == 0); + bool* newIsConeLimitEnabled = reinterpret_cast(MemoryAllocator::alignAddress(newImpulse + nbComponentsToAllocate, GLOBAL_ALIGNMENT)); + assert(reinterpret_cast(newIsConeLimitEnabled) % GLOBAL_ALIGNMENT == 0); + decimal* newConeLimitImpulse = reinterpret_cast(MemoryAllocator::alignAddress(newIsConeLimitEnabled + nbComponentsToAllocate, GLOBAL_ALIGNMENT)); + assert(reinterpret_cast(newConeLimitImpulse) % GLOBAL_ALIGNMENT == 0); + decimal* newConeLimitHalfAngle = reinterpret_cast(MemoryAllocator::alignAddress(newConeLimitImpulse + nbComponentsToAllocate, GLOBAL_ALIGNMENT)); + assert(reinterpret_cast(newConeLimitHalfAngle) % GLOBAL_ALIGNMENT == 0); + decimal* newInverseMassMatrixConeLimit = reinterpret_cast(MemoryAllocator::alignAddress(newConeLimitHalfAngle + nbComponentsToAllocate, GLOBAL_ALIGNMENT)); + assert(reinterpret_cast(newInverseMassMatrixConeLimit) % GLOBAL_ALIGNMENT == 0); + decimal* newBConeLimit = reinterpret_cast(MemoryAllocator::alignAddress(newInverseMassMatrixConeLimit + nbComponentsToAllocate, GLOBAL_ALIGNMENT)); + assert(reinterpret_cast(newBConeLimit) % GLOBAL_ALIGNMENT == 0); + bool* newIsConeLimitViolated = reinterpret_cast(MemoryAllocator::alignAddress(newBConeLimit + nbComponentsToAllocate, GLOBAL_ALIGNMENT)); + assert(reinterpret_cast(newIsConeLimitViolated) % GLOBAL_ALIGNMENT == 0); + Vector3* newConeLimitACrossB = reinterpret_cast(MemoryAllocator::alignAddress(newIsConeLimitViolated + nbComponentsToAllocate, GLOBAL_ALIGNMENT)); + assert(reinterpret_cast(newConeLimitACrossB) % GLOBAL_ALIGNMENT == 0); + assert(reinterpret_cast(newConeLimitACrossB + nbComponentsToAllocate) <= reinterpret_cast(newBuffer) + totalSizeBytes); // If there was already components before if (mNbComponents > 0) { diff --git a/src/components/ColliderComponents.cpp b/src/components/ColliderComponents.cpp index 51e1b8dc..dbc16e64 100644 --- a/src/components/ColliderComponents.cpp +++ b/src/components/ColliderComponents.cpp @@ -38,10 +38,8 @@ ColliderComponents::ColliderComponents(MemoryAllocator& allocator) :Components(allocator, sizeof(Entity) + sizeof(Entity) + sizeof(Collider*) + sizeof(int32) + sizeof(Transform) + sizeof(CollisionShape*) + sizeof(unsigned short) + sizeof(unsigned short) + sizeof(Transform) + sizeof(Array) + sizeof(bool) + - sizeof(bool) + sizeof(Material)) { + sizeof(bool) + sizeof(Material), 13 * GLOBAL_ALIGNMENT) { - // Allocate memory for the components data - allocate(INIT_NB_ALLOCATED_COMPONENTS); } // Allocate memory for a given number of components @@ -50,26 +48,40 @@ void ColliderComponents::allocate(uint32 nbComponentsToAllocate) { assert(nbComponentsToAllocate > mNbAllocatedComponents); // Size for the data of a single component (in bytes) - const size_t totalSizeBytes = nbComponentsToAllocate * mComponentDataSize; + const size_t totalSizeBytes = nbComponentsToAllocate * mComponentDataSize + mAlignmentMarginSize; // Allocate memory void* newBuffer = mMemoryAllocator.allocate(totalSizeBytes); assert(newBuffer != nullptr); + assert(reinterpret_cast(newBuffer) % GLOBAL_ALIGNMENT == 0); // New pointers to components data Entity* newCollidersEntities = static_cast(newBuffer); - Entity* newBodiesEntities = reinterpret_cast(newCollidersEntities + nbComponentsToAllocate); - Collider** newColliders = reinterpret_cast(newBodiesEntities + nbComponentsToAllocate); - int32* newBroadPhaseIds = reinterpret_cast(newColliders + nbComponentsToAllocate); - Transform* newLocalToBodyTransforms = reinterpret_cast(newBroadPhaseIds + nbComponentsToAllocate); - CollisionShape** newCollisionShapes = reinterpret_cast(newLocalToBodyTransforms + nbComponentsToAllocate); - unsigned short* newCollisionCategoryBits = reinterpret_cast(newCollisionShapes + nbComponentsToAllocate); - unsigned short* newCollideWithMaskBits = reinterpret_cast(newCollisionCategoryBits + nbComponentsToAllocate); - Transform* newLocalToWorldTransforms = reinterpret_cast(newCollideWithMaskBits + nbComponentsToAllocate); - Array* newOverlappingPairs = reinterpret_cast*>(newLocalToWorldTransforms + nbComponentsToAllocate); - bool* hasCollisionShapeChangedSize = reinterpret_cast(newOverlappingPairs + nbComponentsToAllocate); - bool* isTrigger = reinterpret_cast(hasCollisionShapeChangedSize + nbComponentsToAllocate); - Material* materials = reinterpret_cast(isTrigger + nbComponentsToAllocate); + Entity* newBodiesEntities = reinterpret_cast(MemoryAllocator::alignAddress(newCollidersEntities + nbComponentsToAllocate, GLOBAL_ALIGNMENT)); + assert(reinterpret_cast(newBodiesEntities) % GLOBAL_ALIGNMENT == 0); + Collider** newColliders = reinterpret_cast(MemoryAllocator::alignAddress(newBodiesEntities + nbComponentsToAllocate, GLOBAL_ALIGNMENT)); + assert(reinterpret_cast(newColliders) % GLOBAL_ALIGNMENT == 0); + int32* newBroadPhaseIds = reinterpret_cast(MemoryAllocator::alignAddress(newColliders + nbComponentsToAllocate, GLOBAL_ALIGNMENT)); + assert(reinterpret_cast(newBroadPhaseIds) % GLOBAL_ALIGNMENT == 0); + Transform* newLocalToBodyTransforms = reinterpret_cast(MemoryAllocator::alignAddress(newBroadPhaseIds + nbComponentsToAllocate, GLOBAL_ALIGNMENT)); + assert(reinterpret_cast(newLocalToBodyTransforms) % GLOBAL_ALIGNMENT == 0); + CollisionShape** newCollisionShapes = reinterpret_cast(MemoryAllocator::alignAddress(newLocalToBodyTransforms + nbComponentsToAllocate, GLOBAL_ALIGNMENT)); + assert(reinterpret_cast(newCollisionShapes) % GLOBAL_ALIGNMENT == 0); + unsigned short* newCollisionCategoryBits = reinterpret_cast(MemoryAllocator::alignAddress(newCollisionShapes + nbComponentsToAllocate, GLOBAL_ALIGNMENT)); + assert(reinterpret_cast(newCollisionCategoryBits) % GLOBAL_ALIGNMENT == 0); + unsigned short* newCollideWithMaskBits = reinterpret_cast(MemoryAllocator::alignAddress(newCollisionCategoryBits + nbComponentsToAllocate, GLOBAL_ALIGNMENT)); + assert(reinterpret_cast(newCollideWithMaskBits) % GLOBAL_ALIGNMENT == 0); + Transform* newLocalToWorldTransforms = reinterpret_cast(MemoryAllocator::alignAddress(newCollideWithMaskBits + nbComponentsToAllocate, GLOBAL_ALIGNMENT)); + assert(reinterpret_cast(newLocalToWorldTransforms) % GLOBAL_ALIGNMENT == 0); + Array* newOverlappingPairs = reinterpret_cast*>(MemoryAllocator::alignAddress(newLocalToWorldTransforms + nbComponentsToAllocate, GLOBAL_ALIGNMENT)); + assert(reinterpret_cast(newOverlappingPairs) % GLOBAL_ALIGNMENT == 0); + bool* hasCollisionShapeChangedSize = reinterpret_cast(MemoryAllocator::alignAddress(newOverlappingPairs + nbComponentsToAllocate, GLOBAL_ALIGNMENT)); + assert(reinterpret_cast(hasCollisionShapeChangedSize) % GLOBAL_ALIGNMENT == 0); + bool* isTrigger = reinterpret_cast(MemoryAllocator::alignAddress(hasCollisionShapeChangedSize + nbComponentsToAllocate, GLOBAL_ALIGNMENT)); + assert(reinterpret_cast(isTrigger) % GLOBAL_ALIGNMENT == 0); + Material* materials = reinterpret_cast(MemoryAllocator::alignAddress(isTrigger + nbComponentsToAllocate, GLOBAL_ALIGNMENT)); + assert(reinterpret_cast(materials) % GLOBAL_ALIGNMENT == 0); + assert(reinterpret_cast(materials + nbComponentsToAllocate) <= reinterpret_cast(newBuffer) + totalSizeBytes); // If there was already components before if (mNbComponents > 0) { diff --git a/src/components/CollisionBodyComponents.cpp b/src/components/CollisionBodyComponents.cpp index 9606cfaa..0c4cd567 100644 --- a/src/components/CollisionBodyComponents.cpp +++ b/src/components/CollisionBodyComponents.cpp @@ -35,10 +35,8 @@ using namespace reactphysics3d; // Constructor CollisionBodyComponents::CollisionBodyComponents(MemoryAllocator& allocator) :Components(allocator, sizeof(Entity) + sizeof(CollisionBody*) + sizeof(Array) + - sizeof(bool) + sizeof(void*)) { + sizeof(bool) + sizeof(void*), 5 * GLOBAL_ALIGNMENT) { - // Allocate memory for the components data - allocate(INIT_NB_ALLOCATED_COMPONENTS); } // Allocate memory for a given number of components @@ -47,18 +45,25 @@ void CollisionBodyComponents::allocate(uint32 nbComponentsToAllocate) { assert(nbComponentsToAllocate > mNbAllocatedComponents); // Size for the data of a single component (in bytes) - const size_t totalSizeBytes = nbComponentsToAllocate * mComponentDataSize; + const size_t totalSizeBytes = nbComponentsToAllocate * mComponentDataSize + mAlignmentMarginSize; // Allocate memory void* newBuffer = mMemoryAllocator.allocate(totalSizeBytes); assert(newBuffer != nullptr); + assert(reinterpret_cast(newBuffer) % GLOBAL_ALIGNMENT == 0); // New pointers to components data Entity* newBodiesEntities = static_cast(newBuffer); - CollisionBody** newBodies = reinterpret_cast(newBodiesEntities + nbComponentsToAllocate); - Array* newColliders = reinterpret_cast*>(newBodies + nbComponentsToAllocate); - bool* newIsActive = reinterpret_cast(newColliders + nbComponentsToAllocate); - void** newUserData = reinterpret_cast(newIsActive + nbComponentsToAllocate); + assert(reinterpret_cast(newBodiesEntities) % GLOBAL_ALIGNMENT == 0); + CollisionBody** newBodies = reinterpret_cast(MemoryAllocator::alignAddress(newBodiesEntities + nbComponentsToAllocate, GLOBAL_ALIGNMENT)); + assert(reinterpret_cast(newBodies) % GLOBAL_ALIGNMENT == 0); + Array* newColliders = reinterpret_cast*>(MemoryAllocator::alignAddress(newBodies + nbComponentsToAllocate, GLOBAL_ALIGNMENT)); + assert(reinterpret_cast(newColliders) % GLOBAL_ALIGNMENT == 0); + bool* newIsActive = reinterpret_cast(MemoryAllocator::alignAddress(newColliders + nbComponentsToAllocate, GLOBAL_ALIGNMENT)); + assert(reinterpret_cast(newIsActive) % GLOBAL_ALIGNMENT == 0); + void** newUserData = reinterpret_cast(MemoryAllocator::alignAddress(newIsActive + nbComponentsToAllocate, GLOBAL_ALIGNMENT)); + assert(reinterpret_cast(newUserData) % GLOBAL_ALIGNMENT == 0); + assert(reinterpret_cast(newUserData + nbComponentsToAllocate) <= reinterpret_cast(newBuffer) + totalSizeBytes); // If there was already components before if (mNbComponents > 0) { diff --git a/src/components/Components.cpp b/src/components/Components.cpp index f09d4fe2..0b5de9c1 100644 --- a/src/components/Components.cpp +++ b/src/components/Components.cpp @@ -31,9 +31,9 @@ using namespace reactphysics3d; // Constructor -Components::Components(MemoryAllocator& allocator, size_t componentDataSize) +Components::Components(MemoryAllocator& allocator, size_t componentDataSize, size_t alignmentMarginSize) : mMemoryAllocator(allocator), mNbComponents(0), mComponentDataSize(componentDataSize), - mNbAllocatedComponents(0), mBuffer(nullptr), mMapEntityToComponentIndex(allocator), + mAlignmentMarginSize(alignmentMarginSize), mNbAllocatedComponents(0), mBuffer(nullptr), mMapEntityToComponentIndex(allocator), mDisabledStartIndex(0) { } @@ -57,6 +57,13 @@ Components::~Components() { } } +// Initialize the components: +void Components::init() { + + // Allocate memory for the components data + allocate(INIT_NB_ALLOCATED_COMPONENTS); +} + // Compute the index where we need to insert the new component uint32 Components::prepareAddComponent(bool isSleeping) { diff --git a/src/components/FixedJointComponents.cpp b/src/components/FixedJointComponents.cpp index 0794c058..96ff3c4f 100644 --- a/src/components/FixedJointComponents.cpp +++ b/src/components/FixedJointComponents.cpp @@ -38,10 +38,8 @@ FixedJointComponents::FixedJointComponents(MemoryAllocator& allocator) sizeof(Vector3) + sizeof(Vector3) + sizeof(Vector3) + sizeof(Matrix3x3) + sizeof(Matrix3x3) + sizeof(Vector3) + sizeof(Vector3) + sizeof(Matrix3x3) + sizeof(Matrix3x3) + - sizeof(Vector3) + sizeof(Vector3) + sizeof(Quaternion)) { + sizeof(Vector3) + sizeof(Vector3) + sizeof(Quaternion), 15 * GLOBAL_ALIGNMENT) { - // Allocate memory for the components data - allocate(INIT_NB_ALLOCATED_COMPONENTS); } // Allocate memory for a given number of components @@ -50,28 +48,44 @@ void FixedJointComponents::allocate(uint32 nbComponentsToAllocate) { assert(nbComponentsToAllocate > mNbAllocatedComponents); // Size for the data of a single component (in bytes) - const size_t totalSizeBytes = nbComponentsToAllocate * mComponentDataSize; + const size_t totalSizeBytes = nbComponentsToAllocate * mComponentDataSize + mAlignmentMarginSize; // Allocate memory void* newBuffer = mMemoryAllocator.allocate(totalSizeBytes); assert(newBuffer != nullptr); + assert(reinterpret_cast(newBuffer) % GLOBAL_ALIGNMENT == 0); // New pointers to components data Entity* newJointEntities = static_cast(newBuffer); - FixedJoint** newJoints = reinterpret_cast(newJointEntities + nbComponentsToAllocate); - Vector3* newLocalAnchorPointBody1 = reinterpret_cast(newJoints + nbComponentsToAllocate); - Vector3* newLocalAnchorPointBody2 = reinterpret_cast(newLocalAnchorPointBody1 + nbComponentsToAllocate); - Vector3* newR1World = reinterpret_cast(newLocalAnchorPointBody2 + nbComponentsToAllocate); - Vector3* newR2World = reinterpret_cast(newR1World + nbComponentsToAllocate); - Matrix3x3* newI1 = reinterpret_cast(newR2World + nbComponentsToAllocate); - Matrix3x3* newI2 = reinterpret_cast(newI1 + nbComponentsToAllocate); - Vector3* newImpulseTranslation = reinterpret_cast(newI2 + nbComponentsToAllocate); - Vector3* newImpulseRotation = reinterpret_cast(newImpulseTranslation + nbComponentsToAllocate); - Matrix3x3* newInverseMassMatrixTranslation = reinterpret_cast(newImpulseRotation + nbComponentsToAllocate); - Matrix3x3* newInverseMassMatrixRotation = reinterpret_cast(newInverseMassMatrixTranslation + nbComponentsToAllocate); - Vector3* newBiasTranslation = reinterpret_cast(newInverseMassMatrixRotation + nbComponentsToAllocate); - Vector3* newBiasRotation = reinterpret_cast(newBiasTranslation + nbComponentsToAllocate); - Quaternion* newInitOrientationDifferenceInv = reinterpret_cast(newBiasRotation + nbComponentsToAllocate); + FixedJoint** newJoints = reinterpret_cast(MemoryAllocator::alignAddress(newJointEntities + nbComponentsToAllocate, GLOBAL_ALIGNMENT)); + assert(reinterpret_cast(newJoints) % GLOBAL_ALIGNMENT == 0); + Vector3* newLocalAnchorPointBody1 = reinterpret_cast(MemoryAllocator::alignAddress(newJoints + nbComponentsToAllocate, GLOBAL_ALIGNMENT)); + assert(reinterpret_cast(newLocalAnchorPointBody1) % GLOBAL_ALIGNMENT == 0); + Vector3* newLocalAnchorPointBody2 = reinterpret_cast(MemoryAllocator::alignAddress(newLocalAnchorPointBody1 + nbComponentsToAllocate, GLOBAL_ALIGNMENT)); + assert(reinterpret_cast(newLocalAnchorPointBody2) % GLOBAL_ALIGNMENT == 0); + Vector3* newR1World = reinterpret_cast(MemoryAllocator::alignAddress(newLocalAnchorPointBody2 + nbComponentsToAllocate, GLOBAL_ALIGNMENT)); + assert(reinterpret_cast(newR1World) % GLOBAL_ALIGNMENT == 0); + Vector3* newR2World = reinterpret_cast(MemoryAllocator::alignAddress(newR1World + nbComponentsToAllocate, GLOBAL_ALIGNMENT)); + assert(reinterpret_cast(newR2World) % GLOBAL_ALIGNMENT == 0); + Matrix3x3* newI1 = reinterpret_cast(MemoryAllocator::alignAddress(newR2World + nbComponentsToAllocate, GLOBAL_ALIGNMENT)); + assert(reinterpret_cast(newI1) % GLOBAL_ALIGNMENT == 0); + Matrix3x3* newI2 = reinterpret_cast(MemoryAllocator::alignAddress(newI1 + nbComponentsToAllocate, GLOBAL_ALIGNMENT)); + assert(reinterpret_cast(newI1) % GLOBAL_ALIGNMENT == 0); + Vector3* newImpulseTranslation = reinterpret_cast(MemoryAllocator::alignAddress(newI2 + nbComponentsToAllocate, GLOBAL_ALIGNMENT)); + assert(reinterpret_cast(newImpulseTranslation) % GLOBAL_ALIGNMENT == 0); + Vector3* newImpulseRotation = reinterpret_cast(MemoryAllocator::alignAddress(newImpulseTranslation + nbComponentsToAllocate, GLOBAL_ALIGNMENT)); + assert(reinterpret_cast(newImpulseRotation) % GLOBAL_ALIGNMENT == 0); + Matrix3x3* newInverseMassMatrixTranslation = reinterpret_cast(MemoryAllocator::alignAddress(newImpulseRotation + nbComponentsToAllocate, GLOBAL_ALIGNMENT)); + assert(reinterpret_cast(newInverseMassMatrixTranslation) % GLOBAL_ALIGNMENT == 0); + Matrix3x3* newInverseMassMatrixRotation = reinterpret_cast(MemoryAllocator::alignAddress(newInverseMassMatrixTranslation + nbComponentsToAllocate, GLOBAL_ALIGNMENT)); + assert(reinterpret_cast(newInverseMassMatrixRotation) % GLOBAL_ALIGNMENT == 0); + Vector3* newBiasTranslation = reinterpret_cast(MemoryAllocator::alignAddress(newInverseMassMatrixRotation + nbComponentsToAllocate, GLOBAL_ALIGNMENT)); + assert(reinterpret_cast(newBiasTranslation) % GLOBAL_ALIGNMENT == 0); + Vector3* newBiasRotation = reinterpret_cast(MemoryAllocator::alignAddress(newBiasTranslation + nbComponentsToAllocate, GLOBAL_ALIGNMENT)); + assert(reinterpret_cast(newBiasRotation) % GLOBAL_ALIGNMENT == 0); + Quaternion* newInitOrientationDifferenceInv = reinterpret_cast(MemoryAllocator::alignAddress(newBiasRotation + nbComponentsToAllocate, GLOBAL_ALIGNMENT)); + assert(reinterpret_cast(newInitOrientationDifferenceInv) % GLOBAL_ALIGNMENT == 0); + assert(reinterpret_cast(newInitOrientationDifferenceInv + nbComponentsToAllocate) <= reinterpret_cast(newBuffer) + totalSizeBytes); // If there was already components before if (mNbComponents > 0) { diff --git a/src/components/HingeJointComponents.cpp b/src/components/HingeJointComponents.cpp index e55a9d2d..22e945c7 100644 --- a/src/components/HingeJointComponents.cpp +++ b/src/components/HingeJointComponents.cpp @@ -43,10 +43,8 @@ HingeJointComponents::HingeJointComponents(MemoryAllocator& allocator) sizeof(Vector3) + sizeof(decimal) + sizeof(decimal) + sizeof(decimal) + sizeof(decimal) + sizeof(decimal) + sizeof(decimal) + sizeof(decimal) + sizeof(bool) + sizeof(bool) + sizeof(decimal) + sizeof(decimal) + - sizeof(bool) + sizeof(bool) + sizeof(decimal) + sizeof(decimal)) { + sizeof(bool) + sizeof(bool) + sizeof(decimal) + sizeof(decimal), 35 * GLOBAL_ALIGNMENT ) { - // Allocate memory for the components data - allocate(INIT_NB_ALLOCATED_COMPONENTS); } // Allocate memory for a given number of components @@ -55,48 +53,84 @@ void HingeJointComponents::allocate(uint32 nbComponentsToAllocate) { assert(nbComponentsToAllocate > mNbAllocatedComponents); // Size for the data of a single component (in bytes) - const size_t totalSizeBytes = nbComponentsToAllocate * mComponentDataSize; + const size_t totalSizeBytes = nbComponentsToAllocate * mComponentDataSize + mAlignmentMarginSize; // Allocate memory void* newBuffer = mMemoryAllocator.allocate(totalSizeBytes); assert(newBuffer != nullptr); + assert(reinterpret_cast(newBuffer) % GLOBAL_ALIGNMENT == 0); // New pointers to components data Entity* newJointEntities = static_cast(newBuffer); - HingeJoint** newJoints = reinterpret_cast(newJointEntities + nbComponentsToAllocate); - Vector3* newLocalAnchorPointBody1 = reinterpret_cast(newJoints + nbComponentsToAllocate); - Vector3* newLocalAnchorPointBody2 = reinterpret_cast(newLocalAnchorPointBody1 + nbComponentsToAllocate); - Vector3* newR1World = reinterpret_cast(newLocalAnchorPointBody2 + nbComponentsToAllocate); - Vector3* newR2World = reinterpret_cast(newR1World + nbComponentsToAllocate); - Matrix3x3* newI1 = reinterpret_cast(newR2World + nbComponentsToAllocate); - Matrix3x3* newI2 = reinterpret_cast(newI1 + nbComponentsToAllocate); - Vector3* newImpulseTranslation = reinterpret_cast(newI2 + nbComponentsToAllocate); - Vector2* newImpulseRotation = reinterpret_cast(newImpulseTranslation + nbComponentsToAllocate); - Matrix3x3* newInverseMassMatrixTranslation = reinterpret_cast(newImpulseRotation + nbComponentsToAllocate); - Matrix2x2* newInverseMassMatrixRotation = reinterpret_cast(newInverseMassMatrixTranslation + nbComponentsToAllocate); - Vector3* newBiasTranslation = reinterpret_cast(newInverseMassMatrixRotation + nbComponentsToAllocate); - Vector2* newBiasRotation = reinterpret_cast(newBiasTranslation + nbComponentsToAllocate); - Quaternion* newInitOrientationDifferenceInv = reinterpret_cast(newBiasRotation + nbComponentsToAllocate); - Vector3* newHingeLocalAxisBody1 = reinterpret_cast(newInitOrientationDifferenceInv + nbComponentsToAllocate); - Vector3* newHingeLocalAxisBody2 = reinterpret_cast(newHingeLocalAxisBody1 + nbComponentsToAllocate); - Vector3* newA1 = reinterpret_cast(newHingeLocalAxisBody2 + nbComponentsToAllocate); - Vector3* newB2CrossA1 = reinterpret_cast(newA1 + nbComponentsToAllocate); - Vector3* newC2CrossA1 = reinterpret_cast(newB2CrossA1 + nbComponentsToAllocate); - decimal* newImpulseLowerLimit = reinterpret_cast(newC2CrossA1 + nbComponentsToAllocate); - decimal* newImpulseUpperLimit = reinterpret_cast(newImpulseLowerLimit + nbComponentsToAllocate); - decimal* newImpulseMotor = reinterpret_cast(newImpulseUpperLimit + nbComponentsToAllocate); - decimal* newInverseMassMatrixLimitMotor = reinterpret_cast(newImpulseMotor + nbComponentsToAllocate); - decimal* newInverseMassMatrixMotor = reinterpret_cast(newInverseMassMatrixLimitMotor + nbComponentsToAllocate); - decimal* newBLowerLimit = reinterpret_cast(newInverseMassMatrixMotor + nbComponentsToAllocate); - decimal* newBUpperLimit = reinterpret_cast(newBLowerLimit + nbComponentsToAllocate); - bool* newIsLimitEnabled = reinterpret_cast(newBUpperLimit + nbComponentsToAllocate); - bool* newIsMotorEnabled = reinterpret_cast(newIsLimitEnabled + nbComponentsToAllocate); - decimal* newLowerLimit = reinterpret_cast(newIsMotorEnabled + nbComponentsToAllocate); - decimal* newUpperLimit = reinterpret_cast(newLowerLimit + nbComponentsToAllocate); - bool* newIsLowerLimitViolated = reinterpret_cast(newUpperLimit + nbComponentsToAllocate); - bool* newIsUpperLimitViolated = reinterpret_cast(newIsLowerLimitViolated + nbComponentsToAllocate); - decimal* newMotorSpeed = reinterpret_cast(newIsUpperLimitViolated + nbComponentsToAllocate); - decimal* newMaxMotorTorque = reinterpret_cast(newMotorSpeed + nbComponentsToAllocate); + HingeJoint** newJoints = reinterpret_cast(MemoryAllocator::alignAddress(newJointEntities + nbComponentsToAllocate, GLOBAL_ALIGNMENT)); + assert(reinterpret_cast(newJoints) % GLOBAL_ALIGNMENT == 0); + Vector3* newLocalAnchorPointBody1 = reinterpret_cast(MemoryAllocator::alignAddress(newJoints + nbComponentsToAllocate, GLOBAL_ALIGNMENT)); + assert(reinterpret_cast(newLocalAnchorPointBody1) % GLOBAL_ALIGNMENT == 0); + Vector3* newLocalAnchorPointBody2 = reinterpret_cast(MemoryAllocator::alignAddress(newLocalAnchorPointBody1 + nbComponentsToAllocate, GLOBAL_ALIGNMENT)); + assert(reinterpret_cast(newLocalAnchorPointBody2) % GLOBAL_ALIGNMENT == 0); + Vector3* newR1World = reinterpret_cast(MemoryAllocator::alignAddress(newLocalAnchorPointBody2 + nbComponentsToAllocate, GLOBAL_ALIGNMENT)); + assert(reinterpret_cast(newR1World) % GLOBAL_ALIGNMENT == 0); + Vector3* newR2World = reinterpret_cast(MemoryAllocator::alignAddress(newR1World + nbComponentsToAllocate, GLOBAL_ALIGNMENT)); + assert(reinterpret_cast(newR2World) % GLOBAL_ALIGNMENT == 0); + Matrix3x3* newI1 = reinterpret_cast(MemoryAllocator::alignAddress(newR2World + nbComponentsToAllocate, GLOBAL_ALIGNMENT)); + assert(reinterpret_cast(newI1) % GLOBAL_ALIGNMENT == 0); + Matrix3x3* newI2 = reinterpret_cast(MemoryAllocator::alignAddress(newI1 + nbComponentsToAllocate, GLOBAL_ALIGNMENT)); + assert(reinterpret_cast(newI2) % GLOBAL_ALIGNMENT == 0); + Vector3* newImpulseTranslation = reinterpret_cast(MemoryAllocator::alignAddress(newI2 + nbComponentsToAllocate, GLOBAL_ALIGNMENT)); + assert(reinterpret_cast(newImpulseTranslation) % GLOBAL_ALIGNMENT == 0); + Vector2* newImpulseRotation = reinterpret_cast(MemoryAllocator::alignAddress(newImpulseTranslation + nbComponentsToAllocate, GLOBAL_ALIGNMENT)); + assert(reinterpret_cast(newImpulseRotation) % GLOBAL_ALIGNMENT == 0); + Matrix3x3* newInverseMassMatrixTranslation = reinterpret_cast(MemoryAllocator::alignAddress(newImpulseRotation + nbComponentsToAllocate, GLOBAL_ALIGNMENT)); + assert(reinterpret_cast(newInverseMassMatrixTranslation) % GLOBAL_ALIGNMENT == 0); + Matrix2x2* newInverseMassMatrixRotation = reinterpret_cast(MemoryAllocator::alignAddress(newInverseMassMatrixTranslation + nbComponentsToAllocate, GLOBAL_ALIGNMENT)); + assert(reinterpret_cast(newInverseMassMatrixRotation) % GLOBAL_ALIGNMENT == 0); + Vector3* newBiasTranslation = reinterpret_cast(MemoryAllocator::alignAddress(newInverseMassMatrixRotation + nbComponentsToAllocate, GLOBAL_ALIGNMENT)); + assert(reinterpret_cast(newBiasTranslation) % GLOBAL_ALIGNMENT == 0); + Vector2* newBiasRotation = reinterpret_cast(MemoryAllocator::alignAddress(newBiasTranslation + nbComponentsToAllocate, GLOBAL_ALIGNMENT)); + assert(reinterpret_cast(newBiasRotation) % GLOBAL_ALIGNMENT == 0); + Quaternion* newInitOrientationDifferenceInv = reinterpret_cast(MemoryAllocator::alignAddress(newBiasRotation + nbComponentsToAllocate, GLOBAL_ALIGNMENT)); + assert(reinterpret_cast(newInitOrientationDifferenceInv) % GLOBAL_ALIGNMENT == 0); + Vector3* newHingeLocalAxisBody1 = reinterpret_cast(MemoryAllocator::alignAddress(newInitOrientationDifferenceInv + nbComponentsToAllocate, GLOBAL_ALIGNMENT)); + assert(reinterpret_cast(newHingeLocalAxisBody1) % GLOBAL_ALIGNMENT == 0); + Vector3* newHingeLocalAxisBody2 = reinterpret_cast(MemoryAllocator::alignAddress(newHingeLocalAxisBody1 + nbComponentsToAllocate, GLOBAL_ALIGNMENT)); + assert(reinterpret_cast(newHingeLocalAxisBody2) % GLOBAL_ALIGNMENT == 0); + Vector3* newA1 = reinterpret_cast(MemoryAllocator::alignAddress(newHingeLocalAxisBody2 + nbComponentsToAllocate, GLOBAL_ALIGNMENT)); + assert(reinterpret_cast(newA1) % GLOBAL_ALIGNMENT == 0); + Vector3* newB2CrossA1 = reinterpret_cast(MemoryAllocator::alignAddress(newA1 + nbComponentsToAllocate, GLOBAL_ALIGNMENT)); + assert(reinterpret_cast(newB2CrossA1) % GLOBAL_ALIGNMENT == 0); + Vector3* newC2CrossA1 = reinterpret_cast(MemoryAllocator::alignAddress(newB2CrossA1 + nbComponentsToAllocate, GLOBAL_ALIGNMENT)); + assert(reinterpret_cast(newC2CrossA1) % GLOBAL_ALIGNMENT == 0); + decimal* newImpulseLowerLimit = reinterpret_cast(MemoryAllocator::alignAddress(newC2CrossA1 + nbComponentsToAllocate, GLOBAL_ALIGNMENT)); + assert(reinterpret_cast(newImpulseLowerLimit) % GLOBAL_ALIGNMENT == 0); + decimal* newImpulseUpperLimit = reinterpret_cast(MemoryAllocator::alignAddress(newImpulseLowerLimit + nbComponentsToAllocate, GLOBAL_ALIGNMENT)); + assert(reinterpret_cast(newImpulseUpperLimit) % GLOBAL_ALIGNMENT == 0); + decimal* newImpulseMotor = reinterpret_cast(MemoryAllocator::alignAddress(newImpulseUpperLimit + nbComponentsToAllocate, GLOBAL_ALIGNMENT)); + assert(reinterpret_cast(newImpulseMotor) % GLOBAL_ALIGNMENT == 0); + decimal* newInverseMassMatrixLimitMotor = reinterpret_cast(MemoryAllocator::alignAddress(newImpulseMotor + nbComponentsToAllocate, GLOBAL_ALIGNMENT)); + assert(reinterpret_cast(newInverseMassMatrixLimitMotor) % GLOBAL_ALIGNMENT == 0); + decimal* newInverseMassMatrixMotor = reinterpret_cast(MemoryAllocator::alignAddress(newInverseMassMatrixLimitMotor + nbComponentsToAllocate, GLOBAL_ALIGNMENT)); + assert(reinterpret_cast(newInverseMassMatrixMotor) % GLOBAL_ALIGNMENT == 0); + decimal* newBLowerLimit = reinterpret_cast(MemoryAllocator::alignAddress(newInverseMassMatrixMotor + nbComponentsToAllocate, GLOBAL_ALIGNMENT)); + assert(reinterpret_cast(newBLowerLimit) % GLOBAL_ALIGNMENT == 0); + decimal* newBUpperLimit = reinterpret_cast(MemoryAllocator::alignAddress(newBLowerLimit + nbComponentsToAllocate, GLOBAL_ALIGNMENT)); + assert(reinterpret_cast(newBUpperLimit) % GLOBAL_ALIGNMENT == 0); + bool* newIsLimitEnabled = reinterpret_cast(MemoryAllocator::alignAddress(newBUpperLimit + nbComponentsToAllocate, GLOBAL_ALIGNMENT)); + assert(reinterpret_cast(newIsLimitEnabled) % GLOBAL_ALIGNMENT == 0); + bool* newIsMotorEnabled = reinterpret_cast(MemoryAllocator::alignAddress(newIsLimitEnabled + nbComponentsToAllocate, GLOBAL_ALIGNMENT)); + assert(reinterpret_cast(newIsMotorEnabled) % GLOBAL_ALIGNMENT == 0); + decimal* newLowerLimit = reinterpret_cast(MemoryAllocator::alignAddress(newIsMotorEnabled + nbComponentsToAllocate, GLOBAL_ALIGNMENT)); + assert(reinterpret_cast(newLowerLimit) % GLOBAL_ALIGNMENT == 0); + decimal* newUpperLimit = reinterpret_cast(MemoryAllocator::alignAddress(newLowerLimit + nbComponentsToAllocate, GLOBAL_ALIGNMENT)); + assert(reinterpret_cast(newUpperLimit) % GLOBAL_ALIGNMENT == 0); + bool* newIsLowerLimitViolated = reinterpret_cast(MemoryAllocator::alignAddress(newUpperLimit + nbComponentsToAllocate, GLOBAL_ALIGNMENT)); + assert(reinterpret_cast(newIsLowerLimitViolated) % GLOBAL_ALIGNMENT == 0); + bool* newIsUpperLimitViolated = reinterpret_cast(MemoryAllocator::alignAddress(newIsLowerLimitViolated + nbComponentsToAllocate, GLOBAL_ALIGNMENT)); + assert(reinterpret_cast(newIsUpperLimitViolated) % GLOBAL_ALIGNMENT == 0); + decimal* newMotorSpeed = reinterpret_cast(MemoryAllocator::alignAddress(newIsUpperLimitViolated + nbComponentsToAllocate, GLOBAL_ALIGNMENT)); + assert(reinterpret_cast(newMotorSpeed) % GLOBAL_ALIGNMENT == 0); + decimal* newMaxMotorTorque = reinterpret_cast(MemoryAllocator::alignAddress(newMotorSpeed + nbComponentsToAllocate, GLOBAL_ALIGNMENT)); + assert(reinterpret_cast(newMaxMotorTorque) % GLOBAL_ALIGNMENT == 0); + assert(reinterpret_cast(newMaxMotorTorque + nbComponentsToAllocate) <= reinterpret_cast(newBuffer) + totalSizeBytes); // If there was already components before if (mNbComponents > 0) { diff --git a/src/components/JointComponents.cpp b/src/components/JointComponents.cpp index 12123c51..947b4ee0 100644 --- a/src/components/JointComponents.cpp +++ b/src/components/JointComponents.cpp @@ -35,10 +35,8 @@ using namespace reactphysics3d; JointComponents::JointComponents(MemoryAllocator& allocator) :Components(allocator, sizeof(Entity) + sizeof(Entity) + sizeof(Entity) + sizeof(Joint*) + sizeof(JointType) + sizeof(JointsPositionCorrectionTechnique) + sizeof(bool) + - sizeof(bool)) { + sizeof(bool), 8 * GLOBAL_ALIGNMENT) { - // Allocate memory for the components data - allocate(INIT_NB_ALLOCATED_COMPONENTS); } // Allocate memory for a given number of components @@ -47,21 +45,30 @@ void JointComponents::allocate(uint32 nbComponentsToAllocate) { assert(nbComponentsToAllocate > mNbAllocatedComponents); // Size for the data of a single component (in bytes) - const size_t totalSizeBytes = nbComponentsToAllocate * mComponentDataSize; + const size_t totalSizeBytes = nbComponentsToAllocate * mComponentDataSize + mAlignmentMarginSize; // Allocate memory void* newBuffer = mMemoryAllocator.allocate(totalSizeBytes); assert(newBuffer != nullptr); + assert(reinterpret_cast(newBuffer) % GLOBAL_ALIGNMENT == 0); // New pointers to components data Entity* newJointsEntities = static_cast(newBuffer); - Entity* newBody1Entities = reinterpret_cast(newJointsEntities + nbComponentsToAllocate); - Entity* newBody2Entities = reinterpret_cast(newBody1Entities + nbComponentsToAllocate); - Joint** newJoints = reinterpret_cast(newBody2Entities + nbComponentsToAllocate); - JointType* newTypes = reinterpret_cast(newJoints + nbComponentsToAllocate); - JointsPositionCorrectionTechnique* newPositionCorrectionTechniques = reinterpret_cast(newTypes + nbComponentsToAllocate); - bool* newIsCollisionEnabled = reinterpret_cast(newPositionCorrectionTechniques + nbComponentsToAllocate); - bool* newIsAlreadyInIsland = reinterpret_cast(newIsCollisionEnabled + nbComponentsToAllocate); + Entity* newBody1Entities = reinterpret_cast(MemoryAllocator::alignAddress(newJointsEntities + nbComponentsToAllocate, GLOBAL_ALIGNMENT)); + assert(reinterpret_cast(newBody1Entities) % GLOBAL_ALIGNMENT == 0); + Entity* newBody2Entities = reinterpret_cast(MemoryAllocator::alignAddress(newBody1Entities + nbComponentsToAllocate, GLOBAL_ALIGNMENT)); + assert(reinterpret_cast(newBody2Entities) % GLOBAL_ALIGNMENT == 0); + Joint** newJoints = reinterpret_cast(MemoryAllocator::alignAddress(newBody2Entities + nbComponentsToAllocate, GLOBAL_ALIGNMENT)); + assert(reinterpret_cast(newJoints) % GLOBAL_ALIGNMENT == 0); + JointType* newTypes = reinterpret_cast(MemoryAllocator::alignAddress(newJoints + nbComponentsToAllocate, GLOBAL_ALIGNMENT)); + assert(reinterpret_cast(newTypes) % GLOBAL_ALIGNMENT == 0); + JointsPositionCorrectionTechnique* newPositionCorrectionTechniques = reinterpret_cast(MemoryAllocator::alignAddress(newTypes + nbComponentsToAllocate, GLOBAL_ALIGNMENT)); + assert(reinterpret_cast(newPositionCorrectionTechniques) % GLOBAL_ALIGNMENT == 0); + bool* newIsCollisionEnabled = reinterpret_cast(MemoryAllocator::alignAddress(newPositionCorrectionTechniques + nbComponentsToAllocate, GLOBAL_ALIGNMENT)); + assert(reinterpret_cast(newIsCollisionEnabled) % GLOBAL_ALIGNMENT == 0); + bool* newIsAlreadyInIsland = reinterpret_cast(MemoryAllocator::alignAddress(newIsCollisionEnabled + nbComponentsToAllocate, GLOBAL_ALIGNMENT)); + assert(reinterpret_cast(newIsAlreadyInIsland) % GLOBAL_ALIGNMENT == 0); + assert(reinterpret_cast(newIsAlreadyInIsland + nbComponentsToAllocate) <= reinterpret_cast(newBuffer) + totalSizeBytes); // If there was already components before if (mNbComponents > 0) { diff --git a/src/components/RigidBodyComponents.cpp b/src/components/RigidBodyComponents.cpp index 3edcefe9..adc0e4df 100644 --- a/src/components/RigidBodyComponents.cpp +++ b/src/components/RigidBodyComponents.cpp @@ -44,10 +44,8 @@ RigidBodyComponents::RigidBodyComponents(MemoryAllocator& allocator) sizeof(Vector3) + sizeof(Vector3) + sizeof(Vector3) + sizeof(Quaternion) + sizeof(Vector3) + sizeof(Vector3) + sizeof(bool) + sizeof(bool) + sizeof(Array) + sizeof(Array) + - sizeof(Vector3) + sizeof(Vector3)) { + sizeof(Vector3) + sizeof(Vector3), 31 * GLOBAL_ALIGNMENT) { - // Allocate memory for the components data - allocate(INIT_NB_ALLOCATED_COMPONENTS); } // Allocate memory for a given number of components @@ -56,44 +54,76 @@ void RigidBodyComponents::allocate(uint32 nbComponentsToAllocate) { assert(nbComponentsToAllocate > mNbAllocatedComponents); // Size for the data of a single component (in bytes) - const size_t totalSizeBytes = nbComponentsToAllocate * mComponentDataSize; + const size_t totalSizeBytes = nbComponentsToAllocate * mComponentDataSize + mAlignmentMarginSize; // Allocate memory void* newBuffer = mMemoryAllocator.allocate(totalSizeBytes); assert(newBuffer != nullptr); + assert(reinterpret_cast(newBuffer) % GLOBAL_ALIGNMENT == 0); // New pointers to components data Entity* newBodiesEntities = static_cast(newBuffer); - RigidBody** newBodies = reinterpret_cast(newBodiesEntities + nbComponentsToAllocate); - bool* newIsAllowedToSleep = reinterpret_cast(newBodies + nbComponentsToAllocate); - bool* newIsSleeping = reinterpret_cast(newIsAllowedToSleep + nbComponentsToAllocate); - decimal* newSleepTimes = reinterpret_cast(newIsSleeping + nbComponentsToAllocate); - BodyType* newBodyTypes = reinterpret_cast(newSleepTimes + nbComponentsToAllocate); - Vector3* newLinearVelocities = reinterpret_cast(newBodyTypes + nbComponentsToAllocate); - Vector3* newAngularVelocities = reinterpret_cast(newLinearVelocities + nbComponentsToAllocate); - Vector3* newExternalForces = reinterpret_cast(newAngularVelocities + nbComponentsToAllocate); - Vector3* newExternalTorques = reinterpret_cast(newExternalForces + nbComponentsToAllocate); - decimal* newLinearDampings = reinterpret_cast(newExternalTorques + nbComponentsToAllocate); - decimal* newAngularDampings = reinterpret_cast(newLinearDampings + nbComponentsToAllocate); - decimal* newMasses = reinterpret_cast(newAngularDampings + nbComponentsToAllocate); - decimal* newInverseMasses = reinterpret_cast(newMasses + nbComponentsToAllocate); - Vector3* newInertiaTensorLocal = reinterpret_cast(newInverseMasses + nbComponentsToAllocate); - Vector3* newInertiaTensorLocalInverses = reinterpret_cast(newInertiaTensorLocal + nbComponentsToAllocate); - Matrix3x3* newInertiaTensorWorldInverses = reinterpret_cast(newInertiaTensorLocalInverses + nbComponentsToAllocate); - Vector3* newConstrainedLinearVelocities = reinterpret_cast(newInertiaTensorWorldInverses + nbComponentsToAllocate); - Vector3* newConstrainedAngularVelocities = reinterpret_cast(newConstrainedLinearVelocities + nbComponentsToAllocate); - Vector3* newSplitLinearVelocities = reinterpret_cast(newConstrainedAngularVelocities + nbComponentsToAllocate); - Vector3* newSplitAngularVelocities = reinterpret_cast(newSplitLinearVelocities + nbComponentsToAllocate); - Vector3* newConstrainedPositions = reinterpret_cast(newSplitAngularVelocities + nbComponentsToAllocate); - Quaternion* newConstrainedOrientations = reinterpret_cast(newConstrainedPositions + nbComponentsToAllocate); - Vector3* newCentersOfMassLocal = reinterpret_cast(newConstrainedOrientations + nbComponentsToAllocate); - Vector3* newCentersOfMassWorld = reinterpret_cast(newCentersOfMassLocal + nbComponentsToAllocate); - bool* newIsGravityEnabled = reinterpret_cast(newCentersOfMassWorld + nbComponentsToAllocate); - bool* newIsAlreadyInIsland = reinterpret_cast(newIsGravityEnabled + nbComponentsToAllocate); - Array* newJoints = reinterpret_cast*>(newIsAlreadyInIsland + nbComponentsToAllocate); - Array* newContactPairs = reinterpret_cast*>(newJoints + nbComponentsToAllocate); - Vector3* newLinearLockAxisFactors = reinterpret_cast(newContactPairs + nbComponentsToAllocate); - Vector3* newAngularLockAxisFactors = reinterpret_cast(newLinearLockAxisFactors + nbComponentsToAllocate); + RigidBody** newBodies = reinterpret_cast(MemoryAllocator::alignAddress(newBodiesEntities + nbComponentsToAllocate, GLOBAL_ALIGNMENT)); + assert(reinterpret_cast(newBodies) % GLOBAL_ALIGNMENT == 0); + bool* newIsAllowedToSleep = reinterpret_cast(MemoryAllocator::alignAddress(newBodies + nbComponentsToAllocate, GLOBAL_ALIGNMENT)); + assert(reinterpret_cast(newIsAllowedToSleep) % GLOBAL_ALIGNMENT == 0); + bool* newIsSleeping = reinterpret_cast(MemoryAllocator::alignAddress(newIsAllowedToSleep + nbComponentsToAllocate, GLOBAL_ALIGNMENT)); + assert(reinterpret_cast(newIsSleeping) % GLOBAL_ALIGNMENT == 0); + decimal* newSleepTimes = reinterpret_cast(MemoryAllocator::alignAddress(newIsSleeping + nbComponentsToAllocate, GLOBAL_ALIGNMENT)); + assert(reinterpret_cast(newSleepTimes) % GLOBAL_ALIGNMENT == 0); + BodyType* newBodyTypes = reinterpret_cast(MemoryAllocator::alignAddress(newSleepTimes + nbComponentsToAllocate, GLOBAL_ALIGNMENT)); + assert(reinterpret_cast(newBodyTypes) % GLOBAL_ALIGNMENT == 0); + Vector3* newLinearVelocities = reinterpret_cast(MemoryAllocator::alignAddress(newBodyTypes + nbComponentsToAllocate, GLOBAL_ALIGNMENT)); + assert(reinterpret_cast(newLinearVelocities) % GLOBAL_ALIGNMENT == 0); + Vector3* newAngularVelocities = reinterpret_cast(MemoryAllocator::alignAddress(newLinearVelocities + nbComponentsToAllocate, GLOBAL_ALIGNMENT)); + assert(reinterpret_cast(newAngularVelocities) % GLOBAL_ALIGNMENT == 0); + Vector3* newExternalForces = reinterpret_cast(MemoryAllocator::alignAddress(newAngularVelocities + nbComponentsToAllocate, GLOBAL_ALIGNMENT)); + assert(reinterpret_cast(newExternalForces) % GLOBAL_ALIGNMENT == 0); + Vector3* newExternalTorques = reinterpret_cast(MemoryAllocator::alignAddress(newExternalForces + nbComponentsToAllocate, GLOBAL_ALIGNMENT)); + assert(reinterpret_cast(newExternalTorques) % GLOBAL_ALIGNMENT == 0); + decimal* newLinearDampings = reinterpret_cast(MemoryAllocator::alignAddress(newExternalTorques + nbComponentsToAllocate, GLOBAL_ALIGNMENT)); + assert(reinterpret_cast(newLinearDampings) % GLOBAL_ALIGNMENT == 0); + decimal* newAngularDampings = reinterpret_cast(MemoryAllocator::alignAddress(newLinearDampings + nbComponentsToAllocate, GLOBAL_ALIGNMENT)); + assert(reinterpret_cast(newAngularDampings) % GLOBAL_ALIGNMENT == 0); + decimal* newMasses = reinterpret_cast(MemoryAllocator::alignAddress(newAngularDampings + nbComponentsToAllocate, GLOBAL_ALIGNMENT)); + assert(reinterpret_cast(newMasses) % GLOBAL_ALIGNMENT == 0); + decimal* newInverseMasses = reinterpret_cast(MemoryAllocator::alignAddress(newMasses + nbComponentsToAllocate, GLOBAL_ALIGNMENT)); + assert(reinterpret_cast(newInverseMasses) % GLOBAL_ALIGNMENT == 0); + Vector3* newInertiaTensorLocal = reinterpret_cast(MemoryAllocator::alignAddress(newInverseMasses + nbComponentsToAllocate, GLOBAL_ALIGNMENT)); + assert(reinterpret_cast(newInertiaTensorLocal) % GLOBAL_ALIGNMENT == 0); + Vector3* newInertiaTensorLocalInverses = reinterpret_cast(MemoryAllocator::alignAddress(newInertiaTensorLocal + nbComponentsToAllocate, GLOBAL_ALIGNMENT)); + assert(reinterpret_cast(newInertiaTensorLocal) % GLOBAL_ALIGNMENT == 0); + Matrix3x3* newInertiaTensorWorldInverses = reinterpret_cast(MemoryAllocator::alignAddress(newInertiaTensorLocalInverses + nbComponentsToAllocate, GLOBAL_ALIGNMENT)); + assert(reinterpret_cast(newInertiaTensorWorldInverses) % GLOBAL_ALIGNMENT == 0); + Vector3* newConstrainedLinearVelocities = reinterpret_cast(MemoryAllocator::alignAddress(newInertiaTensorWorldInverses + nbComponentsToAllocate, GLOBAL_ALIGNMENT)); + assert(reinterpret_cast(newConstrainedLinearVelocities) % GLOBAL_ALIGNMENT == 0); + Vector3* newConstrainedAngularVelocities = reinterpret_cast(MemoryAllocator::alignAddress(newConstrainedLinearVelocities + nbComponentsToAllocate, GLOBAL_ALIGNMENT)); + assert(reinterpret_cast(newConstrainedAngularVelocities) % GLOBAL_ALIGNMENT == 0); + Vector3* newSplitLinearVelocities = reinterpret_cast(MemoryAllocator::alignAddress(newConstrainedAngularVelocities + nbComponentsToAllocate, GLOBAL_ALIGNMENT)); + assert(reinterpret_cast(newSplitLinearVelocities) % GLOBAL_ALIGNMENT == 0); + Vector3* newSplitAngularVelocities = reinterpret_cast(MemoryAllocator::alignAddress(newSplitLinearVelocities + nbComponentsToAllocate, GLOBAL_ALIGNMENT)); + assert(reinterpret_cast(newSplitAngularVelocities) % GLOBAL_ALIGNMENT == 0); + Vector3* newConstrainedPositions = reinterpret_cast(MemoryAllocator::alignAddress(newSplitAngularVelocities + nbComponentsToAllocate, GLOBAL_ALIGNMENT)); + assert(reinterpret_cast(newConstrainedPositions) % GLOBAL_ALIGNMENT == 0); + Quaternion* newConstrainedOrientations = reinterpret_cast(MemoryAllocator::alignAddress(newConstrainedPositions + nbComponentsToAllocate, GLOBAL_ALIGNMENT)); + assert(reinterpret_cast(newConstrainedOrientations) % GLOBAL_ALIGNMENT == 0); + Vector3* newCentersOfMassLocal = reinterpret_cast(MemoryAllocator::alignAddress(newConstrainedOrientations + nbComponentsToAllocate, GLOBAL_ALIGNMENT)); + assert(reinterpret_cast(newCentersOfMassLocal) % GLOBAL_ALIGNMENT == 0); + Vector3* newCentersOfMassWorld = reinterpret_cast(MemoryAllocator::alignAddress(newCentersOfMassLocal + nbComponentsToAllocate, GLOBAL_ALIGNMENT)); + assert(reinterpret_cast(newCentersOfMassWorld) % GLOBAL_ALIGNMENT == 0); + bool* newIsGravityEnabled = reinterpret_cast(MemoryAllocator::alignAddress(newCentersOfMassWorld + nbComponentsToAllocate, GLOBAL_ALIGNMENT)); + assert(reinterpret_cast(newIsGravityEnabled) % GLOBAL_ALIGNMENT == 0); + bool* newIsAlreadyInIsland = reinterpret_cast(MemoryAllocator::alignAddress(newIsGravityEnabled + nbComponentsToAllocate, GLOBAL_ALIGNMENT)); + assert(reinterpret_cast(newIsAlreadyInIsland) % GLOBAL_ALIGNMENT == 0); + Array* newJoints = reinterpret_cast*>(MemoryAllocator::alignAddress(newIsAlreadyInIsland + nbComponentsToAllocate, GLOBAL_ALIGNMENT)); + assert(reinterpret_cast(newJoints) % GLOBAL_ALIGNMENT == 0); + Array* newContactPairs = reinterpret_cast*>(MemoryAllocator::alignAddress(newJoints + nbComponentsToAllocate, GLOBAL_ALIGNMENT)); + assert(reinterpret_cast(newContactPairs) % GLOBAL_ALIGNMENT == 0); + Vector3* newLinearLockAxisFactors = reinterpret_cast(MemoryAllocator::alignAddress(newContactPairs + nbComponentsToAllocate, GLOBAL_ALIGNMENT)); + assert(reinterpret_cast(newLinearLockAxisFactors) % GLOBAL_ALIGNMENT == 0); + Vector3* newAngularLockAxisFactors = reinterpret_cast(MemoryAllocator::alignAddress(newLinearLockAxisFactors + nbComponentsToAllocate, GLOBAL_ALIGNMENT)); + assert(reinterpret_cast(newAngularLockAxisFactors) % GLOBAL_ALIGNMENT == 0); + assert(reinterpret_cast(newAngularLockAxisFactors + nbComponentsToAllocate) <= reinterpret_cast(newBuffer) + totalSizeBytes); // If there was already components before if (mNbComponents > 0) { diff --git a/src/components/SliderJointComponents.cpp b/src/components/SliderJointComponents.cpp index 4a6da7a0..97eeab24 100644 --- a/src/components/SliderJointComponents.cpp +++ b/src/components/SliderJointComponents.cpp @@ -45,10 +45,8 @@ SliderJointComponents::SliderJointComponents(MemoryAllocator& allocator) sizeof(bool) + sizeof(bool) + sizeof(decimal) + sizeof(decimal) + sizeof(bool) + sizeof(bool) + sizeof(decimal) + sizeof(decimal) + sizeof(Vector3) + sizeof(Vector3) + sizeof(Vector3) + sizeof(Vector3) + - sizeof(Vector3) + sizeof(Vector3)) { + sizeof(Vector3) + sizeof(Vector3), 40 * GLOBAL_ALIGNMENT) { - // Allocate memory for the components data - allocate(INIT_NB_ALLOCATED_COMPONENTS); } // Allocate memory for a given number of components @@ -57,53 +55,94 @@ void SliderJointComponents::allocate(uint32 nbComponentsToAllocate) { assert(nbComponentsToAllocate > mNbAllocatedComponents); // Size for the data of a single component (in bytes) - const size_t totalSizeBytes = nbComponentsToAllocate * mComponentDataSize; + const size_t totalSizeBytes = nbComponentsToAllocate * mComponentDataSize + mAlignmentMarginSize; // Allocate memory void* newBuffer = mMemoryAllocator.allocate(totalSizeBytes); assert(newBuffer != nullptr); + assert(reinterpret_cast(newBuffer) % GLOBAL_ALIGNMENT == 0); // New pointers to components data Entity* newJointEntities = static_cast(newBuffer); - SliderJoint** newJoints = reinterpret_cast(newJointEntities + nbComponentsToAllocate); - Vector3* newLocalAnchorPointBody1 = reinterpret_cast(newJoints + nbComponentsToAllocate); - Vector3* newLocalAnchorPointBody2 = reinterpret_cast(newLocalAnchorPointBody1 + nbComponentsToAllocate); - Matrix3x3* newI1 = reinterpret_cast(newLocalAnchorPointBody2 + nbComponentsToAllocate); - Matrix3x3* newI2 = reinterpret_cast(newI1 + nbComponentsToAllocate); - Vector2* newImpulseTranslation = reinterpret_cast(newI2 + nbComponentsToAllocate); - Vector3* newImpulseRotation = reinterpret_cast(newImpulseTranslation + nbComponentsToAllocate); - Matrix2x2* newInverseMassMatrixTranslation = reinterpret_cast(newImpulseRotation + nbComponentsToAllocate); - Matrix3x3* newInverseMassMatrixRotation = reinterpret_cast(newInverseMassMatrixTranslation + nbComponentsToAllocate); - Vector2* newBiasTranslation = reinterpret_cast(newInverseMassMatrixRotation + nbComponentsToAllocate); - Vector3* newBiasRotation = reinterpret_cast(newBiasTranslation + nbComponentsToAllocate); - Quaternion* newInitOrientationDifferenceInv = reinterpret_cast(newBiasRotation + nbComponentsToAllocate); - Vector3* newSliderAxisBody1 = reinterpret_cast(newInitOrientationDifferenceInv + nbComponentsToAllocate); - Vector3* newSliderAxisWorld = reinterpret_cast(newSliderAxisBody1 + nbComponentsToAllocate); - Vector3* newR1 = reinterpret_cast(newSliderAxisWorld + nbComponentsToAllocate); - Vector3* newR2 = reinterpret_cast(newR1 + nbComponentsToAllocate); - Vector3* newN1 = reinterpret_cast(newR2 + nbComponentsToAllocate); - Vector3* newN2 = reinterpret_cast(newN1 + nbComponentsToAllocate); - decimal* newImpulseLowerLimit = reinterpret_cast(newN2 + nbComponentsToAllocate); - decimal* newImpulseUpperLimit = reinterpret_cast(newImpulseLowerLimit + nbComponentsToAllocate); - decimal* newImpulseMotor = reinterpret_cast(newImpulseUpperLimit + nbComponentsToAllocate); - decimal* newInverseMassMatrixLimit = reinterpret_cast(newImpulseMotor + nbComponentsToAllocate); - decimal* newInverseMassMatrixMotor = reinterpret_cast(newInverseMassMatrixLimit + nbComponentsToAllocate); - decimal* newBLowerLimit = reinterpret_cast(newInverseMassMatrixMotor + nbComponentsToAllocate); - decimal* newBUpperLimit = reinterpret_cast(newBLowerLimit + nbComponentsToAllocate); - bool* newIsLimitEnabled = reinterpret_cast(newBUpperLimit + nbComponentsToAllocate); - bool* newIsMotorEnabled = reinterpret_cast(newIsLimitEnabled + nbComponentsToAllocate); - decimal* newLowerLimit = reinterpret_cast(newIsMotorEnabled + nbComponentsToAllocate); - decimal* newUpperLimit = reinterpret_cast(newLowerLimit + nbComponentsToAllocate); - bool* newIsLowerLimitViolated = reinterpret_cast(newUpperLimit + nbComponentsToAllocate); - bool* newIsUpperLimitViolated = reinterpret_cast(newIsLowerLimitViolated + nbComponentsToAllocate); - decimal* newMotorSpeed = reinterpret_cast(newIsUpperLimitViolated + nbComponentsToAllocate); - decimal* newMaxMotorForce = reinterpret_cast(newMotorSpeed + nbComponentsToAllocate); - Vector3* newR2CrossN1 = reinterpret_cast(newMaxMotorForce + nbComponentsToAllocate); - Vector3* newR2CrossN2 = reinterpret_cast(newR2CrossN1 + nbComponentsToAllocate); - Vector3* newR2CrossSliderAxis = reinterpret_cast(newR2CrossN2 + nbComponentsToAllocate); - Vector3* newR1PlusUCrossN1 = reinterpret_cast(newR2CrossSliderAxis + nbComponentsToAllocate); - Vector3* newR1PlusUCrossN2 = reinterpret_cast(newR1PlusUCrossN1 + nbComponentsToAllocate); - Vector3* newR1PlusUCrossSliderAxis = reinterpret_cast(newR1PlusUCrossN2 + nbComponentsToAllocate); + SliderJoint** newJoints = reinterpret_cast(MemoryAllocator::alignAddress(newJointEntities + nbComponentsToAllocate, GLOBAL_ALIGNMENT)); + assert(reinterpret_cast(newJoints) % GLOBAL_ALIGNMENT == 0); + Vector3* newLocalAnchorPointBody1 = reinterpret_cast(MemoryAllocator::alignAddress(newJoints + nbComponentsToAllocate, GLOBAL_ALIGNMENT)); + assert(reinterpret_cast(newLocalAnchorPointBody1) % GLOBAL_ALIGNMENT == 0); + Vector3* newLocalAnchorPointBody2 = reinterpret_cast(MemoryAllocator::alignAddress(newLocalAnchorPointBody1 + nbComponentsToAllocate, GLOBAL_ALIGNMENT)); + assert(reinterpret_cast(newLocalAnchorPointBody2) % GLOBAL_ALIGNMENT == 0); + Matrix3x3* newI1 = reinterpret_cast(MemoryAllocator::alignAddress(newLocalAnchorPointBody2 + nbComponentsToAllocate, GLOBAL_ALIGNMENT)); + assert(reinterpret_cast(newI1) % GLOBAL_ALIGNMENT == 0); + Matrix3x3* newI2 = reinterpret_cast(MemoryAllocator::alignAddress(newI1 + nbComponentsToAllocate, GLOBAL_ALIGNMENT)); + assert(reinterpret_cast(newI2) % GLOBAL_ALIGNMENT == 0); + Vector2* newImpulseTranslation = reinterpret_cast(MemoryAllocator::alignAddress(newI2 + nbComponentsToAllocate, GLOBAL_ALIGNMENT)); + assert(reinterpret_cast(newImpulseTranslation) % GLOBAL_ALIGNMENT == 0); + Vector3* newImpulseRotation = reinterpret_cast(MemoryAllocator::alignAddress(newImpulseTranslation + nbComponentsToAllocate, GLOBAL_ALIGNMENT)); + assert(reinterpret_cast(newImpulseRotation) % GLOBAL_ALIGNMENT == 0); + Matrix2x2* newInverseMassMatrixTranslation = reinterpret_cast(MemoryAllocator::alignAddress(newImpulseRotation + nbComponentsToAllocate, GLOBAL_ALIGNMENT)); + assert(reinterpret_cast(newInverseMassMatrixTranslation) % GLOBAL_ALIGNMENT == 0); + Matrix3x3* newInverseMassMatrixRotation = reinterpret_cast(MemoryAllocator::alignAddress(newInverseMassMatrixTranslation + nbComponentsToAllocate, GLOBAL_ALIGNMENT)); + assert(reinterpret_cast(newInverseMassMatrixRotation) % GLOBAL_ALIGNMENT == 0); + Vector2* newBiasTranslation = reinterpret_cast(MemoryAllocator::alignAddress(newInverseMassMatrixRotation + nbComponentsToAllocate, GLOBAL_ALIGNMENT)); + assert(reinterpret_cast(newBiasTranslation) % GLOBAL_ALIGNMENT == 0); + Vector3* newBiasRotation = reinterpret_cast(MemoryAllocator::alignAddress(newBiasTranslation + nbComponentsToAllocate, GLOBAL_ALIGNMENT)); + assert(reinterpret_cast(newBiasRotation) % GLOBAL_ALIGNMENT == 0); + Quaternion* newInitOrientationDifferenceInv = reinterpret_cast(MemoryAllocator::alignAddress(newBiasRotation + nbComponentsToAllocate, GLOBAL_ALIGNMENT)); + assert(reinterpret_cast(newInitOrientationDifferenceInv) % GLOBAL_ALIGNMENT == 0); + Vector3* newSliderAxisBody1 = reinterpret_cast(MemoryAllocator::alignAddress(newInitOrientationDifferenceInv + nbComponentsToAllocate, GLOBAL_ALIGNMENT)); + assert(reinterpret_cast(newSliderAxisBody1) % GLOBAL_ALIGNMENT == 0); + Vector3* newSliderAxisWorld = reinterpret_cast(MemoryAllocator::alignAddress(newSliderAxisBody1 + nbComponentsToAllocate, GLOBAL_ALIGNMENT)); + assert(reinterpret_cast(newSliderAxisWorld) % GLOBAL_ALIGNMENT == 0); + Vector3* newR1 = reinterpret_cast(MemoryAllocator::alignAddress(newSliderAxisWorld + nbComponentsToAllocate, GLOBAL_ALIGNMENT)); + assert(reinterpret_cast(newR1) % GLOBAL_ALIGNMENT == 0); + Vector3* newR2 = reinterpret_cast(MemoryAllocator::alignAddress(newR1 + nbComponentsToAllocate, GLOBAL_ALIGNMENT)); + assert(reinterpret_cast(newR2) % GLOBAL_ALIGNMENT == 0); + Vector3* newN1 = reinterpret_cast(MemoryAllocator::alignAddress(newR2 + nbComponentsToAllocate, GLOBAL_ALIGNMENT)); + assert(reinterpret_cast(newN1) % GLOBAL_ALIGNMENT == 0); + Vector3* newN2 = reinterpret_cast(MemoryAllocator::alignAddress(newN1 + nbComponentsToAllocate, GLOBAL_ALIGNMENT)); + assert(reinterpret_cast(newN2) % GLOBAL_ALIGNMENT == 0); + decimal* newImpulseLowerLimit = reinterpret_cast(MemoryAllocator::alignAddress(newN2 + nbComponentsToAllocate, GLOBAL_ALIGNMENT)); + assert(reinterpret_cast(newImpulseLowerLimit) % GLOBAL_ALIGNMENT == 0); + decimal* newImpulseUpperLimit = reinterpret_cast(MemoryAllocator::alignAddress(newImpulseLowerLimit + nbComponentsToAllocate, GLOBAL_ALIGNMENT)); + assert(reinterpret_cast(newImpulseUpperLimit) % GLOBAL_ALIGNMENT == 0); + decimal* newImpulseMotor = reinterpret_cast(MemoryAllocator::alignAddress(newImpulseUpperLimit + nbComponentsToAllocate, GLOBAL_ALIGNMENT)); + assert(reinterpret_cast(newImpulseMotor) % GLOBAL_ALIGNMENT == 0); + decimal* newInverseMassMatrixLimit = reinterpret_cast(MemoryAllocator::alignAddress(newImpulseMotor + nbComponentsToAllocate, GLOBAL_ALIGNMENT)); + assert(reinterpret_cast(newInverseMassMatrixLimit) % GLOBAL_ALIGNMENT == 0); + decimal* newInverseMassMatrixMotor = reinterpret_cast(MemoryAllocator::alignAddress(newInverseMassMatrixLimit + nbComponentsToAllocate, GLOBAL_ALIGNMENT)); + assert(reinterpret_cast(newInverseMassMatrixMotor) % GLOBAL_ALIGNMENT == 0); + decimal* newBLowerLimit = reinterpret_cast(MemoryAllocator::alignAddress(newInverseMassMatrixMotor + nbComponentsToAllocate, GLOBAL_ALIGNMENT)); + assert(reinterpret_cast(newBLowerLimit) % GLOBAL_ALIGNMENT == 0); + decimal* newBUpperLimit = reinterpret_cast(MemoryAllocator::alignAddress(newBLowerLimit + nbComponentsToAllocate, GLOBAL_ALIGNMENT)); + assert(reinterpret_cast(newBUpperLimit) % GLOBAL_ALIGNMENT == 0); + bool* newIsLimitEnabled = reinterpret_cast(MemoryAllocator::alignAddress(newBUpperLimit + nbComponentsToAllocate, GLOBAL_ALIGNMENT)); + assert(reinterpret_cast(newIsLimitEnabled) % GLOBAL_ALIGNMENT == 0); + bool* newIsMotorEnabled = reinterpret_cast(MemoryAllocator::alignAddress(newIsLimitEnabled + nbComponentsToAllocate, GLOBAL_ALIGNMENT)); + assert(reinterpret_cast(newIsMotorEnabled) % GLOBAL_ALIGNMENT == 0); + decimal* newLowerLimit = reinterpret_cast(MemoryAllocator::alignAddress(newIsMotorEnabled + nbComponentsToAllocate, GLOBAL_ALIGNMENT)); + assert(reinterpret_cast(newLowerLimit) % GLOBAL_ALIGNMENT == 0); + decimal* newUpperLimit = reinterpret_cast(MemoryAllocator::alignAddress(newLowerLimit + nbComponentsToAllocate, GLOBAL_ALIGNMENT)); + assert(reinterpret_cast(newUpperLimit) % GLOBAL_ALIGNMENT == 0); + bool* newIsLowerLimitViolated = reinterpret_cast(MemoryAllocator::alignAddress(newUpperLimit + nbComponentsToAllocate, GLOBAL_ALIGNMENT)); + assert(reinterpret_cast(newIsLowerLimitViolated) % GLOBAL_ALIGNMENT == 0); + bool* newIsUpperLimitViolated = reinterpret_cast(MemoryAllocator::alignAddress(newIsLowerLimitViolated + nbComponentsToAllocate, GLOBAL_ALIGNMENT)); + assert(reinterpret_cast(newIsUpperLimitViolated) % GLOBAL_ALIGNMENT == 0); + decimal* newMotorSpeed = reinterpret_cast(MemoryAllocator::alignAddress(newIsUpperLimitViolated + nbComponentsToAllocate, GLOBAL_ALIGNMENT)); + assert(reinterpret_cast(newMotorSpeed) % GLOBAL_ALIGNMENT == 0); + decimal* newMaxMotorForce = reinterpret_cast(MemoryAllocator::alignAddress(newMotorSpeed + nbComponentsToAllocate, GLOBAL_ALIGNMENT)); + assert(reinterpret_cast(newMaxMotorForce) % GLOBAL_ALIGNMENT == 0); + Vector3* newR2CrossN1 = reinterpret_cast(MemoryAllocator::alignAddress(newMaxMotorForce + nbComponentsToAllocate, GLOBAL_ALIGNMENT)); + assert(reinterpret_cast(newR2CrossN1) % GLOBAL_ALIGNMENT == 0); + Vector3* newR2CrossN2 = reinterpret_cast(MemoryAllocator::alignAddress(newR2CrossN1 + nbComponentsToAllocate, GLOBAL_ALIGNMENT)); + assert(reinterpret_cast(newR2CrossN2) % GLOBAL_ALIGNMENT == 0); + Vector3* newR2CrossSliderAxis = reinterpret_cast(MemoryAllocator::alignAddress(newR2CrossN2 + nbComponentsToAllocate, GLOBAL_ALIGNMENT)); + assert(reinterpret_cast(newR2CrossSliderAxis) % GLOBAL_ALIGNMENT == 0); + Vector3* newR1PlusUCrossN1 = reinterpret_cast(MemoryAllocator::alignAddress(newR2CrossSliderAxis + nbComponentsToAllocate, GLOBAL_ALIGNMENT)); + assert(reinterpret_cast(newR1PlusUCrossN1) % GLOBAL_ALIGNMENT == 0); + Vector3* newR1PlusUCrossN2 = reinterpret_cast(MemoryAllocator::alignAddress(newR1PlusUCrossN1 + nbComponentsToAllocate, GLOBAL_ALIGNMENT)); + assert(reinterpret_cast(newR1PlusUCrossN2) % GLOBAL_ALIGNMENT == 0); + Vector3* newR1PlusUCrossSliderAxis = reinterpret_cast(MemoryAllocator::alignAddress(newR1PlusUCrossN2 + nbComponentsToAllocate, GLOBAL_ALIGNMENT)); + assert(reinterpret_cast(newR1PlusUCrossSliderAxis) % GLOBAL_ALIGNMENT == 0); + assert(reinterpret_cast(newR1PlusUCrossSliderAxis + nbComponentsToAllocate) <= reinterpret_cast(newBuffer) + totalSizeBytes); // If there was already components before if (mNbComponents > 0) { diff --git a/src/components/TransformComponents.cpp b/src/components/TransformComponents.cpp index aa7d5a58..2970e36e 100644 --- a/src/components/TransformComponents.cpp +++ b/src/components/TransformComponents.cpp @@ -26,6 +26,7 @@ // Libraries #include #include +#include #include #include @@ -35,10 +36,9 @@ using namespace reactphysics3d; // Constructor TransformComponents::TransformComponents(MemoryAllocator& allocator) - :Components(allocator, sizeof(Entity) + sizeof(Transform) + 2 * GLOBAL_ALIGNMENT) { + :Components(allocator, sizeof(Entity) + sizeof(Transform), 2 * GLOBAL_ALIGNMENT) { + - // Allocate memory for the components data - allocate(INIT_NB_ALLOCATED_COMPONENTS); } // Allocate memory for a given number of components @@ -47,17 +47,19 @@ void TransformComponents::allocate(uint32 nbComponentsToAllocate) { assert(nbComponentsToAllocate > mNbAllocatedComponents); // Size for the data of a single component (in bytes) - const size_t totalSizeBytes = nbComponentsToAllocate * mComponentDataSize; + const size_t totalSizeBytes = nbComponentsToAllocate * mComponentDataSize + mAlignmentMarginSize; // Allocate memory void* newBuffer = mMemoryAllocator.allocate(totalSizeBytes); assert(newBuffer != nullptr); + assert(reinterpret_cast(newBuffer) % GLOBAL_ALIGNMENT == 0); // New pointers to components data Entity* newEntities = static_cast(newBuffer); - //assert(reinterpret_cast(newEntities) % GLOBAL_ALIGNMENT == 0); - Transform* newTransforms = reinterpret_cast(newEntities + nbComponentsToAllocate); - //assert(reinterpret_cast(newTransforms) % GLOBAL_ALIGNMENT == 0); + assert(reinterpret_cast(newEntities) % GLOBAL_ALIGNMENT == 0); + Transform* newTransforms = reinterpret_cast(MemoryAllocator::alignAddress(newEntities + nbComponentsToAllocate, GLOBAL_ALIGNMENT)); + assert(reinterpret_cast(newTransforms) % GLOBAL_ALIGNMENT == 0); + assert(reinterpret_cast(newTransforms + nbComponentsToAllocate) <= reinterpret_cast(newBuffer) + totalSizeBytes); // If there was already components before if (mNbComponents > 0) { diff --git a/src/engine/PhysicsWorld.cpp b/src/engine/PhysicsWorld.cpp index b6058e6d..d3529be1 100644 --- a/src/engine/PhysicsWorld.cpp +++ b/src/engine/PhysicsWorld.cpp @@ -106,6 +106,16 @@ PhysicsWorld::PhysicsWorld(MemoryManager& memoryManager, PhysicsCommon& physicsC mNbWorlds++; + mTransformComponents.init(); + mCollidersComponents.init(); + mCollisionBodyComponents.init(); + mRigidBodyComponents.init(); + mJointsComponents.init(); + mBallAndSocketJointsComponents.init(); + mFixedJointsComponents.init(); + mSliderJointsComponents.init(); + mHingeJointsComponents.init(); + RP3D_LOG(mConfig.worldName, Logger::Level::Information, Logger::Category::World, "Physics World: Physics world " + mName + " has been created", __FILE__, __LINE__); RP3D_LOG(mConfig.worldName, Logger::Level::Information, Logger::Category::World,