diff --git a/ephysics/engine/CollisionWorld.hpp b/ephysics/engine/CollisionWorld.hpp index 8ab9dcb..2311190 100644 --- a/ephysics/engine/CollisionWorld.hpp +++ b/ephysics/engine/CollisionWorld.hpp @@ -5,7 +5,6 @@ */ #pragma once -// Libraries #include #include #include @@ -21,194 +20,125 @@ #include #include -/// Namespace ephysics namespace ephysics { - -// Declarations -class CollisionCallback; - -// Class CollisionWorld -/** - * This class represent a world where it is possible to move bodies - * by hand and to test collision between each other. In this kind of - * world, the bodies movement is not computed using the laws of physics. - */ -class CollisionWorld { - - protected : - - // -------------------- Attributes -------------------- // - - /// Reference to the collision detection - CollisionDetection m_collisionDetection; - - /// All the bodies (rigid and soft) of the world - std::set m_bodies; - - /// Current body ID - bodyindex m_currentBodyID; - - /// List of free ID for rigid bodies - std::vector 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::iterator getBodiesBeginIterator(); - - /// Return an iterator to the end of the bodies of the physics world - std::set::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::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::iterator CollisionWorld::getBodiesEndIterator() { - return m_bodies.end(); -} - -// Set the collision dispatch configuration -/// This can be used to replace default collision detection algorithms by your -/// custom algorithm for instance. -/** - * @param CollisionDispatch Pointer to a collision dispatch object describing - * which collision detection algorithm to use for two given collision shapes - */ -inline void CollisionWorld::setCollisionDispatch(CollisionDispatch* collisionDispatch) { - m_collisionDetection.setCollisionDispatch(collisionDispatch); -} - -// Ray cast method -/** - * @param ray Ray to use for raycasting - * @param raycastCallback Pointer to the class with the callback method - * @param raycastWithCategoryMaskBits Bits mask corresponding to the category of - * bodies to be raycasted - */ -inline void CollisionWorld::raycast(const Ray& ray, - RaycastCallback* raycastCallback, - unsigned short raycastWithCategoryMaskBits) const { - m_collisionDetection.raycast(raycastCallback, ray, raycastWithCategoryMaskBits); -} - -// Test if the AABBs of two proxy shapes overlap -/** - * @param shape1 Pointer to the first proxy shape to test - * @param shape2 Pointer to the second proxy shape to test - * @return - */ -inline bool CollisionWorld::testAABBOverlap(const ProxyShape* shape1, - const ProxyShape* shape2) const { - - return m_collisionDetection.testAABBOverlap(shape1, shape2); -} - -// Class CollisionCallback -/** - * This class can be used to register a callback for collision test queries. - * You should implement your own class inherited from this one and implement - * the notifyRaycastHit() method. This method will be called for each ProxyShape - * that is hit by the ray. - */ -class CollisionCallback { - - public: - - /// Destructor - virtual ~CollisionCallback() { - - } - - /// This method will be called for contact - virtual void notifyContact(const ContactPointInfo& contactPointInfo)=0; -}; - + class CollisionCallback; + /** + * @brief This class represent a world where it is possible to move bodies + * by hand and to test collision between each other. In this kind of + * world, the bodies movement is not computed using the laws of physics. + */ + class CollisionWorld { + protected : + CollisionDetection m_collisionDetection; //!< Reference to the collision detection + std::set m_bodies; //!< All the bodies (rigid and soft) of the world + bodyindex m_currentBodyID; //!< Current body ID + std::vector 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::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::iterator getBodiesEndIterator() { + return m_bodies.end(); + } + /// Create a collision body + CollisionBody* createCollisionBody(const etk::Transform3D& transform); + /// Destroy a collision body + void destroyCollisionBody(CollisionBody* collisionBody); + /** + * @brief Set the collision dispatch configuration + * This can be used to replace default collision detection algorithms by your + * custom algorithm for instance. + * @param[in] _CollisionDispatch Pointer to a collision dispatch object describing + * which collision detection algorithm to use for two given collision shapes + */ + void setCollisionDispatch(CollisionDispatch* _collisionDispatch) { + m_collisionDetection.setCollisionDispatch(_collisionDispatch); + } + /** + * @brief Ray cast method + * @param _ray Ray to use for raycasting + * @param _raycastCallback Pointer to the class with the callback method + * @param _raycastWithCategoryMaskBits Bits mask corresponding to the category of bodies to be raycasted + */ + void raycast(const Ray& _ray, + RaycastCallback* _raycastCallback, + unsigned short _raycastWithCategoryMaskBitss = 0xFFFF) const { + m_collisionDetection.raycast(_raycastCallback, _ray, _raycastWithCategoryMaskBits); + } + /// Test if the AABBs of two bodies overlap + bool testAABBOverlap(const CollisionBody* body1, + const CollisionBody* body2) const; + /** + * @brief Test if the AABBs of two proxy shapes overlap + * @param _shape1 Pointer to the first proxy shape to test + * @param _shape2 Pointer to the second proxy shape to test + */ + bool testAABBOverlap(const ProxyShape* _shape1, + const ProxyShape* _shape2) const { + return m_collisionDetection.testAABBOverlap(shape1, shape2); + } + /// Test and report collisions between a given shape and all the others + /// shapes of the world + virtual void testCollision(const ProxyShape* shape, + CollisionCallback* callback); + /// Test and report collisions between two given shapes + virtual void testCollision(const ProxyShape* shape1, + const ProxyShape* shape2, + CollisionCallback* callback); + /// Test and report collisions between a body and all the others bodies of the + /// world + virtual void testCollision(const CollisionBody* body, + CollisionCallback* callback); + /// Test and report collisions between two bodies + virtual void testCollision(const CollisionBody* body1, + const CollisionBody* body2, + CollisionCallback* callback); + /// Test and report collisions between all shapes of the world + virtual void testCollision(CollisionCallback* callback); + friend class CollisionDetection; + friend class CollisionBody; + friend class RigidBody; + friend class ConvexMeshShape; + }; + + /** + * @brief This class can be used to register a callback for collision test queries. + * You should implement your own class inherited from this one and implement + * the notifyRaycastHit() method. This method will be called for each ProxyShape + * that is hit by the ray. + */ + class CollisionCallback { + public: + /** + * @brief Virtualisation of the destructor. + */ + virtual ~CollisionCallback() = default; + /** + * @brief This method will be called for contact. + * @param[in] _contactPointInfo Contact information property. + */ + virtual void notifyContact(const ContactPointInfo& _contactPointInfo)=0; + }; } diff --git a/ephysics/engine/ConstraintSolver.cpp b/ephysics/engine/ConstraintSolver.cpp index 7ae6ae8..724b5c0 100644 --- a/ephysics/engine/ConstraintSolver.cpp +++ b/ephysics/engine/ConstraintSolver.cpp @@ -4,7 +4,6 @@ * @license BSD 3 clauses (see license file) */ -// Libraries #include #include @@ -13,12 +12,7 @@ using namespace ephysics; // Constructor ConstraintSolver::ConstraintSolver(const std::map& mapBodyToVelocityIndex) : m_mapBodyToConstrainedVelocityIndex(mapBodyToVelocityIndex), - mIsWarmStartingActive(true), m_constraintSolverData(mapBodyToVelocityIndex) { - -} - -// Destructor -ConstraintSolver::~ConstraintSolver() { + m_isWarmStartingActive(true), m_constraintSolverData(mapBodyToVelocityIndex) { } @@ -36,7 +30,7 @@ void ConstraintSolver::initializeForIsland(float dt, Island* island) { // Initialize the constraint solver data used to initialize and solve the constraints m_constraintSolverData.timeStep = m_timeStep; - m_constraintSolverData.isWarmStartingActive = mIsWarmStartingActive; + m_constraintSolverData.isWarmStartingActive = m_isWarmStartingActive; // For each joint of the island Joint** joints = island->getJoints(); @@ -46,7 +40,7 @@ void ConstraintSolver::initializeForIsland(float dt, Island* island) { joints[i]->initBeforeSolve(m_constraintSolverData); // Warm-start the constraint if warm-starting is enabled - if (mIsWarmStartingActive) { + if (m_isWarmStartingActive) { joints[i]->warmstart(m_constraintSolverData); } } diff --git a/ephysics/engine/ConstraintSolver.hpp b/ephysics/engine/ConstraintSolver.hpp index b78927b..3036519 100644 --- a/ephysics/engine/ConstraintSolver.hpp +++ b/ephysics/engine/ConstraintSolver.hpp @@ -5,7 +5,6 @@ */ #pragma once -// Libraries #include #include #include @@ -14,169 +13,122 @@ #include namespace ephysics { - -// Structure ConstraintSolverData -/** - * This structure contains data from the constraint solver that are used to solve - * each joint constraint. - */ -struct ConstraintSolverData { - - public : - - /// Current time step of the simulation - float timeStep; - - /// Array with the bodies linear velocities - vec3* linearVelocities; - - /// Array with the bodies angular velocities - vec3* angularVelocities; - - /// Reference to the bodies positions - vec3* positions; - - /// Reference to the bodies orientations - etk::Quaternion* orientations; - - /// Reference to the map that associates rigid body to their index - /// in the constrained velocities array - const std::map& mapBodyToConstrainedVelocityIndex; - - /// True if warm starting of the solver is active - bool isWarmStartingActive; - - /// Constructor - ConstraintSolverData(const std::map& refMapBodyToConstrainedVelocityIndex) - :linearVelocities(NULL), angularVelocities(NULL), - positions(NULL), orientations(NULL), - mapBodyToConstrainedVelocityIndex(refMapBodyToConstrainedVelocityIndex){ - - } - -}; - -// Class ConstraintSolver -/** - * This class represents the constraint solver that is used to solve constraints between - * the rigid bodies. The constraint solver is based on the "Sequential Impulse" technique - * described by Erin Catto in his GDC slides (http://code.google.com/p/box2d/downloads/list). - * - * A constraint between two bodies is represented by a function C(x) which is equal to zero - * when the constraint is satisfied. The condition C(x)=0 describes a valid position and the - * condition dC(x)/dt=0 describes a valid velocity. We have dC(x)/dt = Jv + b = 0 where J is - * the Jacobian matrix of the constraint, v is a vector that contains the velocity of both - * bodies and b is the constraint bias. We are looking for a force F_c that will act on the - * bodies to keep the constraint satisfied. Note that from the virtual work principle, we have - * F_c = J^t * lambda where J^t is the transpose of the Jacobian matrix and lambda is a - * Lagrange multiplier. Therefore, finding the force F_c is equivalent to finding the Lagrange - * multiplier lambda. - - * An impulse P = F * dt where F is a force and dt is the timestep. We can apply impulses a - * body to change its velocity. The idea of the Sequential Impulse technique is to apply - * impulses to bodies of each constraints in order to keep the constraint satisfied. - * - * --- Step 1 --- - * - * First, we int32_tegrate the applied force F_a acting of each rigid body (like gravity, ...) and - * we obtain some new velocities v2' that tends to violate the constraints. - * - * v2' = v1 + dt * M^-1 * F_a - * - * where M is a matrix that contains mass and inertia tensor information. - * - * --- Step 2 --- - * - * During the second step, we iterate over all the constraints for a certain number of - * iterations and for each constraint we compute the impulse to apply to the bodies needed - * so that the new velocity of the bodies satisfy Jv + b = 0. From the Newton law, we know that - * M * deltaV = P_c where M is the mass of the body, deltaV is the difference of velocity and - * P_c is the constraint impulse to apply to the body. Therefore, we have - * v2 = v2' + M^-1 * P_c. For each constraint, we can compute the Lagrange multiplier lambda - * using : lambda = -m_c (Jv2' + b) where m_c = 1 / (J * M^-1 * J^t). Now that we have the - * Lagrange multiplier lambda, we can compute the impulse P_c = J^t * lambda * dt to apply to - * the bodies to satisfy the constraint. - * - * --- Step 3 --- - * - * In the third step, we int32_tegrate the new position x2 of the bodies using the new velocities - * v2 computed in the second step with : x2 = x1 + dt * v2. - * - * Note that in the following code (as it is also explained in the slides from Erin Catto), - * the value lambda is not only the lagrange multiplier but is the multiplication of the - * Lagrange multiplier with the timestep dt. Therefore, in the following code, when we use - * lambda, we mean (lambda * dt). - * - * We are using the accumulated impulse technique that is also described in the slides from - * Erin Catto. - * - * We are also using warm starting. The idea is to warm start the solver at the beginning of - * each step by applying the last impulstes for the constraints that we already existing at the - * previous step. This allows the iterative solver to converge faster towards the solution. - * - * For contact constraints, we are also using split impulses so that the position correction - * that uses Baumgarte stabilization does not change the momentum of the bodies. - * - * There are two ways to apply the friction constraints. Either the friction constraints are - * applied at each contact point or they are applied only at the center of the contact manifold - * between two bodies. If we solve the friction constraints at each contact point, we need - * two constraints (two tangential friction directions) and if we solve the friction - * constraints at the center of the contact manifold, we need two constraints for tangential - * friction but also another twist friction constraint to prevent spin of the body around the - * contact manifold center. - */ -class ConstraintSolver { - - private : - - // -------------------- Attributes -------------------- // - - /// Reference to the map that associates rigid body to their index in - /// the constrained velocities array - const std::map& 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& mapBodyToVelocityIndex); - - /// Destructor - ~ConstraintSolver(); - - /// Initialize the constraint solver for a given island - void initializeForIsland(float dt, Island* island); - - /// Solve the constraints - void solveVelocityConstraints(Island* island); - - /// Solve the position constraints - void solvePositionConstraints(Island* island); - - /// Return true if the Non-Linear-Gauss-Seidel position correction technique is active - bool getIsNonLinearGaussSeidelPositionCorrectionActive() const; - - /// Enable/Disable the Non-Linear-Gauss-Seidel position correction technique. - void setIsNonLinearGaussSeidelPositionCorrectionActive(bool isActive); - - /// Set the constrained velocities arrays - void setConstrainedVelocitiesArrays(vec3* constrainedLinearVelocities, - vec3* constrainedAngularVelocities); - - /// Set the constrained positions/orientations arrays - void setConstrainedPositionsArrays(vec3* constrainedPositions, - etk::Quaternion* constrainedOrientations); -}; + /** + * This structure contains data from the constraint solver that are used to solve + * each joint constraint. + */ + struct ConstraintSolverData { + public : + float timeStep; //!< Current time step of the simulation + vec3* linearVelocities; //!< Array with the bodies linear velocities + vec3* angularVelocities; //!< Array with the bodies angular velocities + vec3* positions; //!< Reference to the bodies positions + etk::Quaternion* orientations; //!< Reference to the bodies orientations + const std::map& 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& refMapBodyToConstrainedVelocityIndex) + :linearVelocities(NULL), angularVelocities(NULL), + positions(NULL), orientations(NULL), + mapBodyToConstrainedVelocityIndex(refMapBodyToConstrainedVelocityIndex){ + + } + }; + + /** + * @brief This class represents the constraint solver that is used to solve constraints between + * the rigid bodies. The constraint solver is based on the "Sequential Impulse" technique + * described by Erin Catto in his GDC slides (http://code.google.com/p/box2d/downloads/list). + * + * A constraint between two bodies is represented by a function C(x) which is equal to zero + * when the constraint is satisfied. The condition C(x)=0 describes a valid position and the + * condition dC(x)/dt=0 describes a valid velocity. We have dC(x)/dt = Jv + b = 0 where J is + * the Jacobian matrix of the constraint, v is a vector that contains the velocity of both + * bodies and b is the constraint bias. We are looking for a force F_c that will act on the + * bodies to keep the constraint satisfied. Note that from the virtual work principle, we have + * F_c = J^t * lambda where J^t is the transpose of the Jacobian matrix and lambda is a + * Lagrange multiplier. Therefore, finding the force F_c is equivalent to finding the Lagrange + * multiplier lambda. + + * An impulse P = F * dt where F is a force and dt is the timestep. We can apply impulses a + * body to change its velocity. The idea of the Sequential Impulse technique is to apply + * impulses to bodies of each constraints in order to keep the constraint satisfied. + * + * --- Step 1 --- + * + * First, we int32_tegrate the applied force F_a acting of each rigid body (like gravity, ...) and + * we obtain some new velocities v2' that tends to violate the constraints. + * + * v2' = v1 + dt * M^-1 * F_a + * + * where M is a matrix that contains mass and inertia tensor information. + * + * --- Step 2 --- + * + * During the second step, we iterate over all the constraints for a certain number of + * iterations and for each constraint we compute the impulse to apply to the bodies needed + * so that the new velocity of the bodies satisfy Jv + b = 0. From the Newton law, we know that + * M * deltaV = P_c where M is the mass of the body, deltaV is the difference of velocity and + * P_c is the constraint impulse to apply to the body. Therefore, we have + * v2 = v2' + M^-1 * P_c. For each constraint, we can compute the Lagrange multiplier lambda + * using : lambda = -m_c (Jv2' + b) where m_c = 1 / (J * M^-1 * J^t). Now that we have the + * Lagrange multiplier lambda, we can compute the impulse P_c = J^t * lambda * dt to apply to + * the bodies to satisfy the constraint. + * + * --- Step 3 --- + * + * In the third step, we int32_tegrate the new position x2 of the bodies using the new velocities + * v2 computed in the second step with : x2 = x1 + dt * v2. + * + * Note that in the following code (as it is also explained in the slides from Erin Catto), + * the value lambda is not only the lagrange multiplier but is the multiplication of the + * Lagrange multiplier with the timestep dt. Therefore, in the following code, when we use + * lambda, we mean (lambda * dt). + * + * We are using the accumulated impulse technique that is also described in the slides from + * Erin Catto. + * + * We are also using warm starting. The idea is to warm start the solver at the beginning of + * each step by applying the last impulstes for the constraints that we already existing at the + * previous step. This allows the iterative solver to converge faster towards the solution. + * + * For contact constraints, we are also using split impulses so that the position correction + * that uses Baumgarte stabilization does not change the momentum of the bodies. + * + * There are two ways to apply the friction constraints. Either the friction constraints are + * applied at each contact point or they are applied only at the center of the contact manifold + * between two bodies. If we solve the friction constraints at each contact point, we need + * two constraints (two tangential friction directions) and if we solve the friction + * constraints at the center of the contact manifold, we need two constraints for tangential + * friction but also another twist friction constraint to prevent spin of the body around the + * contact manifold center. + */ + class ConstraintSolver { + private : + const std::map& m_mapBodyToConstrainedVelocityIndex; //!< Reference to the map that associates rigid body to their index in the constrained velocities array + float m_timeStep; //!< Current time step + bool m_isWarmStartingActive; //!< True if the warm starting of the solver is active + ConstraintSolverData m_constraintSolverData; //!< Constraint solver data used to initialize and solve the constraints + public : + /// Constructor + ConstraintSolver(const std::map& mapBodyToVelocityIndex); + /// Initialize the constraint solver for a given island + void initializeForIsland(float dt, Island* island); + /// Solve the constraints + void solveVelocityConstraints(Island* island); + /// Solve the position constraints + void solvePositionConstraints(Island* island); + /// Return true if the Non-Linear-Gauss-Seidel position correction technique is active + bool getIsNonLinearGaussSeidelPositionCorrectionActive() const; + /// Enable/Disable the Non-Linear-Gauss-Seidel position correction technique. + void setIsNonLinearGaussSeidelPositionCorrectionActive(bool isActive); + /// Set the constrained velocities arrays + void setConstrainedVelocitiesArrays(vec3* constrainedLinearVelocities, + vec3* constrainedAngularVelocities); + /// Set the constrained positions/orientations arrays + void setConstrainedPositionsArrays(vec3* constrainedPositions, + etk::Quaternion* constrainedOrientations); + }; // Set the constrained velocities arrays inline void ConstraintSolver::setConstrainedVelocitiesArrays(vec3* constrainedLinearVelocities, diff --git a/ephysics/engine/ContactSolver.cpp b/ephysics/engine/ContactSolver.cpp index c720a92..e3ab90d 100644 --- a/ephysics/engine/ContactSolver.cpp +++ b/ephysics/engine/ContactSolver.cpp @@ -22,10 +22,10 @@ const float ContactSolver::SLOP= float(0.01); // Constructor ContactSolver::ContactSolver(const std::map& mapBodyToVelocityIndex) :m_splitLinearVelocities(nullptr), m_splitAngularVelocities(nullptr), - mContactConstraints(nullptr), mLinearVelocities(nullptr), mAngularVelocities(nullptr), + m_contactConstraints(nullptr), m_linearVelocities(nullptr), m_angularVelocities(nullptr), m_mapBodyToConstrainedVelocityIndex(mapBodyToVelocityIndex), - mIsWarmStartingActive(true), mIsSplitImpulseActive(true), - mIsSolveFrictionAtContactManifoldCenterActive(true) { + m_isWarmStartingActive(true), m_isSplitImpulseActive(true), + m_isSolveFrictionAtContactManifoldCenterActive(true) { } @@ -50,8 +50,8 @@ void ContactSolver::initializeForIsland(float dt, Island* island) { m_numberContactManifolds = island->getNbContactManifolds(); - mContactConstraints = new ContactManifoldSolver[m_numberContactManifolds]; - assert(mContactConstraints != nullptr); + m_contactConstraints = new ContactManifoldSolver[m_numberContactManifolds]; + assert(m_contactConstraints != nullptr); // For each contact manifold of the island ContactManifold** contactManifolds = island->getContactManifold(); @@ -59,7 +59,7 @@ void ContactSolver::initializeForIsland(float dt, Island* island) { ContactManifold* externalManifold = contactManifolds[i]; - ContactManifoldSolver& int32_ternalManifold = mContactConstraints[i]; + ContactManifoldSolver& int32_ternalManifold = m_contactConstraints[i]; assert(externalManifold->getNbContactPoints() > 0); @@ -90,7 +90,7 @@ void ContactSolver::initializeForIsland(float dt, Island* island) { int32_ternalManifold.isBody2DynamicType = body2->getType() == DYNAMIC; // If we solve the friction constraints at the center of the contact manifold - if (mIsSolveFrictionAtContactManifoldCenterActive) { + if (m_isSolveFrictionAtContactManifoldCenterActive) { int32_ternalManifold.frictionPointBody1 = vec3(0.0f,0.0f,0.0f); int32_ternalManifold.frictionPointBody2 = vec3(0.0f,0.0f,0.0f); } @@ -122,14 +122,14 @@ void ContactSolver::initializeForIsland(float dt, Island* island) { contactPoint.rollingResistanceImpulse = vec3(0.0f,0.0f,0.0f); // If we solve the friction constraints at the center of the contact manifold - if (mIsSolveFrictionAtContactManifoldCenterActive) { + if (m_isSolveFrictionAtContactManifoldCenterActive) { int32_ternalManifold.frictionPointBody1 += p1; int32_ternalManifold.frictionPointBody2 += p2; } } // If we solve the friction constraints at the center of the contact manifold - if (mIsSolveFrictionAtContactManifoldCenterActive) { + if (m_isSolveFrictionAtContactManifoldCenterActive) { int32_ternalManifold.frictionPointBody1 /=static_cast(int32_ternalManifold.nbContacts); int32_ternalManifold.frictionPointBody2 /=static_cast(int32_ternalManifold.nbContacts); @@ -139,7 +139,7 @@ void ContactSolver::initializeForIsland(float dt, Island* island) { int32_ternalManifold.oldFrictionvec2 = externalManifold->getFrictionvec2(); // If warm starting is active - if (mIsWarmStartingActive) { + if (m_isWarmStartingActive) { // Initialize the accumulated impulses with the previous step accumulated impulses int32_ternalManifold.friction1Impulse = externalManifold->getFrictionImpulse1(); @@ -167,22 +167,22 @@ void ContactSolver::initializeContactConstraints() { // For each contact constraint for (uint32_t c=0; cgetPenetrationImpulse(); @@ -259,7 +259,7 @@ void ContactSolver::initializeContactConstraints() { contactPoint.penetrationSplitImpulse = 0.0; // If we solve the friction constraints at the center of the contact manifold - if (mIsSolveFrictionAtContactManifoldCenterActive) { + if (m_isSolveFrictionAtContactManifoldCenterActive) { manifold.normal += contactPoint.normal; } } @@ -272,7 +272,7 @@ void ContactSolver::initializeContactConstraints() { } // If we solve the friction constraints at the center of the contact manifold - if (mIsSolveFrictionAtContactManifoldCenterActive) { + if (m_isSolveFrictionAtContactManifoldCenterActive) { manifold.normal.normalize(); @@ -320,12 +320,12 @@ void ContactSolver::initializeContactConstraints() { void ContactSolver::warmStart() { // Check that warm starting is active - if (!mIsWarmStartingActive) return; + if (!m_isWarmStartingActive) return; // For each constraint for (uint32_t c=0; c SLOP) biasPenetrationDepth = -(beta/m_timeStep) * max(0.0f, float(contactPoint.penetrationDepth - SLOP)); float b = biasPenetrationDepth + contactPoint.restitutionBias; // Compute the Lagrange multiplier lambda - if (mIsSplitImpulseActive) { + if (m_isSplitImpulseActive) { deltaLambda = - (Jv + contactPoint.restitutionBias) * contactPoint.inversePenetrationMass; } @@ -548,7 +548,7 @@ void ContactSolver::solve() { sumPenetrationImpulse += contactPoint.penetrationImpulse; // If the split impulse position correction is active - if (mIsSplitImpulseActive) { + if (m_isSplitImpulseActive) { // Split impulse (position correction) const vec3& v1Split = m_splitLinearVelocities[contactManifold.indexBody1]; @@ -574,7 +574,7 @@ void ContactSolver::solve() { } // If we do not solve the friction constraints at the center of the contact manifold - if (!mIsSolveFrictionAtContactManifoldCenterActive) { + if (!m_isSolveFrictionAtContactManifoldCenterActive) { // --------- Friction 1 --------- // @@ -650,7 +650,7 @@ void ContactSolver::solve() { } // If we solve the friction constraints at the center of the contact manifold - if (mIsSolveFrictionAtContactManifoldCenterActive) { + if (m_isSolveFrictionAtContactManifoldCenterActive) { // ------ First friction constraint at the center of the contact manifol ------ // @@ -768,7 +768,7 @@ void ContactSolver::storeImpulses() { // For each contact manifold for (uint32_t c=0; c #include #include @@ -15,419 +14,228 @@ #include #include -/// ReactPhysics3D namespace namespace ephysics { - - -// Class Contact Solver -/** - * This class represents the contact solver that is used to solve rigid bodies contacts. - * The constraint solver is based on the "Sequential Impulse" technique described by - * Erin Catto in his GDC slides (http://code.google.com/p/box2d/downloads/list). - * - * A constraint between two bodies is represented by a function C(x) which is equal to zero - * when the constraint is satisfied. The condition C(x)=0 describes a valid position and the - * condition dC(x)/dt=0 describes a valid velocity. We have dC(x)/dt = Jv + b = 0 where J is - * the Jacobian matrix of the constraint, v is a vector that contains the velocity of both - * bodies and b is the constraint bias. We are looking for a force F_c that will act on the - * bodies to keep the constraint satisfied. Note that from the virtual work principle, we have - * F_c = J^t * lambda where J^t is the transpose of the Jacobian matrix and lambda is a - * Lagrange multiplier. Therefore, finding the force F_c is equivalent to finding the Lagrange - * multiplier lambda. - * - * An impulse P = F * dt where F is a force and dt is the timestep. We can apply impulses a - * body to change its velocity. The idea of the Sequential Impulse technique is to apply - * impulses to bodies of each constraints in order to keep the constraint satisfied. - * - * --- Step 1 --- - * - * First, we int32_tegrate the applied force F_a acting of each rigid body (like gravity, ...) and - * we obtain some new velocities v2' that tends to violate the constraints. - * - * v2' = v1 + dt * M^-1 * F_a - * - * where M is a matrix that contains mass and inertia tensor information. - * - * --- Step 2 --- - * - * During the second step, we iterate over all the constraints for a certain number of - * iterations and for each constraint we compute the impulse to apply to the bodies needed - * so that the new velocity of the bodies satisfy Jv + b = 0. From the Newton law, we know that - * M * deltaV = P_c where M is the mass of the body, deltaV is the difference of velocity and - * P_c is the constraint impulse to apply to the body. Therefore, we have - * v2 = v2' + M^-1 * P_c. For each constraint, we can compute the Lagrange multiplier lambda - * using : lambda = -m_c (Jv2' + b) where m_c = 1 / (J * M^-1 * J^t). Now that we have the - * Lagrange multiplier lambda, we can compute the impulse P_c = J^t * lambda * dt to apply to - * the bodies to satisfy the constraint. - * - * --- Step 3 --- - * - * In the third step, we int32_tegrate the new position x2 of the bodies using the new velocities - * v2 computed in the second step with : x2 = x1 + dt * v2. - * - * Note that in the following code (as it is also explained in the slides from Erin Catto), - * the value lambda is not only the lagrange multiplier but is the multiplication of the - * Lagrange multiplier with the timestep dt. Therefore, in the following code, when we use - * lambda, we mean (lambda * dt). - * - * We are using the accumulated impulse technique that is also described in the slides from - * Erin Catto. - * - * We are also using warm starting. The idea is to warm start the solver at the beginning of - * each step by applying the last impulstes for the constraints that we already existing at the - * previous step. This allows the iterative solver to converge faster towards the solution. - * - * For contact constraints, we are also using split impulses so that the position correction - * that uses Baumgarte stabilization does not change the momentum of the bodies. - * - * There are two ways to apply the friction constraints. Either the friction constraints are - * applied at each contact point or they are applied only at the center of the contact manifold - * between two bodies. If we solve the friction constraints at each contact point, we need - * two constraints (two tangential friction directions) and if we solve the friction - * constraints at the center of the contact manifold, we need two constraints for tangential - * friction but also another twist friction constraint to prevent spin of the body around the - * contact manifold center. - */ -class ContactSolver { - - private: - - // Structure ContactPointSolver - /** - * Contact solver int32_ternal data structure that to store all the - * information relative to a contact point - */ - struct ContactPointSolver { - - /// Accumulated normal impulse - float penetrationImpulse; - - /// Accumulated impulse in the 1st friction direction - float friction1Impulse; - - /// Accumulated impulse in the 2nd friction direction - float friction2Impulse; - - /// Accumulated split impulse for penetration correction - float penetrationSplitImpulse; - - /// Accumulated rolling resistance impulse - vec3 rollingResistanceImpulse; - - /// Normal vector of the contact - vec3 normal; - - /// First friction vector in the tangent plane - vec3 frictionVector1; - - /// Second friction vector in the tangent plane - vec3 frictionvec2; - - /// Old first friction vector in the tangent plane - vec3 oldFrictionVector1; - - /// Old second friction vector in the tangent plane - vec3 oldFrictionvec2; - - /// Vector from the body 1 center to the contact point - vec3 r1; - - /// Vector from the body 2 center to the contact point - vec3 r2; - - /// Cross product of r1 with 1st friction vector - vec3 r1CrossT1; - - /// Cross product of r1 with 2nd friction vector - vec3 r1CrossT2; - - /// Cross product of r2 with 1st friction vector - vec3 r2CrossT1; - - /// Cross product of r2 with 2nd friction vector - vec3 r2CrossT2; - - /// Cross product of r1 with the contact normal - vec3 r1CrossN; - - /// Cross product of r2 with the contact normal - vec3 r2CrossN; - - /// Penetration depth - float penetrationDepth; - - /// Velocity restitution bias - float restitutionBias; - - /// Inverse of the matrix K for the penenetration - float inversePenetrationMass; - - /// Inverse of the matrix K for the 1st friction - float inverseFriction1Mass; - - /// Inverse of the matrix K for the 2nd friction - float inverseFriction2Mass; - - /// True if the contact was existing last time step - bool isRestingContact; - - /// Pointer to the external contact - ContactPoint* externalContact; - }; - - // Structure ContactManifoldSolver - /** - * Contact solver int32_ternal data structure to store all the - * information relative to a contact manifold. - */ - struct ContactManifoldSolver { - - /// Index of body 1 in the constraint solver - uint32_t indexBody1; - - /// Index of body 2 in the constraint solver - uint32_t indexBody2; - - /// Inverse of the mass of body 1 - float massInverseBody1; - - // Inverse of the mass of body 2 - float massInverseBody2; - - /// Inverse inertia tensor of body 1 - etk::Matrix3x3 inverseInertiaTensorBody1; - - /// Inverse inertia tensor of body 2 - etk::Matrix3x3 inverseInertiaTensorBody2; - - /// Contact point constraints - ContactPointSolver contacts[MAX_CONTACT_POINTS_IN_MANIFOLD]; - - /// Number of contact points - uint32_t nbContacts; - - /// True if the body 1 is of type dynamic - bool isBody1DynamicType; - - /// True if the body 2 is of type dynamic - bool isBody2DynamicType; - - /// Mix of the restitution factor for two bodies - float restitutionFactor; - - /// Mix friction coefficient for the two bodies - float frictionCoefficient; - - /// Rolling resistance factor between the two bodies - float rollingResistanceFactor; - - /// Pointer to the external contact manifold - ContactManifold* externalContactManifold; - - // - Variables used when friction constraints are apply at the center of the manifold-// - - /// Average normal vector of the contact manifold - vec3 normal; - - /// Point on body 1 where to apply the friction constraints - vec3 frictionPointBody1; - - /// Point on body 2 where to apply the friction constraints - vec3 frictionPointBody2; - - /// R1 vector for the friction constraints - vec3 r1Friction; - - /// R2 vector for the friction constraints - vec3 r2Friction; - - /// Cross product of r1 with 1st friction vector - vec3 r1CrossT1; - - /// Cross product of r1 with 2nd friction vector - vec3 r1CrossT2; - - /// Cross product of r2 with 1st friction vector - vec3 r2CrossT1; - - /// Cross product of r2 with 2nd friction vector - vec3 r2CrossT2; - - /// Matrix K for the first friction constraint - float inverseFriction1Mass; - - /// Matrix K for the second friction constraint - float inverseFriction2Mass; - - /// Matrix K for the twist friction constraint - float inverseTwistFrictionMass; - - /// Matrix K for the rolling resistance constraint - etk::Matrix3x3 inverseRollingResistance; - - /// First friction direction at contact manifold center - vec3 frictionVector1; - - /// Second friction direction at contact manifold center - vec3 frictionvec2; - - /// Old 1st friction direction at contact manifold center - vec3 oldFrictionVector1; - - /// Old 2nd friction direction at contact manifold center - vec3 oldFrictionvec2; - - /// First friction direction impulse at manifold center - float friction1Impulse; - - /// Second friction direction impulse at manifold center - float friction2Impulse; - - /// Twist friction impulse at contact manifold center - float frictionTwistImpulse; - - /// Rolling resistance impulse - vec3 rollingResistanceImpulse; - }; - - // -------------------- Constants --------------------- // - - /// Beta value for the penetration depth position correction without split impulses - static const float BETA; - - /// Beta value for the penetration depth position correction with split impulses - static const float BETA_SPLIT_IMPULSE; - - /// Slop distance (allowed penetration distance between bodies) - static const float SLOP; - - // -------------------- Attributes -------------------- // - - /// Split linear velocities for the position contact solver (split impulse) - vec3* m_splitLinearVelocities; - - /// Split angular velocities for the position contact solver (split impulse) - vec3* m_splitAngularVelocities; - - /// Current time step - float m_timeStep; - - /// Contact constraints - ContactManifoldSolver* mContactConstraints; - - /// Number of contact constraints - uint32_t m_numberContactManifolds; - - /// Array of linear velocities - vec3* mLinearVelocities; - - /// Array of angular velocities - vec3* mAngularVelocities; - - /// Reference to the map of rigid body to their index in the constrained velocities array - const std::map& 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& mapBodyToVelocityIndex); - - /// Destructor - virtual ~ContactSolver(); - - /// Initialize the constraint solver for a given island - void initializeForIsland(float dt, Island* island); - - /// Set the split velocities arrays - void setSplitVelocitiesArrays(vec3* splitLinearVelocities, - vec3* splitAngularVelocities); - - /// Set the constrained velocities arrays - void setConstrainedVelocitiesArrays(vec3* constrainedLinearVelocities, - vec3* constrainedAngularVelocities); - - /// Warm start the solver. - void warmStart(); - - /// Store the computed impulses to use them to - /// warm start the solver at the next iteration - void storeImpulses(); - - /// Solve the contacts - void solve(); - - /// Return true if the split impulses position correction technique is used for contacts - bool isSplitImpulseActive() const; - - /// Activate or Deactivate the split impulses for contacts - void setIsSplitImpulseActive(bool isActive); - - /// Activate or deactivate the solving of friction constraints at the center of - /// the contact manifold instead of solving them at each contact point - void setIsSolveFrictionAtContactManifoldCenterActive(bool isActive); - - /// Clean up the constraint solver - void cleanup(); -}; + /** + * @brief This class represents the contact solver that is used to solve rigid bodies contacts. + * The constraint solver is based on the "Sequential Impulse" technique described by + * Erin Catto in his GDC slides (http://code.google.com/p/box2d/downloads/list). + * + * A constraint between two bodies is represented by a function C(x) which is equal to zero + * when the constraint is satisfied. The condition C(x)=0 describes a valid position and the + * condition dC(x)/dt=0 describes a valid velocity. We have dC(x)/dt = Jv + b = 0 where J is + * the Jacobian matrix of the constraint, v is a vector that contains the velocity of both + * bodies and b is the constraint bias. We are looking for a force F_c that will act on the + * bodies to keep the constraint satisfied. Note that from the virtual work principle, we have + * F_c = J^t * lambda where J^t is the transpose of the Jacobian matrix and lambda is a + * Lagrange multiplier. Therefore, finding the force F_c is equivalent to finding the Lagrange + * multiplier lambda. + * + * An impulse P = F * dt where F is a force and dt is the timestep. We can apply impulses a + * body to change its velocity. The idea of the Sequential Impulse technique is to apply + * impulses to bodies of each constraints in order to keep the constraint satisfied. + * + * --- Step 1 --- + * + * First, we int32_tegrate the applied force F_a acting of each rigid body (like gravity, ...) and + * we obtain some new velocities v2' that tends to violate the constraints. + * + * v2' = v1 + dt * M^-1 * F_a + * + * where M is a matrix that contains mass and inertia tensor information. + * + * --- Step 2 --- + * + * During the second step, we iterate over all the constraints for a certain number of + * iterations and for each constraint we compute the impulse to apply to the bodies needed + * so that the new velocity of the bodies satisfy Jv + b = 0. From the Newton law, we know that + * M * deltaV = P_c where M is the mass of the body, deltaV is the difference of velocity and + * P_c is the constraint impulse to apply to the body. Therefore, we have + * v2 = v2' + M^-1 * P_c. For each constraint, we can compute the Lagrange multiplier lambda + * using : lambda = -m_c (Jv2' + b) where m_c = 1 / (J * M^-1 * J^t). Now that we have the + * Lagrange multiplier lambda, we can compute the impulse P_c = J^t * lambda * dt to apply to + * the bodies to satisfy the constraint. + * + * --- Step 3 --- + * + * In the third step, we int32_tegrate the new position x2 of the bodies using the new velocities + * v2 computed in the second step with : x2 = x1 + dt * v2. + * + * Note that in the following code (as it is also explained in the slides from Erin Catto), + * the value lambda is not only the lagrange multiplier but is the multiplication of the + * Lagrange multiplier with the timestep dt. Therefore, in the following code, when we use + * lambda, we mean (lambda * dt). + * + * We are using the accumulated impulse technique that is also described in the slides from + * Erin Catto. + * + * We are also using warm starting. The idea is to warm start the solver at the beginning of + * each step by applying the last impulstes for the constraints that we already existing at the + * previous step. This allows the iterative solver to converge faster towards the solution. + * + * For contact constraints, we are also using split impulses so that the position correction + * that uses Baumgarte stabilization does not change the momentum of the bodies. + * + * There are two ways to apply the friction constraints. Either the friction constraints are + * applied at each contact point or they are applied only at the center of the contact manifold + * between two bodies. If we solve the friction constraints at each contact point, we need + * two constraints (two tangential friction directions) and if we solve the friction + * constraints at the center of the contact manifold, we need two constraints for tangential + * friction but also another twist friction constraint to prevent spin of the body around the + * contact manifold center. + */ + class ContactSolver { + private: + /** + * Contact solver int32_ternal data structure that to store all the + * information relative to a contact point + */ + struct ContactPointSolver { + float penetrationImpulse; //!< Accumulated normal impulse + float friction1Impulse; //!< Accumulated impulse in the 1st friction direction + float friction2Impulse; //!< Accumulated impulse in the 2nd friction direction + float penetrationSplitImpulse; //!< Accumulated split impulse for penetration correction + vec3 rollingResistanceImpulse; //!< Accumulated rolling resistance impulse + vec3 normal; //!< Normal vector of the contact + vec3 frictionVector1; //!< First friction vector in the tangent plane + vec3 frictionvec2; //!< Second friction vector in the tangent plane + vec3 oldFrictionVector1; //!< Old first friction vector in the tangent plane + vec3 oldFrictionvec2; //!< Old second friction vector in the tangent plane + vec3 r1; //!< Vector from the body 1 center to the contact point + vec3 r2; //!< Vector from the body 2 center to the contact point + vec3 r1CrossT1; //!< Cross product of r1 with 1st friction vector + vec3 r1CrossT2; //!< Cross product of r1 with 2nd friction vector + vec3 r2CrossT1; //!< Cross product of r2 with 1st friction vector + vec3 r2CrossT2; //!< Cross product of r2 with 2nd friction vector + vec3 r1CrossN; //!< Cross product of r1 with the contact normal + vec3 r2CrossN; //!< Cross product of r2 with the contact normal + float penetrationDepth; //!< Penetration depth + float restitutionBias; //!< Velocity restitution bias + float inversePenetrationMass; //!< Inverse of the matrix K for the penenetration + float inverseFriction1Mass; //!< Inverse of the matrix K for the 1st friction + float inverseFriction2Mass; //!< Inverse of the matrix K for the 2nd friction + bool isRestingContact; //!< True if the contact was existing last time step + ContactPoint* externalContact; //!< Pointer to the external contact + }; + + /** + * @brief Contact solver int32_ternal data structure to store all the information relative to a contact manifold. + */ + struct ContactManifoldSolver { + uint32_t indexBody1; //!< Index of body 1 in the constraint solver + uint32_t indexBody2; //!< Index of body 2 in the constraint solver + float massInverseBody1; //!< Inverse of the mass of body 1 + float massInverseBody2; //!< Inverse of the mass of body 2 + etk::Matrix3x3 inverseInertiaTensorBody1; //!< Inverse inertia tensor of body 1 + etk::Matrix3x3 inverseInertiaTensorBody2; //!< Inverse inertia tensor of body 2 + ContactPointSolver contacts[MAX_CONTACT_POINTS_IN_MANIFOLD]; //!< Contact point constraints + uint32_t nbContacts; //!< Number of contact points + bool isBody1DynamicType; //!< True if the body 1 is of type dynamic + bool isBody2DynamicType; //!< True if the body 2 is of type dynamic + float restitutionFactor; //!< Mix of the restitution factor for two bodies + float frictionCoefficient; //!< Mix friction coefficient for the two bodies + float rollingResistanceFactor; //!< Rolling resistance factor between the two bodies + ContactManifold* externalContactManifold; //!< Pointer to the external contact manifold + // - Variables used when friction constraints are apply at the center of the manifold-// + vec3 normal; //!< Average normal vector of the contact manifold + vec3 frictionPointBody1; //!< Point on body 1 where to apply the friction constraints + vec3 frictionPointBody2; //!< Point on body 2 where to apply the friction constraints + vec3 r1Friction; //!< R1 vector for the friction constraints + vec3 r2Friction; //!< R2 vector for the friction constraints + vec3 r1CrossT1; //!< Cross product of r1 with 1st friction vector + vec3 r1CrossT2; //!< Cross product of r1 with 2nd friction vector + vec3 r2CrossT1; //!< Cross product of r2 with 1st friction vector + vec3 r2CrossT2; //!< Cross product of r2 with 2nd friction vector + float inverseFriction1Mass; //!< Matrix K for the first friction constraint + float inverseFriction2Mass; //!< Matrix K for the second friction constraint + float inverseTwistFrictionMass; //!< Matrix K for the twist friction constraint + etk::Matrix3x3 inverseRollingResistance; //!< Matrix K for the rolling resistance constraint + vec3 frictionVector1; //!< First friction direction at contact manifold center + vec3 frictionvec2; //!< Second friction direction at contact manifold center + vec3 oldFrictionVector1; //!< Old 1st friction direction at contact manifold center + vec3 oldFrictionvec2; //!< Old 2nd friction direction at contact manifold center + float friction1Impulse; //!< First friction direction impulse at manifold center + float friction2Impulse; //!< Second friction direction impulse at manifold center + float frictionTwistImpulse; //!< Twist friction impulse at contact manifold center + vec3 rollingResistanceImpulse; //!< Rolling resistance impulse + }; + static const float BETA; //!< Beta value for the penetration depth position correction without split impulses + static const float BETA_SPLIT_IMPULSE; //!< Beta value for the penetration depth position correction with split impulses + static const float SLOP; //!< Slop distance (allowed penetration distance between bodies) + vec3* m_splitLinearVelocities; //!< Split linear velocities for the position contact solver (split impulse) + vec3* m_splitAngularVelocities; //!< Split angular velocities for the position contact solver (split impulse) + float m_timeStep; //!< Current time step + ContactManifoldSolver* m_contactConstraints; //!< Contact constraints + uint32_t m_numberContactManifolds; //!< Number of contact constraints + vec3* m_linearVelocities; //!< Array of linear velocities + vec3* m_angularVelocities; //!< Array of angular velocities + const std::map& m_mapBodyToConstrainedVelocityIndex; //!< Reference to the map of rigid body to their index in the constrained velocities array + bool m_isWarmStartingActive; //!< True if the warm starting of the solver is active + bool m_isSplitImpulseActive; //!< True if the split impulse position correction is active + bool m_isSolveFrictionAtContactManifoldCenterActive; //!< True if we solve 3 friction constraints at the contact manifold center only instead of 2 friction constraints at each contact point + /// Initialize the contact constraints before solving the system + void initializeContactConstraints(); + /// Apply an impulse to the two bodies of a constraint + void applyImpulse(const Impulse& impulse, const ContactManifoldSolver& manifold); + /// Apply an impulse to the two bodies of a constraint + void applySplitImpulse(const Impulse& impulse, + const ContactManifoldSolver& manifold); + /// Compute the collision restitution factor from the restitution factor of each body + float computeMixedRestitutionFactor(RigidBody *body1, + RigidBody *body2) const; + /// Compute the mixed friction coefficient from the friction coefficient of each body + float computeMixedFrictionCoefficient(RigidBody* body1, + RigidBody* body2) const; + /// Compute th mixed rolling resistance factor between two bodies + float computeMixedRollingResistance(RigidBody* body1, RigidBody* body2) const; + /// Compute the two unit orthogonal vectors "t1" and "t2" that span the tangential friction + /// plane for a contact point. The two vectors have to be + /// such that : t1 x t2 = contactNormal. + void computeFrictionVectors(const vec3& deltaVelocity, + ContactPointSolver &contactPoint) const; + /// Compute the two unit orthogonal vectors "t1" and "t2" that span the tangential friction + /// plane for a contact manifold. The two vectors have to be + /// such that : t1 x t2 = contactNormal. + void computeFrictionVectors(const vec3& deltaVelocity, + ContactManifoldSolver& contactPoint) const; + /// Compute a penetration constraint impulse + const Impulse computePenetrationImpulse(float deltaLambda, + const ContactPointSolver& contactPoint) const; + /// Compute the first friction constraint impulse + const Impulse computeFriction1Impulse(float deltaLambda, + const ContactPointSolver& contactPoint) const; + /// Compute the second friction constraint impulse + const Impulse computeFriction2Impulse(float deltaLambda, + const ContactPointSolver& contactPoint) const; + public: + /// Constructor + ContactSolver(const std::map& mapBodyToVelocityIndex); + /// Destructor + virtual ~ContactSolver(); + /// Initialize the constraint solver for a given island + void initializeForIsland(float dt, Island* island); + /// Set the split velocities arrays + void setSplitVelocitiesArrays(vec3* splitLinearVelocities, + vec3* splitAngularVelocities); + /// Set the constrained velocities arrays + void setConstrainedVelocitiesArrays(vec3* constrainedLinearVelocities, + vec3* constrainedAngularVelocities); + /// Warm start the solver. + void warmStart(); + /// Store the computed impulses to use them to + /// warm start the solver at the next iteration + void storeImpulses(); + /// Solve the contacts + void solve(); + /// Return true if the split impulses position correction technique is used for contacts + bool isSplitImpulseActive() const; + /// Activate or Deactivate the split impulses for contacts + void setIsSplitImpulseActive(bool isActive); + /// Activate or deactivate the solving of friction constraints at the center of + /// the contact manifold instead of solving them at each contact point + void setIsSolveFrictionAtContactManifoldCenterActive(bool isActive); + /// Clean up the constraint solver + void cleanup(); + }; // Set the split velocities arrays inline void ContactSolver::setSplitVelocitiesArrays(vec3* splitLinearVelocities, @@ -443,24 +251,24 @@ inline void ContactSolver::setConstrainedVelocitiesArrays(vec3* constrainedLinea vec3* constrainedAngularVelocities) { assert(constrainedLinearVelocities != NULL); assert(constrainedAngularVelocities != NULL); - mLinearVelocities = constrainedLinearVelocities; - mAngularVelocities = constrainedAngularVelocities; + m_linearVelocities = constrainedLinearVelocities; + m_angularVelocities = constrainedAngularVelocities; } // Return true if the split impulses position correction technique is used for contacts inline bool ContactSolver::isSplitImpulseActive() const { - return mIsSplitImpulseActive; + return m_isSplitImpulseActive; } // Activate or Deactivate the split impulses for contacts inline void ContactSolver::setIsSplitImpulseActive(bool isActive) { - mIsSplitImpulseActive = isActive; + m_isSplitImpulseActive = isActive; } // Activate or deactivate the solving of friction constraints at the center of // the contact manifold instead of solving them at each contact point inline void ContactSolver::setIsSolveFrictionAtContactManifoldCenterActive(bool isActive) { - mIsSolveFrictionAtContactManifoldCenterActive = isActive; + m_isSolveFrictionAtContactManifoldCenterActive = isActive; } // Compute the collision restitution factor from the restitution factor of each body diff --git a/ephysics/engine/DynamicsWorld.hpp b/ephysics/engine/DynamicsWorld.hpp index eaf4f30..57402dd 100644 --- a/ephysics/engine/DynamicsWorld.hpp +++ b/ephysics/engine/DynamicsWorld.hpp @@ -5,7 +5,6 @@ */ #pragma once -// Libraries #include #include #include @@ -14,275 +13,157 @@ #include #include -/// Namespace ReactPhysics3D namespace ephysics { - -// Class DynamicsWorld -/** - * This class represents a dynamics world. This class inherits from - * the CollisionWorld class. In a dynamics world, bodies can collide - * and their movements are simulated using the laws of physics. - */ -class DynamicsWorld : public CollisionWorld { - - protected : - - // -------------------- Attributes -------------------- // - - /// Contact solver - ContactSolver m_contactSolver; - - /// Constraint solver - ConstraintSolver m_constraintSolver; - - /// Number of iterations for the velocity solver of the Sequential Impulses technique - uint32_t m_nbVelocitySolverIterations; - - /// Number of iterations for the position solver of the Sequential Impulses technique - uint32_t m_nbPositionSolverIterations; - - /// True if the spleeping technique for inactive bodies is enabled - bool m_isSleepingEnabled; - - /// All the rigid bodies of the physics world - std::set m_rigidBodies; - - /// All the joints of the world - std::set m_joints; - - /// Gravity vector of the world - vec3 m_gravity; - - /// Current frame time step (in seconds) - float m_timeStep; - - /// True if the gravity force is on - bool m_isGravityEnabled; - - /// Array of constrained linear velocities (state of the linear velocities - /// after solving the constraints) - vec3* m_constrainedLinearVelocities; - - /// Array of constrained angular velocities (state of the angular velocities - /// after solving the constraints) - vec3* m_constrainedAngularVelocities; - - /// Split linear velocities for the position contact solver (split impulse) - vec3* m_splitLinearVelocities; - - /// Split angular velocities for the position contact solver (split impulse) - vec3* m_splitAngularVelocities; - - /// Array of constrained rigid bodies position (for position error correction) - vec3* m_constrainedPositions; - - /// Array of constrained rigid bodies orientation (for position error correction) - etk::Quaternion* m_constrainedOrientations; - - /// Map body to their index in the constrained velocities array - std::map m_mapBodyToConstrainedVelocityIndex; - - /// Number of islands in the world - uint32_t m_numberIslands; - - /// Current allocated capacity for the islands - uint32_t m_numberIslandsCapacity; - - /// Array with all the islands of awaken bodies - Island** m_islands; - - /// Current allocated capacity for the bodies - uint32_t m_numberBodiesCapacity; - - /// Sleep linear velocity threshold - float m_sleepLinearVelocity; - - /// Sleep angular velocity threshold - float m_sleepAngularVelocity; - - /// Time (in seconds) before a body is put to sleep if its velocity - /// becomes smaller than the sleep velocity. - float m_timeBeforeSleep; - - // -------------------- Methods -------------------- // - - /// Private copy-constructor - DynamicsWorld(const DynamicsWorld& world); - - /// Private assignment operator - DynamicsWorld& operator=(const DynamicsWorld& world); - - /// Integrate the positions and orientations of rigid bodies. - void int32_tegrateRigidBodiesPositions(); - - /// Update the AABBs of the bodies - void updateRigidBodiesAABB(); - - /// Reset the external force and torque applied to the bodies - void resetBodiesForceAndTorque(); - - /// Update the position and orientation of a body - void updatePositionAndOrientationOfBody(RigidBody* body, vec3 newLinVelocity, - vec3 newAngVelocity); - - /// Compute and set the int32_terpolation factor to all bodies - void setInterpolationFactorToAllBodies(); - - /// Initialize the bodies velocities arrays for the next simulation step. - void initVelocityArrays(); - - /// Integrate the velocities of rigid bodies. - void int32_tegrateRigidBodiesVelocities(); - - /// Solve the contacts and constraints - void solveContactsAndConstraints(); - - /// Solve the position error correction of the constraints - void solvePositionCorrection(); - - /// Cleanup the constrained velocities array at each step - void cleanupConstrainedVelocitiesArray(); - - /// Compute the islands of awake bodies. - void computeIslands(); - - /// Update the postion/orientation of the bodies - void updateBodiesState(); - - /// Put bodies to sleep if needed. - void updateSleepingBodies(); - - /// Add the joint to the list of joints of the two bodies involved in the joint - void addJointToBody(Joint* joint); - - public : - - // -------------------- Methods -------------------- // - - /// Constructor - DynamicsWorld(const vec3& m_gravity); - - /// Destructor - virtual ~DynamicsWorld(); - - /// Update the physics simulation - void update(float timeStep); - - /// Get the number of iterations for the velocity constraint solver - uint32_t getNbIterationsVelocitySolver() const; - - /// Set the number of iterations for the velocity constraint solver - void setNbIterationsVelocitySolver(uint32_t nbIterations); - - /// Get the number of iterations for the position constraint solver - uint32_t getNbIterationsPositionSolver() const; - - /// Set the number of iterations for the position constraint solver - void setNbIterationsPositionSolver(uint32_t nbIterations); - - /// Set the position correction technique used for contacts - void setContactsPositionCorrectionTechnique(ContactsPositionCorrectionTechnique technique); - - /// Set the position correction technique used for joints - void setJointsPositionCorrectionTechnique(JointsPositionCorrectionTechnique technique); - - /// Activate or deactivate the solving of friction constraints at the center of - /// the contact manifold instead of solving them at each contact point - void setIsSolveFrictionAtContactManifoldCenterActive(bool isActive); - - /// Create a rigid body int32_to the physics world. - RigidBody* createRigidBody(const etk::Transform3D& transform); - - /// Destroy a rigid body and all the joints which it belongs - void destroyRigidBody(RigidBody* rigidBody); - - /// Create a joint between two bodies in the world and return a pointer to the new joint - Joint* createJoint(const JointInfo& jointInfo); - - /// Destroy a joint - void destroyJoint(Joint* joint); - - /// Return the gravity vector of the world - vec3 getGravity() const; - - /// Set the gravity vector of the world - void setGravity(vec3& gravity); - - /// Return if the gravity is on - bool isGravityEnabled() const; - - /// Enable/Disable the gravity - void setIsGratityEnabled(bool isGravityEnabled); - - /// Return the number of rigid bodies in the world - uint32_t getNbRigidBodies() const; - - /// Return the number of joints in the world - uint32_t getNbJoints() const; - - /// Return an iterator to the beginning of the rigid bodies of the physics world - std::set::iterator getRigidBodiesBeginIterator(); - - /// Return an iterator to the end of the rigid bodies of the physics world - std::set::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 getContactsList() const; - - // -------------------- Friendship -------------------- // - - friend class RigidBody; -}; + /** + * @brief This class represents a dynamics world. This class inherits from + * the CollisionWorld class. In a dynamics world, bodies can collide + * and their movements are simulated using the laws of physics. + */ + class DynamicsWorld : public CollisionWorld { + protected : + ContactSolver m_contactSolver; //!< Contact solver + ConstraintSolver m_constraintSolver; //!< Constraint solver + uint32_t m_nbVelocitySolverIterations; //!< Number of iterations for the velocity solver of the Sequential Impulses technique + uint32_t m_nbPositionSolverIterations; //!< Number of iterations for the position solver of the Sequential Impulses technique + bool m_isSleepingEnabled; //!< True if the spleeping technique for inactive bodies is enabled + std::set m_rigidBodies; //!< All the rigid bodies of the physics world + std::set m_joints; //!< All the joints of the world + vec3 m_gravity; //!< Gravity vector of the world + float m_timeStep; //!< Current frame time step (in seconds) + bool m_isGravityEnabled; //!< True if the gravity force is on + vec3* m_constrainedLinearVelocities; //!< Array of constrained linear velocities (state of the linear velocities after solving the constraints) + vec3* m_constrainedAngularVelocities; //!< Array of constrained angular velocities (state of the angular velocities after solving the constraints) + vec3* m_splitLinearVelocities; //!< Split linear velocities for the position contact solver (split impulse) + vec3* m_splitAngularVelocities; //!< Split angular velocities for the position contact solver (split impulse) + vec3* m_constrainedPositions; //!< Array of constrained rigid bodies position (for position error correction) + etk::Quaternion* m_constrainedOrientations; //!< Array of constrained rigid bodies orientation (for position error correction) + std::map m_mapBodyToConstrainedVelocityIndex; //!< Map body to their index in the constrained velocities array + uint32_t m_numberIslands; //!< Number of islands in the world + uint32_t m_numberIslandsCapacity; //!< Current allocated capacity for the islands + Island** m_islands; //!< Array with all the islands of awaken bodies + uint32_t m_numberBodiesCapacity; //!< Current allocated capacity for the bodies + float m_sleepLinearVelocity; //!< Sleep linear velocity threshold + float m_sleepAngularVelocity; //!< Sleep angular velocity threshold + float m_timeBeforeSleep; //!< Time (in seconds) before a body is put to sleep if its velocity becomes smaller than the sleep velocity. + /// Private copy-constructor + DynamicsWorld(const DynamicsWorld& world); + /// Private assignment operator + DynamicsWorld& operator=(const DynamicsWorld& world); + /// Integrate the positions and orientations of rigid bodies. + void int32_tegrateRigidBodiesPositions(); + /// Update the AABBs of the bodies + void updateRigidBodiesAABB(); + /// Reset the external force and torque applied to the bodies + void resetBodiesForceAndTorque(); + /// Update the position and orientation of a body + void updatePositionAndOrientationOfBody(RigidBody* body, vec3 newLinVelocity, + vec3 newAngVelocity); + /// Compute and set the int32_terpolation factor to all bodies + void setInterpolationFactorToAllBodies(); + /// Initialize the bodies velocities arrays for the next simulation step. + void initVelocityArrays(); + /// Integrate the velocities of rigid bodies. + void int32_tegrateRigidBodiesVelocities(); + /// Solve the contacts and constraints + void solveContactsAndConstraints(); + /// Solve the position error correction of the constraints + void solvePositionCorrection(); + /// Cleanup the constrained velocities array at each step + void cleanupConstrainedVelocitiesArray(); + /// Compute the islands of awake bodies. + void computeIslands(); + /// Update the postion/orientation of the bodies + void updateBodiesState(); + /// Put bodies to sleep if needed. + void updateSleepingBodies(); + /// Add the joint to the list of joints of the two bodies involved in the joint + void addJointToBody(Joint* joint); + public : + /// Constructor + DynamicsWorld(const vec3& m_gravity); + /// Destructor + virtual ~DynamicsWorld(); + /// Update the physics simulation + void update(float timeStep); + /// Get the number of iterations for the velocity constraint solver + uint32_t getNbIterationsVelocitySolver() const; + /// Set the number of iterations for the velocity constraint solver + void setNbIterationsVelocitySolver(uint32_t nbIterations); + /// Get the number of iterations for the position constraint solver + uint32_t getNbIterationsPositionSolver() const; + /// Set the number of iterations for the position constraint solver + void setNbIterationsPositionSolver(uint32_t nbIterations); + /// Set the position correction technique used for contacts + void setContactsPositionCorrectionTechnique(ContactsPositionCorrectionTechnique technique); + /// Set the position correction technique used for joints + void setJointsPositionCorrectionTechnique(JointsPositionCorrectionTechnique technique); + /// Activate or deactivate the solving of friction constraints at the center of + /// the contact manifold instead of solving them at each contact point + void setIsSolveFrictionAtContactManifoldCenterActive(bool isActive); + /// Create a rigid body int32_to the physics world. + RigidBody* createRigidBody(const etk::Transform3D& transform); + /// Destroy a rigid body and all the joints which it belongs + void destroyRigidBody(RigidBody* rigidBody); + /// Create a joint between two bodies in the world and return a pointer to the new joint + Joint* createJoint(const JointInfo& jointInfo); + /// Destroy a joint + void destroyJoint(Joint* joint); + /// Return the gravity vector of the world + vec3 getGravity() const; + /// Set the gravity vector of the world + void setGravity(vec3& gravity); + /// Return if the gravity is on + bool isGravityEnabled() const; + /// Enable/Disable the gravity + void setIsGratityEnabled(bool isGravityEnabled); + /// Return the number of rigid bodies in the world + uint32_t getNbRigidBodies() const; + /// Return the number of joints in the world + uint32_t getNbJoints() const; + /// Return an iterator to the beginning of the rigid bodies of the physics world + std::set::iterator getRigidBodiesBeginIterator(); + /// Return an iterator to the end of the rigid bodies of the physics world + std::set::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 getContactsList() const; + friend class RigidBody; + }; // Reset the external force and torque applied to the bodies inline void DynamicsWorld::resetBodiesForceAndTorque() { diff --git a/ephysics/engine/EventListener.hpp b/ephysics/engine/EventListener.hpp index b4ed74b..35e0c0d 100644 --- a/ephysics/engine/EventListener.hpp +++ b/ephysics/engine/EventListener.hpp @@ -5,52 +5,49 @@ */ #pragma once -// Libraries #include namespace ephysics { - -// Class EventListener -/** - * This class can be used to receive event callbacks from the physics engine. - * In order to receive callbacks, you need to create a new class that inherits from - * this one and you must override the methods you need. Then, you need to register your - * new event listener class to the physics world using the DynamicsWorld::setEventListener() - * method. - */ -class EventListener { - - public : - - /// Constructor - EventListener() {} - - /// Destructor - virtual ~EventListener() {} - - /// Called when a new contact point is found between two bodies that were separated before - /** - * @param contact Information about the contact - */ - virtual void beginContact(const ContactPointInfo& contact) {}; - - /// Called when a new contact point is found between two bodies - /** - * @param contact Information about the contact - */ - virtual void newContact(const ContactPointInfo& contact) {} - - /// Called at the beginning of an int32_ternal tick of the simulation step. - /// Each time the DynamicsWorld::update() method is called, the physics - /// engine will do several int32_ternal simulation steps. This method is - /// called at the beginning of each int32_ternal simulation step. - virtual void beginInternalTick() {} - - /// Called at the end of an int32_ternal tick of the simulation step. - /// Each time the DynamicsWorld::update() metho is called, the physics - /// engine will do several int32_ternal simulation steps. This method is - /// called at the end of each int32_ternal simulation step. - virtual void endInternalTick() {} -}; - + /** + * @brief This class can be used to receive event callbacks from the physics engine. + * In order to receive callbacks, you need to create a new class that inherits from + * this one and you must override the methods you need. Then, you need to register your + * new event listener class to the physics world using the DynamicsWorld::setEventListener() + * method. + */ + class EventListener { + public : + /** + * @brief Generic Constructor + */ + EventListener() {} + /** + * @brief Generic Desstructor take it virtual + */ + virtual ~EventListener() =default; + /** + * @brief Called when a new contact point is found between two bodies that were separated before + * @param contact Information about the contact + */ + virtual void beginContact(const ContactPointInfo& contact) {}; + /** + * @brief Called when a new contact point is found between two bodies + * @param contact Information about the contact + */ + virtual void newContact(const ContactPointInfo& contact) {} + /** + * @brief Called at the beginning of an int32_ternal tick of the simulation step. + * Each time the DynamicsWorld::update() method is called, the physics + * engine will do several int32_ternal simulation steps. This method is + * called at the beginning of each int32_ternal simulation step. + */ + virtual void beginInternalTick() {} + /** + * @brief Called at the end of an int32_ternal tick of the simulation step. + * Each time the DynamicsWorld::update() metho is called, the physics + * engine will do several int32_ternal simulation steps. This method is + * called at the end of each int32_ternal simulation step. + */ + virtual void endInternalTick() {} + }; } diff --git a/ephysics/engine/Impulse.hpp b/ephysics/engine/Impulse.hpp index 6372a82..21fe24a 100644 --- a/ephysics/engine/Impulse.hpp +++ b/ephysics/engine/Impulse.hpp @@ -5,60 +5,39 @@ */ #pragma once -// Libraries #include namespace ephysics { - -// Structure Impulse -/** - * Represents an impulse that we can apply to bodies in the contact or constraint solver. - */ -struct Impulse { - - private: - - // -------------------- Methods -------------------- // - - /// Private assignment operator - Impulse& operator=(const Impulse& impulse); - - public: - - // -------------------- Attributes -------------------- // - - /// Linear impulse applied to the first body - const vec3 linearImpulseBody1; - - /// Angular impulse applied to the first body - const vec3 angularImpulseBody1; - - /// Linear impulse applied to the second body - const vec3 linearImpulseBody2; - - /// Angular impulse applied to the second body - const vec3 angularImpulseBody2; - - // -------------------- Methods -------------------- // - - /// Constructor - Impulse(const vec3& initLinearImpulseBody1, const vec3& initAngularImpulseBody1, - const vec3& initLinearImpulseBody2, const vec3& initAngularImpulseBody2) - : linearImpulseBody1(initLinearImpulseBody1), - angularImpulseBody1(initAngularImpulseBody1), - linearImpulseBody2(initLinearImpulseBody2), - angularImpulseBody2(initAngularImpulseBody2) { - - } - - /// Copy-constructor - Impulse(const Impulse& impulse) - : linearImpulseBody1(impulse.linearImpulseBody1), - angularImpulseBody1(impulse.angularImpulseBody1), - linearImpulseBody2(impulse.linearImpulseBody2), - angularImpulseBody2(impulse.angularImpulseBody2) { -; - } -}; - + /** + * @brief Represents an impulse that we can apply to bodies in the contact or constraint solver. + */ + class Impulse { + private: + /// Private assignment operator + Impulse& operator=(const Impulse& _impulse); + public: + const vec3 linearImpulseBody1; //!< Linear impulse applied to the first body + const vec3 angularImpulseBody1; //!< Angular impulse applied to the first body + const vec3 linearImpulseBody2; //!< Linear impulse applied to the second body + const vec3 angularImpulseBody2; //!< Angular impulse applied to the second body + /// Constructor + Impulse(const vec3& _initLinearImpulseBody1, + const vec3& _initAngularImpulseBody1, + const vec3& _initLinearImpulseBody2, + const vec3& _initAngularImpulseBody2): + linearImpulseBody1(_initLinearImpulseBody1), + angularImpulseBody1(_initAngularImpulseBody1), + linearImpulseBody2(_initLinearImpulseBody2), + angularImpulseBody2(_initAngularImpulseBody2) { + + } + /// Copy-constructor + Impulse(const Impulse& _impulse): + linearImpulseBody1(_impulse.linearImpulseBody1), + angularImpulseBody1(_impulse.angularImpulseBody1), + linearImpulseBody2(_impulse.linearImpulseBody2), + angularImpulseBody2(_impulse.angularImpulseBody2) { + + } + }; } diff --git a/ephysics/engine/Island.cpp b/ephysics/engine/Island.cpp index b9d576b..52d5684 100644 --- a/ephysics/engine/Island.cpp +++ b/ephysics/engine/Island.cpp @@ -4,7 +4,6 @@ * @license BSD 3 clauses (see license file) */ -// Libraries #include using namespace ephysics; diff --git a/ephysics/engine/Island.hpp b/ephysics/engine/Island.hpp index 88c6a21..dbcf5e5 100644 --- a/ephysics/engine/Island.hpp +++ b/ephysics/engine/Island.hpp @@ -5,105 +5,58 @@ */ #pragma once -// Libraries #include #include #include #include namespace ephysics { - -// Class Island -/** - * An island represent an isolated group of awake bodies that are connected with each other by - * some contraints (contacts or joints). - */ -class Island { - - private: - - // -------------------- Attributes -------------------- // - - /// Array with all the bodies of the island - RigidBody** m_bodies; - - /// Array with all the contact manifolds between bodies of the island - ContactManifold** m_contactManifolds; - - /// Array with all the joints between bodies of the island - Joint** m_joints; - - /// Current number of bodies in the island - uint32_t m_numberBodies; - - /// Current number of contact manifold in the island - uint32_t m_numberContactManifolds; - - /// Current number of joints in the island - uint32_t m_numberJoints; - - /// Reference to the memory allocator - MemoryAllocator& m_memoryAllocator; - - /// Number of bytes allocated for the bodies array - size_t m_numberAllocatedBytesBodies; - - /// Number of bytes allocated for the contact manifolds array - size_t m_numberAllocatedBytesContactManifolds; - - /// Number of bytes allocated for the joints array - size_t m_numberAllocatedBytesJoints; - - // -------------------- Methods -------------------- // - - /// Private assignment operator - Island& operator=(const Island& island); - - /// Private copy-constructor - Island(const Island& island); - - public: - - // -------------------- Methods -------------------- // - - /// Constructor - Island(uint32_t nbMaxBodies, uint32_t nbMaxContactManifolds, uint32_t nbMaxJoints, - MemoryAllocator& memoryAllocator); - - /// Destructor - ~Island(); - - /// Add a body int32_to the island - void addBody(RigidBody* body); - - /// Add a contact manifold int32_to the island - void addContactManifold(ContactManifold* contactManifold); - - /// Add a joint int32_to the island - void addJoint(Joint* joint); - - /// Return the number of bodies in the island - uint32_t getNbBodies() const; - - /// Return the number of contact manifolds in the island - uint32_t getNbContactManifolds() const; - - /// Return the number of joints in the island - uint32_t getNbJoints() const; - - /// Return a pointer to the array of bodies - RigidBody** getBodies(); - - /// Return a pointer to the array of contact manifolds - ContactManifold** getContactManifold(); - - /// Return a pointer to the array of joints - Joint** getJoints(); - - // -------------------- Friendship -------------------- // - - friend class DynamicsWorld; -}; + /** + * @brief An island represent an isolated group of awake bodies that are connected with each other by + * some contraints (contacts or joints). + */ + class Island { + private: + RigidBody** m_bodies; //!< Array with all the bodies of the island + ContactManifold** m_contactManifolds; //!< Array with all the contact manifolds between bodies of the island + Joint** m_joints; //!< Array with all the joints between bodies of the island + uint32_t m_numberBodies; //!< Current number of bodies in the island + uint32_t m_numberContactManifolds; //!< Current number of contact manifold in the island + uint32_t m_numberJoints; //!< Current number of joints in the island + MemoryAllocator& m_memoryAllocator; //!< Reference to the memory allocator + size_t m_numberAllocatedBytesBodies; //!< Number of bytes allocated for the bodies array + size_t m_numberAllocatedBytesContactManifolds; //!< Number of bytes allocated for the contact manifolds array + size_t m_numberAllocatedBytesJoints; //!< Number of bytes allocated for the joints array + /// Private assignment operator + Island& operator=(const Island& island); + /// Private copy-constructor + Island(const Island& island); + public: + /// Constructor + Island(uint32_t nbMaxBodies, uint32_t nbMaxContactManifolds, uint32_t nbMaxJoints, + MemoryAllocator& memoryAllocator); + /// Destructor + ~Island(); + /// Add a body int32_to the island + void addBody(RigidBody* body); + /// Add a contact manifold int32_to the island + void addContactManifold(ContactManifold* contactManifold); + /// Add a joint int32_to the island + void addJoint(Joint* joint); + /// Return the number of bodies in the island + uint32_t getNbBodies() const; + /// Return the number of contact manifolds in the island + uint32_t getNbContactManifolds() const; + /// Return the number of joints in the island + uint32_t getNbJoints() const; + /// Return a pointer to the array of bodies + RigidBody** getBodies(); + /// Return a pointer to the array of contact manifolds + ContactManifold** getContactManifold(); + /// Return a pointer to the array of joints + Joint** getJoints(); + friend class DynamicsWorld; + }; // Add a body int32_to the island inline void Island::addBody(RigidBody* body) { diff --git a/ephysics/engine/Material.cpp b/ephysics/engine/Material.cpp index b1142e4..b6c264b 100644 --- a/ephysics/engine/Material.cpp +++ b/ephysics/engine/Material.cpp @@ -23,8 +23,3 @@ Material::Material(const Material& material) m_rollingResistance(material.m_rollingResistance), m_bounciness(material.m_bounciness) { } - -// Destructor -Material::~Material() { - -} diff --git a/ephysics/engine/Material.hpp b/ephysics/engine/Material.hpp index 0d86f90..6aef261 100644 --- a/ephysics/engine/Material.hpp +++ b/ephysics/engine/Material.hpp @@ -4,68 +4,40 @@ * @license BSD 3 clauses (see license file) */ #pragma once - -// Libraries #include #include namespace ephysics { - -// Class Material -/** - * This class contains the material properties of a rigid body that will be use for - * the dynamics simulation like the friction coefficient or the bounciness of the rigid - * body. - */ -class Material { - - private : - - // -------------------- Attributes -------------------- // - - /// Friction coefficient (positive value) - float m_frictionCoefficient; - - /// Rolling resistance factor (positive value) - float m_rollingResistance; - - /// Bounciness during collisions (between 0 and 1) where 1 is for a very bouncy body - float m_bounciness; - - public : - - // -------------------- Methods -------------------- // - - /// Constructor - Material(); - - /// Copy-constructor - Material(const Material& material); - - /// Destructor - ~Material(); - - /// Return the bounciness - float getBounciness() const; - - /// Set the bounciness. - void setBounciness(float bounciness); - - /// Return the friction coefficient - float getFrictionCoefficient() const; - - /// Set the friction coefficient. - void setFrictionCoefficient(float frictionCoefficient); - - /// Return the rolling resistance factor - float getRollingResistance() const; - - /// Set the rolling resistance factor - void setRollingResistance(float rollingResistance); - - /// Overloaded assignment operator - Material& operator=(const Material& material); -}; + /** + * This class contains the material properties of a rigid body that will be use for + * the dynamics simulation like the friction coefficient or the bounciness of the rigid + * body. + */ + class Material { + private : + float m_frictionCoefficient; //!< Friction coefficient (positive value) + float m_rollingResistance; //!< Rolling resistance factor (positive value) + float m_bounciness; //!< Bounciness during collisions (between 0 and 1) where 1 is for a very bouncy body + public : + /// Constructor + Material(); + /// Copy-constructor + Material(const Material& material); + /// Return the bounciness + float getBounciness() const; + /// Set the bounciness. + void setBounciness(float bounciness); + /// Return the friction coefficient + float getFrictionCoefficient() const; + /// Set the friction coefficient. + void setFrictionCoefficient(float frictionCoefficient); + /// Return the rolling resistance factor + float getRollingResistance() const; + /// Set the rolling resistance factor + void setRollingResistance(float rollingResistance); + /// Overloaded assignment operator + Material& operator=(const Material& material); + }; // Return the bounciness /** diff --git a/ephysics/engine/OverlappingPair.cpp b/ephysics/engine/OverlappingPair.cpp index facfd5b..e6f5e8d 100644 --- a/ephysics/engine/OverlappingPair.cpp +++ b/ephysics/engine/OverlappingPair.cpp @@ -16,8 +16,3 @@ OverlappingPair::OverlappingPair(ProxyShape* shape1, ProxyShape* shape2, int32_t m_cachedSeparatingAxis(1.0, 1.0, 1.0) { } - -// Destructor -OverlappingPair::~OverlappingPair() { - -} diff --git a/ephysics/engine/OverlappingPair.hpp b/ephysics/engine/OverlappingPair.hpp index b1497df..efdad1f 100644 --- a/ephysics/engine/OverlappingPair.hpp +++ b/ephysics/engine/OverlappingPair.hpp @@ -4,104 +4,68 @@ * @license BSD 3 clauses (see license file) */ #pragma once - -// Libraries #include #include #include -/// ReactPhysics3D namespace namespace ephysics { - -// Type for the overlapping pair ID -typedef std::pair overlappingpairid; - -// Class OverlappingPair -/** - * This class represents a pair of two proxy collision shapes that are overlapping - * during the broad-phase collision detection. It is created when - * the two proxy collision shapes start to overlap and is destroyed when they do not - * overlap anymore. This class contains a contact manifold that - * store all the contact points between the two bodies. - */ -class OverlappingPair { - - private: - - // -------------------- Attributes -------------------- // - - /// Set of persistent contact manifolds - ContactManifoldSet m_contactManifoldSet; - - /// Cached previous separating axis - vec3 m_cachedSeparatingAxis; - - // -------------------- Methods -------------------- // - - /// Private copy-constructor - OverlappingPair(const OverlappingPair& pair); - - /// Private assignment operator - OverlappingPair& operator=(const OverlappingPair& pair); - - public: - - // -------------------- Methods -------------------- // - - /// Constructor - OverlappingPair(ProxyShape* shape1, ProxyShape* shape2, - int32_t nbMaxContactManifolds, MemoryAllocator& memoryAllocator); - - /// Destructor - ~OverlappingPair(); - - /// Return the pointer to first proxy collision shape - ProxyShape* getShape1() const; - - /// Return the pointer to second body - ProxyShape* getShape2() const; - - /// Add a contact to the contact cache - void addContact(ContactPoint* contact); - - /// Update the contact cache - void update(); - - /// Return the cached separating axis - vec3 getCachedSeparatingAxis() const; - - /// Set the cached separating axis - void setCachedSeparatingAxis(const vec3& axis); - - /// Return the number of contacts in the cache - uint32_t getNbContactPoints() const; - - /// Return the a reference to the contact manifold set - const ContactManifoldSet& getContactManifoldSet(); - - /// Clear the contact points of the contact manifold - void clearContactPoints(); - - /// Return the pair of bodies index - static overlappingpairid computeID(ProxyShape* shape1, ProxyShape* shape2); - - /// Return the pair of bodies index of the pair - static bodyindexpair computeBodiesIndexPair(CollisionBody* body1, CollisionBody* body2); - - // -------------------- Friendship -------------------- // - - friend class DynamicsWorld; -}; + // Type for the overlapping pair ID + typedef std::pair overlappingpairid; + /** + * @brief This class represents a pair of two proxy collision shapes that are overlapping + * during the broad-phase collision detection. It is created when + * the two proxy collision shapes start to overlap and is destroyed when they do not + * overlap anymore. This class contains a contact manifold that + * store all the contact points between the two bodies. + */ + class OverlappingPair { + private: + ContactManifoldSet m_contactManifoldSet; //!< Set of persistent contact manifolds + vec3 m_cachedSeparatingAxis; //!< Cached previous separating axis + /// Private copy-constructor + OverlappingPair(const OverlappingPair& pair); + /// Private assignment operator + OverlappingPair& operator=(const OverlappingPair& pair); + public: + /// Constructor + OverlappingPair(ProxyShape* shape1, + ProxyShape* shape2, + int32_t nbMaxContactManifolds, + MemoryAllocator& memoryAllocator); + /// Return the pointer to first proxy collision shape + ProxyShape* getShape1() const; + /// Return the pointer to second body + ProxyShape* getShape2() const; + /// Add a contact to the contact cache + void addContact(ContactPoint* contact); + /// Update the contact cache + void update(); + /// Return the cached separating axis + vec3 getCachedSeparatingAxis() const; + /// Set the cached separating axis + void setCachedSeparatingAxis(const vec3& axis); + /// Return the number of contacts in the cache + uint32_t getNbContactPoints() const; + /// Return the a reference to the contact manifold set + const ContactManifoldSet& getContactManifoldSet(); + /// Clear the contact points of the contact manifold + void clearContactPoints(); + /// Return the pair of bodies index + static overlappingpairid computeID(ProxyShape* shape1, ProxyShape* shape2); + /// Return the pair of bodies index of the pair + static bodyindexpair computeBodiesIndexPair(CollisionBody* body1, CollisionBody* body2); + friend class DynamicsWorld; + }; // Return the pointer to first body inline ProxyShape* OverlappingPair::getShape1() const { return m_contactManifoldSet.getShape1(); -} +} // Return the pointer to second body inline ProxyShape* OverlappingPair::getShape2() const { return m_contactManifoldSet.getShape2(); -} +} // Add a contact to the contact manifold inline void OverlappingPair::addContact(ContactPoint* contact) { diff --git a/ephysics/engine/Profiler.hpp b/ephysics/engine/Profiler.hpp index 42de6b1..d0eb331 100644 --- a/ephysics/engine/Profiler.hpp +++ b/ephysics/engine/Profiler.hpp @@ -6,254 +6,154 @@ #pragma once #ifdef IS_PROFILING_ACTIVE - -// Libraries #include #include - -/// ReactPhysics3D namespace namespace ephysics { - -// Class ProfileNode -/** - * It represents a profile sample in the profiler tree. - */ -class ProfileNode { - - private : - - // -------------------- Attributes -------------------- // - - /// Name of the node - const char* m_name; - - /// Total number of calls of this node - uint32_t m_numberTotalCalls; - - /// Starting time of the sampling of corresponding block of code - long double m_startTime; - - /// Total time spent in the block of code - long double m_totalTime; - - /// Recursion counter - int32_t m_recursionCounter; - - /// Pointer to the parent node - ProfileNode* m_parentNode; - - /// Pointer to a child node - ProfileNode* m_childNode; - - /// Pointer to a sibling node - ProfileNode* m_siblingNode; - - public : - - // -------------------- Methods -------------------- // - - /// Constructor - ProfileNode(const char* name, ProfileNode* parentNode); - - /// Destructor - ~ProfileNode(); - - /// Return a pointer to a sub node - ProfileNode* findSubNode(const char* name); - - /// Return a pointer to the parent node - ProfileNode* getParentNode(); - - /// Return a pointer to a sibling node - ProfileNode* getSiblingNode(); - - /// Return a pointer to a child node - ProfileNode* getChildNode(); - - /// Return the name of the node - const char* getName(); - - /// Return the total number of call of the corresponding block of code - uint32_t getNbTotalCalls() const; - - /// Return the total time spent in the block of code - long double getTotalTime() const; - - /// Called when we enter the block of code corresponding to this profile node - void enterBlockOfCode(); - - /// Called when we exit the block of code corresponding to this profile node - bool exitBlockOfCode(); - - /// Reset the profiling of the node - void reset(); - - /// Destroy the node - void destroy(); -}; - -// Class ProfileNodeIterator -/** - * This class allows us to iterator over the profiler tree. - */ -class ProfileNodeIterator { - - private : - - // -------------------- Attributes -------------------- // - - /// Current parent node - ProfileNode* m_currentParentNode; - - /// Current child node - ProfileNode* m_currentChildNode; - - public : - - // -------------------- Methods -------------------- // - - /// Constructor - ProfileNodeIterator(ProfileNode* startingNode); - - /// Go to the first node - void first(); - - /// Go to the next node - void next(); - - /// Enter a given child node - void enterChild(int32_t index); - - /// Enter a given parent node - void enterParent(); - - /// Return true if we are at the root of the profiler tree - bool isRoot(); - - /// Return true if we are at the end of a branch of the profiler tree - bool isEnd(); - - /// Return the name of the current node - const char* getCurrentName(); - - /// Return the total time of the current node - long double getCurrentTotalTime(); - - /// Return the total number of calls of the current node - uint32_t getCurrentNbTotalCalls(); - - /// Return the name of the current parent node - const char* getCurrentParentName(); - - /// Return the total time of the current parent node - long double getCurrentParentTotalTime(); - - /// Return the total number of calls of the current parent node - uint32_t getCurrentParentNbTotalCalls(); -}; - -// Class Profiler -/** - * This is the main class of the profiler. This profiler is based on "Real-Time Hierarchical - * Profiling" article from "Game Programming Gems 3" by Greg Hjelstrom and Byon Garrabrant. - */ -class Profiler { - - private : - - // -------------------- Attributes -------------------- // - - /// Root node of the profiler tree - static ProfileNode m_rootNode; - - /// Current node in the current execution - static ProfileNode* m_currentNode; - - /// Frame counter - static uint32_t m_frameCounter; - - /// Starting profiling time - static long double m_profilingStartTime; - - /// Recursively print32_t the report of a given node of the profiler tree - static void print32_tRecursiveNodeReport(ProfileNodeIterator* iterator, - int32_t spacing, - std::ostream& outputStream); - - public : - - // -------------------- Methods -------------------- // - - /// Method called when we want to start profiling a block of code. - static void startProfilingBlock(const char *name); - - /// Method called at the end of the scope where the - /// startProfilingBlock() method has been called. - static void stopProfilingBlock(); - - /// Reset the timing data of the profiler (but not the profiler tree structure) - static void reset(); - - /// Return the number of frames - static uint32_t getNbFrames(); - - /// Return the elasped time since the start/reset of the profiling - static long double getElapsedTimeSinceStart(); - - /// Increment the frame counter - static void incrementFrameCounter(); - - /// Return an iterator over the profiler tree starting at the root - static ProfileNodeIterator* getIterator(); - - /// Print32_t the report of the profiler in a given output stream - static void print32_tReport(std::ostream& outputStream); - - /// Destroy a previously allocated iterator - static void destroyIterator(ProfileNodeIterator* iterator); - - /// Destroy the profiler (release the memory) - static void destroy(); -}; - -// Class ProfileSample -/** - * This class is used to represent a profile sample. It is constructed at the - * beginning of a code block we want to profile and destructed at the end of the - * scope to profile. - */ -class ProfileSample { - - public : - - // -------------------- Methods -------------------- // - - /// Constructor - ProfileSample(const char* name) { - - // Ask the profiler to start profiling a block of code - Profiler::startProfilingBlock(name); - } - - /// Destructor - ~ProfileSample() { - - // Tell the profiler to stop profiling a block of code - Profiler::stopProfilingBlock(); - } -}; + /** + * @brief It represents a profile sample in the profiler tree. + */ + class ProfileNode { + private : + const char* m_name; //!< Name of the node + uint32_t m_numberTotalCalls; //!< Total number of calls of this node + long double m_startTime; //!< Starting time of the sampling of corresponding block of code + long double m_totalTime; //!< Total time spent in the block of code + int32_t m_recursionCounter; //!< Recursion counter + ProfileNode* m_parentNode; //!< Pointer to the parent node + ProfileNode* m_childNode; //!< Pointer to a child node + ProfileNode* m_siblingNode; //!< Pointer to a sibling node + public : + /// Constructor + ProfileNode(const char* name, ProfileNode* parentNode); + /// Destructor + ~ProfileNode(); + /// Return a pointer to a sub node + ProfileNode* findSubNode(const char* name); + /// Return a pointer to the parent node + ProfileNode* getParentNode(); + /// Return a pointer to a sibling node + ProfileNode* getSiblingNode(); + /// Return a pointer to a child node + ProfileNode* getChildNode(); + /// Return the name of the node + const char* getName(); + /// Return the total number of call of the corresponding block of code + uint32_t getNbTotalCalls() const; + /// Return the total time spent in the block of code + long double getTotalTime() const; + /// Called when we enter the block of code corresponding to this profile node + void enterBlockOfCode(); + /// Called when we exit the block of code corresponding to this profile node + bool exitBlockOfCode(); + /// Reset the profiling of the node + void reset(); + /// Destroy the node + void destroy(); + }; + /** + * @brief This class allows us to iterator over the profiler tree. + */ + class ProfileNodeIterator { + private : + ProfileNode* m_currentParentNode; //!< Current parent node + ProfileNode* m_currentChildNode; //!< Current child node + public : + /// Constructor + ProfileNodeIterator(ProfileNode* startingNode); + /// Go to the first node + void first(); + /// Go to the next node + void next(); + /// Enter a given child node + void enterChild(int32_t index); + /// Enter a given parent node + void enterParent(); + /// Return true if we are at the root of the profiler tree + bool isRoot(); + /// Return true if we are at the end of a branch of the profiler tree + bool isEnd(); + /// Return the name of the current node + const char* getCurrentName(); + /// Return the total time of the current node + long double getCurrentTotalTime(); + /// Return the total number of calls of the current node + uint32_t getCurrentNbTotalCalls(); + /// Return the name of the current parent node + const char* getCurrentParentName(); + /// Return the total time of the current parent node + long double getCurrentParentTotalTime(); + /// Return the total number of calls of the current parent node + uint32_t getCurrentParentNbTotalCalls(); + }; + + /** + * @brief This is the main class of the profiler. This profiler is based on "Real-Time Hierarchical + * Profiling" article from "Game Programming Gems 3" by Greg Hjelstrom and Byon Garrabrant. + */ + class Profiler { + private : + static ProfileNode m_rootNode; //!< Root node of the profiler tree + static ProfileNode* m_currentNode; //!< Current node in the current execution + static uint32_t m_frameCounter; //!< Frame counter + static long double m_profilingStartTime; //!< Starting profiling time + private: + static void print32_tRecursiveNodeReport(ProfileNodeIterator* iterator, + int32_t spacing, + std::ostream& outputStream); + public : + /// Method called when we want to start profiling a block of code. + static void startProfilingBlock(const char *name); + /// Method called at the end of the scope where the + /// startProfilingBlock() method has been called. + static void stopProfilingBlock(); + /// Reset the timing data of the profiler (but not the profiler tree structure) + static void reset(); + /// Return the number of frames + static uint32_t getNbFrames(); + /// Return the elasped time since the start/reset of the profiling + static long double getElapsedTimeSinceStart(); + /// Increment the frame counter + static void incrementFrameCounter(); + /// Return an iterator over the profiler tree starting at the root + static ProfileNodeIterator* getIterator(); + /// Print32_t the report of the profiler in a given output stream + static void print32_tReport(std::ostream& outputStream); + /// Destroy a previously allocated iterator + static void destroyIterator(ProfileNodeIterator* iterator); + /// Destroy the profiler (release the memory) + static void destroy(); + }; + + /** + * @brief This class is used to represent a profile sample. It is constructed at the + * beginning of a code block we want to profile and destructed at the end of the + * scope to profile. + */ + class ProfileSample { + public : + /// Constructor + ProfileSample(const char* name) { + // Ask the profiler to start profiling a block of code + Profiler::startProfilingBlock(name); + } + /// Destructor + ~ProfileSample() { + // Tell the profiler to stop profiling a block of code + Profiler::stopProfilingBlock(); + } + }; // Use this macro to start profile a block of code #define PROFILE(name) ProfileSample profileSample(name) // Return true if we are at the root of the profiler tree inline bool ProfileNodeIterator::isRoot() { - return (m_currentParentNode->getParentNode() == NULL); + return (m_currentParentNode->getParentNode() == nullptr); } // Return true if we are at the end of a branch of the profiler tree inline bool ProfileNodeIterator::isEnd() { - return (m_currentChildNode == NULL); + return (m_currentChildNode == nullptr); } // Return the name of the current node @@ -359,7 +259,7 @@ inline void Profiler::destroy() { } -#else // In profile is not active +#else // Empty macro in case profiling is not active #define PROFILE(name) diff --git a/ephysics/engine/Timer.hpp b/ephysics/engine/Timer.hpp index 970c9bf..47d80af 100644 --- a/ephysics/engine/Timer.hpp +++ b/ephysics/engine/Timer.hpp @@ -22,85 +22,50 @@ /// Namespace ReactPhysics3D namespace ephysics { - -// Class Timer -/** - * This class will take care of the time in the physics engine. It - * uses functions that depend on the current platform to get the - * current time. - */ -class Timer { - - private : - - // -------------------- Attributes -------------------- // - - /// Timestep dt of the physics engine (timestep > 0.0) - double m_timeStep; - - /// Last time the timer has been updated - long double m_lastUpdateTime; - - /// Time difference between the two last timer update() calls - long double m_deltaTime; - - /// Used to fix the time step and avoid strange time effects - double m_accumulator; - - /// True if the timer is running - bool m_isRunning; - - // -------------------- Methods -------------------- // - - /// Private copy-constructor - Timer(const Timer& timer); - - /// Private assignment operator - Timer& operator=(const Timer& timer); - - public : - - // -------------------- Methods -------------------- // - - /// Constructor - Timer(double timeStep); - - /// Destructor - virtual ~Timer(); - - /// Return the timestep of the physics engine - double getTimeStep() const; - - /// Set the timestep of the physics engine - void setTimeStep(double timeStep); - - /// Return the current time of the physics engine - long double getPhysicsTime() const; - - /// Start the timer - void start(); - - /// Stop the timer - void stop(); - - /// Return true if the timer is running - bool getIsRunning() const; - - /// True if it's possible to take a new step - bool isPossibleToTakeStep() const; - - /// Compute the time since the last update() call and add it to the accumulator - void update(); - - /// Take a new step => update the timer by adding the timeStep value to the current time - void nextStep(); - - /// Compute the int32_terpolation factor - float computeInterpolationFactor(); - - /// Return the current time of the system in seconds - static long double getCurrentSystemTime(); -}; + /** + * @brief This class will take care of the time in the physics engine. It + * uses functions that depend on the current platform to get the + * current time. + */ + class Timer { + private : + double m_timeStep; //!< Timestep dt of the physics engine (timestep > 0.0) + long double m_lastUpdateTime; //!< Last time the timer has been updated + long double m_deltaTime; //!< Time difference between the two last timer update() calls + double m_accumulator; //!< Used to fix the time step and avoid strange time effects + bool m_isRunning; //!< True if the timer is running + /// Private copy-constructor + Timer(const Timer& timer); + /// Private assignment operator + Timer& operator=(const Timer& timer); + public : + /// Constructor + Timer(double timeStep); + /// Destructor + virtual ~Timer(); + /// Return the timestep of the physics engine + double getTimeStep() const; + /// Set the timestep of the physics engine + void setTimeStep(double timeStep); + /// Return the current time of the physics engine + long double getPhysicsTime() const; + /// Start the timer + void start(); + /// Stop the timer + void stop(); + /// Return true if the timer is running + bool getIsRunning() const; + /// True if it's possible to take a new step + bool isPossibleToTakeStep() const; + /// Compute the time since the last update() call and add it to the accumulator + void update(); + /// Take a new step => update the timer by adding the timeStep value to the current time + void nextStep(); + /// Compute the int32_terpolation factor + float computeInterpolationFactor(); + /// Return the current time of the system in seconds + static long double getCurrentSystemTime(); + }; // Return the timestep of the physics engine inline double Timer::getTimeStep() const {