atria-soft/ephysics
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

[DOC] Update the comments

Browse Source
This commit is contained in:
Edouard DUPIN 2017-06-16 23:05:25 +02:00
parent cd59604b4b
commit 6c3d49de92
16 changed files with 1036 additions and 1752 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

310
ephysics/engine/CollisionWorld.hpp
View File

@ -5,7 +5,6 @@
*/
#pragma once
// Libraries
#include <vector>
#include <set>
#include <list>
@ -21,194 +20,125 @@
#include <ephysics/memory/MemoryAllocator.hpp>
#include <ephysics/engine/EventListener.hpp>
/// Namespace ephysics
namespace ephysics {
// Declarations
class CollisionCallback;
// Class CollisionWorld
/**
* This class represent a world where it is possible to move bodies
* by hand and to test collision between each other. In this kind of
* world, the bodies movement is not computed using the laws of physics.
*/
class CollisionWorld {
protected :
// -------------------- Attributes -------------------- //
/// Reference to the collision detection
CollisionDetection m_collisionDetection;
/// All the bodies (rigid and soft) of the world
std::set<CollisionBody*> m_bodies;
/// Current body ID
bodyindex m_currentBodyID;
/// List of free ID for rigid bodies
std::vector<uint64_t> m_freeBodiesIDs;
/// Memory allocator
MemoryAllocator m_memoryAllocator;
/// Pointer to an event listener object
EventListener* m_eventListener;
// -------------------- Methods -------------------- //
/// Private copy-constructor
CollisionWorld(const CollisionWorld& world);
/// Private assignment operator
CollisionWorld& operator=(const CollisionWorld& world);
/// Return the next available body ID
bodyindex computeNextAvailableBodyID();
/// Reset all the contact manifolds linked list of each body
void resetContactManifoldListsOfBodies();
public :
// -------------------- Methods -------------------- //
/// Constructor
CollisionWorld();
/// Destructor
virtual ~CollisionWorld();
/// Return an iterator to the beginning of the bodies of the physics world
std::set<CollisionBody*>::iterator getBodiesBeginIterator();
/// Return an iterator to the end of the bodies of the physics world
std::set<CollisionBody*>::iterator getBodiesEndIterator();
/// Create a collision body
CollisionBody* createCollisionBody(const etk::Transform3D& transform);
/// Destroy a collision body
void destroyCollisionBody(CollisionBody* collisionBody);
/// Set the collision dispatch configuration
void setCollisionDispatch(CollisionDispatch* collisionDispatch);
/// Ray cast method
void raycast(const Ray& ray, RaycastCallback* raycastCallback,
unsigned short raycastWithCategoryMaskBits = 0xFFFF) const;
/// Test if the AABBs of two bodies overlap
bool testAABBOverlap(const CollisionBody* body1,
const CollisionBody* body2) const;
/// Test if the AABBs of two proxy shapes overlap
bool testAABBOverlap(const ProxyShape* shape1,
const ProxyShape* shape2) const;
/// Test and report collisions between a given shape and all the others
/// shapes of the world
virtual void testCollision(const ProxyShape* shape,
CollisionCallback* callback);
/// Test and report collisions between two given shapes
virtual void testCollision(const ProxyShape* shape1,
const ProxyShape* shape2,
CollisionCallback* callback);
/// Test and report collisions between a body and all the others bodies of the
/// world
virtual void testCollision(const CollisionBody* body,
CollisionCallback* callback);
/// Test and report collisions between two bodies
virtual void testCollision(const CollisionBody* body1,
const CollisionBody* body2,
CollisionCallback* callback);
/// Test and report collisions between all shapes of the world
virtual void testCollision(CollisionCallback* callback);
// -------------------- Friendship -------------------- //
friend class CollisionDetection;
friend class CollisionBody;
friend class RigidBody;
friend class ConvexMeshShape;
};
// Return an iterator to the beginning of the bodies of the physics world
/**
* @return An starting iterator to the set of bodies of the world
*/
inline std::set<CollisionBody*>::iterator CollisionWorld::getBodiesBeginIterator() {
return m_bodies.begin();
}
// Return an iterator to the end of the bodies of the physics world
/**
* @return An ending iterator to the set of bodies of the world
*/
inline std::set<CollisionBody*>::iterator CollisionWorld::getBodiesEndIterator() {
return m_bodies.end();
}
// Set the collision dispatch configuration
/// This can be used to replace default collision detection algorithms by your
/// custom algorithm for instance.
/**
* @param CollisionDispatch Pointer to a collision dispatch object describing
* which collision detection algorithm to use for two given collision shapes
*/
inline void CollisionWorld::setCollisionDispatch(CollisionDispatch* collisionDispatch) {
m_collisionDetection.setCollisionDispatch(collisionDispatch);
}
// Ray cast method
/**
* @param ray Ray to use for raycasting
* @param raycastCallback Pointer to the class with the callback method
* @param raycastWithCategoryMaskBits Bits mask corresponding to the category of
* bodies to be raycasted
*/
inline void CollisionWorld::raycast(const Ray& ray,
RaycastCallback* raycastCallback,
unsigned short raycastWithCategoryMaskBits) const {
m_collisionDetection.raycast(raycastCallback, ray, raycastWithCategoryMaskBits);
}
// Test if the AABBs of two proxy shapes overlap
/**
* @param shape1 Pointer to the first proxy shape to test
* @param shape2 Pointer to the second proxy shape to test
* @return
*/
inline bool CollisionWorld::testAABBOverlap(const ProxyShape* shape1,
const ProxyShape* shape2) const {
return m_collisionDetection.testAABBOverlap(shape1, shape2);
}
// Class CollisionCallback
/**
* This class can be used to register a callback for collision test queries.
* You should implement your own class inherited from this one and implement
* the notifyRaycastHit() method. This method will be called for each ProxyShape
* that is hit by the ray.
*/
class CollisionCallback {
public:
/// Destructor
virtual ~CollisionCallback() {
}
/// This method will be called for contact
virtual void notifyContact(const ContactPointInfo& contactPointInfo)=0;
};
class CollisionCallback;
/**
* @brief This class represent a world where it is possible to move bodies
* by hand and to test collision between each other. In this kind of
* world, the bodies movement is not computed using the laws of physics.
*/
class CollisionWorld {
protected :
CollisionDetection m_collisionDetection; //!< Reference to the collision detection
std::set<CollisionBody*> m_bodies; //!< All the bodies (rigid and soft) of the world
bodyindex m_currentBodyID; //!< Current body ID
std::vector<uint64_t> m_freeBodiesIDs; //!< List of free ID for rigid bodies
MemoryAllocator m_memoryAllocator; //!< Memory allocator
EventListener* m_eventListener; //!< Pointer to an event listener object
/// Private copy-constructor
CollisionWorld(const CollisionWorld& world);
/// Private assignment operator
CollisionWorld& operator=(const CollisionWorld& world);
/// Return the next available body ID
bodyindex computeNextAvailableBodyID();
/// Reset all the contact manifolds linked list of each body
void resetContactManifoldListsOfBodies();
public :
/// Constructor
CollisionWorld();
/// Destructor
virtual ~CollisionWorld();
/**
* @brief Get an iterator to the beginning of the bodies of the physics world
* @return An starting iterator to the set of bodies of the world
*/
std::set<CollisionBody*>::iterator getBodiesBeginIterator() {
return m_bodies.begin();
}
/**
* @brief Get an iterator to the end of the bodies of the physics world
* @return An ending iterator to the set of bodies of the world
*/
std::set<CollisionBody*>::iterator getBodiesEndIterator() {
return m_bodies.end();
}
/// Create a collision body
CollisionBody* createCollisionBody(const etk::Transform3D& transform);
/// Destroy a collision body
void destroyCollisionBody(CollisionBody* collisionBody);
/**
* @brief Set the collision dispatch configuration
* This can be used to replace default collision detection algorithms by your
* custom algorithm for instance.
* @param[in] _CollisionDispatch Pointer to a collision dispatch object describing
* which collision detection algorithm to use for two given collision shapes
*/
void setCollisionDispatch(CollisionDispatch* _collisionDispatch) {
m_collisionDetection.setCollisionDispatch(_collisionDispatch);
}
/**
* @brief Ray cast method
* @param _ray Ray to use for raycasting
* @param _raycastCallback Pointer to the class with the callback method
* @param _raycastWithCategoryMaskBits Bits mask corresponding to the category of bodies to be raycasted
*/
void raycast(const Ray& _ray,
RaycastCallback* _raycastCallback,
unsigned short _raycastWithCategoryMaskBitss = 0xFFFF) const {
m_collisionDetection.raycast(_raycastCallback, _ray, _raycastWithCategoryMaskBits);
}
/// Test if the AABBs of two bodies overlap
bool testAABBOverlap(const CollisionBody* body1,
const CollisionBody* body2) const;
/**
* @brief Test if the AABBs of two proxy shapes overlap
* @param _shape1 Pointer to the first proxy shape to test
* @param _shape2 Pointer to the second proxy shape to test
*/
bool testAABBOverlap(const ProxyShape* _shape1,
const ProxyShape* _shape2) const {
return m_collisionDetection.testAABBOverlap(shape1, shape2);
}
/// Test and report collisions between a given shape and all the others
/// shapes of the world
virtual void testCollision(const ProxyShape* shape,
CollisionCallback* callback);
/// Test and report collisions between two given shapes
virtual void testCollision(const ProxyShape* shape1,
const ProxyShape* shape2,
CollisionCallback* callback);
/// Test and report collisions between a body and all the others bodies of the
/// world
virtual void testCollision(const CollisionBody* body,
CollisionCallback* callback);
/// Test and report collisions between two bodies
virtual void testCollision(const CollisionBody* body1,
const CollisionBody* body2,
CollisionCallback* callback);
/// Test and report collisions between all shapes of the world
virtual void testCollision(CollisionCallback* callback);
friend class CollisionDetection;
friend class CollisionBody;
friend class RigidBody;
friend class ConvexMeshShape;
};
/**
* @brief This class can be used to register a callback for collision test queries.
* You should implement your own class inherited from this one and implement
* the notifyRaycastHit() method. This method will be called for each ProxyShape
* that is hit by the ray.
*/
class CollisionCallback {
public:
/**
* @brief Virtualisation of the destructor.
*/
virtual ~CollisionCallback() = default;
/**
* @brief This method will be called for contact.
* @param[in] _contactPointInfo Contact information property.
*/
virtual void notifyContact(const ContactPointInfo& _contactPointInfo)=0;
};
}

12
ephysics/engine/ConstraintSolver.cpp
View File

