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

Add DynamicsSystem class

Browse Source
This commit is contained in:
Daniel Chappuis 2019-08-08 17:45:22 +02:00
parent db995ea52c
commit 9b38fc1626
25 changed files with 416 additions and 296 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
CMakeLists.txt
View File

@ -129,8 +129,9 @@ SET (REACTPHYSICS3D_HEADERS
"src/engine/Entity.h"
"src/engine/EntityManager.h"
"src/engine/CollisionWorld.h"
"src/engine/ConstraintSolver.h"
"src/engine/ContactSolver.h"
"src/systems/ConstraintSolverSystem.h"
"src/systems/ContactSolverSystem.h"
"src/systems/DynamicsSystem.h"
"src/engine/DynamicsWorld.h"
"src/engine/EventListener.h"
"src/engine/Island.h"
@ -219,8 +220,9 @@ SET (REACTPHYSICS3D_SOURCES
"src/constraint/Joint.cpp"
"src/constraint/SliderJoint.cpp"
"src/engine/CollisionWorld.cpp"
"src/engine/ConstraintSolver.cpp"
"src/engine/ContactSolver.cpp"
"src/systems/ConstraintSolverSystem.cpp"
"src/systems/ContactSolverSystem.cpp"
"src/systems/DynamicsSystem.cpp"
"src/engine/DynamicsWorld.cpp"
"src/engine/Island.cpp"
"src/engine/Material.cpp"

6
src/body/CollisionBody.cpp
View File

@ -214,14 +214,14 @@ const Transform& CollisionBody::getTransform() const {
}
// Update the broad-phase state for this body (because it has moved for instance)
void CollisionBody::updateBroadPhaseState() const {
void CollisionBody::updateBroadPhaseState(decimal timeStep) const {
// For all the proxy collision shapes of the body
const List<Entity>& proxyShapesEntities = mWorld.mCollisionBodyComponents.getProxyShapes(mEntity);
for (uint i=0; i < proxyShapesEntities.size(); i++) {
// Update the proxy
mWorld.mCollisionDetection.updateProxyShape(proxyShapesEntities[i]);
mWorld.mCollisionDetection.updateProxyShape(proxyShapesEntities[i], timeStep);
}
}
@ -390,7 +390,7 @@ void CollisionBody::setTransform(const Transform& transform) {
mWorld.mTransformComponents.setTransform(mEntity, transform);
// Update the broad-phase state of the body
updateBroadPhaseState();
updateBroadPhaseState(0);
RP3D_LOG(mLogger, Logger::Level::Information, Logger::Category::Body,
"Body " + std::to_string(mEntity.id) + ": Set transform=" + transform.to_string());

2
src/body/CollisionBody.h
View File

@ -82,7 +82,7 @@ class CollisionBody {
void removeAllCollisionShapes();
/// Update the broad-phase state for this body (because it has moved for instance)
void updateBroadPhaseState() const;
void updateBroadPhaseState(decimal timeStep) const;
/// Ask the broad-phase to test again the collision shapes of the body for collision
/// (as if the body has moved).

2
src/body/RigidBody.h
View File

@ -218,7 +218,7 @@ class RigidBody : public CollisionBody {
// -------------------- Friendship -------------------- //
friend class DynamicsWorld;
friend class ContactSolver;
friend class ContactSolverSystem;
friend class BallAndSocketJoint;
friend class SliderJoint;
friend class HingeJoint;

12
src/collision/CollisionDetection.h
View File

@ -292,10 +292,10 @@ class CollisionDetection {
void removeProxyCollisionShape(ProxyShape* proxyShape);
/// Update a proxy collision shape (that has moved for instance)
void updateProxyShape(Entity proxyShapeEntity);
void updateProxyShape(Entity proxyShapeEntity, decimal timeStep);
/// Update all the enabled proxy-shapes
void updateProxyShapes();
void updateProxyShapes(decimal timeStep);
/// Add a pair of bodies that cannot collide with each other
void addNoCollisionPair(CollisionBody* body1, CollisionBody* body2);
@ -411,15 +411,15 @@ inline MemoryManager& CollisionDetection::getMemoryManager() const {
}
// Update a proxy collision shape (that has moved for instance)
inline void CollisionDetection::updateProxyShape(Entity proxyShapeEntity) {
inline void CollisionDetection::updateProxyShape(Entity proxyShapeEntity, decimal timeStep) {
// Update the proxy-shape component
mBroadPhaseSystem.updateProxyShape(proxyShapeEntity);
mBroadPhaseSystem.updateProxyShape(proxyShapeEntity, timeStep);
}
// Update all the enabled proxy-shapes
inline void CollisionDetection::updateProxyShapes() {
mBroadPhaseSystem.updateProxyShapes();
inline void CollisionDetection::updateProxyShapes(decimal timeStep) {
mBroadPhaseSystem.updateProxyShapes(timeStep);
}
#ifdef IS_PROFILING_ACTIVE

2
src/collision/ContactManifold.h
View File

@ -124,7 +124,7 @@ class ContactManifold {
friend class Island;
friend class CollisionBody;
friend class ContactManifoldSet;
friend class ContactSolver;
friend class ContactSolverSystem;
friend class CollisionDetection;
};

2
src/collision/ProxyShape.cpp
View File

@ -122,7 +122,7 @@ void ProxyShape::setLocalToBodyTransform(const Transform& transform) {
mBody->mWorld.mRigidBodyComponents.setIsSleeping(mBody->getEntity(), false);
}
mBody->mWorld.mCollisionDetection.updateProxyShape(mEntity);
mBody->mWorld.mCollisionDetection.updateProxyShape(mEntity, 0);
int broadPhaseId = mBody->mWorld.mProxyShapesComponents.getBroadPhaseId(mEntity);

3
src/components/RigidBodyComponents.h
View File

@ -333,7 +333,8 @@ class RigidBodyComponents : public Components {
// -------------------- Friendship -------------------- //
friend class DynamicsWorld;
friend class ContactSolver;
friend class ContactSolverSystem;
friend class DynamicsSystem;
friend class BallAndSocketJoint;
friend class FixedJoint;
friend class HingeJoint;

2
src/constraint/BallAndSocketJoint.cpp
View File

@ -25,7 +25,7 @@
// Libraries
#include "BallAndSocketJoint.h"
#include "engine/ConstraintSolver.h"
#include "systems/ConstraintSolverSystem.h"
#include "components/RigidBodyComponents.h"
using namespace reactphysics3d;

2
src/constraint/ContactPoint.h
View File

@ -136,7 +136,7 @@ class ContactPoint {
// Friendship
friend class ContactManifold;
friend class ContactManifoldSet;
friend class ContactSolver;
friend class ContactSolverSystem;
friend class CollisionDetection;
};

2
src/constraint/FixedJoint.cpp
View File

@ -25,7 +25,7 @@
// Libraries
#include "FixedJoint.h"
#include "engine/ConstraintSolver.h"
#include "systems/ConstraintSolverSystem.h"
#include "components/RigidBodyComponents.h"
using namespace reactphysics3d;

2
src/constraint/HingeJoint.cpp
View File

@ -25,7 +25,7 @@
// Libraries
#include "HingeJoint.h"
#include "engine/ConstraintSolver.h"
#include "systems/ConstraintSolverSystem.h"
#include "components/RigidBodyComponents.h"
using namespace reactphysics3d;

2
src/constraint/Joint.h
View File

@ -213,7 +213,7 @@ class Joint {
friend class DynamicsWorld;
friend class Island;
friend class ConstraintSolver;
friend class ConstraintSolverSystem;
};
// Return the reference to the body 1

2
src/constraint/SliderJoint.cpp
View File

@ -25,7 +25,7 @@
// Libraries
#include "SliderJoint.h"
#include "engine/ConstraintSolver.h"
#include "systems/ConstraintSolverSystem.h"
#include "components/RigidBodyComponents.h"
using namespace reactphysics3d;

2
src/constraint/SliderJoint.h
View File

@ -34,7 +34,7 @@
namespace reactphysics3d {
// Declarations
class ConstraintSolver;
class ConstraintSolverSystem;
// Structure SliderJointInfo
/**

192
src/engine/DynamicsWorld.cpp
View File

@ -50,13 +50,14 @@ 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, mCollisionBodyComponents, mRigidBodyComponents,
mContactSolverSystem(mMemoryManager, mIslands, mCollisionBodyComponents, mRigidBodyComponents,
mProxyShapesComponents, mConfig),
mConstraintSolver(mIslands, mRigidBodyComponents),
mConstraintSolverSystem(mIslands, mRigidBodyComponents),
mDynamicsSystem(mRigidBodyComponents, mTransformComponents, mIsGravityEnabled, mGravity),
mNbVelocitySolverIterations(mConfig.defaultVelocitySolverNbIterations),
mNbPositionSolverIterations(mConfig.defaultPositionSolverNbIterations),
mIsSleepingEnabled(mConfig.isSleepingEnabled), mRigidBodies(mMemoryManager.getPoolAllocator()),
mJoints(mMemoryManager.getPoolAllocator()), mGravity(gravity), mTimeStep(decimal(1.0f / 60.0f)),
mJoints(mMemoryManager.getPoolAllocator()), mGravity(gravity),
mIsGravityEnabled(true), mSleepLinearVelocity(mConfig.defaultSleepLinearVelocity),
mSleepAngularVelocity(mConfig.defaultSleepAngularVelocity), mTimeBeforeSleep(mConfig.defaultTimeBeforeSleep),
mFreeJointsIDs(mMemoryManager.getPoolAllocator()), mCurrentJointId(0) {
@ -64,8 +65,9 @@ DynamicsWorld::DynamicsWorld(const Vector3& gravity, const WorldSettings& worldS
#ifdef IS_PROFILING_ACTIVE
// Set the profiler
mConstraintSolver.setProfiler(mProfiler);
mContactSolver.setProfiler(mProfiler);
mConstraintSolverSystem.setProfiler(mProfiler);
mContactSolverSystem.setProfiler(mProfiler);
mDynamicsSystem.setProfiler(mProfiler);
#endif
@ -113,8 +115,6 @@ void DynamicsWorld::update(decimal timeStep) {
RP3D_PROFILE("DynamicsWorld::update()", mProfiler);
mTimeStep = timeStep;
// Notify the event listener about the beginning of an internal tick
if (mEventListener != nullptr) mEventListener->beginInternalTick();
@ -131,27 +131,30 @@ void DynamicsWorld::update(decimal timeStep) {
mCollisionDetection.reportContacts();
// Integrate the velocities
integrateRigidBodiesVelocities();
mDynamicsSystem.integrateRigidBodiesVelocities(timeStep);
// Solve the contacts and constraints
solveContactsAndConstraints();
solveContactsAndConstraints(timeStep);
// Integrate the position and orientation of each body
integrateRigidBodiesPositions();
mDynamicsSystem.integrateRigidBodiesPositions(timeStep, mContactSolverSystem.isSplitImpulseActive());
// Solve the position correction for constraints
solvePositionCorrection();
// Update the state (positions and velocities) of the bodies
updateBodiesState();
mDynamicsSystem.updateBodiesState();
if (mIsSleepingEnabled) updateSleepingBodies();
// Update the proxy-shapes components
mCollisionDetection.updateProxyShapes(timeStep);
if (mIsSleepingEnabled) updateSleepingBodies(timeStep);
// Notify the event listener about the end of an internal tick
if (mEventListener != nullptr) mEventListener->endInternalTick();
// Reset the external force and torque applied to the bodies
resetBodiesForceAndTorque();
mDynamicsSystem.resetBodiesForceAndTorque();
// Reset the islands
mIslands.clear();
@ -160,161 +163,16 @@ void DynamicsWorld::update(decimal timeStep) {
mMemoryManager.resetFrameAllocator();
}
// Integrate position and orientation of the rigid bodies.
/// The positions and orientations of the bodies are integrated using
/// the sympletic Euler time stepping scheme.
void DynamicsWorld::integrateRigidBodiesPositions() {
RP3D_PROFILE("DynamicsWorld::integrateRigidBodiesPositions()", mProfiler);
const decimal isSplitImpulseActive = mContactSolver.isSplitImpulseActive() ? decimal(1.0) : decimal(0.0);
for (uint32 i=0; i < mRigidBodyComponents.getNbEnabledComponents(); i++) {
// Get the constrained velocity
Vector3 newLinVelocity = mRigidBodyComponents.mConstrainedLinearVelocities[i];
Vector3 newAngVelocity = mRigidBodyComponents.mConstrainedAngularVelocities[i];
// Add the split impulse velocity from Contact Solver (only used
// to update the position)
newLinVelocity += isSplitImpulseActive * mRigidBodyComponents.mSplitLinearVelocities[i];
newAngVelocity += isSplitImpulseActive * mRigidBodyComponents.mSplitAngularVelocities[i];
// Get current position and orientation of the body
const Vector3& currentPosition = mRigidBodyComponents.mCentersOfMassWorld[i];
const Quaternion& currentOrientation = mTransformComponents.getTransform(mRigidBodyComponents.mBodiesEntities[i]).getOrientation();
// Update the new constrained position and orientation of the body
mRigidBodyComponents.mConstrainedPositions[i] = currentPosition + newLinVelocity * mTimeStep;
mRigidBodyComponents.mConstrainedOrientations[i] = currentOrientation + Quaternion(0, newAngVelocity) *
currentOrientation * decimal(0.5) * mTimeStep;
}
}
// Update the postion/orientation of the bodies
void DynamicsWorld::updateBodiesState() {
RP3D_PROFILE("DynamicsWorld::updateBodiesState()", mProfiler);
// TODO : Make sure we compute this in a system
for (uint32 i=0; i < mRigidBodyComponents.getNbEnabledComponents(); i++) {
// Update the linear and angular velocity of the body
mRigidBodyComponents.setLinearVelocity(mRigidBodyComponents.mBodiesEntities[i], mRigidBodyComponents.mConstrainedLinearVelocities[i]);
mRigidBodyComponents.setAngularVelocity(mRigidBodyComponents.mBodiesEntities[i], mRigidBodyComponents.mConstrainedAngularVelocities[i]);
// Update the position of the center of mass of the body
mRigidBodyComponents.mCentersOfMassWorld[i] = mRigidBodyComponents.mConstrainedPositions[i];
// Update the orientation of the body
const Quaternion& constrainedOrientation = mRigidBodyComponents.mConstrainedOrientations[i];
mTransformComponents.getTransform(mRigidBodyComponents.mBodiesEntities[i]).setOrientation(constrainedOrientation.getUnit());
}
// Update the transform of the body (using the new center of mass and new orientation)
for (uint32 i=0; i < mRigidBodyComponents.getNbEnabledComponents(); i++) {
Transform& transform = mTransformComponents.getTransform(mRigidBodyComponents.mBodiesEntities[i]);
const Vector3& centerOfMassWorld = mRigidBodyComponents.mCentersOfMassWorld[i];
const Vector3& centerOfMassLocal = mRigidBodyComponents.mCentersOfMassLocal[i];
transform.setPosition(centerOfMassWorld - transform.getOrientation() * centerOfMassLocal);
}
// Update the world inverse inertia tensor of the body
for (uint32 i=0; i < mRigidBodyComponents.getNbEnabledComponents(); i++) {
Matrix3x3 orientation = mTransformComponents.getTransform(mRigidBodyComponents.mBodiesEntities[i]).getOrientation().getMatrix();
const Matrix3x3& inverseInertiaLocalTensor = mRigidBodyComponents.mInverseInertiaTensorsLocal[i];
mRigidBodyComponents.mInverseInertiaTensorsWorld[i] = orientation * inverseInertiaLocalTensor * orientation.getTranspose();
}
// Update the proxy-shapes components
mCollisionDetection.updateProxyShapes();
}
// Reset the split velocities of the bodies
void DynamicsWorld::resetSplitVelocities() {
for(uint32 i=0; i < mRigidBodyComponents.getNbEnabledComponents(); i++) {
mRigidBodyComponents.mSplitLinearVelocities[i].setToZero();
mRigidBodyComponents.mSplitAngularVelocities[i].setToZero();
}
}
// Integrate the velocities of rigid bodies.
/// This method only set the temporary velocities but does not update
/// the actual velocitiy of the bodies. The velocities updated in this method
/// might violate the constraints and will be corrected in the constraint and
/// contact solver.
void DynamicsWorld::integrateRigidBodiesVelocities() {
RP3D_PROFILE("DynamicsWorld::integrateRigidBodiesVelocities()", mProfiler);
// Reset the split velocities of the bodies
resetSplitVelocities();
// Integration component velocities using force/torque
for (uint32 i=0; i < mRigidBodyComponents.getNbEnabledComponents(); i++) {
assert(mRigidBodyComponents.mSplitLinearVelocities[i] == Vector3(0, 0, 0));
assert(mRigidBodyComponents.mSplitAngularVelocities[i] == Vector3(0, 0, 0));
const Vector3& linearVelocity = mRigidBodyComponents.mLinearVelocities[i];
const Vector3& angularVelocity = mRigidBodyComponents.mAngularVelocities[i];
// Integrate the external force to get the new velocity of the body
mRigidBodyComponents.mConstrainedLinearVelocities[i] = linearVelocity + mTimeStep *
mRigidBodyComponents.mInverseMasses[i] * mRigidBodyComponents.mExternalForces[i];
mRigidBodyComponents.mConstrainedAngularVelocities[i] = angularVelocity + mTimeStep *
mRigidBodyComponents.mInverseInertiaTensorsWorld[i] * mRigidBodyComponents.mExternalTorques[i];
}
// Apply gravity force
for (uint32 i=0; i < mRigidBodyComponents.getNbEnabledComponents(); i++) {
// If the gravity has to be applied to this rigid body
if (mRigidBodyComponents.mIsGravityEnabled[i] && mIsGravityEnabled) {
// Integrate the gravity force
mRigidBodyComponents.mConstrainedLinearVelocities[i] = mRigidBodyComponents.mConstrainedLinearVelocities[i] + mTimeStep *
mRigidBodyComponents.mInverseMasses[i] * mRigidBodyComponents.mInitMasses[i] * mGravity;
}
}
// Apply the velocity damping
// Damping force : F_c = -c' * v (c=damping factor)
// Equation : m * dv/dt = -c' * v
// => dv/dt = -c * v (with c=c'/m)
// => dv/dt + c * v = 0
// Solution : v(t) = v0 * e^(-c * t)
// => v(t + dt) = v0 * e^(-c(t + dt))
// = v0 * e^(-ct) * e^(-c * dt)
// = v(t) * e^(-c * dt)
// => v2 = v1 * e^(-c * dt)
// Using Taylor Serie for e^(-x) : e^x ~ 1 + x + x^2/2! + ...
// => e^(-x) ~ 1 - x
// => v2 = v1 * (1 - c * dt)
for (uint32 i=0; i < mRigidBodyComponents.getNbEnabledComponents(); i++) {
const decimal linDampingFactor = mRigidBodyComponents.mLinearDampings[i];
const decimal angDampingFactor = mRigidBodyComponents.mAngularDampings[i];
const decimal linearDamping = pow(decimal(1.0) - linDampingFactor, mTimeStep);
const decimal angularDamping = pow(decimal(1.0) - angDampingFactor, mTimeStep);
mRigidBodyComponents.mConstrainedLinearVelocities[i] = mRigidBodyComponents.mConstrainedLinearVelocities[i] * linearDamping;
mRigidBodyComponents.mConstrainedAngularVelocities[i] = mRigidBodyComponents.mConstrainedAngularVelocities[i] * angularDamping;
}
}
// Solve the contacts and constraints
void DynamicsWorld::solveContactsAndConstraints() {
void DynamicsWorld::solveContactsAndConstraints(decimal timeStep) {
RP3D_PROFILE("DynamicsWorld::solveContactsAndConstraints()", mProfiler);
// ---------- Solve velocity constraints for joints and contacts ---------- //
// Initialize the contact solver
mContactSolver.init(mCollisionDetection.mCurrentContactManifolds, mCollisionDetection.mCurrentContactPoints, mTimeStep);
mContactSolverSystem.init(mCollisionDetection.mCurrentContactManifolds, mCollisionDetection.mCurrentContactPoints, timeStep);
// For each island of the world
for (uint islandIndex = 0; islandIndex < mIslands.getNbIslands(); islandIndex++) {
@ -323,7 +181,7 @@ void DynamicsWorld::solveContactsAndConstraints() {
if (mIslands.joints[islandIndex].size() > 0) {
// Initialize the constraint solver
mConstraintSolver.initializeForIsland(mTimeStep, islandIndex);
mConstraintSolverSystem.initializeForIsland(timeStep, islandIndex);
}
}
@ -335,14 +193,14 @@ void DynamicsWorld::solveContactsAndConstraints() {
// Solve the constraints
if (mIslands.joints[islandIndex].size() > 0) {
mConstraintSolver.solveVelocityConstraints(islandIndex);
mConstraintSolverSystem.solveVelocityConstraints(islandIndex);
}
}
mContactSolver.solve();
mContactSolverSystem.solve();
}
mContactSolver.storeImpulses();
mContactSolverSystem.storeImpulses();
}
// Solve the position error correction of the constraints
@ -364,7 +222,7 @@ void DynamicsWorld::solvePositionCorrection() {
for (uint i=0; i<mNbPositionSolverIterations; i++) {
// Solve the position constraints
mConstraintSolver.solvePositionConstraints(islandIndex);
mConstraintSolverSystem.solvePositionConstraints(islandIndex);
}
}
}
@ -798,7 +656,7 @@ void DynamicsWorld::createIslands() {
// Put bodies to sleep if needed.
/// For each island, if all the bodies have been almost still for a long enough period of
/// time, we put all the bodies of the island to sleep.
void DynamicsWorld::updateSleepingBodies() {
void DynamicsWorld::updateSleepingBodies(decimal timeStep) {
RP3D_PROFILE("DynamicsWorld::updateSleepingBodies()", mProfiler);
@ -831,7 +689,7 @@ void DynamicsWorld::updateSleepingBodies() {
// Increase the sleep time
decimal sleepTime = mRigidBodyComponents.getSleepTime(bodyEntity);
mRigidBodyComponents.setSleepTime(bodyEntity, sleepTime + mTimeStep);
mRigidBodyComponents.setSleepTime(bodyEntity, sleepTime + timeStep);
sleepTime = mRigidBodyComponents.getSleepTime(bodyEntity);
if (sleepTime < minSleepTime) {
minSleepTime = sleepTime;

67
src/engine/DynamicsWorld.h
View File

@ -28,10 +28,11 @@
// Libraries
#include "CollisionWorld.h"
#include "ConstraintSolver.h"
#include "systems/ConstraintSolverSystem.h"
#include "configuration.h"
#include "utils/Logger.h"
#include "engine/ContactSolver.h"
#include "systems/ContactSolverSystem.h"
#include "systems/DynamicsSystem.h"
#include "engine/Islands.h"
/// Namespace ReactPhysics3D
@ -57,11 +58,14 @@ class DynamicsWorld : public CollisionWorld {
/// All the islands of bodies of the current frame
Islands mIslands;
/// Contact solver
ContactSolver mContactSolver;
/// Contact solver system
ContactSolverSystem mContactSolverSystem;
/// Constraint solver
ConstraintSolver mConstraintSolver;
/// Constraint solver system
ConstraintSolverSystem mConstraintSolverSystem;
/// Dynamics system
DynamicsSystem mDynamicsSystem;
/// Number of iterations for the velocity solver of the Sequential Impulses technique
uint mNbVelocitySolverIterations;
@ -81,9 +85,6 @@ class DynamicsWorld : public CollisionWorld {
/// Gravity vector of the world
Vector3 mGravity;
/// Current frame time step (in seconds)
decimal mTimeStep;
/// True if the gravity force is on
bool mIsGravityEnabled;
@ -105,20 +106,8 @@ class DynamicsWorld : public CollisionWorld {
// -------------------- Methods -------------------- //
/// Integrate the positions and orientations of rigid bodies.
void integrateRigidBodiesPositions();
/// Reset the external force and torque applied to the bodies
void resetBodiesForceAndTorque();
/// Reset the split velocities of the bodies
void resetSplitVelocities();
/// Integrate the velocities of rigid bodies.
void integrateRigidBodiesVelocities();
/// Solve the contacts and constraints
void solveContactsAndConstraints();
void solveContactsAndConstraints(decimal timeStep);
/// Solve the position error correction of the constraints
void solvePositionCorrection();
@ -129,11 +118,8 @@ class DynamicsWorld : public CollisionWorld {
/// Compute the islands using potential contacts and joints and create the actual contacts.
void createIslands();
/// Update the postion/orientation of the bodies
void updateBodiesState();
/// Put bodies to sleep if needed.
void updateSleepingBodies();
void updateSleepingBodies(decimal timeStep);
/// Add the joint to the list of joints of the two bodies involved in the joint
void addJointToBody(Joint* joint);
@ -197,9 +183,6 @@ class DynamicsWorld : public CollisionWorld {
/// Set the gravity vector of the world
void setGravity(Vector3& gravity);
/// Return the current time step
decimal getTimeStep() const;
/// Return if the gravity is on
bool isGravityEnabled() const;
@ -244,16 +227,6 @@ class DynamicsWorld : public CollisionWorld {
friend class RigidBody;
};
// Reset the external force and torque applied to the bodies
inline void DynamicsWorld::resetBodiesForceAndTorque() {
// For each body of the world
for (uint32 i=0; i < mRigidBodyComponents.getNbComponents(); i++) {
mRigidBodyComponents.mExternalForces[i].setToZero();
mRigidBodyComponents.mExternalTorques[i].setToZero();
}
}
// Get the number of iterations for the velocity constraint solver
/**
* @return The number of iterations of the velocity constraint solver
@ -299,10 +272,10 @@ inline void DynamicsWorld::setNbIterationsPositionSolver(uint nbIterations) {
inline void DynamicsWorld::setContactsPositionCorrectionTechnique(
ContactsPositionCorrectionTechnique technique) {
if (technique == ContactsPositionCorrectionTechnique::BAUMGARTE_CONTACTS) {
mContactSolver.setIsSplitImpulseActive(false);
mContactSolverSystem.setIsSplitImpulseActive(false);
}
else {
mContactSolver.setIsSplitImpulseActive(true);
mContactSolverSystem.setIsSplitImpulseActive(true);
}
}
@ -313,10 +286,10 @@ inline void DynamicsWorld::setContactsPositionCorrectionTechnique(
inline void DynamicsWorld::setJointsPositionCorrectionTechnique(
JointsPositionCorrectionTechnique technique) {
if (technique == JointsPositionCorrectionTechnique::BAUMGARTE_JOINTS) {
mConstraintSolver.setIsNonLinearGaussSeidelPositionCorrectionActive(false);
mConstraintSolverSystem.setIsNonLinearGaussSeidelPositionCorrectionActive(false);
}
else {
mConstraintSolver.setIsNonLinearGaussSeidelPositionCorrectionActive(true);
mConstraintSolverSystem.setIsNonLinearGaussSeidelPositionCorrectionActive(true);
}
}
@ -339,14 +312,6 @@ inline void DynamicsWorld::setGravity(Vector3& gravity) {
"Dynamics World: Set gravity vector to " + gravity.to_string());
}
// Return the current time step
/**
* @return The current time step (in seconds)
*/
inline decimal DynamicsWorld::getTimeStep() const {
return mTimeStep;
}
// Return if the gravity is enaled
/**
* @return True if the gravity is enabled in the world

17
src/systems/BroadPhaseSystem.cpp
View File

@ -109,7 +109,7 @@ void BroadPhaseSystem::removeProxyCollisionShape(ProxyShape* proxyShape) {
}
// Update the broad-phase state of a single proxy-shape
void BroadPhaseSystem::updateProxyShape(Entity proxyShapeEntity) {
void BroadPhaseSystem::updateProxyShape(Entity proxyShapeEntity, decimal timeStep) {
assert(mProxyShapesComponents.mMapEntityToComponentIndex.containsKey(proxyShapeEntity));
@ -117,14 +117,14 @@ void BroadPhaseSystem::updateProxyShape(Entity proxyShapeEntity) {
uint32 index = mProxyShapesComponents.mMapEntityToComponentIndex[proxyShapeEntity];
// Update the proxy-shape component
updateProxyShapesComponents(index, 1);
updateProxyShapesComponents(index, 1, timeStep);
}
// Update the broad-phase state of all the enabled proxy-shapes
void BroadPhaseSystem::updateProxyShapes() {
void BroadPhaseSystem::updateProxyShapes(decimal timeStep) {
// Update all the enabled proxy-shape components
updateProxyShapesComponents(0, mProxyShapesComponents.getNbEnabledComponents());
updateProxyShapesComponents(0, mProxyShapesComponents.getNbEnabledComponents(), timeStep);
}
// Notify the broad-phase that a collision shape has moved and need to be updated
@ -146,7 +146,7 @@ void BroadPhaseSystem::updateProxyShapeInternal(int broadPhaseId, const AABB& aa
}
// Update the broad-phase state of some proxy-shapes components
void BroadPhaseSystem::updateProxyShapesComponents(uint32 startIndex, uint32 nbItems) {
void BroadPhaseSystem::updateProxyShapesComponents(uint32 startIndex, uint32 nbItems, decimal timeStep) {
assert(nbItems > 0);
assert(startIndex < mProxyShapesComponents.getNbComponents());
@ -158,13 +158,6 @@ void BroadPhaseSystem::updateProxyShapesComponents(uint32 startIndex, uint32 nbI
nbItems = endIndex - startIndex;
assert(nbItems >= 0);
// Get the time step if we are in a dynamics world
DynamicsWorld* dynamicsWorld = dynamic_cast<DynamicsWorld*>(mCollisionDetection.getWorld());
decimal timeStep = decimal(0.0);
if (dynamicsWorld != nullptr) {
timeStep = dynamicsWorld->getTimeStep();
}
// For each proxy-shape component to update
for (uint32 i = startIndex; i < startIndex + nbItems; i++) {

8
src/systems/BroadPhaseSystem.h
View File

@ -97,7 +97,7 @@ class BroadPhaseRaycastCallback : public DynamicAABBTreeRaycastCallback {
};
// Class BroadPhaseAlgorithm
// Class BroadPhaseSystem
/**
* This class represents the broad-phase collision detection. The
* goal of the broad-phase collision detection is to compute the pairs of proxy shapes
@ -143,7 +143,7 @@ class BroadPhaseSystem {
void updateProxyShapeInternal(int broadPhaseId, const AABB& aabb, const Vector3& displacement);
/// Update the broad-phase state of some proxy-shapes components
void updateProxyShapesComponents(uint32 startIndex, uint32 nbItems);
void updateProxyShapesComponents(uint32 startIndex, uint32 nbItems, decimal timeStep);
public :
@ -169,10 +169,10 @@ class BroadPhaseSystem {
void removeProxyCollisionShape(ProxyShape* proxyShape);
/// Update the broad-phase state of a single proxy-shape
void updateProxyShape(Entity proxyShapeEntity);
void updateProxyShape(Entity proxyShapeEntity, decimal timeStep);
/// Update the broad-phase state of all the enabled proxy-shapes
void updateProxyShapes();
void updateProxyShapes(decimal timeStep);
/// Add a collision shape in the array of shapes that have moved in the last simulation step
/// and that need to be tested again for broad-phase overlapping.

10
src/engine/ConstraintSolver.cpp → src/systems/ConstraintSolverSystem.cpp
View File

@ -24,14 +24,14 @@
********************************************************************************/
// Libraries
#include "ConstraintSolver.h"
#include "systems/ConstraintSolverSystem.h"
#include "utils/Profiler.h"
#include "engine/Island.h"
using namespace reactphysics3d;
// Constructor
ConstraintSolver::ConstraintSolver(Islands& islands, RigidBodyComponents& rigidBodyComponents)
ConstraintSolverSystem::ConstraintSolverSystem(Islands& islands, RigidBodyComponents& rigidBodyComponents)
: mIsWarmStartingActive(true), mIslands(islands), mConstraintSolverData(rigidBodyComponents) {
#ifdef IS_PROFILING_ACTIVE
@ -43,7 +43,7 @@ ConstraintSolver::ConstraintSolver(Islands& islands, RigidBodyComponents& rigidB
}
// Initialize the constraint solver for a given island
void ConstraintSolver::initializeForIsland(decimal dt, uint islandIndex) {
void ConstraintSolverSystem::initializeForIsland(decimal dt, uint islandIndex) {
RP3D_PROFILE("ConstraintSolver::initializeForIsland()", mProfiler);
@ -71,7 +71,7 @@ void ConstraintSolver::initializeForIsland(decimal dt, uint islandIndex) {
}
// Solve the velocity constraints
void ConstraintSolver::solveVelocityConstraints(uint islandIndex) {
void ConstraintSolverSystem::solveVelocityConstraints(uint islandIndex) {
RP3D_PROFILE("ConstraintSolver::solveVelocityConstraints()", mProfiler);
@ -86,7 +86,7 @@ void ConstraintSolver::solveVelocityConstraints(uint islandIndex) {
}
// Solve the position constraints
void ConstraintSolver::solvePositionConstraints(uint islandIndex) {
void ConstraintSolverSystem::solvePositionConstraints(uint islandIndex) {
RP3D_PROFILE("ConstraintSolver::solvePositionConstraints()", mProfiler);

14
src/engine/ConstraintSolver.h → src/systems/ConstraintSolverSystem.h
View File

@ -23,8 +23,8 @@
* *
********************************************************************************/
#ifndef REACTPHYSICS3D_CONSTRAINT_SOLVER_H
#define REACTPHYSICS3D_CONSTRAINT_SOLVER_H
#ifndef REACTPHYSICS3D_CONSTRAINT_SOLVER_SYSTEM_H
#define REACTPHYSICS3D_CONSTRAINT_SOLVER_SYSTEM_H
// Libraries
#include "configuration.h"
@ -66,7 +66,7 @@ struct ConstraintSolverData {
};
// Class ConstraintSolver
// Class ConstraintSolverSystem
/**
* This class represents the constraint solver that is used to solve constraints between
* the rigid bodies. The constraint solver is based on the "Sequential Impulse" technique
@ -135,7 +135,7 @@ struct ConstraintSolverData {
* friction but also another twist friction constraint to prevent spin of the body around the
* contact manifold center.
*/
class ConstraintSolver {
class ConstraintSolverSystem {
private :
@ -164,10 +164,10 @@ class ConstraintSolver {
// -------------------- Methods -------------------- //
/// Constructor
ConstraintSolver(Islands& islands, RigidBodyComponents& rigidBodyComponents);
ConstraintSolverSystem(Islands& islands, RigidBodyComponents& rigidBodyComponents);
/// Destructor
~ConstraintSolver() = default;
~ConstraintSolverSystem() = default;
/// Initialize the constraint solver for a given island
void initializeForIsland(decimal dt, uint islandIndex);
@ -196,7 +196,7 @@ class ConstraintSolver {
#ifdef IS_PROFILING_ACTIVE
// Set the profiler
inline void ConstraintSolver::setProfiler(Profiler* profiler) {
inline void ConstraintSolverSystem::setProfiler(Profiler* profiler) {
mProfiler = profiler;
}

30
src/engine/ContactSolver.cpp → src/systems/ContactSolverSystem.cpp
View File

@ -24,8 +24,8 @@
********************************************************************************/
// Libraries
#include "ContactSolver.h"
#include "DynamicsWorld.h"
#include "systems/ContactSolverSystem.h"
#include "engine/DynamicsWorld.h"
#include "body/RigidBody.h"
#include "constraint/ContactPoint.h"
#include "utils/Profiler.h"
@ -38,12 +38,12 @@ using namespace reactphysics3d;
using namespace std;
// Constants initialization
const decimal ContactSolver::BETA = decimal(0.2);
const decimal ContactSolver::BETA_SPLIT_IMPULSE = decimal(0.2);
const decimal ContactSolver::SLOP = decimal(0.01);
const decimal ContactSolverSystem::BETA = decimal(0.2);
const decimal ContactSolverSystem::BETA_SPLIT_IMPULSE = decimal(0.2);
const decimal ContactSolverSystem::SLOP = decimal(0.01);
// Constructor
ContactSolver::ContactSolver(MemoryManager& memoryManager, Islands& islands, CollisionBodyComponents& bodyComponents,
ContactSolverSystem::ContactSolverSystem(MemoryManager& memoryManager, Islands& islands, CollisionBodyComponents& bodyComponents,
RigidBodyComponents& rigidBodyComponents, ProxyShapeComponents& proxyShapeComponents,
const WorldSettings& worldSettings)
:mMemoryManager(memoryManager), mContactConstraints(nullptr), mContactPoints(nullptr),
@ -59,7 +59,7 @@ ContactSolver::ContactSolver(MemoryManager& memoryManager, Islands& islands, Col
}
// Initialize the contact constraints
void ContactSolver::init(List<ContactManifold>* contactManifolds, List<ContactPoint>* contactPoints, decimal timeStep) {
void ContactSolverSystem::init(List<ContactManifold>* contactManifolds, List<ContactPoint>* contactPoints, decimal timeStep) {
mAllContactManifolds = contactManifolds;
mAllContactPoints = contactPoints;
@ -102,7 +102,7 @@ void ContactSolver::init(List<ContactManifold>* contactManifolds, List<ContactPo
}
// Initialize the constraint solver for a given island
void ContactSolver::initializeForIsland(uint islandIndex) {
void ContactSolverSystem::initializeForIsland(uint islandIndex) {
RP3D_PROFILE("ContactSolver::initializeForIsland()", mProfiler);
@ -332,7 +332,7 @@ void ContactSolver::initializeForIsland(uint islandIndex) {
/// For each constraint, we apply the previous impulse (from the previous step)
/// at the beginning. With this technique, we will converge faster towards
/// the solution of the linear system
void ContactSolver::warmStart() {
void ContactSolverSystem::warmStart() {
RP3D_PROFILE("ContactSolver::warmStart()", mProfiler);
@ -486,7 +486,7 @@ void ContactSolver::warmStart() {
}
// Solve the contacts
void ContactSolver::solve() {
void ContactSolverSystem::solve() {
RP3D_PROFILE("ContactSolver::solve()", mProfiler);
@ -764,7 +764,7 @@ void ContactSolver::solve() {
}
// Compute the collision restitution factor from the restitution factor of each body
decimal ContactSolver::computeMixedRestitutionFactor(RigidBody* body1,
decimal ContactSolverSystem::computeMixedRestitutionFactor(RigidBody* body1,
RigidBody* body2) const {
decimal restitution1 = body1->getMaterial().getBounciness();
decimal restitution2 = body2->getMaterial().getBounciness();
@ -774,7 +774,7 @@ decimal ContactSolver::computeMixedRestitutionFactor(RigidBody* body1,
}
// Compute the mixed friction coefficient from the friction coefficient of each body
decimal ContactSolver::computeMixedFrictionCoefficient(RigidBody *body1,
decimal ContactSolverSystem::computeMixedFrictionCoefficient(RigidBody *body1,
RigidBody *body2) const {
// Use the geometric mean to compute the mixed friction coefficient
return std::sqrt(body1->getMaterial().getFrictionCoefficient() *
@ -782,14 +782,14 @@ decimal ContactSolver::computeMixedFrictionCoefficient(RigidBody *body1,
}
// Compute th mixed rolling resistance factor between two bodies
inline decimal ContactSolver::computeMixedRollingResistance(RigidBody* body1,
inline decimal ContactSolverSystem::computeMixedRollingResistance(RigidBody* body1,
RigidBody* body2) const {
return decimal(0.5f) * (body1->getMaterial().getRollingResistance() + body2->getMaterial().getRollingResistance());
}
// Store the computed impulses to use them to
// warm start the solver at the next iteration
void ContactSolver::storeImpulses() {
void ContactSolverSystem::storeImpulses() {
RP3D_PROFILE("ContactSolver::storeImpulses()", mProfiler);
@ -816,7 +816,7 @@ void ContactSolver::storeImpulses() {
// Compute the two unit orthogonal vectors "t1" and "t2" that span the tangential friction plane
// for a contact manifold. The two vectors have to be such that : t1 x t2 = contactNormal.
void ContactSolver::computeFrictionVectors(const Vector3& deltaVelocity,
void ContactSolverSystem::computeFrictionVectors(const Vector3& deltaVelocity,
ContactManifoldSolver& contact) const {
RP3D_PROFILE("ContactSolver::computeFrictionVectors()", mProfiler);

20
src/engine/ContactSolver.h → src/systems/ContactSolverSystem.h
View File

@ -23,8 +23,8 @@
* *
********************************************************************************/
#ifndef REACTPHYSICS3D_CONTACT_SOLVER_H
#define REACTPHYSICS3D_CONTACT_SOLVER_H
#ifndef REACTPHYSICS3D_CONTACT_SOLVER_SYSTEM_H
#define REACTPHYSICS3D_CONTACT_SOLVER_SYSTEM_H
// Libraries
#include "configuration.h"
@ -48,9 +48,9 @@ class DynamicsComponents;
class RigidBodyComponents;
class ProxyShapeComponents;
// Class Contact Solver
// Class ContactSolverSystem
/**
* This class represents the contact solver that is used to solve rigid bodies contacts.
* This class represents the contact solver system that is used to solve rigid bodies contacts.
* The constraint solver is based on the "Sequential Impulse" technique described by
* Erin Catto in his GDC slides (http://code.google.com/p/box2d/downloads/list).
*
@ -117,7 +117,7 @@ class ProxyShapeComponents;
* friction but also another twist friction constraint to prevent spin of the body around the
* contact manifold center.
*/
class ContactSolver {
class ContactSolverSystem {
private:
@ -360,12 +360,12 @@ class ContactSolver {
// -------------------- Methods -------------------- //
/// Constructor
ContactSolver(MemoryManager& memoryManager, Islands& islands, CollisionBodyComponents& bodyComponents,
ContactSolverSystem(MemoryManager& memoryManager, Islands& islands, CollisionBodyComponents& bodyComponents,
RigidBodyComponents& rigidBodyComponents, ProxyShapeComponents& proxyShapeComponents,
const WorldSettings& worldSettings);
/// Destructor
~ContactSolver() = default;
~ContactSolverSystem() = default;
/// Initialize the contact constraints
void init(List<ContactManifold>* contactManifolds, List<ContactPoint>* contactPoints, decimal timeStep);
@ -395,19 +395,19 @@ class ContactSolver {
};
// Return true if the split impulses position correction technique is used for contacts
inline bool ContactSolver::isSplitImpulseActive() const {
inline bool ContactSolverSystem::isSplitImpulseActive() const {
return mIsSplitImpulseActive;
}
// Activate or Deactivate the split impulses for contacts
inline void ContactSolver::setIsSplitImpulseActive(bool isActive) {
inline void ContactSolverSystem::setIsSplitImpulseActive(bool isActive) {
mIsSplitImpulseActive = isActive;
}
#ifdef IS_PROFILING_ACTIVE
// Set the profiler
inline void ContactSolver::setProfiler(Profiler* profiler) {
inline void ContactSolverSystem::setProfiler(Profiler* profiler) {
mProfiler = profiler;
}

190
src/systems/DynamicsSystem.cpp Normal file
View File

@ -0,0 +1,190 @@
/********************************************************************************
* ReactPhysics3D physics library, http://www.reactphysics3d.com *
* Copyright (c) 2010-2018 Daniel Chappuis *
*********************************************************************************
* *
* This software is provided 'as-is', without any express or implied warranty. *
* In no event will the authors be held liable for any damages arising from the *
* use of this software. *
* *
* Permission is granted to anyone to use this software for any purpose, *
* including commercial applications, and to alter it and redistribute it *
* freely, subject to the following restrictions: *
* *
* 1. The origin of this software must not be misrepresented; you must not claim *
* that you wrote the original software. If you use this software in a *
* product, an acknowledgment in the product documentation would be *
* appreciated but is not required. *
* *
* 2. Altered source versions must be plainly marked as such, and must not be *
* misrepresented as being the original software. *
* *
* 3. This notice may not be removed or altered from any source distribution. *
* *
********************************************************************************/
// Libraries
#include "systems/DynamicsSystem.h"
using namespace reactphysics3d;
// Constructor
DynamicsSystem::DynamicsSystem(RigidBodyComponents& rigidBodyComponents, TransformComponents& transformComponents, bool& isGravityEnabled, Vector3& gravity)
:mRigidBodyComponents(rigidBodyComponents), mTransformComponents(transformComponents), mIsGravityEnabled(isGravityEnabled),
mGravity(gravity) {
}
// Integrate position and orientation of the rigid bodies.
/// The positions and orientations of the bodies are integrated using
/// the sympletic Euler time stepping scheme.
void DynamicsSystem::integrateRigidBodiesPositions(decimal timeStep, bool isSplitImpulseActive) {
RP3D_PROFILE("DynamicsSystem::integrateRigidBodiesPositions()", mProfiler);
const decimal isSplitImpulseFactor = isSplitImpulseActive ? decimal(1.0) : decimal(0.0);
for (uint32 i=0; i < mRigidBodyComponents.getNbEnabledComponents(); i++) {
// Get the constrained velocity
Vector3 newLinVelocity = mRigidBodyComponents.mConstrainedLinearVelocities[i];
Vector3 newAngVelocity = mRigidBodyComponents.mConstrainedAngularVelocities[i];
// Add the split impulse velocity from Contact Solver (only used
// to update the position)
newLinVelocity += isSplitImpulseFactor * mRigidBodyComponents.mSplitLinearVelocities[i];
newAngVelocity += isSplitImpulseFactor * mRigidBodyComponents.mSplitAngularVelocities[i];
// Get current position and orientation of the body
const Vector3& currentPosition = mRigidBodyComponents.mCentersOfMassWorld[i];
const Quaternion& currentOrientation = mTransformComponents.getTransform(mRigidBodyComponents.mBodiesEntities[i]).getOrientation();
// Update the new constrained position and orientation of the body
mRigidBodyComponents.mConstrainedPositions[i] = currentPosition + newLinVelocity * timeStep;
mRigidBodyComponents.mConstrainedOrientations[i] = currentOrientation + Quaternion(0, newAngVelocity) *
currentOrientation * decimal(0.5) * timeStep;
}
}
// Update the postion/orientation of the bodies
void DynamicsSystem::updateBodiesState() {
RP3D_PROFILE("DynamicsSystem::updateBodiesState()", mProfiler);
// TODO : Make sure we compute this in a system
for (uint32 i=0; i < mRigidBodyComponents.getNbEnabledComponents(); i++) {
// Update the linear and angular velocity of the body
mRigidBodyComponents.setLinearVelocity(mRigidBodyComponents.mBodiesEntities[i], mRigidBodyComponents.mConstrainedLinearVelocities[i]);
mRigidBodyComponents.setAngularVelocity(mRigidBodyComponents.mBodiesEntities[i], mRigidBodyComponents.mConstrainedAngularVelocities[i]);
// Update the position of the center of mass of the body
mRigidBodyComponents.mCentersOfMassWorld[i] = mRigidBodyComponents.mConstrainedPositions[i];
// Update the orientation of the body
const Quaternion& constrainedOrientation = mRigidBodyComponents.mConstrainedOrientations[i];
mTransformComponents.getTransform(mRigidBodyComponents.mBodiesEntities[i]).setOrientation(constrainedOrientation.getUnit());
}
// Update the transform of the body (using the new center of mass and new orientation)
for (uint32 i=0; i < mRigidBodyComponents.getNbEnabledComponents(); i++) {
Transform& transform = mTransformComponents.getTransform(mRigidBodyComponents.mBodiesEntities[i]);
const Vector3& centerOfMassWorld = mRigidBodyComponents.mCentersOfMassWorld[i];
const Vector3& centerOfMassLocal = mRigidBodyComponents.mCentersOfMassLocal[i];
transform.setPosition(centerOfMassWorld - transform.getOrientation() * centerOfMassLocal);
}
// Update the world inverse inertia tensor of the body
for (uint32 i=0; i < mRigidBodyComponents.getNbEnabledComponents(); i++) {
Matrix3x3 orientation = mTransformComponents.getTransform(mRigidBodyComponents.mBodiesEntities[i]).getOrientation().getMatrix();
const Matrix3x3& inverseInertiaLocalTensor = mRigidBodyComponents.mInverseInertiaTensorsLocal[i];
mRigidBodyComponents.mInverseInertiaTensorsWorld[i] = orientation * inverseInertiaLocalTensor * orientation.getTranspose();
}
}
// Integrate the velocities of rigid bodies.
/// This method only set the temporary velocities but does not update
/// the actual velocitiy of the bodies. The velocities updated in this method
/// might violate the constraints and will be corrected in the constraint and
/// contact solver.
void DynamicsSystem::integrateRigidBodiesVelocities(decimal timeStep) {
RP3D_PROFILE("DynamicsSystem::integrateRigidBodiesVelocities()", mProfiler);
// Reset the split velocities of the bodies
resetSplitVelocities();
// Integration component velocities using force/torque
for (uint32 i=0; i < mRigidBodyComponents.getNbEnabledComponents(); i++) {
assert(mRigidBodyComponents.mSplitLinearVelocities[i] == Vector3(0, 0, 0));
assert(mRigidBodyComponents.mSplitAngularVelocities[i] == Vector3(0, 0, 0));
const Vector3& linearVelocity = mRigidBodyComponents.mLinearVelocities[i];
const Vector3& angularVelocity = mRigidBodyComponents.mAngularVelocities[i];
// Integrate the external force to get the new velocity of the body
mRigidBodyComponents.mConstrainedLinearVelocities[i] = linearVelocity + timeStep *
mRigidBodyComponents.mInverseMasses[i] * mRigidBodyComponents.mExternalForces[i];
mRigidBodyComponents.mConstrainedAngularVelocities[i] = angularVelocity + timeStep *
mRigidBodyComponents.mInverseInertiaTensorsWorld[i] * mRigidBodyComponents.mExternalTorques[i];
}
// Apply gravity force
for (uint32 i=0; i < mRigidBodyComponents.getNbEnabledComponents(); i++) {
// If the gravity has to be applied to this rigid body
if (mRigidBodyComponents.mIsGravityEnabled[i] && mIsGravityEnabled) {
// Integrate the gravity force
mRigidBodyComponents.mConstrainedLinearVelocities[i] = mRigidBodyComponents.mConstrainedLinearVelocities[i] + timeStep *
mRigidBodyComponents.mInverseMasses[i] * mRigidBodyComponents.mInitMasses[i] * mGravity;
}
}
// Apply the velocity damping
// Damping force : F_c = -c' * v (c=damping factor)
// Equation : m * dv/dt = -c' * v
// => dv/dt = -c * v (with c=c'/m)
// => dv/dt + c * v = 0
// Solution : v(t) = v0 * e^(-c * t)
// => v(t + dt) = v0 * e^(-c(t + dt))
// = v0 * e^(-ct) * e^(-c * dt)
// = v(t) * e^(-c * dt)
// => v2 = v1 * e^(-c * dt)
// Using Taylor Serie for e^(-x) : e^x ~ 1 + x + x^2/2! + ...
// => e^(-x) ~ 1 - x
// => v2 = v1 * (1 - c * dt)
for (uint32 i=0; i < mRigidBodyComponents.getNbEnabledComponents(); i++) {
const decimal linDampingFactor = mRigidBodyComponents.mLinearDampings[i];
const decimal angDampingFactor = mRigidBodyComponents.mAngularDampings[i];
const decimal linearDamping = std::pow(decimal(1.0) - linDampingFactor, timeStep);
const decimal angularDamping = std::pow(decimal(1.0) - angDampingFactor, timeStep);
mRigidBodyComponents.mConstrainedLinearVelocities[i] = mRigidBodyComponents.mConstrainedLinearVelocities[i] * linearDamping;
mRigidBodyComponents.mConstrainedAngularVelocities[i] = mRigidBodyComponents.mConstrainedAngularVelocities[i] * angularDamping;
}
}
// Reset the external force and torque applied to the bodies
void DynamicsSystem::resetBodiesForceAndTorque() {
// For each body of the world
for (uint32 i=0; i < mRigidBodyComponents.getNbComponents(); i++) {
mRigidBodyComponents.mExternalForces[i].setToZero();
mRigidBodyComponents.mExternalTorques[i].setToZero();
}
}
// Reset the split velocities of the bodies
void DynamicsSystem::resetSplitVelocities() {
for(uint32 i=0; i < mRigidBodyComponents.getNbEnabledComponents(); i++) {
mRigidBodyComponents.mSplitLinearVelocities[i].setToZero();
mRigidBodyComponents.mSplitAngularVelocities[i].setToZero();
}
}

111
src/systems/DynamicsSystem.h Normal file
View File

@ -0,0 +1,111 @@
/********************************************************************************
* ReactPhysics3D physics library, http://www.reactphysics3d.com *
* Copyright (c) 2010-2018 Daniel Chappuis *
*********************************************************************************
* *
* This software is provided 'as-is', without any express or implied warranty. *
* In no event will the authors be held liable for any damages arising from the *
* use of this software. *
* *
* Permission is granted to anyone to use this software for any purpose, *
* including commercial applications, and to alter it and redistribute it *
* freely, subject to the following restrictions: *
* *
* 1. The origin of this software must not be misrepresented; you must not claim *
* that you wrote the original software. If you use this software in a *
* product, an acknowledgment in the product documentation would be *
* appreciated but is not required. *
* *
* 2. Altered source versions must be plainly marked as such, and must not be *
* misrepresented as being the original software. *
* *
* 3. This notice may not be removed or altered from any source distribution. *
* *
********************************************************************************/
#ifndef REACTPHYSICS3D_DYNAMICS_SYSTEM_H
#define REACTPHYSICS3D_DYNAMICS_SYSTEM_H
// Libraries
#include "utils/Profiler.h"
#include "components/RigidBodyComponents.h"
#include "components/TransformComponents.h"
namespace reactphysics3d {
// Class DynamicsSystem
/**
* This class is responsible to compute and update the dynamics of the bodies that are simulated
* using physics.
*/
class DynamicsSystem {
private :
// -------------------- Attributes -------------------- //
/// Reference to the rigid body components
RigidBodyComponents& mRigidBodyComponents;
/// Reference to the transform components
TransformComponents& mTransformComponents;
/// Reference to the variable to know if gravity is enabled in the world
bool& mIsGravityEnabled;
/// Reference to the world gravity vector
Vector3& mGravity;
#ifdef IS_PROFILING_ACTIVE
/// Pointer to the profiler
Profiler* mProfiler;
#endif
public :
// -------------------- Methods -------------------- //
/// Constructor
DynamicsSystem(RigidBodyComponents& rigidBodyComponents, TransformComponents& transformComponents,
bool& isGravityEnabled, Vector3& gravity);
/// Destructor
~DynamicsSystem() = default;
#ifdef IS_PROFILING_ACTIVE
/// Set the profiler
void setProfiler(Profiler* profiler);
#endif
/// Integrate the positions and orientations of rigid bodies.
void integrateRigidBodiesPositions(decimal timeStep, bool isSplitImpulseActive);
/// Integrate the velocities of rigid bodies.
void integrateRigidBodiesVelocities(decimal timeStep);
/// Update the postion/orientation of the bodies
void updateBodiesState();
/// Reset the external force and torque applied to the bodies
void resetBodiesForceAndTorque();
/// Reset the split velocities of the bodies
void resetSplitVelocities();
};
#ifdef IS_PROFILING_ACTIVE
// Set the profiler
inline void DynamicsSystem::setProfiler(Profiler* profiler) {
mProfiler = profiler;
}
#endif
}
#endif