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

308
ephysics/engine/CollisionWorld.hpp
View File

@ -5,7 +5,6 @@
*/ */
#pragma once #pragma once
// Libraries
#include <vector> #include <vector>
#include <set> #include <set>
#include <list> #include <list>
@ -21,194 +20,125 @@
#include <ephysics/memory/MemoryAllocator.hpp> #include <ephysics/memory/MemoryAllocator.hpp>
#include <ephysics/engine/EventListener.hpp> #include <ephysics/engine/EventListener.hpp>
/// Namespace ephysics
namespace ephysics { namespace ephysics {
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;
};
// Declarations /**
class CollisionCallback; * @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
// Class CollisionWorld * the notifyRaycastHit() method. This method will be called for each ProxyShape
/** * that is hit by the ray.
* 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 class CollisionCallback {
* world, the bodies movement is not computed using the laws of physics. public:
*/ /**
class CollisionWorld { * @brief Virtualisation of the destructor.
*/
protected : virtual ~CollisionCallback() = default;
/**
// -------------------- Attributes -------------------- // * @brief This method will be called for contact.
* @param[in] _contactPointInfo Contact information property.
/// Reference to the collision detection */
CollisionDetection m_collisionDetection; virtual void notifyContact(const ContactPointInfo& _contactPointInfo)=0;
};
/// 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;
};
} }

12
ephysics/engine/ConstraintSolver.cpp
View File

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

274
ephysics/engine/ConstraintSolver.hpp
View File

