diff --git a/src/body/CollisionBody.cpp b/src/body/CollisionBody.cpp index a0c3c068..3eaeb3a7 100644 --- a/src/body/CollisionBody.cpp +++ b/src/body/CollisionBody.cpp @@ -86,9 +86,10 @@ ProxyShape* CollisionBody::addCollisionShape(CollisionShape* collisionShape, con Vector3 localBoundsMax; // TODO : Maybe this method can directly returns an AABB collisionShape->getLocalBounds(localBoundsMin, localBoundsMax); + const Transform localToWorldTransform = mWorld.mTransformComponents.getTransform(mEntity) * transform; ProxyShapeComponents::ProxyShapeComponent proxyShapeComponent(mEntity, proxyShape, AABB(localBoundsMin, localBoundsMax), - transform, collisionShape, decimal(1), 0x0001, 0xFFFF); + transform, collisionShape, decimal(1), 0x0001, 0xFFFF, localToWorldTransform); bool isActive = mWorld.mCollisionBodyComponents.getIsActive(mEntity); mWorld.mProxyShapesComponents.addComponent(proxyShapeEntity, !isActive, proxyShapeComponent); @@ -218,6 +219,11 @@ void CollisionBody::updateBroadPhaseState(decimal timeStep) const { const List& proxyShapesEntities = mWorld.mCollisionBodyComponents.getProxyShapes(mEntity); for (uint i=0; i < proxyShapesEntities.size(); i++) { + // Update the local-to-world transform of the proxy-shape + mWorld.mProxyShapesComponents.setLocalToWorldTransform(proxyShapesEntities[i], + mWorld.mTransformComponents.getTransform(mEntity) * + mWorld.mProxyShapesComponents.getLocalToBodyTransform(proxyShapesEntities[i])); + // Update the proxy mWorld.mCollisionDetection.updateProxyShape(proxyShapesEntities[i], timeStep); } diff --git a/src/body/RigidBody.cpp b/src/body/RigidBody.cpp index 592a57d5..5bb38982 100644 --- a/src/body/RigidBody.cpp +++ b/src/body/RigidBody.cpp @@ -336,10 +336,10 @@ ProxyShape* RigidBody::addCollisionShape(CollisionShape* collisionShape, Vector3 localBoundsMax; // TODO : Maybe this method can directly returns an AABB collisionShape->getLocalBounds(localBoundsMin, localBoundsMax); - + const Transform localToWorldTransform = mWorld.mTransformComponents.getTransform(mEntity) * transform; ProxyShapeComponents::ProxyShapeComponent proxyShapeComponent(mEntity, proxyShape, AABB(localBoundsMin, localBoundsMax), - transform, collisionShape, mass, 0x0001, 0xFFFF); + transform, collisionShape, mass, 0x0001, 0xFFFF, localToWorldTransform); bool isSleeping = mWorld.mRigidBodyComponents.getIsSleeping(mEntity); mWorld.mProxyShapesComponents.addComponent(proxyShapeEntity, isSleeping, proxyShapeComponent); @@ -510,9 +510,6 @@ void RigidBody::setTransform(const Transform& transform) { CollisionBody::setTransform(transform); - // Update the transform of the body - mWorld.mTransformComponents.setTransform(mEntity, transform); - // Awake the body if it is sleeping setIsSleeping(false); } diff --git a/src/collision/ProxyShape.cpp b/src/collision/ProxyShape.cpp index cc81f8c8..4dcb6d8b 100644 --- a/src/collision/ProxyShape.cpp +++ b/src/collision/ProxyShape.cpp @@ -115,6 +115,10 @@ void ProxyShape::setLocalToBodyTransform(const Transform& transform) { mBody->mWorld.mProxyShapesComponents.setLocalToBodyTransform(mEntity, transform); + // Update the local-to-world transform + const Transform& bodyTransform = mBody->mWorld.mTransformComponents.getTransform(mBody->getEntity()); + mBody->mWorld.mProxyShapesComponents.setLocalToWorldTransform(mEntity, bodyTransform * transform); + RigidBody* rigidBody = static_cast(mBody); if (rigidBody != nullptr) { mBody->mWorld.mRigidBodyComponents.setIsSleeping(mBody->getEntity(), false); @@ -122,8 +126,6 @@ void ProxyShape::setLocalToBodyTransform(const Transform& transform) { mBody->mWorld.mCollisionDetection.updateProxyShape(mEntity, 0); - int broadPhaseId = mBody->mWorld.mProxyShapesComponents.getBroadPhaseId(mEntity); - RP3D_LOG(mLogger, Logger::Level::Information, Logger::Category::ProxyShape, "ProxyShape " + std::to_string(broadPhaseId) + ": Set localToBodyTransform=" + transform.to_string()); @@ -183,7 +185,7 @@ bool ProxyShape::raycast(const Ray& ray, RaycastInfo& raycastInfo) { if (!mBody->isActive()) return false; // Convert the ray into the local-space of the collision shape - const Transform localToWorldTransform = getLocalToWorldTransform(); + const Transform localToWorldTransform = mBody->mWorld.mProxyShapesComponents.getLocalToWorldTransform(mEntity); const Transform worldToLocalTransform = localToWorldTransform.getInverse(); Ray rayLocal(worldToLocalTransform * ray.point1, worldToLocalTransform * ray.point2, @@ -222,8 +224,7 @@ unsigned short ProxyShape::getCollideWithMaskBits() const { * shape to the world-space */ const Transform ProxyShape::getLocalToWorldTransform() const { - return mBody->mWorld.mTransformComponents.getTransform(mBody->getEntity()) * - mBody->mWorld.mProxyShapesComponents.getLocalToBodyTransform(mEntity); + return mBody->mWorld.mProxyShapesComponents.getLocalToWorldTransform(mEntity); } #ifdef IS_PROFILING_ACTIVE diff --git a/src/components/ProxyShapeComponents.cpp b/src/components/ProxyShapeComponents.cpp index 4f06aebb..ab98115b 100644 --- a/src/components/ProxyShapeComponents.cpp +++ b/src/components/ProxyShapeComponents.cpp @@ -37,7 +37,7 @@ using namespace reactphysics3d; ProxyShapeComponents::ProxyShapeComponents(MemoryAllocator& allocator) :Components(allocator, sizeof(Entity) + sizeof(Entity) + sizeof(ProxyShape*) + sizeof(int) + sizeof(Transform) + sizeof(CollisionShape*) + sizeof(decimal) + sizeof(unsigned short) + - sizeof(unsigned short)) { + sizeof(unsigned short) + sizeof(Transform)) { // Allocate memory for the components data allocate(INIT_NB_ALLOCATED_COMPONENTS); @@ -65,6 +65,7 @@ void ProxyShapeComponents::allocate(uint32 nbComponentsToAllocate) { decimal* newMasses = reinterpret_cast(newCollisionShapes + nbComponentsToAllocate); unsigned short* newCollisionCategoryBits = reinterpret_cast(newMasses + nbComponentsToAllocate); unsigned short* newCollideWithMaskBits = reinterpret_cast(newCollisionCategoryBits + nbComponentsToAllocate); + Transform* newLocalToWorldTransforms = reinterpret_cast(newCollideWithMaskBits + nbComponentsToAllocate); // If there was already components before if (mNbComponents > 0) { @@ -79,6 +80,7 @@ void ProxyShapeComponents::allocate(uint32 nbComponentsToAllocate) { memcpy(newMasses, mMasses, mNbComponents * sizeof(decimal)); memcpy(newCollisionCategoryBits, mCollisionCategoryBits, mNbComponents * sizeof(unsigned short)); memcpy(newCollideWithMaskBits, mCollideWithMaskBits, mNbComponents * sizeof(unsigned short)); + memcpy(newLocalToWorldTransforms, mLocalToWorldTransforms, mNbComponents * sizeof(Transform)); // Deallocate previous memory mMemoryAllocator.release(mBuffer, mNbAllocatedComponents * mComponentDataSize); @@ -95,6 +97,7 @@ void ProxyShapeComponents::allocate(uint32 nbComponentsToAllocate) { mMasses = newMasses; mCollisionCategoryBits = newCollisionCategoryBits; mCollideWithMaskBits = newCollideWithMaskBits; + mLocalToWorldTransforms = newLocalToWorldTransforms; mNbAllocatedComponents = nbComponentsToAllocate; } @@ -115,6 +118,7 @@ void ProxyShapeComponents::addComponent(Entity proxyShapeEntity, bool isSleeping new (mMasses + index) decimal(component.mass); new (mCollisionCategoryBits + index) unsigned short(component.collisionCategoryBits); new (mCollideWithMaskBits + index) unsigned short(component.collideWithMaskBits); + new (mLocalToWorldTransforms + index) Transform(component.localToWorldTransform); // Map the entity with the new component lookup index mMapEntityToComponentIndex.add(Pair(proxyShapeEntity, index)); @@ -140,6 +144,7 @@ void ProxyShapeComponents::moveComponentToIndex(uint32 srcIndex, uint32 destInde new (mMasses + destIndex) decimal(mMasses[srcIndex]); new (mCollisionCategoryBits + destIndex) unsigned short(mCollisionCategoryBits[srcIndex]); new (mCollideWithMaskBits + destIndex) unsigned short(mCollideWithMaskBits[srcIndex]); + new (mLocalToWorldTransforms + destIndex) Transform(mLocalToWorldTransforms[srcIndex]); // Destroy the source component destroyComponent(srcIndex); @@ -165,6 +170,7 @@ void ProxyShapeComponents::swapComponents(uint32 index1, uint32 index2) { decimal mass1 = mMasses[index1]; unsigned short collisionCategoryBits1 = mCollisionCategoryBits[index1]; unsigned short collideWithMaskBits1 = mCollideWithMaskBits[index1]; + Transform localToWorldTransform1 = mLocalToWorldTransforms[index1]; // Destroy component 1 destroyComponent(index1); @@ -181,6 +187,7 @@ void ProxyShapeComponents::swapComponents(uint32 index1, uint32 index2) { new (mMasses + index2) decimal(mass1); new (mCollisionCategoryBits + index2) unsigned short(collisionCategoryBits1); new (mCollideWithMaskBits + index2) unsigned short(collideWithMaskBits1); + new (mLocalToWorldTransforms + index2) Transform(localToWorldTransform1); // Update the entity to component index mapping mMapEntityToComponentIndex.add(Pair(proxyShapeEntity1, index2)); @@ -204,4 +211,5 @@ void ProxyShapeComponents::destroyComponent(uint32 index) { mProxyShapes[index] = nullptr; mLocalToBodyTransforms[index].~Transform(); mCollisionShapes[index] = nullptr; + mLocalToWorldTransforms[index].~Transform(); } diff --git a/src/components/ProxyShapeComponents.h b/src/components/ProxyShapeComponents.h index c5d9cf25..9c7e5650 100644 --- a/src/components/ProxyShapeComponents.h +++ b/src/components/ProxyShapeComponents.h @@ -65,7 +65,6 @@ class ProxyShapeComponents : public Components { ProxyShape** mProxyShapes; /// Ids of the proxy-shapes for the broad-phase algorithm - // TODO : Try to change type to uint32 int32* mBroadPhaseIds; /// Transform from local-space of the proxy-shape to the body-space of its body @@ -90,6 +89,9 @@ class ProxyShapeComponents : public Components { /// proxy shape will collide with every collision categories by default. unsigned short* mCollideWithMaskBits; + /// Array with the local-to-world transforms of the proxy-shapes + Transform* mLocalToWorldTransforms; + // -------------------- Methods -------------------- // /// Allocate memory for a given number of components @@ -112,18 +114,20 @@ class ProxyShapeComponents : public Components { Entity bodyEntity; ProxyShape* proxyShape; AABB localBounds; - Transform localToBodyTransform; + const Transform& localToBodyTransform; CollisionShape* collisionShape; decimal mass; unsigned short collisionCategoryBits; unsigned short collideWithMaskBits; + const Transform& localToWorldTransform; /// Constructor - ProxyShapeComponent(Entity bodyEntity, ProxyShape* proxyShape, AABB localBounds, Transform localToBodyTransform, + ProxyShapeComponent(Entity bodyEntity, ProxyShape* proxyShape, AABB localBounds, const Transform& localToBodyTransform, CollisionShape* collisionShape, decimal mass, unsigned short collisionCategoryBits, - unsigned short collideWithMaskBits) + unsigned short collideWithMaskBits, const Transform& localToWorldTransform) :bodyEntity(bodyEntity), proxyShape(proxyShape), localBounds(localBounds), localToBodyTransform(localToBodyTransform), - collisionShape(collisionShape), mass(mass), collisionCategoryBits(collisionCategoryBits), collideWithMaskBits(collideWithMaskBits) { + collisionShape(collisionShape), mass(mass), collisionCategoryBits(collisionCategoryBits), collideWithMaskBits(collideWithMaskBits), + localToWorldTransform(localToWorldTransform) { } }; @@ -175,6 +179,12 @@ class ProxyShapeComponents : public Components { /// Set the "collide with" mask bits of a given proxy-shape void setCollideWithMaskBits(Entity proxyShapeEntity, unsigned short collideWithMaskBits); + /// Return the local-to-world transform of a proxy-shape + const Transform& getLocalToWorldTransform(Entity proxyShapeEntity) const; + + /// Set the local-to-world transform of a proxy-shape + void setLocalToWorldTransform(Entity proxyShapeEntity, const Transform& transform); + // -------------------- Friendship -------------------- // friend class BroadPhaseSystem; @@ -276,6 +286,22 @@ inline void ProxyShapeComponents::setCollideWithMaskBits(Entity proxyShapeEntity mCollideWithMaskBits[mMapEntityToComponentIndex[proxyShapeEntity]] = collideWithMaskBits; } +// Return the local-to-world transform of a proxy-shape +inline const Transform& ProxyShapeComponents::getLocalToWorldTransform(Entity proxyShapeEntity) const { + + assert(mMapEntityToComponentIndex.containsKey(proxyShapeEntity)); + + return mLocalToWorldTransforms[mMapEntityToComponentIndex[proxyShapeEntity]]; +} + +// Set the local-to-world transform of a proxy-shape +inline void ProxyShapeComponents::setLocalToWorldTransform(Entity proxyShapeEntity, const Transform& transform) { + + assert(mMapEntityToComponentIndex.containsKey(proxyShapeEntity)); + + mLocalToWorldTransforms[mMapEntityToComponentIndex[proxyShapeEntity]] = transform; +} + } #endif diff --git a/src/constraint/BallAndSocketJoint.cpp b/src/constraint/BallAndSocketJoint.cpp index c098289d..b138afdf 100644 --- a/src/constraint/BallAndSocketJoint.cpp +++ b/src/constraint/BallAndSocketJoint.cpp @@ -39,8 +39,8 @@ BallAndSocketJoint::BallAndSocketJoint(Entity entity, DynamicsWorld& world, cons : Joint(entity, world) { // Get the transforms of the two bodies - Transform& body1Transform = mWorld.mTransformComponents.getTransform(jointInfo.body1->getEntity()); - Transform& body2Transform = mWorld.mTransformComponents.getTransform(jointInfo.body2->getEntity()); + const Transform& body1Transform = mWorld.mTransformComponents.getTransform(jointInfo.body1->getEntity()); + const Transform& body2Transform = mWorld.mTransformComponents.getTransform(jointInfo.body2->getEntity()); // Compute the local-space anchor point for each body mWorld.mBallAndSocketJointsComponents.setLocalAnchorPointBody1(entity, body1Transform.getInverse() * jointInfo.anchorPointWorldSpace); diff --git a/src/constraint/HingeJoint.cpp b/src/constraint/HingeJoint.cpp index 6141f1bf..dd10ad27 100644 --- a/src/constraint/HingeJoint.cpp +++ b/src/constraint/HingeJoint.cpp @@ -40,8 +40,8 @@ HingeJoint::HingeJoint(Entity entity, DynamicsWorld &world, const HingeJointInfo assert(upperLimit >= decimal(0) && upperLimit <= decimal(2.0) * PI); // Compute the local-space anchor point for each body - Transform& transform1 = mWorld.mTransformComponents.getTransform(jointInfo.body1->getEntity()); - Transform& transform2 = mWorld.mTransformComponents.getTransform(jointInfo.body2->getEntity()); + const Transform& transform1 = mWorld.mTransformComponents.getTransform(jointInfo.body1->getEntity()); + const Transform& transform2 = mWorld.mTransformComponents.getTransform(jointInfo.body2->getEntity()); mWorld.mHingeJointsComponents.setLocalAnchorPointBody1(mEntity, transform1.getInverse() * jointInfo.anchorPointWorldSpace); mWorld.mHingeJointsComponents.setLocalAnchorPointBody2(mEntity, transform2.getInverse() * jointInfo.anchorPointWorldSpace); diff --git a/src/engine/DynamicsWorld.cpp b/src/engine/DynamicsWorld.cpp index 0bc02766..7af7f783 100644 --- a/src/engine/DynamicsWorld.cpp +++ b/src/engine/DynamicsWorld.cpp @@ -55,7 +55,7 @@ DynamicsWorld::DynamicsWorld(const Vector3& gravity, const WorldSettings& worldS mConstraintSolverSystem(*this, mIslands, mRigidBodyComponents, mTransformComponents, mJointsComponents, mBallAndSocketJointsComponents, mFixedJointsComponents, mHingeJointsComponents, mSliderJointsComponents), - mDynamicsSystem(*this, mRigidBodyComponents, mTransformComponents, mIsGravityEnabled, mGravity), + mDynamicsSystem(*this, mCollisionBodyComponents, mRigidBodyComponents, mTransformComponents, mProxyShapesComponents, mIsGravityEnabled, mGravity), mNbVelocitySolverIterations(mConfig.defaultVelocitySolverNbIterations), mNbPositionSolverIterations(mConfig.defaultPositionSolverNbIterations), mIsSleepingEnabled(mConfig.isSleepingEnabled), mRigidBodies(mMemoryManager.getPoolAllocator()), diff --git a/src/systems/CollisionDetectionSystem.cpp b/src/systems/CollisionDetectionSystem.cpp index a45ef2e6..5dea49e9 100644 --- a/src/systems/CollisionDetectionSystem.cpp +++ b/src/systems/CollisionDetectionSystem.cpp @@ -202,7 +202,7 @@ void CollisionDetectionSystem::updateOverlappingPairs(const List> // Compute the middle-phase collision detection void CollisionDetectionSystem::computeMiddlePhase(OverlappingPairMap& overlappingPairs, NarrowPhaseInput& narrowPhaseInput) { - RP3D_PROFILE("CollisionDetection::computeMiddlePhase()", mProfiler); + RP3D_PROFILE("CollisionDetectionSystem::computeMiddlePhase()", mProfiler); // Reserve memory for the narrow-phase input using cached capacity from previous frame narrowPhaseInput.reserveMemory(); @@ -262,8 +262,9 @@ void CollisionDetectionSystem::computeMiddlePhase(OverlappingPairMap& overlappin // No middle-phase is necessary, simply create a narrow phase info // for the narrow-phase collision detection narrowPhaseInput.addNarrowPhaseTest(pair, shape1->getCollisionShape(), shape2->getCollisionShape(), - shape1->getLocalToWorldTransform(), shape2->getLocalToWorldTransform(), - algorithmType, mMemoryManager.getSingleFrameAllocator()); + mProxyShapesComponents.getLocalToWorldTransform(proxyShape1Entity), + mProxyShapesComponents.getLocalToWorldTransform(proxyShape2Entity), + algorithmType, mMemoryManager.getSingleFrameAllocator()); } // Concave vs Convex algorithm @@ -318,8 +319,8 @@ void CollisionDetectionSystem::computeConvexVsConcaveMiddlePhase(OverlappingPair assert(algorithmType != NarrowPhaseAlgorithmType::None); // Compute the convex shape AABB in the local-space of the convex shape - const Transform convexToConcaveTransform = concaveProxyShape->getLocalToWorldTransform().getInverse() * - convexProxyShape->getLocalToWorldTransform(); + const Transform convexToConcaveTransform = mProxyShapesComponents.getLocalToWorldTransform(concaveProxyShape->getEntity()).getInverse() * + mProxyShapesComponents.getLocalToWorldTransform(convexProxyShape->getEntity()); AABB aabb; convexShape->computeAABB(aabb, convexToConcaveTransform); @@ -356,7 +357,8 @@ void CollisionDetectionSystem::computeConvexVsConcaveMiddlePhase(OverlappingPair // Create a narrow phase info for the narrow-phase collision detection narrowPhaseInput.addNarrowPhaseTest(pair, isShape1Convex ? convexShape : triangleShape, isShape1Convex ? triangleShape : convexShape, - shape1->getLocalToWorldTransform(), shape2->getLocalToWorldTransform(), + mProxyShapesComponents.getLocalToWorldTransform(shape1->getEntity()), + mProxyShapesComponents.getLocalToWorldTransform(shape2->getEntity()), algorithmType, allocator); } } diff --git a/src/systems/ContactSolverSystem.cpp b/src/systems/ContactSolverSystem.cpp index 6b270cab..3d783e35 100644 --- a/src/systems/ContactSolverSystem.cpp +++ b/src/systems/ContactSolverSystem.cpp @@ -124,11 +124,6 @@ void ContactSolverSystem::initializeForIsland(uint islandIndex) { assert(!mBodyComponents.getIsEntityDisabled(externalManifold.bodyEntity1)); assert(!mBodyComponents.getIsEntityDisabled(externalManifold.bodyEntity2)); - // Get the two contact shapes - // TODO : Do we really need to get the proxy-shape here - const ProxyShape* shape1 = mProxyShapeComponents.getProxyShape(externalManifold.proxyShapeEntity1); - const ProxyShape* shape2 = mProxyShapeComponents.getProxyShape(externalManifold.proxyShapeEntity2); - // Get the position of the two bodies const Vector3& x1 = mRigidBodyComponents.getCenterOfMassWorld(externalManifold.bodyEntity1); const Vector3& x2 = mRigidBodyComponents.getCenterOfMassWorld(externalManifold.bodyEntity2); @@ -165,8 +160,8 @@ void ContactSolverSystem::initializeForIsland(uint islandIndex) { ContactPoint& externalContact = (*mAllContactPoints)[c]; // Get the contact point on the two bodies - Vector3 p1 = shape1->getLocalToWorldTransform() * externalContact.getLocalPointOnShape1(); - Vector3 p2 = shape2->getLocalToWorldTransform() * externalContact.getLocalPointOnShape2(); + Vector3 p1 = mProxyShapeComponents.getLocalToWorldTransform(externalManifold.proxyShapeEntity1) * externalContact.getLocalPointOnShape1(); + Vector3 p2 = mProxyShapeComponents.getLocalToWorldTransform(externalManifold.proxyShapeEntity2) * externalContact.getLocalPointOnShape2(); new (mContactPoints + mNbContactPoints) ContactPointSolver(); mContactPoints[mNbContactPoints].externalContact = &externalContact; diff --git a/src/systems/DynamicsSystem.cpp b/src/systems/DynamicsSystem.cpp index 4dcee2ac..1b7278a5 100644 --- a/src/systems/DynamicsSystem.cpp +++ b/src/systems/DynamicsSystem.cpp @@ -31,9 +31,10 @@ using namespace reactphysics3d; // Constructor -DynamicsSystem::DynamicsSystem(DynamicsWorld& world, RigidBodyComponents& rigidBodyComponents, TransformComponents& transformComponents, bool& isGravityEnabled, Vector3& gravity) - :mWorld(world), mRigidBodyComponents(rigidBodyComponents), mTransformComponents(transformComponents), mIsGravityEnabled(isGravityEnabled), - mGravity(gravity) { +DynamicsSystem::DynamicsSystem(DynamicsWorld& world, CollisionBodyComponents& collisionBodyComponents, RigidBodyComponents& rigidBodyComponents, + TransformComponents& transformComponents, ProxyShapeComponents& proxyShapeComponents, bool& isGravityEnabled, Vector3& gravity) + :mWorld(world), mCollisionBodyComponents(collisionBodyComponents), mRigidBodyComponents(rigidBodyComponents), mTransformComponents(transformComponents), mProxyShapeComponents(proxyShapeComponents), + mIsGravityEnabled(isGravityEnabled), mGravity(gravity) { } @@ -64,7 +65,7 @@ void DynamicsSystem::integrateRigidBodiesPositions(decimal timeStep, bool isSpli // Update the new constrained position and orientation of the body mRigidBodyComponents.mConstrainedPositions[i] = currentPosition + newLinVelocity * timeStep; mRigidBodyComponents.mConstrainedOrientations[i] = currentOrientation + Quaternion(0, newAngVelocity) * - currentOrientation * decimal(0.5) * timeStep; + currentOrientation * decimal(0.5) * timeStep; } } @@ -95,6 +96,20 @@ void DynamicsSystem::updateBodiesState() { const Vector3& centerOfMassLocal = mRigidBodyComponents.mCentersOfMassLocal[i]; transform.setPosition(centerOfMassWorld - transform.getOrientation() * centerOfMassLocal); } + + // Update the local-to-world transform of the proxy-shapes + for (uint32 i=0; i < mRigidBodyComponents.getNbEnabledComponents(); i++) { + + // For all the proxy collision shapes of the body + const List& proxyShapesEntities = mCollisionBodyComponents.getProxyShapes(mRigidBodyComponents.mBodiesEntities[i]); + for (uint j=0; j < proxyShapesEntities.size(); j++) { + + // Update the local-to-world transform of the proxy-shape + mProxyShapeComponents.setLocalToWorldTransform(proxyShapesEntities[j], + mTransformComponents.getTransform(mRigidBodyComponents.mBodiesEntities[i]) * + mProxyShapeComponents.getLocalToBodyTransform(proxyShapesEntities[j])); + } + } } // Integrate the velocities of rigid bodies. diff --git a/src/systems/DynamicsSystem.h b/src/systems/DynamicsSystem.h index a5c89c22..ed2d7d8a 100644 --- a/src/systems/DynamicsSystem.h +++ b/src/systems/DynamicsSystem.h @@ -28,8 +28,10 @@ // Libraries #include "utils/Profiler.h" +#include "components/CollisionBodyComponents.h" #include "components/RigidBodyComponents.h" #include "components/TransformComponents.h" +#include "components/ProxyShapeComponents.h" namespace reactphysics3d { @@ -49,12 +51,18 @@ class DynamicsSystem { /// Physics world DynamicsWorld& mWorld; + /// Reference to the collision body components + CollisionBodyComponents& mCollisionBodyComponents; + /// Reference to the rigid body components RigidBodyComponents& mRigidBodyComponents; /// Reference to the transform components TransformComponents& mTransformComponents; + /// Reference to the proxy-shapes components + ProxyShapeComponents& mProxyShapeComponents; + /// Reference to the variable to know if gravity is enabled in the world bool& mIsGravityEnabled; @@ -72,8 +80,9 @@ class DynamicsSystem { // -------------------- Methods -------------------- // /// Constructor - DynamicsSystem(DynamicsWorld& world, RigidBodyComponents& rigidBodyComponents, TransformComponents& transformComponents, - bool& isGravityEnabled, Vector3& gravity); + DynamicsSystem(DynamicsWorld& world, CollisionBodyComponents& collisionBodyComponents, + RigidBodyComponents& rigidBodyComponents, TransformComponents& transformComponents, + ProxyShapeComponents& proxyShapeComponents, bool& isGravityEnabled, Vector3& gravity); /// Destructor ~DynamicsSystem() = default; diff --git a/src/systems/SolveSliderJointSystem.cpp b/src/systems/SolveSliderJointSystem.cpp index aee14d08..e2f32ae4 100644 --- a/src/systems/SolveSliderJointSystem.cpp +++ b/src/systems/SolveSliderJointSystem.cpp @@ -619,7 +619,7 @@ void SolveSliderJointSystem::solvePositionConstraint() { const Entity body1Entity = mJointComponents.getBody1Entity(jointEntity); const Entity body2Entity = mJointComponents.getBody2Entity(jointEntity); - // Recompute the inertia tensor of bodies + // Recompute the inverse inertia tensors mSliderJointComponents.mI1[i] = RigidBody::getInertiaTensorInverseWorld(mWorld, body1Entity); mSliderJointComponents.mI2[i] = RigidBody::getInertiaTensorInverseWorld(mWorld, body2Entity); } @@ -665,11 +665,11 @@ void SolveSliderJointSystem::solvePositionConstraint() { const decimal inverseMassBody1 = mRigidBodyComponents.mInverseMasses[componentIndexBody1]; const decimal inverseMassBody2 = mRigidBodyComponents.mInverseMasses[componentIndexBody2]; - const Vector3& r1 = mSliderJointComponents.getR1(jointEntity); - const Vector3& r2 = mSliderJointComponents.getR2(jointEntity); + const Vector3& r1 = mSliderJointComponents.mR1[i]; + const Vector3& r2 = mSliderJointComponents.mR2[i]; - const Vector3& n1 = mSliderJointComponents.getN1(jointEntity); - const Vector3& n2 = mSliderJointComponents.getN2(jointEntity); + const Vector3& n1 = mSliderJointComponents.mN1[i]; + const Vector3& n2 = mSliderJointComponents.mN2[i]; Vector3& x1 = mRigidBodyComponents.mConstrainedPositions[componentIndexBody1]; Vector3& x2 = mRigidBodyComponents.mConstrainedPositions[componentIndexBody2]; @@ -683,29 +683,29 @@ void SolveSliderJointSystem::solvePositionConstraint() { // Compute the two orthogonal vectors to the slider axis in world-space mSliderJointComponents.mSliderAxisWorld[i] = q1 * mSliderJointComponents.mSliderAxisBody1[i]; mSliderJointComponents.mSliderAxisWorld[i].normalize(); - mSliderJointComponents.setN1(jointEntity, mSliderJointComponents.mSliderAxisWorld[i].getOneUnitOrthogonalVector()); - mSliderJointComponents.setN2(jointEntity, mSliderJointComponents.mSliderAxisWorld[i].cross(n1)); + mSliderJointComponents.mN1[i] = mSliderJointComponents.mSliderAxisWorld[i].getOneUnitOrthogonalVector(); + mSliderJointComponents.mN2[i] = mSliderJointComponents.mSliderAxisWorld[i].cross(n1); // Check if the limit constraints are violated or not decimal uDotSliderAxis = u.dot(mSliderJointComponents.mSliderAxisWorld[i]); decimal lowerLimitError = uDotSliderAxis - mSliderJointComponents.getLowerLimit(jointEntity); decimal upperLimitError = mSliderJointComponents.getUpperLimit(jointEntity) - uDotSliderAxis; - mSliderJointComponents.setIsLowerLimitViolated(jointEntity, lowerLimitError <= 0); - mSliderJointComponents.setIsUpperLimitViolated(jointEntity, upperLimitError <= 0); + mSliderJointComponents.mIsLowerLimitViolated[i] = lowerLimitError <= 0; + mSliderJointComponents.mIsUpperLimitViolated[i] = upperLimitError <= 0; // Compute the cross products used in the Jacobians - mSliderJointComponents.setR2CrossN1(jointEntity, r2.cross(n1)); - mSliderJointComponents.setR2CrossN2(jointEntity, r2.cross(n2)); - mSliderJointComponents.setR2CrossSliderAxis(jointEntity, r2.cross(mSliderJointComponents.mSliderAxisWorld[i])); + mSliderJointComponents.mR2CrossN1[i] = r2.cross(n1); + mSliderJointComponents.mR2CrossN2[i] = r2.cross(n2); + mSliderJointComponents.mR2CrossSliderAxis[i] = r2.cross(mSliderJointComponents.mSliderAxisWorld[i]); const Vector3 r1PlusU = r1 + u; - mSliderJointComponents.setR1PlusUCrossN1(jointEntity, r1PlusU.cross(n1)); - mSliderJointComponents.setR1PlusUCrossN2(jointEntity, r1PlusU.cross(n2)); - mSliderJointComponents.setR1PlusUCrossSliderAxis(jointEntity, r1PlusU.cross(mSliderJointComponents.mSliderAxisWorld[i])); + mSliderJointComponents.mR1PlusUCrossN1[i] = r1PlusU.cross(n1); + mSliderJointComponents.mR1PlusUCrossN2[i] = r1PlusU.cross(n2); + mSliderJointComponents.mR1PlusUCrossSliderAxis[i] = r1PlusU.cross(mSliderJointComponents.mSliderAxisWorld[i]); - const Vector3& r2CrossN1 = mSliderJointComponents.getR2CrossN1(jointEntity); - const Vector3& r2CrossN2 = mSliderJointComponents.getR2CrossN2(jointEntity); - const Vector3& r1PlusUCrossN1 = mSliderJointComponents.getR1PlusUCrossN1(jointEntity); - const Vector3& r1PlusUCrossN2 = mSliderJointComponents.getR1PlusUCrossN2(jointEntity); + const Vector3& r2CrossN1 = mSliderJointComponents.mR2CrossN1[i]; + const Vector3& r2CrossN2 = mSliderJointComponents.mR2CrossN2[i]; + const Vector3& r1PlusUCrossN1 = mSliderJointComponents.mR1PlusUCrossN1[i]; + const Vector3& r1PlusUCrossN2 = mSliderJointComponents.mR1PlusUCrossN2[i]; // --------------- Translation Constraints --------------- // @@ -730,18 +730,17 @@ void SolveSliderJointSystem::solvePositionConstraint() { const decimal el22 = sumInverseMass + r1PlusUCrossN2.dot(I1R1PlusUCrossN2) + r2CrossN2.dot(I2R2CrossN2); Matrix2x2 matrixKTranslation(el11, el12, el21, el22); - Matrix2x2& inverseMassMatrixTranslation = mSliderJointComponents.getInverseMassMatrixTranslation(jointEntity); - inverseMassMatrixTranslation.setToZero(); + mSliderJointComponents.mInverseMassMatrixTranslation[i].setToZero(); if (mRigidBodyComponents.mBodyTypes[componentIndexBody1] == BodyType::DYNAMIC || mRigidBodyComponents.mBodyTypes[componentIndexBody2] == BodyType::DYNAMIC) { - mSliderJointComponents.setInverseMassMatrixTranslation(jointEntity, matrixKTranslation.getInverse()); + mSliderJointComponents.mInverseMassMatrixTranslation[i] = matrixKTranslation.getInverse(); } // Compute the position error for the 2 translation constraints const Vector2 translationError(u.dot(n1), u.dot(n2)); // Compute the Lagrange multiplier lambda for the 2 translation constraints - Vector2 lambdaTranslation = inverseMassMatrixTranslation * (-translationError); + Vector2 lambdaTranslation = mSliderJointComponents.mInverseMassMatrixTranslation[i] * (-translationError); // Compute the impulse P=J^T * lambda for the 2 translation constraints of body 1 const Vector3 linearImpulseBody1 = -n1 * lambdaTranslation.x - n2 * lambdaTranslation.y;