From 4dae9449c94a5772efe07e789544515ca81c6704 Mon Sep 17 00:00:00 2001 From: Daniel Chappuis Date: Sat, 3 Jul 2021 22:28:19 +0200 Subject: [PATCH] Fix issue with sleeping bodies and joints and small changes --- include/reactphysics3d/engine/PhysicsWorld.h | 2 +- src/constraint/BallAndSocketJoint.cpp | 14 ++++- src/engine/PhysicsWorld.cpp | 58 +++++++++----------- 3 files changed, 40 insertions(+), 34 deletions(-) diff --git a/include/reactphysics3d/engine/PhysicsWorld.h b/include/reactphysics3d/engine/PhysicsWorld.h index a6f894eb..539c18fe 100644 --- a/include/reactphysics3d/engine/PhysicsWorld.h +++ b/include/reactphysics3d/engine/PhysicsWorld.h @@ -379,7 +379,7 @@ class PhysicsWorld { RigidBody* createRigidBody(const Transform& transform); /// Disable the joints for pair of sleeping bodies - void disableJointsOfSleepingBodies(); + void enableDisableJoints(); /// Destroy a rigid body and all the joints which it belongs void destroyRigidBody(RigidBody* rigidBody); diff --git a/src/constraint/BallAndSocketJoint.cpp b/src/constraint/BallAndSocketJoint.cpp index 2b63c63c..68ca766b 100644 --- a/src/constraint/BallAndSocketJoint.cpp +++ b/src/constraint/BallAndSocketJoint.cpp @@ -64,6 +64,8 @@ BallAndSocketJoint::BallAndSocketJoint(Entity entity, PhysicsWorld& world, const // Enable/disable the cone limit of the joint void BallAndSocketJoint::enableConeLimit(bool isLimitEnabled) { mWorld.mBallAndSocketJointsComponents.setIsConeLimitEnabled(mEntity, isLimitEnabled); + + awakeBodies(); } // Return true if the cone limit or the joint is enabled @@ -76,17 +78,27 @@ bool BallAndSocketJoint::isConeLimitEnabled() const { * @param coneHalfAngle The angle of the cone limit (in radian) from [0; PI] */ void BallAndSocketJoint::setConeLimitHalfAngle(decimal coneHalfAngle) { - mWorld.mBallAndSocketJointsComponents.setConeLimitHalfAngle(mEntity, coneHalfAngle); + + if (mWorld.mBallAndSocketJointsComponents.getConeLimitHalfAngle(mEntity) != coneHalfAngle) { + + mWorld.mBallAndSocketJointsComponents.setConeLimitHalfAngle(mEntity, coneHalfAngle); + + awakeBodies(); + } } // Set the normalized cone limit axis of body 1 in local-space of body 1 void BallAndSocketJoint::setConeLimitLocalAxisBody1(const Vector3& localAxisBody1) { mWorld.mBallAndSocketJointsComponents.setConeLimitLocalAxisBody1(mEntity, localAxisBody1); + + awakeBodies(); } // Set the normalized cone limit axis of body 2 in local-space of body 2 void BallAndSocketJoint::setConeLimitLocalAxisBody2(const Vector3& localAxisBody2) { mWorld.mBallAndSocketJointsComponents.setConeLimitLocalAxisBody2(mEntity, localAxisBody2); + + awakeBodies(); } // Return the cone angle limit (in radians) from [0; PI] diff --git a/src/engine/PhysicsWorld.cpp b/src/engine/PhysicsWorld.cpp index a6fe538d..6a2689f6 100644 --- a/src/engine/PhysicsWorld.cpp +++ b/src/engine/PhysicsWorld.cpp @@ -244,28 +244,6 @@ void PhysicsWorld::setBodyDisabled(Entity bodyEntity, bool isDisabled) { mCollidersComponents.setIsEntityDisabled(collidersEntities[i], isDisabled); } - - // Disable the joints of the body if necessary - // For each joint of the body - const Array& joints = mRigidBodyComponents.getJoints(bodyEntity); - const uint32 nbJoints = joints.size(); - for(uint32 i=0; i < nbJoints; i++) { - - const Entity body1Entity = mJointsComponents.getBody1Entity(joints[i]); - const Entity body2Entity = mJointsComponents.getBody2Entity(joints[i]); - - // If both bodies of the joint are disabled - if (mRigidBodyComponents.getIsEntityDisabled(body1Entity) && mRigidBodyComponents.getIsEntityDisabled(body2Entity)) { - - // We disable the joint - setJointDisabled(joints[i], true); - } - else { - - // Enable the joint - setJointDisabled(joints[i], false); - } - } } // Notify the world whether a joint is disabled or not @@ -349,8 +327,8 @@ void PhysicsWorld::update(decimal timeStep) { // Recompute the inverse inertia tensors of rigid bodies updateBodiesInverseWorldInertiaTensors(); - // Disable the joints for pair of sleeping bodies - disableJointsOfSleepingBodies(); + // Enable or disable the joints + enableDisableJoints(); // Integrate the velocities mDynamicsSystem.integrateRigidBodiesVelocities(timeStep); @@ -441,20 +419,36 @@ void PhysicsWorld::solvePositionCorrection() { } } -// Disable the joints for pair of sleeping bodies -void PhysicsWorld::disableJointsOfSleepingBodies() { +// Enable or disable the joints +void PhysicsWorld::enableDisableJoints() { + + const uint32 nbJointComponents = mJointsComponents.getNbComponents(); + + Array jointsEntites(mMemoryManager.getHeapAllocator(), nbJointComponents); + + // Get all the joints entities + for (uint32 i = 0; i < nbJointComponents; i++) { + jointsEntites.add(mJointsComponents.mJointEntities[i]); + } // For each joint - for (uint32 i=0; i < mJointsComponents.getNbEnabledComponents(); i++) { + for (uint32 i = 0; i < nbJointComponents; i++) { - Entity body1 = mJointsComponents.mBody1Entities[i]; - Entity body2 = mJointsComponents.mBody2Entities[i]; + uint32 jointEntityIndex = mJointsComponents.getEntityIndex(jointsEntites[i]); + + Entity body1 = mJointsComponents.mBody1Entities[jointEntityIndex]; + Entity body2 = mJointsComponents.mBody2Entities[jointEntityIndex]; // If both bodies of the joint are disabled - if (mCollisionBodyComponents.getIsEntityDisabled(body1) && mCollisionBodyComponents.getIsEntityDisabled(body2)) { + if (mCollisionBodyComponents.getIsEntityDisabled(body1) || mCollisionBodyComponents.getIsEntityDisabled(body2)) { // Disable the joint - setJointDisabled(mJointsComponents.mJointEntities[i], true); + setJointDisabled(jointsEntites[i], true); + } + else { + + // Enable the joint + setJointDisabled(jointsEntites[i], false); } } } @@ -822,7 +816,7 @@ void PhysicsWorld::createIslands() { // Compute the body index in the array (Note that it could have changed because of the previous call to rigidBodyToVisit->setIsSleeping(false)) const uint32 bodyToVisitIndex = mRigidBodyComponents.getEntityIndex(bodyToVisitEntity); - // If the currenbodyEntityIndicesToVisitt body is static, we do not want to perform the DFS search across that body + // If the current body is static, we do not want to perform the DFS search across that body if (mRigidBodyComponents.mBodyTypes[bodyToVisitIndex] == BodyType::STATIC) { staticBodiesAddedToIsland.add(bodyToVisitEntity);