@ -4,7 +4,6 @@
* @license BSD 3 clauses (see license file)
*/
// Libraries
#include <ephysics/engine/ConstraintSolver.hpp>
#include <ephysics/engine/Profiler.hpp>
@ -13,12 +12,7 @@ using namespace ephysics;
// Constructor
ConstraintSolver::ConstraintSolver(const std::map<RigidBody*, uint32_t>& mapBodyToVelocityIndex)
: m_mapBodyToConstrainedVelocityIndex(mapBodyToVelocityIndex),
mIsWarmStartingActive(true), m_constraintSolverData(mapBodyToVelocityIndex) {
}
// Destructor
ConstraintSolver::~ConstraintSolver() {
m_isWarmStartingActive(true), m_constraintSolverData(mapBodyToVelocityIndex) {
}
@ -36,7 +30,7 @@ void ConstraintSolver::initializeForIsland(float dt, Island* island) {
// Initialize the constraint solver data used to initialize and solve the constraints
m_constraintSolverData.timeStep = m_timeStep;
m_constraintSolverData.isWarmStartingActive = mIsWarmStartingActive;
m_constraintSolverData.isWarmStartingActive = m_isWarmStartingActive;
// For each joint of the island
Joint** joints = island->getJoints();
@ -46,7 +40,7 @@ void ConstraintSolver::initializeForIsland(float dt, Island* island) {
joints[i]->initBeforeSolve(m_constraintSolverData);
// Warm-start the constraint if warm-starting is enabled
if (mIsWarmStartingActive) {
if (m_isWarmStartingActive) {
joints[i]->warmstart(m_constraintSolverData);
}
}

280
ephysics/engine/ConstraintSolver.hpp
View File

@ -5,7 +5,6 @@
*/
#pragma once
// Libraries
#include <ephysics/configuration.hpp>
#include <ephysics/mathematics/mathematics.hpp>
#include <ephysics/constraint/Joint.hpp>
@ -14,169 +13,122 @@
#include <set>
namespace ephysics {
// Structure ConstraintSolverData
/**
* This structure contains data from the constraint solver that are used to solve
* each joint constraint.
*/
struct ConstraintSolverData {
public :
/// Current time step of the simulation
float timeStep;
/// Array with the bodies linear velocities
vec3* linearVelocities;
/// Array with the bodies angular velocities
vec3* angularVelocities;
/// Reference to the bodies positions
vec3* positions;
/// Reference to the bodies orientations
etk::Quaternion* orientations;
/// Reference to the map that associates rigid body to their index
/// in the constrained velocities array
const std::map<RigidBody*, uint32_t>& mapBodyToConstrainedVelocityIndex;
/// True if warm starting of the solver is active
bool isWarmStartingActive;
/// Constructor
ConstraintSolverData(const std::map<RigidBody*, uint32_t>& refMapBodyToConstrainedVelocityIndex)
:linearVelocities(NULL), angularVelocities(NULL),
positions(NULL), orientations(NULL),
mapBodyToConstrainedVelocityIndex(refMapBodyToConstrainedVelocityIndex){
}
};
// Class ConstraintSolver
/**
* 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
* described by Erin Catto in his GDC slides (http://code.google.com/p/box2d/downloads/list).
*
* A constraint between two bodies is represented by a function C(x) which is equal to zero
* when the constraint is satisfied. The condition C(x)=0 describes a valid position and the
* condition dC(x)/dt=0 describes a valid velocity. We have dC(x)/dt = Jv + b = 0 where J is
* the Jacobian matrix of the constraint, v is a vector that contains the velocity of both
* bodies and b is the constraint bias. We are looking for a force F_c that will act on the
* bodies to keep the constraint satisfied. Note that from the virtual work principle, we have
* F_c = J^t * lambda where J^t is the transpose of the Jacobian matrix and lambda is a
* Lagrange multiplier. Therefore, finding the force F_c is equivalent to finding the Lagrange
* multiplier lambda.
* An impulse P = F * dt where F is a force and dt is the timestep. We can apply impulses a
* body to change its velocity. The idea of the Sequential Impulse technique is to apply
* impulses to bodies of each constraints in order to keep the constraint satisfied.
*
* --- Step 1 ---
*
* First, we int32_tegrate the applied force F_a acting of each rigid body (like gravity, ...) and
* we obtain some new velocities v2' that tends to violate the constraints.
*
* v2' = v1 + dt * M^-1 * F_a
*
* where M is a matrix that contains mass and inertia tensor information.
*
* --- Step 2 ---
*
* During the second step, we iterate over all the constraints for a certain number of
* iterations and for each constraint we compute the impulse to apply to the bodies needed
* so that the new velocity of the bodies satisfy Jv + b = 0. From the Newton law, we know that
* M * deltaV = P_c where M is the mass of the body, deltaV is the difference of velocity and
* P_c is the constraint impulse to apply to the body. Therefore, we have
* v2 = v2' + M^-1 * P_c. For each constraint, we can compute the Lagrange multiplier lambda
* using : lambda = -m_c (Jv2' + b) where m_c = 1 / (J * M^-1 * J^t). Now that we have the
* Lagrange multiplier lambda, we can compute the impulse P_c = J^t * lambda * dt to apply to
* the bodies to satisfy the constraint.
*
* --- Step 3 ---
*
* In the third step, we int32_tegrate the new position x2 of the bodies using the new velocities
* v2 computed in the second step with : x2 = x1 + dt * v2.
*
* Note that in the following code (as it is also explained in the slides from Erin Catto),
* the value lambda is not only the lagrange multiplier but is the multiplication of the
* Lagrange multiplier with the timestep dt. Therefore, in the following code, when we use
* lambda, we mean (lambda * dt).
*
* We are using the accumulated impulse technique that is also described in the slides from
* Erin Catto.
*
* We are also using warm starting. The idea is to warm start the solver at the beginning of
* each step by applying the last impulstes for the constraints that we already existing at the
* previous step. This allows the iterative solver to converge faster towards the solution.
*
* For contact constraints, we are also using split impulses so that the position correction
* that uses Baumgarte stabilization does not change the momentum of the bodies.
*
* There are two ways to apply the friction constraints. Either the friction constraints are
* applied at each contact point or they are applied only at the center of the contact manifold
* between two bodies. If we solve the friction constraints at each contact point, we need
* two constraints (two tangential friction directions) and if we solve the friction
* constraints at the center of the contact manifold, we need two constraints for tangential
* friction but also another twist friction constraint to prevent spin of the body around the
* contact manifold center.
*/
class ConstraintSolver {
private :
// -------------------- Attributes -------------------- //
/// Reference to the map that associates rigid body to their index in
/// the constrained velocities array
const std::map<RigidBody*, uint32_t>& m_mapBodyToConstrainedVelocityIndex;
/// Current time step
float m_timeStep;
/// True if the warm starting of the solver is active
bool mIsWarmStartingActive;
/// Constraint solver data used to initialize and solve the constraints
ConstraintSolverData m_constraintSolverData;
public :
// -------------------- Methods -------------------- //
/// Constructor
ConstraintSolver(const std::map<RigidBody*, uint32_t>& mapBodyToVelocityIndex);
/// Destructor
~ConstraintSolver();
/// Initialize the constraint solver for a given island
void initializeForIsland(float dt, Island* island);
/// Solve the constraints
void solveVelocityConstraints(Island* island);
/// Solve the position constraints
void solvePositionConstraints(Island* island);
/// Return true if the Non-Linear-Gauss-Seidel position correction technique is active
bool getIsNonLinearGaussSeidelPositionCorrectionActive() const;
/// Enable/Disable the Non-Linear-Gauss-Seidel position correction technique.
void setIsNonLinearGaussSeidelPositionCorrectionActive(bool isActive);
/// Set the constrained velocities arrays
void setConstrainedVelocitiesArrays(vec3* constrainedLinearVelocities,
vec3* constrainedAngularVelocities);
/// Set the constrained positions/orientations arrays
void setConstrainedPositionsArrays(vec3* constrainedPositions,
etk::Quaternion* constrainedOrientations);
};
/**
* This structure contains data from the constraint solver that are used to solve
* each joint constraint.
*/
struct ConstraintSolverData {
public :
float timeStep; //!< Current time step of the simulation
vec3* linearVelocities; //!< Array with the bodies linear velocities
vec3* angularVelocities; //!< Array with the bodies angular velocities
vec3* positions; //!< Reference to the bodies positions
etk::Quaternion* orientations; //!< Reference to the bodies orientations
const std::map<RigidBody*, uint32_t>& mapBodyToConstrainedVelocityIndex; //!< Reference to the map that associates rigid body to their index in the constrained velocities array
bool isWarmStartingActive; //!< True if warm starting of the solver is active
/// Constructor
ConstraintSolverData(const std::map<RigidBody*, uint32_t>& refMapBodyToConstrainedVelocityIndex)
:linearVelocities(NULL), angularVelocities(NULL),
positions(NULL), orientations(NULL),
mapBodyToConstrainedVelocityIndex(refMapBodyToConstrainedVelocityIndex){
}
};
/**
* @brief 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
* described by Erin Catto in his GDC slides (http://code.google.com/p/box2d/downloads/list).
*
* A constraint between two bodies is represented by a function C(x) which is equal to zero
* when the constraint is satisfied. The condition C(x)=0 describes a valid position and the
* condition dC(x)/dt=0 describes a valid velocity. We have dC(x)/dt = Jv + b = 0 where J is
* the Jacobian matrix of the constraint, v is a vector that contains the velocity of both
* bodies and b is the constraint bias. We are looking for a force F_c that will act on the
* bodies to keep the constraint satisfied. Note that from the virtual work principle, we have
* F_c = J^t * lambda where J^t is the transpose of the Jacobian matrix and lambda is a
* Lagrange multiplier. Therefore, finding the force F_c is equivalent to finding the Lagrange
* multiplier lambda.
* An impulse P = F * dt where F is a force and dt is the timestep. We can apply impulses a
* body to change its velocity. The idea of the Sequential Impulse technique is to apply
* impulses to bodies of each constraints in order to keep the constraint satisfied.
*
* --- Step 1 ---
*
* First, we int32_tegrate the applied force F_a acting of each rigid body (like gravity, ...) and
* we obtain some new velocities v2' that tends to violate the constraints.
*
* v2' = v1 + dt * M^-1 * F_a
*
* where M is a matrix that contains mass and inertia tensor information.
*
* --- Step 2 ---
*
* During the second step, we iterate over all the constraints for a certain number of
* iterations and for each constraint we compute the impulse to apply to the bodies needed
* so that the new velocity of the bodies satisfy Jv + b = 0. From the Newton law, we know that
* M * deltaV = P_c where M is the mass of the body, deltaV is the difference of velocity and
* P_c is the constraint impulse to apply to the body. Therefore, we have
* v2 = v2' + M^-1 * P_c. For each constraint, we can compute the Lagrange multiplier lambda
* using : lambda = -m_c (Jv2' + b) where m_c = 1 / (J * M^-1 * J^t). Now that we have the
* Lagrange multiplier lambda, we can compute the impulse P_c = J^t * lambda * dt to apply to
* the bodies to satisfy the constraint.
*
* --- Step 3 ---
*
* In the third step, we int32_tegrate the new position x2 of the bodies using the new velocities
* v2 computed in the second step with : x2 = x1 + dt * v2.
*
* Note that in the following code (as it is also explained in the slides from Erin Catto),
* the value lambda is not only the lagrange multiplier but is the multiplication of the
* Lagrange multiplier with the timestep dt. Therefore, in the following code, when we use
* lambda, we mean (lambda * dt).
*
* We are using the accumulated impulse technique that is also described in the slides from
* Erin Catto.
*
* We are also using warm starting. The idea is to warm start the solver at the beginning of
* each step by applying the last impulstes for the constraints that we already existing at the
* previous step. This allows the iterative solver to converge faster towards the solution.
*
* For contact constraints, we are also using split impulses so that the position correction
* that uses Baumgarte stabilization does not change the momentum of the bodies.
*
* There are two ways to apply the friction constraints. Either the friction constraints are
* applied at each contact point or they are applied only at the center of the contact manifold
* between two bodies. If we solve the friction constraints at each contact point, we need
* two constraints (two tangential friction directions) and if we solve the friction
* constraints at the center of the contact manifold, we need two constraints for tangential
* friction but also another twist friction constraint to prevent spin of the body around the
* contact manifold center.
*/
class ConstraintSolver {
private :
const std::map<RigidBody*, uint32_t>& m_mapBodyToConstrainedVelocityIndex; //!< Reference to the map that associates rigid body to their index in the constrained velocities array
float m_timeStep; //!< Current time step
bool m_isWarmStartingActive; //!< True if the warm starting of the solver is active
ConstraintSolverData m_constraintSolverData; //!< Constraint solver data used to initialize and solve the constraints
public :
/// Constructor
ConstraintSolver(const std::map<RigidBody*, uint32_t>& mapBodyToVelocityIndex);
/// Initialize the constraint solver for a given island
void initializeForIsland(float dt, Island* island);
/// Solve the constraints
void solveVelocityConstraints(Island* island);
/// Solve the position constraints
void solvePositionConstraints(Island* island);
/// Return true if the Non-Linear-Gauss-Seidel position correction technique is active
bool getIsNonLinearGaussSeidelPositionCorrectionActive() const;
/// Enable/Disable the Non-Linear-Gauss-Seidel position correction technique.
void setIsNonLinearGaussSeidelPositionCorrectionActive(bool isActive);
/// Set the constrained velocities arrays
void setConstrainedVelocitiesArrays(vec3* constrainedLinearVelocities,
vec3* constrainedAngularVelocities);
/// Set the constrained positions/orientations arrays
void setConstrainedPositionsArrays(vec3* constrainedPositions,
etk::Quaternion* constrainedOrientations);
};
// Set the constrained velocities arrays
inline void ConstraintSolver::setConstrainedVelocitiesArrays(vec3* constrainedLinearVelocities,

84
ephysics/engine/ContactSolver.cpp
View File

@ -22,10 +22,10 @@ const float ContactSolver::SLOP= float(0.01);
// Constructor
ContactSolver::ContactSolver(const std::map<RigidBody*, uint32_t>& mapBodyToVelocityIndex)
:m_splitLinearVelocities(nullptr), m_splitAngularVelocities(nullptr),
mContactConstraints(nullptr), mLinearVelocities(nullptr), mAngularVelocities(nullptr),
m_contactConstraints(nullptr), m_linearVelocities(nullptr), m_angularVelocities(nullptr),
m_mapBodyToConstrainedVelocityIndex(mapBodyToVelocityIndex),
mIsWarmStartingActive(true), mIsSplitImpulseActive(true),
mIsSolveFrictionAtContactManifoldCenterActive(true) {
m_isWarmStartingActive(true), m_isSplitImpulseActive(true),
m_isSolveFrictionAtContactManifoldCenterActive(true) {
}
@ -50,8 +50,8 @@ void ContactSolver::initializeForIsland(float dt, Island* island) {
m_numberContactManifolds = island->getNbContactManifolds();
mContactConstraints = new ContactManifoldSolver[m_numberContactManifolds];
assert(mContactConstraints != nullptr);
m_contactConstraints = new ContactManifoldSolver[m_numberContactManifolds];
assert(m_contactConstraints != nullptr);
// For each contact manifold of the island
ContactManifold** contactManifolds = island->getContactManifold();
@ -59,7 +59,7 @@ void ContactSolver::initializeForIsland(float dt, Island* island) {
ContactManifold* externalManifold = contactManifolds[i];
ContactManifoldSolver& int32_ternalManifold = mContactConstraints[i];
ContactManifoldSolver& int32_ternalManifold = m_contactConstraints[i];
assert(externalManifold->getNbContactPoints() > 0);
@ -90,7 +90,7 @@ void ContactSolver::initializeForIsland(float dt, Island* island) {
int32_ternalManifold.isBody2DynamicType = body2->getType() == DYNAMIC;
// If we solve the friction constraints at the center of the contact manifold
if (mIsSolveFrictionAtContactManifoldCenterActive) {
if (m_isSolveFrictionAtContactManifoldCenterActive) {
int32_ternalManifold.frictionPointBody1 = vec3(0.0f,0.0f,0.0f);
int32_ternalManifold.frictionPointBody2 = vec3(0.0f,0.0f,0.0f);
}
@ -122,14 +122,14 @@ void ContactSolver::initializeForIsland(float dt, Island* island) {
contactPoint.rollingResistanceImpulse = vec3(0.0f,0.0f,0.0f);
// If we solve the friction constraints at the center of the contact manifold
if (mIsSolveFrictionAtContactManifoldCenterActive) {
if (m_isSolveFrictionAtContactManifoldCenterActive) {
int32_ternalManifold.frictionPointBody1 += p1;
int32_ternalManifold.frictionPointBody2 += p2;
}
}
// If we solve the friction constraints at the center of the contact manifold
if (mIsSolveFrictionAtContactManifoldCenterActive) {
if (m_isSolveFrictionAtContactManifoldCenterActive) {
int32_ternalManifold.frictionPointBody1 /=static_cast<float>(int32_ternalManifold.nbContacts);
int32_ternalManifold.frictionPointBody2 /=static_cast<float>(int32_ternalManifold.nbContacts);
@ -139,7 +139,7 @@ void ContactSolver::initializeForIsland(float dt, Island* island) {
int32_ternalManifold.oldFrictionvec2 = externalManifold->getFrictionvec2();
// If warm starting is active
if (mIsWarmStartingActive) {
if (m_isWarmStartingActive) {
// Initialize the accumulated impulses with the previous step accumulated impulses
int32_ternalManifold.friction1Impulse = externalManifold->getFrictionImpulse1();
@ -167,22 +167,22 @@ void ContactSolver::initializeContactConstraints() {
// For each contact constraint
for (uint32_t c=0; c<m_numberContactManifolds; c++) {
ContactManifoldSolver& manifold = mContactConstraints[c];
ContactManifoldSolver& manifold = m_contactConstraints[c];
// Get the inertia tensors of both bodies
etk::Matrix3x3& I1 = manifold.inverseInertiaTensorBody1;
etk::Matrix3x3& I2 = manifold.inverseInertiaTensorBody2;
// If we solve the friction constraints at the center of the contact manifold
if (mIsSolveFrictionAtContactManifoldCenterActive) {
if (m_isSolveFrictionAtContactManifoldCenterActive) {
manifold.normal = vec3(0.0, 0.0, 0.0);
}
// Get the velocities of the bodies
const vec3& v1 = mLinearVelocities[manifold.indexBody1];
const vec3& w1 = mAngularVelocities[manifold.indexBody1];
const vec3& v2 = mLinearVelocities[manifold.indexBody2];
const vec3& w2 = mAngularVelocities[manifold.indexBody2];
const vec3& v1 = m_linearVelocities[manifold.indexBody1];
const vec3& w1 = m_angularVelocities[manifold.indexBody1];
const vec3& v2 = m_linearVelocities[manifold.indexBody2];
const vec3& w2 = m_angularVelocities[manifold.indexBody2];
// For each contact point constraint
for (uint32_t i=0; i<manifold.nbContacts; i++) {
@ -205,7 +205,7 @@ void ContactSolver::initializeContactConstraints() {
0.0f;
// If we do not solve the friction constraints at the center of the contact manifold
if (!mIsSolveFrictionAtContactManifoldCenterActive) {
if (!m_isSolveFrictionAtContactManifoldCenterActive) {
// Compute the friction vectors
computeFrictionVectors(deltaV, contactPoint);
@ -246,7 +246,7 @@ void ContactSolver::initializeContactConstraints() {
}
// If the warm starting of the contact solver is active
if (mIsWarmStartingActive) {
if (m_isWarmStartingActive) {
// Get the cached accumulated impulses from the previous step
contactPoint.penetrationImpulse = externalContact->getPenetrationImpulse();
@ -259,7 +259,7 @@ void ContactSolver::initializeContactConstraints() {
contactPoint.penetrationSplitImpulse = 0.0;
// If we solve the friction constraints at the center of the contact manifold
if (mIsSolveFrictionAtContactManifoldCenterActive) {
if (m_isSolveFrictionAtContactManifoldCenterActive) {
manifold.normal += contactPoint.normal;
}
}
@ -272,7 +272,7 @@ void ContactSolver::initializeContactConstraints() {
}
// If we solve the friction constraints at the center of the contact manifold
if (mIsSolveFrictionAtContactManifoldCenterActive) {
if (m_isSolveFrictionAtContactManifoldCenterActive) {
manifold.normal.normalize();
@ -320,12 +320,12 @@ void ContactSolver::initializeContactConstraints() {
void ContactSolver::warmStart() {
// Check that warm starting is active
if (!mIsWarmStartingActive) return;
if (!m_isWarmStartingActive) return;
// For each constraint
for (uint32_t c=0; c<m_numberContactManifolds; c++) {
ContactManifoldSolver& contactManifold = mContactConstraints[c];
ContactManifoldSolver& contactManifold = m_contactConstraints[c];
bool atLeastOneRestingContactPoint = false;
@ -348,7 +348,7 @@ void ContactSolver::warmStart() {
applyImpulse(impulsePenetration, contactManifold);
// If we do not solve the friction constraints at the center of the contact manifold
if (!mIsSolveFrictionAtContactManifoldCenterActive) {
if (!m_isSolveFrictionAtContactManifoldCenterActive) {
// Project the old friction impulses (with old friction vectors) int32_to
// the new friction vectors to get the new friction impulses
@ -404,7 +404,7 @@ void ContactSolver::warmStart() {
// If we solve the friction constraints at the center of the contact manifold and there is
// at least one resting contact point in the contact manifold
if (mIsSolveFrictionAtContactManifoldCenterActive && atLeastOneRestingContactPoint) {
if (m_isSolveFrictionAtContactManifoldCenterActive && atLeastOneRestingContactPoint) {
// Project the old friction impulses (with old friction vectors) int32_to the new friction
// vectors to get the new friction impulses
@ -497,15 +497,15 @@ void ContactSolver::solve() {
// For each contact manifold
for (uint32_t c=0; c<m_numberContactManifolds; c++) {
ContactManifoldSolver& contactManifold = mContactConstraints[c];
ContactManifoldSolver& contactManifold = m_contactConstraints[c];
float sumPenetrationImpulse = 0.0;
// Get the constrained velocities
const vec3& v1 = mLinearVelocities[contactManifold.indexBody1];
const vec3& w1 = mAngularVelocities[contactManifold.indexBody1];
const vec3& v2 = mLinearVelocities[contactManifold.indexBody2];
const vec3& w2 = mAngularVelocities[contactManifold.indexBody2];
const vec3& v1 = m_linearVelocities[contactManifold.indexBody1];
const vec3& w1 = m_angularVelocities[contactManifold.indexBody1];
const vec3& v2 = m_linearVelocities[contactManifold.indexBody2];
const vec3& w2 = m_angularVelocities[contactManifold.indexBody2];
for (uint32_t i=0; i<contactManifold.nbContacts; i++) {
@ -519,14 +519,14 @@ void ContactSolver::solve() {
float Jv = deltaVDotN;
// Compute the bias "b" of the constraint
float beta = mIsSplitImpulseActive ? BETA_SPLIT_IMPULSE : BETA;
float beta = m_isSplitImpulseActive ? BETA_SPLIT_IMPULSE : BETA;
float biasPenetrationDepth = 0.0;
if (contactPoint.penetrationDepth > SLOP) biasPenetrationDepth = -(beta/m_timeStep) *
max(0.0f, float(contactPoint.penetrationDepth - SLOP));
float b = biasPenetrationDepth + contactPoint.restitutionBias;
// Compute the Lagrange multiplier lambda
if (mIsSplitImpulseActive) {
if (m_isSplitImpulseActive) {
deltaLambda = - (Jv + contactPoint.restitutionBias) *
contactPoint.inversePenetrationMass;
}
@ -548,7 +548,7 @@ void ContactSolver::solve() {
sumPenetrationImpulse += contactPoint.penetrationImpulse;
// If the split impulse position correction is active
if (mIsSplitImpulseActive) {
if (m_isSplitImpulseActive) {
// Split impulse (position correction)
const vec3& v1Split = m_splitLinearVelocities[contactManifold.indexBody1];
@ -574,7 +574,7 @@ void ContactSolver::solve() {
}
// If we do not solve the friction constraints at the center of the contact manifold
if (!mIsSolveFrictionAtContactManifoldCenterActive) {
if (!m_isSolveFrictionAtContactManifoldCenterActive) {
// --------- Friction 1 --------- //
@ -650,7 +650,7 @@ void ContactSolver::solve() {
}
// If we solve the friction constraints at the center of the contact manifold
if (mIsSolveFrictionAtContactManifoldCenterActive) {
if (m_isSolveFrictionAtContactManifoldCenterActive) {
// ------ First friction constraint at the center of the contact manifol ------ //
@ -768,7 +768,7 @@ void ContactSolver::storeImpulses() {
// For each contact manifold
for (uint32_t c=0; c<m_numberContactManifolds; c++) {
ContactManifoldSolver& manifold = mContactConstraints[c];
ContactManifoldSolver& manifold = m_contactConstraints[c];
for (uint32_t i=0; i<manifold.nbContacts; i++) {
@ -797,15 +797,15 @@ void ContactSolver::applyImpulse(const Impulse& impulse,
const ContactManifoldSolver& manifold) {
// Update the velocities of the body 1 by applying the impulse P
mLinearVelocities[manifold.indexBody1] += manifold.massInverseBody1 *
m_linearVelocities[manifold.indexBody1] += manifold.massInverseBody1 *
impulse.linearImpulseBody1;
mAngularVelocities[manifold.indexBody1] += manifold.inverseInertiaTensorBody1 *
m_angularVelocities[manifold.indexBody1] += manifold.inverseInertiaTensorBody1 *
impulse.angularImpulseBody1;
// Update the velocities of the body 1 by applying the impulse P
mLinearVelocities[manifold.indexBody2] += manifold.massInverseBody2 *
m_linearVelocities[manifold.indexBody2] += manifold.massInverseBody2 *
impulse.linearImpulseBody2;
mAngularVelocities[manifold.indexBody2] += manifold.inverseInertiaTensorBody2 *
m_angularVelocities[manifold.indexBody2] += manifold.inverseInertiaTensorBody2 *
impulse.angularImpulseBody2;
}
@ -889,8 +889,8 @@ void ContactSolver::computeFrictionVectors(const vec3& deltaVelocity,
// Clean up the constraint solver
void ContactSolver::cleanup() {
if (mContactConstraints != nullptr) {
delete[] mContactConstraints;
mContactConstraints = nullptr;
if (m_contactConstraints != nullptr) {
delete[] m_contactConstraints;
m_contactConstraints = nullptr;
}
}

644
ephysics/engine/ContactSolver.hpp
View File

@ -5,7 +5,6 @@
*/
#pragma once
// Libraries
#include <ephysics/constraint/ContactPoint.hpp>
#include <ephysics/configuration.hpp>
#include <ephysics/constraint/Joint.hpp>
@ -15,419 +14,228 @@
#include <map>
#include <set>
/// ReactPhysics3D namespace
namespace ephysics {
// Class Contact Solver
/**
* This class represents the contact solver 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).
*
* A constraint between two bodies is represented by a function C(x) which is equal to zero
* when the constraint is satisfied. The condition C(x)=0 describes a valid position and the
* condition dC(x)/dt=0 describes a valid velocity. We have dC(x)/dt = Jv + b = 0 where J is
* the Jacobian matrix of the constraint, v is a vector that contains the velocity of both
* bodies and b is the constraint bias. We are looking for a force F_c that will act on the
* bodies to keep the constraint satisfied. Note that from the virtual work principle, we have
* F_c = J^t * lambda where J^t is the transpose of the Jacobian matrix and lambda is a
* Lagrange multiplier. Therefore, finding the force F_c is equivalent to finding the Lagrange
* multiplier lambda.
*
* An impulse P = F * dt where F is a force and dt is the timestep. We can apply impulses a
* body to change its velocity. The idea of the Sequential Impulse technique is to apply
* impulses to bodies of each constraints in order to keep the constraint satisfied.
*
* --- Step 1 ---
*
* First, we int32_tegrate the applied force F_a acting of each rigid body (like gravity, ...) and
* we obtain some new velocities v2' that tends to violate the constraints.
*
* v2' = v1 + dt * M^-1 * F_a
*
* where M is a matrix that contains mass and inertia tensor information.
*
* --- Step 2 ---
*
* During the second step, we iterate over all the constraints for a certain number of
* iterations and for each constraint we compute the impulse to apply to the bodies needed
* so that the new velocity of the bodies satisfy Jv + b = 0. From the Newton law, we know that
* M * deltaV = P_c where M is the mass of the body, deltaV is the difference of velocity and
* P_c is the constraint impulse to apply to the body. Therefore, we have
* v2 = v2' + M^-1 * P_c. For each constraint, we can compute the Lagrange multiplier lambda
* using : lambda = -m_c (Jv2' + b) where m_c = 1 / (J * M^-1 * J^t). Now that we have the
* Lagrange multiplier lambda, we can compute the impulse P_c = J^t * lambda * dt to apply to
* the bodies to satisfy the constraint.
*
* --- Step 3 ---
*
* In the third step, we int32_tegrate the new position x2 of the bodies using the new velocities
* v2 computed in the second step with : x2 = x1 + dt * v2.
*
* Note that in the following code (as it is also explained in the slides from Erin Catto),
* the value lambda is not only the lagrange multiplier but is the multiplication of the
* Lagrange multiplier with the timestep dt. Therefore, in the following code, when we use
* lambda, we mean (lambda * dt).
*
* We are using the accumulated impulse technique that is also described in the slides from
* Erin Catto.
*
* We are also using warm starting. The idea is to warm start the solver at the beginning of
* each step by applying the last impulstes for the constraints that we already existing at the
* previous step. This allows the iterative solver to converge faster towards the solution.
*
* For contact constraints, we are also using split impulses so that the position correction
* that uses Baumgarte stabilization does not change the momentum of the bodies.
*
* There are two ways to apply the friction constraints. Either the friction constraints are
* applied at each contact point or they are applied only at the center of the contact manifold
* between two bodies. If we solve the friction constraints at each contact point, we need
* two constraints (two tangential friction directions) and if we solve the friction
* constraints at the center of the contact manifold, we need two constraints for tangential
* friction but also another twist friction constraint to prevent spin of the body around the
* contact manifold center.
*/
class ContactSolver {
private:
// Structure ContactPointSolver
/**
* Contact solver int32_ternal data structure that to store all the
* information relative to a contact point
*/
struct ContactPointSolver {
/// Accumulated normal impulse
float penetrationImpulse;
/// Accumulated impulse in the 1st friction direction
float friction1Impulse;
/// Accumulated impulse in the 2nd friction direction
float friction2Impulse;
/// Accumulated split impulse for penetration correction
float penetrationSplitImpulse;
/// Accumulated rolling resistance impulse
vec3 rollingResistanceImpulse;
/// Normal vector of the contact
vec3 normal;
/// First friction vector in the tangent plane
vec3 frictionVector1;
/// Second friction vector in the tangent plane
vec3 frictionvec2;
/// Old first friction vector in the tangent plane
vec3 oldFrictionVector1;
/// Old second friction vector in the tangent plane
vec3 oldFrictionvec2;
/// Vector from the body 1 center to the contact point
vec3 r1;
/// Vector from the body 2 center to the contact point
vec3 r2;
/// Cross product of r1 with 1st friction vector
vec3 r1CrossT1;
/// Cross product of r1 with 2nd friction vector
vec3 r1CrossT2;
/// Cross product of r2 with 1st friction vector
vec3 r2CrossT1;
/// Cross product of r2 with 2nd friction vector
vec3 r2CrossT2;
/// Cross product of r1 with the contact normal
vec3 r1CrossN;
/// Cross product of r2 with the contact normal
vec3 r2CrossN;
/// Penetration depth
float penetrationDepth;
/// Velocity restitution bias
float restitutionBias;
/// Inverse of the matrix K for the penenetration
float inversePenetrationMass;
/// Inverse of the matrix K for the 1st friction
float inverseFriction1Mass;
/// Inverse of the matrix K for the 2nd friction
float inverseFriction2Mass;
/// True if the contact was existing last time step
bool isRestingContact;
/// Pointer to the external contact
ContactPoint* externalContact;
};
// Structure ContactManifoldSolver
/**
* Contact solver int32_ternal data structure to store all the
* information relative to a contact manifold.
*/
struct ContactManifoldSolver {
/// Index of body 1 in the constraint solver
uint32_t indexBody1;
/// Index of body 2 in the constraint solver
uint32_t indexBody2;
/// Inverse of the mass of body 1
float massInverseBody1;
// Inverse of the mass of body 2
float massInverseBody2;
/// Inverse inertia tensor of body 1
etk::Matrix3x3 inverseInertiaTensorBody1;
/// Inverse inertia tensor of body 2
etk::Matrix3x3 inverseInertiaTensorBody2;
/// Contact point constraints
ContactPointSolver contacts[MAX_CONTACT_POINTS_IN_MANIFOLD];
/// Number of contact points
uint32_t nbContacts;
/// True if the body 1 is of type dynamic
bool isBody1DynamicType;
/// True if the body 2 is of type dynamic
bool isBody2DynamicType;
/// Mix of the restitution factor for two bodies
float restitutionFactor;
/// Mix friction coefficient for the two bodies
float frictionCoefficient;
/// Rolling resistance factor between the two bodies
float rollingResistanceFactor;
/// Pointer to the external contact manifold
ContactManifold* externalContactManifold;
// - Variables used when friction constraints are apply at the center of the manifold-//
/// Average normal vector of the contact manifold
vec3 normal;
/// Point on body 1 where to apply the friction constraints
vec3 frictionPointBody1;
/// Point on body 2 where to apply the friction constraints
vec3 frictionPointBody2;
/// R1 vector for the friction constraints
vec3 r1Friction;
/// R2 vector for the friction constraints
vec3 r2Friction;
/// Cross product of r1 with 1st friction vector
vec3 r1CrossT1;
/// Cross product of r1 with 2nd friction vector
vec3 r1CrossT2;
/// Cross product of r2 with 1st friction vector
vec3 r2CrossT1;
/// Cross product of r2 with 2nd friction vector
vec3 r2CrossT2;
/// Matrix K for the first friction constraint
float inverseFriction1Mass;
/// Matrix K for the second friction constraint
float inverseFriction2Mass;
/// Matrix K for the twist friction constraint
float inverseTwistFrictionMass;
/// Matrix K for the rolling resistance constraint
etk::Matrix3x3 inverseRollingResistance;
/// First friction direction at contact manifold center
vec3 frictionVector1;
/// Second friction direction at contact manifold center
vec3 frictionvec2;
/// Old 1st friction direction at contact manifold center
vec3 oldFrictionVector1;
/// Old 2nd friction direction at contact manifold center
vec3 oldFrictionvec2;
/// First friction direction impulse at manifold center
float friction1Impulse;
/// Second friction direction impulse at manifold center
float friction2Impulse;
/// Twist friction impulse at contact manifold center
float frictionTwistImpulse;
/// Rolling resistance impulse
vec3 rollingResistanceImpulse;
};
// -------------------- Constants --------------------- //
/// Beta value for the penetration depth position correction without split impulses
static const float BETA;
/// Beta value for the penetration depth position correction with split impulses
static const float BETA_SPLIT_IMPULSE;
/// Slop distance (allowed penetration distance between bodies)
static const float SLOP;
// -------------------- Attributes -------------------- //
/// Split linear velocities for the position contact solver (split impulse)
vec3* m_splitLinearVelocities;
/// Split angular velocities for the position contact solver (split impulse)
vec3* m_splitAngularVelocities;
/// Current time step
float m_timeStep;
/// Contact constraints
ContactManifoldSolver* mContactConstraints;
/// Number of contact constraints
uint32_t m_numberContactManifolds;
/// Array of linear velocities
vec3* mLinearVelocities;
/// Array of angular velocities
vec3* mAngularVelocities;
/// Reference to the map of rigid body to their index in the constrained velocities array
const std::map<RigidBody*, uint32_t>& m_mapBodyToConstrainedVelocityIndex;
/// True if the warm starting of the solver is active
bool mIsWarmStartingActive;
/// True if the split impulse position correction is active
bool mIsSplitImpulseActive;
/// True if we solve 3 friction constraints at the contact manifold center only
/// instead of 2 friction constraints at each contact point
bool mIsSolveFrictionAtContactManifoldCenterActive;
// -------------------- Methods -------------------- //
/// Initialize the contact constraints before solving the system
void initializeContactConstraints();
/// Apply an impulse to the two bodies of a constraint
void applyImpulse(const Impulse& impulse, const ContactManifoldSolver& manifold);
/// Apply an impulse to the two bodies of a constraint
void applySplitImpulse(const Impulse& impulse,
const ContactManifoldSolver& manifold);
/// Compute the collision restitution factor from the restitution factor of each body
float computeMixedRestitutionFactor(RigidBody *body1,
RigidBody *body2) const;
/// Compute the mixed friction coefficient from the friction coefficient of each body
float computeMixedFrictionCoefficient(RigidBody* body1,
RigidBody* body2) const;
/// Compute th mixed rolling resistance factor between two bodies
float computeMixedRollingResistance(RigidBody* body1, RigidBody* body2) const;
/// Compute the two unit orthogonal vectors "t1" and "t2" that span the tangential friction
/// plane for a contact point. The two vectors have to be
/// such that : t1 x t2 = contactNormal.
void computeFrictionVectors(const vec3& deltaVelocity,
ContactPointSolver &contactPoint) const;
/// 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 computeFrictionVectors(const vec3& deltaVelocity,
ContactManifoldSolver& contactPoint) const;
/// Compute a penetration constraint impulse
const Impulse computePenetrationImpulse(float deltaLambda,
const ContactPointSolver& contactPoint) const;
/// Compute the first friction constraint impulse
const Impulse computeFriction1Impulse(float deltaLambda,
const ContactPointSolver& contactPoint) const;
/// Compute the second friction constraint impulse
const Impulse computeFriction2Impulse(float deltaLambda,
const ContactPointSolver& contactPoint) const;
public:
// -------------------- Methods -------------------- //
/// Constructor
ContactSolver(const std::map<RigidBody*, uint32_t>& mapBodyToVelocityIndex);
/// Destructor
virtual ~ContactSolver();
/// Initialize the constraint solver for a given island
void initializeForIsland(float dt, Island* island);
/// Set the split velocities arrays
void setSplitVelocitiesArrays(vec3* splitLinearVelocities,
vec3* splitAngularVelocities);
/// Set the constrained velocities arrays
void setConstrainedVelocitiesArrays(vec3* constrainedLinearVelocities,
vec3* constrainedAngularVelocities);
/// Warm start the solver.
void warmStart();
/// Store the computed impulses to use them to
/// warm start the solver at the next iteration
void storeImpulses();
/// Solve the contacts
void solve();
/// Return true if the split impulses position correction technique is used for contacts
bool isSplitImpulseActive() const;
/// Activate or Deactivate the split impulses for contacts
void setIsSplitImpulseActive(bool isActive);
/// Activate or deactivate the solving of friction constraints at the center of
/// the contact manifold instead of solving them at each contact point
void setIsSolveFrictionAtContactManifoldCenterActive(bool isActive);
/// Clean up the constraint solver
void cleanup();
};
/**
* @brief This class represents the contact solver 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).
*
* A constraint between two bodies is represented by a function C(x) which is equal to zero
* when the constraint is satisfied. The condition C(x)=0 describes a valid position and the
* condition dC(x)/dt=0 describes a valid velocity. We have dC(x)/dt = Jv + b = 0 where J is
* the Jacobian matrix of the constraint, v is a vector that contains the velocity of both
* bodies and b is the constraint bias. We are looking for a force F_c that will act on the
* bodies to keep the constraint satisfied. Note that from the virtual work principle, we have
* F_c = J^t * lambda where J^t is the transpose of the Jacobian matrix and lambda is a
* Lagrange multiplier. Therefore, finding the force F_c is equivalent to finding the Lagrange
* multiplier lambda.
*
* An impulse P = F * dt where F is a force and dt is the timestep. We can apply impulses a
* body to change its velocity. The idea of the Sequential Impulse technique is to apply
* impulses to bodies of each constraints in order to keep the constraint satisfied.
*
* --- Step 1 ---
*
* First, we int32_tegrate the applied force F_a acting of each rigid body (like gravity, ...) and
* we obtain some new velocities v2' that tends to violate the constraints.
*
* v2' = v1 + dt * M^-1 * F_a
*
* where M is a matrix that contains mass and inertia tensor information.
*
* --- Step 2 ---
*
* During the second step, we iterate over all the constraints for a certain number of
* iterations and for each constraint we compute the impulse to apply to the bodies needed
* so that the new velocity of the bodies satisfy Jv + b = 0. From the Newton law, we know that
* M * deltaV = P_c where M is the mass of the body, deltaV is the difference of velocity and
* P_c is the constraint impulse to apply to the body. Therefore, we have
* v2 = v2' + M^-1 * P_c. For each constraint, we can compute the Lagrange multiplier lambda
* using : lambda = -m_c (Jv2' + b) where m_c = 1 / (J * M^-1 * J^t). Now that we have the
* Lagrange multiplier lambda, we can compute the impulse P_c = J^t * lambda * dt to apply to
* the bodies to satisfy the constraint.
*
* --- Step 3 ---
*
* In the third step, we int32_tegrate the new position x2 of the bodies using the new velocities
* v2 computed in the second step with : x2 = x1 + dt * v2.
*
* Note that in the following code (as it is also explained in the slides from Erin Catto),
* the value lambda is not only the lagrange multiplier but is the multiplication of the
* Lagrange multiplier with the timestep dt. Therefore, in the following code, when we use
* lambda, we mean (lambda * dt).
*
* We are using the accumulated impulse technique that is also described in the slides from
* Erin Catto.
*
* We are also using warm starting. The idea is to warm start the solver at the beginning of
* each step by applying the last impulstes for the constraints that we already existing at the
* previous step. This allows the iterative solver to converge faster towards the solution.
*
* For contact constraints, we are also using split impulses so that the position correction
* that uses Baumgarte stabilization does not change the momentum of the bodies.
*
* There are two ways to apply the friction constraints. Either the friction constraints are
* applied at each contact point or they are applied only at the center of the contact manifold
* between two bodies. If we solve the friction constraints at each contact point, we need
* two constraints (two tangential friction directions) and if we solve the friction
* constraints at the center of the contact manifold, we need two constraints for tangential
* friction but also another twist friction constraint to prevent spin of the body around the
* contact manifold center.
*/
class ContactSolver {
private:
/**
* Contact solver int32_ternal data structure that to store all the
* information relative to a contact point
*/
struct ContactPointSolver {
float penetrationImpulse; //!< Accumulated normal impulse
float friction1Impulse; //!< Accumulated impulse in the 1st friction direction
float friction2Impulse; //!< Accumulated impulse in the 2nd friction direction
float penetrationSplitImpulse; //!< Accumulated split impulse for penetration correction
vec3 rollingResistanceImpulse; //!< Accumulated rolling resistance impulse
vec3 normal; //!< Normal vector of the contact
vec3 frictionVector1; //!< First friction vector in the tangent plane
vec3 frictionvec2; //!< Second friction vector in the tangent plane
vec3 oldFrictionVector1; //!< Old first friction vector in the tangent plane
vec3 oldFrictionvec2; //!< Old second friction vector in the tangent plane
vec3 r1; //!< Vector from the body 1 center to the contact point
vec3 r2; //!< Vector from the body 2 center to the contact point
vec3 r1CrossT1; //!< Cross product of r1 with 1st friction vector
vec3 r1CrossT2; //!< Cross product of r1 with 2nd friction vector
vec3 r2CrossT1; //!< Cross product of r2 with 1st friction vector
vec3 r2CrossT2; //!< Cross product of r2 with 2nd friction vector
vec3 r1CrossN; //!< Cross product of r1 with the contact normal
vec3 r2CrossN; //!< Cross product of r2 with the contact normal
float penetrationDepth; //!< Penetration depth
float restitutionBias; //!< Velocity restitution bias
float inversePenetrationMass; //!< Inverse of the matrix K for the penenetration
float inverseFriction1Mass; //!< Inverse of the matrix K for the 1st friction
float inverseFriction2Mass; //!< Inverse of the matrix K for the 2nd friction
bool isRestingContact; //!< True if the contact was existing last time step
ContactPoint* externalContact; //!< Pointer to the external contact
};
/**
* @brief Contact solver int32_ternal data structure to store all the information relative to a contact manifold.
*/
struct ContactManifoldSolver {
uint32_t indexBody1; //!< Index of body 1 in the constraint solver
uint32_t indexBody2; //!< Index of body 2 in the constraint solver
float massInverseBody1; //!< Inverse of the mass of body 1
float massInverseBody2; //!< Inverse of the mass of body 2
etk::Matrix3x3 inverseInertiaTensorBody1; //!< Inverse inertia tensor of body 1
etk::Matrix3x3 inverseInertiaTensorBody2; //!< Inverse inertia tensor of body 2
ContactPointSolver contacts[MAX_CONTACT_POINTS_IN_MANIFOLD]; //!< Contact point constraints
uint32_t nbContacts; //!< Number of contact points
bool isBody1DynamicType; //!< True if the body 1 is of type dynamic
bool isBody2DynamicType; //!< True if the body 2 is of type dynamic
float restitutionFactor; //!< Mix of the restitution factor for two bodies
float frictionCoefficient; //!< Mix friction coefficient for the two bodies
float rollingResistanceFactor; //!< Rolling resistance factor between the two bodies
ContactManifold* externalContactManifold; //!< Pointer to the external contact manifold
// - Variables used when friction constraints are apply at the center of the manifold-//
vec3 normal; //!< Average normal vector of the contact manifold
vec3 frictionPointBody1; //!< Point on body 1 where to apply the friction constraints
vec3 frictionPointBody2; //!< Point on body 2 where to apply the friction constraints
vec3 r1Friction; //!< R1 vector for the friction constraints
vec3 r2Friction; //!< R2 vector for the friction constraints
vec3 r1CrossT1; //!< Cross product of r1 with 1st friction vector
vec3 r1CrossT2; //!< Cross product of r1 with 2nd friction vector
vec3 r2CrossT1; //!< Cross product of r2 with 1st friction vector
vec3 r2CrossT2; //!< Cross product of r2 with 2nd friction vector
float inverseFriction1Mass; //!< Matrix K for the first friction constraint
float inverseFriction2Mass; //!< Matrix K for the second friction constraint
float inverseTwistFrictionMass; //!< Matrix K for the twist friction constraint
etk::Matrix3x3 inverseRollingResistance; //!< Matrix K for the rolling resistance constraint
vec3 frictionVector1; //!< First friction direction at contact manifold center
vec3 frictionvec2; //!< Second friction direction at contact manifold center
vec3 oldFrictionVector1; //!< Old 1st friction direction at contact manifold center
vec3 oldFrictionvec2; //!< Old 2nd friction direction at contact manifold center
float friction1Impulse; //!< First friction direction impulse at manifold center
float friction2Impulse; //!< Second friction direction impulse at manifold center
float frictionTwistImpulse; //!< Twist friction impulse at contact manifold center
vec3 rollingResistanceImpulse; //!< Rolling resistance impulse
};
static const float BETA; //!< Beta value for the penetration depth position correction without split impulses
static const float BETA_SPLIT_IMPULSE; //!< Beta value for the penetration depth position correction with split impulses
static const float SLOP; //!< Slop distance (allowed penetration distance between bodies)
vec3* m_splitLinearVelocities; //!< Split linear velocities for the position contact solver (split impulse)
vec3* m_splitAngularVelocities; //!< Split angular velocities for the position contact solver (split impulse)
float m_timeStep; //!< Current time step
ContactManifoldSolver* m_contactConstraints; //!< Contact constraints
uint32_t m_numberContactManifolds; //!< Number of contact constraints
vec3* m_linearVelocities; //!< Array of linear velocities
vec3* m_angularVelocities; //!< Array of angular velocities
const std::map<RigidBody*, uint32_t>& m_mapBodyToConstrainedVelocityIndex; //!< Reference to the map of rigid body to their index in the constrained velocities array
bool m_isWarmStartingActive; //!< True if the warm starting of the solver is active
bool m_isSplitImpulseActive; //!< True if the split impulse position correction is active
bool m_isSolveFrictionAtContactManifoldCenterActive; //!< True if we solve 3 friction constraints at the contact manifold center only instead of 2 friction constraints at each contact point
/// Initialize the contact constraints before solving the system
void initializeContactConstraints();
/// Apply an impulse to the two bodies of a constraint
void applyImpulse(const Impulse& impulse, const ContactManifoldSolver& manifold);
/// Apply an impulse to the two bodies of a constraint
void applySplitImpulse(const Impulse& impulse,
const ContactManifoldSolver& manifold);
/// Compute the collision restitution factor from the restitution factor of each body
float computeMixedRestitutionFactor(RigidBody *body1,
RigidBody *body2) const;
/// Compute the mixed friction coefficient from the friction coefficient of each body
float computeMixedFrictionCoefficient(RigidBody* body1,
RigidBody* body2) const;
/// Compute th mixed rolling resistance factor between two bodies
float computeMixedRollingResistance(RigidBody* body1, RigidBody* body2) const;
/// Compute the two unit orthogonal vectors "t1" and "t2" that span the tangential friction
/// plane for a contact point. The two vectors have to be
/// such that : t1 x t2 = contactNormal.
void computeFrictionVectors(const vec3& deltaVelocity,
ContactPointSolver &contactPoint) const;
/// 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 computeFrictionVectors(const vec3& deltaVelocity,
ContactManifoldSolver& contactPoint) const;
/// Compute a penetration constraint impulse
const Impulse computePenetrationImpulse(float deltaLambda,
const ContactPointSolver& contactPoint) const;
/// Compute the first friction constraint impulse
const Impulse computeFriction1Impulse(float deltaLambda,
const ContactPointSolver& contactPoint) const;
/// Compute the second friction constraint impulse
const Impulse computeFriction2Impulse(float deltaLambda,
const ContactPointSolver& contactPoint) const;
public:
/// Constructor
ContactSolver(const std::map<RigidBody*, uint32_t>& mapBodyToVelocityIndex);
/// Destructor
virtual ~ContactSolver();
/// Initialize the constraint solver for a given island
void initializeForIsland(float dt, Island* island);
/// Set the split velocities arrays
void setSplitVelocitiesArrays(vec3* splitLinearVelocities,
vec3* splitAngularVelocities);
/// Set the constrained velocities arrays
void setConstrainedVelocitiesArrays(vec3* constrainedLinearVelocities,
vec3* constrainedAngularVelocities);
/// Warm start the solver.
void warmStart();
/// Store the computed impulses to use them to
/// warm start the solver at the next iteration
void storeImpulses();
/// Solve the contacts
void solve();
/// Return true if the split impulses position correction technique is used for contacts
bool isSplitImpulseActive() const;
/// Activate or Deactivate the split impulses for contacts
void setIsSplitImpulseActive(bool isActive);
/// Activate or deactivate the solving of friction constraints at the center of
/// the contact manifold instead of solving them at each contact point
void setIsSolveFrictionAtContactManifoldCenterActive(bool isActive);
/// Clean up the constraint solver
void cleanup();
};
// Set the split velocities arrays
inline void ContactSolver::setSplitVelocitiesArrays(vec3* splitLinearVelocities,
@ -443,24 +251,24 @@ inline void ContactSolver::setConstrainedVelocitiesArrays(vec3* constrainedLinea
vec3* constrainedAngularVelocities) {
assert(constrainedLinearVelocities != NULL);
assert(constrainedAngularVelocities != NULL);
mLinearVelocities = constrainedLinearVelocities;
mAngularVelocities = constrainedAngularVelocities;
m_linearVelocities = constrainedLinearVelocities;
m_angularVelocities = constrainedAngularVelocities;
}
// Return true if the split impulses position correction technique is used for contacts
inline bool ContactSolver::isSplitImpulseActive() const {
return mIsSplitImpulseActive;
return m_isSplitImpulseActive;
}
// Activate or Deactivate the split impulses for contacts
inline void ContactSolver::setIsSplitImpulseActive(bool isActive) {
mIsSplitImpulseActive = isActive;
m_isSplitImpulseActive = isActive;
}
// Activate or deactivate the solving of friction constraints at the center of
// the contact manifold instead of solving them at each contact point
inline void ContactSolver::setIsSolveFrictionAtContactManifoldCenterActive(bool isActive) {
mIsSolveFrictionAtContactManifoldCenterActive = isActive;
m_isSolveFrictionAtContactManifoldCenterActive = isActive;
}
// Compute the collision restitution factor from the restitution factor of each body

419
ephysics/engine/DynamicsWorld.hpp
View File

@ -5,7 +5,6 @@
*/
#pragma once
// Libraries
#include <ephysics/engine/CollisionWorld.hpp>
#include <ephysics/collision/CollisionDetection.hpp>
#include <ephysics/engine/ContactSolver.hpp>
@ -14,275 +13,157 @@
#include <ephysics/engine/Island.hpp>
#include <ephysics/configuration.hpp>
/// Namespace ReactPhysics3D
namespace ephysics {
// Class DynamicsWorld
/**
* This class represents a dynamics world. This class inherits from
* the CollisionWorld class. In a dynamics world, bodies can collide
* and their movements are simulated using the laws of physics.
*/
class DynamicsWorld : public CollisionWorld {
protected :
// -------------------- Attributes -------------------- //
/// Contact solver
ContactSolver m_contactSolver;
/// Constraint solver
ConstraintSolver m_constraintSolver;
/// Number of iterations for the velocity solver of the Sequential Impulses technique
uint32_t m_nbVelocitySolverIterations;
/// Number of iterations for the position solver of the Sequential Impulses technique
uint32_t m_nbPositionSolverIterations;
/// True if the spleeping technique for inactive bodies is enabled
bool m_isSleepingEnabled;
/// All the rigid bodies of the physics world
std::set<RigidBody*> m_rigidBodies;
/// All the joints of the world
std::set<Joint*> m_joints;
/// Gravity vector of the world
vec3 m_gravity;
/// Current frame time step (in seconds)
float m_timeStep;
/// True if the gravity force is on
bool m_isGravityEnabled;
/// Array of constrained linear velocities (state of the linear velocities
/// after solving the constraints)
vec3* m_constrainedLinearVelocities;
/// Array of constrained angular velocities (state of the angular velocities
/// after solving the constraints)
vec3* m_constrainedAngularVelocities;
/// Split linear velocities for the position contact solver (split impulse)
vec3* m_splitLinearVelocities;
/// Split angular velocities for the position contact solver (split impulse)
vec3* m_splitAngularVelocities;
/// Array of constrained rigid bodies position (for position error correction)
vec3* m_constrainedPositions;
/// Array of constrained rigid bodies orientation (for position error correction)
etk::Quaternion* m_constrainedOrientations;
/// Map body to their index in the constrained velocities array
std::map<RigidBody*, uint32_t> m_mapBodyToConstrainedVelocityIndex;
/// Number of islands in the world
uint32_t m_numberIslands;
/// Current allocated capacity for the islands
uint32_t m_numberIslandsCapacity;
/// Array with all the islands of awaken bodies
Island** m_islands;
/// Current allocated capacity for the bodies
uint32_t m_numberBodiesCapacity;
/// Sleep linear velocity threshold
float m_sleepLinearVelocity;
/// Sleep angular velocity threshold
float m_sleepAngularVelocity;
/// Time (in seconds) before a body is put to sleep if its velocity
/// becomes smaller than the sleep velocity.
float m_timeBeforeSleep;
// -------------------- Methods -------------------- //
/// Private copy-constructor
DynamicsWorld(const DynamicsWorld& world);
/// Private assignment operator
DynamicsWorld& operator=(const DynamicsWorld& world);
/// Integrate the positions and orientations of rigid bodies.
void int32_tegrateRigidBodiesPositions();
/// Update the AABBs of the bodies
void updateRigidBodiesAABB();
/// Reset the external force and torque applied to the bodies
void resetBodiesForceAndTorque();
/// Update the position and orientation of a body
void updatePositionAndOrientationOfBody(RigidBody* body, vec3 newLinVelocity,
vec3 newAngVelocity);
/// Compute and set the int32_terpolation factor to all bodies
void setInterpolationFactorToAllBodies();
/// Initialize the bodies velocities arrays for the next simulation step.
void initVelocityArrays();
/// Integrate the velocities of rigid bodies.
void int32_tegrateRigidBodiesVelocities();
/// Solve the contacts and constraints
void solveContactsAndConstraints();
/// Solve the position error correction of the constraints
void solvePositionCorrection();
/// Cleanup the constrained velocities array at each step
void cleanupConstrainedVelocitiesArray();
/// Compute the islands of awake bodies.
void computeIslands();
/// Update the postion/orientation of the bodies
void updateBodiesState();
/// Put bodies to sleep if needed.
void updateSleepingBodies();
/// Add the joint to the list of joints of the two bodies involved in the joint
void addJointToBody(Joint* joint);
public :
// -------------------- Methods -------------------- //
/// Constructor
DynamicsWorld(const vec3& m_gravity);
/// Destructor
virtual ~DynamicsWorld();
/// Update the physics simulation
void update(float timeStep);
/// Get the number of iterations for the velocity constraint solver
uint32_t getNbIterationsVelocitySolver() const;
/// Set the number of iterations for the velocity constraint solver
void setNbIterationsVelocitySolver(uint32_t nbIterations);
/// Get the number of iterations for the position constraint solver
uint32_t getNbIterationsPositionSolver() const;
/// Set the number of iterations for the position constraint solver
void setNbIterationsPositionSolver(uint32_t nbIterations);
/// Set the position correction technique used for contacts
void setContactsPositionCorrectionTechnique(ContactsPositionCorrectionTechnique technique);
/// Set the position correction technique used for joints
void setJointsPositionCorrectionTechnique(JointsPositionCorrectionTechnique technique);
/// Activate or deactivate the solving of friction constraints at the center of
/// the contact manifold instead of solving them at each contact point
void setIsSolveFrictionAtContactManifoldCenterActive(bool isActive);
/// Create a rigid body int32_to the physics world.
RigidBody* createRigidBody(const etk::Transform3D& transform);
/// Destroy a rigid body and all the joints which it belongs
void destroyRigidBody(RigidBody* rigidBody);
/// Create a joint between two bodies in the world and return a pointer to the new joint
Joint* createJoint(const JointInfo& jointInfo);
/// Destroy a joint
void destroyJoint(Joint* joint);
/// Return the gravity vector of the world
vec3 getGravity() const;
/// Set the gravity vector of the world
void setGravity(vec3& gravity);
/// Return if the gravity is on
bool isGravityEnabled() const;
/// Enable/Disable the gravity
void setIsGratityEnabled(bool isGravityEnabled);
/// Return the number of rigid bodies in the world
uint32_t getNbRigidBodies() const;
/// Return the number of joints in the world
uint32_t getNbJoints() const;
/// Return an iterator to the beginning of the rigid bodies of the physics world
std::set<RigidBody*>::iterator getRigidBodiesBeginIterator();
/// Return an iterator to the end of the rigid bodies of the physics world
std::set<RigidBody*>::iterator getRigidBodiesEndIterator();
/// Return true if the sleeping technique is enabled
bool isSleepingEnabled() const;
/// Enable/Disable the sleeping technique
void enableSleeping(bool isSleepingEnabled);
/// Return the current sleep linear velocity
float getSleepLinearVelocity() const;
/// Set the sleep linear velocity.
void setSleepLinearVelocity(float sleepLinearVelocity);
/// Return the current sleep angular velocity
float getSleepAngularVelocity() const;
/// Set the sleep angular velocity.
void setSleepAngularVelocity(float sleepAngularVelocity);
/// Return the time a body is required to stay still before sleeping
float getTimeBeforeSleep() const;
/// Set the time a body is required to stay still before sleeping
void setTimeBeforeSleep(float timeBeforeSleep);
/// Set an event listener object to receive events callbacks.
void setEventListener(EventListener* eventListener);
/// Test and report collisions between a given shape and all the others
/// shapes of the world
virtual void testCollision(const ProxyShape* shape,
CollisionCallback* callback);
/// Test and report collisions between two given shapes
virtual void testCollision(const ProxyShape* shape1,
const ProxyShape* shape2,
CollisionCallback* callback);
/// Test and report collisions between a body and all
/// the others bodies of the world
virtual void testCollision(const CollisionBody* body,
CollisionCallback* callback);
/// Test and report collisions between two bodies
virtual void testCollision(const CollisionBody* body1,
const CollisionBody* body2,
CollisionCallback* callback);
/// Test and report collisions between all shapes of the world
virtual void testCollision(CollisionCallback* callback);
/// Return the list of all contacts of the world
std::vector<const ContactManifold*> getContactsList() const;
// -------------------- Friendship -------------------- //
friend class RigidBody;
};
/**
* @brief This class represents a dynamics world. This class inherits from
* the CollisionWorld class. In a dynamics world, bodies can collide
* and their movements are simulated using the laws of physics.
*/
class DynamicsWorld : public CollisionWorld {
protected :
ContactSolver m_contactSolver; //!< Contact solver
ConstraintSolver m_constraintSolver; //!< Constraint solver
uint32_t m_nbVelocitySolverIterations; //!< Number of iterations for the velocity solver of the Sequential Impulses technique
uint32_t m_nbPositionSolverIterations; //!< Number of iterations for the position solver of the Sequential Impulses technique
bool m_isSleepingEnabled; //!< True if the spleeping technique for inactive bodies is enabled
std::set<RigidBody*> m_rigidBodies; //!< All the rigid bodies of the physics world
std::set<Joint*> m_joints; //!< All the joints of the world
vec3 m_gravity; //!< Gravity vector of the world
float m_timeStep; //!< Current frame time step (in seconds)
bool m_isGravityEnabled; //!< True if the gravity force is on
vec3* m_constrainedLinearVelocities; //!< Array of constrained linear velocities (state of the linear velocities after solving the constraints)
vec3* m_constrainedAngularVelocities; //!< Array of constrained angular velocities (state of the angular velocities after solving the constraints)
vec3* m_splitLinearVelocities; //!< Split linear velocities for the position contact solver (split impulse)
vec3* m_splitAngularVelocities; //!< Split angular velocities for the position contact solver (split impulse)
vec3* m_constrainedPositions; //!< Array of constrained rigid bodies position (for position error correction)
etk::Quaternion* m_constrainedOrientations; //!< Array of constrained rigid bodies orientation (for position error correction)
std::map<RigidBody*, uint32_t> m_mapBodyToConstrainedVelocityIndex; //!< Map body to their index in the constrained velocities array
uint32_t m_numberIslands; //!< Number of islands in the world
uint32_t m_numberIslandsCapacity; //!< Current allocated capacity for the islands
Island** m_islands; //!< Array with all the islands of awaken bodies
uint32_t m_numberBodiesCapacity; //!< Current allocated capacity for the bodies
float m_sleepLinearVelocity; //!< Sleep linear velocity threshold
float m_sleepAngularVelocity; //!< Sleep angular velocity threshold
float m_timeBeforeSleep; //!< Time (in seconds) before a body is put to sleep if its velocity becomes smaller than the sleep velocity.
/// Private copy-constructor
DynamicsWorld(const DynamicsWorld& world);
/// Private assignment operator
DynamicsWorld& operator=(const DynamicsWorld& world);
/// Integrate the positions and orientations of rigid bodies.
void int32_tegrateRigidBodiesPositions();
/// Update the AABBs of the bodies
void updateRigidBodiesAABB();
/// Reset the external force and torque applied to the bodies
void resetBodiesForceAndTorque();
/// Update the position and orientation of a body
void updatePositionAndOrientationOfBody(RigidBody* body, vec3 newLinVelocity,
vec3 newAngVelocity);
/// Compute and set the int32_terpolation factor to all bodies
void setInterpolationFactorToAllBodies();
/// Initialize the bodies velocities arrays for the next simulation step.
void initVelocityArrays();
/// Integrate the velocities of rigid bodies.
void int32_tegrateRigidBodiesVelocities();
/// Solve the contacts and constraints
void solveContactsAndConstraints();
/// Solve the position error correction of the constraints
void solvePositionCorrection();
/// Cleanup the constrained velocities array at each step
void cleanupConstrainedVelocitiesArray();
/// Compute the islands of awake bodies.
void computeIslands();
/// Update the postion/orientation of the bodies
void updateBodiesState();
/// Put bodies to sleep if needed.
void updateSleepingBodies();
/// Add the joint to the list of joints of the two bodies involved in the joint
void addJointToBody(Joint* joint);
public :
/// Constructor
DynamicsWorld(const vec3& m_gravity);
/// Destructor
virtual ~DynamicsWorld();
/// Update the physics simulation
void update(float timeStep);
/// Get the number of iterations for the velocity constraint solver
uint32_t getNbIterationsVelocitySolver() const;
/// Set the number of iterations for the velocity constraint solver
void setNbIterationsVelocitySolver(uint32_t nbIterations);
/// Get the number of iterations for the position constraint solver
uint32_t getNbIterationsPositionSolver() const;
/// Set the number of iterations for the position constraint solver
void setNbIterationsPositionSolver(uint32_t nbIterations);
/// Set the position correction technique used for contacts
void setContactsPositionCorrectionTechnique(ContactsPositionCorrectionTechnique technique);
/// Set the position correction technique used for joints
void setJointsPositionCorrectionTechnique(JointsPositionCorrectionTechnique technique);
/// Activate or deactivate the solving of friction constraints at the center of
/// the contact manifold instead of solving them at each contact point
void setIsSolveFrictionAtContactManifoldCenterActive(bool isActive);
/// Create a rigid body int32_to the physics world.
RigidBody* createRigidBody(const etk::Transform3D& transform);
/// Destroy a rigid body and all the joints which it belongs
void destroyRigidBody(RigidBody* rigidBody);
/// Create a joint between two bodies in the world and return a pointer to the new joint
Joint* createJoint(const JointInfo& jointInfo);
/// Destroy a joint
void destroyJoint(Joint* joint);
/// Return the gravity vector of the world
vec3 getGravity() const;
/// Set the gravity vector of the world
void setGravity(vec3& gravity);
/// Return if the gravity is on
bool isGravityEnabled() const;
/// Enable/Disable the gravity
void setIsGratityEnabled(bool isGravityEnabled);
/// Return the number of rigid bodies in the world
uint32_t getNbRigidBodies() const;
/// Return the number of joints in the world
uint32_t getNbJoints() const;
/// Return an iterator to the beginning of the rigid bodies of the physics world
std::set<RigidBody*>::iterator getRigidBodiesBeginIterator();
/// Return an iterator to the end of the rigid bodies of the physics world
std::set<RigidBody*>::iterator getRigidBodiesEndIterator();
/// Return true if the sleeping technique is enabled
bool isSleepingEnabled() const;
/// Enable/Disable the sleeping technique
void enableSleeping(bool isSleepingEnabled);
/// Return the current sleep linear velocity
float getSleepLinearVelocity() const;
/// Set the sleep linear velocity.
void setSleepLinearVelocity(float sleepLinearVelocity);
/// Return the current sleep angular velocity
float getSleepAngularVelocity() const;
/// Set the sleep angular velocity.
void setSleepAngularVelocity(float sleepAngularVelocity);
/// Return the time a body is required to stay still before sleeping
float getTimeBeforeSleep() const;
/// Set the time a body is required to stay still before sleeping
void setTimeBeforeSleep(float timeBeforeSleep);
/// Set an event listener object to receive events callbacks.
void setEventListener(EventListener* eventListener);
/// Test and report collisions between a given shape and all the others
/// shapes of the world
virtual void testCollision(const ProxyShape* shape,
CollisionCallback* callback);
/// Test and report collisions between two given shapes
virtual void testCollision(const ProxyShape* shape1,
const ProxyShape* shape2,
CollisionCallback* callback);
/// Test and report collisions between a body and all
/// the others bodies of the world
virtual void testCollision(const CollisionBody* body,
CollisionCallback* callback);
/// Test and report collisions between two bodies
virtual void testCollision(const CollisionBody* body1,
const CollisionBody* body2,
CollisionCallback* callback);
/// Test and report collisions between all shapes of the world
virtual void testCollision(CollisionCallback* callback);
/// Return the list of all contacts of the world
std::vector<const ContactManifold*> getContactsList() const;
friend class RigidBody;
};
// Reset the external force and torque applied to the bodies
inline void DynamicsWorld::resetBodiesForceAndTorque() {

87
ephysics/engine/EventListener.hpp
View File

@ -5,52 +5,49 @@
*/
#pragma once
// Libraries
#include <ephysics/constraint/ContactPoint.hpp>
namespace ephysics {
// Class EventListener
/**
* This class can be used to receive event callbacks from the physics engine.
* In order to receive callbacks, you need to create a new class that inherits from
* this one and you must override the methods you need. Then, you need to register your
* new event listener class to the physics world using the DynamicsWorld::setEventListener()
* method.
*/
class EventListener {
public :
/// Constructor
EventListener() {}
/// Destructor
virtual ~EventListener() {}
/// Called when a new contact point is found between two bodies that were separated before
/**
* @param contact Information about the contact
*/
virtual void beginContact(const ContactPointInfo& contact) {};
/// Called when a new contact point is found between two bodies
/**
* @param contact Information about the contact
*/
virtual void newContact(const ContactPointInfo& contact) {}
/// Called at the beginning of an int32_ternal tick of the simulation step.
/// Each time the DynamicsWorld::update() method is called, the physics
/// engine will do several int32_ternal simulation steps. This method is
/// called at the beginning of each int32_ternal simulation step.
virtual void beginInternalTick() {}
/// Called at the end of an int32_ternal tick of the simulation step.
/// Each time the DynamicsWorld::update() metho is called, the physics
/// engine will do several int32_ternal simulation steps. This method is
/// called at the end of each int32_ternal simulation step.
virtual void endInternalTick() {}
};
/**
* @brief This class can be used to receive event callbacks from the physics engine.
* In order to receive callbacks, you need to create a new class that inherits from
* this one and you must override the methods you need. Then, you need to register your
* new event listener class to the physics world using the DynamicsWorld::setEventListener()
* method.
*/
class EventListener {
public :
/**
* @brief Generic Constructor
*/
EventListener() {}
/**
* @brief Generic Desstructor take it virtual
*/
virtual ~EventListener() =default;
/**
* @brief Called when a new contact point is found between two bodies that were separated before
* @param contact Information about the contact
*/
virtual void beginContact(const ContactPointInfo& contact) {};
/**
* @brief Called when a new contact point is found between two bodies
* @param contact Information about the contact
*/
virtual void newContact(const ContactPointInfo& contact) {}
/**
* @brief Called at the beginning of an int32_ternal tick of the simulation step.
* Each time the DynamicsWorld::update() method is called, the physics
* engine will do several int32_ternal simulation steps. This method is
* called at the beginning of each int32_ternal simulation step.
*/
virtual void beginInternalTick() {}
/**
* @brief Called at the end of an int32_ternal tick of the simulation step.
* Each time the DynamicsWorld::update() metho is called, the physics
* engine will do several int32_ternal simulation steps. This method is
* called at the end of each int32_ternal simulation step.
*/
virtual void endInternalTick() {}
};
}

85
ephysics/engine/Impulse.hpp
View File

@ -5,60 +5,39 @@
*/
#pragma once
// Libraries
#include <ephysics/mathematics/mathematics.hpp>
namespace ephysics {
// Structure Impulse
/**
* Represents an impulse that we can apply to bodies in the contact or constraint solver.
*/
struct Impulse {
private:
// -------------------- Methods -------------------- //
/// Private assignment operator
Impulse& operator=(const Impulse& impulse);
public:
// -------------------- Attributes -------------------- //
/// Linear impulse applied to the first body
const vec3 linearImpulseBody1;
/// Angular impulse applied to the first body
const vec3 angularImpulseBody1;
/// Linear impulse applied to the second body
const vec3 linearImpulseBody2;
/// Angular impulse applied to the second body
const vec3 angularImpulseBody2;
// -------------------- Methods -------------------- //
/// Constructor
Impulse(const vec3& initLinearImpulseBody1, const vec3& initAngularImpulseBody1,
const vec3& initLinearImpulseBody2, const vec3& initAngularImpulseBody2)
: linearImpulseBody1(initLinearImpulseBody1),
angularImpulseBody1(initAngularImpulseBody1),
linearImpulseBody2(initLinearImpulseBody2),
angularImpulseBody2(initAngularImpulseBody2) {
}
/// Copy-constructor
Impulse(const Impulse& impulse)
: linearImpulseBody1(impulse.linearImpulseBody1),
angularImpulseBody1(impulse.angularImpulseBody1),
linearImpulseBody2(impulse.linearImpulseBody2),
angularImpulseBody2(impulse.angularImpulseBody2) {
;
}
};
/**
* @brief Represents an impulse that we can apply to bodies in the contact or constraint solver.
*/
class Impulse {
private:
/// Private assignment operator
Impulse& operator=(const Impulse& _impulse);
public:
const vec3 linearImpulseBody1; //!< Linear impulse applied to the first body
const vec3 angularImpulseBody1; //!< Angular impulse applied to the first body
const vec3 linearImpulseBody2; //!< Linear impulse applied to the second body
const vec3 angularImpulseBody2; //!< Angular impulse applied to the second body
/// Constructor
Impulse(const vec3& _initLinearImpulseBody1,
const vec3& _initAngularImpulseBody1,
const vec3& _initLinearImpulseBody2,
const vec3& _initAngularImpulseBody2):
linearImpulseBody1(_initLinearImpulseBody1),
angularImpulseBody1(_initAngularImpulseBody1),
linearImpulseBody2(_initLinearImpulseBody2),
angularImpulseBody2(_initAngularImpulseBody2) {
}
/// Copy-constructor
Impulse(const Impulse& _impulse):
linearImpulseBody1(_impulse.linearImpulseBody1),
angularImpulseBody1(_impulse.angularImpulseBody1),
linearImpulseBody2(_impulse.linearImpulseBody2),
angularImpulseBody2(_impulse.angularImpulseBody2) {
}
};
}

1
ephysics/engine/Island.cpp
View File

@ -4,7 +4,6 @@
* @license BSD 3 clauses (see license file)
*/
// Libraries
#include <ephysics/engine/Island.hpp>
using namespace ephysics;

139
ephysics/engine/Island.hpp
View File

@ -5,105 +5,58 @@
*/
#pragma once
// Libraries
#include <ephysics/memory/MemoryAllocator.hpp>
#include <ephysics/body/RigidBody.hpp>
#include <ephysics/constraint/Joint.hpp>
#include <ephysics/collision/ContactManifold.hpp>
namespace ephysics {
// Class Island
/**
* An island represent an isolated group of awake bodies that are connected with each other by
* some contraints (contacts or joints).
*/
class Island {
private:
// -------------------- Attributes -------------------- //
/// Array with all the bodies of the island
RigidBody** m_bodies;
/// Array with all the contact manifolds between bodies of the island
ContactManifold** m_contactManifolds;
/// Array with all the joints between bodies of the island
Joint** m_joints;
/// Current number of bodies in the island
uint32_t m_numberBodies;
/// Current number of contact manifold in the island
uint32_t m_numberContactManifolds;
/// Current number of joints in the island
uint32_t m_numberJoints;
/// Reference to the memory allocator
MemoryAllocator& m_memoryAllocator;
/// Number of bytes allocated for the bodies array
size_t m_numberAllocatedBytesBodies;
/// Number of bytes allocated for the contact manifolds array
size_t m_numberAllocatedBytesContactManifolds;
/// Number of bytes allocated for the joints array
size_t m_numberAllocatedBytesJoints;
// -------------------- Methods -------------------- //
/// Private assignment operator
Island& operator=(const Island& island);
/// Private copy-constructor
Island(const Island& island);
public:
// -------------------- Methods -------------------- //
/// Constructor
Island(uint32_t nbMaxBodies, uint32_t nbMaxContactManifolds, uint32_t nbMaxJoints,
MemoryAllocator& memoryAllocator);
/// Destructor
~Island();
/// Add a body int32_to the island
void addBody(RigidBody* body);
/// Add a contact manifold int32_to the island
void addContactManifold(ContactManifold* contactManifold);
/// Add a joint int32_to the island
void addJoint(Joint* joint);
/// Return the number of bodies in the island
uint32_t getNbBodies() const;
/// Return the number of contact manifolds in the island
uint32_t getNbContactManifolds() const;
/// Return the number of joints in the island
uint32_t getNbJoints() const;
/// Return a pointer to the array of bodies
RigidBody** getBodies();
/// Return a pointer to the array of contact manifolds
ContactManifold** getContactManifold();
/// Return a pointer to the array of joints
Joint** getJoints();
// -------------------- Friendship -------------------- //
friend class DynamicsWorld;
};
/**
* @brief An island represent an isolated group of awake bodies that are connected with each other by
* some contraints (contacts or joints).
*/
class Island {
private:
RigidBody** m_bodies; //!< Array with all the bodies of the island
ContactManifold** m_contactManifolds; //!< Array with all the contact manifolds between bodies of the island
Joint** m_joints; //!< Array with all the joints between bodies of the island
uint32_t m_numberBodies; //!< Current number of bodies in the island
uint32_t m_numberContactManifolds; //!< Current number of contact manifold in the island
uint32_t m_numberJoints; //!< Current number of joints in the island
MemoryAllocator& m_memoryAllocator; //!< Reference to the memory allocator
size_t m_numberAllocatedBytesBodies; //!< Number of bytes allocated for the bodies array
size_t m_numberAllocatedBytesContactManifolds; //!< Number of bytes allocated for the contact manifolds array
size_t m_numberAllocatedBytesJoints; //!< Number of bytes allocated for the joints array
/// Private assignment operator
Island& operator=(const Island& island);
/// Private copy-constructor
Island(const Island& island);
public:
/// Constructor
Island(uint32_t nbMaxBodies, uint32_t nbMaxContactManifolds, uint32_t nbMaxJoints,
MemoryAllocator& memoryAllocator);
/// Destructor
~Island();
/// Add a body int32_to the island
void addBody(RigidBody* body);
/// Add a contact manifold int32_to the island
void addContactManifold(ContactManifold* contactManifold);
/// Add a joint int32_to the island
void addJoint(Joint* joint);
/// Return the number of bodies in the island
uint32_t getNbBodies() const;
/// Return the number of contact manifolds in the island
uint32_t getNbContactManifolds() const;
/// Return the number of joints in the island
uint32_t getNbJoints() const;
/// Return a pointer to the array of bodies
RigidBody** getBodies();
/// Return a pointer to the array of contact manifolds
ContactManifold** getContactManifold();
/// Return a pointer to the array of joints
Joint** getJoints();
friend class DynamicsWorld;
};
// Add a body int32_to the island
inline void Island::addBody(RigidBody* body) {

5
ephysics/engine/Material.cpp
View File

@ -23,8 +23,3 @@ Material::Material(const Material& material)
m_rollingResistance(material.m_rollingResistance), m_bounciness(material.m_bounciness) {
}
// Destructor
Material::~Material() {
}

88
ephysics/engine/Material.hpp
View File

@ -4,68 +4,40 @@
* @license BSD 3 clauses (see license file)
*/
#pragma once
// Libraries
#include <cassert>
#include <ephysics/configuration.hpp>
namespace ephysics {
// Class Material
/**
* This class contains the material properties of a rigid body that will be use for
* the dynamics simulation like the friction coefficient or the bounciness of the rigid
* body.
*/
class Material {
private :
// -------------------- Attributes -------------------- //
/// Friction coefficient (positive value)
float m_frictionCoefficient;
/// Rolling resistance factor (positive value)
float m_rollingResistance;
/// Bounciness during collisions (between 0 and 1) where 1 is for a very bouncy body
float m_bounciness;
public :
// -------------------- Methods -------------------- //
/// Constructor
Material();
/// Copy-constructor
Material(const Material& material);
/// Destructor
~Material();
/// Return the bounciness
float getBounciness() const;
/// Set the bounciness.
void setBounciness(float bounciness);
/// Return the friction coefficient
float getFrictionCoefficient() const;
/// Set the friction coefficient.
void setFrictionCoefficient(float frictionCoefficient);
/// Return the rolling resistance factor
float getRollingResistance() const;
/// Set the rolling resistance factor
void setRollingResistance(float rollingResistance);
/// Overloaded assignment operator
Material& operator=(const Material& material);
};
/**
* This class contains the material properties of a rigid body that will be use for
* the dynamics simulation like the friction coefficient or the bounciness of the rigid
* body.
*/
class Material {
private :
float m_frictionCoefficient; //!< Friction coefficient (positive value)
float m_rollingResistance; //!< Rolling resistance factor (positive value)
float m_bounciness; //!< Bounciness during collisions (between 0 and 1) where 1 is for a very bouncy body
public :
/// Constructor
Material();
/// Copy-constructor
Material(const Material& material);
/// Return the bounciness
float getBounciness() const;
/// Set the bounciness.
void setBounciness(float bounciness);
/// Return the friction coefficient
float getFrictionCoefficient() const;
/// Set the friction coefficient.
void setFrictionCoefficient(float frictionCoefficient);
/// Return the rolling resistance factor
float getRollingResistance() const;
/// Set the rolling resistance factor
void setRollingResistance(float rollingResistance);
/// Overloaded assignment operator
Material& operator=(const Material& material);
};
// Return the bounciness
/**

5
ephysics/engine/OverlappingPair.cpp
View File

@ -16,8 +16,3 @@ OverlappingPair::OverlappingPair(ProxyShape* shape1, ProxyShape* shape2, int32_t
m_cachedSeparatingAxis(1.0, 1.0, 1.0) {
}
// Destructor
OverlappingPair::~OverlappingPair() {
}

134
ephysics/engine/OverlappingPair.hpp
View File

@ -4,104 +4,68 @@
* @license BSD 3 clauses (see license file)
*/
#pragma once
// Libraries
#include <ephysics/collision/ContactManifoldSet.hpp>
#include <ephysics/collision/ProxyShape.hpp>
#include <ephysics/collision/shapes/CollisionShape.hpp>
/// ReactPhysics3D namespace
namespace ephysics {
// Type for the overlapping pair ID
typedef std::pair<uint32_t, uint32_t> overlappingpairid;
// Class OverlappingPair
/**
* This class represents a pair of two proxy collision shapes that are overlapping
* during the broad-phase collision detection. It is created when
* the two proxy collision shapes start to overlap and is destroyed when they do not
* overlap anymore. This class contains a contact manifold that
* store all the contact points between the two bodies.
*/
class OverlappingPair {
private:
// -------------------- Attributes -------------------- //
/// Set of persistent contact manifolds
ContactManifoldSet m_contactManifoldSet;
/// Cached previous separating axis
vec3 m_cachedSeparatingAxis;
// -------------------- Methods -------------------- //
/// Private copy-constructor
OverlappingPair(const OverlappingPair& pair);
/// Private assignment operator
OverlappingPair& operator=(const OverlappingPair& pair);
public:
// -------------------- Methods -------------------- //
/// Constructor
OverlappingPair(ProxyShape* shape1, ProxyShape* shape2,
int32_t nbMaxContactManifolds, MemoryAllocator& memoryAllocator);
/// Destructor
~OverlappingPair();
/// Return the pointer to first proxy collision shape
ProxyShape* getShape1() const;
/// Return the pointer to second body
ProxyShape* getShape2() const;
/// Add a contact to the contact cache
void addContact(ContactPoint* contact);
/// Update the contact cache
void update();
/// Return the cached separating axis
vec3 getCachedSeparatingAxis() const;
/// Set the cached separating axis
void setCachedSeparatingAxis(const vec3& axis);
/// Return the number of contacts in the cache
uint32_t getNbContactPoints() const;
/// Return the a reference to the contact manifold set
const ContactManifoldSet& getContactManifoldSet();
/// Clear the contact points of the contact manifold
void clearContactPoints();
/// Return the pair of bodies index
static overlappingpairid computeID(ProxyShape* shape1, ProxyShape* shape2);
/// Return the pair of bodies index of the pair
static bodyindexpair computeBodiesIndexPair(CollisionBody* body1, CollisionBody* body2);
// -------------------- Friendship -------------------- //
friend class DynamicsWorld;
};
// Type for the overlapping pair ID
typedef std::pair<uint32_t, uint32_t> overlappingpairid;
/**
* @brief This class represents a pair of two proxy collision shapes that are overlapping
* during the broad-phase collision detection. It is created when
* the two proxy collision shapes start to overlap and is destroyed when they do not
* overlap anymore. This class contains a contact manifold that
* store all the contact points between the two bodies.
*/
class OverlappingPair {
private:
ContactManifoldSet m_contactManifoldSet; //!< Set of persistent contact manifolds
vec3 m_cachedSeparatingAxis; //!< Cached previous separating axis
/// Private copy-constructor
OverlappingPair(const OverlappingPair& pair);
/// Private assignment operator
OverlappingPair& operator=(const OverlappingPair& pair);
public:
/// Constructor
OverlappingPair(ProxyShape* shape1,
ProxyShape* shape2,
int32_t nbMaxContactManifolds,
MemoryAllocator& memoryAllocator);
/// Return the pointer to first proxy collision shape
ProxyShape* getShape1() const;
/// Return the pointer to second body
ProxyShape* getShape2() const;
/// Add a contact to the contact cache
void addContact(ContactPoint* contact);
/// Update the contact cache
void update();
/// Return the cached separating axis
vec3 getCachedSeparatingAxis() const;
/// Set the cached separating axis
void setCachedSeparatingAxis(const vec3& axis);
/// Return the number of contacts in the cache
uint32_t getNbContactPoints() const;
/// Return the a reference to the contact manifold set
const ContactManifoldSet& getContactManifoldSet();
/// Clear the contact points of the contact manifold
void clearContactPoints();
/// Return the pair of bodies index
static overlappingpairid computeID(ProxyShape* shape1, ProxyShape* shape2);
/// Return the pair of bodies index of the pair
static bodyindexpair computeBodiesIndexPair(CollisionBody* body1, CollisionBody* body2);
friend class DynamicsWorld;
};
// Return the pointer to first body
inline ProxyShape* OverlappingPair::getShape1() const {
return m_contactManifoldSet.getShape1();
}
}
// Return the pointer to second body
inline ProxyShape* OverlappingPair::getShape2() const {
return m_contactManifoldSet.getShape2();
}
}
// Add a contact to the contact manifold
inline void OverlappingPair::addContact(ContactPoint* contact) {

372
ephysics/engine/Profiler.hpp
View File

@ -6,254 +6,154 @@
#pragma once
#ifdef IS_PROFILING_ACTIVE
// Libraries
#include <ephysics/configuration.hpp>
#include <ephysics/Timer.hpp>
/// ReactPhysics3D namespace
namespace ephysics {
// Class ProfileNode
/**
* It represents a profile sample in the profiler tree.
*/
class ProfileNode {
private :
// -------------------- Attributes -------------------- //
/// Name of the node
const char* m_name;
/// Total number of calls of this node
uint32_t m_numberTotalCalls;
/// Starting time of the sampling of corresponding block of code
long double m_startTime;
/// Total time spent in the block of code
long double m_totalTime;
/// Recursion counter
int32_t m_recursionCounter;
/// Pointer to the parent node
ProfileNode* m_parentNode;
/// Pointer to a child node
ProfileNode* m_childNode;
/// Pointer to a sibling node
ProfileNode* m_siblingNode;
public :
// -------------------- Methods -------------------- //
/// Constructor
ProfileNode(const char* name, ProfileNode* parentNode);
/// Destructor
~ProfileNode();
/// Return a pointer to a sub node
ProfileNode* findSubNode(const char* name);
/// Return a pointer to the parent node
ProfileNode* getParentNode();
/// Return a pointer to a sibling node
ProfileNode* getSiblingNode();
/// Return a pointer to a child node
ProfileNode* getChildNode();
/// Return the name of the node
const char* getName();
/// Return the total number of call of the corresponding block of code
uint32_t getNbTotalCalls() const;
/// Return the total time spent in the block of code
long double getTotalTime() const;
/// Called when we enter the block of code corresponding to this profile node
void enterBlockOfCode();
/// Called when we exit the block of code corresponding to this profile node
bool exitBlockOfCode();
/// Reset the profiling of the node
void reset();
/// Destroy the node
void destroy();
};
// Class ProfileNodeIterator
/**
* This class allows us to iterator over the profiler tree.
*/
class ProfileNodeIterator {
private :
// -------------------- Attributes -------------------- //
/// Current parent node
ProfileNode* m_currentParentNode;
/// Current child node
ProfileNode* m_currentChildNode;
public :
// -------------------- Methods -------------------- //
/// Constructor
ProfileNodeIterator(ProfileNode* startingNode);
/// Go to the first node
void first();
/// Go to the next node
void next();
/// Enter a given child node
void enterChild(int32_t index);
/// Enter a given parent node
void enterParent();
/// Return true if we are at the root of the profiler tree
bool isRoot();
/// Return true if we are at the end of a branch of the profiler tree
bool isEnd();
/// Return the name of the current node
const char* getCurrentName();
/// Return the total time of the current node
long double getCurrentTotalTime();
/// Return the total number of calls of the current node
uint32_t getCurrentNbTotalCalls();
/// Return the name of the current parent node
const char* getCurrentParentName();
/// Return the total time of the current parent node
long double getCurrentParentTotalTime();
/// Return the total number of calls of the current parent node
uint32_t getCurrentParentNbTotalCalls();
};
// Class Profiler
/**
* This is the main class of the profiler. This profiler is based on "Real-Time Hierarchical
* Profiling" article from "Game Programming Gems 3" by Greg Hjelstrom and Byon Garrabrant.
*/
class Profiler {
private :
// -------------------- Attributes -------------------- //
/// Root node of the profiler tree
static ProfileNode m_rootNode;
/// Current node in the current execution
static ProfileNode* m_currentNode;
/// Frame counter
static uint32_t m_frameCounter;
/// Starting profiling time
static long double m_profilingStartTime;
/// Recursively print32_t the report of a given node of the profiler tree
static void print32_tRecursiveNodeReport(ProfileNodeIterator* iterator,
int32_t spacing,
std::ostream& outputStream);
public :
// -------------------- Methods -------------------- //
/// Method called when we want to start profiling a block of code.
static void startProfilingBlock(const char *name);
/// Method called at the end of the scope where the
/// startProfilingBlock() method has been called.
static void stopProfilingBlock();
/// Reset the timing data of the profiler (but not the profiler tree structure)
static void reset();
/// Return the number of frames
static uint32_t getNbFrames();
/// Return the elasped time since the start/reset of the profiling
static long double getElapsedTimeSinceStart();
/// Increment the frame counter
static void incrementFrameCounter();
/// Return an iterator over the profiler tree starting at the root
static ProfileNodeIterator* getIterator();
/// Print32_t the report of the profiler in a given output stream
static void print32_tReport(std::ostream& outputStream);
/// Destroy a previously allocated iterator
static void destroyIterator(ProfileNodeIterator* iterator);
/// Destroy the profiler (release the memory)
static void destroy();
};
// Class ProfileSample
/**
* This class is used to represent a profile sample. It is constructed at the
* beginning of a code block we want to profile and destructed at the end of the
* scope to profile.
*/
class ProfileSample {
public :
// -------------------- Methods -------------------- //
/// Constructor
ProfileSample(const char* name) {
// Ask the profiler to start profiling a block of code
Profiler::startProfilingBlock(name);
}
/// Destructor
~ProfileSample() {
// Tell the profiler to stop profiling a block of code
Profiler::stopProfilingBlock();
}
};
/**
* @brief It represents a profile sample in the profiler tree.
*/
class ProfileNode {
private :
const char* m_name; //!< Name of the node
uint32_t m_numberTotalCalls; //!< Total number of calls of this node
long double m_startTime; //!< Starting time of the sampling of corresponding block of code
long double m_totalTime; //!< Total time spent in the block of code
int32_t m_recursionCounter; //!< Recursion counter
ProfileNode* m_parentNode; //!< Pointer to the parent node
ProfileNode* m_childNode; //!< Pointer to a child node
ProfileNode* m_siblingNode; //!< Pointer to a sibling node
public :
/// Constructor
ProfileNode(const char* name, ProfileNode* parentNode);
/// Destructor
~ProfileNode();
/// Return a pointer to a sub node
ProfileNode* findSubNode(const char* name);
/// Return a pointer to the parent node
ProfileNode* getParentNode();
/// Return a pointer to a sibling node
ProfileNode* getSiblingNode();
/// Return a pointer to a child node
ProfileNode* getChildNode();
/// Return the name of the node
const char* getName();
/// Return the total number of call of the corresponding block of code
uint32_t getNbTotalCalls() const;
/// Return the total time spent in the block of code
long double getTotalTime() const;
/// Called when we enter the block of code corresponding to this profile node
void enterBlockOfCode();
/// Called when we exit the block of code corresponding to this profile node
bool exitBlockOfCode();
/// Reset the profiling of the node
void reset();
/// Destroy the node
void destroy();
};
/**
* @brief This class allows us to iterator over the profiler tree.
*/
class ProfileNodeIterator {
private :
ProfileNode* m_currentParentNode; //!< Current parent node
ProfileNode* m_currentChildNode; //!< Current child node
public :
/// Constructor
ProfileNodeIterator(ProfileNode* startingNode);
/// Go to the first node
void first();
/// Go to the next node
void next();
/// Enter a given child node
void enterChild(int32_t index);
/// Enter a given parent node
void enterParent();
/// Return true if we are at the root of the profiler tree
bool isRoot();
/// Return true if we are at the end of a branch of the profiler tree
bool isEnd();
/// Return the name of the current node
const char* getCurrentName();
/// Return the total time of the current node
long double getCurrentTotalTime();
/// Return the total number of calls of the current node
uint32_t getCurrentNbTotalCalls();
/// Return the name of the current parent node
const char* getCurrentParentName();
/// Return the total time of the current parent node
long double getCurrentParentTotalTime();
/// Return the total number of calls of the current parent node
uint32_t getCurrentParentNbTotalCalls();
};
/**
* @brief This is the main class of the profiler. This profiler is based on "Real-Time Hierarchical
* Profiling" article from "Game Programming Gems 3" by Greg Hjelstrom and Byon Garrabrant.
*/
class Profiler {
private :
static ProfileNode m_rootNode; //!< Root node of the profiler tree
static ProfileNode* m_currentNode; //!< Current node in the current execution
static uint32_t m_frameCounter; //!< Frame counter
static long double m_profilingStartTime; //!< Starting profiling time
private:
static void print32_tRecursiveNodeReport(ProfileNodeIterator* iterator,
int32_t spacing,
std::ostream& outputStream);
public :
/// Method called when we want to start profiling a block of code.
static void startProfilingBlock(const char *name);
/// Method called at the end of the scope where the
/// startProfilingBlock() method has been called.
static void stopProfilingBlock();
/// Reset the timing data of the profiler (but not the profiler tree structure)
static void reset();
/// Return the number of frames
static uint32_t getNbFrames();
/// Return the elasped time since the start/reset of the profiling
static long double getElapsedTimeSinceStart();
/// Increment the frame counter
static void incrementFrameCounter();
/// Return an iterator over the profiler tree starting at the root
static ProfileNodeIterator* getIterator();
/// Print32_t the report of the profiler in a given output stream
static void print32_tReport(std::ostream& outputStream);
/// Destroy a previously allocated iterator
static void destroyIterator(ProfileNodeIterator* iterator);
/// Destroy the profiler (release the memory)
static void destroy();
};
/**
* @brief This class is used to represent a profile sample. It is constructed at the
* beginning of a code block we want to profile and destructed at the end of the
* scope to profile.
*/
class ProfileSample {
public :
/// Constructor
ProfileSample(const char* name) {
// Ask the profiler to start profiling a block of code
Profiler::startProfilingBlock(name);
}
/// Destructor
~ProfileSample() {
// Tell the profiler to stop profiling a block of code
Profiler::stopProfilingBlock();
}
};
// Use this macro to start profile a block of code
#define PROFILE(name) ProfileSample profileSample(name)
// Return true if we are at the root of the profiler tree
inline bool ProfileNodeIterator::isRoot() {
return (m_currentParentNode->getParentNode() == NULL);
return (m_currentParentNode->getParentNode() == nullptr);
}
// Return true if we are at the end of a branch of the profiler tree
inline bool ProfileNodeIterator::isEnd() {
return (m_currentChildNode == NULL);
return (m_currentChildNode == nullptr);
}
// Return the name of the current node
@ -359,7 +259,7 @@ inline void Profiler::destroy() {
}
#else // In profile is not active
#else
// Empty macro in case profiling is not active
#define PROFILE(name)

123
ephysics/engine/Timer.hpp
View File

@ -22,85 +22,50 @@
/// Namespace ReactPhysics3D
namespace ephysics {
// Class Timer
/**
* This class will take care of the time in the physics engine. It
* uses functions that depend on the current platform to get the
* current time.
*/
class Timer {
private :
// -------------------- Attributes -------------------- //
/// Timestep dt of the physics engine (timestep > 0.0)
double m_timeStep;
/// Last time the timer has been updated
long double m_lastUpdateTime;
/// Time difference between the two last timer update() calls
long double m_deltaTime;
/// Used to fix the time step and avoid strange time effects
double m_accumulator;
/// True if the timer is running
bool m_isRunning;
// -------------------- Methods -------------------- //
/// Private copy-constructor
Timer(const Timer& timer);
/// Private assignment operator
Timer& operator=(const Timer& timer);
public :
// -------------------- Methods -------------------- //
/// Constructor
Timer(double timeStep);
/// Destructor
virtual ~Timer();
/// Return the timestep of the physics engine
double getTimeStep() const;
/// Set the timestep of the physics engine
void setTimeStep(double timeStep);
/// Return the current time of the physics engine
long double getPhysicsTime() const;
/// Start the timer
void start();
/// Stop the timer
void stop();
/// Return true if the timer is running
bool getIsRunning() const;
/// True if it's possible to take a new step
bool isPossibleToTakeStep() const;
/// Compute the time since the last update() call and add it to the accumulator
void update();
/// Take a new step => update the timer by adding the timeStep value to the current time
void nextStep();
/// Compute the int32_terpolation factor
float computeInterpolationFactor();
/// Return the current time of the system in seconds
static long double getCurrentSystemTime();
};
/**
* @brief This class will take care of the time in the physics engine. It
* uses functions that depend on the current platform to get the
* current time.
*/
class Timer {
private :
double m_timeStep; //!< Timestep dt of the physics engine (timestep > 0.0)
long double m_lastUpdateTime; //!< Last time the timer has been updated
long double m_deltaTime; //!< Time difference between the two last timer update() calls
double m_accumulator; //!< Used to fix the time step and avoid strange time effects
bool m_isRunning; //!< True if the timer is running
/// Private copy-constructor
Timer(const Timer& timer);
/// Private assignment operator
Timer& operator=(const Timer& timer);
public :
/// Constructor
Timer(double timeStep);
/// Destructor
virtual ~Timer();
/// Return the timestep of the physics engine
double getTimeStep() const;
/// Set the timestep of the physics engine
void setTimeStep(double timeStep);
/// Return the current time of the physics engine
long double getPhysicsTime() const;
/// Start the timer
void start();
/// Stop the timer
void stop();
/// Return true if the timer is running
bool getIsRunning() const;
/// True if it's possible to take a new step
bool isPossibleToTakeStep() const;
/// Compute the time since the last update() call and add it to the accumulator
void update();
/// Take a new step => update the timer by adding the timeStep value to the current time
void nextStep();
/// Compute the int32_terpolation factor
float computeInterpolationFactor();
/// Return the current time of the system in seconds
static long double getCurrentSystemTime();
};
// Return the timestep of the physics engine
inline double Timer::getTimeStep() const {