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

Add constrained position/orientation to DynamicsComponents

Browse Source
This commit is contained in:
Daniel Chappuis 2019-05-20 07:42:24 +02:00
parent a11d884ce1
commit 1bc7e0710b
9 changed files with 112 additions and 65 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

20
src/components/DynamicsComponents.cpp
View File

@ -38,7 +38,7 @@ DynamicsComponents::DynamicsComponents(MemoryAllocator& allocator)
:Components(allocator, sizeof(Entity) + sizeof(Vector3) + sizeof(Vector3) + sizeof(Vector3) + sizeof(Vector3) +
sizeof(Vector3) + sizeof(Vector3) + sizeof(Vector3) + sizeof(Vector3) + sizeof(decimal) +
sizeof(decimal) + sizeof(decimal) + sizeof(decimal) + sizeof(Matrix3x3) + sizeof(Matrix3x3) +
sizeof(bool) + sizeof(bool)) {
sizeof(Vector3) + sizeof(Quaternion) + sizeof(bool) + sizeof(bool)) {
// Allocate memory for the components data
allocate(INIT_NB_ALLOCATED_COMPONENTS);
@ -72,7 +72,9 @@ void DynamicsComponents::allocate(uint32 nbComponentsToAllocate) {
decimal* newInverseMasses = reinterpret_cast<decimal*>(newInitMasses + nbComponentsToAllocate);
Matrix3x3* newInertiaTensorLocalInverses = reinterpret_cast<Matrix3x3*>(newInverseMasses + nbComponentsToAllocate);
Matrix3x3* newInertiaTensorWorldInverses = reinterpret_cast<Matrix3x3*>(newInertiaTensorLocalInverses + nbComponentsToAllocate);
bool* newIsGravityEnabled = reinterpret_cast<bool*>(newInertiaTensorWorldInverses + nbComponentsToAllocate);
Vector3* newConstrainedPositions = reinterpret_cast<Vector3*>(newInertiaTensorWorldInverses + nbComponentsToAllocate);
Quaternion* newConstrainedOrientations = reinterpret_cast<Quaternion*>(newConstrainedPositions + nbComponentsToAllocate);
bool* newIsGravityEnabled = reinterpret_cast<bool*>(newConstrainedOrientations + nbComponentsToAllocate);
bool* newIsAlreadyInIsland = reinterpret_cast<bool*>(newIsGravityEnabled + nbComponentsToAllocate);
// If there was already components before
@ -94,6 +96,8 @@ void DynamicsComponents::allocate(uint32 nbComponentsToAllocate) {
memcpy(newInverseMasses, mInverseMasses, mNbComponents * sizeof(decimal));
memcpy(newInertiaTensorLocalInverses, mInverseInertiaTensorsLocal, mNbComponents * sizeof(Matrix3x3));
memcpy(newInertiaTensorWorldInverses, mInverseInertiaTensorsWorld, mNbComponents * sizeof(Matrix3x3));
memcpy(newConstrainedPositions, mConstrainedPositions, mNbComponents * sizeof(Vector3));
memcpy(newConstrainedOrientations, mConstrainedOrientations, mNbComponents * sizeof(Quaternion));
memcpy(newIsGravityEnabled, mIsGravityEnabled, mNbComponents * sizeof(bool));
memcpy(newIsAlreadyInIsland, mIsAlreadyInIsland, mNbComponents * sizeof(bool));
@ -117,6 +121,8 @@ void DynamicsComponents::allocate(uint32 nbComponentsToAllocate) {
mInverseMasses = newInverseMasses;
mInverseInertiaTensorsLocal = newInertiaTensorLocalInverses;
mInverseInertiaTensorsWorld = newInertiaTensorWorldInverses;
mConstrainedPositions = newConstrainedPositions;
mConstrainedOrientations = newConstrainedOrientations;
mIsGravityEnabled = newIsGravityEnabled;
mIsAlreadyInIsland = newIsAlreadyInIsland;
mNbAllocatedComponents = nbComponentsToAllocate;
@ -144,6 +150,8 @@ void DynamicsComponents::addComponent(Entity bodyEntity, bool isSleeping, const
mInverseMasses[index] = decimal(1.0);
new (mInverseInertiaTensorsLocal + index) Matrix3x3(1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0);
new (mInverseInertiaTensorsWorld + index) Matrix3x3(1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0);
new (mConstrainedPositions + index) Vector3(0, 0, 0);
new (mConstrainedOrientations + index) Quaternion(0, 0, 0, 1);
mIsGravityEnabled[index] = true;
mIsAlreadyInIsland[index] = false;
@ -178,6 +186,8 @@ void DynamicsComponents::moveComponentToIndex(uint32 srcIndex, uint32 destIndex)
mInverseMasses[destIndex] = mInverseMasses[srcIndex];
mInverseInertiaTensorsLocal[destIndex] = mInverseInertiaTensorsLocal[srcIndex];
mInverseInertiaTensorsWorld[destIndex] = mInverseInertiaTensorsWorld[srcIndex];
mConstrainedPositions[destIndex] = mConstrainedPositions[srcIndex];
mConstrainedOrientations[destIndex] = mConstrainedOrientations[srcIndex];
mIsGravityEnabled[destIndex] = mIsGravityEnabled[srcIndex];
mIsAlreadyInIsland[destIndex] = mIsAlreadyInIsland[srcIndex];
@ -214,6 +224,8 @@ void DynamicsComponents::swapComponents(uint32 index1, uint32 index2) {
decimal inverseMass1 = mInverseMasses[index1];
Matrix3x3 inertiaTensorLocalInverse1 = mInverseInertiaTensorsLocal[index1];
Matrix3x3 inertiaTensorWorldInverse1 = mInverseInertiaTensorsWorld[index1];
Vector3 constrainedPosition1 = mConstrainedPositions[index1];
Quaternion constrainedOrientation1 = mConstrainedOrientations[index1];
bool isGravityEnabled1 = mIsGravityEnabled[index1];
bool isAlreadyInIsland1 = mIsAlreadyInIsland[index1];
@ -238,6 +250,8 @@ void DynamicsComponents::swapComponents(uint32 index1, uint32 index2) {
mInverseMasses[index2] = inverseMass1;
mInverseInertiaTensorsLocal[index2] = inertiaTensorLocalInverse1;
mInverseInertiaTensorsWorld[index2] = inertiaTensorWorldInverse1;
mConstrainedPositions[index2] = constrainedPosition1;
mConstrainedOrientations[index2] = constrainedOrientation1;
mIsGravityEnabled[index2] = isGravityEnabled1;
mIsAlreadyInIsland[index2] = isAlreadyInIsland1;
@ -269,4 +283,6 @@ void DynamicsComponents::destroyComponent(uint32 index) {
mExternalTorques[index].~Vector3();
mInverseInertiaTensorsLocal[index].~Matrix3x3();
mInverseInertiaTensorsWorld[index].~Matrix3x3();
mConstrainedPositions[index].~Vector3();
mConstrainedOrientations[index].~Quaternion();
}

50
src/components/DynamicsComponents.h
View File

@ -96,6 +96,12 @@ class DynamicsComponents : public Components {
/// Array with the inverse of the world inertia tensor of each component
Matrix3x3* mInverseInertiaTensorsWorld;
/// Array with the constrained position of each component (for position error correction)
Vector3* mConstrainedPositions;
/// Array of constrained orientation for each component (for position error correction)
Quaternion* mConstrainedOrientations;
/// True if the gravity needs to be applied to this component
bool* mIsGravityEnabled;
@ -184,6 +190,12 @@ class DynamicsComponents : public Components {
/// Return the inverse world inertia tensor of an entity
const Matrix3x3& getInertiaTensorWorldInverse(Entity bodyEntity);
/// Return the constrained position of an entity
const Vector3& getConstrainedPosition(Entity bodyEntity);
/// Return the constrained orientation of an entity
const Quaternion& getConstrainedOrientation(Entity bodyEntity);
/// Return true if gravity is enabled for this entity
bool getIsGravityEnabled(Entity bodyEntity) const;
@ -232,6 +244,12 @@ class DynamicsComponents : public Components {
/// Set the inverse world inertia tensor of an entity
void setInverseInertiaTensorWorld(Entity bodyEntity, const Matrix3x3& inertiaTensorWorldInverse);
/// Set the constrained position of an entity
void setConstrainedPosition(Entity bodyEntity, const Vector3& constrainedPosition);
/// Set the constrained orientation of an entity
void setConstrainedOrientation(Entity bodyEntity, const Quaternion& constrainedOrientation);
/// Set the value to know if the gravity is enabled for this entity
bool setIsGravityEnabled(Entity bodyEntity, bool isGravityEnabled);
@ -378,6 +396,22 @@ inline const Matrix3x3& DynamicsComponents::getInertiaTensorWorldInverse(Entity
return mInverseInertiaTensorsWorld[mMapEntityToComponentIndex[bodyEntity]];
}
// Return the constrained position of an entity
inline const Vector3& DynamicsComponents::getConstrainedPosition(Entity bodyEntity) {
assert(mMapEntityToComponentIndex.containsKey(bodyEntity));
return mConstrainedPositions[mMapEntityToComponentIndex[bodyEntity]];
}
// Return the constrained orientation of an entity
inline const Quaternion& DynamicsComponents::getConstrainedOrientation(Entity bodyEntity) {
assert(mMapEntityToComponentIndex.containsKey(bodyEntity));
return mConstrainedOrientations[mMapEntityToComponentIndex[bodyEntity]];
}
// Set the constrained linear velocity of an entity
inline void DynamicsComponents::setConstrainedLinearVelocity(Entity bodyEntity, const Vector3& constrainedLinearVelocity) {
@ -474,6 +508,22 @@ inline void DynamicsComponents::setInverseInertiaTensorWorld(Entity bodyEntity,
mInverseInertiaTensorsWorld[mMapEntityToComponentIndex[bodyEntity]] = inertiaTensorWorldInverse;
}
// Set the constrained position of an entity
inline void DynamicsComponents::setConstrainedPosition(Entity bodyEntity, const Vector3& constrainedPosition) {
assert(mMapEntityToComponentIndex.containsKey(bodyEntity));
mConstrainedPositions[mMapEntityToComponentIndex[bodyEntity]] = constrainedPosition;
}
// Set the constrained orientation of an entity
inline void DynamicsComponents::setConstrainedOrientation(Entity bodyEntity, const Quaternion& constrainedOrientation) {
assert(mMapEntityToComponentIndex.containsKey(bodyEntity));
mConstrainedOrientations[mMapEntityToComponentIndex[bodyEntity]] = constrainedOrientation;
}
// Return true if gravity is enabled for this entity
inline bool DynamicsComponents::getIsGravityEnabled(Entity bodyEntity) const {

13
src/constraint/BallAndSocketJoint.cpp
View File

@ -169,10 +169,10 @@ void BallAndSocketJoint::solvePositionConstraint(const ConstraintSolverData& con
if (mPositionCorrectionTechnique != JointsPositionCorrectionTechnique::NON_LINEAR_GAUSS_SEIDEL) return;
// Get the bodies center of mass and orientations
Vector3& x1 = constraintSolverData.positions[mIndexBody1];
Vector3& x2 = constraintSolverData.positions[mIndexBody2];
Quaternion& q1 = constraintSolverData.orientations[mIndexBody1];
Quaternion& q2 = constraintSolverData.orientations[mIndexBody2];
Vector3 x1 = constraintSolverData.dynamicsComponents.getConstrainedPosition(mBody1Entity);
Vector3 x2 = constraintSolverData.dynamicsComponents.getConstrainedPosition(mBody2Entity);
Quaternion q1 = constraintSolverData.dynamicsComponents.getConstrainedOrientation(mBody1Entity);
Quaternion q2 = constraintSolverData.dynamicsComponents.getConstrainedOrientation(mBody2Entity);
// Get the inverse mass and inverse inertia tensors of the bodies
const decimal inverseMassBody1 = constraintSolverData.dynamicsComponents.getMassInverse(mBody1Entity);
@ -234,5 +234,10 @@ void BallAndSocketJoint::solvePositionConstraint(const ConstraintSolverData& con
x2 += v2;
q2 += Quaternion(0, w2) * q2 * decimal(0.5);
q2.normalize();
constraintSolverData.dynamicsComponents.setConstrainedPosition(mBody1Entity, x1);
constraintSolverData.dynamicsComponents.setConstrainedPosition(mBody2Entity, x2);
constraintSolverData.dynamicsComponents.setConstrainedOrientation(mBody1Entity, q1);
constraintSolverData.dynamicsComponents.setConstrainedOrientation(mBody2Entity, q2);
}

13
src/constraint/FixedJoint.cpp
View File

@ -235,10 +235,10 @@ void FixedJoint::solvePositionConstraint(const ConstraintSolverData& constraintS
if (mPositionCorrectionTechnique != JointsPositionCorrectionTechnique::NON_LINEAR_GAUSS_SEIDEL) return;
// Get the bodies positions and orientations
Vector3& x1 = constraintSolverData.positions[mIndexBody1];
Vector3& x2 = constraintSolverData.positions[mIndexBody2];
Quaternion& q1 = constraintSolverData.orientations[mIndexBody1];
Quaternion& q2 = constraintSolverData.orientations[mIndexBody2];
Vector3 x1 = constraintSolverData.dynamicsComponents.getConstrainedPosition(mBody1Entity);
Vector3 x2 = constraintSolverData.dynamicsComponents.getConstrainedPosition(mBody2Entity);
Quaternion q1 = constraintSolverData.dynamicsComponents.getConstrainedOrientation(mBody1Entity);
Quaternion q2 = constraintSolverData.dynamicsComponents.getConstrainedOrientation(mBody2Entity);
// Get the inverse mass and inverse inertia tensors of the bodies
decimal inverseMassBody1 = constraintSolverData.dynamicsComponents.getMassInverse(mBody1Entity);
@ -357,5 +357,10 @@ void FixedJoint::solvePositionConstraint(const ConstraintSolverData& constraintS
// Update the body position/orientation of body 2
q2 += Quaternion(0, w2) * q2 * decimal(0.5);
q2.normalize();
constraintSolverData.dynamicsComponents.setConstrainedPosition(mBody1Entity, x1);
constraintSolverData.dynamicsComponents.setConstrainedPosition(mBody2Entity, x2);
constraintSolverData.dynamicsComponents.setConstrainedOrientation(mBody1Entity, q1);
constraintSolverData.dynamicsComponents.setConstrainedOrientation(mBody2Entity, q2);
}

13
src/constraint/HingeJoint.cpp
View File

@ -414,10 +414,10 @@ void HingeJoint::solvePositionConstraint(const ConstraintSolverData& constraintS
if (mPositionCorrectionTechnique != JointsPositionCorrectionTechnique::NON_LINEAR_GAUSS_SEIDEL) return;
// Get the bodies positions and orientations
Vector3& x1 = constraintSolverData.positions[mIndexBody1];
Vector3& x2 = constraintSolverData.positions[mIndexBody2];
Quaternion& q1 = constraintSolverData.orientations[mIndexBody1];
Quaternion& q2 = constraintSolverData.orientations[mIndexBody2];
Vector3 x1 = constraintSolverData.dynamicsComponents.getConstrainedPosition(mBody1Entity);
Vector3 x2 = constraintSolverData.dynamicsComponents.getConstrainedPosition(mBody2Entity);
Quaternion q1 = constraintSolverData.dynamicsComponents.getConstrainedOrientation(mBody1Entity);
Quaternion q2 = constraintSolverData.dynamicsComponents.getConstrainedOrientation(mBody2Entity);
// Get the inverse mass and inverse inertia tensors of the bodies
decimal inverseMassBody1 = constraintSolverData.dynamicsComponents.getMassInverse(mBody1Entity);
@ -614,6 +614,11 @@ void HingeJoint::solvePositionConstraint(const ConstraintSolverData& constraintS
q2.normalize();
}
}
constraintSolverData.dynamicsComponents.setConstrainedPosition(mBody1Entity, x1);
constraintSolverData.dynamicsComponents.setConstrainedPosition(mBody2Entity, x2);
constraintSolverData.dynamicsComponents.setConstrainedOrientation(mBody1Entity, q1);
constraintSolverData.dynamicsComponents.setConstrainedOrientation(mBody2Entity, q2);
}

13
src/constraint/SliderJoint.cpp
View File

@ -446,10 +446,10 @@ void SliderJoint::solvePositionConstraint(const ConstraintSolverData& constraint
if (mPositionCorrectionTechnique != JointsPositionCorrectionTechnique::NON_LINEAR_GAUSS_SEIDEL) return;
// Get the bodies positions and orientations
Vector3& x1 = constraintSolverData.positions[mIndexBody1];
Vector3& x2 = constraintSolverData.positions[mIndexBody2];
Quaternion& q1 = constraintSolverData.orientations[mIndexBody1];
Quaternion& q2 = constraintSolverData.orientations[mIndexBody2];
Vector3 x1 = constraintSolverData.dynamicsComponents.getConstrainedPosition(mBody1Entity);
Vector3 x2 = constraintSolverData.dynamicsComponents.getConstrainedPosition(mBody2Entity);
Quaternion q1 = constraintSolverData.dynamicsComponents.getConstrainedOrientation(mBody1Entity);
Quaternion q2 = constraintSolverData.dynamicsComponents.getConstrainedOrientation(mBody2Entity);
// Get the inverse mass and inverse inertia tensors of the bodies
const decimal inverseMassBody1 = constraintSolverData.dynamicsComponents.getMassInverse(mBody1Entity);
@ -689,6 +689,11 @@ void SliderJoint::solvePositionConstraint(const ConstraintSolverData& constraint
q2.normalize();
}
}
constraintSolverData.dynamicsComponents.setConstrainedPosition(mBody1Entity, x1);
constraintSolverData.dynamicsComponents.setConstrainedPosition(mBody2Entity, x2);
constraintSolverData.dynamicsComponents.setConstrainedOrientation(mBody1Entity, q1);
constraintSolverData.dynamicsComponents.setConstrainedOrientation(mBody2Entity, q2);
}
// Enable/Disable the limits of the joint

23
src/engine/ConstraintSolver.h
View File

@ -54,18 +54,12 @@ struct ConstraintSolverData {
/// Reference to the dynamics components
DynamicsComponents& dynamicsComponents;
/// Reference to the bodies positions
Vector3* positions;
/// Reference to the bodies orientations
Quaternion* orientations;
/// True if warm starting of the solver is active
bool isWarmStartingActive;
/// Constructor
ConstraintSolverData(DynamicsComponents& dynamicsComponents)
:dynamicsComponents(dynamicsComponents), positions(nullptr), orientations(nullptr) {
:dynamicsComponents(dynamicsComponents) {
}
@ -189,10 +183,6 @@ class ConstraintSolver {
/// Enable/Disable the Non-Linear-Gauss-Seidel position correction technique.
void setIsNonLinearGaussSeidelPositionCorrectionActive(bool isActive);
/// Set the constrained positions/orientations arrays
void setConstrainedPositionsArrays(Vector3* constrainedPositions,
Quaternion* constrainedOrientations);
#ifdef IS_PROFILING_ACTIVE
/// Set the profiler
@ -202,17 +192,6 @@ class ConstraintSolver {
};
// Set the constrained positions/orientations arrays
inline void ConstraintSolver::setConstrainedPositionsArrays(Vector3* constrainedPositions,
Quaternion* constrainedOrientations) {
assert(constrainedPositions != nullptr);
assert(constrainedOrientations != nullptr);
mConstraintSolverData.positions = constrainedPositions;
mConstraintSolverData.orientations = constrainedOrientations;
}
#ifdef IS_PROFILING_ACTIVE
// Set the profiler

26
src/engine/DynamicsWorld.cpp
View File

@ -56,8 +56,7 @@ DynamicsWorld::DynamicsWorld(const Vector3& gravity, const WorldSettings& worldS
mNbPositionSolverIterations(mConfig.defaultPositionSolverNbIterations),
mIsSleepingEnabled(mConfig.isSleepingEnabled), mRigidBodies(mMemoryManager.getPoolAllocator()),
mJoints(mMemoryManager.getPoolAllocator()), mGravity(gravity), mTimeStep(decimal(1.0f / 60.0f)),
mIsGravityEnabled(true), mConstrainedPositions(nullptr),
mConstrainedOrientations(nullptr), mSleepLinearVelocity(mConfig.defaultSleepLinearVelocity),
mIsGravityEnabled(true), mSleepLinearVelocity(mConfig.defaultSleepLinearVelocity),
mSleepAngularVelocity(mConfig.defaultSleepAngularVelocity), mTimeBeforeSleep(mConfig.defaultTimeBeforeSleep),
mFreeJointsIDs(mMemoryManager.getPoolAllocator()), mCurrentJointId(0) {
@ -176,7 +175,6 @@ void DynamicsWorld::integrateRigidBodiesPositions() {
RigidBody* body = static_cast<RigidBody*>(mBodyComponents.getBody(bodyEntity));
// Get the constrained velocity
uint indexArray = body->mArrayIndex;
Vector3 newLinVelocity = mDynamicsComponents.getConstrainedLinearVelocity(bodyEntity);
Vector3 newAngVelocity = mDynamicsComponents.getConstrainedAngularVelocity(bodyEntity);
@ -196,10 +194,10 @@ void DynamicsWorld::integrateRigidBodiesPositions() {
const Quaternion& currentOrientation = mTransformComponents.getTransform(body->getEntity()).getOrientation();
// Update the new constrained position and orientation of the body
mConstrainedPositions[indexArray] = currentPosition + newLinVelocity * mTimeStep;
mConstrainedOrientations[indexArray] = currentOrientation +
mDynamicsComponents.setConstrainedPosition(bodyEntity, currentPosition + newLinVelocity * mTimeStep);
mDynamicsComponents.setConstrainedOrientation(bodyEntity, currentOrientation +
Quaternion(0, newAngVelocity) *
currentOrientation * decimal(0.5) * mTimeStep;
currentOrientation * decimal(0.5) * mTimeStep);
}
}
}
@ -229,10 +227,11 @@ void DynamicsWorld::updateBodiesState() {
mDynamicsComponents.setAngularVelocity(bodyEntity, mDynamicsComponents.getConstrainedAngularVelocity(bodyEntity));
// Update the position of the center of mass of the body
body->mCenterOfMassWorld = mConstrainedPositions[index];
body->mCenterOfMassWorld = mDynamicsComponents.getConstrainedPosition(bodyEntity);
// Update the orientation of the body
mTransformComponents.getTransform(bodyEntity).setOrientation(mConstrainedOrientations[index].getUnit());
const Quaternion& constrainedOrientation = mDynamicsComponents.getConstrainedOrientation(bodyEntity);
mTransformComponents.getTransform(bodyEntity).setOrientation(constrainedOrientation.getUnit());
// Update the transform of the body (using the new center of mass and new orientation)
body->updateTransformWithCenterOfMass();
@ -256,13 +255,6 @@ void DynamicsWorld::initVelocityArrays() {
assert(mDynamicsComponents.getNbComponents() == nbBodies);
mConstrainedPositions = static_cast<Vector3*>(mMemoryManager.allocate(MemoryManager::AllocationType::Frame,
nbBodies * sizeof(Vector3)));
mConstrainedOrientations = static_cast<Quaternion*>(mMemoryManager.allocate(MemoryManager::AllocationType::Frame,
nbBodies * sizeof(Quaternion)));
assert(mConstrainedPositions != nullptr);
assert(mConstrainedOrientations != nullptr);
// Initialize the map of body indexes in the velocity arrays
uint i = 0;
for (List<RigidBody*>::Iterator it = mRigidBodies.begin(); it != mRigidBodies.end(); ++it) {
@ -348,10 +340,6 @@ void DynamicsWorld::solveContactsAndConstraints() {
RP3D_PROFILE("DynamicsWorld::solveContactsAndConstraints()", mProfiler);
// Set the velocities arrays
mConstraintSolver.setConstrainedPositionsArrays(mConstrainedPositions,
mConstrainedOrientations);
// ---------- Solve velocity constraints for joints and contacts ---------- //
// Initialize the contact solver

6
src/engine/DynamicsWorld.h
View File

@ -88,12 +88,6 @@ class DynamicsWorld : public CollisionWorld {
/// True if the gravity force is on
bool mIsGravityEnabled;
/// Array of constrained rigid bodies position (for position error correction)
Vector3* mConstrainedPositions;
/// Array of constrained rigid bodies orientation (for position error correction)
Quaternion* mConstrainedOrientations;
/// Sleep linear velocity threshold
decimal mSleepLinearVelocity;