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

Use DynamicsComponents for constrained linear/angular velocities in solvers

Browse Source
This commit is contained in:
Daniel Chappuis 2019-05-16 17:46:26 +02:00
parent ac0e620f02
commit 9afedae1a7
16 changed files with 277 additions and 200 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
src/body/RigidBody.h
View File

@ -53,6 +53,7 @@ class RigidBody : public CollisionBody {
private :
/// Index of the body in arrays for contact/constraint solver
// TODO : REMOVE THIS
uint mArrayIndex;
protected :

8
src/components/Components.h
View File

@ -121,6 +121,9 @@ class Components {
/// Return the number of enabled components
uint32 getNbEnabledComponents() const;
/// Return the index in the arrays for a given entity
uint32 getEntityIndex(Entity entity) const;
};
// Return true if an entity is sleeping
@ -144,6 +147,11 @@ inline uint32 Components::getNbEnabledComponents() const {
return mDisabledStartIndex;
}
// Return the index in the arrays for a given entity
inline uint32 Components::getEntityIndex(Entity entity) const {
assert(hasComponent(entity));
return mMapEntityToComponentIndex[entity];
}
}
#endif

20
src/components/DynamicsComponents.cpp
View File

@ -35,7 +35,7 @@ using namespace reactphysics3d;
// Constructor
DynamicsComponents::DynamicsComponents(MemoryAllocator& allocator)
:Components(allocator, sizeof(Entity) + sizeof(Vector3) + sizeof (Vector3) + sizeof(bool)) {
:Components(allocator, sizeof(Entity) + sizeof(Vector3) + sizeof(Vector3) + sizeof(Vector3) + sizeof(Vector3) + sizeof(bool)) {
// Allocate memory for the components data
allocate(INIT_NB_ALLOCATED_COMPONENTS);
@ -57,7 +57,9 @@ void DynamicsComponents::allocate(uint32 nbComponentsToAllocate) {
Entity* newBodies = static_cast<Entity*>(newBuffer);
Vector3* newLinearVelocities = reinterpret_cast<Vector3*>(newBodies + nbComponentsToAllocate);
Vector3* newAngularVelocities = reinterpret_cast<Vector3*>(newLinearVelocities + nbComponentsToAllocate);
bool* newIsAlreadyInIsland = reinterpret_cast<bool*>(newAngularVelocities + nbComponentsToAllocate);
Vector3* newConstrainedLinearVelocities = reinterpret_cast<Vector3*>(newAngularVelocities + nbComponentsToAllocate);
Vector3* newConstrainedAngularVelocities = reinterpret_cast<Vector3*>(newConstrainedLinearVelocities + nbComponentsToAllocate);
bool* newIsAlreadyInIsland = reinterpret_cast<bool*>(newConstrainedAngularVelocities + nbComponentsToAllocate);
// If there was already components before
if (mNbComponents > 0) {
@ -66,6 +68,8 @@ void DynamicsComponents::allocate(uint32 nbComponentsToAllocate) {
memcpy(newBodies, mBodies, mNbComponents * sizeof(Entity));
memcpy(newLinearVelocities, mLinearVelocities, mNbComponents * sizeof(Vector3));
memcpy(newAngularVelocities, mAngularVelocities, mNbComponents * sizeof(Vector3));
memcpy(newConstrainedLinearVelocities, mConstrainedLinearVelocities, mNbComponents * sizeof(Vector3));
memcpy(newConstrainedAngularVelocities, mConstrainedAngularVelocities, mNbComponents * sizeof(Vector3));
memcpy(newIsAlreadyInIsland, mIsAlreadyInIsland, mNbComponents * sizeof(bool));
// Deallocate previous memory
@ -76,6 +80,8 @@ void DynamicsComponents::allocate(uint32 nbComponentsToAllocate) {
mBodies = newBodies;
mLinearVelocities = newLinearVelocities;
mAngularVelocities = newAngularVelocities;
mConstrainedLinearVelocities = newConstrainedLinearVelocities;
mConstrainedAngularVelocities = newConstrainedAngularVelocities;
mIsAlreadyInIsland = newIsAlreadyInIsland;
mNbAllocatedComponents = nbComponentsToAllocate;
}
@ -90,6 +96,8 @@ void DynamicsComponents::addComponent(Entity bodyEntity, bool isSleeping, const
new (mBodies + index) Entity(bodyEntity);
new (mLinearVelocities + index) Vector3(component.linearVelocity);
new (mAngularVelocities + index) Vector3(component.angularVelocity);
new (mConstrainedLinearVelocities + index) Vector3(0, 0, 0);
new (mConstrainedAngularVelocities + index) Vector3(0, 0, 0);
mIsAlreadyInIsland[index] = false;
// Map the entity with the new component lookup index
@ -111,6 +119,8 @@ void DynamicsComponents::moveComponentToIndex(uint32 srcIndex, uint32 destIndex)
new (mBodies + destIndex) Entity(mBodies[srcIndex]);
new (mLinearVelocities + destIndex) Vector3(mLinearVelocities[srcIndex]);
new (mAngularVelocities + destIndex) Vector3(mAngularVelocities[srcIndex]);
new (mConstrainedLinearVelocities + destIndex) Vector3(mConstrainedLinearVelocities[srcIndex]);
new (mConstrainedAngularVelocities + destIndex) Vector3(mConstrainedAngularVelocities[srcIndex]);
mIsAlreadyInIsland[destIndex] = mIsAlreadyInIsland[srcIndex];
// Destroy the source component
@ -134,6 +144,8 @@ void DynamicsComponents::swapComponents(uint32 index1, uint32 index2) {
Entity entity1(mBodies[index1]);
Vector3 linearVelocity1(mLinearVelocities[index1]);
Vector3 angularVelocity1(mAngularVelocities[index1]);
Vector3 constrainedLinearVelocity1(mConstrainedLinearVelocities[index1]);
Vector3 constrainedAngularVelocity1(mConstrainedAngularVelocities[index1]);
bool isAlreadyInIsland1 = mIsAlreadyInIsland[index1];
// Destroy component 1
@ -145,6 +157,8 @@ void DynamicsComponents::swapComponents(uint32 index1, uint32 index2) {
new (mBodies + index2) Entity(entity1);
new (mLinearVelocities + index2) Vector3(linearVelocity1);
new (mAngularVelocities + index2) Vector3(angularVelocity1);
new (mConstrainedLinearVelocities + index2) Vector3(constrainedLinearVelocity1);
new (mConstrainedAngularVelocities + index2) Vector3(constrainedAngularVelocity1);
mIsAlreadyInIsland[index2] = isAlreadyInIsland1;
// Update the entity to component index mapping
@ -167,4 +181,6 @@ void DynamicsComponents::destroyComponent(uint32 index) {
mBodies[index].~Entity();
mLinearVelocities[index].~Vector3();
mAngularVelocities[index].~Vector3();
mConstrainedLinearVelocities[index].~Vector3();
mConstrainedAngularVelocities[index].~Vector3();
}

69
src/components/DynamicsComponents.h
View File

@ -59,6 +59,12 @@ class DynamicsComponents : public Components {
/// Array with the angular velocity of each component
Vector3* mAngularVelocities;
/// Array with the constrained linear velocity of each component
Vector3* mConstrainedLinearVelocities;
/// Array with the constrained angular velocity of each component
Vector3* mConstrainedAngularVelocities;
/// Array with the boolean value to know if the body has already been added into an island
bool* mIsAlreadyInIsland;
@ -103,10 +109,16 @@ class DynamicsComponents : public Components {
void addComponent(Entity bodyEntity, bool isSleeping, const DynamicsComponent& component);
/// Return the linear velocity of an entity
Vector3& getLinearVelocity(Entity bodyEntity) const;
const Vector3& getLinearVelocity(Entity bodyEntity) const;
/// Return the angular velocity of an entity
Vector3& getAngularVelocity(Entity bodyEntity) const;
const Vector3& getAngularVelocity(Entity bodyEntity) const;
/// Return the constrained linear velocity of an entity
const Vector3& getConstrainedLinearVelocity(Entity bodyEntity) const;
/// Return the constrained angular velocity of an entity
const Vector3& getConstrainedAngularVelocity(Entity bodyEntity) const;
/// Return true if the entity is already in an island
bool getIsAlreadyInIsland(Entity bodyEntity) const;
@ -117,17 +129,31 @@ class DynamicsComponents : public Components {
/// Set the angular velocity of an entity
void setAngularVelocity(Entity bodyEntity, const Vector3& angularVelocity);
/// Set the constrained linear velocity of an entity
void setConstrainedLinearVelocity(Entity bodyEntity, const Vector3& constrainedLinearVelocity);
/// Set the constrained angular velocity of an entity
void setConstrainedAngularVelocity(Entity bodyEntity, const Vector3& constrainedAngularVelocity);
/// Set the value to know if the entity is already in an island
bool setIsAlreadyInIsland(Entity bodyEntity, bool isAlreadyInIsland);
// -------------------- Friendship -------------------- //
friend class BroadPhaseSystem;
friend class DynamicsWorld;
friend class ContactSolver;
friend class BallAndSocketJoint;
friend class FixedJoint;
friend class HingeJoint;
friend class SliderJoint;
};
// Return the linear velocity of an entity
inline Vector3& DynamicsComponents::getLinearVelocity(Entity bodyEntity) const {
inline const Vector3& DynamicsComponents::getLinearVelocity(Entity bodyEntity) const {
assert(mMapEntityToComponentIndex.containsKey(bodyEntity));
@ -135,7 +161,7 @@ inline Vector3& DynamicsComponents::getLinearVelocity(Entity bodyEntity) const {
}
// Return the angular velocity of an entity
inline Vector3& DynamicsComponents::getAngularVelocity(Entity bodyEntity) const {
inline const Vector3 &DynamicsComponents::getAngularVelocity(Entity bodyEntity) const {
assert(mMapEntityToComponentIndex.containsKey(bodyEntity));
@ -158,6 +184,41 @@ inline void DynamicsComponents::setAngularVelocity(Entity bodyEntity, const Vect
mAngularVelocities[mMapEntityToComponentIndex[bodyEntity]] = angularVelocity;
}
// Return the constrained linear velocity of an entity
inline const Vector3 &DynamicsComponents::getConstrainedLinearVelocity(Entity bodyEntity) const {
assert(mMapEntityToComponentIndex.containsKey(bodyEntity));
// TODO : DELETE THIS
uint testIndex = mMapEntityToComponentIndex[bodyEntity];
return mConstrainedLinearVelocities[mMapEntityToComponentIndex[bodyEntity]];
}
// Return the constrained angular velocity of an entity
inline const Vector3 &DynamicsComponents::getConstrainedAngularVelocity(Entity bodyEntity) const {
assert(mMapEntityToComponentIndex.containsKey(bodyEntity));
return mConstrainedAngularVelocities[mMapEntityToComponentIndex[bodyEntity]];
}
// Set the constrained linear velocity of an entity
inline void DynamicsComponents::setConstrainedLinearVelocity(Entity bodyEntity, const Vector3& constrainedLinearVelocity) {
assert(mMapEntityToComponentIndex.containsKey(bodyEntity));
mConstrainedLinearVelocities[mMapEntityToComponentIndex[bodyEntity]] = constrainedLinearVelocity;
}
// Set the constrained angular velocity of an entity
inline void DynamicsComponents::setConstrainedAngularVelocity(Entity bodyEntity, const Vector3& constrainedAngularVelocity) {
assert(mMapEntityToComponentIndex.containsKey(bodyEntity));
mConstrainedAngularVelocities[mMapEntityToComponentIndex[bodyEntity]] = constrainedAngularVelocity;
}
// Return true if the entity is already in an island
inline bool DynamicsComponents::getIsAlreadyInIsland(Entity bodyEntity) const {

23
src/constraint/BallAndSocketJoint.cpp
View File

@ -26,6 +26,7 @@
// Libraries
#include "BallAndSocketJoint.h"
#include "engine/ConstraintSolver.h"
#include "components/DynamicsComponents.h"
using namespace reactphysics3d;
@ -98,11 +99,14 @@ void BallAndSocketJoint::initBeforeSolve(const ConstraintSolverData& constraintS
// Warm start the constraint (apply the previous impulse at the beginning of the step)
void BallAndSocketJoint::warmstart(const ConstraintSolverData& constraintSolverData) {
uint32 dynamicsComponentIndexBody1 = constraintSolverData.dynamicsComponents.getEntityIndex(mBody1Entity);
uint32 dynamicsComponentIndexBody2 = constraintSolverData.dynamicsComponents.getEntityIndex(mBody2Entity);
// Get the velocities
Vector3& v1 = constraintSolverData.linearVelocities[mIndexBody1];
Vector3& v2 = constraintSolverData.linearVelocities[mIndexBody2];
Vector3& w1 = constraintSolverData.angularVelocities[mIndexBody1];
Vector3& w2 = constraintSolverData.angularVelocities[mIndexBody2];
Vector3& v1 = constraintSolverData.dynamicsComponents.mConstrainedLinearVelocities[dynamicsComponentIndexBody1];
Vector3& v2 = constraintSolverData.dynamicsComponents.mConstrainedLinearVelocities[dynamicsComponentIndexBody2];
Vector3& w1 = constraintSolverData.dynamicsComponents.mConstrainedAngularVelocities[dynamicsComponentIndexBody1];
Vector3& w2 = constraintSolverData.dynamicsComponents.mConstrainedAngularVelocities[dynamicsComponentIndexBody2];
// Compute the impulse P=J^T * lambda for the body 1
const Vector3 linearImpulseBody1 = -mImpulse;
@ -123,11 +127,14 @@ void BallAndSocketJoint::warmstart(const ConstraintSolverData& constraintSolverD
// Solve the velocity constraint
void BallAndSocketJoint::solveVelocityConstraint(const ConstraintSolverData& constraintSolverData) {
uint32 dynamicsComponentIndexBody1 = constraintSolverData.dynamicsComponents.getEntityIndex(mBody1Entity);
uint32 dynamicsComponentIndexBody2 = constraintSolverData.dynamicsComponents.getEntityIndex(mBody2Entity);
// Get the velocities
Vector3& v1 = constraintSolverData.linearVelocities[mIndexBody1];
Vector3& v2 = constraintSolverData.linearVelocities[mIndexBody2];
Vector3& w1 = constraintSolverData.angularVelocities[mIndexBody1];
Vector3& w2 = constraintSolverData.angularVelocities[mIndexBody2];
Vector3& v1 = constraintSolverData.dynamicsComponents.mConstrainedLinearVelocities[dynamicsComponentIndexBody1];
Vector3& v2 = constraintSolverData.dynamicsComponents.mConstrainedLinearVelocities[dynamicsComponentIndexBody2];
Vector3& w1 = constraintSolverData.dynamicsComponents.mConstrainedAngularVelocities[dynamicsComponentIndexBody1];
Vector3& w2 = constraintSolverData.dynamicsComponents.mConstrainedAngularVelocities[dynamicsComponentIndexBody2];
// Compute J*v
const Vector3 Jv = v2 + w2.cross(mR2World) - v1 - w1.cross(mR1World);

23
src/constraint/FixedJoint.cpp
View File

@ -26,6 +26,7 @@
// Libraries
#include "FixedJoint.h"
#include "engine/ConstraintSolver.h"
#include "components/DynamicsComponents.h"
using namespace reactphysics3d;
@ -129,11 +130,14 @@ void FixedJoint::initBeforeSolve(const ConstraintSolverData& constraintSolverDat
// Warm start the constraint (apply the previous impulse at the beginning of the step)
void FixedJoint::warmstart(const ConstraintSolverData& constraintSolverData) {
uint32 dynamicsComponentIndexBody1 = constraintSolverData.dynamicsComponents.getEntityIndex(mBody1Entity);
uint32 dynamicsComponentIndexBody2 = constraintSolverData.dynamicsComponents.getEntityIndex(mBody2Entity);
// Get the velocities
Vector3& v1 = constraintSolverData.linearVelocities[mIndexBody1];
Vector3& v2 = constraintSolverData.linearVelocities[mIndexBody2];
Vector3& w1 = constraintSolverData.angularVelocities[mIndexBody1];
Vector3& w2 = constraintSolverData.angularVelocities[mIndexBody2];
Vector3& v1 = constraintSolverData.dynamicsComponents.mConstrainedLinearVelocities[dynamicsComponentIndexBody1];
Vector3& v2 = constraintSolverData.dynamicsComponents.mConstrainedLinearVelocities[dynamicsComponentIndexBody2];
Vector3& w1 = constraintSolverData.dynamicsComponents.mConstrainedAngularVelocities[dynamicsComponentIndexBody1];
Vector3& w2 = constraintSolverData.dynamicsComponents.mConstrainedAngularVelocities[dynamicsComponentIndexBody2];
// Get the inverse mass of the bodies
const decimal inverseMassBody1 = mBody1->mMassInverse;
@ -164,11 +168,14 @@ void FixedJoint::warmstart(const ConstraintSolverData& constraintSolverData) {
// Solve the velocity constraint
void FixedJoint::solveVelocityConstraint(const ConstraintSolverData& constraintSolverData) {
uint32 dynamicsComponentIndexBody1 = constraintSolverData.dynamicsComponents.getEntityIndex(mBody1Entity);
uint32 dynamicsComponentIndexBody2 = constraintSolverData.dynamicsComponents.getEntityIndex(mBody2Entity);
// Get the velocities
Vector3& v1 = constraintSolverData.linearVelocities[mIndexBody1];
Vector3& v2 = constraintSolverData.linearVelocities[mIndexBody2];
Vector3& w1 = constraintSolverData.angularVelocities[mIndexBody1];
Vector3& w2 = constraintSolverData.angularVelocities[mIndexBody2];
Vector3& v1 = constraintSolverData.dynamicsComponents.mConstrainedLinearVelocities[dynamicsComponentIndexBody1];
Vector3& v2 = constraintSolverData.dynamicsComponents.mConstrainedLinearVelocities[dynamicsComponentIndexBody2];
Vector3& w1 = constraintSolverData.dynamicsComponents.mConstrainedAngularVelocities[dynamicsComponentIndexBody1];
Vector3& w2 = constraintSolverData.dynamicsComponents.mConstrainedAngularVelocities[dynamicsComponentIndexBody2];
// Get the inverse mass of the bodies
decimal inverseMassBody1 = mBody1->mMassInverse;

23
src/constraint/HingeJoint.cpp
View File

@ -26,6 +26,7 @@
// Libraries
#include "HingeJoint.h"
#include "engine/ConstraintSolver.h"
#include "components/DynamicsComponents.h"
using namespace reactphysics3d;
@ -198,11 +199,14 @@ void HingeJoint::initBeforeSolve(const ConstraintSolverData& constraintSolverDat
// Warm start the constraint (apply the previous impulse at the beginning of the step)
void HingeJoint::warmstart(const ConstraintSolverData& constraintSolverData) {
uint32 dynamicsComponentIndexBody1 = constraintSolverData.dynamicsComponents.getEntityIndex(mBody1Entity);
uint32 dynamicsComponentIndexBody2 = constraintSolverData.dynamicsComponents.getEntityIndex(mBody2Entity);
// Get the velocities
Vector3& v1 = constraintSolverData.linearVelocities[mIndexBody1];
Vector3& v2 = constraintSolverData.linearVelocities[mIndexBody2];
Vector3& w1 = constraintSolverData.angularVelocities[mIndexBody1];
Vector3& w2 = constraintSolverData.angularVelocities[mIndexBody2];
Vector3& v1 = constraintSolverData.dynamicsComponents.mConstrainedLinearVelocities[dynamicsComponentIndexBody1];
Vector3& v2 = constraintSolverData.dynamicsComponents.mConstrainedLinearVelocities[dynamicsComponentIndexBody2];
Vector3& w1 = constraintSolverData.dynamicsComponents.mConstrainedAngularVelocities[dynamicsComponentIndexBody1];
Vector3& w2 = constraintSolverData.dynamicsComponents.mConstrainedAngularVelocities[dynamicsComponentIndexBody2];
// Get the inverse mass and inverse inertia tensors of the bodies
const decimal inverseMassBody1 = mBody1->mMassInverse;
@ -254,11 +258,14 @@ void HingeJoint::warmstart(const ConstraintSolverData& constraintSolverData) {
// Solve the velocity constraint
void HingeJoint::solveVelocityConstraint(const ConstraintSolverData& constraintSolverData) {
uint32 dynamicsComponentIndexBody1 = constraintSolverData.dynamicsComponents.getEntityIndex(mBody1Entity);
uint32 dynamicsComponentIndexBody2 = constraintSolverData.dynamicsComponents.getEntityIndex(mBody2Entity);
// Get the velocities
Vector3& v1 = constraintSolverData.linearVelocities[mIndexBody1];
Vector3& v2 = constraintSolverData.linearVelocities[mIndexBody2];
Vector3& w1 = constraintSolverData.angularVelocities[mIndexBody1];
Vector3& w2 = constraintSolverData.angularVelocities[mIndexBody2];
Vector3& v1 = constraintSolverData.dynamicsComponents.mConstrainedLinearVelocities[dynamicsComponentIndexBody1];
Vector3& v2 = constraintSolverData.dynamicsComponents.mConstrainedLinearVelocities[dynamicsComponentIndexBody2];
Vector3& w1 = constraintSolverData.dynamicsComponents.mConstrainedAngularVelocities[dynamicsComponentIndexBody1];
Vector3& w2 = constraintSolverData.dynamicsComponents.mConstrainedAngularVelocities[dynamicsComponentIndexBody2];
// Get the inverse mass and inverse inertia tensors of the bodies
decimal inverseMassBody1 = mBody1->mMassInverse;

3
src/constraint/Joint.cpp
View File

@ -30,7 +30,8 @@ using namespace reactphysics3d;
// Constructor
Joint::Joint(uint id, const JointInfo& jointInfo)
:mId(id), mBody1(jointInfo.body1), mBody2(jointInfo.body2), mType(jointInfo.type),
:mId(id), mBody1(jointInfo.body1), mBody2(jointInfo.body2), mBody1Entity(jointInfo.body1->getEntity()),
mBody2Entity(jointInfo.body2->getEntity()), mType(jointInfo.type),
mPositionCorrectionTechnique(jointInfo.positionCorrectionTechnique),
mIsCollisionEnabled(jointInfo.isCollisionEnabled), mIsAlreadyInIsland(false) {

8
src/constraint/Joint.h
View File

@ -124,11 +124,19 @@ class Joint {
uint mId;
/// Pointer to the first body of the joint
// TODO : Use Entities instead
RigidBody* const mBody1;
/// Pointer to the second body of the joint
// TODO : Use Entities instead
RigidBody* const mBody2;
/// Entity of the first body of the joint
Entity mBody1Entity;
/// Entity of the second body of the joint
Entity mBody2Entity;
/// Type of the joint
const JointType mType;

23
src/constraint/SliderJoint.cpp
View File

@ -26,6 +26,7 @@
// Libraries
#include "SliderJoint.h"
#include "engine/ConstraintSolver.h"
#include "components/DynamicsComponents.h"
using namespace reactphysics3d;
@ -216,11 +217,14 @@ void SliderJoint::initBeforeSolve(const ConstraintSolverData& constraintSolverDa
// Warm start the constraint (apply the previous impulse at the beginning of the step)
void SliderJoint::warmstart(const ConstraintSolverData& constraintSolverData) {
uint32 dynamicsComponentIndexBody1 = constraintSolverData.dynamicsComponents.getEntityIndex(mBody1Entity);
uint32 dynamicsComponentIndexBody2 = constraintSolverData.dynamicsComponents.getEntityIndex(mBody2Entity);
// Get the velocities
Vector3& v1 = constraintSolverData.linearVelocities[mIndexBody1];
Vector3& v2 = constraintSolverData.linearVelocities[mIndexBody2];
Vector3& w1 = constraintSolverData.angularVelocities[mIndexBody1];
Vector3& w2 = constraintSolverData.angularVelocities[mIndexBody2];
Vector3& v1 = constraintSolverData.dynamicsComponents.mConstrainedLinearVelocities[dynamicsComponentIndexBody1];
Vector3& v2 = constraintSolverData.dynamicsComponents.mConstrainedLinearVelocities[dynamicsComponentIndexBody2];
Vector3& w1 = constraintSolverData.dynamicsComponents.mConstrainedAngularVelocities[dynamicsComponentIndexBody1];
Vector3& w2 = constraintSolverData.dynamicsComponents.mConstrainedAngularVelocities[dynamicsComponentIndexBody2];
// Get the inverse mass and inverse inertia tensors of the bodies
const decimal inverseMassBody1 = mBody1->mMassInverse;
@ -275,11 +279,14 @@ void SliderJoint::warmstart(const ConstraintSolverData& constraintSolverData) {
// Solve the velocity constraint
void SliderJoint::solveVelocityConstraint(const ConstraintSolverData& constraintSolverData) {
uint32 dynamicsComponentIndexBody1 = constraintSolverData.dynamicsComponents.getEntityIndex(mBody1Entity);
uint32 dynamicsComponentIndexBody2 = constraintSolverData.dynamicsComponents.getEntityIndex(mBody2Entity);
// Get the velocities
Vector3& v1 = constraintSolverData.linearVelocities[mIndexBody1];
Vector3& v2 = constraintSolverData.linearVelocities[mIndexBody2];
Vector3& w1 = constraintSolverData.angularVelocities[mIndexBody1];
Vector3& w2 = constraintSolverData.angularVelocities[mIndexBody2];
Vector3& v1 = constraintSolverData.dynamicsComponents.mConstrainedLinearVelocities[dynamicsComponentIndexBody1];
Vector3& v2 = constraintSolverData.dynamicsComponents.mConstrainedLinearVelocities[dynamicsComponentIndexBody2];
Vector3& w1 = constraintSolverData.dynamicsComponents.mConstrainedAngularVelocities[dynamicsComponentIndexBody1];
Vector3& w2 = constraintSolverData.dynamicsComponents.mConstrainedAngularVelocities[dynamicsComponentIndexBody2];
// Get the inverse mass and inverse inertia tensors of the bodies
decimal inverseMassBody1 = mBody1->mMassInverse;

3
src/engine/ConstraintSolver.cpp
View File

@ -31,7 +31,8 @@
using namespace reactphysics3d;
// Constructor
ConstraintSolver::ConstraintSolver(Islands& islands) : mIsWarmStartingActive(true), mIslands(islands) {
ConstraintSolver::ConstraintSolver(Islands& islands, DynamicsComponents& dynamicsComponents)
: mIsWarmStartingActive(true), mIslands(islands), mConstraintSolverData(dynamicsComponents) {
#ifdef IS_PROFILING_ACTIVE

29
src/engine/ConstraintSolver.h
View File

@ -37,6 +37,7 @@ namespace reactphysics3d {
class Joint;
class Island;
class Profiler;
class DynamicsComponents;
// Structure ConstraintSolverData
/**
@ -50,11 +51,8 @@ struct ConstraintSolverData {
/// Current time step of the simulation
decimal timeStep;
/// Array with the bodies linear velocities
Vector3* linearVelocities;
/// Array with the bodies angular velocities
Vector3* angularVelocities;
/// Reference to the dynamics components
DynamicsComponents& dynamicsComponents;
/// Reference to the bodies positions
Vector3* positions;
@ -66,8 +64,8 @@ struct ConstraintSolverData {
bool isWarmStartingActive;
/// Constructor
ConstraintSolverData() :linearVelocities(nullptr), angularVelocities(nullptr),
positions(nullptr), orientations(nullptr) {
ConstraintSolverData(DynamicsComponents& dynamicsComponents)
:dynamicsComponents(dynamicsComponents), positions(nullptr), orientations(nullptr) {
}
@ -171,7 +169,7 @@ class ConstraintSolver {
// -------------------- Methods -------------------- //
/// Constructor
ConstraintSolver(Islands& islands);
ConstraintSolver(Islands& islands, DynamicsComponents& dynamicsComponents);
/// Destructor
~ConstraintSolver() = default;
@ -191,10 +189,6 @@ class ConstraintSolver {
/// Enable/Disable the Non-Linear-Gauss-Seidel position correction technique.
void setIsNonLinearGaussSeidelPositionCorrectionActive(bool isActive);
/// Set the constrained velocities arrays
void setConstrainedVelocitiesArrays(Vector3* constrainedLinearVelocities,
Vector3* constrainedAngularVelocities);
/// Set the constrained positions/orientations arrays
void setConstrainedPositionsArrays(Vector3* constrainedPositions,
Quaternion* constrainedOrientations);
@ -208,17 +202,6 @@ class ConstraintSolver {
};
// Set the constrained velocities arrays
inline void ConstraintSolver::setConstrainedVelocitiesArrays(Vector3* constrainedLinearVelocities,
Vector3* constrainedAngularVelocities) {
assert(constrainedLinearVelocities != nullptr);
assert(constrainedAngularVelocities != nullptr);
mConstraintSolverData.linearVelocities = constrainedLinearVelocities;
mConstraintSolverData.angularVelocities = constrainedAngularVelocities;
}
// Set the constrained positions/orientations arrays
inline void ConstraintSolver::setConstrainedPositionsArrays(Vector3* constrainedPositions,
Quaternion* constrainedOrientations) {

147
src/engine/ContactSolver.cpp
View File

@ -31,6 +31,7 @@
#include "utils/Profiler.h"
#include "engine/Island.h"
#include "components/BodyComponents.h"
#include "components/DynamicsComponents.h"
#include "components/ProxyShapeComponents.h"
#include "collision/ContactManifold.h"
@ -43,13 +44,14 @@ const decimal ContactSolver::BETA_SPLIT_IMPULSE = decimal(0.2);
const decimal ContactSolver::SLOP = decimal(0.01);
// Constructor
ContactSolver::ContactSolver(MemoryManager& memoryManager, Islands& islands, BodyComponents& bodyComponents,
ContactSolver::ContactSolver(MemoryManager& memoryManager, Islands& islands, BodyComponents& bodyComponents, DynamicsComponents& dynamicsComponents,
ProxyShapeComponents& proxyShapeComponents, const WorldSettings& worldSettings)
:mMemoryManager(memoryManager), mSplitLinearVelocities(nullptr),
mSplitAngularVelocities(nullptr), mContactConstraints(nullptr),
mContactPoints(nullptr), mLinearVelocities(nullptr), mAngularVelocities(nullptr),
mContactPoints(nullptr),
mIslands(islands), mAllContactManifolds(nullptr), mAllContactPoints(nullptr), mBodyComponents(bodyComponents),
mProxyShapeComponents(proxyShapeComponents), mIsSplitImpulseActive(true), mWorldSettings(worldSettings) {
mDynamicsComponents(dynamicsComponents), mProxyShapeComponents(proxyShapeComponents), mIsSplitImpulseActive(true),
mWorldSettings(worldSettings) {
#ifdef IS_PROFILING_ACTIVE
mProfiler = nullptr;
@ -141,6 +143,8 @@ void ContactSolver::initializeForIsland(uint islandIndex) {
new (mContactConstraints + mNbContactManifolds) ContactManifoldSolver();
mContactConstraints[mNbContactManifolds].indexBody1 = body1->mArrayIndex;
mContactConstraints[mNbContactManifolds].indexBody2 = body2->mArrayIndex;
mContactConstraints[mNbContactManifolds].dynamicsComponentIndexBody1 = mDynamicsComponents.getEntityIndex(body1->getEntity());
mContactConstraints[mNbContactManifolds].dynamicsComponentIndexBody2 = mDynamicsComponents.getEntityIndex(body2->getEntity());
mContactConstraints[mNbContactManifolds].inverseInertiaTensorBody1 = body1->getInertiaTensorInverseWorld();
mContactConstraints[mNbContactManifolds].inverseInertiaTensorBody2 = body2->getInertiaTensorInverseWorld();
mContactConstraints[mNbContactManifolds].massInverseBody1 = body1->mMassInverse;
@ -154,10 +158,10 @@ void ContactSolver::initializeForIsland(uint islandIndex) {
mContactConstraints[mNbContactManifolds].frictionPointBody2.setToZero();
// Get the velocities of the bodies
const Vector3& v1 = mLinearVelocities[mContactConstraints[mNbContactManifolds].indexBody1];
const Vector3& w1 = mAngularVelocities[mContactConstraints[mNbContactManifolds].indexBody1];
const Vector3& v2 = mLinearVelocities[mContactConstraints[mNbContactManifolds].indexBody2];
const Vector3& w2 = mAngularVelocities[mContactConstraints[mNbContactManifolds].indexBody2];
const Vector3& v1 = mDynamicsComponents.getLinearVelocity(externalManifold.bodyEntity1);
const Vector3& w1 = mDynamicsComponents.getAngularVelocity(externalManifold.bodyEntity1);
const Vector3& v2 = mDynamicsComponents.getLinearVelocity(externalManifold.bodyEntity2);
const Vector3& w2 = mDynamicsComponents.getAngularVelocity(externalManifold.bodyEntity2);
// For each contact point of the contact manifold
assert(externalManifold.getNbContactPoints() > 0);
@ -346,6 +350,7 @@ void ContactSolver::warmStart() {
for (short int i=0; i<mContactConstraints[c].nbContacts; i++) {
// If it is not a new contact (this contact was already existing at last time step)
if (mContactPoints[contactPointIndex].isRestingContact) {
@ -357,22 +362,22 @@ void ContactSolver::warmStart() {
Vector3 impulsePenetration(mContactPoints[contactPointIndex].normal.x * mContactPoints[contactPointIndex].penetrationImpulse,
mContactPoints[contactPointIndex].normal.y * mContactPoints[contactPointIndex].penetrationImpulse,
mContactPoints[contactPointIndex].normal.z * mContactPoints[contactPointIndex].penetrationImpulse);
mLinearVelocities[mContactConstraints[c].indexBody1].x -= mContactConstraints[c].massInverseBody1 * impulsePenetration.x;
mLinearVelocities[mContactConstraints[c].indexBody1].y -= mContactConstraints[c].massInverseBody1 * impulsePenetration.y;
mLinearVelocities[mContactConstraints[c].indexBody1].z -= mContactConstraints[c].massInverseBody1 * impulsePenetration.z;
mDynamicsComponents.mConstrainedLinearVelocities[mContactConstraints[c].dynamicsComponentIndexBody1].x -= mContactConstraints[c].massInverseBody1 * impulsePenetration.x;
mDynamicsComponents.mConstrainedLinearVelocities[mContactConstraints[c].dynamicsComponentIndexBody1].y -= mContactConstraints[c].massInverseBody1 * impulsePenetration.y;
mDynamicsComponents.mConstrainedLinearVelocities[mContactConstraints[c].dynamicsComponentIndexBody1].z -= mContactConstraints[c].massInverseBody1 * impulsePenetration.z;
mAngularVelocities[mContactConstraints[c].indexBody1].x -= mContactPoints[contactPointIndex].i1TimesR1CrossN.x * mContactPoints[contactPointIndex].penetrationImpulse;
mAngularVelocities[mContactConstraints[c].indexBody1].y -= mContactPoints[contactPointIndex].i1TimesR1CrossN.y * mContactPoints[contactPointIndex].penetrationImpulse;
mAngularVelocities[mContactConstraints[c].indexBody1].z -= mContactPoints[contactPointIndex].i1TimesR1CrossN.z * mContactPoints[contactPointIndex].penetrationImpulse;
mDynamicsComponents.mConstrainedAngularVelocities[mContactConstraints[c].dynamicsComponentIndexBody1].x -= mContactPoints[contactPointIndex].i1TimesR1CrossN.x * mContactPoints[contactPointIndex].penetrationImpulse;
mDynamicsComponents.mConstrainedAngularVelocities[mContactConstraints[c].dynamicsComponentIndexBody1].y -= mContactPoints[contactPointIndex].i1TimesR1CrossN.y * mContactPoints[contactPointIndex].penetrationImpulse;
mDynamicsComponents.mConstrainedAngularVelocities[mContactConstraints[c].dynamicsComponentIndexBody1].z -= mContactPoints[contactPointIndex].i1TimesR1CrossN.z * mContactPoints[contactPointIndex].penetrationImpulse;
// Update the velocities of the body 2 by applying the impulse P
mLinearVelocities[mContactConstraints[c].indexBody2].x += mContactConstraints[c].massInverseBody2 * impulsePenetration.x;
mLinearVelocities[mContactConstraints[c].indexBody2].y += mContactConstraints[c].massInverseBody2 * impulsePenetration.y;
mLinearVelocities[mContactConstraints[c].indexBody2].z += mContactConstraints[c].massInverseBody2 * impulsePenetration.z;
mDynamicsComponents.mConstrainedLinearVelocities[mContactConstraints[c].dynamicsComponentIndexBody2].x += mContactConstraints[c].massInverseBody2 * impulsePenetration.x;
mDynamicsComponents.mConstrainedLinearVelocities[mContactConstraints[c].dynamicsComponentIndexBody2].y += mContactConstraints[c].massInverseBody2 * impulsePenetration.y;
mDynamicsComponents.mConstrainedLinearVelocities[mContactConstraints[c].dynamicsComponentIndexBody2].z += mContactConstraints[c].massInverseBody2 * impulsePenetration.z;
mAngularVelocities[mContactConstraints[c].indexBody2].x += mContactPoints[contactPointIndex].i2TimesR2CrossN.x * mContactPoints[contactPointIndex].penetrationImpulse;
mAngularVelocities[mContactConstraints[c].indexBody2].y += mContactPoints[contactPointIndex].i2TimesR2CrossN.y * mContactPoints[contactPointIndex].penetrationImpulse;
mAngularVelocities[mContactConstraints[c].indexBody2].z += mContactPoints[contactPointIndex].i2TimesR2CrossN.z * mContactPoints[contactPointIndex].penetrationImpulse;
mDynamicsComponents.mConstrainedAngularVelocities[mContactConstraints[c].dynamicsComponentIndexBody2].x += mContactPoints[contactPointIndex].i2TimesR2CrossN.x * mContactPoints[contactPointIndex].penetrationImpulse;
mDynamicsComponents.mConstrainedAngularVelocities[mContactConstraints[c].dynamicsComponentIndexBody2].y += mContactPoints[contactPointIndex].i2TimesR2CrossN.y * mContactPoints[contactPointIndex].penetrationImpulse;
mDynamicsComponents.mConstrainedAngularVelocities[mContactConstraints[c].dynamicsComponentIndexBody2].z += mContactPoints[contactPointIndex].i2TimesR2CrossN.z * mContactPoints[contactPointIndex].penetrationImpulse;
}
else { // If it is a new contact point
@ -412,12 +417,12 @@ void ContactSolver::warmStart() {
mContactConstraints[c].r2CrossT1.z * mContactConstraints[c].friction1Impulse);
// Update the velocities of the body 1 by applying the impulse P
mLinearVelocities[mContactConstraints[c].indexBody1] -= mContactConstraints[c].massInverseBody1 * linearImpulseBody2;
mAngularVelocities[mContactConstraints[c].indexBody1] += mContactConstraints[c].inverseInertiaTensorBody1 * angularImpulseBody1;
mDynamicsComponents.mConstrainedLinearVelocities[mContactConstraints[c].dynamicsComponentIndexBody1] -= mContactConstraints[c].massInverseBody1 * linearImpulseBody2;
mDynamicsComponents.mConstrainedAngularVelocities[mContactConstraints[c].dynamicsComponentIndexBody1] += mContactConstraints[c].inverseInertiaTensorBody1 * angularImpulseBody1;
// Update the velocities of the body 1 by applying the impulse P
mLinearVelocities[mContactConstraints[c].indexBody2] += mContactConstraints[c].massInverseBody2 * linearImpulseBody2;
mAngularVelocities[mContactConstraints[c].indexBody2] += mContactConstraints[c].inverseInertiaTensorBody2 * angularImpulseBody2;
mDynamicsComponents.mConstrainedLinearVelocities[mContactConstraints[c].dynamicsComponentIndexBody2] += mContactConstraints[c].massInverseBody2 * linearImpulseBody2;
mDynamicsComponents.mConstrainedAngularVelocities[mContactConstraints[c].dynamicsComponentIndexBody2] += mContactConstraints[c].inverseInertiaTensorBody2 * angularImpulseBody2;
// ------ Second friction constraint at the center of the contact manifold ----- //
@ -433,18 +438,18 @@ void ContactSolver::warmStart() {
angularImpulseBody2.z = mContactConstraints[c].r2CrossT2.z * mContactConstraints[c].friction2Impulse;
// Update the velocities of the body 1 by applying the impulse P
mLinearVelocities[mContactConstraints[c].indexBody1].x -= mContactConstraints[c].massInverseBody1 * linearImpulseBody2.x;
mLinearVelocities[mContactConstraints[c].indexBody1].y -= mContactConstraints[c].massInverseBody1 * linearImpulseBody2.y;
mLinearVelocities[mContactConstraints[c].indexBody1].z -= mContactConstraints[c].massInverseBody1 * linearImpulseBody2.z;
mDynamicsComponents.mConstrainedLinearVelocities[mContactConstraints[c].dynamicsComponentIndexBody1].x -= mContactConstraints[c].massInverseBody1 * linearImpulseBody2.x;
mDynamicsComponents.mConstrainedLinearVelocities[mContactConstraints[c].dynamicsComponentIndexBody1].y -= mContactConstraints[c].massInverseBody1 * linearImpulseBody2.y;
mDynamicsComponents.mConstrainedLinearVelocities[mContactConstraints[c].dynamicsComponentIndexBody1].z -= mContactConstraints[c].massInverseBody1 * linearImpulseBody2.z;
mAngularVelocities[mContactConstraints[c].indexBody1] += mContactConstraints[c].inverseInertiaTensorBody1 * angularImpulseBody1;
mDynamicsComponents.mConstrainedAngularVelocities[mContactConstraints[c].dynamicsComponentIndexBody1] += mContactConstraints[c].inverseInertiaTensorBody1 * angularImpulseBody1;
// Update the velocities of the body 2 by applying the impulse P
mLinearVelocities[mContactConstraints[c].indexBody2].x += mContactConstraints[c].massInverseBody2 * linearImpulseBody2.x;
mLinearVelocities[mContactConstraints[c].indexBody2].y += mContactConstraints[c].massInverseBody2 * linearImpulseBody2.y;
mLinearVelocities[mContactConstraints[c].indexBody2].z += mContactConstraints[c].massInverseBody2 * linearImpulseBody2.z;
mDynamicsComponents.mConstrainedLinearVelocities[mContactConstraints[c].dynamicsComponentIndexBody2].x += mContactConstraints[c].massInverseBody2 * linearImpulseBody2.x;
mDynamicsComponents.mConstrainedLinearVelocities[mContactConstraints[c].dynamicsComponentIndexBody2].y += mContactConstraints[c].massInverseBody2 * linearImpulseBody2.y;
mDynamicsComponents.mConstrainedLinearVelocities[mContactConstraints[c].dynamicsComponentIndexBody2].z += mContactConstraints[c].massInverseBody2 * linearImpulseBody2.z;
mAngularVelocities[mContactConstraints[c].indexBody2] += mContactConstraints[c].inverseInertiaTensorBody2 * angularImpulseBody2;
mDynamicsComponents.mConstrainedAngularVelocities[mContactConstraints[c].dynamicsComponentIndexBody2] += mContactConstraints[c].inverseInertiaTensorBody2 * angularImpulseBody2;
// ------ Twist friction constraint at the center of the contact manifold ------ //
@ -458,10 +463,10 @@ void ContactSolver::warmStart() {
angularImpulseBody2.z = mContactConstraints[c].normal.z * mContactConstraints[c].frictionTwistImpulse;
// Update the velocities of the body 1 by applying the impulse P
mAngularVelocities[mContactConstraints[c].indexBody1] += mContactConstraints[c].inverseInertiaTensorBody1 * angularImpulseBody1;
mDynamicsComponents.mConstrainedAngularVelocities[mContactConstraints[c].dynamicsComponentIndexBody1] += mContactConstraints[c].inverseInertiaTensorBody1 * angularImpulseBody1;
// Update the velocities of the body 2 by applying the impulse P
mAngularVelocities[mContactConstraints[c].indexBody2] += mContactConstraints[c].inverseInertiaTensorBody2 * angularImpulseBody2;
mDynamicsComponents.mConstrainedAngularVelocities[mContactConstraints[c].dynamicsComponentIndexBody2] += mContactConstraints[c].inverseInertiaTensorBody2 * angularImpulseBody2;
// ------ Rolling resistance at the center of the contact manifold ------ //
@ -469,10 +474,10 @@ void ContactSolver::warmStart() {
angularImpulseBody2 = mContactConstraints[c].rollingResistanceImpulse;
// Update the velocities of the body 1 by applying the impulse P
mAngularVelocities[mContactConstraints[c].indexBody1] -= mContactConstraints[c].inverseInertiaTensorBody1 * angularImpulseBody2;
mDynamicsComponents.mConstrainedAngularVelocities[mContactConstraints[c].dynamicsComponentIndexBody1] -= mContactConstraints[c].inverseInertiaTensorBody1 * angularImpulseBody2;
// Update the velocities of the body 1 by applying the impulse P
mAngularVelocities[mContactConstraints[c].indexBody2] += mContactConstraints[c].inverseInertiaTensorBody2 * angularImpulseBody2;
mDynamicsComponents.mConstrainedAngularVelocities[mContactConstraints[c].dynamicsComponentIndexBody2] += mContactConstraints[c].inverseInertiaTensorBody2 * angularImpulseBody2;
}
else { // If it is a new contact manifold
@ -500,10 +505,10 @@ void ContactSolver::solve() {
decimal sumPenetrationImpulse = 0.0;
// Get the constrained velocities
const Vector3& v1 = mLinearVelocities[mContactConstraints[c].indexBody1];
const Vector3& w1 = mAngularVelocities[mContactConstraints[c].indexBody1];
const Vector3& v2 = mLinearVelocities[mContactConstraints[c].indexBody2];
const Vector3& w2 = mAngularVelocities[mContactConstraints[c].indexBody2];
const Vector3& v1 = mDynamicsComponents.mConstrainedLinearVelocities[mContactConstraints[c].dynamicsComponentIndexBody1];
const Vector3& w1 = mDynamicsComponents.mConstrainedAngularVelocities[mContactConstraints[c].dynamicsComponentIndexBody1];
const Vector3& v2 = mDynamicsComponents.mConstrainedLinearVelocities[mContactConstraints[c].dynamicsComponentIndexBody2];
const Vector3& w2 = mDynamicsComponents.mConstrainedAngularVelocities[mContactConstraints[c].dynamicsComponentIndexBody2];
for (short int i=0; i<mContactConstraints[c].nbContacts; i++) {
@ -546,22 +551,22 @@ void ContactSolver::solve() {
mContactPoints[contactPointIndex].normal.z * deltaLambda);
// Update the velocities of the body 1 by applying the impulse P
mLinearVelocities[mContactConstraints[c].indexBody1].x -= mContactConstraints[c].massInverseBody1 * linearImpulse.x;
mLinearVelocities[mContactConstraints[c].indexBody1].y -= mContactConstraints[c].massInverseBody1 * linearImpulse.y;
mLinearVelocities[mContactConstraints[c].indexBody1].z -= mContactConstraints[c].massInverseBody1 * linearImpulse.z;
mDynamicsComponents.mConstrainedLinearVelocities[mContactConstraints[c].dynamicsComponentIndexBody1].x -= mContactConstraints[c].massInverseBody1 * linearImpulse.x;
mDynamicsComponents.mConstrainedLinearVelocities[mContactConstraints[c].dynamicsComponentIndexBody1].y -= mContactConstraints[c].massInverseBody1 * linearImpulse.y;
mDynamicsComponents.mConstrainedLinearVelocities[mContactConstraints[c].dynamicsComponentIndexBody1].z -= mContactConstraints[c].massInverseBody1 * linearImpulse.z;
mAngularVelocities[mContactConstraints[c].indexBody1].x -= mContactPoints[contactPointIndex].i1TimesR1CrossN.x * deltaLambda;
mAngularVelocities[mContactConstraints[c].indexBody1].y -= mContactPoints[contactPointIndex].i1TimesR1CrossN.y * deltaLambda;
mAngularVelocities[mContactConstraints[c].indexBody1].z -= mContactPoints[contactPointIndex].i1TimesR1CrossN.z * deltaLambda;
mDynamicsComponents.mConstrainedAngularVelocities[mContactConstraints[c].dynamicsComponentIndexBody1].x -= mContactPoints[contactPointIndex].i1TimesR1CrossN.x * deltaLambda;
mDynamicsComponents.mConstrainedAngularVelocities[mContactConstraints[c].dynamicsComponentIndexBody1].y -= mContactPoints[contactPointIndex].i1TimesR1CrossN.y * deltaLambda;
mDynamicsComponents.mConstrainedAngularVelocities[mContactConstraints[c].dynamicsComponentIndexBody1].z -= mContactPoints[contactPointIndex].i1TimesR1CrossN.z * deltaLambda;
// Update the velocities of the body 2 by applying the impulse P
mLinearVelocities[mContactConstraints[c].indexBody2].x += mContactConstraints[c].massInverseBody2 * linearImpulse.x;
mLinearVelocities[mContactConstraints[c].indexBody2].y += mContactConstraints[c].massInverseBody2 * linearImpulse.y;
mLinearVelocities[mContactConstraints[c].indexBody2].z += mContactConstraints[c].massInverseBody2 * linearImpulse.z;
mDynamicsComponents.mConstrainedLinearVelocities[mContactConstraints[c].dynamicsComponentIndexBody2].x += mContactConstraints[c].massInverseBody2 * linearImpulse.x;
mDynamicsComponents.mConstrainedLinearVelocities[mContactConstraints[c].dynamicsComponentIndexBody2].y += mContactConstraints[c].massInverseBody2 * linearImpulse.y;
mDynamicsComponents.mConstrainedLinearVelocities[mContactConstraints[c].dynamicsComponentIndexBody2].z += mContactConstraints[c].massInverseBody2 * linearImpulse.z;
mAngularVelocities[mContactConstraints[c].indexBody2].x += mContactPoints[contactPointIndex].i2TimesR2CrossN.x * deltaLambda;
mAngularVelocities[mContactConstraints[c].indexBody2].y += mContactPoints[contactPointIndex].i2TimesR2CrossN.y * deltaLambda;
mAngularVelocities[mContactConstraints[c].indexBody2].z += mContactPoints[contactPointIndex].i2TimesR2CrossN.z * deltaLambda;
mDynamicsComponents.mConstrainedAngularVelocities[mContactConstraints[c].dynamicsComponentIndexBody2].x += mContactPoints[contactPointIndex].i2TimesR2CrossN.x * deltaLambda;
mDynamicsComponents.mConstrainedAngularVelocities[mContactConstraints[c].dynamicsComponentIndexBody2].y += mContactPoints[contactPointIndex].i2TimesR2CrossN.y * deltaLambda;
mDynamicsComponents.mConstrainedAngularVelocities[mContactConstraints[c].dynamicsComponentIndexBody2].z += mContactPoints[contactPointIndex].i2TimesR2CrossN.z * deltaLambda;
sumPenetrationImpulse += mContactPoints[contactPointIndex].penetrationImpulse;
@ -653,18 +658,18 @@ void ContactSolver::solve() {
mContactConstraints[c].r2CrossT1.z * deltaLambda);
// Update the velocities of the body 1 by applying the impulse P
mLinearVelocities[mContactConstraints[c].indexBody1].x -= mContactConstraints[c].massInverseBody1 * linearImpulseBody2.x;
mLinearVelocities[mContactConstraints[c].indexBody1].y -= mContactConstraints[c].massInverseBody1 * linearImpulseBody2.y;
mLinearVelocities[mContactConstraints[c].indexBody1].z -= mContactConstraints[c].massInverseBody1 * linearImpulseBody2.z;
mDynamicsComponents.mConstrainedLinearVelocities[mContactConstraints[c].dynamicsComponentIndexBody1].x -= mContactConstraints[c].massInverseBody1 * linearImpulseBody2.x;
mDynamicsComponents.mConstrainedLinearVelocities[mContactConstraints[c].dynamicsComponentIndexBody1].y -= mContactConstraints[c].massInverseBody1 * linearImpulseBody2.y;
mDynamicsComponents.mConstrainedLinearVelocities[mContactConstraints[c].dynamicsComponentIndexBody1].z -= mContactConstraints[c].massInverseBody1 * linearImpulseBody2.z;
mAngularVelocities[mContactConstraints[c].indexBody1] += mContactConstraints[c].inverseInertiaTensorBody1 * angularImpulseBody1;
mDynamicsComponents.mConstrainedAngularVelocities[mContactConstraints[c].dynamicsComponentIndexBody1] += mContactConstraints[c].inverseInertiaTensorBody1 * angularImpulseBody1;
// Update the velocities of the body 2 by applying the impulse P
mLinearVelocities[mContactConstraints[c].indexBody2].x += mContactConstraints[c].massInverseBody2 * linearImpulseBody2.x;
mLinearVelocities[mContactConstraints[c].indexBody2].y += mContactConstraints[c].massInverseBody2 * linearImpulseBody2.y;
mLinearVelocities[mContactConstraints[c].indexBody2].z += mContactConstraints[c].massInverseBody2 * linearImpulseBody2.z;
mDynamicsComponents.mConstrainedLinearVelocities[mContactConstraints[c].dynamicsComponentIndexBody2].x += mContactConstraints[c].massInverseBody2 * linearImpulseBody2.x;
mDynamicsComponents.mConstrainedLinearVelocities[mContactConstraints[c].dynamicsComponentIndexBody2].y += mContactConstraints[c].massInverseBody2 * linearImpulseBody2.y;
mDynamicsComponents.mConstrainedLinearVelocities[mContactConstraints[c].dynamicsComponentIndexBody2].z += mContactConstraints[c].massInverseBody2 * linearImpulseBody2.z;
mAngularVelocities[mContactConstraints[c].indexBody2] += mContactConstraints[c].inverseInertiaTensorBody2 * angularImpulseBody2;
mDynamicsComponents.mConstrainedAngularVelocities[mContactConstraints[c].dynamicsComponentIndexBody2] += mContactConstraints[c].inverseInertiaTensorBody2 * angularImpulseBody2;
// ------ Second friction constraint at the center of the contact manifol ----- //
@ -702,16 +707,16 @@ void ContactSolver::solve() {
angularImpulseBody2.z = mContactConstraints[c].r2CrossT2.z * deltaLambda;
// Update the velocities of the body 1 by applying the impulse P
mLinearVelocities[mContactConstraints[c].indexBody1].x -= mContactConstraints[c].massInverseBody1 * linearImpulseBody2.x;
mLinearVelocities[mContactConstraints[c].indexBody1].y -= mContactConstraints[c].massInverseBody1 * linearImpulseBody2.y;
mLinearVelocities[mContactConstraints[c].indexBody1].z -= mContactConstraints[c].massInverseBody1 * linearImpulseBody2.z;
mAngularVelocities[mContactConstraints[c].indexBody1] += mContactConstraints[c].inverseInertiaTensorBody1 * angularImpulseBody1;
mDynamicsComponents.mConstrainedLinearVelocities[mContactConstraints[c].dynamicsComponentIndexBody1].x -= mContactConstraints[c].massInverseBody1 * linearImpulseBody2.x;
mDynamicsComponents.mConstrainedLinearVelocities[mContactConstraints[c].dynamicsComponentIndexBody1].y -= mContactConstraints[c].massInverseBody1 * linearImpulseBody2.y;
mDynamicsComponents.mConstrainedLinearVelocities[mContactConstraints[c].dynamicsComponentIndexBody1].z -= mContactConstraints[c].massInverseBody1 * linearImpulseBody2.z;
mDynamicsComponents.mConstrainedAngularVelocities[mContactConstraints[c].dynamicsComponentIndexBody1] += mContactConstraints[c].inverseInertiaTensorBody1 * angularImpulseBody1;
// Update the velocities of the body 2 by applying the impulse P
mLinearVelocities[mContactConstraints[c].indexBody2].x += mContactConstraints[c].massInverseBody2 * linearImpulseBody2.x;
mLinearVelocities[mContactConstraints[c].indexBody2].y += mContactConstraints[c].massInverseBody2 * linearImpulseBody2.y;
mLinearVelocities[mContactConstraints[c].indexBody2].z += mContactConstraints[c].massInverseBody2 * linearImpulseBody2.z;
mAngularVelocities[mContactConstraints[c].indexBody2] += mContactConstraints[c].inverseInertiaTensorBody2 * angularImpulseBody2;
mDynamicsComponents.mConstrainedLinearVelocities[mContactConstraints[c].dynamicsComponentIndexBody2].x += mContactConstraints[c].massInverseBody2 * linearImpulseBody2.x;
mDynamicsComponents.mConstrainedLinearVelocities[mContactConstraints[c].dynamicsComponentIndexBody2].y += mContactConstraints[c].massInverseBody2 * linearImpulseBody2.y;
mDynamicsComponents.mConstrainedLinearVelocities[mContactConstraints[c].dynamicsComponentIndexBody2].z += mContactConstraints[c].massInverseBody2 * linearImpulseBody2.z;
mDynamicsComponents.mConstrainedAngularVelocities[mContactConstraints[c].dynamicsComponentIndexBody2] += mContactConstraints[c].inverseInertiaTensorBody2 * angularImpulseBody2;
// ------ Twist friction constraint at the center of the contact manifol ------ //
@ -734,10 +739,10 @@ void ContactSolver::solve() {
angularImpulseBody2.z = mContactConstraints[c].normal.z * deltaLambda;
// Update the velocities of the body 1 by applying the impulse P
mAngularVelocities[mContactConstraints[c].indexBody1] -= mContactConstraints[c].inverseInertiaTensorBody1 * angularImpulseBody2;
mDynamicsComponents.mConstrainedAngularVelocities[mContactConstraints[c].dynamicsComponentIndexBody1] -= mContactConstraints[c].inverseInertiaTensorBody1 * angularImpulseBody2;
// Update the velocities of the body 1 by applying the impulse P
mAngularVelocities[mContactConstraints[c].indexBody2] += mContactConstraints[c].inverseInertiaTensorBody2 * angularImpulseBody2;
mDynamicsComponents.mConstrainedAngularVelocities[mContactConstraints[c].dynamicsComponentIndexBody2] += mContactConstraints[c].inverseInertiaTensorBody2 * angularImpulseBody2;
// --------- Rolling resistance constraint at the center of the contact manifold --------- //
@ -755,10 +760,10 @@ void ContactSolver::solve() {
deltaLambdaRolling = mContactConstraints[c].rollingResistanceImpulse - lambdaTempRolling;
// Update the velocities of the body 1 by applying the impulse P
mAngularVelocities[mContactConstraints[c].indexBody1] -= mContactConstraints[c].inverseInertiaTensorBody1 * deltaLambdaRolling;
mDynamicsComponents.mConstrainedAngularVelocities[mContactConstraints[c].dynamicsComponentIndexBody1] -= mContactConstraints[c].inverseInertiaTensorBody1 * deltaLambdaRolling;
// Update the velocities of the body 2 by applying the impulse P
mAngularVelocities[mContactConstraints[c].indexBody2] += mContactConstraints[c].inverseInertiaTensorBody2 * deltaLambdaRolling;
mDynamicsComponents.mConstrainedAngularVelocities[mContactConstraints[c].dynamicsComponentIndexBody2] += mContactConstraints[c].inverseInertiaTensorBody2 * deltaLambdaRolling;
}
}
}

38
src/engine/ContactSolver.h
View File

@ -44,6 +44,7 @@ class Profiler;
class Island;
class RigidBody;
class BodyComponents;
class DynamicsComponents;
class ProxyShapeComponents;
// Class Contact Solver
@ -174,11 +175,19 @@ class ContactSolver {
ContactManifold* externalContactManifold;
/// Index of body 1 in the constraint solver
// TODO : Remove this
int32 indexBody1;
/// Index of body 2 in the constraint solver
// TODO : Remove this
int32 indexBody2;
/// Index of body 1 in the dynamics components arrays
uint32 dynamicsComponentIndexBody1;
/// Index of body 2 in the dynamics components arrays
uint32 dynamicsComponentIndexBody2;
/// Inverse of the mass of body 1
decimal massInverseBody1;
@ -307,14 +316,6 @@ class ContactSolver {
/// Number of contact constraints
uint mNbContactManifolds;
/// Array of linear velocities
// TODO : Use List<> here
Vector3* mLinearVelocities;
/// Array of angular velocities
// TODO : Use List<> here
Vector3* mAngularVelocities;
/// Reference to the islands
Islands& mIslands;
@ -327,6 +328,9 @@ class ContactSolver {
/// Reference to the body components
BodyComponents& mBodyComponents;
/// Reference to the dynamics components
DynamicsComponents& mDynamicsComponents;
/// Reference to the proxy-shapes components
// TODO : Do we really need to use this ?
ProxyShapeComponents& mProxyShapeComponents;
@ -372,7 +376,8 @@ class ContactSolver {
/// Constructor
ContactSolver(MemoryManager& memoryManager, Islands& islands, BodyComponents& bodyComponents,
ProxyShapeComponents& proxyShapeComponents, const WorldSettings& worldSettings);
DynamicsComponents& dynamicsComponents, ProxyShapeComponents& proxyShapeComponents,
const WorldSettings& worldSettings);
/// Destructor
~ContactSolver() = default;
@ -387,10 +392,6 @@ class ContactSolver {
void setSplitVelocitiesArrays(Vector3* splitLinearVelocities,
Vector3* splitAngularVelocities);
/// Set the constrained velocities arrays
void setConstrainedVelocitiesArrays(Vector3* constrainedLinearVelocities,
Vector3* constrainedAngularVelocities);
/// Store the computed impulses to use them to
/// warm start the solver at the next iteration
void storeImpulses();
@ -423,17 +424,6 @@ inline void ContactSolver::setSplitVelocitiesArrays(Vector3* splitLinearVelociti
mSplitAngularVelocities = splitAngularVelocities;
}
// Set the constrained velocities arrays
inline void ContactSolver::setConstrainedVelocitiesArrays(Vector3* constrainedLinearVelocities,
Vector3* constrainedAngularVelocities) {
assert(constrainedLinearVelocities != nullptr);
assert(constrainedAngularVelocities != nullptr);
mLinearVelocities = constrainedLinearVelocities;
mAngularVelocities = constrainedAngularVelocities;
}
// Return true if the split impulses position correction technique is used for contacts
inline bool ContactSolver::isSplitImpulseActive() const {
return mIsSplitImpulseActive;

49
src/engine/DynamicsWorld.cpp
View File

@ -50,19 +50,15 @@ using namespace std;
DynamicsWorld::DynamicsWorld(const Vector3& gravity, const WorldSettings& worldSettings, Logger* logger, Profiler* profiler)
: CollisionWorld(worldSettings, logger, profiler),
mIslands(mMemoryManager.getSingleFrameAllocator()),
mContactSolver(mMemoryManager, mIslands, mBodyComponents, mProxyShapesComponents, mConfig),
mConstraintSolver(mIslands),
mContactSolver(mMemoryManager, mIslands, mBodyComponents, mDynamicsComponents, mProxyShapesComponents, mConfig),
mConstraintSolver(mIslands, mDynamicsComponents),
mNbVelocitySolverIterations(mConfig.defaultVelocitySolverNbIterations),
mNbPositionSolverIterations(mConfig.defaultPositionSolverNbIterations),
mIsSleepingEnabled(mConfig.isSleepingEnabled), mRigidBodies(mMemoryManager.getPoolAllocator()),
mJoints(mMemoryManager.getPoolAllocator()), mGravity(gravity), mTimeStep(decimal(1.0f / 60.0f)),
mIsGravityEnabled(true), mConstrainedLinearVelocities(nullptr),
mConstrainedAngularVelocities(nullptr), mSplitLinearVelocities(nullptr),
mSplitAngularVelocities(nullptr), mConstrainedPositions(nullptr),
mConstrainedOrientations(nullptr),
mSleepLinearVelocity(mConfig.defaultSleepLinearVelocity),
mSleepAngularVelocity(mConfig.defaultSleepAngularVelocity),
mTimeBeforeSleep(mConfig.defaultTimeBeforeSleep),
mIsGravityEnabled(true), mSplitLinearVelocities(nullptr), mSplitAngularVelocities(nullptr), mConstrainedPositions(nullptr),
mConstrainedOrientations(nullptr), mSleepLinearVelocity(mConfig.defaultSleepLinearVelocity),
mSleepAngularVelocity(mConfig.defaultSleepAngularVelocity), mTimeBeforeSleep(mConfig.defaultTimeBeforeSleep),
mFreeJointsIDs(mMemoryManager.getPoolAllocator()), mCurrentJointId(0) {
#ifdef IS_PROFILING_ACTIVE
@ -181,8 +177,8 @@ void DynamicsWorld::integrateRigidBodiesPositions() {
// Get the constrained velocity
uint indexArray = body->mArrayIndex;
Vector3 newLinVelocity = mConstrainedLinearVelocities[indexArray];
Vector3 newAngVelocity = mConstrainedAngularVelocities[indexArray];
Vector3 newLinVelocity = mDynamicsComponents.getConstrainedLinearVelocity(bodyEntity);
Vector3 newAngVelocity = mDynamicsComponents.getConstrainedAngularVelocity(bodyEntity);
// TODO : Remove this
Vector3 testLinVel = newLinVelocity;
@ -229,8 +225,8 @@ void DynamicsWorld::updateBodiesState() {
uint index = body->mArrayIndex;
// Update the linear and angular velocity of the body
mDynamicsComponents.setLinearVelocity(bodyEntity, mConstrainedLinearVelocities[index]);
mDynamicsComponents.setAngularVelocity(bodyEntity, mConstrainedAngularVelocities[index]);
mDynamicsComponents.setLinearVelocity(bodyEntity, mDynamicsComponents.getConstrainedLinearVelocity(bodyEntity));
mDynamicsComponents.setAngularVelocity(bodyEntity, mDynamicsComponents.getConstrainedAngularVelocity(bodyEntity));
// Update the position of the center of mass of the body
body->mCenterOfMassWorld = mConstrainedPositions[index];
@ -267,18 +263,12 @@ void DynamicsWorld::initVelocityArrays() {
nbBodies * sizeof(Vector3)));
mSplitAngularVelocities = static_cast<Vector3*>(mMemoryManager.allocate(MemoryManager::AllocationType::Frame,
nbBodies * sizeof(Vector3)));
mConstrainedLinearVelocities = static_cast<Vector3*>(mMemoryManager.allocate(MemoryManager::AllocationType::Frame,
nbBodies * sizeof(Vector3)));
mConstrainedAngularVelocities = static_cast<Vector3*>(mMemoryManager.allocate(MemoryManager::AllocationType::Frame,
nbBodies * sizeof(Vector3)));
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(mSplitLinearVelocities != nullptr);
assert(mSplitAngularVelocities != nullptr);
assert(mConstrainedLinearVelocities != nullptr);
assert(mConstrainedAngularVelocities != nullptr);
assert(mConstrainedPositions != nullptr);
assert(mConstrainedOrientations != nullptr);
@ -324,18 +314,17 @@ void DynamicsWorld::integrateRigidBodiesVelocities() {
assert(indexBody < mRigidBodies.size());
// Integrate the external force to get the new velocity of the body
mConstrainedLinearVelocities[indexBody] = body->getLinearVelocity() +
mTimeStep * body->mMassInverse * body->mExternalForce;
mConstrainedAngularVelocities[indexBody] = body->getAngularVelocity() +
mTimeStep * body->getInertiaTensorInverseWorld() *
body->mExternalTorque;
mDynamicsComponents.setConstrainedLinearVelocity(bodyEntity, body->getLinearVelocity() +
mTimeStep * body->mMassInverse * body->mExternalForce);
mDynamicsComponents.setConstrainedAngularVelocity(bodyEntity, body->getAngularVelocity() +
mTimeStep * body->getInertiaTensorInverseWorld() * body->mExternalTorque);
// If the gravity has to be applied to this rigid body
if (body->isGravityEnabled() && mIsGravityEnabled) {
// Integrate the gravity force
mConstrainedLinearVelocities[indexBody] += mTimeStep * body->mMassInverse *
body->getMass() * mGravity;
mDynamicsComponents.setConstrainedLinearVelocity(bodyEntity, mDynamicsComponents.getConstrainedLinearVelocity(bodyEntity) + mTimeStep * body->mMassInverse *
body->getMass() * mGravity);
}
// Apply the velocity damping
@ -355,8 +344,8 @@ void DynamicsWorld::integrateRigidBodiesVelocities() {
decimal angDampingFactor = body->getAngularDamping();
decimal linearDamping = pow(decimal(1.0) - linDampingFactor, mTimeStep);
decimal angularDamping = pow(decimal(1.0) - angDampingFactor, mTimeStep);
mConstrainedLinearVelocities[indexBody] *= linearDamping;
mConstrainedAngularVelocities[indexBody] *= angularDamping;
mDynamicsComponents.setConstrainedLinearVelocity(bodyEntity, mDynamicsComponents.getConstrainedLinearVelocity(bodyEntity) * linearDamping);
mDynamicsComponents.setConstrainedAngularVelocity(bodyEntity, mDynamicsComponents.getConstrainedAngularVelocity(bodyEntity) * angularDamping);
}
}
}
@ -368,10 +357,6 @@ void DynamicsWorld::solveContactsAndConstraints() {
// Set the velocities arrays
mContactSolver.setSplitVelocitiesArrays(mSplitLinearVelocities, mSplitAngularVelocities);
mContactSolver.setConstrainedVelocitiesArrays(mConstrainedLinearVelocities,
mConstrainedAngularVelocities);
mConstraintSolver.setConstrainedVelocitiesArrays(mConstrainedLinearVelocities,
mConstrainedAngularVelocities);
mConstraintSolver.setConstrainedPositionsArrays(mConstrainedPositions,
mConstrainedOrientations);

10
src/engine/DynamicsWorld.h
View File

@ -88,16 +88,6 @@ class DynamicsWorld : public CollisionWorld {
/// True if the gravity force is on
bool mIsGravityEnabled;
// TODO : Move this into dynamic components
/// Array of constrained linear velocities (state of the linear velocities
/// after solving the constraints)
Vector3* mConstrainedLinearVelocities;
// TODO : Move this into dynamic components
/// Array of constrained angular velocities (state of the angular velocities
/// after solving the constraints)
Vector3* mConstrainedAngularVelocities;
// TODO : Move this into dynamic components
/// Split linear velocities for the position contact solver (split impulse)
Vector3* mSplitLinearVelocities;