From cef1f6cd22e8c564105b86920a66c09b74801a99 Mon Sep 17 00:00:00 2001 From: Daniel Chappuis Date: Mon, 19 Aug 2019 18:38:14 +0200 Subject: [PATCH] Working on JointComponents --- src/components/JointComponents.cpp | 25 ++++++++++- src/components/JointComponents.h | 64 ++++++++++++++++++++++++++- src/constraint/BallAndSocketJoint.cpp | 4 +- src/constraint/FixedJoint.cpp | 6 +-- src/constraint/HingeJoint.cpp | 10 ++--- src/constraint/Joint.cpp | 22 +++++++-- src/constraint/Joint.h | 26 ----------- src/constraint/SliderJoint.cpp | 12 ++--- src/engine/DynamicsWorld.cpp | 3 +- 9 files changed, 122 insertions(+), 50 deletions(-) diff --git a/src/components/JointComponents.cpp b/src/components/JointComponents.cpp index 41301d70..f867c3f1 100644 --- a/src/components/JointComponents.cpp +++ b/src/components/JointComponents.cpp @@ -33,8 +33,8 @@ using namespace reactphysics3d; // Constructor JointComponents::JointComponents(MemoryAllocator& allocator) - :Components(allocator, sizeof(Entity) + sizeof(Entity) + sizeof(Entity) + - sizeof(Joint*)) { + :Components(allocator, sizeof(Entity) + sizeof(Entity) + sizeof(Entity) + sizeof(Joint*) + + sizeof(JointType) + sizeof(JointsPositionCorrectionTechnique) + sizeof (bool)) { // Allocate memory for the components data allocate(INIT_NB_ALLOCATED_COMPONENTS); @@ -57,6 +57,9 @@ void JointComponents::allocate(uint32 nbComponentsToAllocate) { 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); // If there was already components before if (mNbComponents > 0) { @@ -66,6 +69,9 @@ void JointComponents::allocate(uint32 nbComponentsToAllocate) { memcpy(newBody1Entities, mBody1Entities, mNbComponents * sizeof(Entity)); memcpy(newBody2Entities, mBody2Entities, mNbComponents * sizeof(Entity)); memcpy(newJoints, mJoints, mNbComponents * sizeof(Joint*)); + memcpy(newTypes, mTypes, mNbComponents * sizeof(JointType)); + memcpy(newPositionCorrectionTechniques, mPositionCorrectionTechniques, mNbComponents * sizeof(JointsPositionCorrectionTechnique)); + memcpy(newIsCollisionEnabled, mIsCollisionEnabled, mNbComponents * sizeof(bool)); // Deallocate previous memory mMemoryAllocator.release(mBuffer, mNbAllocatedComponents * mComponentDataSize); @@ -77,6 +83,9 @@ void JointComponents::allocate(uint32 nbComponentsToAllocate) { mBody1Entities = newBody1Entities; mBody2Entities = newBody2Entities; mJoints = newJoints; + mTypes = newTypes; + mPositionCorrectionTechniques = newPositionCorrectionTechniques; + mIsCollisionEnabled = newIsCollisionEnabled; } // Add a component @@ -90,6 +99,9 @@ void JointComponents::addComponent(Entity jointEntity, bool isSleeping, const Jo new (mBody1Entities + index) Entity(component.body1Entity); new (mBody2Entities + index) Entity(component.body2Entity); mJoints[index] = component.joint; + new (mTypes + index) JointType(component.jointType); + new (mPositionCorrectionTechniques + index) JointsPositionCorrectionTechnique(component.positionCorrectionTechnique); + mIsCollisionEnabled[index] = component.isCollisionEnabled; // Map the entity with the new component lookup index mMapEntityToComponentIndex.add(Pair(jointEntity, index)); @@ -111,6 +123,9 @@ void JointComponents::moveComponentToIndex(uint32 srcIndex, uint32 destIndex) { new (mBody1Entities + destIndex) Entity(mBody1Entities[srcIndex]); new (mBody2Entities + destIndex) Entity(mBody2Entities[srcIndex]); mJoints[destIndex] = mJoints[srcIndex]; + new (mTypes + destIndex) JointType(mTypes[srcIndex]); + new (mPositionCorrectionTechniques + destIndex) JointsPositionCorrectionTechnique(mPositionCorrectionTechniques[srcIndex]); + mIsCollisionEnabled[destIndex] = mIsCollisionEnabled[srcIndex]; // Destroy the source component destroyComponent(srcIndex); @@ -131,6 +146,9 @@ void JointComponents::swapComponents(uint32 index1, uint32 index2) { Entity body1Entity1(mBody1Entities[index1]); Entity body2Entity1(mBody2Entities[index1]); Joint* joint1 = mJoints[index1]; + JointType jointType1(mTypes[index1]); + JointsPositionCorrectionTechnique positionCorrectionTechnique1(mPositionCorrectionTechniques[index1]); + bool isCollisionEnabled1 = mIsCollisionEnabled[index1]; // Destroy component 1 destroyComponent(index1); @@ -142,6 +160,9 @@ void JointComponents::swapComponents(uint32 index1, uint32 index2) { new (mBody1Entities + index2) Entity(body1Entity1); new (mBody2Entities + index2) Entity(body2Entity1); mJoints[index2] = joint1; + new (mTypes + index2) JointType(jointType1); + new (mPositionCorrectionTechniques + index2) JointsPositionCorrectionTechnique(positionCorrectionTechnique1); + mIsCollisionEnabled[index2] = isCollisionEnabled1; // Update the entity to component index mapping mMapEntityToComponentIndex.add(Pair(jointEntity1, index2)); diff --git a/src/components/JointComponents.h b/src/components/JointComponents.h index fe25ab34..7193620c 100644 --- a/src/components/JointComponents.h +++ b/src/components/JointComponents.h @@ -39,6 +39,7 @@ namespace reactphysics3d { class MemoryAllocator; class EntityManager; class Joint; +enum class JointType; // Class JointComponents /** @@ -63,6 +64,15 @@ class JointComponents : public Components { /// Array with pointers to the joints Joint** mJoints; + /// Array of type of the joints + JointType* mTypes; + + /// Array of position correction techniques used for the joints + JointsPositionCorrectionTechnique* mPositionCorrectionTechniques; + + /// Array of boolean values to know if the two bodies of the constraint are allowed to collide with each other + bool* mIsCollisionEnabled; + // -------------------- Methods -------------------- // /// Allocate memory for a given number of components @@ -85,10 +95,15 @@ class JointComponents : public Components { const Entity body1Entity; const Entity body2Entity; Joint* joint; + JointType jointType; + JointsPositionCorrectionTechnique positionCorrectionTechnique; + bool isCollisionEnabled; /// Constructor - JointComponent(Entity body1Entity, Entity body2Entity, Joint* joint) - : body1Entity(body1Entity), body2Entity(body2Entity), joint(joint) { + JointComponent(Entity body1Entity, Entity body2Entity, Joint* joint, JointType jointType, + JointsPositionCorrectionTechnique positionCorrectionTechnique, bool isCollisionEnabled) + : body1Entity(body1Entity), body2Entity(body2Entity), joint(joint), jointType(jointType), + positionCorrectionTechnique(positionCorrectionTechnique), isCollisionEnabled(isCollisionEnabled) { } }; @@ -113,6 +128,21 @@ class JointComponents : public Components { /// Return a pointer to the joint Joint* getJoint(Entity jointEntity) const; + /// Return the type of a joint + JointType getType(Entity jointEntity) const; + + /// Return the position correction technique of a joint + JointsPositionCorrectionTechnique getPositionCorrectionTechnique(Entity jointEntity) const; + + /// Set the position correction technique of a joint + void getPositionCorrectionTechnique(Entity jointEntity, JointsPositionCorrectionTechnique positionCorrectionTechnique); + + /// Return true if the collision is enabled between the two bodies of a joint + bool getIsCollisionEnabled(Entity jointEntity) const; + + /// Set whether the collision is enabled between the two bodies of a joint + void setIsCollisionEnabled(Entity jointEntity, bool isCollisionEnabled); + // -------------------- Friendship -------------------- // friend class BroadPhaseSystem; @@ -136,6 +166,36 @@ inline Joint* JointComponents::getJoint(Entity jointEntity) const { return mJoints[mMapEntityToComponentIndex[jointEntity]]; } +// Return the type of a joint +inline JointType JointComponents::getType(Entity jointEntity) const { + assert(mMapEntityToComponentIndex.containsKey(jointEntity)); + return mTypes[mMapEntityToComponentIndex[jointEntity]]; +} + +// Return the position correction technique of a joint +inline JointsPositionCorrectionTechnique JointComponents::getPositionCorrectionTechnique(Entity jointEntity) const { + assert(mMapEntityToComponentIndex.containsKey(jointEntity)); + return mPositionCorrectionTechniques[mMapEntityToComponentIndex[jointEntity]]; +} + +// Set the position correction technique of a joint +inline void JointComponents::getPositionCorrectionTechnique(Entity jointEntity, JointsPositionCorrectionTechnique positionCorrectionTechnique) { + assert(mMapEntityToComponentIndex.containsKey(jointEntity)); + mPositionCorrectionTechniques[mMapEntityToComponentIndex[jointEntity]] = positionCorrectionTechnique; +} + +// Return true if the collision is enabled between the two bodies of a joint +inline bool JointComponents::getIsCollisionEnabled(Entity jointEntity) const { + assert(mMapEntityToComponentIndex.containsKey(jointEntity)); + return mIsCollisionEnabled[mMapEntityToComponentIndex[jointEntity]]; +} + +// Set whether the collision is enabled between the two bodies of a joint +inline void JointComponents::setIsCollisionEnabled(Entity jointEntity, bool isCollisionEnabled) { + assert(mMapEntityToComponentIndex.containsKey(jointEntity)); + mIsCollisionEnabled[mMapEntityToComponentIndex[jointEntity]] = isCollisionEnabled; +} + } #endif diff --git a/src/constraint/BallAndSocketJoint.cpp b/src/constraint/BallAndSocketJoint.cpp index bf1ff7f9..8bd52a0d 100644 --- a/src/constraint/BallAndSocketJoint.cpp +++ b/src/constraint/BallAndSocketJoint.cpp @@ -95,7 +95,7 @@ void BallAndSocketJoint::initBeforeSolve(const ConstraintSolverData& constraintS // Compute the bias "b" of the constraint mBiasVector.setToZero(); - if (mPositionCorrectionTechnique == JointsPositionCorrectionTechnique::BAUMGARTE_JOINTS) { + if (mWorld.mJointsComponents.getPositionCorrectionTechnique(mEntity) == JointsPositionCorrectionTechnique::BAUMGARTE_JOINTS) { decimal biasFactor = (BETA / constraintSolverData.timeStep); mBiasVector = biasFactor * (x2 + mR2World - x1 - mR1World); } @@ -189,7 +189,7 @@ void BallAndSocketJoint::solvePositionConstraint(const ConstraintSolverData& con // If the error position correction technique is not the non-linear-gauss-seidel, we do // do not execute this method - if (mPositionCorrectionTechnique != JointsPositionCorrectionTechnique::NON_LINEAR_GAUSS_SEIDEL) return; + if (mWorld.mJointsComponents.getPositionCorrectionTechnique(mEntity) != JointsPositionCorrectionTechnique::NON_LINEAR_GAUSS_SEIDEL) return; // Get the bodies center of mass and orientations Vector3 x1 = constraintSolverData.rigidBodyComponents.getConstrainedPosition(body1Entity); diff --git a/src/constraint/FixedJoint.cpp b/src/constraint/FixedJoint.cpp index b971cba3..afb84b7f 100644 --- a/src/constraint/FixedJoint.cpp +++ b/src/constraint/FixedJoint.cpp @@ -108,7 +108,7 @@ void FixedJoint::initBeforeSolve(const ConstraintSolverData& constraintSolverDat // Compute the bias "b" of the constraint for the 3 translation constraints decimal biasFactor = (BETA / constraintSolverData.timeStep); mBiasTranslation.setToZero(); - if (mPositionCorrectionTechnique == JointsPositionCorrectionTechnique::BAUMGARTE_JOINTS) { + if (mWorld.mJointsComponents.getPositionCorrectionTechnique(mEntity) == JointsPositionCorrectionTechnique::BAUMGARTE_JOINTS) { mBiasTranslation = biasFactor * (x2 + mR2World - x1 - mR1World); } @@ -123,7 +123,7 @@ void FixedJoint::initBeforeSolve(const ConstraintSolverData& constraintSolverDat // Compute the bias "b" for the 3 rotation constraints mBiasRotation.setToZero(); - if (mPositionCorrectionTechnique == JointsPositionCorrectionTechnique::BAUMGARTE_JOINTS) { + if (mWorld.mJointsComponents.getPositionCorrectionTechnique(mEntity) == JointsPositionCorrectionTechnique::BAUMGARTE_JOINTS) { const Quaternion qError = orientationBody2 * mInitOrientationDifferenceInv * orientationBody1.getInverse(); mBiasRotation = biasFactor * decimal(2.0) * qError.getVectorV(); } @@ -256,7 +256,7 @@ void FixedJoint::solvePositionConstraint(const ConstraintSolverData& constraintS // If the error position correction technique is not the non-linear-gauss-seidel, we do // do not execute this method - if (mPositionCorrectionTechnique != JointsPositionCorrectionTechnique::NON_LINEAR_GAUSS_SEIDEL) return; + if (mWorld.mJointsComponents.getPositionCorrectionTechnique(mEntity) != JointsPositionCorrectionTechnique::NON_LINEAR_GAUSS_SEIDEL) return; // Get the bodies positions and orientations Vector3 x1 = constraintSolverData.rigidBodyComponents.getConstrainedPosition(body1Entity); diff --git a/src/constraint/HingeJoint.cpp b/src/constraint/HingeJoint.cpp index 22593dc0..12f1c6bb 100644 --- a/src/constraint/HingeJoint.cpp +++ b/src/constraint/HingeJoint.cpp @@ -139,7 +139,7 @@ void HingeJoint::initBeforeSolve(const ConstraintSolverData& constraintSolverDat // Compute the bias "b" of the translation constraints mBTranslation.setToZero(); decimal biasFactor = (BETA / constraintSolverData.timeStep); - if (mPositionCorrectionTechnique == JointsPositionCorrectionTechnique::BAUMGARTE_JOINTS) { + if (mWorld.mJointsComponents.getPositionCorrectionTechnique(mEntity) == JointsPositionCorrectionTechnique::BAUMGARTE_JOINTS) { mBTranslation = biasFactor * (x2 + mR2World - x1 - mR1World); } @@ -165,7 +165,7 @@ void HingeJoint::initBeforeSolve(const ConstraintSolverData& constraintSolverDat // Compute the bias "b" of the rotation constraints mBRotation.setToZero(); - if (mPositionCorrectionTechnique == JointsPositionCorrectionTechnique::BAUMGARTE_JOINTS) { + if (mWorld.mJointsComponents.getPositionCorrectionTechnique(mEntity) == JointsPositionCorrectionTechnique::BAUMGARTE_JOINTS) { mBRotation = biasFactor * Vector2(mA1.dot(b2), mA1.dot(c2)); } @@ -192,13 +192,13 @@ void HingeJoint::initBeforeSolve(const ConstraintSolverData& constraintSolverDat // Compute the bias "b" of the lower limit constraint mBLowerLimit = 0.0; - if (mPositionCorrectionTechnique == JointsPositionCorrectionTechnique::BAUMGARTE_JOINTS) { + if (mWorld.mJointsComponents.getPositionCorrectionTechnique(mEntity) == JointsPositionCorrectionTechnique::BAUMGARTE_JOINTS) { mBLowerLimit = biasFactor * lowerLimitError; } // Compute the bias "b" of the upper limit constraint mBUpperLimit = 0.0; - if (mPositionCorrectionTechnique == JointsPositionCorrectionTechnique::BAUMGARTE_JOINTS) { + if (mWorld.mJointsComponents.getPositionCorrectionTechnique(mEntity) == JointsPositionCorrectionTechnique::BAUMGARTE_JOINTS) { mBUpperLimit = biasFactor * upperLimitError; } } @@ -426,7 +426,7 @@ void HingeJoint::solvePositionConstraint(const ConstraintSolverData& constraintS // If the error position correction technique is not the non-linear-gauss-seidel, we do // do not execute this method - if (mPositionCorrectionTechnique != JointsPositionCorrectionTechnique::NON_LINEAR_GAUSS_SEIDEL) return; + if (mWorld.mJointsComponents.getPositionCorrectionTechnique(mEntity) != JointsPositionCorrectionTechnique::NON_LINEAR_GAUSS_SEIDEL) return; // Get the bodies entities Entity body1Entity = mWorld.mJointsComponents.getBody1Entity(mEntity); diff --git a/src/constraint/Joint.cpp b/src/constraint/Joint.cpp index 85b2df58..9bb68be3 100644 --- a/src/constraint/Joint.cpp +++ b/src/constraint/Joint.cpp @@ -31,9 +31,7 @@ using namespace reactphysics3d; // Constructor Joint::Joint(Entity entity, DynamicsWorld& world, const JointInfo& jointInfo) - :mEntity(entity), mWorld(world), mType(jointInfo.type), - mPositionCorrectionTechnique(jointInfo.positionCorrectionTechnique), - mIsCollisionEnabled(jointInfo.isCollisionEnabled), mIsAlreadyInIsland(false) { + :mEntity(entity), mWorld(world), mIsAlreadyInIsland(false) { } @@ -55,6 +53,24 @@ RigidBody* Joint::getBody2() const { return mWorld.mRigidBodyComponents.getRigidBody(body2Entiy); } + +// Return the type of the joint +/** + * @return The type of the joint + */ +JointType Joint::getType() const { + return mWorld.mJointsComponents.getType(mEntity); +} + +// Return true if the collision between the two bodies of the joint is enabled +/** + * @return True if the collision is enabled between the two bodies of the joint + * is enabled and false otherwise + */ +bool Joint::isCollisionEnabled() const { + return mWorld.mJointsComponents.getIsCollisionEnabled(mEntity); +} + // Awake the two bodies of the joint void Joint::awakeBodies() const { diff --git a/src/constraint/Joint.h b/src/constraint/Joint.h index e1e7e960..eabd11dc 100644 --- a/src/constraint/Joint.h +++ b/src/constraint/Joint.h @@ -126,15 +126,6 @@ class Joint { /// Reference to the physics world DynamicsWorld& mWorld; - /// Type of the joint - const JointType mType; - - /// Position correction technique used for the constraint (used for joints) - JointsPositionCorrectionTechnique mPositionCorrectionTechnique; - - /// True if the two bodies of the constraint are allowed to collide with each other - bool mIsCollisionEnabled; - /// True if the joint has already been added into an island bool mIsAlreadyInIsland; @@ -205,23 +196,6 @@ class Joint { friend class ConstraintSolverSystem; }; -// Return the type of the joint -/** - * @return The type of the joint - */ -inline JointType Joint::getType() const { - return mType; -} - -// Return true if the collision between the two bodies of the joint is enabled -/** - * @return True if the collision is enabled between the two bodies of the joint - * is enabled and false otherwise - */ -inline bool Joint::isCollisionEnabled() const { - return mIsCollisionEnabled; -} - // Return the entity id of the joint /** * @return The entity of the joint diff --git a/src/constraint/SliderJoint.cpp b/src/constraint/SliderJoint.cpp index 57a236bd..6fd63529 100644 --- a/src/constraint/SliderJoint.cpp +++ b/src/constraint/SliderJoint.cpp @@ -161,7 +161,7 @@ void SliderJoint::initBeforeSolve(const ConstraintSolverData& constraintSolverDa // Compute the bias "b" of the translation constraint mBTranslation.setToZero(); decimal biasFactor = (BETA / constraintSolverData.timeStep); - if (mPositionCorrectionTechnique == JointsPositionCorrectionTechnique::BAUMGARTE_JOINTS) { + if (mWorld.mJointsComponents.getPositionCorrectionTechnique(mEntity) == JointsPositionCorrectionTechnique::BAUMGARTE_JOINTS) { mBTranslation.x = u.dot(mN1); mBTranslation.y = u.dot(mN2); mBTranslation *= biasFactor; @@ -178,7 +178,7 @@ void SliderJoint::initBeforeSolve(const ConstraintSolverData& constraintSolverDa // Compute the bias "b" of the rotation constraint mBRotation.setToZero(); - if (mPositionCorrectionTechnique == JointsPositionCorrectionTechnique::BAUMGARTE_JOINTS) { + if (mWorld.mJointsComponents.getPositionCorrectionTechnique(mEntity) == JointsPositionCorrectionTechnique::BAUMGARTE_JOINTS) { const Quaternion qError = orientationBody2 * mInitOrientationDifferenceInv * orientationBody1.getInverse(); mBRotation = biasFactor * decimal(2.0) * qError.getVectorV(); } @@ -195,13 +195,13 @@ void SliderJoint::initBeforeSolve(const ConstraintSolverData& constraintSolverDa // Compute the bias "b" of the lower limit constraint mBLowerLimit = 0.0; - if (mPositionCorrectionTechnique == JointsPositionCorrectionTechnique::BAUMGARTE_JOINTS) { + if (mWorld.mJointsComponents.getPositionCorrectionTechnique(mEntity) == JointsPositionCorrectionTechnique::BAUMGARTE_JOINTS) { mBLowerLimit = biasFactor * lowerLimitError; } // Compute the bias "b" of the upper limit constraint mBUpperLimit = 0.0; - if (mPositionCorrectionTechnique == JointsPositionCorrectionTechnique::BAUMGARTE_JOINTS) { + if (mWorld.mJointsComponents.getPositionCorrectionTechnique(mEntity) == JointsPositionCorrectionTechnique::BAUMGARTE_JOINTS) { mBUpperLimit = biasFactor * upperLimitError; } } @@ -211,7 +211,7 @@ void SliderJoint::initBeforeSolve(const ConstraintSolverData& constraintSolverDa // Compute the inverse of mass matrix K=JM^-1J^t for the motor (1x1 matrix) mInverseMassMatrixMotor = sumInverseMass; - mInverseMassMatrixMotor = (mInverseMassMatrixMotor > 0.0) ? + mInverseMassMatrixMotor = (mInverseMassMatrixMotor > decimal(0.0)) ? decimal(1.0) / mInverseMassMatrixMotor : decimal(0.0); } @@ -462,7 +462,7 @@ void SliderJoint::solvePositionConstraint(const ConstraintSolverData& constraint // If the error position correction technique is not the non-linear-gauss-seidel, we do // do not execute this method - if (mPositionCorrectionTechnique != JointsPositionCorrectionTechnique::NON_LINEAR_GAUSS_SEIDEL) return; + if (mWorld.mJointsComponents.getPositionCorrectionTechnique(mEntity) != JointsPositionCorrectionTechnique::NON_LINEAR_GAUSS_SEIDEL) return; // Get the bodies entities const Entity body1Entity = mWorld.mJointsComponents.getBody1Entity(mEntity); diff --git a/src/engine/DynamicsWorld.cpp b/src/engine/DynamicsWorld.cpp index bae7d6c2..65041ed8 100644 --- a/src/engine/DynamicsWorld.cpp +++ b/src/engine/DynamicsWorld.cpp @@ -375,7 +375,8 @@ Joint* DynamicsWorld::createJoint(const JointInfo& jointInfo) { bool isJointDisabled = mRigidBodyComponents.getIsEntityDisabled(jointInfo.body1->getEntity()) && mRigidBodyComponents.getIsEntityDisabled(jointInfo.body2->getEntity()); - JointComponents::JointComponent jointComponent(jointInfo.body1->getEntity(), jointInfo.body2->getEntity(), newJoint); + JointComponents::JointComponent jointComponent(jointInfo.body1->getEntity(), jointInfo.body2->getEntity(), newJoint, jointInfo.type, + jointInfo.positionCorrectionTechnique, jointInfo.isCollisionEnabled); mJointsComponents.addComponent(entity, isJointDisabled, jointComponent); // If the collision between the two bodies of the constraint is disabled