@ -5,7 +5,6 @@
*/ */
#pragma once #pragma once
// Libraries
#include <ephysics/configuration.hpp> #include <ephysics/configuration.hpp>
#include <ephysics/mathematics/mathematics.hpp> #include <ephysics/mathematics/mathematics.hpp>
#include <ephysics/constraint/Joint.hpp> #include <ephysics/constraint/Joint.hpp>
@ -14,169 +13,122 @@
#include <set> #include <set>
namespace ephysics { namespace ephysics {
/**
* 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){
// Structure ConstraintSolverData }
/** };
* This structure contains data from the constraint solver that are used to solve
* each joint constraint.
*/
struct ConstraintSolverData {
public : /**
* @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.
/// Current time step of the simulation * An impulse P = F * dt where F is a force and dt is the timestep. We can apply impulses a
float timeStep; * 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.
/// Array with the bodies linear velocities *
vec3* linearVelocities; * --- Step 1 ---
*
/// Array with the bodies angular velocities * First, we int32_tegrate the applied force F_a acting of each rigid body (like gravity, ...) and
vec3* angularVelocities; * we obtain some new velocities v2' that tends to violate the constraints.
*
/// Reference to the bodies positions * v2' = v1 + dt * M^-1 * F_a
vec3* positions; *
* where M is a matrix that contains mass and inertia tensor information.
/// Reference to the bodies orientations *
etk::Quaternion* orientations; * --- Step 2 ---
*
/// Reference to the map that associates rigid body to their index * During the second step, we iterate over all the constraints for a certain number of
/// in the constrained velocities array * iterations and for each constraint we compute the impulse to apply to the bodies needed
const std::map<RigidBody*, uint32_t>& mapBodyToConstrainedVelocityIndex; * 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
/// True if warm starting of the solver is active * P_c is the constraint impulse to apply to the body. Therefore, we have
bool isWarmStartingActive; * 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
/// Constructor * Lagrange multiplier lambda, we can compute the impulse P_c = J^t * lambda * dt to apply to
ConstraintSolverData(const std::map<RigidBody*, uint32_t>& refMapBodyToConstrainedVelocityIndex) * the bodies to satisfy the constraint.
:linearVelocities(NULL), angularVelocities(NULL), *
positions(NULL), orientations(NULL), * --- Step 3 ---
mapBodyToConstrainedVelocityIndex(refMapBodyToConstrainedVelocityIndex){ *
* 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
// Class ConstraintSolver * Lagrange multiplier with the timestep dt. Therefore, in the following code, when we use
/** * lambda, we mean (lambda * dt).
* 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 * We are using the accumulated impulse technique that is also described in the slides from
* described by Erin Catto in his GDC slides (http://code.google.com/p/box2d/downloads/list). * Erin Catto.
* *
* A constraint between two bodies is represented by a function C(x) which is equal to zero * We are also using warm starting. The idea is to warm start the solver at the beginning of
* when the constraint is satisfied. The condition C(x)=0 describes a valid position and the * each step by applying the last impulstes for the constraints that we already existing at the
* condition dC(x)/dt=0 describes a valid velocity. We have dC(x)/dt = Jv + b = 0 where J is * previous step. This allows the iterative solver to converge faster towards the solution.
* 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 * For contact constraints, we are also using split impulses so that the position correction
* bodies to keep the constraint satisfied. Note that from the virtual work principle, we have * that uses Baumgarte stabilization does not change the momentum of the bodies.
* 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 * There are two ways to apply the friction constraints. Either the friction constraints are
* multiplier lambda. * 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
* An impulse P = F * dt where F is a force and dt is the timestep. We can apply impulses a * two constraints (two tangential friction directions) and if we solve the friction
* body to change its velocity. The idea of the Sequential Impulse technique is to apply * constraints at the center of the contact manifold, we need two constraints for tangential
* impulses to bodies of each constraints in order to keep the constraint satisfied. * friction but also another twist friction constraint to prevent spin of the body around the
* * contact manifold center.
* --- Step 1 --- */
* class ConstraintSolver {
* First, we int32_tegrate the applied force F_a acting of each rigid body (like gravity, ...) and private :
* we obtain some new velocities v2' that tends to violate the constraints. 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
* v2' = v1 + dt * M^-1 * F_a 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
* where M is a matrix that contains mass and inertia tensor information. public :
* /// Constructor
* --- Step 2 --- ConstraintSolver(const std::map<RigidBody*, uint32_t>& mapBodyToVelocityIndex);
* /// Initialize the constraint solver for a given island
* During the second step, we iterate over all the constraints for a certain number of void initializeForIsland(float dt, Island* island);
* iterations and for each constraint we compute the impulse to apply to the bodies needed /// Solve the constraints
* so that the new velocity of the bodies satisfy Jv + b = 0. From the Newton law, we know that void solveVelocityConstraints(Island* island);
* M * deltaV = P_c where M is the mass of the body, deltaV is the difference of velocity and /// Solve the position constraints
* P_c is the constraint impulse to apply to the body. Therefore, we have void solvePositionConstraints(Island* island);
* v2 = v2' + M^-1 * P_c. For each constraint, we can compute the Lagrange multiplier lambda /// Return true if the Non-Linear-Gauss-Seidel position correction technique is active
* using : lambda = -m_c (Jv2' + b) where m_c = 1 / (J * M^-1 * J^t). Now that we have the bool getIsNonLinearGaussSeidelPositionCorrectionActive() const;
* Lagrange multiplier lambda, we can compute the impulse P_c = J^t * lambda * dt to apply to /// Enable/Disable the Non-Linear-Gauss-Seidel position correction technique.
* the bodies to satisfy the constraint. void setIsNonLinearGaussSeidelPositionCorrectionActive(bool isActive);
* /// Set the constrained velocities arrays
* --- Step 3 --- void setConstrainedVelocitiesArrays(vec3* constrainedLinearVelocities,
* vec3* constrainedAngularVelocities);
* In the third step, we int32_tegrate the new position x2 of the bodies using the new velocities /// Set the constrained positions/orientations arrays
* v2 computed in the second step with : x2 = x1 + dt * v2. void setConstrainedPositionsArrays(vec3* constrainedPositions,
* etk::Quaternion* constrainedOrientations);
* 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);
};
// Set the constrained velocities arrays // Set the constrained velocities arrays
inline void ConstraintSolver::setConstrainedVelocitiesArrays(vec3* constrainedLinearVelocities, 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 // Constructor
ContactSolver::ContactSolver(const std::map<RigidBody*, uint32_t>& mapBodyToVelocityIndex) ContactSolver::ContactSolver(const std::map<RigidBody*, uint32_t>& mapBodyToVelocityIndex)
:m_splitLinearVelocities(nullptr), m_splitAngularVelocities(nullptr), :m_splitLinearVelocities(nullptr), m_splitAngularVelocities(nullptr),
mContactConstraints(nullptr), mLinearVelocities(nullptr), mAngularVelocities(nullptr), m_contactConstraints(nullptr), m_linearVelocities(nullptr), m_angularVelocities(nullptr),
m_mapBodyToConstrainedVelocityIndex(mapBodyToVelocityIndex), m_mapBodyToConstrainedVelocityIndex(mapBodyToVelocityIndex),
mIsWarmStartingActive(true), mIsSplitImpulseActive(true), m_isWarmStartingActive(true), m_isSplitImpulseActive(true),
mIsSolveFrictionAtContactManifoldCenterActive(true) { m_isSolveFrictionAtContactManifoldCenterActive(true) {
} }
@ -50,8 +50,8 @@ void ContactSolver::initializeForIsland(float dt, Island* island) {
m_numberContactManifolds = island->getNbContactManifolds(); m_numberContactManifolds = island->getNbContactManifolds();
mContactConstraints = new ContactManifoldSolver[m_numberContactManifolds]; m_contactConstraints = new ContactManifoldSolver[m_numberContactManifolds];
assert(mContactConstraints != nullptr); assert(m_contactConstraints != nullptr);
// For each contact manifold of the island // For each contact manifold of the island
ContactManifold** contactManifolds = island->getContactManifold(); ContactManifold** contactManifolds = island->getContactManifold();
@ -59,7 +59,7 @@ void ContactSolver::initializeForIsland(float dt, Island* island) {
ContactManifold* externalManifold = contactManifolds[i]; ContactManifold* externalManifold = contactManifolds[i];
ContactManifoldSolver& int32_ternalManifold = mContactConstraints[i]; ContactManifoldSolver& int32_ternalManifold = m_contactConstraints[i];
assert(externalManifold->getNbContactPoints() > 0); assert(externalManifold->getNbContactPoints() > 0);
@ -90,7 +90,7 @@ void ContactSolver::initializeForIsland(float dt, Island* island) {
int32_ternalManifold.isBody2DynamicType = body2->getType() == DYNAMIC; int32_ternalManifold.isBody2DynamicType = body2->getType() == DYNAMIC;
// If we solve the friction constraints at the center of the contact manifold // 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.frictionPointBody1 = vec3(0.0f,0.0f,0.0f);
int32_ternalManifold.frictionPointBody2 = 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); contactPoint.rollingResistanceImpulse = vec3(0.0f,0.0f,0.0f);
// If we solve the friction constraints at the center of the contact manifold // If we solve the friction constraints at the center of the contact manifold
if (mIsSolveFrictionAtContactManifoldCenterActive) { if (m_isSolveFrictionAtContactManifoldCenterActive) {
int32_ternalManifold.frictionPointBody1 += p1; int32_ternalManifold.frictionPointBody1 += p1;
int32_ternalManifold.frictionPointBody2 += p2; int32_ternalManifold.frictionPointBody2 += p2;
} }
} }
// If we solve the friction constraints at the center of the contact manifold // 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.frictionPointBody1 /=static_cast<float>(int32_ternalManifold.nbContacts);
int32_ternalManifold.frictionPointBody2 /=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(); int32_ternalManifold.oldFrictionvec2 = externalManifold->getFrictionvec2();
// If warm starting is active // If warm starting is active
if (mIsWarmStartingActive) { if (m_isWarmStartingActive) {
// Initialize the accumulated impulses with the previous step accumulated impulses // Initialize the accumulated impulses with the previous step accumulated impulses
int32_ternalManifold.friction1Impulse = externalManifold->getFrictionImpulse1(); int32_ternalManifold.friction1Impulse = externalManifold->getFrictionImpulse1();
@ -167,22 +167,22 @@ void ContactSolver::initializeContactConstraints() {
// For each contact constraint // For each contact constraint
for (uint32_t c=0; c<m_numberContactManifolds; c++) { 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 // Get the inertia tensors of both bodies
etk::Matrix3x3& I1 = manifold.inverseInertiaTensorBody1; etk::Matrix3x3& I1 = manifold.inverseInertiaTensorBody1;
etk::Matrix3x3& I2 = manifold.inverseInertiaTensorBody2; etk::Matrix3x3& I2 = manifold.inverseInertiaTensorBody2;
// If we solve the friction constraints at the center of the contact manifold // 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); manifold.normal = vec3(0.0, 0.0, 0.0);
} }
// Get the velocities of the bodies // Get the velocities of the bodies
const vec3& v1 = mLinearVelocities[manifold.indexBody1]; const vec3& v1 = m_linearVelocities[manifold.indexBody1];
const vec3& w1 = mAngularVelocities[manifold.indexBody1]; const vec3& w1 = m_angularVelocities[manifold.indexBody1];
const vec3& v2 = mLinearVelocities[manifold.indexBody2]; const vec3& v2 = m_linearVelocities[manifold.indexBody2];
const vec3& w2 = mAngularVelocities[manifold.indexBody2]; const vec3& w2 = m_angularVelocities[manifold.indexBody2];
// For each contact point constraint // For each contact point constraint
for (uint32_t i=0; i<manifold.nbContacts; i++) { for (uint32_t i=0; i<manifold.nbContacts; i++) {
@ -205,7 +205,7 @@ void ContactSolver::initializeContactConstraints() {
0.0f; 0.0f;
// If we do not solve the friction constraints at the center of the contact manifold // If we do not solve the friction constraints at the center of the contact manifold
if (!mIsSolveFrictionAtContactManifoldCenterActive) { if (!m_isSolveFrictionAtContactManifoldCenterActive) {
// Compute the friction vectors // Compute the friction vectors
computeFrictionVectors(deltaV, contactPoint); computeFrictionVectors(deltaV, contactPoint);
@ -246,7 +246,7 @@ void ContactSolver::initializeContactConstraints() {
} }
// If the warm starting of the contact solver is active // If the warm starting of the contact solver is active
if (mIsWarmStartingActive) { if (m_isWarmStartingActive) {
// Get the cached accumulated impulses from the previous step // Get the cached accumulated impulses from the previous step
contactPoint.penetrationImpulse = externalContact->getPenetrationImpulse(); contactPoint.penetrationImpulse = externalContact->getPenetrationImpulse();
@ -259,7 +259,7 @@ void ContactSolver::initializeContactConstraints() {
contactPoint.penetrationSplitImpulse = 0.0; contactPoint.penetrationSplitImpulse = 0.0;
// If we solve the friction constraints at the center of the contact manifold // If we solve the friction constraints at the center of the contact manifold
if (mIsSolveFrictionAtContactManifoldCenterActive) { if (m_isSolveFrictionAtContactManifoldCenterActive) {
manifold.normal += contactPoint.normal; 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 we solve the friction constraints at the center of the contact manifold
if (mIsSolveFrictionAtContactManifoldCenterActive) { if (m_isSolveFrictionAtContactManifoldCenterActive) {
manifold.normal.normalize(); manifold.normal.normalize();
@ -320,12 +320,12 @@ void ContactSolver::initializeContactConstraints() {
void ContactSolver::warmStart() { void ContactSolver::warmStart() {
// Check that warm starting is active // Check that warm starting is active
if (!mIsWarmStartingActive) return; if (!m_isWarmStartingActive) return;
// For each constraint // For each constraint
for (uint32_t c=0; c<m_numberContactManifolds; c++) { for (uint32_t c=0; c<m_numberContactManifolds; c++) {
ContactManifoldSolver& contactManifold = mContactConstraints[c]; ContactManifoldSolver& contactManifold = m_contactConstraints[c];
bool atLeastOneRestingContactPoint = false; bool atLeastOneRestingContactPoint = false;
@ -348,7 +348,7 @@ void ContactSolver::warmStart() {
applyImpulse(impulsePenetration, contactManifold); applyImpulse(impulsePenetration, contactManifold);
// If we do not solve the friction constraints at the center of the contact manifold // 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 // Project the old friction impulses (with old friction vectors) int32_to
// the new friction vectors to get the new friction impulses // 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 // 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 // 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 // Project the old friction impulses (with old friction vectors) int32_to the new friction
// vectors to get the new friction impulses // vectors to get the new friction impulses
@ -497,15 +497,15 @@ void ContactSolver::solve() {
// For each contact manifold // For each contact manifold
for (uint32_t c=0; c<m_numberContactManifolds; c++) { for (uint32_t c=0; c<m_numberContactManifolds; c++) {
ContactManifoldSolver& contactManifold = mContactConstraints[c]; ContactManifoldSolver& contactManifold = m_contactConstraints[c];
float sumPenetrationImpulse = 0.0; float sumPenetrationImpulse = 0.0;
// Get the constrained velocities // Get the constrained velocities
const vec3& v1 = mLinearVelocities[contactManifold.indexBody1]; const vec3& v1 = m_linearVelocities[contactManifold.indexBody1];
const vec3& w1 = mAngularVelocities[contactManifold.indexBody1]; const vec3& w1 = m_angularVelocities[contactManifold.indexBody1];
const vec3& v2 = mLinearVelocities[contactManifold.indexBody2]; const vec3& v2 = m_linearVelocities[contactManifold.indexBody2];
const vec3& w2 = mAngularVelocities[contactManifold.indexBody2]; const vec3& w2 = m_angularVelocities[contactManifold.indexBody2];
for (uint32_t i=0; i<contactManifold.nbContacts; i++) { for (uint32_t i=0; i<contactManifold.nbContacts; i++) {
@ -519,14 +519,14 @@ void ContactSolver::solve() {
float Jv = deltaVDotN; float Jv = deltaVDotN;
// Compute the bias "b" of the constraint // 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; float biasPenetrationDepth = 0.0;
if (contactPoint.penetrationDepth > SLOP) biasPenetrationDepth = -(beta/m_timeStep) * if (contactPoint.penetrationDepth > SLOP) biasPenetrationDepth = -(beta/m_timeStep) *
max(0.0f, float(contactPoint.penetrationDepth - SLOP)); max(0.0f, float(contactPoint.penetrationDepth - SLOP));
float b = biasPenetrationDepth + contactPoint.restitutionBias; float b = biasPenetrationDepth + contactPoint.restitutionBias;
// Compute the Lagrange multiplier lambda // Compute the Lagrange multiplier lambda
if (mIsSplitImpulseActive) { if (m_isSplitImpulseActive) {
deltaLambda = - (Jv + contactPoint.restitutionBias) * deltaLambda = - (Jv + contactPoint.restitutionBias) *
contactPoint.inversePenetrationMass; contactPoint.inversePenetrationMass;
} }
@ -548,7 +548,7 @@ void ContactSolver::solve() {
sumPenetrationImpulse += contactPoint.penetrationImpulse; sumPenetrationImpulse += contactPoint.penetrationImpulse;
// If the split impulse position correction is active // If the split impulse position correction is active
if (mIsSplitImpulseActive) { if (m_isSplitImpulseActive) {
// Split impulse (position correction) // Split impulse (position correction)
const vec3& v1Split = m_splitLinearVelocities[contactManifold.indexBody1]; 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 we do not solve the friction constraints at the center of the contact manifold
if (!mIsSolveFrictionAtContactManifoldCenterActive) { if (!m_isSolveFrictionAtContactManifoldCenterActive) {
// --------- Friction 1 --------- // // --------- Friction 1 --------- //
@ -650,7 +650,7 @@ void ContactSolver::solve() {
} }
// If we solve the friction constraints at the center of the contact manifold // 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 ------ // // ------ First friction constraint at the center of the contact manifol ------ //
@ -768,7 +768,7 @@ void ContactSolver::storeImpulses() {
// For each contact manifold // For each contact manifold
for (uint32_t c=0; c<m_numberContactManifolds; c++) { 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++) { for (uint32_t i=0; i<manifold.nbContacts; i++) {
@ -797,15 +797,15 @@ void ContactSolver::applyImpulse(const Impulse& impulse,
const ContactManifoldSolver& manifold) { const ContactManifoldSolver& manifold) {
// Update the velocities of the body 1 by applying the impulse P // 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; impulse.linearImpulseBody1;
mAngularVelocities[manifold.indexBody1] += manifold.inverseInertiaTensorBody1 * m_angularVelocities[manifold.indexBody1] += manifold.inverseInertiaTensorBody1 *
impulse.angularImpulseBody1; impulse.angularImpulseBody1;
// Update the velocities of the body 1 by applying the impulse P // 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; impulse.linearImpulseBody2;
mAngularVelocities[manifold.indexBody2] += manifold.inverseInertiaTensorBody2 * m_angularVelocities[manifold.indexBody2] += manifold.inverseInertiaTensorBody2 *
impulse.angularImpulseBody2; impulse.angularImpulseBody2;
} }
@ -889,8 +889,8 @@ void ContactSolver::computeFrictionVectors(const vec3& deltaVelocity,
// Clean up the constraint solver // Clean up the constraint solver
void ContactSolver::cleanup() { void ContactSolver::cleanup() {
if (mContactConstraints != nullptr) { if (m_contactConstraints != nullptr) {
delete[] mContactConstraints; delete[] m_contactConstraints;
mContactConstraints = nullptr; m_contactConstraints = nullptr;
} }
} }

644
ephysics/engine/ContactSolver.hpp
View File

@ -5,7 +5,6 @@
*/ */
#pragma once #pragma once
// Libraries
#include <ephysics/constraint/ContactPoint.hpp> #include <ephysics/constraint/ContactPoint.hpp>
#include <ephysics/configuration.hpp> #include <ephysics/configuration.hpp>
#include <ephysics/constraint/Joint.hpp> #include <ephysics/constraint/Joint.hpp>
@ -15,419 +14,228 @@
#include <map> #include <map>
#include <set> #include <set>
/// ReactPhysics3D namespace
namespace ephysics { namespace ephysics {
/**
* @brief This class represents the contact solver that is used to solve rigid bodies contacts.
// Class Contact Solver * 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).
* 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 * A constraint between two bodies is represented by a function C(x) which is equal to zero
* Erin Catto in his GDC slides (http://code.google.com/p/box2d/downloads/list). * 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
* A constraint between two bodies is represented by a function C(x) which is equal to zero * the Jacobian matrix of the constraint, v is a vector that contains the velocity of both
* when the constraint is satisfied. The condition C(x)=0 describes a valid position and the * bodies and b is the constraint bias. We are looking for a force F_c that will act on the
* condition dC(x)/dt=0 describes a valid velocity. We have dC(x)/dt = Jv + b = 0 where J is * bodies to keep the constraint satisfied. Note that from the virtual work principle, we have
* the Jacobian matrix of the constraint, v is a vector that contains the velocity of both * F_c = J^t * lambda where J^t is the transpose of the Jacobian matrix and lambda is a
* bodies and b is the constraint bias. We are looking for a force F_c that will act on the * Lagrange multiplier. Therefore, finding the force F_c is equivalent to finding the Lagrange
* bodies to keep the constraint satisfied. Note that from the virtual work principle, we have * multiplier lambda.
* 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 * An impulse P = F * dt where F is a force and dt is the timestep. We can apply impulses a
* multiplier lambda. * 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.
* 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 * --- Step 1 ---
* impulses to bodies of each constraints in order to keep the constraint satisfied. *
* * First, we int32_tegrate the applied force F_a acting of each rigid body (like gravity, ...) and
* --- Step 1 --- * we obtain some new velocities v2' that tends to violate the constraints.
* *
* First, we int32_tegrate the applied force F_a acting of each rigid body (like gravity, ...) and * v2' = v1 + dt * M^-1 * F_a
* we obtain some new velocities v2' that tends to violate the constraints. *
* * where M is a matrix that contains mass and inertia tensor information.
* v2' = v1 + dt * M^-1 * F_a *
* * --- Step 2 ---
* where M is a matrix that contains mass and inertia tensor information. *
* * During the second step, we iterate over all the constraints for a certain number of
* --- Step 2 --- * 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
* During the second step, we iterate over all the constraints for a certain number of * M * deltaV = P_c where M is the mass of the body, deltaV is the difference of velocity and
* iterations and for each constraint we compute the impulse to apply to the bodies needed * P_c is the constraint impulse to apply to the body. Therefore, we have
* so that the new velocity of the bodies satisfy Jv + b = 0. From the Newton law, we know that * v2 = v2' + M^-1 * P_c. For each constraint, we can compute the Lagrange multiplier lambda
* M * deltaV = P_c where M is the mass of the body, deltaV is the difference of velocity and * using : lambda = -m_c (Jv2' + b) where m_c = 1 / (J * M^-1 * J^t). Now that we have the
* P_c is the constraint impulse to apply to the body. Therefore, we have * Lagrange multiplier lambda, we can compute the impulse P_c = J^t * lambda * dt to apply to
* v2 = v2' + M^-1 * P_c. For each constraint, we can compute the Lagrange multiplier lambda * the bodies to satisfy the constraint.
* 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 * --- Step 3 ---
* the bodies to satisfy the constraint. *
* * In the third step, we int32_tegrate the new position x2 of the bodies using the new velocities
* --- Step 3 --- * v2 computed in the second step with : x2 = x1 + dt * v2.
* *
* In the third step, we int32_tegrate the new position x2 of the bodies using the new velocities * Note that in the following code (as it is also explained in the slides from Erin Catto),
* v2 computed in the second step with : x2 = x1 + dt * v2. * 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
* Note that in the following code (as it is also explained in the slides from Erin Catto), * lambda, we mean (lambda * dt).
* 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 * We are using the accumulated impulse technique that is also described in the slides from
* lambda, we mean (lambda * dt). * Erin Catto.
* *
* We are using the accumulated impulse technique that is also described in the slides from * We are also using warm starting. The idea is to warm start the solver at the beginning of
* Erin Catto. * 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.
* 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 * For contact constraints, we are also using split impulses so that the position correction
* previous step. This allows the iterative solver to converge faster towards the solution. * that uses Baumgarte stabilization does not change the momentum of the bodies.
* *
* For contact constraints, we are also using split impulses so that the position correction * There are two ways to apply the friction constraints. Either the friction constraints are
* that uses Baumgarte stabilization does not change the momentum of the bodies. * 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
* There are two ways to apply the friction constraints. Either the friction constraints are * two constraints (two tangential friction directions) and if we solve the friction
* applied at each contact point or they are applied only at the center of the contact manifold * constraints at the center of the contact manifold, we need two constraints for tangential
* between two bodies. If we solve the friction constraints at each contact point, we need * friction but also another twist friction constraint to prevent spin of the body around the
* two constraints (two tangential friction directions) and if we solve the friction * contact manifold center.
* 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 class ContactSolver {
* contact manifold center. private:
*/ /**
class ContactSolver { * Contact solver int32_ternal data structure that to store all the
* information relative to a contact point
private: */
struct ContactPointSolver {
// Structure ContactPointSolver float penetrationImpulse; //!< Accumulated normal impulse
/** float friction1Impulse; //!< Accumulated impulse in the 1st friction direction
* Contact solver int32_ternal data structure that to store all the float friction2Impulse; //!< Accumulated impulse in the 2nd friction direction
* information relative to a contact point float penetrationSplitImpulse; //!< Accumulated split impulse for penetration correction
*/ vec3 rollingResistanceImpulse; //!< Accumulated rolling resistance impulse
struct ContactPointSolver { vec3 normal; //!< Normal vector of the contact
vec3 frictionVector1; //!< First friction vector in the tangent plane
/// Accumulated normal impulse vec3 frictionvec2; //!< Second friction vector in the tangent plane
float penetrationImpulse; vec3 oldFrictionVector1; //!< Old first friction vector in the tangent plane
vec3 oldFrictionvec2; //!< Old second friction vector in the tangent plane
/// Accumulated impulse in the 1st friction direction vec3 r1; //!< Vector from the body 1 center to the contact point
float friction1Impulse; vec3 r2; //!< Vector from the body 2 center to the contact point
vec3 r1CrossT1; //!< Cross product of r1 with 1st friction vector
/// Accumulated impulse in the 2nd friction direction vec3 r1CrossT2; //!< Cross product of r1 with 2nd friction vector
float friction2Impulse; vec3 r2CrossT1; //!< Cross product of r2 with 1st friction vector
vec3 r2CrossT2; //!< Cross product of r2 with 2nd friction vector
/// Accumulated split impulse for penetration correction vec3 r1CrossN; //!< Cross product of r1 with the contact normal
float penetrationSplitImpulse; vec3 r2CrossN; //!< Cross product of r2 with the contact normal
float penetrationDepth; //!< Penetration depth
/// Accumulated rolling resistance impulse float restitutionBias; //!< Velocity restitution bias
vec3 rollingResistanceImpulse; float inversePenetrationMass; //!< Inverse of the matrix K for the penenetration
float inverseFriction1Mass; //!< Inverse of the matrix K for the 1st friction
/// Normal vector of the contact float inverseFriction2Mass; //!< Inverse of the matrix K for the 2nd friction
vec3 normal; bool isRestingContact; //!< True if the contact was existing last time step
ContactPoint* externalContact; //!< Pointer to the external contact
/// First friction vector in the tangent plane };
vec3 frictionVector1;
/**
/// Second friction vector in the tangent plane * @brief Contact solver int32_ternal data structure to store all the information relative to a contact manifold.
vec3 frictionvec2; */
struct ContactManifoldSolver {
/// Old first friction vector in the tangent plane uint32_t indexBody1; //!< Index of body 1 in the constraint solver
vec3 oldFrictionVector1; uint32_t indexBody2; //!< Index of body 2 in the constraint solver
float massInverseBody1; //!< Inverse of the mass of body 1
/// Old second friction vector in the tangent plane float massInverseBody2; //!< Inverse of the mass of body 2
vec3 oldFrictionvec2; etk::Matrix3x3 inverseInertiaTensorBody1; //!< Inverse inertia tensor of body 1
etk::Matrix3x3 inverseInertiaTensorBody2; //!< Inverse inertia tensor of body 2
/// Vector from the body 1 center to the contact point ContactPointSolver contacts[MAX_CONTACT_POINTS_IN_MANIFOLD]; //!< Contact point constraints
vec3 r1; uint32_t nbContacts; //!< Number of contact points
bool isBody1DynamicType; //!< True if the body 1 is of type dynamic
/// Vector from the body 2 center to the contact point bool isBody2DynamicType; //!< True if the body 2 is of type dynamic
vec3 r2; float restitutionFactor; //!< Mix of the restitution factor for two bodies
float frictionCoefficient; //!< Mix friction coefficient for the two bodies
/// Cross product of r1 with 1st friction vector float rollingResistanceFactor; //!< Rolling resistance factor between the two bodies
vec3 r1CrossT1; ContactManifold* externalContactManifold; //!< Pointer to the external contact manifold
// - Variables used when friction constraints are apply at the center of the manifold-//
/// Cross product of r1 with 2nd friction vector vec3 normal; //!< Average normal vector of the contact manifold
vec3 r1CrossT2; vec3 frictionPointBody1; //!< Point on body 1 where to apply the friction constraints
vec3 frictionPointBody2; //!< Point on body 2 where to apply the friction constraints
/// Cross product of r2 with 1st friction vector vec3 r1Friction; //!< R1 vector for the friction constraints
vec3 r2CrossT1; vec3 r2Friction; //!< R2 vector for the friction constraints
vec3 r1CrossT1; //!< Cross product of r1 with 1st friction vector
/// Cross product of r2 with 2nd friction vector vec3 r1CrossT2; //!< Cross product of r1 with 2nd friction vector
vec3 r2CrossT2; vec3 r2CrossT1; //!< Cross product of r2 with 1st friction vector
vec3 r2CrossT2; //!< Cross product of r2 with 2nd friction vector
/// Cross product of r1 with the contact normal float inverseFriction1Mass; //!< Matrix K for the first friction constraint
vec3 r1CrossN; float inverseFriction2Mass; //!< Matrix K for the second friction constraint
float inverseTwistFrictionMass; //!< Matrix K for the twist friction constraint
/// Cross product of r2 with the contact normal etk::Matrix3x3 inverseRollingResistance; //!< Matrix K for the rolling resistance constraint
vec3 r2CrossN; vec3 frictionVector1; //!< First friction direction at contact manifold center
vec3 frictionvec2; //!< Second friction direction at contact manifold center
/// Penetration depth vec3 oldFrictionVector1; //!< Old 1st friction direction at contact manifold center
float penetrationDepth; vec3 oldFrictionvec2; //!< Old 2nd friction direction at contact manifold center
float friction1Impulse; //!< First friction direction impulse at manifold center
/// Velocity restitution bias float friction2Impulse; //!< Second friction direction impulse at manifold center
float restitutionBias; float frictionTwistImpulse; //!< Twist friction impulse at contact manifold center
vec3 rollingResistanceImpulse; //!< Rolling resistance impulse
/// Inverse of the matrix K for the penenetration };
float inversePenetrationMass; 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
/// Inverse of the matrix K for the 1st friction static const float SLOP; //!< Slop distance (allowed penetration distance between bodies)
float inverseFriction1Mass; 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)
/// Inverse of the matrix K for the 2nd friction float m_timeStep; //!< Current time step
float inverseFriction2Mass; ContactManifoldSolver* m_contactConstraints; //!< Contact constraints
uint32_t m_numberContactManifolds; //!< Number of contact constraints
/// True if the contact was existing last time step vec3* m_linearVelocities; //!< Array of linear velocities
bool isRestingContact; 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
/// Pointer to the external contact bool m_isWarmStartingActive; //!< True if the warm starting of the solver is active
ContactPoint* externalContact; 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
// Structure ContactManifoldSolver void initializeContactConstraints();
/** /// Apply an impulse to the two bodies of a constraint
* Contact solver int32_ternal data structure to store all the void applyImpulse(const Impulse& impulse, const ContactManifoldSolver& manifold);
* information relative to a contact manifold. /// Apply an impulse to the two bodies of a constraint
*/ void applySplitImpulse(const Impulse& impulse,
struct ContactManifoldSolver { const ContactManifoldSolver& manifold);
/// Compute the collision restitution factor from the restitution factor of each body
/// Index of body 1 in the constraint solver float computeMixedRestitutionFactor(RigidBody *body1,
uint32_t indexBody1; RigidBody *body2) const;
/// Compute the mixed friction coefficient from the friction coefficient of each body
/// Index of body 2 in the constraint solver float computeMixedFrictionCoefficient(RigidBody* body1,
uint32_t indexBody2; RigidBody* body2) const;
/// Compute th mixed rolling resistance factor between two bodies
/// Inverse of the mass of body 1 float computeMixedRollingResistance(RigidBody* body1, RigidBody* body2) const;
float massInverseBody1; /// 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
// Inverse of the mass of body 2 /// such that : t1 x t2 = contactNormal.
float massInverseBody2; void computeFrictionVectors(const vec3& deltaVelocity,
ContactPointSolver &contactPoint) const;
/// Inverse inertia tensor of body 1 /// Compute the two unit orthogonal vectors "t1" and "t2" that span the tangential friction
etk::Matrix3x3 inverseInertiaTensorBody1; /// plane for a contact manifold. The two vectors have to be
/// such that : t1 x t2 = contactNormal.
/// Inverse inertia tensor of body 2 void computeFrictionVectors(const vec3& deltaVelocity,
etk::Matrix3x3 inverseInertiaTensorBody2; ContactManifoldSolver& contactPoint) const;
/// Compute a penetration constraint impulse
/// Contact point constraints const Impulse computePenetrationImpulse(float deltaLambda,
ContactPointSolver contacts[MAX_CONTACT_POINTS_IN_MANIFOLD]; const ContactPointSolver& contactPoint) const;
/// Compute the first friction constraint impulse
/// Number of contact points const Impulse computeFriction1Impulse(float deltaLambda,
uint32_t nbContacts; const ContactPointSolver& contactPoint) const;
/// Compute the second friction constraint impulse
/// True if the body 1 is of type dynamic const Impulse computeFriction2Impulse(float deltaLambda,
bool isBody1DynamicType; const ContactPointSolver& contactPoint) const;
public:
/// True if the body 2 is of type dynamic /// Constructor
bool isBody2DynamicType; ContactSolver(const std::map<RigidBody*, uint32_t>& mapBodyToVelocityIndex);
/// Destructor
/// Mix of the restitution factor for two bodies virtual ~ContactSolver();
float restitutionFactor; /// Initialize the constraint solver for a given island
void initializeForIsland(float dt, Island* island);
/// Mix friction coefficient for the two bodies /// Set the split velocities arrays
float frictionCoefficient; void setSplitVelocitiesArrays(vec3* splitLinearVelocities,
vec3* splitAngularVelocities);
/// Rolling resistance factor between the two bodies /// Set the constrained velocities arrays
float rollingResistanceFactor; void setConstrainedVelocitiesArrays(vec3* constrainedLinearVelocities,
vec3* constrainedAngularVelocities);
/// Pointer to the external contact manifold /// Warm start the solver.
ContactManifold* externalContactManifold; void warmStart();
/// Store the computed impulses to use them to
// - Variables used when friction constraints are apply at the center of the manifold-// /// warm start the solver at the next iteration
void storeImpulses();
/// Average normal vector of the contact manifold /// Solve the contacts
vec3 normal; void solve();
/// Return true if the split impulses position correction technique is used for contacts
/// Point on body 1 where to apply the friction constraints bool isSplitImpulseActive() const;
vec3 frictionPointBody1; /// Activate or Deactivate the split impulses for contacts
void setIsSplitImpulseActive(bool isActive);
/// Point on body 2 where to apply the friction constraints /// Activate or deactivate the solving of friction constraints at the center of
vec3 frictionPointBody2; /// the contact manifold instead of solving them at each contact point
void setIsSolveFrictionAtContactManifoldCenterActive(bool isActive);
/// R1 vector for the friction constraints /// Clean up the constraint solver
vec3 r1Friction; void cleanup();
};
/// 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();
};
// Set the split velocities arrays // Set the split velocities arrays
inline void ContactSolver::setSplitVelocitiesArrays(vec3* splitLinearVelocities, inline void ContactSolver::setSplitVelocitiesArrays(vec3* splitLinearVelocities,
@ -443,24 +251,24 @@ inline void ContactSolver::setConstrainedVelocitiesArrays(vec3* constrainedLinea
vec3* constrainedAngularVelocities) { vec3* constrainedAngularVelocities) {
assert(constrainedLinearVelocities != NULL); assert(constrainedLinearVelocities != NULL);
assert(constrainedAngularVelocities != NULL); assert(constrainedAngularVelocities != NULL);
mLinearVelocities = constrainedLinearVelocities; m_linearVelocities = constrainedLinearVelocities;
mAngularVelocities = constrainedAngularVelocities; m_angularVelocities = constrainedAngularVelocities;
} }
// Return true if the split impulses position correction technique is used for contacts // Return true if the split impulses position correction technique is used for contacts
inline bool ContactSolver::isSplitImpulseActive() const { inline bool ContactSolver::isSplitImpulseActive() const {
return mIsSplitImpulseActive; return m_isSplitImpulseActive;
} }
// Activate or Deactivate the split impulses for contacts // Activate or Deactivate the split impulses for contacts
inline void ContactSolver::setIsSplitImpulseActive(bool isActive) { inline void ContactSolver::setIsSplitImpulseActive(bool isActive) {
mIsSplitImpulseActive = isActive; m_isSplitImpulseActive = isActive;
} }
// Activate or deactivate the solving of friction constraints at the center of // Activate or deactivate the solving of friction constraints at the center of
// the contact manifold instead of solving them at each contact point // the contact manifold instead of solving them at each contact point
inline void ContactSolver::setIsSolveFrictionAtContactManifoldCenterActive(bool isActive) { inline void ContactSolver::setIsSolveFrictionAtContactManifoldCenterActive(bool isActive) {
mIsSolveFrictionAtContactManifoldCenterActive = isActive; m_isSolveFrictionAtContactManifoldCenterActive = isActive;
} }
// Compute the collision restitution factor from the restitution factor of each body // 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 #pragma once
// Libraries
#include <ephysics/engine/CollisionWorld.hpp> #include <ephysics/engine/CollisionWorld.hpp>
#include <ephysics/collision/CollisionDetection.hpp> #include <ephysics/collision/CollisionDetection.hpp>
#include <ephysics/engine/ContactSolver.hpp> #include <ephysics/engine/ContactSolver.hpp>
@ -14,275 +13,157 @@
#include <ephysics/engine/Island.hpp> #include <ephysics/engine/Island.hpp>
#include <ephysics/configuration.hpp> #include <ephysics/configuration.hpp>
/// Namespace ReactPhysics3D
namespace ephysics { namespace ephysics {
/**
// Class DynamicsWorld * @brief This class represents a dynamics world. This class inherits from
/** * the CollisionWorld class. In a dynamics world, bodies can collide
* This class represents a dynamics world. This class inherits from * and their movements are simulated using the laws of physics.
* 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 :
class DynamicsWorld : public CollisionWorld { ContactSolver m_contactSolver; //!< Contact solver
ConstraintSolver m_constraintSolver; //!< Constraint solver
protected : 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
// -------------------- Attributes -------------------- // 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
/// Contact solver std::set<Joint*> m_joints; //!< All the joints of the world
ContactSolver m_contactSolver; vec3 m_gravity; //!< Gravity vector of the world
float m_timeStep; //!< Current frame time step (in seconds)
/// Constraint solver bool m_isGravityEnabled; //!< True if the gravity force is on
ConstraintSolver m_constraintSolver; 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)
/// Number of iterations for the velocity solver of the Sequential Impulses technique vec3* m_splitLinearVelocities; //!< Split linear velocities for the position contact solver (split impulse)
uint32_t m_nbVelocitySolverIterations; 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)
/// Number of iterations for the position solver of the Sequential Impulses technique etk::Quaternion* m_constrainedOrientations; //!< Array of constrained rigid bodies orientation (for position error correction)
uint32_t m_nbPositionSolverIterations; 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
/// True if the spleeping technique for inactive bodies is enabled uint32_t m_numberIslandsCapacity; //!< Current allocated capacity for the islands
bool m_isSleepingEnabled; Island** m_islands; //!< Array with all the islands of awaken bodies
uint32_t m_numberBodiesCapacity; //!< Current allocated capacity for the bodies
/// All the rigid bodies of the physics world float m_sleepLinearVelocity; //!< Sleep linear velocity threshold
std::set<RigidBody*> m_rigidBodies; 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.
/// All the joints of the world /// Private copy-constructor
std::set<Joint*> m_joints; DynamicsWorld(const DynamicsWorld& world);
/// Private assignment operator
/// Gravity vector of the world DynamicsWorld& operator=(const DynamicsWorld& world);
vec3 m_gravity; /// Integrate the positions and orientations of rigid bodies.
void int32_tegrateRigidBodiesPositions();
/// Current frame time step (in seconds) /// Update the AABBs of the bodies
float m_timeStep; void updateRigidBodiesAABB();
/// Reset the external force and torque applied to the bodies
/// True if the gravity force is on void resetBodiesForceAndTorque();
bool m_isGravityEnabled; /// Update the position and orientation of a body
void updatePositionAndOrientationOfBody(RigidBody* body, vec3 newLinVelocity,
/// Array of constrained linear velocities (state of the linear velocities vec3 newAngVelocity);
/// after solving the constraints) /// Compute and set the int32_terpolation factor to all bodies
vec3* m_constrainedLinearVelocities; void setInterpolationFactorToAllBodies();
/// Initialize the bodies velocities arrays for the next simulation step.
/// Array of constrained angular velocities (state of the angular velocities void initVelocityArrays();
/// after solving the constraints) /// Integrate the velocities of rigid bodies.
vec3* m_constrainedAngularVelocities; void int32_tegrateRigidBodiesVelocities();
/// Solve the contacts and constraints
/// Split linear velocities for the position contact solver (split impulse) void solveContactsAndConstraints();
vec3* m_splitLinearVelocities; /// Solve the position error correction of the constraints
void solvePositionCorrection();
/// Split angular velocities for the position contact solver (split impulse) /// Cleanup the constrained velocities array at each step
vec3* m_splitAngularVelocities; void cleanupConstrainedVelocitiesArray();
/// Compute the islands of awake bodies.
/// Array of constrained rigid bodies position (for position error correction) void computeIslands();
vec3* m_constrainedPositions; /// Update the postion/orientation of the bodies
void updateBodiesState();
/// Array of constrained rigid bodies orientation (for position error correction) /// Put bodies to sleep if needed.
etk::Quaternion* m_constrainedOrientations; void updateSleepingBodies();
/// Add the joint to the list of joints of the two bodies involved in the joint
/// Map body to their index in the constrained velocities array void addJointToBody(Joint* joint);
std::map<RigidBody*, uint32_t> m_mapBodyToConstrainedVelocityIndex; public :
/// Constructor
/// Number of islands in the world DynamicsWorld(const vec3& m_gravity);
uint32_t m_numberIslands; /// Destructor
virtual ~DynamicsWorld();
/// Current allocated capacity for the islands /// Update the physics simulation
uint32_t m_numberIslandsCapacity; void update(float timeStep);
/// Get the number of iterations for the velocity constraint solver
/// Array with all the islands of awaken bodies uint32_t getNbIterationsVelocitySolver() const;
Island** m_islands; /// Set the number of iterations for the velocity constraint solver
void setNbIterationsVelocitySolver(uint32_t nbIterations);
/// Current allocated capacity for the bodies /// Get the number of iterations for the position constraint solver
uint32_t m_numberBodiesCapacity; uint32_t getNbIterationsPositionSolver() const;
/// Set the number of iterations for the position constraint solver
/// Sleep linear velocity threshold void setNbIterationsPositionSolver(uint32_t nbIterations);
float m_sleepLinearVelocity; /// Set the position correction technique used for contacts
void setContactsPositionCorrectionTechnique(ContactsPositionCorrectionTechnique technique);
/// Sleep angular velocity threshold /// Set the position correction technique used for joints
float m_sleepAngularVelocity; void setJointsPositionCorrectionTechnique(JointsPositionCorrectionTechnique technique);
/// Activate or deactivate the solving of friction constraints at the center of
/// Time (in seconds) before a body is put to sleep if its velocity /// the contact manifold instead of solving them at each contact point
/// becomes smaller than the sleep velocity. void setIsSolveFrictionAtContactManifoldCenterActive(bool isActive);
float m_timeBeforeSleep; /// Create a rigid body int32_to the physics world.
RigidBody* createRigidBody(const etk::Transform3D& transform);
// -------------------- Methods -------------------- // /// Destroy a rigid body and all the joints which it belongs
void destroyRigidBody(RigidBody* rigidBody);
/// Private copy-constructor /// Create a joint between two bodies in the world and return a pointer to the new joint
DynamicsWorld(const DynamicsWorld& world); Joint* createJoint(const JointInfo& jointInfo);
/// Destroy a joint
/// Private assignment operator void destroyJoint(Joint* joint);
DynamicsWorld& operator=(const DynamicsWorld& world); /// Return the gravity vector of the world
vec3 getGravity() const;
/// Integrate the positions and orientations of rigid bodies. /// Set the gravity vector of the world
void int32_tegrateRigidBodiesPositions(); void setGravity(vec3& gravity);
/// Return if the gravity is on
/// Update the AABBs of the bodies bool isGravityEnabled() const;
void updateRigidBodiesAABB(); /// Enable/Disable the gravity
void setIsGratityEnabled(bool isGravityEnabled);
/// Reset the external force and torque applied to the bodies /// Return the number of rigid bodies in the world
void resetBodiesForceAndTorque(); uint32_t getNbRigidBodies() const;
/// Return the number of joints in the world
/// Update the position and orientation of a body uint32_t getNbJoints() const;
void updatePositionAndOrientationOfBody(RigidBody* body, vec3 newLinVelocity, /// Return an iterator to the beginning of the rigid bodies of the physics world
vec3 newAngVelocity); std::set<RigidBody*>::iterator getRigidBodiesBeginIterator();
/// Return an iterator to the end of the rigid bodies of the physics world
/// Compute and set the int32_terpolation factor to all bodies std::set<RigidBody*>::iterator getRigidBodiesEndIterator();
void setInterpolationFactorToAllBodies(); /// Return true if the sleeping technique is enabled
bool isSleepingEnabled() const;
/// Initialize the bodies velocities arrays for the next simulation step. /// Enable/Disable the sleeping technique
void initVelocityArrays(); void enableSleeping(bool isSleepingEnabled);
/// Return the current sleep linear velocity
/// Integrate the velocities of rigid bodies. float getSleepLinearVelocity() const;
void int32_tegrateRigidBodiesVelocities(); /// Set the sleep linear velocity.
void setSleepLinearVelocity(float sleepLinearVelocity);
/// Solve the contacts and constraints /// Return the current sleep angular velocity
void solveContactsAndConstraints(); float getSleepAngularVelocity() const;
/// Set the sleep angular velocity.
/// Solve the position error correction of the constraints void setSleepAngularVelocity(float sleepAngularVelocity);
void solvePositionCorrection(); /// Return the time a body is required to stay still before sleeping
float getTimeBeforeSleep() const;
/// Cleanup the constrained velocities array at each step /// Set the time a body is required to stay still before sleeping
void cleanupConstrainedVelocitiesArray(); void setTimeBeforeSleep(float timeBeforeSleep);
/// Set an event listener object to receive events callbacks.
/// Compute the islands of awake bodies. void setEventListener(EventListener* eventListener);
void computeIslands(); /// Test and report collisions between a given shape and all the others
/// shapes of the world
/// Update the postion/orientation of the bodies virtual void testCollision(const ProxyShape* shape,
void updateBodiesState(); CollisionCallback* callback);
/// Test and report collisions between two given shapes
/// Put bodies to sleep if needed. virtual void testCollision(const ProxyShape* shape1,
void updateSleepingBodies(); const ProxyShape* shape2,
CollisionCallback* callback);
/// Add the joint to the list of joints of the two bodies involved in the joint /// Test and report collisions between a body and all
void addJointToBody(Joint* joint); /// the others bodies of the world
virtual void testCollision(const CollisionBody* body,
public : CollisionCallback* callback);
/// Test and report collisions between two bodies
// -------------------- Methods -------------------- // virtual void testCollision(const CollisionBody* body1,
const CollisionBody* body2,
/// Constructor CollisionCallback* callback);
DynamicsWorld(const vec3& m_gravity); /// Test and report collisions between all shapes of the world
virtual void testCollision(CollisionCallback* callback);
/// Destructor /// Return the list of all contacts of the world
virtual ~DynamicsWorld(); std::vector<const ContactManifold*> getContactsList() const;
friend class RigidBody;
/// 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;
};
// Reset the external force and torque applied to the bodies // Reset the external force and torque applied to the bodies
inline void DynamicsWorld::resetBodiesForceAndTorque() { inline void DynamicsWorld::resetBodiesForceAndTorque() {

87
ephysics/engine/EventListener.hpp
View File

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

81
ephysics/engine/Impulse.hpp
View File

@ -5,60 +5,39 @@
*/ */
#pragma once #pragma once
// Libraries
#include <ephysics/mathematics/mathematics.hpp> #include <ephysics/mathematics/mathematics.hpp>
namespace ephysics { namespace ephysics {
/**
* @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) {
// Structure Impulse }
/** /// Copy-constructor
* Represents an impulse that we can apply to bodies in the contact or constraint solver. Impulse(const Impulse& _impulse):
*/ linearImpulseBody1(_impulse.linearImpulseBody1),
struct Impulse { angularImpulseBody1(_impulse.angularImpulseBody1),
linearImpulseBody2(_impulse.linearImpulseBody2),
private: angularImpulseBody2(_impulse.angularImpulseBody2) {
// -------------------- 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) {
;
}
};
}
};
} }

1
ephysics/engine/Island.cpp
View File

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

139
ephysics/engine/Island.hpp
View File

@ -5,105 +5,58 @@
*/ */
#pragma once #pragma once
// Libraries
#include <ephysics/memory/MemoryAllocator.hpp> #include <ephysics/memory/MemoryAllocator.hpp>
#include <ephysics/body/RigidBody.hpp> #include <ephysics/body/RigidBody.hpp>
#include <ephysics/constraint/Joint.hpp> #include <ephysics/constraint/Joint.hpp>
#include <ephysics/collision/ContactManifold.hpp> #include <ephysics/collision/ContactManifold.hpp>
namespace ephysics { namespace ephysics {
/**
// Class Island * @brief An island represent an isolated group of awake bodies that are connected with each other by
/** * some contraints (contacts or joints).
* An island represent an isolated group of awake bodies that are connected with each other by */
* some contraints (contacts or joints). class Island {
*/ private:
class Island { 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
private: Joint** m_joints; //!< Array with all the joints between bodies of the island
uint32_t m_numberBodies; //!< Current number of bodies in the island
// -------------------- Attributes -------------------- // uint32_t m_numberContactManifolds; //!< Current number of contact manifold in the island
uint32_t m_numberJoints; //!< Current number of joints in the island
/// Array with all the bodies of the island MemoryAllocator& m_memoryAllocator; //!< Reference to the memory allocator
RigidBody** m_bodies; 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
/// Array with all the contact manifolds between bodies of the island size_t m_numberAllocatedBytesJoints; //!< Number of bytes allocated for the joints array
ContactManifold** m_contactManifolds; /// Private assignment operator
Island& operator=(const Island& island);
/// Array with all the joints between bodies of the island /// Private copy-constructor
Joint** m_joints; Island(const Island& island);
public:
/// Current number of bodies in the island /// Constructor
uint32_t m_numberBodies; Island(uint32_t nbMaxBodies, uint32_t nbMaxContactManifolds, uint32_t nbMaxJoints,
MemoryAllocator& memoryAllocator);
/// Current number of contact manifold in the island /// Destructor
uint32_t m_numberContactManifolds; ~Island();
/// Add a body int32_to the island
/// Current number of joints in the island void addBody(RigidBody* body);
uint32_t m_numberJoints; /// Add a contact manifold int32_to the island
void addContactManifold(ContactManifold* contactManifold);
/// Reference to the memory allocator /// Add a joint int32_to the island
MemoryAllocator& m_memoryAllocator; void addJoint(Joint* joint);
/// Return the number of bodies in the island
/// Number of bytes allocated for the bodies array uint32_t getNbBodies() const;
size_t m_numberAllocatedBytesBodies; /// Return the number of contact manifolds in the island
uint32_t getNbContactManifolds() const;
/// Number of bytes allocated for the contact manifolds array /// Return the number of joints in the island
size_t m_numberAllocatedBytesContactManifolds; uint32_t getNbJoints() const;
/// Return a pointer to the array of bodies
/// Number of bytes allocated for the joints array RigidBody** getBodies();
size_t m_numberAllocatedBytesJoints; /// Return a pointer to the array of contact manifolds
ContactManifold** getContactManifold();
// -------------------- Methods -------------------- // /// Return a pointer to the array of joints
Joint** getJoints();
/// Private assignment operator friend class DynamicsWorld;
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;
};
// Add a body int32_to the island // Add a body int32_to the island
inline void Island::addBody(RigidBody* body) { 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) { 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) * @license BSD 3 clauses (see license file)
*/ */
#pragma once #pragma once
// Libraries
#include <cassert> #include <cassert>
#include <ephysics/configuration.hpp> #include <ephysics/configuration.hpp>
namespace ephysics { 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
* This class contains the material properties of a rigid body that will be use for * body.
* the dynamics simulation like the friction coefficient or the bounciness of the rigid */
* body. class Material {
*/ private :
class Material { float m_frictionCoefficient; //!< Friction coefficient (positive value)
float m_rollingResistance; //!< Rolling resistance factor (positive value)
private : float m_bounciness; //!< Bounciness during collisions (between 0 and 1) where 1 is for a very bouncy body
public :
// -------------------- Attributes -------------------- // /// Constructor
Material();
/// Friction coefficient (positive value) /// Copy-constructor
float m_frictionCoefficient; Material(const Material& material);
/// Return the bounciness
/// Rolling resistance factor (positive value) float getBounciness() const;
float m_rollingResistance; /// Set the bounciness.
void setBounciness(float bounciness);
/// Bounciness during collisions (between 0 and 1) where 1 is for a very bouncy body /// Return the friction coefficient
float m_bounciness; float getFrictionCoefficient() const;
/// Set the friction coefficient.
public : void setFrictionCoefficient(float frictionCoefficient);
/// Return the rolling resistance factor
// -------------------- Methods -------------------- // float getRollingResistance() const;
/// Set the rolling resistance factor
/// Constructor void setRollingResistance(float rollingResistance);
Material(); /// Overloaded assignment operator
Material& operator=(const Material& 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);
};
// Return the bounciness // 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) { m_cachedSeparatingAxis(1.0, 1.0, 1.0) {
} }
// Destructor
OverlappingPair::~OverlappingPair() {
}

130
ephysics/engine/OverlappingPair.hpp
View File

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

372
ephysics/engine/Profiler.hpp
View File

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

123
ephysics/engine/Timer.hpp
View File

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