mrq/reactphysics3d
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Working on JointComponents

Browse Source
This commit is contained in:
Daniel Chappuis 2019-08-19 18:38:14 +02:00
parent 0230b74462
commit cef1f6cd22
9 changed files with 122 additions and 50 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

25
src/components/JointComponents.cpp
View File

@ -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<Entity*>(newJointsEntities + nbComponentsToAllocate);
Entity* newBody2Entities = reinterpret_cast<Entity*>(newBody1Entities + nbComponentsToAllocate);
Joint** newJoints = reinterpret_cast<Joint**>(newBody2Entities + nbComponentsToAllocate);
JointType* newTypes = reinterpret_cast<JointType*>(newJoints + nbComponentsToAllocate);
JointsPositionCorrectionTechnique* newPositionCorrectionTechniques = reinterpret_cast<JointsPositionCorrectionTechnique*>(newTypes + nbComponentsToAllocate);
bool* newIsCollisionEnabled = reinterpret_cast<bool*>(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<Entity, uint32>(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<Entity, uint32>(jointEntity1, index2));

64
src/components/JointComponents.h
View File

@ -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

4
src/constraint/BallAndSocketJoint.cpp
View File

@ -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);

6
src/constraint/FixedJoint.cpp
View File

@ -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);

10
src/constraint/HingeJoint.cpp
View File

@ -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);

22
src/constraint/Joint.cpp
View File

@ -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 {

26
src/constraint/Joint.h
View File

@ -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

12
src/constraint/SliderJoint.cpp
View File

@ -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);

3
src/engine/DynamicsWorld.cpp
View File

@ -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