diff --git a/ephysics/body/Body.hpp b/ephysics/body/Body.hpp index c0b9478..f7b1daf 100644 --- a/ephysics/body/Body.hpp +++ b/ephysics/body/Body.hpp @@ -10,154 +10,147 @@ #include namespace ephysics { -/** - * @brief Represent a body of the physics engine. You should not - * instantiante this class but instantiate the CollisionBody or RigidBody - * classes instead. - */ -class Body { - protected : - bodyindex m_id; //!< ID of the body - bool m_isAlreadyInIsland; //!< True if the body has already been added in an island (for sleeping technique) - bool m_isAllowedToSleep; //!< True if the body is allowed to go to sleep for better efficiency - /** - * @brief True if the body is active. - * An inactive body does not participate in collision detection, is not simulated and will not be hit in a ray casting query. - * A body is active by default. If you set this value to "false", all the proxy shapes of this body will be removed from the broad-phase. - * If you set this value to "true", all the proxy shapes will be added to the broad-phase. - * A joint connected to an inactive body will also be inactive. - */ - bool m_isActive; - bool m_isSleeping; //!< True if the body is sleeping (for sleeping technique) - float m_sleepTime; //!< Elapsed time since the body velocity was bellow the sleep velocity - void* m_userData; //!< Pointer that can be used to attach user data to the body - /// Private copy-constructor - Body(const Body& body); - /// Private assignment operator - Body& operator=(const Body& body); - public : - /** - * @brief Constructor - * @param[in] _id ID of the new body - */ - Body(bodyindex _id); - /** - * @brtief Virtualize Destructor - */ - virtual ~Body() = default; - /** - * @brief Return the id of the body - * @return The ID of the body - */ - bodyindex getID() const { - return m_id; - } - /** - * @brief Return whether or not the body is allowed to sleep - * @return True if the body is allowed to sleep and false otherwise - */ - bool isAllowedToSleep() const { - return m_isAllowedToSleep; - } - /** - * @brief Set whether or not the body is allowed to go to sleep - * @param[in] _isAllowedToSleep True if the body is allowed to sleep - */ - void setIsAllowedToSleep(bool _isAllowedToSleep) { - m_isAllowedToSleep = _isAllowedToSleep; - if (!m_isAllowedToSleep) { - setIsSleeping(false); + /** + * @brief Represent a body of the physics engine. You should not + * instantiante this class but instantiate the CollisionBody or RigidBody + * classes instead. + */ + class Body { + protected : + bodyindex m_id; //!< ID of the body + bool m_isAlreadyInIsland; //!< True if the body has already been added in an island (for sleeping technique) + bool m_isAllowedToSleep; //!< True if the body is allowed to go to sleep for better efficiency + /** + * @brief True if the body is active. + * An inactive body does not participate in collision detection, is not simulated and will not be hit in a ray casting query. + * A body is active by default. If you set this value to "false", all the proxy shapes of this body will be removed from the broad-phase. + * If you set this value to "true", all the proxy shapes will be added to the broad-phase. + * A joint connected to an inactive body will also be inactive. + */ + bool m_isActive; + bool m_isSleeping; //!< True if the body is sleeping (for sleeping technique) + float m_sleepTime; //!< Elapsed time since the body velocity was bellow the sleep velocity + void* m_userData; //!< Pointer that can be used to attach user data to the body + /// Private copy-constructor + Body(const Body& body); + /// Private assignment operator + Body& operator=(const Body& body); + public : + /** + * @brief Constructor + * @param[in] _id ID of the new body + */ + Body(bodyindex _id); + /** + * @brtief Virtualize Destructor + */ + virtual ~Body() = default; + /** + * @brief Return the id of the body + * @return The ID of the body + */ + bodyindex getID() const { + return m_id; } - } - /** - * @brief Return whether or not the body is sleeping - * @return True if the body is currently sleeping and false otherwise - */ - bool isSleeping() const { - return m_isSleeping; - } - /** - * @brief Return true if the body is active - * @return True if the body currently active and false otherwise - */ - bool isActive() const { - return m_isActive; - } - /** - * @brief Set whether or not the body is active - * @param[in] _isActive True if you want to activate the body - */ - void setIsActive(bool _isActive) { - m_isActive = _isActive; - } - /** - * @brief Set the variable to know whether or not the body is sleeping - * @param[in] _isSleeping Set the new status - */ - void setIsSleeping(bool _isSleeping) { - if (_isSleeping) { - m_sleepTime = 0.0f; - } else { - if (m_isSleeping) { - m_sleepTime = 0.0f; + /** + * @brief Return whether or not the body is allowed to sleep + * @return True if the body is allowed to sleep and false otherwise + */ + bool isAllowedToSleep() const { + return m_isAllowedToSleep; + } + /** + * @brief Set whether or not the body is allowed to go to sleep + * @param[in] _isAllowedToSleep True if the body is allowed to sleep + */ + void setIsAllowedToSleep(bool _isAllowedToSleep) { + m_isAllowedToSleep = _isAllowedToSleep; + if (!m_isAllowedToSleep) { + setIsSleeping(false); } } - m_isSleeping = _isSleeping; - } - /** - * @brief Return a pointer to the user data attached to this body - * @return A pointer to the user data you have attached to the body - */ - void* getUserData() const { - return m_userData; - } - /** - * @brief Attach user data to this body - * @param[in] _userData A pointer to the user data you want to attach to the body - */ - void setUserData(void* _userData) { - m_userData = _userData; - } - /** - * @brief Smaller than operator - * @param[in] _obj Other object to compare - * @return true if the current element is smaller - */ - bool operator<(const Body& _obj) const { - return (m_id < _obj.m_id); - } - /** - * @brief Larger than operator - * @param[in] _obj Other object to compare - * @return true if the current element is Bigger - */ - bool operator>(const Body& _obj) const { - return (m_id > _obj.m_id); - } - /** - * @brief Equal operator - * @param[in] _obj Other object to compare - * @return true if the curretn element is equal - */ - bool operator==(const Body& _obj) const { - return (m_id == _obj.m_id); - } - /** - * @brief Not equal operator - * @param[in] _obj Other object to compare - * @return true if the curretn element is NOT equal - */ - bool operator!=(const Body& _obj) const { - return (m_id != _obj.m_id); - } - friend class DynamicsWorld; -}; - - - - - - - + /** + * @brief Return whether or not the body is sleeping + * @return True if the body is currently sleeping and false otherwise + */ + bool isSleeping() const { + return m_isSleeping; + } + /** + * @brief Return true if the body is active + * @return True if the body currently active and false otherwise + */ + bool isActive() const { + return m_isActive; + } + /** + * @brief Set whether or not the body is active + * @param[in] _isActive True if you want to activate the body + */ + void setIsActive(bool _isActive) { + m_isActive = _isActive; + } + /** + * @brief Set the variable to know whether or not the body is sleeping + * @param[in] _isSleeping Set the new status + */ + void setIsSleeping(bool _isSleeping) { + if (_isSleeping) { + m_sleepTime = 0.0f; + } else { + if (m_isSleeping) { + m_sleepTime = 0.0f; + } + } + m_isSleeping = _isSleeping; + } + /** + * @brief Return a pointer to the user data attached to this body + * @return A pointer to the user data you have attached to the body + */ + void* getUserData() const { + return m_userData; + } + /** + * @brief Attach user data to this body + * @param[in] _userData A pointer to the user data you want to attach to the body + */ + void setUserData(void* _userData) { + m_userData = _userData; + } + /** + * @brief Smaller than operator + * @param[in] _obj Other object to compare + * @return true if the current element is smaller + */ + bool operator<(const Body& _obj) const { + return (m_id < _obj.m_id); + } + /** + * @brief Larger than operator + * @param[in] _obj Other object to compare + * @return true if the current element is Bigger + */ + bool operator>(const Body& _obj) const { + return (m_id > _obj.m_id); + } + /** + * @brief Equal operator + * @param[in] _obj Other object to compare + * @return true if the curretn element is equal + */ + bool operator==(const Body& _obj) const { + return (m_id == _obj.m_id); + } + /** + * @brief Not equal operator + * @param[in] _obj Other object to compare + * @return true if the curretn element is NOT equal + */ + bool operator!=(const Body& _obj) const { + return (m_id != _obj.m_id); + } + friend class DynamicsWorld; + }; } diff --git a/ephysics/collision/CollisionDetection.cpp b/ephysics/collision/CollisionDetection.cpp index 5c3243c..8cc9d1d 100644 --- a/ephysics/collision/CollisionDetection.cpp +++ b/ephysics/collision/CollisionDetection.cpp @@ -511,3 +511,86 @@ void TestCollisionBetweenShapesCallback::notifyContact(OverlappingPair* overlapp const ContactPointInfo& contactInfo) { m_collisionCallback->notifyContact(contactInfo); } + +// Return the Narrow-phase collision detection algorithm to use between two types of shapes +NarrowPhaseAlgorithm* CollisionDetection::getCollisionAlgorithm(CollisionShapeType shape1Type, CollisionShapeType shape2Type) const { + return m_collisionMatrix[shape1Type][shape2Type]; +} + +// Set the collision dispatch configuration +void CollisionDetection::setCollisionDispatch(CollisionDispatch* collisionDispatch) { + m_collisionDispatch = collisionDispatch; + + m_collisionDispatch->init(this, &m_memoryAllocator); + + // Fill-in the collision matrix with the new algorithms to use + fillInCollisionMatrix(); +} + +// Add a body to the collision detection +void CollisionDetection::addProxyCollisionShape(ProxyShape* proxyShape, + const AABB& aabb) { + + // Add the body to the broad-phase + m_broadPhaseAlgorithm.addProxyCollisionShape(proxyShape, aabb); + + m_isCollisionShapesAdded = true; +} + +// Add a pair of bodies that cannot collide with each other +void CollisionDetection::addNoCollisionPair(CollisionBody* body1, + CollisionBody* body2) { + m_noCollisionPairs.insert(OverlappingPair::computeBodiesIndexPair(body1, body2)); +} + +// Remove a pair of bodies that cannot collide with each other +void CollisionDetection::removeNoCollisionPair(CollisionBody* body1, + CollisionBody* body2) { + m_noCollisionPairs.erase(OverlappingPair::computeBodiesIndexPair(body1, body2)); +} + +// Ask for a collision shape to be tested again during broad-phase. +/// We simply put the shape in the list of collision shape that have moved in the +/// previous frame so that it is tested for collision again in the broad-phase. +void CollisionDetection::askForBroadPhaseCollisionCheck(ProxyShape* shape) { + m_broadPhaseAlgorithm.addMovedCollisionShape(shape->m_broadPhaseID); +} + +// Update a proxy collision shape (that has moved for instance) +void CollisionDetection::updateProxyCollisionShape(ProxyShape* shape, const AABB& aabb, + const vec3& displacement, bool forceReinsert) { + m_broadPhaseAlgorithm.updateProxyCollisionShape(shape, aabb, displacement); +} + +// Ray casting method +void CollisionDetection::raycast(RaycastCallback* raycastCallback, + const Ray& ray, + unsigned short raycastWithCategoryMaskBits) const { + + PROFILE("CollisionDetection::raycast()"); + + RaycastTest rayCastTest(raycastCallback); + + // Ask the broad-phase algorithm to call the testRaycastAgainstShape() + // callback method for each proxy shape hit by the ray in the broad-phase + m_broadPhaseAlgorithm.raycast(ray, rayCastTest, raycastWithCategoryMaskBits); +} + +// Test if the AABBs of two proxy shapes overlap +bool CollisionDetection::testAABBOverlap(const ProxyShape* shape1, + const ProxyShape* shape2) const { + + // If one of the shape's body is not active, we return no overlap + if (!shape1->getBody()->isActive() || !shape2->getBody()->isActive()) { + return false; + } + + return m_broadPhaseAlgorithm.testOverlappingShapes(shape1, shape2); +} + +// Return a pointer to the world +CollisionWorld* CollisionDetection::getWorld() { + return m_world; +} + + diff --git a/ephysics/collision/CollisionDetection.hpp b/ephysics/collision/CollisionDetection.hpp index bd37ec8..a5c0100 100644 --- a/ephysics/collision/CollisionDetection.hpp +++ b/ephysics/collision/CollisionDetection.hpp @@ -140,85 +140,4 @@ namespace ephysics { friend class ConvexMeshShape; }; -// Return the Narrow-phase collision detection algorithm to use between two types of shapes -NarrowPhaseAlgorithm* CollisionDetection::getCollisionAlgorithm(CollisionShapeType shape1Type, CollisionShapeType shape2Type) const { - return m_collisionMatrix[shape1Type][shape2Type]; -} - -// Set the collision dispatch configuration -void CollisionDetection::setCollisionDispatch(CollisionDispatch* collisionDispatch) { - m_collisionDispatch = collisionDispatch; - - m_collisionDispatch->init(this, &m_memoryAllocator); - - // Fill-in the collision matrix with the new algorithms to use - fillInCollisionMatrix(); -} - -// Add a body to the collision detection -void CollisionDetection::addProxyCollisionShape(ProxyShape* proxyShape, - const AABB& aabb) { - - // Add the body to the broad-phase - m_broadPhaseAlgorithm.addProxyCollisionShape(proxyShape, aabb); - - m_isCollisionShapesAdded = true; -} - -// Add a pair of bodies that cannot collide with each other -void CollisionDetection::addNoCollisionPair(CollisionBody* body1, - CollisionBody* body2) { - m_noCollisionPairs.insert(OverlappingPair::computeBodiesIndexPair(body1, body2)); -} - -// Remove a pair of bodies that cannot collide with each other -void CollisionDetection::removeNoCollisionPair(CollisionBody* body1, - CollisionBody* body2) { - m_noCollisionPairs.erase(OverlappingPair::computeBodiesIndexPair(body1, body2)); -} - -// Ask for a collision shape to be tested again during broad-phase. -/// We simply put the shape in the list of collision shape that have moved in the -/// previous frame so that it is tested for collision again in the broad-phase. -void CollisionDetection::askForBroadPhaseCollisionCheck(ProxyShape* shape) { - m_broadPhaseAlgorithm.addMovedCollisionShape(shape->m_broadPhaseID); -} - -// Update a proxy collision shape (that has moved for instance) -void CollisionDetection::updateProxyCollisionShape(ProxyShape* shape, const AABB& aabb, - const vec3& displacement, bool forceReinsert) { - m_broadPhaseAlgorithm.updateProxyCollisionShape(shape, aabb, displacement); -} - -// Ray casting method -void CollisionDetection::raycast(RaycastCallback* raycastCallback, - const Ray& ray, - unsigned short raycastWithCategoryMaskBits) const { - - PROFILE("CollisionDetection::raycast()"); - - RaycastTest rayCastTest(raycastCallback); - - // Ask the broad-phase algorithm to call the testRaycastAgainstShape() - // callback method for each proxy shape hit by the ray in the broad-phase - m_broadPhaseAlgorithm.raycast(ray, rayCastTest, raycastWithCategoryMaskBits); -} - -// Test if the AABBs of two proxy shapes overlap -bool CollisionDetection::testAABBOverlap(const ProxyShape* shape1, - const ProxyShape* shape2) const { - - // If one of the shape's body is not active, we return no overlap - if (!shape1->getBody()->isActive() || !shape2->getBody()->isActive()) { - return false; - } - - return m_broadPhaseAlgorithm.testOverlappingShapes(shape1, shape2); -} - -// Return a pointer to the world -CollisionWorld* CollisionDetection::getWorld() { - return m_world; -} - } diff --git a/ephysics/collision/CollisionShapeInfo.hpp b/ephysics/collision/CollisionShapeInfo.hpp index 57feb05..45b92ca 100644 --- a/ephysics/collision/CollisionShapeInfo.hpp +++ b/ephysics/collision/CollisionShapeInfo.hpp @@ -5,49 +5,35 @@ */ #pragma once -// Libraries #include -/// Namespace ReactPhysics3D namespace ephysics { - -class OverlappingPair; - -// Class CollisionShapeInfo -/** - * This structure regroups different things about a collision shape. This is - * used to pass information about a collision shape to a collision algorithm. - */ -struct CollisionShapeInfo { - - public: - - /// Broadphase overlapping pair - OverlappingPair* overlappingPair; - - /// Proxy shape - ProxyShape* proxyShape; - - /// Pointer to the collision shape - const CollisionShape* collisionShape; - - /// etk::Transform3D that maps from collision shape local-space to world-space - etk::Transform3D shapeToWorldTransform; - - /// Cached collision data of the proxy shape - void** cachedCollisionData; - - /// Constructor - CollisionShapeInfo(ProxyShape* proxyCollisionShape, const CollisionShape* shape, - const etk::Transform3D& shapeLocalToWorldTransform, OverlappingPair* pair, - void** cachedData) - : overlappingPair(pair), proxyShape(proxyCollisionShape), collisionShape(shape), - shapeToWorldTransform(shapeLocalToWorldTransform), - cachedCollisionData(cachedData) { - - } -}; - + class OverlappingPair; + /** + * @brief It regroups different things about a collision shape. This is + * used to pass information about a collision shape to a collision algorithm. + */ + struct CollisionShapeInfo { + public: + OverlappingPair* overlappingPair; //!< Broadphase overlapping pair + ProxyShape* proxyShape; //!< Proxy shape + const CollisionShape* collisionShape; //!< Pointer to the collision shape + etk::Transform3D shapeToWorldTransform; //!< etk::Transform3D that maps from collision shape local-space to world-space + void** cachedCollisionData; //!< Cached collision data of the proxy shape + /// Constructor + CollisionShapeInfo(ProxyShape* _proxyCollisionShape, + const CollisionShape* _shape, + const etk::Transform3D& _shapeLocalToWorldTransform, + OverlappingPair* _pair, + void** _cachedData): + overlappingPair(_pair), + proxyShape(_proxyCollisionShape), + collisionShape(_shape), + shapeToWorldTransform(_shapeLocalToWorldTransform), + cachedCollisionData(_cachedData) { + + } + }; } diff --git a/ephysics/collision/ContactManifold.cpp b/ephysics/collision/ContactManifold.cpp index ce266c3..d609f9b 100644 --- a/ephysics/collision/ContactManifold.cpp +++ b/ephysics/collision/ContactManifold.cpp @@ -243,3 +243,120 @@ void ContactManifold::clear() { } m_nbContactPoints = 0; } + +// Return a pointer to the first proxy shape of the contact +ProxyShape* ContactManifold::getShape1() const { + return m_shape1; +} + +// Return a pointer to the second proxy shape of the contact +ProxyShape* ContactManifold::getShape2() const { + return m_shape2; +} + +// Return a pointer to the first body of the contact manifold +CollisionBody* ContactManifold::getBody1() const { + return m_shape1->getBody(); +} + +// Return a pointer to the second body of the contact manifold +CollisionBody* ContactManifold::getBody2() const { + return m_shape2->getBody(); +} + +// Return the normal direction Id +int16_t ContactManifold::getNormalDirectionId() const { + return m_normalDirectionId; +} + +// Return the number of contact points in the manifold +uint32_t ContactManifold::getNbContactPoints() const { + return m_nbContactPoints; +} + +// Return the first friction vector at the center of the contact manifold +const vec3& ContactManifold::getFrictionVector1() const { + return m_frictionVector1; +} + +// set the first friction vector at the center of the contact manifold +void ContactManifold::setFrictionVector1(const vec3& frictionVector1) { + m_frictionVector1 = frictionVector1; +} + +// Return the second friction vector at the center of the contact manifold +const vec3& ContactManifold::getFrictionvec2() const { + return m_frictionvec2; +} + +// set the second friction vector at the center of the contact manifold +void ContactManifold::setFrictionvec2(const vec3& frictionvec2) { + m_frictionvec2 = frictionvec2; +} + +// Return the first friction accumulated impulse +float ContactManifold::getFrictionImpulse1() const { + return m_frictionImpulse1; +} + +// Set the first friction accumulated impulse +void ContactManifold::setFrictionImpulse1(float frictionImpulse1) { + m_frictionImpulse1 = frictionImpulse1; +} + +// Return the second friction accumulated impulse +float ContactManifold::getFrictionImpulse2() const { + return m_frictionImpulse2; +} + +// Set the second friction accumulated impulse +void ContactManifold::setFrictionImpulse2(float frictionImpulse2) { + m_frictionImpulse2 = frictionImpulse2; +} + +// Return the friction twist accumulated impulse +float ContactManifold::getFrictionTwistImpulse() const { + return m_frictionTwistImpulse; +} + +// Set the friction twist accumulated impulse +void ContactManifold::setFrictionTwistImpulse(float frictionTwistImpulse) { + m_frictionTwistImpulse = frictionTwistImpulse; +} + +// Set the accumulated rolling resistance impulse +void ContactManifold::setRollingResistanceImpulse(const vec3& rollingResistanceImpulse) { + m_rollingResistanceImpulse = rollingResistanceImpulse; +} + +// Return a contact point of the manifold +ContactPoint* ContactManifold::getContactPoint(uint32_t index) const { + assert(index < m_nbContactPoints); + return m_contactPoints[index]; +} + +// Return true if the contact manifold has already been added int32_to an island +bool ContactManifold::isAlreadyInIsland() const { + return m_isAlreadyInIsland; +} + +// Return the normalized averaged normal vector +vec3 ContactManifold::getAverageContactNormal() const { + vec3 averageNormal; + for (uint32_t i=0; igetNormal(); + } + return averageNormal.safeNormalized(); +} + +// Return the largest depth of all the contact points +float ContactManifold::getLargestContactDepth() const { + float largestDepth = 0.0f; + for (uint32_t i=0; igetPenetrationDepth(); + if (depth > largestDepth) { + largestDepth = depth; + } + } + return largestDepth; +} diff --git a/ephysics/collision/ContactManifold.hpp b/ephysics/collision/ContactManifold.hpp index 3119e11..fb81bb9 100644 --- a/ephysics/collision/ContactManifold.hpp +++ b/ephysics/collision/ContactManifold.hpp @@ -135,123 +135,6 @@ namespace ephysics { friend class CollisionBody; }; -// Return a pointer to the first proxy shape of the contact -ProxyShape* ContactManifold::getShape1() const { - return m_shape1; -} - -// Return a pointer to the second proxy shape of the contact -ProxyShape* ContactManifold::getShape2() const { - return m_shape2; -} - -// Return a pointer to the first body of the contact manifold -CollisionBody* ContactManifold::getBody1() const { - return m_shape1->getBody(); -} - -// Return a pointer to the second body of the contact manifold -CollisionBody* ContactManifold::getBody2() const { - return m_shape2->getBody(); -} - -// Return the normal direction Id -int16_t ContactManifold::getNormalDirectionId() const { - return m_normalDirectionId; -} - -// Return the number of contact points in the manifold -uint32_t ContactManifold::getNbContactPoints() const { - return m_nbContactPoints; -} - -// Return the first friction vector at the center of the contact manifold -const vec3& ContactManifold::getFrictionVector1() const { - return m_frictionVector1; -} - -// set the first friction vector at the center of the contact manifold -void ContactManifold::setFrictionVector1(const vec3& frictionVector1) { - m_frictionVector1 = frictionVector1; -} - -// Return the second friction vector at the center of the contact manifold -const vec3& ContactManifold::getFrictionvec2() const { - return m_frictionvec2; -} - -// set the second friction vector at the center of the contact manifold -void ContactManifold::setFrictionvec2(const vec3& frictionvec2) { - m_frictionvec2 = frictionvec2; -} - -// Return the first friction accumulated impulse -float ContactManifold::getFrictionImpulse1() const { - return m_frictionImpulse1; -} - -// Set the first friction accumulated impulse -void ContactManifold::setFrictionImpulse1(float frictionImpulse1) { - m_frictionImpulse1 = frictionImpulse1; -} - -// Return the second friction accumulated impulse -float ContactManifold::getFrictionImpulse2() const { - return m_frictionImpulse2; -} - -// Set the second friction accumulated impulse -void ContactManifold::setFrictionImpulse2(float frictionImpulse2) { - m_frictionImpulse2 = frictionImpulse2; -} - -// Return the friction twist accumulated impulse -float ContactManifold::getFrictionTwistImpulse() const { - return m_frictionTwistImpulse; -} - -// Set the friction twist accumulated impulse -void ContactManifold::setFrictionTwistImpulse(float frictionTwistImpulse) { - m_frictionTwistImpulse = frictionTwistImpulse; -} - -// Set the accumulated rolling resistance impulse -void ContactManifold::setRollingResistanceImpulse(const vec3& rollingResistanceImpulse) { - m_rollingResistanceImpulse = rollingResistanceImpulse; -} - -// Return a contact point of the manifold -ContactPoint* ContactManifold::getContactPoint(uint32_t index) const { - assert(index < m_nbContactPoints); - return m_contactPoints[index]; -} - -// Return true if the contact manifold has already been added int32_to an island -bool ContactManifold::isAlreadyInIsland() const { - return m_isAlreadyInIsland; -} - -// Return the normalized averaged normal vector -vec3 ContactManifold::getAverageContactNormal() const { - vec3 averageNormal; - for (uint32_t i=0; igetNormal(); - } - return averageNormal.safeNormalized(); -} - -// Return the largest depth of all the contact points -float ContactManifold::getLargestContactDepth() const { - float largestDepth = 0.0f; - for (uint32_t i=0; igetPenetrationDepth(); - if (depth > largestDepth) { - largestDepth = depth; - } - } - return largestDepth; -} - } diff --git a/ephysics/collision/ContactManifoldSet.cpp b/ephysics/collision/ContactManifoldSet.cpp index eb8c4b8..a435785 100644 --- a/ephysics/collision/ContactManifoldSet.cpp +++ b/ephysics/collision/ContactManifoldSet.cpp @@ -215,3 +215,33 @@ void ContactManifoldSet::removeManifold(int32_t index) { m_nbManifolds--; } + +// Return the first proxy shape +ProxyShape* ContactManifoldSet::getShape1() const { + return m_shape1; +} + +// Return the second proxy shape +ProxyShape* ContactManifoldSet::getShape2() const { + return m_shape2; +} + +// Return the number of manifolds in the set +int32_t ContactManifoldSet::getNbContactManifolds() const { + return m_nbManifolds; +} + +// Return a given contact manifold +ContactManifold* ContactManifoldSet::getContactManifold(int32_t index) const { + assert(index >= 0 && index < m_nbManifolds); + return m_manifolds[index]; +} + +// Return the total number of contact points in the set of manifolds +int32_t ContactManifoldSet::getTotalNbContactPoints() const { + int32_t nbPoints = 0; + for (int32_t i=0; igetNbContactPoints(); + } + return nbPoints; +} diff --git a/ephysics/collision/ContactManifoldSet.hpp b/ephysics/collision/ContactManifoldSet.hpp index 95ead24..afbbd41 100644 --- a/ephysics/collision/ContactManifoldSet.hpp +++ b/ephysics/collision/ContactManifoldSet.hpp @@ -60,35 +60,5 @@ namespace ephysics { int32_t getTotalNbContactPoints() const; }; -// Return the first proxy shape -ProxyShape* ContactManifoldSet::getShape1() const { - return m_shape1; -} - -// Return the second proxy shape -ProxyShape* ContactManifoldSet::getShape2() const { - return m_shape2; -} - -// Return the number of manifolds in the set -int32_t ContactManifoldSet::getNbContactManifolds() const { - return m_nbManifolds; -} - -// Return a given contact manifold -ContactManifold* ContactManifoldSet::getContactManifold(int32_t index) const { - assert(index >= 0 && index < m_nbManifolds); - return m_manifolds[index]; -} - -// Return the total number of contact points in the set of manifolds -int32_t ContactManifoldSet::getTotalNbContactPoints() const { - int32_t nbPoints = 0; - for (int32_t i=0; igetNbContactPoints(); - } - return nbPoints; -} - } diff --git a/ephysics/collision/ProxyShape.cpp b/ephysics/collision/ProxyShape.cpp index 5480dcd..16109a0 100644 --- a/ephysics/collision/ProxyShape.cpp +++ b/ephysics/collision/ProxyShape.cpp @@ -25,7 +25,6 @@ ProxyShape::ProxyShape(CollisionBody* body, CollisionShape* shape, const etk::Tr // Destructor ProxyShape::~ProxyShape() { - // Release the cached collision data memory if (m_cachedCollisionData != NULL) { free(m_cachedCollisionData); @@ -71,3 +70,148 @@ bool ProxyShape::raycast(const Ray& ray, RaycastInfo& raycastInfo) { return isHit; } + +// Return the pointer to the cached collision data +void** ProxyShape::getCachedCollisionData() { + return &m_cachedCollisionData; +} + +// Return the collision shape +/** + * @return Pointer to the int32_ternal collision shape + */ +const CollisionShape* ProxyShape::getCollisionShape() const { + return m_collisionShape; +} + +// Return the parent body +/** + * @return Pointer to the parent body + */ +CollisionBody* ProxyShape::getBody() const { + return m_body; +} + +// Return the mass of the collision shape +/** + * @return Mass of the collision shape (in kilograms) + */ +float ProxyShape::getMass() const { + return m_mass; +} + +// Return a pointer to the user data attached to this body +/** + * @return A pointer to the user data stored int32_to the proxy shape + */ +void* ProxyShape::getUserData() const { + return m_userData; +} + +// Attach user data to this body +/** + * @param userData Pointer to the user data you want to store within the proxy shape + */ +void ProxyShape::setUserData(void* userData) { + m_userData = userData; +} + +// Return the local to parent body transform +/** + * @return The transformation that transforms the local-space of the collision shape + * to the local-space of the parent body + */ +const etk::Transform3D& ProxyShape::getLocalToBodyTransform() const { + return m_localToBodyTransform; +} + +// Set the local to parent body transform +void ProxyShape::setLocalToBodyTransform(const etk::Transform3D& transform) { + + m_localToBodyTransform = transform; + + m_body->setIsSleeping(false); + + // Notify the body that the proxy shape has to be updated in the broad-phase + m_body->updateProxyShapeInBroadPhase(this, true); +} + +// Return the local to world transform +/** + * @return The transformation that transforms the local-space of the collision + * shape to the world-space + */ +const etk::Transform3D ProxyShape::getLocalToWorldTransform() const { + return m_body->m_transform * m_localToBodyTransform; +} + +// Return the next proxy shape in the linked list of proxy shapes +/** + * @return Pointer to the next proxy shape in the linked list of proxy shapes + */ +ProxyShape* ProxyShape::getNext() { + return m_next; +} + +// Return the next proxy shape in the linked list of proxy shapes +/** + * @return Pointer to the next proxy shape in the linked list of proxy shapes + */ +const ProxyShape* ProxyShape::getNext() const { + return m_next; +} + +// Return the collision category bits +/** + * @return The collision category bits mask of the proxy shape + */ +unsigned short ProxyShape::getCollisionCategoryBits() const { + return m_collisionCategoryBits; +} + +// Set the collision category bits +/** + * @param collisionCategoryBits The collision category bits mask of the proxy shape + */ +void ProxyShape::setCollisionCategoryBits(unsigned short collisionCategoryBits) { + m_collisionCategoryBits = collisionCategoryBits; +} + +// Return the collision bits mask +/** + * @return The bits mask that specifies with which collision category this shape will collide + */ +unsigned short ProxyShape::getCollideWithMaskBits() const { + return m_collideWithMaskBits; +} + +// Set the collision bits mask +/** + * @param collideWithMaskBits The bits mask that specifies with which collision category this shape will collide + */ +void ProxyShape::setCollideWithMaskBits(unsigned short collideWithMaskBits) { + m_collideWithMaskBits = collideWithMaskBits; +} + +// Return the local scaling vector of the collision shape +/** + * @return The local scaling vector + */ +vec3 ProxyShape::getLocalScaling() const { + return m_collisionShape->getScaling(); +} + +// Set the local scaling vector of the collision shape +/** + * @param scaling The new local scaling vector + */ +void ProxyShape::setLocalScaling(const vec3& scaling) { + + // Set the local scaling of the collision shape + m_collisionShape->setLocalScaling(scaling); + + m_body->setIsSleeping(false); + + // Notify the body that the proxy shape has to be updated in the broad-phase + m_body->updateProxyShapeInBroadPhase(this, true); +} diff --git a/ephysics/collision/ProxyShape.hpp b/ephysics/collision/ProxyShape.hpp index 06ec9ea..bf78503 100644 --- a/ephysics/collision/ProxyShape.hpp +++ b/ephysics/collision/ProxyShape.hpp @@ -5,301 +5,128 @@ */ #pragma once -// Libraries #include #include namespace ephysics { - -// Class ProxyShape -/** - * The CollisionShape instances are supposed to be unique for memory optimization. For instance, - * consider two rigid bodies with the same sphere collision shape. In this situation, we will have - * a unique instance of SphereShape but we need to differentiate between the two instances during - * the collision detection. They do not have the same position in the world and they do not - * belong to the same rigid body. The ProxyShape class is used for that purpose by attaching a - * rigid body with one of its collision shape. A body can have multiple proxy shapes (one for - * each collision shape attached to the body). - */ -class ProxyShape { - - protected: - - // -------------------- Attributes -------------------- // - - /// Pointer to the parent body - CollisionBody* m_body; - - /// Internal collision shape - CollisionShape* m_collisionShape; - - /// Local-space to parent body-space transform (does not change over time) - etk::Transform3D m_localToBodyTransform; - - /// Mass (in kilogramms) of the corresponding collision shape - float m_mass; - - /// Pointer to the next proxy shape of the body (linked list) - ProxyShape* m_next; - - /// Broad-phase ID (node ID in the dynamic AABB tree) - int32_t m_broadPhaseID; - - /// Cached collision data - void* m_cachedCollisionData; - - /// Pointer to user data - void* m_userData; - - /// Bits used to define the collision category of this shape. - /// You can set a single bit to one to define a category value for this - /// shape. This value is one (0x0001) by default. This variable can be used - /// together with the m_collideWithMaskBits variable so that given - /// categories of shapes collide with each other and do not collide with - /// other categories. - unsigned short m_collisionCategoryBits; - - /// Bits mask used to state which collision categories this shape can - /// collide with. This value is 0xFFFF by default. It means that this - /// proxy shape will collide with every collision categories by default. - unsigned short m_collideWithMaskBits; - - // -------------------- Methods -------------------- // - - /// Private copy-constructor - ProxyShape(const ProxyShape& proxyShape); - - /// Private assignment operator - ProxyShape& operator=(const ProxyShape& proxyShape); - - public: - - // -------------------- Methods -------------------- // - - /// Constructor - ProxyShape(CollisionBody* body, CollisionShape* shape, - const etk::Transform3D& transform, float mass); - - /// Destructor - virtual ~ProxyShape(); - - /// Return the collision shape - const CollisionShape* getCollisionShape() const; - - /// Return the parent body - CollisionBody* getBody() const; - - /// Return the mass of the collision shape - float getMass() const; - - /// Return a pointer to the user data attached to this body - void* getUserData() const; - - /// Attach user data to this body - void setUserData(void* userData); - - /// Return the local to parent body transform - const etk::Transform3D& getLocalToBodyTransform() const; - - /// Set the local to parent body transform - void setLocalToBodyTransform(const etk::Transform3D& transform); - - /// Return the local to world transform - const etk::Transform3D getLocalToWorldTransform() const; - - /// Return true if a point is inside the collision shape - bool testPointInside(const vec3& worldPoint); - - /// Raycast method with feedback information - bool raycast(const Ray& ray, RaycastInfo& raycastInfo); - - /// Return the collision bits mask - unsigned short getCollideWithMaskBits() const; - - /// Set the collision bits mask - void setCollideWithMaskBits(unsigned short collideWithMaskBits); - - /// Return the collision category bits - unsigned short getCollisionCategoryBits() const; - - /// Set the collision category bits - void setCollisionCategoryBits(unsigned short collisionCategoryBits); - - /// Return the next proxy shape in the linked list of proxy shapes - ProxyShape* getNext(); - - /// Return the next proxy shape in the linked list of proxy shapes - const ProxyShape* getNext() const; - - /// Return the pointer to the cached collision data - void** getCachedCollisionData(); - - /// Return the local scaling vector of the collision shape - vec3 getLocalScaling() const; - - /// Set the local scaling vector of the collision shape - virtual void setLocalScaling(const vec3& scaling); - - // -------------------- Friendship -------------------- // - - friend class OverlappingPair; - friend class CollisionBody; - friend class RigidBody; - friend class BroadPhaseAlgorithm; - friend class DynamicAABBTree; - friend class CollisionDetection; - friend class CollisionWorld; - friend class DynamicsWorld; - friend class EPAAlgorithm; - friend class GJKAlgorithm; - friend class ConvexMeshShape; - -}; - -// Return the pointer to the cached collision data -void** ProxyShape::getCachedCollisionData() { - return &m_cachedCollisionData; -} - -// Return the collision shape -/** - * @return Pointer to the int32_ternal collision shape - */ -const CollisionShape* ProxyShape::getCollisionShape() const { - return m_collisionShape; -} - -// Return the parent body -/** - * @return Pointer to the parent body - */ -CollisionBody* ProxyShape::getBody() const { - return m_body; -} - -// Return the mass of the collision shape -/** - * @return Mass of the collision shape (in kilograms) - */ -float ProxyShape::getMass() const { - return m_mass; -} - -// Return a pointer to the user data attached to this body -/** - * @return A pointer to the user data stored int32_to the proxy shape - */ -void* ProxyShape::getUserData() const { - return m_userData; -} - -// Attach user data to this body -/** - * @param userData Pointer to the user data you want to store within the proxy shape - */ -void ProxyShape::setUserData(void* userData) { - m_userData = userData; -} - -// Return the local to parent body transform -/** - * @return The transformation that transforms the local-space of the collision shape - * to the local-space of the parent body - */ -const etk::Transform3D& ProxyShape::getLocalToBodyTransform() const { - return m_localToBodyTransform; -} - -// Set the local to parent body transform -void ProxyShape::setLocalToBodyTransform(const etk::Transform3D& transform) { - - m_localToBodyTransform = transform; - - m_body->setIsSleeping(false); - - // Notify the body that the proxy shape has to be updated in the broad-phase - m_body->updateProxyShapeInBroadPhase(this, true); -} - -// Return the local to world transform -/** - * @return The transformation that transforms the local-space of the collision - * shape to the world-space - */ -const etk::Transform3D ProxyShape::getLocalToWorldTransform() const { - return m_body->m_transform * m_localToBodyTransform; -} - -// Return the next proxy shape in the linked list of proxy shapes -/** - * @return Pointer to the next proxy shape in the linked list of proxy shapes - */ -ProxyShape* ProxyShape::getNext() { - return m_next; -} - -// Return the next proxy shape in the linked list of proxy shapes -/** - * @return Pointer to the next proxy shape in the linked list of proxy shapes - */ -const ProxyShape* ProxyShape::getNext() const { - return m_next; -} - -// Return the collision category bits -/** - * @return The collision category bits mask of the proxy shape - */ -unsigned short ProxyShape::getCollisionCategoryBits() const { - return m_collisionCategoryBits; -} - -// Set the collision category bits -/** - * @param collisionCategoryBits The collision category bits mask of the proxy shape - */ -void ProxyShape::setCollisionCategoryBits(unsigned short collisionCategoryBits) { - m_collisionCategoryBits = collisionCategoryBits; -} - -// Return the collision bits mask -/** - * @return The bits mask that specifies with which collision category this shape will collide - */ -unsigned short ProxyShape::getCollideWithMaskBits() const { - return m_collideWithMaskBits; -} - -// Set the collision bits mask -/** - * @param collideWithMaskBits The bits mask that specifies with which collision category this shape will collide - */ -void ProxyShape::setCollideWithMaskBits(unsigned short collideWithMaskBits) { - m_collideWithMaskBits = collideWithMaskBits; -} - -// Return the local scaling vector of the collision shape -/** - * @return The local scaling vector - */ -vec3 ProxyShape::getLocalScaling() const { - return m_collisionShape->getScaling(); -} - -// Set the local scaling vector of the collision shape -/** - * @param scaling The new local scaling vector - */ -void ProxyShape::setLocalScaling(const vec3& scaling) { - - // Set the local scaling of the collision shape - m_collisionShape->setLocalScaling(scaling); - - m_body->setIsSleeping(false); - - // Notify the body that the proxy shape has to be updated in the broad-phase - m_body->updateProxyShapeInBroadPhase(this, true); -} + /** + * @breif The CollisionShape instances are supposed to be unique for memory optimization. For instance, + * consider two rigid bodies with the same sphere collision shape. In this situation, we will have + * a unique instance of SphereShape but we need to differentiate between the two instances during + * the collision detection. They do not have the same position in the world and they do not + * belong to the same rigid body. The ProxyShape class is used for that purpose by attaching a + * rigid body with one of its collision shape. A body can have multiple proxy shapes (one for + * each collision shape attached to the body). + */ + class ProxyShape { + protected: + CollisionBody* m_body; //!< Pointer to the parent body + CollisionShape* m_collisionShape; //!< Internal collision shape + etk::Transform3D m_localToBodyTransform; //!< Local-space to parent body-space transform (does not change over time) + float m_mass; //!< Mass (in kilogramms) of the corresponding collision shape + ProxyShape* m_next; //!< Pointer to the next proxy shape of the body (linked list) + int32_t m_broadPhaseID; //!< Broad-phase ID (node ID in the dynamic AABB tree) + void* m_cachedCollisionData; //!< Cached collision data + void* m_userData; //!< Pointer to user data + /** + * @brief Bits used to define the collision category of this shape. + * You can set a single bit to one to define a category value for this + * shape. This value is one (0x0001) by default. This variable can be used + * together with the m_collideWithMaskBits variable so that given + * categories of shapes collide with each other and do not collide with + * other categories. + */ + unsigned short m_collisionCategoryBits; + /** + * @brief Bits mask used to state which collision categories this shape can + * collide with. This value is 0xFFFF by default. It means that this + * proxy shape will collide with every collision categories by default. + */ + unsigned short m_collideWithMaskBits; + /// Private copy-constructor + ProxyShape(const ProxyShape&) = delete; + /// Private assignment operator + ProxyShape& operator=(const ProxyShape&) = delete; + public: + /// Constructor + ProxyShape(CollisionBody* _body, + CollisionShape* _shape, + const etk::Transform3D& _transform, + float _mass); + + /// Destructor + virtual ~ProxyShape(); + + /// Return the collision shape + const CollisionShape* getCollisionShape() const; + + /// Return the parent body + CollisionBody* getBody() const; + + /// Return the mass of the collision shape + float getMass() const; + + /// Return a pointer to the user data attached to this body + void* getUserData() const; + + /// Attach user data to this body + void setUserData(void* _userData); + + /// Return the local to parent body transform + const etk::Transform3D& getLocalToBodyTransform() const; + + /// Set the local to parent body transform + void setLocalToBodyTransform(const etk::Transform3D& _transform); + + /// Return the local to world transform + const etk::Transform3D getLocalToWorldTransform() const; + + /// Return true if a point is inside the collision shape + bool testPointInside(const vec3& _worldPoint); + + /// Raycast method with feedback information + bool raycast(const Ray& _ray, RaycastInfo& _raycastInfo); + + /// Return the collision bits mask + unsigned short getCollideWithMaskBits() const; + + /// Set the collision bits mask + void setCollideWithMaskBits(unsigned short _collideWithMaskBits); + + /// Return the collision category bits + unsigned short getCollisionCategoryBits() const; + + /// Set the collision category bits + void setCollisionCategoryBits(unsigned short _collisionCategoryBits); + + /// Return the next proxy shape in the linked list of proxy shapes + ProxyShape* getNext(); + + /// Return the next proxy shape in the linked list of proxy shapes + const ProxyShape* getNext() const; + + /// Return the pointer to the cached collision data + void** getCachedCollisionData(); + + /// Return the local scaling vector of the collision shape + vec3 getLocalScaling() const; + + /// Set the local scaling vector of the collision shape + virtual void setLocalScaling(const vec3& _scaling); + + friend class OverlappingPair; + friend class CollisionBody; + friend class RigidBody; + friend class BroadPhaseAlgorithm; + friend class DynamicAABBTree; + friend class CollisionDetection; + friend class CollisionWorld; + friend class DynamicsWorld; + friend class EPAAlgorithm; + friend class GJKAlgorithm; + friend class ConvexMeshShape; + + }; } diff --git a/ephysics/collision/RaycastInfo.hpp b/ephysics/collision/RaycastInfo.hpp index 7a78728..afc9a61 100644 --- a/ephysics/collision/RaycastInfo.hpp +++ b/ephysics/collision/RaycastInfo.hpp @@ -5,126 +5,81 @@ */ #pragma once -// Libraries #include #include -/// ReactPhysics3D namespace namespace ephysics { - -// Declarations -class CollisionBody; -class ProxyShape; -class CollisionShape; - -// Structure RaycastInfo -/** - * This structure contains the information about a raycast hit. - */ -struct RaycastInfo { - - private: - - // -------------------- Methods -------------------- // - - /// Private copy constructor - RaycastInfo(const RaycastInfo& raycastInfo); - - /// Private assignment operator - RaycastInfo& operator=(const RaycastInfo& raycastInfo); - - public: - - // -------------------- Attributes -------------------- // - - /// Hit point in world-space coordinates - vec3 worldPoint; - - /// Surface normal at hit point in world-space coordinates - vec3 worldNormal; - - /// Fraction distance of the hit point between point1 and point2 of the ray - /// The hit point "p" is such that p = point1 + hitFraction * (point2 - point1) - float hitFraction; - - /// Mesh subpart index that has been hit (only used for triangles mesh and -1 otherwise) - int32_t meshSubpart; - - /// Hit triangle index (only used for triangles mesh and -1 otherwise) - int32_t triangleIndex; - - /// Pointer to the hit collision body - CollisionBody* body; - - /// Pointer to the hit proxy collision shape - ProxyShape* proxyShape; - - // -------------------- Methods -------------------- // - - /// Constructor - RaycastInfo() : meshSubpart(-1), triangleIndex(-1), body(NULL), proxyShape(NULL) { - - } - - /// Destructor - virtual ~RaycastInfo() { - - } -}; - -// Class RaycastCallback -/** - * This class can be used to register a callback for ray casting 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 RaycastCallback { - - public: - - // -------------------- Methods -------------------- // - - /// Destructor - virtual ~RaycastCallback() { - - } - - /// This method will be called for each ProxyShape that is hit by the - /// ray. You cannot make any assumptions about the order of the - /// calls. You should use the return value to control the continuation - /// of the ray. The returned value is the next maxFraction value to use. - /// If you return a fraction of 0.0, it means that the raycast should - /// terminate. If you return a fraction of 1.0, it indicates that the - /// ray is not clipped and the ray cast should continue as if no hit - /// occurred. If you return the fraction in the parameter (hitFraction - /// value in the RaycastInfo object), the current ray will be clipped - /// to this fraction in the next queries. If you return -1.0, it will - /// ignore this ProxyShape and continue the ray cast. - /** - * @param raycastInfo Information about the raycast hit - * @return Value that controls the continuation of the ray after a hit - */ - virtual float notifyRaycastHit(const RaycastInfo& raycastInfo)=0; - -}; - -/// Structure RaycastTest -struct RaycastTest { - - public: - - /// User callback class - RaycastCallback* userCallback; - - /// Constructor - RaycastTest(RaycastCallback* callback) { - userCallback = callback; - } - - /// Ray cast test against a proxy shape - float raycastAgainstShape(ProxyShape* shape, const Ray& ray); -}; + class CollisionBody; + class ProxyShape; + class CollisionShape; + /** + * @brief It contains the information about a raycast hit. + */ + struct RaycastInfo { + private: + /// Private copy constructor + RaycastInfo(const RaycastInfo&) = delete; + /// Private assignment operator + RaycastInfo& operator=(const RaycastInfo&) = delete; + public: + vec3 worldPoint; //!< Hit point in world-space coordinates + vec3 worldNormal; //!< Surface normal at hit point in world-space coordinates + float hitFraction; //!< Fraction distance of the hit point between point1 and point2 of the ray. The hit point "p" is such that p = point1 + hitFraction * (point2 - point1) + int32_t meshSubpart; //!< Mesh subpart index that has been hit (only used for triangles mesh and -1 otherwise) + int32_t triangleIndex; //!< Hit triangle index (only used for triangles mesh and -1 otherwise) + CollisionBody* body; //!< Pointer to the hit collision body + ProxyShape* proxyShape; //!< Pointer to the hit proxy collision shape + /// Constructor + RaycastInfo() : + meshSubpart(-1), + triangleIndex(-1), + body(nullptr), + proxyShape(nullptr) { + + } + + /// Destructor + virtual ~RaycastInfo() = default; + }; + + /** + * @brief It can be used to register a callback for ray casting 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 RaycastCallback { + public: + /// Destructor + virtual ~RaycastCallback() = default; + /** + * @brief This method will be called for each ProxyShape that is hit by the + * ray. You cannot make any assumptions about the order of the + * calls. You should use the return value to control the continuation + * of the ray. The returned value is the next maxFraction value to use. + * If you return a fraction of 0.0, it means that the raycast should + * terminate. If you return a fraction of 1.0, it indicates that the + * ray is not clipped and the ray cast should continue as if no hit + * occurred. If you return the fraction in the parameter (hitFraction + * value in the RaycastInfo object), the current ray will be clipped + * to this fraction in the next queries. If you return -1.0, it will + * ignore this ProxyShape and continue the ray cast. + * @param[in] _raycastInfo Information about the raycast hit + * @return Value that controls the continuation of the ray after a hit + */ + virtual float notifyRaycastHit(const RaycastInfo& _raycastInfo)=0; + }; + + struct RaycastTest { + public: + RaycastCallback* userCallback; //!< User callback class + /// Constructor + RaycastTest(RaycastCallback* _callback) { + userCallback = _callback; + } + /// Ray cast test against a proxy shape + float raycastAgainstShape(ProxyShape* _shape, const Ray& _ray); + }; } diff --git a/ephysics/collision/TriangleVertexArray.cpp b/ephysics/collision/TriangleVertexArray.cpp index 27a6ece..2497b72 100644 --- a/ephysics/collision/TriangleVertexArray.cpp +++ b/ephysics/collision/TriangleVertexArray.cpp @@ -40,3 +40,43 @@ TriangleVertexArray::TriangleVertexArray(uint32_t nbVertices, void* verticesStar TriangleVertexArray::~TriangleVertexArray() { } + +// Return the vertex data type +TriangleVertexArray::VertexDataType TriangleVertexArray::getVertexDataType() const { + return m_vertexDataType; +} + +// Return the index data type +TriangleVertexArray::IndexDataType TriangleVertexArray::getIndexDataType() const { + return m_indexDataType; +} + +// Return the number of vertices +uint32_t TriangleVertexArray::getNbVertices() const { + return m_numberVertices; +} + +// Return the number of triangles +uint32_t TriangleVertexArray::getNbTriangles() const { + return m_numberTriangles; +} + +// Return the vertices stride (number of bytes) +int32_t TriangleVertexArray::getVerticesStride() const { + return m_verticesStride; +} + +// Return the indices stride (number of bytes) +int32_t TriangleVertexArray::getIndicesStride() const { + return m_indicesStride; +} + +// Return the pointer to the start of the vertices array +unsigned char* TriangleVertexArray::getVerticesStart() const { + return m_verticesStart; +} + +// Return the pointer to the start of the indices array +unsigned char* TriangleVertexArray::getIndicesStart() const { + return m_indicesStart; +} \ No newline at end of file diff --git a/ephysics/collision/TriangleVertexArray.hpp b/ephysics/collision/TriangleVertexArray.hpp index ae90601..dfcbf86 100644 --- a/ephysics/collision/TriangleVertexArray.hpp +++ b/ephysics/collision/TriangleVertexArray.hpp @@ -8,105 +8,55 @@ #include namespace ephysics { -/** - * This class is used to describe the vertices and faces of a triangular mesh. - * A TriangleVertexArray represents a continuous array of vertices and indexes - * of a triangular mesh. When you create a TriangleVertexArray, no data is copied - * int32_to the array. It only stores pointer to the data. The purpose is to allow - * the user to share vertices data between the physics engine and the rendering - * part. Therefore, make sure that the data pointed by a TriangleVertexArray - * remains valid during the TriangleVertexArray life. - */ -class TriangleVertexArray { - public: - /// Data type for the vertices in the array - enum VertexDataType {VERTEX_FLOAT_TYPE, VERTEX_DOUBLE_TYPE}; + /** + * This class is used to describe the vertices and faces of a triangular mesh. + * A TriangleVertexArray represents a continuous array of vertices and indexes + * of a triangular mesh. When you create a TriangleVertexArray, no data is copied + * int32_to the array. It only stores pointer to the data. The purpose is to allow + * the user to share vertices data between the physics engine and the rendering + * part. Therefore, make sure that the data pointed by a TriangleVertexArray + * remains valid during the TriangleVertexArray life. + */ + class TriangleVertexArray { + public: + /// Data type for the vertices in the array + enum VertexDataType {VERTEX_FLOAT_TYPE, VERTEX_DOUBLE_TYPE}; + /// Data type for the indices in the array + enum IndexDataType {INDEX_INTEGER_TYPE, INDEX_SHORT_TYPE}; + protected: + uint32_t m_numberVertices; //!< Number of vertices in the array + unsigned char* m_verticesStart; //!< Pointer to the first vertex value in the array + int32_t m_verticesStride; //!< Stride (number of bytes) between the beginning of two vertices values in the array + uint32_t m_numberTriangles; //!< Number of triangles in the array + unsigned char* m_indicesStart; //!< Pointer to the first vertex index of the array + int32_t m_indicesStride; //!< Stride (number of bytes) between the beginning of two indices in the array + VertexDataType m_vertexDataType; //!< Data type of the vertices in the array + IndexDataType m_indexDataType; //!< Data type of the indices in the array + public: + /// Constructor + TriangleVertexArray(uint32_t nbVertices, void* verticesStart, int32_t verticesStride, + uint32_t nbTriangles, void* indexesStart, int32_t indexesStride, + VertexDataType vertexDataType, IndexDataType indexDataType); + /// Destructor + virtual ~TriangleVertexArray(); + /// Return the vertex data type + VertexDataType getVertexDataType() const; + /// Return the index data type + IndexDataType getIndexDataType() const; + /// Return the number of vertices + uint32_t getNbVertices() const; + /// Return the number of triangles + uint32_t getNbTriangles() const; + /// Return the vertices stride (number of bytes) + int32_t getVerticesStride() const; + /// Return the indices stride (number of bytes) + int32_t getIndicesStride() const; + /// Return the pointer to the start of the vertices array + unsigned char* getVerticesStart() const; + /// Return the pointer to the start of the indices array + unsigned char* getIndicesStart() const; + }; - /// Data type for the indices in the array - enum IndexDataType {INDEX_INTEGER_TYPE, INDEX_SHORT_TYPE}; - - protected: - uint32_t m_numberVertices; //!< Number of vertices in the array - unsigned char* m_verticesStart; //!< Pointer to the first vertex value in the array - int32_t m_verticesStride; //!< Stride (number of bytes) between the beginning of two vertices values in the array - uint32_t m_numberTriangles; //!< Number of triangles in the array - unsigned char* m_indicesStart; //!< Pointer to the first vertex index of the array - int32_t m_indicesStride; //!< Stride (number of bytes) between the beginning of two indices in the array - VertexDataType m_vertexDataType; //!< Data type of the vertices in the array - IndexDataType m_indexDataType; //!< Data type of the indices in the array - public: - /// Constructor - TriangleVertexArray(uint32_t nbVertices, void* verticesStart, int32_t verticesStride, - uint32_t nbTriangles, void* indexesStart, int32_t indexesStride, - VertexDataType vertexDataType, IndexDataType indexDataType); - - /// Destructor - virtual ~TriangleVertexArray(); - - /// Return the vertex data type - VertexDataType getVertexDataType() const; - - /// Return the index data type - IndexDataType getIndexDataType() const; - - /// Return the number of vertices - uint32_t getNbVertices() const; - - /// Return the number of triangles - uint32_t getNbTriangles() const; - - /// Return the vertices stride (number of bytes) - int32_t getVerticesStride() const; - - /// Return the indices stride (number of bytes) - int32_t getIndicesStride() const; - - /// Return the pointer to the start of the vertices array - unsigned char* getVerticesStart() const; - - /// Return the pointer to the start of the indices array - unsigned char* getIndicesStart() const; -}; - -// Return the vertex data type -inline TriangleVertexArray::VertexDataType TriangleVertexArray::getVertexDataType() const { - return m_vertexDataType; -} - -// Return the index data type -inline TriangleVertexArray::IndexDataType TriangleVertexArray::getIndexDataType() const { - return m_indexDataType; -} - -// Return the number of vertices -inline uint32_t TriangleVertexArray::getNbVertices() const { - return m_numberVertices; -} - -// Return the number of triangles -inline uint32_t TriangleVertexArray::getNbTriangles() const { - return m_numberTriangles; -} - -// Return the vertices stride (number of bytes) -inline int32_t TriangleVertexArray::getVerticesStride() const { - return m_verticesStride; -} - -// Return the indices stride (number of bytes) -inline int32_t TriangleVertexArray::getIndicesStride() const { - return m_indicesStride; -} - -// Return the pointer to the start of the vertices array -inline unsigned char* TriangleVertexArray::getVerticesStart() const { - return m_verticesStart; -} - -// Return the pointer to the start of the indices array -inline unsigned char* TriangleVertexArray::getIndicesStart() const { - return m_indicesStart; -} } diff --git a/ephysics/collision/broadphase/BroadPhaseAlgorithm.cpp b/ephysics/collision/broadphase/BroadPhaseAlgorithm.cpp index eef9be7..e5c5513 100644 --- a/ephysics/collision/broadphase/BroadPhaseAlgorithm.cpp +++ b/ephysics/collision/broadphase/BroadPhaseAlgorithm.cpp @@ -254,7 +254,7 @@ float BroadPhaseRaycastCallback::raycastBroadPhaseShape(int32_t nodeId, const Ra return hitFraction; } -bool BroadPhaseAlgorithm::testOverlappingShapes(const _ProxyShape* shape1, +bool BroadPhaseAlgorithm::testOverlappingShapes(const ProxyShape* _shape1, const ProxyShape* _shape2) const { // Get the two AABBs of the collision shapes const AABB& aabb1 = m_dynamicAABBTree.getFatAABB(_shape1->m_broadPhaseID); diff --git a/ephysics/collision/broadphase/DynamicAABBTree.cpp b/ephysics/collision/broadphase/DynamicAABBTree.cpp index ac42c2a..7f5ba0a 100644 --- a/ephysics/collision/broadphase/DynamicAABBTree.cpp +++ b/ephysics/collision/broadphase/DynamicAABBTree.cpp @@ -4,7 +4,6 @@ * @license BSD 3 clauses (see license file) */ -// Libraries #include #include #include @@ -13,19 +12,15 @@ using namespace ephysics; -// Initialization of static variables const int32_t TreeNode::NULL_TREE_NODE = -1; -// Constructor DynamicAABBTree::DynamicAABBTree(float extraAABBGap) : m_extraAABBGap(extraAABBGap) { init(); } -// Destructor DynamicAABBTree::~DynamicAABBTree() { - // Free the allocated memory for the nodes free(m_nodes); } @@ -673,6 +668,60 @@ void DynamicAABBTree::raycast(const Ray& ray, DynamicAABBTreeRaycastCallback &ca } } +// Return true if the node is a leaf of the tree +bool TreeNode::isLeaf() const { + return (height == 0); +} + +// Return the fat AABB corresponding to a given node ID +const AABB& DynamicAABBTree::getFatAABB(int32_t nodeID) const { + assert(nodeID >= 0 && nodeID < m_numberAllocatedNodes); + return m_nodes[nodeID].aabb; +} + +// Return the pointer to the data array of a given leaf node of the tree +int32_t* DynamicAABBTree::getNodeDataInt(int32_t nodeID) const { + assert(nodeID >= 0 && nodeID < m_numberAllocatedNodes); + assert(m_nodes[nodeID].isLeaf()); + return m_nodes[nodeID].dataInt; +} + +// Return the pointer to the data pointer of a given leaf node of the tree +void* DynamicAABBTree::getNodeDataPointer(int32_t nodeID) const { + assert(nodeID >= 0 && nodeID < m_numberAllocatedNodes); + assert(m_nodes[nodeID].isLeaf()); + return m_nodes[nodeID].dataPointer; +} + +// Return the root AABB of the tree +AABB DynamicAABBTree::getRootAABB() const { + return getFatAABB(m_rootNodeID); +} + +// Add an object int32_to the tree. This method creates a new leaf node in the tree and +// returns the ID of the corresponding node. +int32_t DynamicAABBTree::addObject(const AABB& aabb, int32_t data1, int32_t data2) { + + int32_t nodeId = addObjectInternal(aabb); + + m_nodes[nodeId].dataInt[0] = data1; + m_nodes[nodeId].dataInt[1] = data2; + + return nodeId; +} + +// Add an object int32_to the tree. This method creates a new leaf node in the tree and +// returns the ID of the corresponding node. +int32_t DynamicAABBTree::addObject(const AABB& aabb, void* data) { + + int32_t nodeId = addObjectInternal(aabb); + + m_nodes[nodeId].dataPointer = data; + + return nodeId; +} + + #ifndef NDEBUG // Check if the tree structure is valid (for debugging purpose) diff --git a/ephysics/collision/broadphase/DynamicAABBTree.hpp b/ephysics/collision/broadphase/DynamicAABBTree.hpp index 5cc8949..1bf60bf 100644 --- a/ephysics/collision/broadphase/DynamicAABBTree.hpp +++ b/ephysics/collision/broadphase/DynamicAABBTree.hpp @@ -5,267 +5,135 @@ */ #pragma once -// Libraries #include #include #include -/// Namespace ReactPhysics3D namespace ephysics { - -// Declarations -class BroadPhaseAlgorithm; -class BroadPhaseRaycastTestCallback; -class DynamicAABBTreeOverlapCallback; -struct RaycastTest; - -// Structure TreeNode -/** - * This structure represents a node of the dynamic AABB tree. - */ -struct TreeNode { - - // -------------------- Constants -------------------- // - - /// Null tree node constant - const static int32_t NULL_TREE_NODE; - - // -------------------- Attributes -------------------- // - - // A node is either in the tree (has a parent) or in the free nodes list - // (has a next node) - union { - - /// Parent node ID - int32_t parentID; - - /// Next allocated node ID - int32_t nextNodeID; - }; - - // A node is either a leaf (has data) or is an int32_ternal node (has children) - union { - - /// Left and right child of the node (children[0] = left child) - int32_t children[2]; - - /// Two pieces of data stored at that node (in case the node is a leaf) + class BroadPhaseAlgorithm; + class BroadPhaseRaycastTestCallback; + class DynamicAABBTreeOverlapCallback; + struct RaycastTest; + /** + * @brief It represents a node of the dynamic AABB tree. + */ + struct TreeNode { + const static int32_t NULL_TREE_NODE; //!< Null tree node constant + /** + * @brief A node is either in the tree (has a parent) or in the free nodes list (has a next node) + */ union { - int32_t dataInt[2]; - void* dataPointer; + int32_t parentID; //!< Parent node ID + int32_t nextNodeID; //!< Next allocated node ID }; + /** + * @brief A node is either a leaf (has data) or is an int32_ternal node (has children) + */ + union { + int32_t children[2]; //!< Left and right child of the node (children[0] = left child) + //! Two pieces of data stored at that node (in case the node is a leaf) + union { + int32_t dataInt[2]; + void* dataPointer; + }; + }; + int16_t height; //!< Height of the node in the tree + AABB aabb; //!< Fat axis aligned bounding box (AABB) corresponding to the node + /// Return true if the node is a leaf of the tree + bool isLeaf() const; + }; + + /** + * @brief Overlapping callback method that has to be used as parameter of the + * reportAllShapesOverlappingWithNode() method. + */ + class DynamicAABBTreeOverlapCallback { + public : + virtual ~DynamicAABBTreeOverlapCallback() = default; + // Called when a overlapping node has been found during the call to + // DynamicAABBTree:reportAllShapesOverlappingWithAABB() + virtual void notifyOverlappingNode(int32_t nodeId)=0; + }; + + /** + * @brief Raycast callback in the Dynamic AABB Tree called when the AABB of a leaf + * node is hit by the ray. + */ + class DynamicAABBTreeRaycastCallback { + public: + virtual ~DynamicAABBTreeRaycastCallback() = default; + // Called when the AABB of a leaf node is hit by a ray + virtual float raycastBroadPhaseShape(int32_t nodeId, const Ray& ray)=0; + }; + + /** + * @brief It implements a dynamic AABB tree that is used for broad-phase + * collision detection. This data structure is inspired by Nathanael Presson's + * dynamic tree implementation in BulletPhysics. The following implementation is + * based on the one from Erin Catto in Box2D as described in the book + * "Introduction to Game Physics with Box2D" by Ian Parberry. + */ + class DynamicAABBTree { + private: + TreeNode* m_nodes; //!< Pointer to the memory location of the nodes of the tree + int32_t m_rootNodeID; //!< ID of the root node of the tree + int32_t m_freeNodeID; //!< ID of the first node of the list of free (allocated) nodes in the tree that we can use + int32_t m_numberAllocatedNodes; //!< Number of allocated nodes in the tree + int32_t m_numberNodes; //!< Number of nodes in the tree + float m_extraAABBGap; //!< Extra AABB Gap used to allow the collision shape to move a little bit without triggering a large modification of the tree which can be costly + /// Allocate and return a node to use in the tree + int32_t allocateNode(); + /// Release a node + void releaseNode(int32_t _nodeID); + /// Insert a leaf node in the tree + void insertLeafNode(int32_t _nodeID); + /// Remove a leaf node from the tree + void removeLeafNode(int32_t _nodeID); + /// Balance the sub-tree of a given node using left or right rotations. + int32_t balanceSubTreeAtNode(int32_t _nodeID); + /// Compute the height of a given node in the tree + int32_t computeHeight(int32_t _nodeID); + /// Internally add an object int32_to the tree + int32_t addObjectInternal(const AABB& _aabb); + /// Initialize the tree + void init(); + #ifndef NDEBUG + /// Check if the tree structure is valid (for debugging purpose) + void check() const; + /// Check if the node structure is valid (for debugging purpose) + void checkNode(int32_t _nodeID) const; + #endif + public: + /// Constructor + DynamicAABBTree(float _extraAABBGap = 0.0f); + /// Destructor + virtual ~DynamicAABBTree(); + /// Add an object int32_to the tree (where node data are two int32_tegers) + int32_t addObject(const AABB& _aabb, int32_t _data1, int32_t _data2); + /// Add an object int32_to the tree (where node data is a pointer) + int32_t addObject(const AABB& _aabb, void* _data); + /// Remove an object from the tree + void removeObject(int32_t _nodeID); + /// Update the dynamic tree after an object has moved. + bool updateObject(int32_t _nodeID, const AABB& _newAABB, const vec3& _displacement, bool _forceReinsert = false); + /// Return the fat AABB corresponding to a given node ID + const AABB& getFatAABB(int32_t _nodeID) const; + /// Return the pointer to the data array of a given leaf node of the tree + int32_t* getNodeDataInt(int32_t _nodeID) const; + /// Return the data pointer of a given leaf node of the tree + void* getNodeDataPointer(int32_t _nodeID) const; + /// Report all shapes overlapping with the AABB given in parameter. + void reportAllShapesOverlappingWithAABB(const AABB& _aabb, + DynamicAABBTreeOverlapCallback& _callback) const; + /// Ray casting method + void raycast(const Ray& _ray, DynamicAABBTreeRaycastCallback& _callback) const; + /// Compute the height of the tree + int32_t computeHeight(); + /// Return the root AABB of the tree + AABB getRootAABB() const; + /// Clear all the nodes and reset the tree + void reset(); }; - /// Height of the node in the tree - int16_t height; - - /// Fat axis aligned bounding box (AABB) corresponding to the node - AABB aabb; - - // -------------------- Methods -------------------- // - - /// Return true if the node is a leaf of the tree - bool isLeaf() const; -}; - -// Class DynamicAABBTreeOverlapCallback -/** - * Overlapping callback method that has to be used as parameter of the - * reportAllShapesOverlappingWithNode() method. - */ -class DynamicAABBTreeOverlapCallback { - - public : - virtual ~DynamicAABBTreeOverlapCallback() = default; - - // Called when a overlapping node has been found during the call to - // DynamicAABBTree:reportAllShapesOverlappingWithAABB() - virtual void notifyOverlappingNode(int32_t nodeId)=0; -}; - -// Class DynamicAABBTreeRaycastCallback -/** - * Raycast callback in the Dynamic AABB Tree called when the AABB of a leaf - * node is hit by the ray. - */ -class DynamicAABBTreeRaycastCallback { - - public: - virtual ~DynamicAABBTreeRaycastCallback() = default; - - // Called when the AABB of a leaf node is hit by a ray - virtual float raycastBroadPhaseShape(int32_t nodeId, const Ray& ray)=0; - -}; - -// Class DynamicAABBTree -/** - * This class implements a dynamic AABB tree that is used for broad-phase - * collision detection. This data structure is inspired by Nathanael Presson's - * dynamic tree implementation in BulletPhysics. The following implementation is - * based on the one from Erin Catto in Box2D as described in the book - * "Introduction to Game Physics with Box2D" by Ian Parberry. - */ -class DynamicAABBTree { - - private: - - // -------------------- Attributes -------------------- // - - /// Pointer to the memory location of the nodes of the tree - TreeNode* m_nodes; - - /// ID of the root node of the tree - int32_t m_rootNodeID; - - /// ID of the first node of the list of free (allocated) nodes in the tree that we can use - int32_t m_freeNodeID; - - /// Number of allocated nodes in the tree - int32_t m_numberAllocatedNodes; - - /// Number of nodes in the tree - int32_t m_numberNodes; - - /// Extra AABB Gap used to allow the collision shape to move a little bit - /// without triggering a large modification of the tree which can be costly - float m_extraAABBGap; - - // -------------------- Methods -------------------- // - - /// Allocate and return a node to use in the tree - int32_t allocateNode(); - - /// Release a node - void releaseNode(int32_t nodeID); - - /// Insert a leaf node in the tree - void insertLeafNode(int32_t nodeID); - - /// Remove a leaf node from the tree - void removeLeafNode(int32_t nodeID); - - /// Balance the sub-tree of a given node using left or right rotations. - int32_t balanceSubTreeAtNode(int32_t nodeID); - - /// Compute the height of a given node in the tree - int32_t computeHeight(int32_t nodeID); - - /// Internally add an object int32_to the tree - int32_t addObjectInternal(const AABB& aabb); - - /// Initialize the tree - void init(); - -#ifndef NDEBUG - - /// Check if the tree structure is valid (for debugging purpose) - void check() const; - - /// Check if the node structure is valid (for debugging purpose) - void checkNode(int32_t nodeID) const; - -#endif - - public: - - // -------------------- Methods -------------------- // - - /// Constructor - DynamicAABBTree(float extraAABBGap = 0.0f); - - /// Destructor - virtual ~DynamicAABBTree(); - - /// Add an object int32_to the tree (where node data are two int32_tegers) - int32_t addObject(const AABB& aabb, int32_t data1, int32_t data2); - - /// Add an object int32_to the tree (where node data is a pointer) - int32_t addObject(const AABB& aabb, void* data); - - /// Remove an object from the tree - void removeObject(int32_t nodeID); - - /// Update the dynamic tree after an object has moved. - bool updateObject(int32_t nodeID, const AABB& newAABB, const vec3& displacement, bool forceReinsert = false); - - /// Return the fat AABB corresponding to a given node ID - const AABB& getFatAABB(int32_t nodeID) const; - - /// Return the pointer to the data array of a given leaf node of the tree - int32_t* getNodeDataInt(int32_t nodeID) const; - - /// Return the data pointer of a given leaf node of the tree - void* getNodeDataPointer(int32_t nodeID) const; - - /// Report all shapes overlapping with the AABB given in parameter. - void reportAllShapesOverlappingWithAABB(const AABB& aabb, - DynamicAABBTreeOverlapCallback& callback) const; - - /// Ray casting method - void raycast(const Ray& ray, DynamicAABBTreeRaycastCallback& callback) const; - - /// Compute the height of the tree - int32_t computeHeight(); - - /// Return the root AABB of the tree - AABB getRootAABB() const; - - /// Clear all the nodes and reset the tree - void reset(); -}; - -// Return true if the node is a leaf of the tree -bool TreeNode::isLeaf() const { - return (height == 0); -} - -// Return the fat AABB corresponding to a given node ID -const AABB& DynamicAABBTree::getFatAABB(int32_t nodeID) const { - assert(nodeID >= 0 && nodeID < m_numberAllocatedNodes); - return m_nodes[nodeID].aabb; -} - -// Return the pointer to the data array of a given leaf node of the tree -int32_t* DynamicAABBTree::getNodeDataInt(int32_t nodeID) const { - assert(nodeID >= 0 && nodeID < m_numberAllocatedNodes); - assert(m_nodes[nodeID].isLeaf()); - return m_nodes[nodeID].dataInt; -} - -// Return the pointer to the data pointer of a given leaf node of the tree -void* DynamicAABBTree::getNodeDataPointer(int32_t nodeID) const { - assert(nodeID >= 0 && nodeID < m_numberAllocatedNodes); - assert(m_nodes[nodeID].isLeaf()); - return m_nodes[nodeID].dataPointer; -} - -// Return the root AABB of the tree -AABB DynamicAABBTree::getRootAABB() const { - return getFatAABB(m_rootNodeID); -} - -// Add an object int32_to the tree. This method creates a new leaf node in the tree and -// returns the ID of the corresponding node. -int32_t DynamicAABBTree::addObject(const AABB& aabb, int32_t data1, int32_t data2) { - - int32_t nodeId = addObjectInternal(aabb); - - m_nodes[nodeId].dataInt[0] = data1; - m_nodes[nodeId].dataInt[1] = data2; - - return nodeId; -} - -// Add an object int32_to the tree. This method creates a new leaf node in the tree and -// returns the ID of the corresponding node. -int32_t DynamicAABBTree::addObject(const AABB& aabb, void* data) { - - int32_t nodeId = addObjectInternal(aabb); - - m_nodes[nodeId].dataPointer = data; - - return nodeId; -} } diff --git a/ephysics/collision/narrowphase/ConcaveVsConvexAlgorithm.hpp b/ephysics/collision/narrowphase/ConcaveVsConvexAlgorithm.hpp index 1bdd7ec..d1c1cb4 100644 --- a/ephysics/collision/narrowphase/ConcaveVsConvexAlgorithm.hpp +++ b/ephysics/collision/narrowphase/ConcaveVsConvexAlgorithm.hpp @@ -122,9 +122,8 @@ namespace ephysics { std::vector _contactPoints, NarrowPhaseCallback* _narrowPhaseCallback); /// Add a triangle vertex int32_to the set of processed triangles - void addProcessedVertex(std::unordered_multimap& _processTriangleVertices, - const vec3& _vertex) { - processTriangleVertices.insert(std::make_pair(int32_t(vertex.x() * vertex.y() * vertex.z()), vertex)); + void addProcessedVertex(std::unordered_multimap& _processTriangleVertices, const vec3& _vertex) { + _processTriangleVertices.insert(std::make_pair(int32_t(_vertex.x() * _vertex.y() * _vertex.z()), _vertex)); } /// Return true if the vertex is in the set of already processed vertices bool hasVertexBeenProcessed(const std::unordered_multimap& _processTriangleVertices, diff --git a/ephysics/collision/narrowphase/DefaultCollisionDispatch.hpp b/ephysics/collision/narrowphase/DefaultCollisionDispatch.hpp index dbb6a97..e1022aa 100644 --- a/ephysics/collision/narrowphase/DefaultCollisionDispatch.hpp +++ b/ephysics/collision/narrowphase/DefaultCollisionDispatch.hpp @@ -5,48 +5,32 @@ */ #pragma once -// Libraries #include #include #include #include namespace ephysics { - -// Class DefaultCollisionDispatch /** - * This is the default collision dispatch configuration use in ReactPhysics3D. + * @brief This is the default collision dispatch configuration use in ephysics. * Collision dispatching decides which collision * algorithm to use given two types of proxy collision shapes. */ class DefaultCollisionDispatch : public CollisionDispatch { - protected: - - /// Sphere vs Sphere collision algorithm - SphereVsSphereAlgorithm mSphereVsSphereAlgorithm; - - /// Concave vs Convex collision algorithm - ConcaveVsConvexAlgorithm mConcaveVsConvexAlgorithm; - - /// GJK Algorithm - GJKAlgorithm mGJKAlgorithm; - + SphereVsSphereAlgorithm mSphereVsSphereAlgorithm; //!< Sphere vs Sphere collision algorithm + ConcaveVsConvexAlgorithm mConcaveVsConvexAlgorithm; //!< Concave vs Convex collision algorithm + GJKAlgorithm mGJKAlgorithm; //!< GJK Algorithm public: - /// Constructor DefaultCollisionDispatch(); - /// Destructor virtual ~DefaultCollisionDispatch(); - /// Initialize the collision dispatch configuration - virtual void init(CollisionDetection* collisionDetection, - MemoryAllocator* memoryAllocator); - + virtual void init(CollisionDetection* _collisionDetection, MemoryAllocator* _memoryAllocator); /// Select and return the narrow-phase collision detection algorithm to /// use between two types of collision shapes. - virtual NarrowPhaseAlgorithm* selectAlgorithm(int32_t type1, int32_t type2); + virtual NarrowPhaseAlgorithm* selectAlgorithm(int32_t _type1, int32_t _type2); }; } diff --git a/ephysics/collision/narrowphase/GJK/GJKAlgorithm.cpp b/ephysics/collision/narrowphase/GJK/GJKAlgorithm.cpp index 0d0614d..3b3200d 100644 --- a/ephysics/collision/narrowphase/GJK/GJKAlgorithm.cpp +++ b/ephysics/collision/narrowphase/GJK/GJKAlgorithm.cpp @@ -82,7 +82,7 @@ void GJKAlgorithm::testCollision(const CollisionShapeInfo& shape1Info, Simplex simplex; // Get the previous point V (last cached separating axis) - vec3 v = mCurrentOverlappingPair->getCachedSeparatingAxis(); + vec3 v = m_currentOverlappingPair->getCachedSeparatingAxis(); // Initialize the upper bound for the square distance float distSquare = DECIMAL_LARGEST; @@ -103,7 +103,7 @@ void GJKAlgorithm::testCollision(const CollisionShapeInfo& shape1Info, if (vDotw > 0.0 && vDotw * vDotw > distSquare * marginSquare) { // Cache the current separating axis for frame coherence - mCurrentOverlappingPair->setCachedSeparatingAxis(v); + m_currentOverlappingPair->setCachedSeparatingAxis(v); // No int32_tersection, we return return; diff --git a/ephysics/collision/narrowphase/NarrowPhaseAlgorithm.cpp b/ephysics/collision/narrowphase/NarrowPhaseAlgorithm.cpp index 4a47592..7805eb6 100644 --- a/ephysics/collision/narrowphase/NarrowPhaseAlgorithm.cpp +++ b/ephysics/collision/narrowphase/NarrowPhaseAlgorithm.cpp @@ -4,25 +4,24 @@ * @license BSD 3 clauses (see license file) */ -// Libraries #include -// We want to use the ReactPhysics3D namespace using namespace ephysics; -// Constructor NarrowPhaseAlgorithm::NarrowPhaseAlgorithm() - : m_memoryAllocator(NULL), mCurrentOverlappingPair(NULL) { + : m_memoryAllocator(NULL), m_currentOverlappingPair(NULL) { } -// Destructor NarrowPhaseAlgorithm::~NarrowPhaseAlgorithm() { } -// Initalize the algorithm void NarrowPhaseAlgorithm::init(CollisionDetection* collisionDetection, MemoryAllocator* memoryAllocator) { m_collisionDetection = collisionDetection; m_memoryAllocator = memoryAllocator; } + +void NarrowPhaseAlgorithm::setCurrentOverlappingPair(OverlappingPair* _overlappingPair) { + m_currentOverlappingPair = _overlappingPair; +} diff --git a/ephysics/collision/narrowphase/NarrowPhaseAlgorithm.hpp b/ephysics/collision/narrowphase/NarrowPhaseAlgorithm.hpp index 4eec984..86acf0f 100644 --- a/ephysics/collision/narrowphase/NarrowPhaseAlgorithm.hpp +++ b/ephysics/collision/narrowphase/NarrowPhaseAlgorithm.hpp @@ -5,90 +5,58 @@ */ #pragma once -// Libraries #include #include #include #include #include -/// Namespace ReactPhysics3D namespace ephysics { -class CollisionDetection; - -// Class NarrowPhaseCallback -/** - * This abstract class is the base class for a narrow-phase collision - * callback class. - */ -class NarrowPhaseCallback { - - public: - virtual ~NarrowPhaseCallback() = default; - - /// Called by a narrow-phase collision algorithm when a new contact has been found - virtual void notifyContact(OverlappingPair* overlappingPair, - const ContactPointInfo& contactInfo)=0; - -}; - -// Class NarrowPhaseAlgorithm -/** - * This abstract class is the base class for a narrow-phase collision - * detection algorithm. The goal of the narrow phase algorithm is to - * compute information about the contact between two proxy shapes. - */ -class NarrowPhaseAlgorithm { - - protected : - - // -------------------- Attributes -------------------- // - - /// Pointer to the collision detection object - CollisionDetection* m_collisionDetection; - - /// Pointer to the memory allocator - MemoryAllocator* m_memoryAllocator; - - /// Overlapping pair of the bodies currently tested for collision - OverlappingPair* mCurrentOverlappingPair; - - // -------------------- Methods -------------------- // - - /// Private copy-constructor - NarrowPhaseAlgorithm(const NarrowPhaseAlgorithm& algorithm); - - /// Private assignment operator - NarrowPhaseAlgorithm& operator=(const NarrowPhaseAlgorithm& algorithm); - - public : - - // -------------------- Methods -------------------- // - - /// Constructor - NarrowPhaseAlgorithm(); - - /// Destructor - virtual ~NarrowPhaseAlgorithm(); - - /// Initalize the algorithm - virtual void init(CollisionDetection* collisionDetection, MemoryAllocator* memoryAllocator); - - /// Set the current overlapping pair of bodies - void setCurrentOverlappingPair(OverlappingPair* overlappingPair); - - /// Compute a contact info if the two bounding volume collide - virtual void testCollision(const CollisionShapeInfo& shape1Info, - const CollisionShapeInfo& shape2Info, - NarrowPhaseCallback* narrowPhaseCallback)=0; -}; - -// Set the current overlapping pair of bodies -void NarrowPhaseAlgorithm::setCurrentOverlappingPair(OverlappingPair* overlappingPair) { - mCurrentOverlappingPair = overlappingPair; -} - + class CollisionDetection; + + /** + * @brief It is the base class for a narrow-phase collision + * callback class. + */ + class NarrowPhaseCallback { + public: + virtual ~NarrowPhaseCallback() = default; + /// Called by a narrow-phase collision algorithm when a new contact has been found + virtual void notifyContact(OverlappingPair* _overlappingPair, + const ContactPointInfo& _contactInfo) = 0; + + }; + + /** + * @breif It is the base class for a narrow-phase collision + * detection algorithm. The goal of the narrow phase algorithm is to + * compute information about the contact between two proxy shapes. + */ + class NarrowPhaseAlgorithm { + protected : + CollisionDetection* m_collisionDetection; //!< Pointer to the collision detection object + MemoryAllocator* m_memoryAllocator; //!< Pointer to the memory allocator + OverlappingPair* m_currentOverlappingPair; //!< Overlapping pair of the bodies currently tested for collision + /// Private copy-constructor + NarrowPhaseAlgorithm(const NarrowPhaseAlgorithm& algorithm) = delete; + /// Private assignment operator + NarrowPhaseAlgorithm& operator=(const NarrowPhaseAlgorithm& algorithm) = delete; + public : + /// Constructor + NarrowPhaseAlgorithm(); + /// Destructor + virtual ~NarrowPhaseAlgorithm(); + /// Initalize the algorithm + virtual void init(CollisionDetection* _collisionDetection, + MemoryAllocator* _memoryAllocator); + /// Set the current overlapping pair of bodies + void setCurrentOverlappingPair(OverlappingPair* _overlappingPair); + /// Compute a contact info if the two bounding volume collide + virtual void testCollision(const CollisionShapeInfo& _shape1Info, + const CollisionShapeInfo& _shape2Info, + NarrowPhaseCallback* _narrowPhaseCallback) = 0; + }; } diff --git a/ephysics/collision/narrowphase/SphereVsSphereAlgorithm.cpp b/ephysics/collision/narrowphase/SphereVsSphereAlgorithm.cpp index e95ed16..639b9e2 100644 --- a/ephysics/collision/narrowphase/SphereVsSphereAlgorithm.cpp +++ b/ephysics/collision/narrowphase/SphereVsSphereAlgorithm.cpp @@ -25,7 +25,7 @@ void ephysics::SphereVsSphereAlgorithm::testCollision(const ephysics::CollisionS vec3 vectorBetweenCenters = transform2.getPosition() - transform1.getPosition(); float squaredDistanceBetweenCenters = vectorBetweenCenters.length2(); // Compute the sum of the radius - float sumRadius = _sphereShape1->getRadius() + sphereShape2->getRadius(); + float sumRadius = sphereShape1->getRadius() + sphereShape2->getRadius(); // If the sphere collision shapes intersect if (squaredDistanceBetweenCenters <= sumRadius * sumRadius) { vec3 centerSphere2InBody1LocalSpace = transform1.getInverse() * transform2.getPosition(); @@ -44,6 +44,6 @@ void ephysics::SphereVsSphereAlgorithm::testCollision(const ephysics::CollisionS intersectionOnBody1, intersectionOnBody2); // Notify about the new contact - _narrowPhaseCallback->notifyContact(shape1Info.overlappingPair, contactInfo); + _narrowPhaseCallback->notifyContact(_shape1Info.overlappingPair, contactInfo); } } diff --git a/ephysics/collision/narrowphase/SphereVsSphereAlgorithm.hpp b/ephysics/collision/narrowphase/SphereVsSphereAlgorithm.hpp index d4c7963..9b3a049 100644 --- a/ephysics/collision/narrowphase/SphereVsSphereAlgorithm.hpp +++ b/ephysics/collision/narrowphase/SphereVsSphereAlgorithm.hpp @@ -10,31 +10,30 @@ #include namespace ephysics { -/** - * @brief It is used to compute the narrow-phase collision detection - * between two sphere collision shapes. - */ -class SphereVsSphereAlgorithm : public NarrowPhaseAlgorithm { - protected : - SphereVsSphereAlgorithm(const SphereVsSphereAlgorithm& algorithm) = delete; - SphereVsSphereAlgorithm& operator=(const SphereVsSphereAlgorithm& algorithm) = delete; - public : - /** - * @brief Constructor - */ - SphereVsSphereAlgorithm(); - /** - * @brief Destructor - */ - virtual ~SphereVsSphereAlgorithm() = default; - /** - * @brief Compute a contact info if the two bounding volume collide - */ - virtual void testCollision(const CollisionShapeInfo& _shape1Info, - const CollisionShapeInfo& _shape2Info, - NarrowPhaseCallback* _narrowPhaseCallback); -}; - + /** + * @brief It is used to compute the narrow-phase collision detection + * between two sphere collision shapes. + */ + class SphereVsSphereAlgorithm : public NarrowPhaseAlgorithm { + protected : + SphereVsSphereAlgorithm(const SphereVsSphereAlgorithm&) = delete; + SphereVsSphereAlgorithm& operator=(const SphereVsSphereAlgorithm&) = delete; + public : + /** + * @brief Constructor + */ + SphereVsSphereAlgorithm(); + /** + * @brief Destructor + */ + virtual ~SphereVsSphereAlgorithm() = default; + /** + * @brief Compute a contact info if the two bounding volume collide + */ + virtual void testCollision(const CollisionShapeInfo& _shape1Info, + const CollisionShapeInfo& _shape2Info, + NarrowPhaseCallback* _narrowPhaseCallback); + }; } diff --git a/ephysics/collision/shapes/AABB.cpp b/ephysics/collision/shapes/AABB.cpp index 5466f11..ffa4eeb 100644 --- a/ephysics/collision/shapes/AABB.cpp +++ b/ephysics/collision/shapes/AABB.cpp @@ -190,7 +190,7 @@ bool AABB::testCollisionTriangleAABB(const vec3* _trianglePoints) const { bool AABB::contains(const vec3& _point) const { return _point.x() >= m_minCoordinates.x() - MACHINE_EPSILON && _point.x() <= m_maxCoordinates.x() + MACHINE_EPSILON && _point.y() >= m_minCoordinates.y() - MACHINE_EPSILON && _point.y() <= m_maxCoordinates.y() + MACHINE_EPSILON - && _point.z() >= m_minCoordinates.z() - MACHINE_EPSILON && _point.z() <= m_maxCoordinates.z() + MACHINE_EPSILON); + && _point.z() >= m_minCoordinates.z() - MACHINE_EPSILON && _point.z() <= m_maxCoordinates.z() + MACHINE_EPSILON; } AABB& AABB::operator=(const AABB& _aabb) { diff --git a/ephysics/collision/shapes/BoxShape.cpp b/ephysics/collision/shapes/BoxShape.cpp index 4c409d0..c8973d7 100644 --- a/ephysics/collision/shapes/BoxShape.cpp +++ b/ephysics/collision/shapes/BoxShape.cpp @@ -107,3 +107,59 @@ bool BoxShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* pro return true; } + + + + +// Return the extents of the box +/** + * @return The vector with the three extents of the box shape (in meters) + */ +vec3 BoxShape::getExtent() const { + return m_extent + vec3(m_margin, m_margin, m_margin); +} + +// Set the scaling vector of the collision shape +void BoxShape::setLocalScaling(const vec3& scaling) { + + m_extent = (m_extent / m_scaling) * scaling; + + CollisionShape::setLocalScaling(scaling); +} + +// Return the local bounds of the shape in x, y and z directions +/// This method is used to compute the AABB of the box +/** + * @param min The minimum bounds of the shape in local-space coordinates + * @param max The maximum bounds of the shape in local-space coordinates + */ +void BoxShape::getLocalBounds(vec3& _min, vec3& _max) const { + + // Maximum bounds + _max = m_extent + vec3(m_margin, m_margin, m_margin); + + // Minimum bounds + _min = -_max; +} + +// Return the number of bytes used by the collision shape +size_t BoxShape::getSizeInBytes() const { + return sizeof(BoxShape); +} + +// Return a local support point in a given direction without the objec margin +vec3 BoxShape::getLocalSupportPointWithoutMargin(const vec3& direction, + void** cachedCollisionData) const { + + return vec3(direction.x() < 0.0 ? -m_extent.x() : m_extent.x(), + direction.y() < 0.0 ? -m_extent.y() : m_extent.y(), + direction.z() < 0.0 ? -m_extent.z() : m_extent.z()); +} + +// Return true if a point is inside the collision shape +bool BoxShape::testPointInside(const vec3& localPoint, ProxyShape* proxyShape) const { + return (localPoint.x() < m_extent[0] && localPoint.x() > -m_extent[0] && + localPoint.y() < m_extent[1] && localPoint.y() > -m_extent[1] && + localPoint.z() < m_extent[2] && localPoint.z() > -m_extent[2]); +} + diff --git a/ephysics/collision/shapes/BoxShape.hpp b/ephysics/collision/shapes/BoxShape.hpp index b79c02c..44970f3 100644 --- a/ephysics/collision/shapes/BoxShape.hpp +++ b/ephysics/collision/shapes/BoxShape.hpp @@ -56,56 +56,4 @@ class BoxShape : public ConvexShape { virtual void computeLocalInertiaTensor(etk::Matrix3x3& tensor, float mass) const; }; -// Return the extents of the box -/** - * @return The vector with the three extents of the box shape (in meters) - */ -vec3 BoxShape::getExtent() const { - return m_extent + vec3(m_margin, m_margin, m_margin); -} - -// Set the scaling vector of the collision shape -void BoxShape::setLocalScaling(const vec3& scaling) { - - m_extent = (m_extent / m_scaling) * scaling; - - CollisionShape::setLocalScaling(scaling); -} - -// Return the local bounds of the shape in x, y and z directions -/// This method is used to compute the AABB of the box -/** - * @param min The minimum bounds of the shape in local-space coordinates - * @param max The maximum bounds of the shape in local-space coordinates - */ -void BoxShape::getLocalBounds(vec3& _min, vec3& _max) const { - - // Maximum bounds - _max = m_extent + vec3(m_margin, m_margin, m_margin); - - // Minimum bounds - _min = -_max; -} - -// Return the number of bytes used by the collision shape -size_t BoxShape::getSizeInBytes() const { - return sizeof(BoxShape); -} - -// Return a local support point in a given direction without the objec margin -vec3 BoxShape::getLocalSupportPointWithoutMargin(const vec3& direction, - void** cachedCollisionData) const { - - return vec3(direction.x() < 0.0 ? -m_extent.x() : m_extent.x(), - direction.y() < 0.0 ? -m_extent.y() : m_extent.y(), - direction.z() < 0.0 ? -m_extent.z() : m_extent.z()); -} - -// Return true if a point is inside the collision shape -bool BoxShape::testPointInside(const vec3& localPoint, ProxyShape* proxyShape) const { - return (localPoint.x() < m_extent[0] && localPoint.x() > -m_extent[0] && - localPoint.y() < m_extent[1] && localPoint.y() > -m_extent[1] && - localPoint.z() < m_extent[2] && localPoint.z() > -m_extent[2]); -} - } diff --git a/ephysics/collision/shapes/CapsuleShape.cpp b/ephysics/collision/shapes/CapsuleShape.cpp index fedfa7c..7f462f6 100644 --- a/ephysics/collision/shapes/CapsuleShape.cpp +++ b/ephysics/collision/shapes/CapsuleShape.cpp @@ -261,3 +261,79 @@ bool CapsuleShape::raycastWithSphereEndCap(const vec3& point1, const vec3& point return false; } + + + +// Get the radius of the capsule +/** + * @return The radius of the capsule shape (in meters) + */ +float CapsuleShape::getRadius() const { + return m_margin; +} + +// Return the height of the capsule +/** + * @return The height of the capsule shape (in meters) + */ +float CapsuleShape::getHeight() const { + return m_halfHeight + m_halfHeight; +} + +// Set the scaling vector of the collision shape +void CapsuleShape::setLocalScaling(const vec3& scaling) { + + m_halfHeight = (m_halfHeight / m_scaling.y()) * scaling.y(); + m_margin = (m_margin / m_scaling.x()) * scaling.x(); + + CollisionShape::setLocalScaling(scaling); +} + +// Return the number of bytes used by the collision shape +size_t CapsuleShape::getSizeInBytes() const { + return sizeof(CapsuleShape); +} + +// Return the local bounds of the shape in x, y and z directions +// This method is used to compute the AABB of the box +/** + * @param min The minimum bounds of the shape in local-space coordinates + * @param max The maximum bounds of the shape in local-space coordinates + */ +void CapsuleShape::getLocalBounds(vec3& min, vec3& max) const { + + // Maximum bounds + max.setX(m_margin); + max.setY(m_halfHeight + m_margin); + max.setZ(m_margin); + + // Minimum bounds + min.setX(-m_margin); + min.setY(-max.y()); + min.setZ(min.x()); +} + +// Return a local support point in a given direction without the object margin. +/// A capsule is the convex hull of two spheres S1 and S2. The support point in the direction "d" +/// of the convex hull of a set of convex objects is the support point "p" in the set of all +/// support points from all the convex objects with the maximum dot product with the direction "d". +/// Therefore, in this method, we compute the support points of both top and bottom spheres of +/// the capsule and return the point with the maximum dot product with the direction vector. Note +/// that the object margin is implicitly the radius and height of the capsule. +vec3 CapsuleShape::getLocalSupportPointWithoutMargin(const vec3& direction, + void** cachedCollisionData) const { + + // Support point top sphere + float dotProductTop = m_halfHeight * direction.y(); + + // Support point bottom sphere + float dotProductBottom = -m_halfHeight * direction.y(); + + // Return the point with the maximum dot product + if (dotProductTop > dotProductBottom) { + return vec3(0, m_halfHeight, 0); + } + else { + return vec3(0, -m_halfHeight, 0); + } +} \ No newline at end of file diff --git a/ephysics/collision/shapes/CapsuleShape.hpp b/ephysics/collision/shapes/CapsuleShape.hpp index de8a31c..893b18c 100644 --- a/ephysics/collision/shapes/CapsuleShape.hpp +++ b/ephysics/collision/shapes/CapsuleShape.hpp @@ -10,7 +10,6 @@ #include namespace ephysics { - /** * @brief It represents a capsule collision shape that is defined around the Y axis. * A capsule shape can be seen as the convex hull of two spheres. @@ -25,123 +24,35 @@ namespace ephysics { float m_halfHeight; //!< Half height of the capsule (height = distance between the centers of the two spheres) /// Private copy-constructor CapsuleShape(const CapsuleShape& shape); - /// Private assignment operator CapsuleShape& operator=(const CapsuleShape& shape); - /// Return a local support point in a given direction without the object margin virtual vec3 getLocalSupportPointWithoutMargin(const vec3& direction, void** cachedCollisionData) const; - /// Return true if a point is inside the collision shape virtual bool testPointInside(const vec3& localPoint, ProxyShape* proxyShape) const; - /// Raycast method with feedback information virtual bool raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape) const; - /// Raycasting method between a ray one of the two spheres end cap of the capsule bool raycastWithSphereEndCap(const vec3& point1, const vec3& point2, const vec3& sphereCenter, float maxFraction, vec3& hitLocalPoint, float& hitFraction) const; - /// Return the number of bytes used by the collision shape virtual size_t getSizeInBytes() const; - public : /// Constructor CapsuleShape(float _radius, float _height); - /// Destructor virtual ~CapsuleShape(); - /// Return the radius of the capsule float getRadius() const; - /// Return the height of the capsule float getHeight() const; - /// Set the scaling vector of the collision shape virtual void setLocalScaling(const vec3& scaling); - /// Return the local bounds of the shape in x, y and z directions virtual void getLocalBounds(vec3& min, vec3& max) const; - /// Return the local inertia tensor of the collision shape virtual void computeLocalInertiaTensor(etk::Matrix3x3& tensor, float mass) const; }; - -// Get the radius of the capsule -/** - * @return The radius of the capsule shape (in meters) - */ -float CapsuleShape::getRadius() const { - return m_margin; -} - -// Return the height of the capsule -/** - * @return The height of the capsule shape (in meters) - */ -float CapsuleShape::getHeight() const { - return m_halfHeight + m_halfHeight; -} - -// Set the scaling vector of the collision shape -void CapsuleShape::setLocalScaling(const vec3& scaling) { - - m_halfHeight = (m_halfHeight / m_scaling.y()) * scaling.y(); - m_margin = (m_margin / m_scaling.x()) * scaling.x(); - - CollisionShape::setLocalScaling(scaling); -} - -// Return the number of bytes used by the collision shape -size_t CapsuleShape::getSizeInBytes() const { - return sizeof(CapsuleShape); -} - -// Return the local bounds of the shape in x, y and z directions -// This method is used to compute the AABB of the box -/** - * @param min The minimum bounds of the shape in local-space coordinates - * @param max The maximum bounds of the shape in local-space coordinates - */ -void CapsuleShape::getLocalBounds(vec3& min, vec3& max) const { - - // Maximum bounds - max.setX(m_margin); - max.setY(m_halfHeight + m_margin); - max.setZ(m_margin); - - // Minimum bounds - min.setX(-m_margin); - min.setY(-max.y()); - min.setZ(min.x()); -} - -// Return a local support point in a given direction without the object margin. -/// A capsule is the convex hull of two spheres S1 and S2. The support point in the direction "d" -/// of the convex hull of a set of convex objects is the support point "p" in the set of all -/// support points from all the convex objects with the maximum dot product with the direction "d". -/// Therefore, in this method, we compute the support points of both top and bottom spheres of -/// the capsule and return the point with the maximum dot product with the direction vector. Note -/// that the object margin is implicitly the radius and height of the capsule. -vec3 CapsuleShape::getLocalSupportPointWithoutMargin(const vec3& direction, - void** cachedCollisionData) const { - - // Support point top sphere - float dotProductTop = m_halfHeight * direction.y(); - - // Support point bottom sphere - float dotProductBottom = -m_halfHeight * direction.y(); - - // Return the point with the maximum dot product - if (dotProductTop > dotProductBottom) { - return vec3(0, m_halfHeight, 0); - } - else { - return vec3(0, -m_halfHeight, 0); - } -} - } diff --git a/ephysics/collision/shapes/CollisionShape.cpp b/ephysics/collision/shapes/CollisionShape.cpp index 527fbdb..ed3f82c 100644 --- a/ephysics/collision/shapes/CollisionShape.cpp +++ b/ephysics/collision/shapes/CollisionShape.cpp @@ -54,3 +54,14 @@ void CollisionShape::computeAABB(AABB& aabb, const etk::Transform3D& transform) aabb.setMin(minCoordinates); aabb.setMax(maxCoordinates); } + +int32_t CollisionShape::computeNbMaxContactManifolds(CollisionShapeType shapeType1, + CollisionShapeType shapeType2) { + // If both shapes are convex + if (isConvex(shapeType1) && isConvex(shapeType2)) { + return NB_MAX_CONTACT_MANIFOLDS_CONVEX_SHAPE; + } // If there is at least one concave shape + else { + return NB_MAX_CONTACT_MANIFOLDS_CONCAVE_SHAPE; + } +} \ No newline at end of file diff --git a/ephysics/collision/shapes/CollisionShape.hpp b/ephysics/collision/shapes/CollisionShape.hpp index 803d4e7..182f469 100644 --- a/ephysics/collision/shapes/CollisionShape.hpp +++ b/ephysics/collision/shapes/CollisionShape.hpp @@ -42,67 +42,57 @@ class CollisionShape { virtual size_t getSizeInBytes() const = 0; public : /// Constructor - CollisionShape(CollisionShapeType type); + CollisionShape(CollisionShapeType _type); /// Destructor virtual ~CollisionShape(); - /// Return the type of the collision shapes - CollisionShapeType getType() const; - /// Return true if the collision shape is convex, false if it is concave - virtual bool isConvex() const=0; + /** + * @brief Get the type of the collision shapes + * @return The type of the collision shape (box, sphere, cylinder, ...) + */ + CollisionShapeType getType() const { + return m_type; + } + /** + * @brief Check if the shape is convex + * @return true If the collision shape is convex + * @return false If it is concave + */ + virtual bool isConvex() const = 0; /// Return the local bounds of the shape in x, y and z directions - virtual void getLocalBounds(vec3& min, vec3& max) const=0; + virtual void getLocalBounds(vec3& _min, vec3& _max) const=0; /// Return the scaling vector of the collision shape - vec3 getScaling() const; + vec3 getScaling() const { + return m_scaling; + } /// Set the local scaling vector of the collision shape - virtual void setLocalScaling(const vec3& scaling); + virtual void setLocalScaling(const vec3& _scaling) { + m_scaling = _scaling; + } /// Return the local inertia tensor of the collision shapes - virtual void computeLocalInertiaTensor(etk::Matrix3x3& tensor, float mass) const=0; + virtual void computeLocalInertiaTensor(etk::Matrix3x3& _tensor, float _mass) const=0; /// Compute the world-space AABB of the collision shape given a transform - virtual void computeAABB(AABB& aabb, const etk::Transform3D& transform) const; - /// Return true if the collision shape type is a convex shape - static bool isConvex(CollisionShapeType shapeType); - /// Return the maximum number of contact manifolds in an overlapping pair given two shape types - static int32_t computeNbMaxContactManifolds(CollisionShapeType shapeType1, - CollisionShapeType shapeType2); + virtual void computeAABB(AABB& _aabb, const etk::Transform3D& _transform) const; + /** + * @brief Check if the shape is convex + * @param[in] _shapeType shape type + * @return true If the collision shape is convex + * @return false If it is concave + */ + static bool isConvex(CollisionShapeType _shapeType) { + return _shapeType != CONCAVE_MESH + && _shapeType != HEIGHTFIELD; + } + /** + * @brief Get the maximum number of contact + * @return The maximum number of contact manifolds in an overlapping pair given two shape types + */ + static int32_t computeNbMaxContactManifolds(CollisionShapeType _shapeType1, + CollisionShapeType _shapeType2); friend class ProxyShape; friend class CollisionWorld; }; -// Return the type of the collision shape -/** - * @return The type of the collision shape (box, sphere, cylinder, ...) - */ -CollisionShapeType CollisionShape::getType() const { - return m_type; -} -// Return true if the collision shape type is a convex shape -bool CollisionShape::isConvex(CollisionShapeType shapeType) { - return shapeType != CONCAVE_MESH && shapeType != HEIGHTFIELD; -} - -// Return the scaling vector of the collision shape -vec3 CollisionShape::getScaling() const { - return m_scaling; -} - -// Set the scaling vector of the collision shape -void CollisionShape::setLocalScaling(const vec3& scaling) { - m_scaling = scaling; -} - -// Return the maximum number of contact manifolds allowed in an overlapping -// pair wit the given two collision shape types -int32_t CollisionShape::computeNbMaxContactManifolds(CollisionShapeType shapeType1, - CollisionShapeType shapeType2) { - // If both shapes are convex - if (isConvex(shapeType1) && isConvex(shapeType2)) { - return NB_MAX_CONTACT_MANIFOLDS_CONVEX_SHAPE; - } // If there is at least one concave shape - else { - return NB_MAX_CONTACT_MANIFOLDS_CONCAVE_SHAPE; - } -} } diff --git a/ephysics/collision/shapes/ConcaveMeshShape.cpp b/ephysics/collision/shapes/ConcaveMeshShape.cpp index 15138e2..a63c0ae 100644 --- a/ephysics/collision/shapes/ConcaveMeshShape.cpp +++ b/ephysics/collision/shapes/ConcaveMeshShape.cpp @@ -182,3 +182,69 @@ void ConcaveMeshRaycastCallback::raycastTriangles() { } } } + +// Return the number of bytes used by the collision shape +size_t ConcaveMeshShape::getSizeInBytes() const { + return sizeof(ConcaveMeshShape); +} + +// Return the local bounds of the shape in x, y and z directions. +// This method is used to compute the AABB of the box +/** + * @param min The minimum bounds of the shape in local-space coordinates + * @param max The maximum bounds of the shape in local-space coordinates + */ +void ConcaveMeshShape::getLocalBounds(vec3& min, vec3& max) const { + + // Get the AABB of the whole tree + AABB treeAABB = m_dynamicAABBTree.getRootAABB(); + + min = treeAABB.getMin(); + max = treeAABB.getMax(); +} + +// Set the local scaling vector of the collision shape +void ConcaveMeshShape::setLocalScaling(const vec3& scaling) { + + CollisionShape::setLocalScaling(scaling); + + // Reset the Dynamic AABB Tree + m_dynamicAABBTree.reset(); + + // Rebuild Dynamic AABB Tree here + initBVHTree(); +} + +// Return the local inertia tensor of the shape +/** + * @param[out] tensor The 3x3 inertia tensor matrix of the shape in local-space + * coordinates + * @param mass Mass to use to compute the inertia tensor of the collision shape + */ +void ConcaveMeshShape::computeLocalInertiaTensor(etk::Matrix3x3& _tensor, float _mass) const { + + // Default inertia tensor + // Note that this is not very realistic for a concave triangle mesh. + // However, in most cases, it will only be used static bodies and therefore, + // the inertia tensor is not used. + _tensor.setValue(_mass, 0, 0, + 0, _mass, 0, + 0, 0, _mass); +} + +// Called when a overlapping node has been found during the call to +// DynamicAABBTree:reportAllShapesOverlappingWithAABB() +void ConvexTriangleAABBOverlapCallback::notifyOverlappingNode(int32_t _nodeId) { + + // Get the node data (triangle index and mesh subpart index) + int32_t* data = m_dynamicAABBTree.getNodeDataInt(_nodeId); + + // Get the triangle vertices for this node from the concave mesh shape + vec3 trianglePoints[3]; + m_concaveMeshShape.getTriangleVerticesWithIndexPointer(data[0], data[1], trianglePoints); + + // Call the callback to test narrow-phase collision with this triangle + m_triangleTestCallback.testTriangle(trianglePoints); +} + + diff --git a/ephysics/collision/shapes/ConcaveMeshShape.hpp b/ephysics/collision/shapes/ConcaveMeshShape.hpp index b04be78..5d3b25f 100644 --- a/ephysics/collision/shapes/ConcaveMeshShape.hpp +++ b/ephysics/collision/shapes/ConcaveMeshShape.hpp @@ -12,7 +12,6 @@ #include namespace ephysics { - class ConcaveMeshShape; class ConvexTriangleAABBOverlapCallback : public DynamicAABBTreeOverlapCallback { private: @@ -32,10 +31,8 @@ namespace ephysics { // Called when a overlapping node has been found during the call to // DynamicAABBTree:reportAllShapesOverlappingWithAABB() virtual void notifyOverlappingNode(int32_t _nodeId); - }; - /// Class ConcaveMeshRaycastCallback class ConcaveMeshRaycastCallback : public DynamicAABBTreeRaycastCallback { private : std::vector m_hitAABBNodes; @@ -110,68 +107,4 @@ namespace ephysics { friend class ConcaveMeshRaycastCallback; }; -// Return the number of bytes used by the collision shape -size_t ConcaveMeshShape::getSizeInBytes() const { - return sizeof(ConcaveMeshShape); -} - -// Return the local bounds of the shape in x, y and z directions. -// This method is used to compute the AABB of the box -/** - * @param min The minimum bounds of the shape in local-space coordinates - * @param max The maximum bounds of the shape in local-space coordinates - */ -void ConcaveMeshShape::getLocalBounds(vec3& min, vec3& max) const { - - // Get the AABB of the whole tree - AABB treeAABB = m_dynamicAABBTree.getRootAABB(); - - min = treeAABB.getMin(); - max = treeAABB.getMax(); -} - -// Set the local scaling vector of the collision shape -void ConcaveMeshShape::setLocalScaling(const vec3& scaling) { - - CollisionShape::setLocalScaling(scaling); - - // Reset the Dynamic AABB Tree - m_dynamicAABBTree.reset(); - - // Rebuild Dynamic AABB Tree here - initBVHTree(); -} - -// Return the local inertia tensor of the shape -/** - * @param[out] tensor The 3x3 inertia tensor matrix of the shape in local-space - * coordinates - * @param mass Mass to use to compute the inertia tensor of the collision shape - */ -void ConcaveMeshShape::computeLocalInertiaTensor(etk::Matrix3x3& _tensor, float _mass) const { - - // Default inertia tensor - // Note that this is not very realistic for a concave triangle mesh. - // However, in most cases, it will only be used static bodies and therefore, - // the inertia tensor is not used. - tensor.setValue(mass, 0, 0, - 0, mass, 0, - 0, 0, mass); -} - -// Called when a overlapping node has been found during the call to -// DynamicAABBTree:reportAllShapesOverlappingWithAABB() -void ConvexTriangleAABBOverlapCallback::notifyOverlappingNode(int32_t _nodeId) { - - // Get the node data (triangle index and mesh subpart index) - int32_t* data = m_dynamicAABBTree.getNodeDataInt(nodeId); - - // Get the triangle vertices for this node from the concave mesh shape - vec3 trianglePoints[3]; - m_concaveMeshShape.getTriangleVerticesWithIndexPointer(data[0], data[1], trianglePoints); - - // Call the callback to test narrow-phase collision with this triangle - m_triangleTestCallback.testTriangle(trianglePoints); -} - } diff --git a/ephysics/collision/shapes/ConcaveShape.cpp b/ephysics/collision/shapes/ConcaveShape.cpp index ea4c6ba..da76acc 100644 --- a/ephysics/collision/shapes/ConcaveShape.cpp +++ b/ephysics/collision/shapes/ConcaveShape.cpp @@ -22,3 +22,44 @@ ConcaveShape::ConcaveShape(CollisionShapeType type) ConcaveShape::~ConcaveShape() { } + +// Return the triangle margin +float ConcaveShape::getTriangleMargin() const { + return m_triangleMargin; +} + +/// Return true if the collision shape is convex, false if it is concave +bool ConcaveShape::isConvex() const { + return false; +} + +// Return true if a point is inside the collision shape +bool ConcaveShape::testPointInside(const vec3& localPoint, ProxyShape* proxyShape) const { + return false; +} + +// Return true if the smooth mesh collision is enabled +bool ConcaveShape::getIsSmoothMeshCollisionEnabled() const { + return m_isSmoothMeshCollisionEnabled; +} + +// Enable/disable the smooth mesh collision algorithm +/// Smooth mesh collision is used to avoid collisions against some int32_ternal edges +/// of the triangle mesh. If it is enabled, collsions with the mesh will be smoother +/// but collisions computation is a bit more expensive. +void ConcaveShape::setIsSmoothMeshCollisionEnabled(bool isEnabled) { + m_isSmoothMeshCollisionEnabled = isEnabled; +} + +// Return the raycast test type (front, back, front-back) +TriangleRaycastSide ConcaveShape::getRaycastTestType() const { + return m_raycastTestType; +} + +// Set the raycast test type (front, back, front-back) +/** + * @param testType Raycast test type for the triangle (front, back, front-back) + */ +void ConcaveShape::setRaycastTestType(TriangleRaycastSide testType) { + m_raycastTestType = testType; +} diff --git a/ephysics/collision/shapes/ConcaveShape.hpp b/ephysics/collision/shapes/ConcaveShape.hpp index e7bc415..9ed4bc9 100644 --- a/ephysics/collision/shapes/ConcaveShape.hpp +++ b/ephysics/collision/shapes/ConcaveShape.hpp @@ -56,47 +56,6 @@ namespace ephysics { void setIsSmoothMeshCollisionEnabled(bool _isEnabled); }; -// Return the triangle margin -float ConcaveShape::getTriangleMargin() const { - return m_triangleMargin; -} - -/// Return true if the collision shape is convex, false if it is concave -bool ConcaveShape::isConvex() const { - return false; -} - -// Return true if a point is inside the collision shape -bool ConcaveShape::testPointInside(const vec3& localPoint, ProxyShape* proxyShape) const { - return false; -} - -// Return true if the smooth mesh collision is enabled -bool ConcaveShape::getIsSmoothMeshCollisionEnabled() const { - return m_isSmoothMeshCollisionEnabled; -} - -// Enable/disable the smooth mesh collision algorithm -/// Smooth mesh collision is used to avoid collisions against some int32_ternal edges -/// of the triangle mesh. If it is enabled, collsions with the mesh will be smoother -/// but collisions computation is a bit more expensive. -void ConcaveShape::setIsSmoothMeshCollisionEnabled(bool isEnabled) { - m_isSmoothMeshCollisionEnabled = isEnabled; -} - -// Return the raycast test type (front, back, front-back) -TriangleRaycastSide ConcaveShape::getRaycastTestType() const { - return m_raycastTestType; -} - -// Set the raycast test type (front, back, front-back) -/** - * @param testType Raycast test type for the triangle (front, back, front-back) - */ -void ConcaveShape::setRaycastTestType(TriangleRaycastSide testType) { - m_raycastTestType = testType; -} - } diff --git a/ephysics/collision/shapes/ConeShape.cpp b/ephysics/collision/shapes/ConeShape.cpp index ccb5732..557d497 100644 --- a/ephysics/collision/shapes/ConeShape.cpp +++ b/ephysics/collision/shapes/ConeShape.cpp @@ -190,3 +190,73 @@ bool ConeShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* pr return true; } + +// Return the radius +/** + * @return Radius of the cone (in meters) + */ +float ConeShape::getRadius() const { + return m_radius; +} + +// Return the height +/** + * @return Height of the cone (in meters) + */ +float ConeShape::getHeight() const { + return float(2.0) * m_halfHeight; +} + +// Set the scaling vector of the collision shape +void ConeShape::setLocalScaling(const vec3& scaling) { + + m_halfHeight = (m_halfHeight / m_scaling.y()) * scaling.y(); + m_radius = (m_radius / m_scaling.x()) * scaling.x(); + + CollisionShape::setLocalScaling(scaling); +} + +// Return the number of bytes used by the collision shape +size_t ConeShape::getSizeInBytes() const { + return sizeof(ConeShape); +} + +// Return the local bounds of the shape in x, y and z directions +/** + * @param min The minimum bounds of the shape in local-space coordinates + * @param max The maximum bounds of the shape in local-space coordinates + */ +void ConeShape::getLocalBounds(vec3& min, vec3& max) const { + + // Maximum bounds + max.setX(m_radius + m_margin); + max.setY(m_halfHeight + m_margin); + max.setZ(max.x()); + + // Minimum bounds + min.setX(-max.x()); + min.setY(-max.y()); + min.setZ(min.x()); +} + +// Return the local inertia tensor of the collision shape +/** + * @param[out] tensor The 3x3 inertia tensor matrix of the shape in local-space + * coordinates + * @param mass Mass to use to compute the inertia tensor of the collision shape + */ +void ConeShape::computeLocalInertiaTensor(etk::Matrix3x3& tensor, float mass) const { + float rSquare = m_radius * m_radius; + float diagXZ = float(0.15) * mass * (rSquare + m_halfHeight); + tensor.setValue(diagXZ, 0.0, 0.0, + 0.0, float(0.3) * mass * rSquare, + 0.0, 0.0, 0.0, diagXZ); +} + +// Return true if a point is inside the collision shape +bool ConeShape::testPointInside(const vec3& localPoint, ProxyShape* proxyShape) const { + const float radiusHeight = m_radius * (-localPoint.y() + m_halfHeight) / + (m_halfHeight * float(2.0)); + return (localPoint.y() < m_halfHeight && localPoint.y() > -m_halfHeight) && + (localPoint.x() * localPoint.x() + localPoint.z() * localPoint.z() < radiusHeight *radiusHeight); +} diff --git a/ephysics/collision/shapes/ConeShape.hpp b/ephysics/collision/shapes/ConeShape.hpp index b7f7db4..85b4fc8 100644 --- a/ephysics/collision/shapes/ConeShape.hpp +++ b/ephysics/collision/shapes/ConeShape.hpp @@ -56,75 +56,4 @@ namespace ephysics { /// Return the local inertia tensor of the collision shape virtual void computeLocalInertiaTensor(etk::Matrix3x3& _tensor, float _mass) const; }; - -// Return the radius -/** - * @return Radius of the cone (in meters) - */ -float ConeShape::getRadius() const { - return m_radius; -} - -// Return the height -/** - * @return Height of the cone (in meters) - */ -float ConeShape::getHeight() const { - return float(2.0) * m_halfHeight; -} - -// Set the scaling vector of the collision shape -void ConeShape::setLocalScaling(const vec3& scaling) { - - m_halfHeight = (m_halfHeight / m_scaling.y()) * scaling.y(); - m_radius = (m_radius / m_scaling.x()) * scaling.x(); - - CollisionShape::setLocalScaling(scaling); -} - -// Return the number of bytes used by the collision shape -size_t ConeShape::getSizeInBytes() const { - return sizeof(ConeShape); -} - -// Return the local bounds of the shape in x, y and z directions -/** - * @param min The minimum bounds of the shape in local-space coordinates - * @param max The maximum bounds of the shape in local-space coordinates - */ -void ConeShape::getLocalBounds(vec3& min, vec3& max) const { - - // Maximum bounds - max.setX(m_radius + m_margin); - max.setY(m_halfHeight + m_margin); - max.setZ(max.x()); - - // Minimum bounds - min.setX(-max.x()); - min.setY(-max.y()); - min.setZ(min.x()); -} - -// Return the local inertia tensor of the collision shape -/** - * @param[out] tensor The 3x3 inertia tensor matrix of the shape in local-space - * coordinates - * @param mass Mass to use to compute the inertia tensor of the collision shape - */ -void ConeShape::computeLocalInertiaTensor(etk::Matrix3x3& tensor, float mass) const { - float rSquare = m_radius * m_radius; - float diagXZ = float(0.15) * mass * (rSquare + m_halfHeight); - tensor.setValue(diagXZ, 0.0, 0.0, - 0.0, float(0.3) * mass * rSquare, - 0.0, 0.0, 0.0, diagXZ); -} - -// Return true if a point is inside the collision shape -bool ConeShape::testPointInside(const vec3& localPoint, ProxyShape* proxyShape) const { - const float radiusHeight = m_radius * (-localPoint.y() + m_halfHeight) / - (m_halfHeight * float(2.0)); - return (localPoint.y() < m_halfHeight && localPoint.y() > -m_halfHeight) && - (localPoint.x() * localPoint.x() + localPoint.z() * localPoint.z() < radiusHeight *radiusHeight); -} - } diff --git a/ephysics/collision/shapes/ConvexMeshShape.cpp b/ephysics/collision/shapes/ConvexMeshShape.cpp index 8679218..eb02f37 100644 --- a/ephysics/collision/shapes/ConvexMeshShape.cpp +++ b/ephysics/collision/shapes/ConvexMeshShape.cpp @@ -257,3 +257,127 @@ void ConvexMeshShape::recalculateBounds() { bool ConvexMeshShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape) const { return proxyShape->m_body->m_world.m_collisionDetection.m_narrowPhaseGJKAlgorithm.raycast(ray, proxyShape, raycastInfo); } + +/// Set the scaling vector of the collision shape +void ConvexMeshShape::setLocalScaling(const vec3& scaling) { + ConvexShape::setLocalScaling(scaling); + recalculateBounds(); +} + +// Return the number of bytes used by the collision shape +size_t ConvexMeshShape::getSizeInBytes() const { + return sizeof(ConvexMeshShape); +} + +// Return the local bounds of the shape in x, y and z directions +/** + * @param min The minimum bounds of the shape in local-space coordinates + * @param max The maximum bounds of the shape in local-space coordinates + */ +void ConvexMeshShape::getLocalBounds(vec3& min, vec3& max) const { + min = m_minBounds; + max = m_maxBounds; +} + +// Return the local inertia tensor of the collision shape. +/// The local inertia tensor of the convex mesh is approximated using the inertia tensor +/// of its bounding box. +/** +* @param[out] tensor The 3x3 inertia tensor matrix of the shape in local-space +* coordinates +* @param mass Mass to use to compute the inertia tensor of the collision shape +*/ +void ConvexMeshShape::computeLocalInertiaTensor(etk::Matrix3x3& tensor, float mass) const { + float factor = (1.0f / float(3.0)) * mass; + vec3 realExtent = 0.5f * (m_maxBounds - m_minBounds); + assert(realExtent.x() > 0 && realExtent.y() > 0 && realExtent.z() > 0); + float xSquare = realExtent.x() * realExtent.x(); + float ySquare = realExtent.y() * realExtent.y(); + float zSquare = realExtent.z() * realExtent.z(); + tensor.setValue(factor * (ySquare + zSquare), 0.0, 0.0, + 0.0, factor * (xSquare + zSquare), 0.0, + 0.0, 0.0, factor * (xSquare + ySquare)); +} + +// Add a vertex int32_to the convex mesh +/** + * @param vertex Vertex to be added + */ +void ConvexMeshShape::addVertex(const vec3& vertex) { + + // Add the vertex in to vertices array + m_vertices.push_back(vertex); + m_numberVertices++; + + // Update the bounds of the mesh + if (vertex.x() * m_scaling.x() > m_maxBounds.x()) { + m_maxBounds.setX(vertex.x() * m_scaling.x()); + } + if (vertex.x() * m_scaling.x() < m_minBounds.x()) { + m_minBounds.setX(vertex.x() * m_scaling.x()); + } + if (vertex.y() * m_scaling.y() > m_maxBounds.y()) { + m_maxBounds.setY(vertex.y() * m_scaling.y()); + } + if (vertex.y() * m_scaling.y() < m_minBounds.y()) { + m_minBounds.setY(vertex.y() * m_scaling.y()); + } + if (vertex.z() * m_scaling.z() > m_maxBounds.z()) { + m_maxBounds.setZ(vertex.z() * m_scaling.z()); + } + if (vertex.z() * m_scaling.z() < m_minBounds.z()) { + m_minBounds.setZ(vertex.z() * m_scaling.z()); + } +} + +// Add an edge int32_to the convex mesh by specifying the two vertex indices of the edge. +/// Note that the vertex indices start at zero and need to correspond to the order of +/// the vertices in the vertices array in the constructor or the order of the calls +/// of the addVertex() methods that you use to add vertices int32_to the convex mesh. +/** +* @param v1 Index of the first vertex of the edge to add +* @param v2 Index of the second vertex of the edge to add +*/ +void ConvexMeshShape::addEdge(uint32_t v1, uint32_t v2) { + + // If the entry for vertex v1 does not exist in the adjacency list + if (m_edgesAdjacencyList.count(v1) == 0) { + m_edgesAdjacencyList.insert(std::make_pair(v1, std::set())); + } + + // If the entry for vertex v2 does not exist in the adjacency list + if (m_edgesAdjacencyList.count(v2) == 0) { + m_edgesAdjacencyList.insert(std::make_pair(v2, std::set())); + } + + // Add the edge in the adjacency list + m_edgesAdjacencyList[v1].insert(v2); + m_edgesAdjacencyList[v2].insert(v1); +} + +// Return true if the edges information is used to speed up the collision detection +/** + * @return True if the edges information is used and false otherwise + */ +bool ConvexMeshShape::isEdgesInformationUsed() const { + return m_isEdgesInformationUsed; +} + +// Set the variable to know if the edges information is used to speed up the +// collision detection +/** + * @param isEdgesUsed True if you want to use the edges information to speed up + * the collision detection with the convex mesh shape + */ +void ConvexMeshShape::setIsEdgesInformationUsed(bool isEdgesUsed) { + m_isEdgesInformationUsed = isEdgesUsed; +} + +// Return true if a point is inside the collision shape +bool ConvexMeshShape::testPointInside(const vec3& localPoint, + ProxyShape* proxyShape) const { + + // Use the GJK algorithm to test if the point is inside the convex mesh + return proxyShape->m_body->m_world.m_collisionDetection. + m_narrowPhaseGJKAlgorithm.testPointInside(localPoint, proxyShape); +} \ No newline at end of file diff --git a/ephysics/collision/shapes/ConvexMeshShape.hpp b/ephysics/collision/shapes/ConvexMeshShape.hpp index 1733bff..24b42b0 100644 --- a/ephysics/collision/shapes/ConvexMeshShape.hpp +++ b/ephysics/collision/shapes/ConvexMeshShape.hpp @@ -5,7 +5,6 @@ */ #pragma once -// Libraries #include #include #include @@ -15,240 +14,73 @@ #include #include -/// ReactPhysics3D namespace namespace ephysics { - -// Declaration -class CollisionWorld; - -// Class ConvexMeshShape -/** - * This class represents a convex mesh shape. In order to create a convex mesh shape, you - * need to indicate the local-space position of the mesh vertices. You do it either by - * passing a vertices array to the constructor or using the addVertex() method. Make sure - * that the set of vertices that you use to create the shape are indeed part of a convex - * mesh. The center of mass of the shape will be at the origin of the local-space geometry - * that you use to create the mesh. The method used for collision detection with a convex - * mesh shape has an O(n) running time with "n" beeing the number of vertices in the mesh. - * Therefore, you should try not to use too many vertices. However, it is possible to speed - * up the collision detection by using the edges information of your mesh. The running time - * of the collision detection that uses the edges is almost O(1) constant time at the cost - * of additional memory used to store the vertices. You can indicate edges information - * with the addEdge() method. Then, you must use the setIsEdgesInformationUsed(true) method - * in order to use the edges information for collision detection. - */ -class ConvexMeshShape : public ConvexShape { - - protected : - - // -------------------- Attributes -------------------- // - - /// Array with the vertices of the mesh - std::vector m_vertices; - - /// Number of vertices in the mesh - uint32_t m_numberVertices; - - /// Mesh minimum bounds in the three local x, y and z directions - vec3 m_minBounds; - - /// Mesh maximum bounds in the three local x, y and z directions - vec3 m_maxBounds; - - /// True if the shape contains the edges of the convex mesh in order to - /// make the collision detection faster - bool m_isEdgesInformationUsed; - - /// Adjacency list representing the edges of the mesh - std::map > m_edgesAdjacencyList; - - // -------------------- Methods -------------------- // - - /// Private copy-constructor - ConvexMeshShape(const ConvexMeshShape& shape); - - /// Private assignment operator - ConvexMeshShape& operator=(const ConvexMeshShape& shape); - - /// Recompute the bounds of the mesh - void recalculateBounds(); - - /// Set the scaling vector of the collision shape - virtual void setLocalScaling(const vec3& scaling); - - /// Return a local support point in a given direction without the object margin. - virtual vec3 getLocalSupportPointWithoutMargin(const vec3& direction, - void** cachedCollisionData) const; - - /// Return true if a point is inside the collision shape - virtual bool testPointInside(const vec3& localPoint, ProxyShape* proxyShape) const; - - /// Raycast method with feedback information - virtual bool raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape) const; - - /// Return the number of bytes used by the collision shape - virtual size_t getSizeInBytes() const; - - public : - - // -------------------- Methods -------------------- // - - /// Constructor to initialize with an array of 3D vertices. - ConvexMeshShape(const float* arrayVertices, uint32_t nbVertices, int32_t stride, - float margin = OBJECT_MARGIN); - - /// Constructor to initialize with a triangle vertex array - ConvexMeshShape(TriangleVertexArray* triangleVertexArray, bool isEdgesInformationUsed = true, - float margin = OBJECT_MARGIN); - - /// Constructor. - ConvexMeshShape(float margin = OBJECT_MARGIN); - - /// Destructor - virtual ~ConvexMeshShape(); - - /// Return the local bounds of the shape in x, y and z directions - virtual void getLocalBounds(vec3& min, vec3& max) const; - - /// Return the local inertia tensor of the collision shape. - virtual void computeLocalInertiaTensor(etk::Matrix3x3& tensor, float mass) const; - - /// Add a vertex int32_to the convex mesh - void addVertex(const vec3& vertex); - - /// Add an edge int32_to the convex mesh by specifying the two vertex indices of the edge. - void addEdge(uint32_t v1, uint32_t v2); - - /// Return true if the edges information is used to speed up the collision detection - bool isEdgesInformationUsed() const; - - /// Set the variable to know if the edges information is used to speed up the - /// collision detection - void setIsEdgesInformationUsed(bool isEdgesUsed); -}; - -/// Set the scaling vector of the collision shape -void ConvexMeshShape::setLocalScaling(const vec3& scaling) { - ConvexShape::setLocalScaling(scaling); - recalculateBounds(); + class CollisionWorld; + /** + * @brief It represents a convex mesh shape. In order to create a convex mesh shape, you + * need to indicate the local-space position of the mesh vertices. You do it either by + * passing a vertices array to the constructor or using the addVertex() method. Make sure + * that the set of vertices that you use to create the shape are indeed part of a convex + * mesh. The center of mass of the shape will be at the origin of the local-space geometry + * that you use to create the mesh. The method used for collision detection with a convex + * mesh shape has an O(n) running time with "n" beeing the number of vertices in the mesh. + * Therefore, you should try not to use too many vertices. However, it is possible to speed + * up the collision detection by using the edges information of your mesh. The running time + * of the collision detection that uses the edges is almost O(1) constant time at the cost + * of additional memory used to store the vertices. You can indicate edges information + * with the addEdge() method. Then, you must use the setIsEdgesInformationUsed(true) method + * in order to use the edges information for collision detection. + */ + class ConvexMeshShape : public ConvexShape { + protected : + std::vector m_vertices; //!< Array with the vertices of the mesh + uint32_t m_numberVertices; //!< Number of vertices in the mesh + vec3 m_minBounds; //!< Mesh minimum bounds in the three local x, y and z directions + vec3 m_maxBounds; //!< Mesh maximum bounds in the three local x, y and z directions + bool m_isEdgesInformationUsed; //!< True if the shape contains the edges of the convex mesh in order to make the collision detection faster + std::map > m_edgesAdjacencyList; //!< Adjacency list representing the edges of the mesh + /// Private copy-constructor + ConvexMeshShape(const ConvexMeshShape& shape); + /// Private assignment operator + ConvexMeshShape& operator=(const ConvexMeshShape& shape); + /// Recompute the bounds of the mesh + void recalculateBounds(); + /// Set the scaling vector of the collision shape + virtual void setLocalScaling(const vec3& scaling); + /// Return a local support point in a given direction without the object margin. + virtual vec3 getLocalSupportPointWithoutMargin(const vec3& direction, + void** cachedCollisionData) const; + /// Return true if a point is inside the collision shape + virtual bool testPointInside(const vec3& localPoint, ProxyShape* proxyShape) const; + /// Raycast method with feedback information + virtual bool raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape) const; + /// Return the number of bytes used by the collision shape + virtual size_t getSizeInBytes() const; + public : + /// Constructor to initialize with an array of 3D vertices. + ConvexMeshShape(const float* arrayVertices, uint32_t nbVertices, int32_t stride, + float margin = OBJECT_MARGIN); + /// Constructor to initialize with a triangle vertex array + ConvexMeshShape(TriangleVertexArray* triangleVertexArray, bool isEdgesInformationUsed = true, + float margin = OBJECT_MARGIN); + /// Constructor. + ConvexMeshShape(float margin = OBJECT_MARGIN); + /// Destructor + virtual ~ConvexMeshShape(); + /// Return the local bounds of the shape in x, y and z directions + virtual void getLocalBounds(vec3& min, vec3& max) const; + /// Return the local inertia tensor of the collision shape. + virtual void computeLocalInertiaTensor(etk::Matrix3x3& tensor, float mass) const; + /// Add a vertex int32_to the convex mesh + void addVertex(const vec3& vertex); + /// Add an edge int32_to the convex mesh by specifying the two vertex indices of the edge. + void addEdge(uint32_t v1, uint32_t v2); + /// Return true if the edges information is used to speed up the collision detection + bool isEdgesInformationUsed() const; + /// Set the variable to know if the edges information is used to speed up the + /// collision detection + void setIsEdgesInformationUsed(bool isEdgesUsed); + }; } -// Return the number of bytes used by the collision shape -size_t ConvexMeshShape::getSizeInBytes() const { - return sizeof(ConvexMeshShape); -} -// Return the local bounds of the shape in x, y and z directions -/** - * @param min The minimum bounds of the shape in local-space coordinates - * @param max The maximum bounds of the shape in local-space coordinates - */ -void ConvexMeshShape::getLocalBounds(vec3& min, vec3& max) const { - min = m_minBounds; - max = m_maxBounds; -} - -// Return the local inertia tensor of the collision shape. -/// The local inertia tensor of the convex mesh is approximated using the inertia tensor -/// of its bounding box. -/** -* @param[out] tensor The 3x3 inertia tensor matrix of the shape in local-space -* coordinates -* @param mass Mass to use to compute the inertia tensor of the collision shape -*/ -void ConvexMeshShape::computeLocalInertiaTensor(etk::Matrix3x3& tensor, float mass) const { - float factor = (1.0f / float(3.0)) * mass; - vec3 realExtent = 0.5f * (m_maxBounds - m_minBounds); - assert(realExtent.x() > 0 && realExtent.y() > 0 && realExtent.z() > 0); - float xSquare = realExtent.x() * realExtent.x(); - float ySquare = realExtent.y() * realExtent.y(); - float zSquare = realExtent.z() * realExtent.z(); - tensor.setValue(factor * (ySquare + zSquare), 0.0, 0.0, - 0.0, factor * (xSquare + zSquare), 0.0, - 0.0, 0.0, factor * (xSquare + ySquare)); -} - -// Add a vertex int32_to the convex mesh -/** - * @param vertex Vertex to be added - */ -void ConvexMeshShape::addVertex(const vec3& vertex) { - - // Add the vertex in to vertices array - m_vertices.push_back(vertex); - m_numberVertices++; - - // Update the bounds of the mesh - if (vertex.x() * m_scaling.x() > m_maxBounds.x()) { - m_maxBounds.setX(vertex.x() * m_scaling.x()); - } - if (vertex.x() * m_scaling.x() < m_minBounds.x()) { - m_minBounds.setX(vertex.x() * m_scaling.x()); - } - if (vertex.y() * m_scaling.y() > m_maxBounds.y()) { - m_maxBounds.setY(vertex.y() * m_scaling.y()); - } - if (vertex.y() * m_scaling.y() < m_minBounds.y()) { - m_minBounds.setY(vertex.y() * m_scaling.y()); - } - if (vertex.z() * m_scaling.z() > m_maxBounds.z()) { - m_maxBounds.setZ(vertex.z() * m_scaling.z()); - } - if (vertex.z() * m_scaling.z() < m_minBounds.z()) { - m_minBounds.setZ(vertex.z() * m_scaling.z()); - } -} - -// Add an edge int32_to the convex mesh by specifying the two vertex indices of the edge. -/// Note that the vertex indices start at zero and need to correspond to the order of -/// the vertices in the vertices array in the constructor or the order of the calls -/// of the addVertex() methods that you use to add vertices int32_to the convex mesh. -/** -* @param v1 Index of the first vertex of the edge to add -* @param v2 Index of the second vertex of the edge to add -*/ -void ConvexMeshShape::addEdge(uint32_t v1, uint32_t v2) { - - // If the entry for vertex v1 does not exist in the adjacency list - if (m_edgesAdjacencyList.count(v1) == 0) { - m_edgesAdjacencyList.insert(std::make_pair(v1, std::set())); - } - - // If the entry for vertex v2 does not exist in the adjacency list - if (m_edgesAdjacencyList.count(v2) == 0) { - m_edgesAdjacencyList.insert(std::make_pair(v2, std::set())); - } - - // Add the edge in the adjacency list - m_edgesAdjacencyList[v1].insert(v2); - m_edgesAdjacencyList[v2].insert(v1); -} - -// Return true if the edges information is used to speed up the collision detection -/** - * @return True if the edges information is used and false otherwise - */ -bool ConvexMeshShape::isEdgesInformationUsed() const { - return m_isEdgesInformationUsed; -} - -// Set the variable to know if the edges information is used to speed up the -// collision detection -/** - * @param isEdgesUsed True if you want to use the edges information to speed up - * the collision detection with the convex mesh shape - */ -void ConvexMeshShape::setIsEdgesInformationUsed(bool isEdgesUsed) { - m_isEdgesInformationUsed = isEdgesUsed; -} - -// Return true if a point is inside the collision shape -bool ConvexMeshShape::testPointInside(const vec3& localPoint, - ProxyShape* proxyShape) const { - - // Use the GJK algorithm to test if the point is inside the convex mesh - return proxyShape->m_body->m_world.m_collisionDetection. - m_narrowPhaseGJKAlgorithm.testPointInside(localPoint, proxyShape); -} - -} diff --git a/ephysics/collision/shapes/ConvexShape.hpp b/ephysics/collision/shapes/ConvexShape.hpp index 5f000ca..55c38cd 100644 --- a/ephysics/collision/shapes/ConvexShape.hpp +++ b/ephysics/collision/shapes/ConvexShape.hpp @@ -5,80 +5,46 @@ */ #pragma once -// Libraries #include - -/// ReactPhysics3D namespace namespace ephysics { - -// Class ConvexShape /** - * This abstract class represents a convex collision shape associated with a + * @brief It represents a convex collision shape associated with a * body that is used during the narrow-phase collision detection. */ class ConvexShape : public CollisionShape { - protected : - - // -------------------- Attributes -------------------- // - - /// Margin used for the GJK collision detection algorithm - float m_margin; - - // -------------------- Methods -------------------- // - + float m_margin; //!< Margin used for the GJK collision detection algorithm /// Private copy-constructor ConvexShape(const ConvexShape& shape); - /// Private assignment operator ConvexShape& operator=(const ConvexShape& shape); - // Return a local support point in a given direction with the object margin vec3 getLocalSupportPointWithMargin(const vec3& direction, void** cachedCollisionData) const; - /// Return a local support point in a given direction without the object margin virtual vec3 getLocalSupportPointWithoutMargin(const vec3& direction, void** cachedCollisionData) const=0; - /// Return true if a point is inside the collision shape virtual bool testPointInside(const vec3& worldPoint, ProxyShape* proxyShape) const=0; - public : - - // -------------------- Methods -------------------- // - /// Constructor ConvexShape(CollisionShapeType type, float margin); - /// Destructor virtual ~ConvexShape(); - - /// Return the current object margin - float getMargin() const; - - /// Return true if the collision shape is convex, false if it is concave - virtual bool isConvex() const; - - // -------------------- Friendship -------------------- // - + /** + * @brief Get the current object margin + * @return The margin (in meters) around the collision shape + */ + float getMargin() const { + return m_margin; + } + virtual bool isConvex() const override { + return true; + } friend class GJKAlgorithm; friend class EPAAlgorithm; }; -/// Return true if the collision shape is convex, false if it is concave -bool ConvexShape::isConvex() const { - return true; -} - -// Return the current collision shape margin -/** - * @return The margin (in meters) around the collision shape - */ -float ConvexShape::getMargin() const { - return m_margin; -} - } diff --git a/ephysics/collision/shapes/CylinderShape.cpp b/ephysics/collision/shapes/CylinderShape.cpp index afa4a11..027a5bf 100644 --- a/ephysics/collision/shapes/CylinderShape.cpp +++ b/ephysics/collision/shapes/CylinderShape.cpp @@ -216,3 +216,70 @@ bool CylinderShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape return true; } + +// Return the radius +/** + * @return Radius of the cylinder (in meters) + */ +float CylinderShape::getRadius() const { + return mRadius; +} + +// Return the height +/** + * @return Height of the cylinder (in meters) + */ +float CylinderShape::getHeight() const { + return m_halfHeight + m_halfHeight; +} + +// Set the scaling vector of the collision shape +void CylinderShape::setLocalScaling(const vec3& scaling) { + + m_halfHeight = (m_halfHeight / m_scaling.y()) * scaling.y(); + mRadius = (mRadius / m_scaling.x()) * scaling.x(); + + CollisionShape::setLocalScaling(scaling); +} + +// Return the number of bytes used by the collision shape +size_t CylinderShape::getSizeInBytes() const { + return sizeof(CylinderShape); +} + +// Return the local bounds of the shape in x, y and z directions +/** + * @param min The minimum bounds of the shape in local-space coordinates + * @param max The maximum bounds of the shape in local-space coordinates + */ +void CylinderShape::getLocalBounds(vec3& min, vec3& max) const { + // Maximum bounds + max.setX(mRadius + m_margin); + max.setY(m_halfHeight + m_margin); + max.setZ(max.x()); + // Minimum bounds + min.setX(-max.x()); + min.setY(-max.y()); + min.setZ(min.x()); +} + +// Return the local inertia tensor of the cylinder +/** + * @param[out] tensor The 3x3 inertia tensor matrix of the shape in local-space + * coordinates + * @param mass Mass to use to compute the inertia tensor of the collision shape + */ +void CylinderShape::computeLocalInertiaTensor(etk::Matrix3x3& tensor, float mass) const { + float height = float(2.0) * m_halfHeight; + float diag = (1.0f / float(12.0)) * mass * (3 * mRadius * mRadius + height * height); + tensor.setValue(diag, 0.0, 0.0, 0.0, + 0.5f * mass * mRadius * mRadius, 0.0, + 0.0, 0.0, diag); +} + +// Return true if a point is inside the collision shape +bool CylinderShape::testPointInside(const vec3& localPoint, ProxyShape* proxyShape) const{ + return ( (localPoint.x() * localPoint.x() + localPoint.z() * localPoint.z()) < mRadius * mRadius + && localPoint.y() < m_halfHeight + && localPoint.y() > -m_halfHeight); +} diff --git a/ephysics/collision/shapes/CylinderShape.hpp b/ephysics/collision/shapes/CylinderShape.hpp index c46102c..283d8a1 100644 --- a/ephysics/collision/shapes/CylinderShape.hpp +++ b/ephysics/collision/shapes/CylinderShape.hpp @@ -5,18 +5,13 @@ */ #pragma once -// Libraries #include #include #include - -/// ReactPhysics3D namespace namespace ephysics { - -// Class CylinderShape /** - * This class represents a cylinder collision shape around the Y axis + * @brief It represents a cylinder collision shape around the Y axis * and centered at the origin. The cylinder is defined by its height * and the radius of its base. The "transform" of the corresponding * rigid body gives an orientation and a position to the cylinder. @@ -29,131 +24,39 @@ namespace ephysics { * default margin distance by not using the "margin" parameter in the constructor. */ class CylinderShape : public ConvexShape { - protected : - - // -------------------- Attributes -------------------- // - - /// Radius of the base - float mRadius; - - /// Half height of the cylinder - float m_halfHeight; - - // -------------------- Methods -------------------- // - + float mRadius; //!< Radius of the base + float m_halfHeight; //!< Half height of the cylinder /// Private copy-constructor CylinderShape(const CylinderShape& shape); - /// Private assignment operator CylinderShape& operator=(const CylinderShape& shape); - /// Return a local support point in a given direction without the object margin virtual vec3 getLocalSupportPointWithoutMargin(const vec3& direction, void** cachedCollisionData) const; - /// Return true if a point is inside the collision shape virtual bool testPointInside(const vec3& localPoint, ProxyShape* proxyShape) const; - /// Raycast method with feedback information virtual bool raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape) const; - /// Return the number of bytes used by the collision shape virtual size_t getSizeInBytes() const; - public : - - // -------------------- Methods -------------------- // - /// Constructor CylinderShape(float radius, float height, float margin = OBJECT_MARGIN); - /// Destructor virtual ~CylinderShape(); - /// Return the radius float getRadius() const; - /// Return the height float getHeight() const; - /// Set the scaling vector of the collision shape virtual void setLocalScaling(const vec3& scaling); - /// Return the local bounds of the shape in x, y and z directions virtual void getLocalBounds(vec3& min, vec3& max) const; - /// Return the local inertia tensor of the collision shape virtual void computeLocalInertiaTensor(etk::Matrix3x3& tensor, float mass) const; }; -// Return the radius -/** - * @return Radius of the cylinder (in meters) - */ -float CylinderShape::getRadius() const { - return mRadius; -} - -// Return the height -/** - * @return Height of the cylinder (in meters) - */ -float CylinderShape::getHeight() const { - return m_halfHeight + m_halfHeight; -} - -// Set the scaling vector of the collision shape -void CylinderShape::setLocalScaling(const vec3& scaling) { - - m_halfHeight = (m_halfHeight / m_scaling.y()) * scaling.y(); - mRadius = (mRadius / m_scaling.x()) * scaling.x(); - - CollisionShape::setLocalScaling(scaling); -} - -// Return the number of bytes used by the collision shape -size_t CylinderShape::getSizeInBytes() const { - return sizeof(CylinderShape); -} - -// Return the local bounds of the shape in x, y and z directions -/** - * @param min The minimum bounds of the shape in local-space coordinates - * @param max The maximum bounds of the shape in local-space coordinates - */ -void CylinderShape::getLocalBounds(vec3& min, vec3& max) const { - // Maximum bounds - max.setX(mRadius + m_margin); - max.setY(m_halfHeight + m_margin); - max.setZ(max.x()); - // Minimum bounds - min.setX(-max.x()); - min.setY(-max.y()); - min.setZ(min.x()); -} - -// Return the local inertia tensor of the cylinder -/** - * @param[out] tensor The 3x3 inertia tensor matrix of the shape in local-space - * coordinates - * @param mass Mass to use to compute the inertia tensor of the collision shape - */ -void CylinderShape::computeLocalInertiaTensor(etk::Matrix3x3& tensor, float mass) const { - float height = float(2.0) * m_halfHeight; - float diag = (1.0f / float(12.0)) * mass * (3 * mRadius * mRadius + height * height); - tensor.setValue(diag, 0.0, 0.0, 0.0, - 0.5f * mass * mRadius * mRadius, 0.0, - 0.0, 0.0, diag); -} - -// Return true if a point is inside the collision shape -bool CylinderShape::testPointInside(const vec3& localPoint, ProxyShape* proxyShape) const{ - return ( (localPoint.x() * localPoint.x() + localPoint.z() * localPoint.z()) < mRadius * mRadius - && localPoint.y() < m_halfHeight - && localPoint.y() > -m_halfHeight); -} - } diff --git a/ephysics/collision/shapes/HeightFieldShape.cpp b/ephysics/collision/shapes/HeightFieldShape.cpp index 73e767f..916240e 100644 --- a/ephysics/collision/shapes/HeightFieldShape.cpp +++ b/ephysics/collision/shapes/HeightFieldShape.cpp @@ -246,3 +246,61 @@ void TriangleOverlapCallback::testTriangle(const vec3* trianglePoints) { m_isHit = true; } } + +// Return the number of rows in the height field +int32_t HeightFieldShape::getNbRows() const { + return m_numberRows; +} + +// Return the number of columns in the height field +int32_t HeightFieldShape::getNbColumns() const { + return m_numberColumns; +} + +// Return the type of height value in the height field +HeightFieldShape::HeightDataType HeightFieldShape::getHeightDataType() const { + return m_heightDataType; +} + +// Return the number of bytes used by the collision shape +size_t HeightFieldShape::getSizeInBytes() const { + return sizeof(HeightFieldShape); +} + +// Set the local scaling vector of the collision shape +void HeightFieldShape::setLocalScaling(const vec3& scaling) { + CollisionShape::setLocalScaling(scaling); +} + +// Return the height of a given (x,y) point in the height field +float HeightFieldShape::getHeightAt(int32_t x, int32_t y) const { + + switch(m_heightDataType) { + case HEIGHT_FLOAT_TYPE : return ((float*)m_heightFieldData)[y * m_numberColumns + x]; + case HEIGHT_DOUBLE_TYPE : return ((double*)m_heightFieldData)[y * m_numberColumns + x]; + case HEIGHT_INT_TYPE : return ((int32_t*)m_heightFieldData)[y * m_numberColumns + x] * m_integerHeightScale; + default: assert(false); return 0; + } +} + +// Return the closest inside int32_teger grid value of a given floating grid value +int32_t HeightFieldShape::computeIntegerGridValue(float value) const { + return (value < 0.0f) ? value - 0.5f : value + float(0.5); +} + +// Return the local inertia tensor +/** + * @param[out] tensor The 3x3 inertia tensor matrix of the shape in local-space + * coordinates + * @param mass Mass to use to compute the inertia tensor of the collision shape + */ +void HeightFieldShape::computeLocalInertiaTensor(etk::Matrix3x3& tensor, float mass) const { + + // Default inertia tensor + // Note that this is not very realistic for a concave triangle mesh. + // However, in most cases, it will only be used static bodies and therefore, + // the inertia tensor is not used. + tensor.setValue(mass, 0, 0, + 0, mass, 0, + 0, 0, mass); +} diff --git a/ephysics/collision/shapes/HeightFieldShape.hpp b/ephysics/collision/shapes/HeightFieldShape.hpp index 5083d74..cca2858 100644 --- a/ephysics/collision/shapes/HeightFieldShape.hpp +++ b/ephysics/collision/shapes/HeightFieldShape.hpp @@ -5,7 +5,6 @@ */ #pragma once -// Libraries #include #include #include @@ -43,188 +42,89 @@ namespace ephysics { }; -/** - * @brief This class represents a static height field that can be used to represent - * a terrain. The height field is made of a grid with rows and columns with a - * height value at each grid point. Note that the height values are not copied int32_to the shape - * but are shared instead. The height values can be of type int32_teger, float or double. - * When creating a HeightFieldShape, you need to specify the minimum and maximum height value of - * your height field. Note that the HeightFieldShape will be re-centered based on its AABB. It means - * that for instance, if the minimum height value is -200 and the maximum value is 400, the final - * minimum height of the field in the simulation will be -300 and the maximum height will be 300. - */ -class HeightFieldShape : public ConcaveShape { - - public: - - /// Data type for the height data of the height field - enum HeightDataType {HEIGHT_FLOAT_TYPE, HEIGHT_DOUBLE_TYPE, HEIGHT_INT_TYPE}; - - protected: - - // -------------------- Attributes -------------------- // - - /// Number of columns in the grid of the height field - int32_t m_numberColumns; - - /// Number of rows in the grid of the height field - int32_t m_numberRows; - - /// Height field width - float m_width; - - /// Height field length - float m_length; - - /// Minimum height of the height field - float m_minHeight; - - /// Maximum height of the height field - float m_maxHeight; - - /// Up axis direction (0 => x, 1 => y, 2 => z) - int32_t m_upAxis; - - /// Height values scale for height field with int32_teger height values - float m_integerHeightScale; - - /// Data type of the height values - HeightDataType m_heightDataType; - - /// Array of data with all the height values of the height field - const void* m_heightFieldData; - - /// Local AABB of the height field (without scaling) - AABB m_AABB; - - // -------------------- Methods -------------------- // - - /// Private copy-constructor - HeightFieldShape(const HeightFieldShape& shape); - - /// Private assignment operator - HeightFieldShape& operator=(const HeightFieldShape& shape); - - /// Raycast method with feedback information - virtual bool raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape) const; - - /// Return the number of bytes used by the collision shape - virtual size_t getSizeInBytes() const; - - /// Insert all the triangles int32_to the dynamic AABB tree - void initBVHTree(); - - /// Return the three vertices coordinates (in the array outTriangleVertices) of a triangle - /// given the start vertex index pointer of the triangle. - void getTriangleVerticesWithIndexPointer(int32_t subPart, int32_t triangleIndex, - vec3* outTriangleVertices) const; - - /// Return the vertex (local-coordinates) of the height field at a given (x,y) position - vec3 getVertexAt(int32_t x, int32_t y) const; - - /// Return the height of a given (x,y) point in the height field - float getHeightAt(int32_t x, int32_t y) const; - - /// Return the closest inside int32_teger grid value of a given floating grid value - int32_t computeIntegerGridValue(float value) const; - - /// Compute the min/max grid coords corresponding to the int32_tersection of the AABB of the height field and the AABB to collide - void computeMinMaxGridCoordinates(int32_t* minCoords, int32_t* maxCoords, const AABB& aabbToCollide) const; - - public: - - /// Constructor - HeightFieldShape(int32_t nbGridColumns, int32_t nbGridRows, float minHeight, float maxHeight, - const void* heightFieldData, HeightDataType dataType, - int32_t upAxis = 1, float int32_tegerHeightScale = 1.0f); - - /// Destructor - ~HeightFieldShape(); - - /// Return the number of rows in the height field - int32_t getNbRows() const; - - /// Return the number of columns in the height field - int32_t getNbColumns() const; - - /// Return the type of height value in the height field - HeightDataType getHeightDataType() const; - - /// Return the local bounds of the shape in x, y and z directions. - virtual void getLocalBounds(vec3& min, vec3& max) const; - - /// Set the local scaling vector of the collision shape - virtual void setLocalScaling(const vec3& scaling); - - /// Return the local inertia tensor of the collision shape - virtual void computeLocalInertiaTensor(etk::Matrix3x3& tensor, float mass) const; - - /// Use a callback method on all triangles of the concave shape inside a given AABB - virtual void testAllTriangles(TriangleCallback& callback, const AABB& localAABB) const; - - // ---------- Friendship ----------- // - - friend class ConvexTriangleAABBOverlapCallback; - friend class ConcaveMeshRaycastCallback; -}; - -// Return the number of rows in the height field -int32_t HeightFieldShape::getNbRows() const { - return m_numberRows; -} - -// Return the number of columns in the height field -int32_t HeightFieldShape::getNbColumns() const { - return m_numberColumns; -} - -// Return the type of height value in the height field -HeightFieldShape::HeightDataType HeightFieldShape::getHeightDataType() const { - return m_heightDataType; -} - -// Return the number of bytes used by the collision shape -size_t HeightFieldShape::getSizeInBytes() const { - return sizeof(HeightFieldShape); -} - -// Set the local scaling vector of the collision shape -void HeightFieldShape::setLocalScaling(const vec3& scaling) { - CollisionShape::setLocalScaling(scaling); -} - -// Return the height of a given (x,y) point in the height field -float HeightFieldShape::getHeightAt(int32_t x, int32_t y) const { - - switch(m_heightDataType) { - case HEIGHT_FLOAT_TYPE : return ((float*)m_heightFieldData)[y * m_numberColumns + x]; - case HEIGHT_DOUBLE_TYPE : return ((double*)m_heightFieldData)[y * m_numberColumns + x]; - case HEIGHT_INT_TYPE : return ((int32_t*)m_heightFieldData)[y * m_numberColumns + x] * m_integerHeightScale; - default: assert(false); return 0; - } -} - -// Return the closest inside int32_teger grid value of a given floating grid value -int32_t HeightFieldShape::computeIntegerGridValue(float value) const { - return (value < 0.0f) ? value - 0.5f : value + float(0.5); -} - -// Return the local inertia tensor -/** - * @param[out] tensor The 3x3 inertia tensor matrix of the shape in local-space - * coordinates - * @param mass Mass to use to compute the inertia tensor of the collision shape - */ -void HeightFieldShape::computeLocalInertiaTensor(etk::Matrix3x3& tensor, float mass) const { - - // Default inertia tensor - // Note that this is not very realistic for a concave triangle mesh. - // However, in most cases, it will only be used static bodies and therefore, - // the inertia tensor is not used. - tensor.setValue(mass, 0, 0, - 0, mass, 0, - 0, 0, mass); -} + /** + * @brief This class represents a static height field that can be used to represent + * a terrain. The height field is made of a grid with rows and columns with a + * height value at each grid point. Note that the height values are not copied int32_to the shape + * but are shared instead. The height values can be of type int32_teger, float or double. + * When creating a HeightFieldShape, you need to specify the minimum and maximum height value of + * your height field. Note that the HeightFieldShape will be re-centered based on its AABB. It means + * that for instance, if the minimum height value is -200 and the maximum value is 400, the final + * minimum height of the field in the simulation will be -300 and the maximum height will be 300. + */ + class HeightFieldShape : public ConcaveShape { + public: + /** + * @brief Data type for the height data of the height field + */ + enum HeightDataType { + HEIGHT_FLOAT_TYPE, + HEIGHT_DOUBLE_TYPE, + HEIGHT_INT_TYPE + }; + protected: + int32_t m_numberColumns; //!< Number of columns in the grid of the height field + int32_t m_numberRows; //!< Number of rows in the grid of the height field + float m_width; //!< Height field width + float m_length; //!< Height field length + float m_minHeight; //!< Minimum height of the height field + float m_maxHeight; //!< Maximum height of the height field + int32_t m_upAxis; //!< Up axis direction (0 => x, 1 => y, 2 => z) + float m_integerHeightScale; //!< Height values scale for height field with int32_teger height values + HeightDataType m_heightDataType; //!< Data type of the height values + const void* m_heightFieldData; //!< Array of data with all the height values of the height field + AABB m_AABB; //!< Local AABB of the height field (without scaling) + /// Private copy-constructor + HeightFieldShape(const HeightFieldShape& _shape); + /// Private assignment operator + HeightFieldShape& operator=(const HeightFieldShape& _shape); + /// Raycast method with feedback information + virtual bool raycast(const Ray& _ray, RaycastInfo& _raycastInfo, ProxyShape* _proxyShape) const; + /// Return the number of bytes used by the collision shape + virtual size_t getSizeInBytes() const; + /// Insert all the triangles int32_to the dynamic AABB tree + void initBVHTree(); + /// Return the three vertices coordinates (in the array outTriangleVertices) of a triangle + /// given the start vertex index pointer of the triangle. + void getTriangleVerticesWithIndexPointer(int32_t _subPart, + int32_t _triangleIndex, + vec3* _outTriangleVertices) const; + /// Return the vertex (local-coordinates) of the height field at a given (x,y) position + vec3 getVertexAt(int32_t _x, int32_t _y) const; + /// Return the height of a given (x,y) point in the height field + float getHeightAt(int32_t _x, int32_t _y) const; + /// Return the closest inside int32_teger grid value of a given floating grid value + int32_t computeIntegerGridValue(float _value) const; + /// Compute the min/max grid coords corresponding to the int32_tersection of the AABB of the height field and the AABB to collide + void computeMinMaxGridCoordinates(int32_t* _minCoords, int32_t* _maxCoords, const AABB& _aabbToCollide) const; + public: + /// Constructor + HeightFieldShape(int32_t _nbGridColumns, + int32_t _nbGridRows, + float _minHeight, + float _maxHeight, + const void* _heightFieldData, + HeightDataType _dataType, + int32_t _upAxis = 1, float _integerHeightScale = 1.0f); + /// Destructor + ~HeightFieldShape(); + /// Return the number of rows in the height field + int32_t getNbRows() const; + /// Return the number of columns in the height field + int32_t getNbColumns() const; + /// Return the type of height value in the height field + HeightDataType getHeightDataType() const; + /// Return the local bounds of the shape in x, y and z directions. + virtual void getLocalBounds(vec3& _min, vec3& _max) const; + /// Set the local scaling vector of the collision shape + virtual void setLocalScaling(const vec3& _scaling); + /// Return the local inertia tensor of the collision shape + virtual void computeLocalInertiaTensor(etk::Matrix3x3& _tensor, float _mass) const; + /// Use a callback method on all triangles of the concave shape inside a given AABB + virtual void testAllTriangles(TriangleCallback& _callback, const AABB& _localAABB) const; + friend class ConvexTriangleAABBOverlapCallback; + friend class ConcaveMeshRaycastCallback; + }; } diff --git a/ephysics/collision/shapes/SphereShape.cpp b/ephysics/collision/shapes/SphereShape.cpp index 9a45172..f5ba5a2 100644 --- a/ephysics/collision/shapes/SphereShape.cpp +++ b/ephysics/collision/shapes/SphereShape.cpp @@ -25,6 +25,39 @@ SphereShape::~SphereShape() { } + +void SphereShape::setLocalScaling(const vec3& _scaling) { + m_margin = (m_margin / m_scaling.x()) * _scaling.x(); + CollisionShape::setLocalScaling(_scaling); +} + +void SphereShape::computeLocalInertiaTensor(etk::Matrix3x3& _tensor, float _mass) const { + float diag = 0.4f * _mass * m_margin * m_margin; + _tensor.setValue(diag, 0.0f, 0.0f, + 0.0f, diag, 0.0f, + 0.0f, 0.0f, diag); +} + +void SphereShape::computeAABB(AABB& _aabb, const etk::Transform3D& _transform) const { + // Get the local extents in x,y and z direction + vec3 extents(m_margin, m_margin, m_margin); + // Update the AABB with the new minimum and maximum coordinates + _aabb.setMin(_transform.getPosition() - extents); + _aabb.setMax(_transform.getPosition() + extents); +} + +void SphereShape::getLocalBounds(vec3& _min, vec3& _max) const { + // Maximum bounds + _max.setX(m_margin); + _max.setY(m_margin); + _max.setZ(m_margin); + // Minimum bounds + _min.setX(-m_margin); + _min.setY(_min.x()); + _min.setZ(_min.x()); +} + + // Raycast method with feedback information bool SphereShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape) const { diff --git a/ephysics/collision/shapes/SphereShape.hpp b/ephysics/collision/shapes/SphereShape.hpp index 8a0e43d..898a40b 100644 --- a/ephysics/collision/shapes/SphereShape.hpp +++ b/ephysics/collision/shapes/SphereShape.hpp @@ -11,127 +11,78 @@ namespace ephysics { -/** - * @brief Represents a sphere collision shape that is centered - * at the origin and defined by its radius. This collision shape does not - * have an explicit object margin distance. The margin is implicitly the - * radius of the sphere. Therefore, no need to specify an object margin - * for a sphere shape. - */ -class SphereShape : public ConvexShape { - protected : - SphereShape(const SphereShape& shape); - SphereShape& operator=(const SphereShape& shape) = delete; - /// Return a local support point in a given direction without the object margin - virtual vec3 getLocalSupportPointWithoutMargin(const vec3& direction, - void** cachedCollisionData) const; - /// Return true if a point is inside the collision shape - virtual bool testPointInside(const vec3& localPoint, ProxyShape* proxyShape) const; - /// Raycast method with feedback information - virtual bool raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape) const; - /// Return the number of bytes used by the collision shape - virtual size_t getSizeInBytes() const; - public : - /// Constructor - SphereShape(float radius); - /// Destructor - virtual ~SphereShape(); - /// Return the radius of the sphere - float getRadius() const; - /// Set the scaling vector of the collision shape - virtual void setLocalScaling(const vec3& scaling); - /// Return the local bounds of the shape in x, y and z directions. - virtual void getLocalBounds(vec3& min, vec3& max) const; - /// Return the local inertia tensor of the collision shape - virtual void computeLocalInertiaTensor(etk::Matrix3x3& tensor, float mass) const; - /// Update the AABB of a body using its collision shape - virtual void computeAABB(AABB& aabb, const etk::Transform3D& transform) const; -}; - -// Get the radius of the sphere -/** - * @brief - * @return Radius of the sphere (in meters) - */ -float SphereShape::getRadius() const { - return m_margin; -} - - * @brief -// Set the scaling vector of the collision shape -void SphereShape::setLocalScaling(const vec3& scaling) { - - m_margin = (m_margin / m_scaling.x()) * scaling.x(); - - CollisionShape::setLocalScaling(scaling); -} - - * @brief -// Return the number of bytes used by the collision shape -size_t SphereShape::getSizeInBytes() const { - return sizeof(SphereShape); -} - -/** - * @brief Get a local support point in a given direction without the object margin - * @param[in] _direction - * @param[in] _cachedCollisionData - * @return the center of the sphere (the radius is taken int32_to account in the object margin) - */ -vec3 SphereShape::getLocalSupportPointWithoutMargin(const vec3& _direction, void** _cachedCollisionData) const { - return vec3(0.0, 0.0, 0.0); -} - -/** - * @brief Get the local bounds of the shape in x, y and z directions. - * This method is used to compute the AABB of the box - * @param _min The minimum bounds of the shape in local-space coordinates - * @param _max The maximum bounds of the shape in local-space coordinates - */ -void SphereShape::getLocalBounds(vec3& _min, vec3& _max) const { - // Maximum bounds - _max.setX(m_margin); - _max.setY(m_margin); - _max.setZ(m_margin); - // Minimum bounds - _min.setX(-m_margin); - _min.setY(_min.x()); - _min.setZ(_min.x()); -} - -/** - * @brief Compute the local inertia tensor of the sphere - * @param[out] _tensor The 3x3 inertia tensor matrix of the shape in local-space coordinates - * @param[in] _mass Mass to use to compute the inertia tensor of the collision shape - */ -void SphereShape::computeLocalInertiaTensor(etk::Matrix3x3& _tensor, float _mass) const { - float _diag = 0.4f * _mass * m_margin * m_margin; - tensor.setValue(_diag, 0.0f, 0.0f, - 0.0f, _diag, 0.0f, - 0.0f, 0.0f, _diag); -} - -/** - * @brief Update the AABB of a body using its collision shape - * @param[out] _aabb The axis-aligned bounding box (AABB) of the collision shape computed in world-space coordinates - * @param[in] _transform etk::Transform3D used to compute the AABB of the collision shape - */ -void SphereShape::computeAABB(AABB& _aabb, const etk::Transform3D& _transform) const { - // Get the local extents in x,y and z direction - vec3 extents(m_margin, m_margin, m_margin); - // Update the AABB with the new minimum and maximum coordinates - _aabb.setMin(_transform.getPosition() - extents); - _aabb.setMax(_transform.getPosition() + extents); -} - -/** - * @brief Test if a point is inside a shape - * @param[in] _localPoint Point to check - * @param[in] _proxyShape Shape to check - * @return true if a point is inside the collision shape - */ -bool SphereShape::testPointInside(const vec3& _localPoint, ProxyShape* _proxyShape) const { - return (_localPoint.length2() < m_margin * m_margin); -} + /** + * @brief Represents a sphere collision shape that is centered + * at the origin and defined by its radius. This collision shape does not + * have an explicit object margin distance. The margin is implicitly the + * radius of the sphere. Therefore, no need to specify an object margin + * for a sphere shape. + */ + class SphereShape : public ConvexShape { + protected : + SphereShape(const SphereShape& _shape); + SphereShape& operator=(const SphereShape& _shape) = delete; + /** + * @brief Get a local support point in a given direction without the object margin + * @param[in] _direction + * @param[in] _cachedCollisionData + * @return the center of the sphere (the radius is taken int32_to account in the object margin) + */ + vec3 getLocalSupportPointWithoutMargin(const vec3& _direction, void** _cachedCollisionData) const { + return vec3(0.0, 0.0, 0.0); + } + /** + * @brief Test if a point is inside a shape + * @param[in] _localPoint Point to check + * @param[in] _proxyShape Shape to check + * @return true if a point is inside the collision shape + */ + bool testPointInside(const vec3& _localPoint, ProxyShape* _proxyShape) const { + return (_localPoint.length2() < m_margin * m_margin); + } + /// Raycast method with feedback information + virtual bool raycast(const Ray& _ray, RaycastInfo& _raycastInfo, ProxyShape* _proxyShape) const; + /** + * @brief Return the number of bytes used by the collision shape + */ + virtual size_t getSizeInBytes() const { + return sizeof(SphereShape); + } + public : + /// Constructor + SphereShape(float _radius); + /// Destructor + virtual ~SphereShape(); + /** + * @brief Get the radius of the sphere + * @return Radius of the sphere (in meters) + */ + float getRadius() const { + return m_margin; + } + /** + * @brief Set the scaling vector of the collision shape + */ + virtual void setLocalScaling(const vec3& _scaling); + /** + * @brief Get the local bounds of the shape in x, y and z directions. + * This method is used to compute the AABB of the box + * @param _min The minimum bounds of the shape in local-space coordinates + * @param _max The maximum bounds of the shape in local-space coordinates + */ + virtual void getLocalBounds(vec3& _min, vec3& _max) const; + /** + * @brief Compute the local inertia tensor of the sphere + * @param[out] _tensor The 3x3 inertia tensor matrix of the shape in local-space coordinates + * @param[in] _mass Mass to use to compute the inertia tensor of the collision shape + */ + virtual void computeLocalInertiaTensor(etk::Matrix3x3& _tensor, float _mass) const; + /** + * @brief Update the AABB of a body using its collision shape + * @param[out] _aabb The axis-aligned bounding box (AABB) of the collision shape computed in world-space coordinates + * @param[in] _transform etk::Transform3D used to compute the AABB of the collision shape + */ + virtual void computeAABB(AABB& _aabb, const etk::Transform3D& _transform) const; + }; } diff --git a/ephysics/collision/shapes/TriangleShape.cpp b/ephysics/collision/shapes/TriangleShape.cpp index 2a46894..dd3de02 100644 --- a/ephysics/collision/shapes/TriangleShape.cpp +++ b/ephysics/collision/shapes/TriangleShape.cpp @@ -106,3 +106,99 @@ bool TriangleShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape return true; } + +// Return the number of bytes used by the collision shape +size_t TriangleShape::getSizeInBytes() const { + return sizeof(TriangleShape); +} + +// Return a local support point in a given direction without the object margin +vec3 TriangleShape::getLocalSupportPointWithoutMargin(const vec3& direction, + void** cachedCollisionData) const { + vec3 dotProducts(direction.dot(m_points[0]), direction.dot(m_points[1]), direction.dot(m_points[2])); + return m_points[dotProducts.getMaxAxis()]; +} + +// Return the local bounds of the shape in x, y and z directions. +// This method is used to compute the AABB of the box +/** + * @param min The minimum bounds of the shape in local-space coordinates + * @param max The maximum bounds of the shape in local-space coordinates + */ +void TriangleShape::getLocalBounds(vec3& min, vec3& max) const { + + const vec3 xAxis(m_points[0].x(), m_points[1].x(), m_points[2].x()); + const vec3 yAxis(m_points[0].y(), m_points[1].y(), m_points[2].y()); + const vec3 zAxis(m_points[0].z(), m_points[1].z(), m_points[2].z()); + min.setValue(xAxis.getMin(), yAxis.getMin(), zAxis.getMin()); + max.setValue(xAxis.getMax(), yAxis.getMax(), zAxis.getMax()); + + min -= vec3(m_margin, m_margin, m_margin); + max += vec3(m_margin, m_margin, m_margin); +} + +// Set the local scaling vector of the collision shape +void TriangleShape::setLocalScaling(const vec3& scaling) { + + m_points[0] = (m_points[0] / m_scaling) * scaling; + m_points[1] = (m_points[1] / m_scaling) * scaling; + m_points[2] = (m_points[2] / m_scaling) * scaling; + + CollisionShape::setLocalScaling(scaling); +} + +// Return the local inertia tensor of the triangle shape +/** + * @param[out] tensor The 3x3 inertia tensor matrix of the shape in local-space + * coordinates + * @param mass Mass to use to compute the inertia tensor of the collision shape + */ +void TriangleShape::computeLocalInertiaTensor(etk::Matrix3x3& tensor, float mass) const { + tensor.setZero(); +} + +// Update the AABB of a body using its collision shape +/** + * @param[out] aabb The axis-aligned bounding box (AABB) of the collision shape + * computed in world-space coordinates + * @param transform etk::Transform3D used to compute the AABB of the collision shape + */ +void TriangleShape::computeAABB(AABB& aabb, const etk::Transform3D& transform) const { + + const vec3 worldPoint1 = transform * m_points[0]; + const vec3 worldPoint2 = transform * m_points[1]; + const vec3 worldPoint3 = transform * m_points[2]; + + const vec3 xAxis(worldPoint1.x(), worldPoint2.x(), worldPoint3.x()); + const vec3 yAxis(worldPoint1.y(), worldPoint2.y(), worldPoint3.y()); + const vec3 zAxis(worldPoint1.z(), worldPoint2.z(), worldPoint3.z()); + aabb.setMin(vec3(xAxis.getMin(), yAxis.getMin(), zAxis.getMin())); + aabb.setMax(vec3(xAxis.getMax(), yAxis.getMax(), zAxis.getMax())); +} + +// Return true if a point is inside the collision shape +bool TriangleShape::testPointInside(const vec3& localPoint, ProxyShape* proxyShape) const { + return false; +} + +// Return the raycast test type (front, back, front-back) +TriangleRaycastSide TriangleShape::getRaycastTestType() const { + return m_raycastTestType; +} + +// Set the raycast test type (front, back, front-back) +/** + * @param testType Raycast test type for the triangle (front, back, front-back) + */ +void TriangleShape::setRaycastTestType(TriangleRaycastSide testType) { + m_raycastTestType = testType; +} + +// Return the coordinates of a given vertex of the triangle +/** + * @param index Index (0 to 2) of a vertex of the triangle + */ +vec3 TriangleShape::getVertex(int32_t index) const { + assert(index >= 0 && index < 3); + return m_points[index]; +} \ No newline at end of file diff --git a/ephysics/collision/shapes/TriangleShape.hpp b/ephysics/collision/shapes/TriangleShape.hpp index 0ec4251..bec1bcc 100644 --- a/ephysics/collision/shapes/TriangleShape.hpp +++ b/ephysics/collision/shapes/TriangleShape.hpp @@ -9,170 +9,58 @@ #include namespace ephysics { - -/** - * @brief Raycast test side for the triangle - */ -enum TriangleRaycastSide { - FRONT, //!< Raycast against front triangle - BACK, //!< Raycast against back triangle - FRONT_AND_BACK //!< Raycast against front and back triangle -}; - -/** - * This class represents a triangle collision shape that is centered - * at the origin and defined three points. - */ -class TriangleShape : public ConvexShape { - protected: - vec3 m_points[3]; //!< Three points of the triangle - TriangleRaycastSide m_raycastTestType; //!< Raycast test type for the triangle (front, back, front-back) - /// Private copy-constructor - TriangleShape(const TriangleShape& shape); - - /// Private assignment operator - TriangleShape& operator=(const TriangleShape& shape); - - /// Return a local support point in a given direction without the object margin - virtual vec3 getLocalSupportPointWithoutMargin(const vec3& direction, - void** cachedCollisionData) const; - - /// Return true if a point is inside the collision shape - virtual bool testPointInside(const vec3& localPoint, ProxyShape* proxyShape) const; - - /// Raycast method with feedback information - virtual bool raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape) const; - - /// Return the number of bytes used by the collision shape - virtual size_t getSizeInBytes() const; - - public: - /// Constructor - TriangleShape(const vec3& point1, const vec3& point2, const vec3& point3, - float margin = OBJECT_MARGIN); - - /// Destructor - virtual ~TriangleShape(); - - /// Return the local bounds of the shape in x, y and z directions. - virtual void getLocalBounds(vec3& min, vec3& max) const; - - /// Set the local scaling vector of the collision shape - virtual void setLocalScaling(const vec3& scaling); - - /// Return the local inertia tensor of the collision shape - virtual void computeLocalInertiaTensor(etk::Matrix3x3& tensor, float mass) const; - - /// Update the AABB of a body using its collision shape - virtual void computeAABB(AABB& aabb, const etk::Transform3D& transform) const; - - /// Return the raycast test type (front, back, front-back) - TriangleRaycastSide getRaycastTestType() const; - - // Set the raycast test type (front, back, front-back) - void setRaycastTestType(TriangleRaycastSide testType); - - /// Return the coordinates of a given vertex of the triangle - vec3 getVertex(int32_t index) const; - friend class ConcaveMeshRaycastCallback; - friend class TriangleOverlapCallback; -}; - -// Return the number of bytes used by the collision shape -size_t TriangleShape::getSizeInBytes() const { - return sizeof(TriangleShape); -} - -// Return a local support point in a given direction without the object margin -vec3 TriangleShape::getLocalSupportPointWithoutMargin(const vec3& direction, - void** cachedCollisionData) const { - vec3 dotProducts(direction.dot(m_points[0]), direction.dot(m_points[1]), direction.dot(m_points[2])); - return m_points[dotProducts.getMaxAxis()]; -} - -// Return the local bounds of the shape in x, y and z directions. -// This method is used to compute the AABB of the box -/** - * @param min The minimum bounds of the shape in local-space coordinates - * @param max The maximum bounds of the shape in local-space coordinates - */ -void TriangleShape::getLocalBounds(vec3& min, vec3& max) const { - - const vec3 xAxis(m_points[0].x(), m_points[1].x(), m_points[2].x()); - const vec3 yAxis(m_points[0].y(), m_points[1].y(), m_points[2].y()); - const vec3 zAxis(m_points[0].z(), m_points[1].z(), m_points[2].z()); - min.setValue(xAxis.getMin(), yAxis.getMin(), zAxis.getMin()); - max.setValue(xAxis.getMax(), yAxis.getMax(), zAxis.getMax()); - - min -= vec3(m_margin, m_margin, m_margin); - max += vec3(m_margin, m_margin, m_margin); -} - -// Set the local scaling vector of the collision shape -void TriangleShape::setLocalScaling(const vec3& scaling) { - - m_points[0] = (m_points[0] / m_scaling) * scaling; - m_points[1] = (m_points[1] / m_scaling) * scaling; - m_points[2] = (m_points[2] / m_scaling) * scaling; - - CollisionShape::setLocalScaling(scaling); -} - -// Return the local inertia tensor of the triangle shape -/** - * @param[out] tensor The 3x3 inertia tensor matrix of the shape in local-space - * coordinates - * @param mass Mass to use to compute the inertia tensor of the collision shape - */ -void TriangleShape::computeLocalInertiaTensor(etk::Matrix3x3& tensor, float mass) const { - tensor.setZero(); -} - -// Update the AABB of a body using its collision shape -/** - * @param[out] aabb The axis-aligned bounding box (AABB) of the collision shape - * computed in world-space coordinates - * @param transform etk::Transform3D used to compute the AABB of the collision shape - */ -void TriangleShape::computeAABB(AABB& aabb, const etk::Transform3D& transform) const { - - const vec3 worldPoint1 = transform * m_points[0]; - const vec3 worldPoint2 = transform * m_points[1]; - const vec3 worldPoint3 = transform * m_points[2]; - - const vec3 xAxis(worldPoint1.x(), worldPoint2.x(), worldPoint3.x()); - const vec3 yAxis(worldPoint1.y(), worldPoint2.y(), worldPoint3.y()); - const vec3 zAxis(worldPoint1.z(), worldPoint2.z(), worldPoint3.z()); - aabb.setMin(vec3(xAxis.getMin(), yAxis.getMin(), zAxis.getMin())); - aabb.setMax(vec3(xAxis.getMax(), yAxis.getMax(), zAxis.getMax())); -} - -// Return true if a point is inside the collision shape -bool TriangleShape::testPointInside(const vec3& localPoint, ProxyShape* proxyShape) const { - return false; -} - -// Return the raycast test type (front, back, front-back) -TriangleRaycastSide TriangleShape::getRaycastTestType() const { - return m_raycastTestType; -} - -// Set the raycast test type (front, back, front-back) -/** - * @param testType Raycast test type for the triangle (front, back, front-back) - */ -void TriangleShape::setRaycastTestType(TriangleRaycastSide testType) { - m_raycastTestType = testType; -} - -// Return the coordinates of a given vertex of the triangle -/** - * @param index Index (0 to 2) of a vertex of the triangle - */ -vec3 TriangleShape::getVertex(int32_t index) const { - assert(index >= 0 && index < 3); - return m_points[index]; -} - + /** + * @brief Raycast test side for the triangle + */ + enum TriangleRaycastSide { + FRONT, //!< Raycast against front triangle + BACK, //!< Raycast against back triangle + FRONT_AND_BACK //!< Raycast against front and back triangle + }; + + /** + * This class represents a triangle collision shape that is centered + * at the origin and defined three points. + */ + class TriangleShape : public ConvexShape { + protected: + vec3 m_points[3]; //!< Three points of the triangle + TriangleRaycastSide m_raycastTestType; //!< Raycast test type for the triangle (front, back, front-back) + /// Private copy-constructor + TriangleShape(const TriangleShape& shape); + /// Private assignment operator + TriangleShape& operator=(const TriangleShape& shape); + /// Return a local support point in a given direction without the object margin + virtual vec3 getLocalSupportPointWithoutMargin(const vec3& direction, + void** cachedCollisionData) const; + /// Return true if a point is inside the collision shape + virtual bool testPointInside(const vec3& localPoint, ProxyShape* proxyShape) const; + /// Raycast method with feedback information + virtual bool raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape) const; + /// Return the number of bytes used by the collision shape + virtual size_t getSizeInBytes() const; + public: + /// Constructor + TriangleShape(const vec3& point1, const vec3& point2, const vec3& point3, + float margin = OBJECT_MARGIN); + /// Destructor + virtual ~TriangleShape(); + /// Return the local bounds of the shape in x, y and z directions. + virtual void getLocalBounds(vec3& min, vec3& max) const; + /// Set the local scaling vector of the collision shape + virtual void setLocalScaling(const vec3& scaling); + /// Return the local inertia tensor of the collision shape + virtual void computeLocalInertiaTensor(etk::Matrix3x3& tensor, float mass) const; + /// Update the AABB of a body using its collision shape + virtual void computeAABB(AABB& aabb, const etk::Transform3D& transform) const; + /// Return the raycast test type (front, back, front-back) + TriangleRaycastSide getRaycastTestType() const; + // Set the raycast test type (front, back, front-back) + void setRaycastTestType(TriangleRaycastSide testType); + /// Return the coordinates of a given vertex of the triangle + vec3 getVertex(int32_t index) const; + friend class ConcaveMeshRaycastCallback; + friend class TriangleOverlapCallback; + }; } diff --git a/ephysics/constraint/ContactPoint.cpp b/ephysics/constraint/ContactPoint.cpp index c555c34..3b86380 100644 --- a/ephysics/constraint/ContactPoint.cpp +++ b/ephysics/constraint/ContactPoint.cpp @@ -37,3 +37,133 @@ ContactPoint::ContactPoint(const ContactPointInfo& contactInfo) ContactPoint::~ContactPoint() { } + +// Return the reference to the body 1 +CollisionBody* ContactPoint::getBody1() const { + return m_body1; +} + +// Return the reference to the body 2 +CollisionBody* ContactPoint::getBody2() const { + return m_body2; +} + +// Return the normal vector of the contact +vec3 ContactPoint::getNormal() const { + return m_normal; +} + +// Set the penetration depth of the contact +void ContactPoint::setPenetrationDepth(float penetrationDepth) { + this->m_penetrationDepth = penetrationDepth; +} + +// Return the contact point on body 1 +vec3 ContactPoint::getLocalPointOnBody1() const { + return m_localPointOnBody1; +} + +// Return the contact point on body 2 +vec3 ContactPoint::getLocalPointOnBody2() const { + return m_localPointOnBody2; +} + +// Return the contact world point on body 1 +vec3 ContactPoint::getWorldPointOnBody1() const { + return m_worldPointOnBody1; +} + +// Return the contact world point on body 2 +vec3 ContactPoint::getWorldPointOnBody2() const { + return m_worldPointOnBody2; +} + +// Return the cached penetration impulse +float ContactPoint::getPenetrationImpulse() const { + return m_penetrationImpulse; +} + +// Return the cached first friction impulse +float ContactPoint::getFrictionImpulse1() const { + return m_frictionImpulse1; +} + +// Return the cached second friction impulse +float ContactPoint::getFrictionImpulse2() const { + return m_frictionImpulse2; +} + +// Return the cached rolling resistance impulse +vec3 ContactPoint::getRollingResistanceImpulse() const { + return m_rollingResistanceImpulse; +} + +// Set the cached penetration impulse +void ContactPoint::setPenetrationImpulse(float impulse) { + m_penetrationImpulse = impulse; +} + +// Set the first cached friction impulse +void ContactPoint::setFrictionImpulse1(float impulse) { + m_frictionImpulse1 = impulse; +} + +// Set the second cached friction impulse +void ContactPoint::setFrictionImpulse2(float impulse) { + m_frictionImpulse2 = impulse; +} + +// Set the cached rolling resistance impulse +void ContactPoint::setRollingResistanceImpulse(const vec3& impulse) { + m_rollingResistanceImpulse = impulse; +} + +// Set the contact world point on body 1 +void ContactPoint::setWorldPointOnBody1(const vec3& worldPoint) { + m_worldPointOnBody1 = worldPoint; +} + +// Set the contact world point on body 2 +void ContactPoint::setWorldPointOnBody2(const vec3& worldPoint) { + m_worldPointOnBody2 = worldPoint; +} + +// Return true if the contact is a resting contact +bool ContactPoint::getIsRestingContact() const { + return m_isRestingContact; +} + +// Set the m_isRestingContact variable +void ContactPoint::setIsRestingContact(bool isRestingContact) { + m_isRestingContact = isRestingContact; +} + +// Get the first friction vector +vec3 ContactPoint::getFrictionVector1() const { + return m_frictionVectors[0]; +} + +// Set the first friction vector +void ContactPoint::setFrictionVector1(const vec3& frictionVector1) { + m_frictionVectors[0] = frictionVector1; +} + +// Get the second friction vector +vec3 ContactPoint::getFrictionvec2() const { + return m_frictionVectors[1]; +} + +// Set the second friction vector +void ContactPoint::setFrictionvec2(const vec3& frictionvec2) { + m_frictionVectors[1] = frictionvec2; +} + +// Return the penetration depth of the contact +float ContactPoint::getPenetrationDepth() const { + return m_penetrationDepth; +} + +// Return the number of bytes used by the contact point +size_t ContactPoint::getSizeInBytes() const { + return sizeof(ContactPoint); +} diff --git a/ephysics/constraint/ContactPoint.hpp b/ephysics/constraint/ContactPoint.hpp index 902975c..8141073 100644 --- a/ephysics/constraint/ContactPoint.hpp +++ b/ephysics/constraint/ContactPoint.hpp @@ -21,23 +21,14 @@ namespace ephysics { */ struct ContactPointInfo { public: - /// First proxy shape of the contact - ProxyShape* shape1; - /// Second proxy shape of the contact - ProxyShape* shape2; - /// First collision shape - const CollisionShape* collisionShape1; - /// Second collision shape - const CollisionShape* collisionShape2; - /// Normalized normal vector of the collision contact in world space - vec3 normal; - /// Penetration depth of the contact - float penetrationDepth; - /// Contact point of body 1 in local space of body 1 - vec3 localPoint1; - /// Contact point of body 2 in local space of body 2 - vec3 localPoint2; - /// Constructor + ProxyShape* shape1; //!< First proxy shape of the contact + ProxyShape* shape2; //!< Second proxy shape of the contact + const CollisionShape* collisionShape1; //!< First collision shape + const CollisionShape* collisionShape2; //!< Second collision shape + vec3 normal; //!< Normalized normal vector of the collision contact in world space + float penetrationDepth; //!< Penetration depth of the contact + vec3 localPoint1; //!< Contact point of body 1 in local space of body 1 + vec3 localPoint2; //!< Contact point of body 2 in local space of body 2 ContactPointInfo(ProxyShape* _proxyShape1, ProxyShape* _proxyShape2, const CollisionShape* _collShape1, @@ -141,134 +132,4 @@ namespace ephysics { size_t getSizeInBytes() const; }; -// Return the reference to the body 1 -CollisionBody* ContactPoint::getBody1() const { - return m_body1; -} - -// Return the reference to the body 2 -CollisionBody* ContactPoint::getBody2() const { - return m_body2; -} - -// Return the normal vector of the contact -vec3 ContactPoint::getNormal() const { - return m_normal; -} - -// Set the penetration depth of the contact -void ContactPoint::setPenetrationDepth(float penetrationDepth) { - this->m_penetrationDepth = penetrationDepth; -} - -// Return the contact point on body 1 -vec3 ContactPoint::getLocalPointOnBody1() const { - return m_localPointOnBody1; -} - -// Return the contact point on body 2 -vec3 ContactPoint::getLocalPointOnBody2() const { - return m_localPointOnBody2; -} - -// Return the contact world point on body 1 -vec3 ContactPoint::getWorldPointOnBody1() const { - return m_worldPointOnBody1; -} - -// Return the contact world point on body 2 -vec3 ContactPoint::getWorldPointOnBody2() const { - return m_worldPointOnBody2; -} - -// Return the cached penetration impulse -float ContactPoint::getPenetrationImpulse() const { - return m_penetrationImpulse; -} - -// Return the cached first friction impulse -float ContactPoint::getFrictionImpulse1() const { - return m_frictionImpulse1; -} - -// Return the cached second friction impulse -float ContactPoint::getFrictionImpulse2() const { - return m_frictionImpulse2; -} - -// Return the cached rolling resistance impulse -vec3 ContactPoint::getRollingResistanceImpulse() const { - return m_rollingResistanceImpulse; -} - -// Set the cached penetration impulse -void ContactPoint::setPenetrationImpulse(float impulse) { - m_penetrationImpulse = impulse; -} - -// Set the first cached friction impulse -void ContactPoint::setFrictionImpulse1(float impulse) { - m_frictionImpulse1 = impulse; -} - -// Set the second cached friction impulse -void ContactPoint::setFrictionImpulse2(float impulse) { - m_frictionImpulse2 = impulse; -} - -// Set the cached rolling resistance impulse -void ContactPoint::setRollingResistanceImpulse(const vec3& impulse) { - m_rollingResistanceImpulse = impulse; -} - -// Set the contact world point on body 1 -void ContactPoint::setWorldPointOnBody1(const vec3& worldPoint) { - m_worldPointOnBody1 = worldPoint; -} - -// Set the contact world point on body 2 -void ContactPoint::setWorldPointOnBody2(const vec3& worldPoint) { - m_worldPointOnBody2 = worldPoint; -} - -// Return true if the contact is a resting contact -bool ContactPoint::getIsRestingContact() const { - return m_isRestingContact; -} - -// Set the m_isRestingContact variable -void ContactPoint::setIsRestingContact(bool isRestingContact) { - m_isRestingContact = isRestingContact; -} - -// Get the first friction vector -vec3 ContactPoint::getFrictionVector1() const { - return m_frictionVectors[0]; -} - -// Set the first friction vector -void ContactPoint::setFrictionVector1(const vec3& frictionVector1) { - m_frictionVectors[0] = frictionVector1; -} - -// Get the second friction vector -vec3 ContactPoint::getFrictionvec2() const { - return m_frictionVectors[1]; -} - -// Set the second friction vector -void ContactPoint::setFrictionvec2(const vec3& frictionvec2) { - m_frictionVectors[1] = frictionvec2; -} - -// Return the penetration depth of the contact -float ContactPoint::getPenetrationDepth() const { - return m_penetrationDepth; -} - -// Return the number of bytes used by the contact point -size_t ContactPoint::getSizeInBytes() const { - return sizeof(ContactPoint); -} - } diff --git a/ephysics/constraint/FixedJoint.hpp b/ephysics/constraint/FixedJoint.hpp index 0c317d0..7f313bb 100644 --- a/ephysics/constraint/FixedJoint.hpp +++ b/ephysics/constraint/FixedJoint.hpp @@ -11,125 +11,69 @@ namespace ephysics { -// Structure FixedJointInfo -/** - * This structure is used to gather the information needed to create a fixed - * joint. This structure will be used to create the actual fixed joint. - */ -struct FixedJointInfo : public JointInfo { - - public : - - // -------------------- Attributes -------------------- // - - /// Anchor point (in world-space coordinates) - vec3 m_anchorPointWorldSpace; - - /// Constructor - /** - * @param rigidBody1 The first body of the joint - * @param rigidBody2 The second body of the joint - * @param initAnchorPointWorldSpace The initial anchor point of the joint in - * world-space coordinates - */ - FixedJointInfo(RigidBody* rigidBody1, RigidBody* rigidBody2, - const vec3& initAnchorPointWorldSpace) - : JointInfo(rigidBody1, rigidBody2, FIXEDJOINT), - m_anchorPointWorldSpace(initAnchorPointWorldSpace){} -}; - -// Class FixedJoint -/** - * This class represents a fixed joint that is used to forbid any translation or rotation - * between two bodies. - */ -class FixedJoint : public Joint { - - private : - - // -------------------- Constants -------------------- // - - // Beta value for the bias factor of position correction - static const float BETA; - - // -------------------- Attributes -------------------- // - - /// Anchor point of body 1 (in local-space coordinates of body 1) - vec3 m_localAnchorPointBody1; - - /// Anchor point of body 2 (in local-space coordinates of body 2) - vec3 m_localAnchorPointBody2; - - /// Vector from center of body 2 to anchor point in world-space - vec3 m_r1World; - - /// Vector from center of body 2 to anchor point in world-space - vec3 m_r2World; - - /// Inertia tensor of body 1 (in world-space coordinates) - etk::Matrix3x3 m_i1; - - /// Inertia tensor of body 2 (in world-space coordinates) - etk::Matrix3x3 m_i2; - - /// Accumulated impulse for the 3 translation constraints - vec3 m_impulseTranslation; - - /// Accumulate impulse for the 3 rotation constraints - vec3 m_impulseRotation; - - /// Inverse mass matrix K=JM^-1J^-t of the 3 translation constraints (3x3 matrix) - etk::Matrix3x3 m_inverseMassMatrixTranslation; - - /// Inverse mass matrix K=JM^-1J^-t of the 3 rotation constraints (3x3 matrix) - etk::Matrix3x3 m_inverseMassMatrixRotation; - - /// Bias vector for the 3 translation constraints - vec3 m_biasTranslation; - - /// Bias vector for the 3 rotation constraints - vec3 m_biasRotation; - - /// Inverse of the initial orientation difference between the two bodies - etk::Quaternion m_initOrientationDifferenceInv; - - // -------------------- Methods -------------------- // - - /// Private copy-constructor - FixedJoint(const FixedJoint& constraint); - - /// Private assignment operator - FixedJoint& operator=(const FixedJoint& constraint); - - /// Return the number of bytes used by the joint - virtual size_t getSizeInBytes() const; - - /// Initialize before solving the constraint - virtual void initBeforeSolve(const ConstraintSolverData& constraintSolverData); - - /// Warm start the constraint (apply the previous impulse at the beginning of the step) - virtual void warmstart(const ConstraintSolverData& constraintSolverData); - - /// Solve the velocity constraint - virtual void solveVelocityConstraint(const ConstraintSolverData& constraintSolverData); - - /// Solve the position constraint (for position error correction) - virtual void solvePositionConstraint(const ConstraintSolverData& constraintSolverData); - - public : - - // -------------------- Methods -------------------- // - - /// Constructor - FixedJoint(const FixedJointInfo& jointInfo); - - /// Destructor - virtual ~FixedJoint(); -}; - -// Return the number of bytes used by the joint -size_t FixedJoint::getSizeInBytes() const { - return sizeof(FixedJoint); + /** + * This structure is used to gather the information needed to create a fixed + * joint. This structure will be used to create the actual fixed joint. + */ + struct FixedJointInfo : public JointInfo { + public : + vec3 m_anchorPointWorldSpace; //!< Anchor point (in world-space coordinates) + /** + * @breif Contructor + * @param _rigidBody1 The first body of the joint + * @param _rigidBody2 The second body of the joint + * @param _initAnchorPointWorldSpace The initial anchor point of the joint in world-space coordinates + */ + FixedJointInfo(RigidBody* _rigidBody1, + RigidBody* _rigidBody2, + const vec3& _initAnchorPointWorldSpace): + JointInfo(_rigidBody1, _rigidBody2, FIXEDJOINT), + m_anchorPointWorldSpace(_initAnchorPointWorldSpace){ + + } + }; + + /** + * @breif It represents a fixed joint that is used to forbid any translation or rotation + * between two bodies. + */ + class FixedJoint : public Joint { + private : + static const float BETA; //!< Beta value for the bias factor of position correction + vec3 m_localAnchorPointBody1; //!< Anchor point of body 1 (in local-space coordinates of body 1) + vec3 m_localAnchorPointBody2; //!< Anchor point of body 2 (in local-space coordinates of body 2) + vec3 m_r1World; //!< Vector from center of body 2 to anchor point in world-space + vec3 m_r2World; //!< Vector from center of body 2 to anchor point in world-space + etk::Matrix3x3 m_i1; //!< Inertia tensor of body 1 (in world-space coordinates) + etk::Matrix3x3 m_i2; //!< Inertia tensor of body 2 (in world-space coordinates) + vec3 m_impulseTranslation; //!< Accumulated impulse for the 3 translation constraints + vec3 m_impulseRotation; //!< Accumulate impulse for the 3 rotation constraints + etk::Matrix3x3 m_inverseMassMatrixTranslation; //!< Inverse mass matrix K=JM^-1J^-t of the 3 translation constraints (3x3 matrix) + etk::Matrix3x3 m_inverseMassMatrixRotation; //!< Inverse mass matrix K=JM^-1J^-t of the 3 rotation constraints (3x3 matrix) + vec3 m_biasTranslation; //!< Bias vector for the 3 translation constraints + vec3 m_biasRotation; //!< Bias vector for the 3 rotation constraints + etk::Quaternion m_initOrientationDifferenceInv; //!< Inverse of the initial orientation difference between the two bodies + /// Private copy-constructor + FixedJoint(const FixedJoint& constraint); + /// Private assignment operator + FixedJoint& operator=(const FixedJoint& constraint); + /// Return the number of bytes used by the joint + virtual size_t getSizeInBytes() const override { + return sizeof(FixedJoint); + } + /// Initialize before solving the constraint + virtual void initBeforeSolve(const ConstraintSolverData& constraintSolverData); + /// Warm start the constraint (apply the previous impulse at the beginning of the step) + virtual void warmstart(const ConstraintSolverData& constraintSolverData); + /// Solve the velocity constraint + virtual void solveVelocityConstraint(const ConstraintSolverData& constraintSolverData); + /// Solve the position constraint (for position error correction) + virtual void solvePositionConstraint(const ConstraintSolverData& constraintSolverData); + public : + /// Constructor + FixedJoint(const FixedJointInfo& jointInfo); + /// Destructor + virtual ~FixedJoint(); + }; } -} diff --git a/ephysics/constraint/HingeJoint.cpp b/ephysics/constraint/HingeJoint.cpp index cb71213..838c1e9 100644 --- a/ephysics/constraint/HingeJoint.cpp +++ b/ephysics/constraint/HingeJoint.cpp @@ -18,13 +18,13 @@ const float HingeJoint::BETA = float(0.2); HingeJoint::HingeJoint(const HingeJointInfo& jointInfo) : Joint(jointInfo), m_impulseTranslation(0, 0, 0), m_impulseRotation(0, 0), m_impulseLowerLimit(0), m_impulseUpperLimit(0), m_impulseMotor(0), - mIsLimitEnabled(jointInfo.isLimitEnabled), mIsMotorEnabled(jointInfo.isMotorEnabled), - mLowerLimit(jointInfo.minAngleLimit), mUpperLimit(jointInfo.maxAngleLimit), - mIsLowerLimitViolated(false), mIsUpperLimitViolated(false), - mMotorSpeed(jointInfo.motorSpeed), mMaxMotorTorque(jointInfo.maxMotorTorque) { + m_isLimitEnabled(jointInfo.isLimitEnabled), m_isMotorEnabled(jointInfo.isMotorEnabled), + m_lowerLimit(jointInfo.minAngleLimit), m_upperLimit(jointInfo.maxAngleLimit), + m_isLowerLimitViolated(false), m_isUpperLimitViolated(false), + m_motorSpeed(jointInfo.motorSpeed), m_maxMotorTorque(jointInfo.maxMotorTorque) { - assert(mLowerLimit <= 0 && mLowerLimit >= -2.0 * PI); - assert(mUpperLimit >= 0 && mUpperLimit <= 2.0 * PI); + assert(m_lowerLimit <= 0 && m_lowerLimit >= -2.0 * PI); + assert(m_upperLimit >= 0 && m_upperLimit <= 2.0 * PI); // Compute the local-space anchor point for each body etk::Transform3D transform1 = m_body1->getTransform(); @@ -33,10 +33,10 @@ HingeJoint::HingeJoint(const HingeJointInfo& jointInfo) m_localAnchorPointBody2 = transform2.getInverse() * jointInfo.m_anchorPointWorldSpace; // Compute the local-space hinge axis - mHingeLocalAxisBody1 = transform1.getOrientation().getInverse() * jointInfo.rotationAxisWorld; - mHingeLocalAxisBody2 = transform2.getOrientation().getInverse() * jointInfo.rotationAxisWorld; - mHingeLocalAxisBody1.normalize(); - mHingeLocalAxisBody2.normalize(); + m_hingeLocalAxisBody1 = transform1.getOrientation().getInverse() * jointInfo.rotationAxisWorld; + m_hingeLocalAxisBody2 = transform2.getOrientation().getInverse() * jointInfo.rotationAxisWorld; + m_hingeLocalAxisBody1.normalize(); + m_hingeLocalAxisBody2.normalize(); // Compute the inverse of the initial orientation difference between the two bodies m_initOrientationDifferenceInv = transform2.getOrientation() * @@ -75,28 +75,28 @@ void HingeJoint::initBeforeSolve(const ConstraintSolverData& constraintSolverDat float hingeAngle = computeCurrentHingeAngle(orientationBody1, orientationBody2); // Check if the limit constraints are violated or not - float lowerLimitError = hingeAngle - mLowerLimit; - float upperLimitError = mUpperLimit - hingeAngle; - bool oldIsLowerLimitViolated = mIsLowerLimitViolated; - mIsLowerLimitViolated = lowerLimitError <= 0; - if (mIsLowerLimitViolated != oldIsLowerLimitViolated) { + float lowerLimitError = hingeAngle - m_lowerLimit; + float upperLimitError = m_upperLimit - hingeAngle; + bool oldIsLowerLimitViolated = m_isLowerLimitViolated; + m_isLowerLimitViolated = lowerLimitError <= 0; + if (m_isLowerLimitViolated != oldIsLowerLimitViolated) { m_impulseLowerLimit = 0.0; } - bool oldIsUpperLimitViolated = mIsUpperLimitViolated; - mIsUpperLimitViolated = upperLimitError <= 0; - if (mIsUpperLimitViolated != oldIsUpperLimitViolated) { + bool oldIsUpperLimitViolated = m_isUpperLimitViolated; + m_isUpperLimitViolated = upperLimitError <= 0; + if (m_isUpperLimitViolated != oldIsUpperLimitViolated) { m_impulseUpperLimit = 0.0; } // Compute vectors needed in the Jacobian - mA1 = orientationBody1 * mHingeLocalAxisBody1; - vec3 a2 = orientationBody2 * mHingeLocalAxisBody2; + mA1 = orientationBody1 * m_hingeLocalAxisBody1; + vec3 a2 = orientationBody2 * m_hingeLocalAxisBody2; mA1.normalize(); a2.normalize(); const vec3 b2 = a2.getOrthoVector(); const vec3 c2 = a2.cross(b2); - mB2CrossA1 = b2.cross(mA1); - mC2CrossA1 = c2.cross(mA1); + m_b2CrossA1 = b2.cross(mA1); + m_c2CrossA1 = c2.cross(mA1); // Compute the corresponding skew-symmetric matrices etk::Matrix3x3 skewSymmetricMatrixU1= etk::Matrix3x3::computeSkewSymmetricMatrixForCrossProduct(m_r1World); @@ -115,25 +115,25 @@ void HingeJoint::initBeforeSolve(const ConstraintSolverData& constraintSolverDat } // Compute the bias "b" of the translation constraints - mBTranslation.setZero(); + m_bTranslation.setZero(); float biasFactor = (BETA / constraintSolverData.timeStep); if (m_positionCorrectionTechnique == BAUMGARTE_JOINTS) { - mBTranslation = biasFactor * (x2 + m_r2World - x1 - m_r1World); + m_bTranslation = biasFactor * (x2 + m_r2World - x1 - m_r1World); } // Compute the inverse mass matrix K=JM^-1J^t for the 2 rotation constraints (2x2 matrix) - vec3 I1B2CrossA1 = m_i1 * mB2CrossA1; - vec3 I1C2CrossA1 = m_i1 * mC2CrossA1; - vec3 I2B2CrossA1 = m_i2 * mB2CrossA1; - vec3 I2C2CrossA1 = m_i2 * mC2CrossA1; - const float el11 = mB2CrossA1.dot(I1B2CrossA1) + - mB2CrossA1.dot(I2B2CrossA1); - const float el12 = mB2CrossA1.dot(I1C2CrossA1) + - mB2CrossA1.dot(I2C2CrossA1); - const float el21 = mC2CrossA1.dot(I1B2CrossA1) + - mC2CrossA1.dot(I2B2CrossA1); - const float el22 = mC2CrossA1.dot(I1C2CrossA1) + - mC2CrossA1.dot(I2C2CrossA1); + vec3 I1B2CrossA1 = m_i1 * m_b2CrossA1; + vec3 I1C2CrossA1 = m_i1 * m_c2CrossA1; + vec3 I2B2CrossA1 = m_i2 * m_b2CrossA1; + vec3 I2C2CrossA1 = m_i2 * m_c2CrossA1; + const float el11 = m_b2CrossA1.dot(I1B2CrossA1) + + m_b2CrossA1.dot(I2B2CrossA1); + const float el12 = m_b2CrossA1.dot(I1C2CrossA1) + + m_b2CrossA1.dot(I2C2CrossA1); + const float el21 = m_c2CrossA1.dot(I1B2CrossA1) + + m_c2CrossA1.dot(I2B2CrossA1); + const float el22 = m_c2CrossA1.dot(I1C2CrossA1) + + m_c2CrossA1.dot(I2C2CrossA1); const etk::Matrix2x2 matrixKRotation(el11, el12, el21, el22); m_inverseMassMatrixRotation.setZero(); if (m_body1->getType() == DYNAMIC || m_body2->getType() == DYNAMIC) { @@ -141,9 +141,9 @@ void HingeJoint::initBeforeSolve(const ConstraintSolverData& constraintSolverDat } // Compute the bias "b" of the rotation constraints - mBRotation.setZero(); + m_bRotation.setZero(); if (m_positionCorrectionTechnique == BAUMGARTE_JOINTS) { - mBRotation = biasFactor * vec2(mA1.dot(b2), mA1.dot(c2)); + m_bRotation = biasFactor * vec2(mA1.dot(b2), mA1.dot(c2)); } // If warm-starting is not enabled @@ -158,25 +158,25 @@ void HingeJoint::initBeforeSolve(const ConstraintSolverData& constraintSolverDat } // If the motor or limits are enabled - if (mIsMotorEnabled || (mIsLimitEnabled && (mIsLowerLimitViolated || mIsUpperLimitViolated))) { + if (m_isMotorEnabled || (m_isLimitEnabled && (m_isLowerLimitViolated || m_isUpperLimitViolated))) { // Compute the inverse of the mass matrix K=JM^-1J^t for the limits and motor (1x1 matrix) m_inverseMassMatrixLimitMotor = mA1.dot(m_i1 * mA1) + mA1.dot(m_i2 * mA1); m_inverseMassMatrixLimitMotor = (m_inverseMassMatrixLimitMotor > 0.0) ? 1.0f / m_inverseMassMatrixLimitMotor : 0.0f; - if (mIsLimitEnabled) { + if (m_isLimitEnabled) { // Compute the bias "b" of the lower limit constraint - mBLowerLimit = 0.0; + m_bLowerLimit = 0.0; if (m_positionCorrectionTechnique == BAUMGARTE_JOINTS) { - mBLowerLimit = biasFactor * lowerLimitError; + m_bLowerLimit = biasFactor * lowerLimitError; } // Compute the bias "b" of the upper limit constraint - mBUpperLimit = 0.0; + m_bUpperLimit = 0.0; if (m_positionCorrectionTechnique == BAUMGARTE_JOINTS) { - mBUpperLimit = biasFactor * upperLimitError; + m_bUpperLimit = biasFactor * upperLimitError; } } } @@ -196,7 +196,7 @@ void HingeJoint::warmstart(const ConstraintSolverData& constraintSolverData) { const float inverseMassBody2 = m_body2->m_massInverse; // Compute the impulse P=J^T * lambda for the 2 rotation constraints - vec3 rotationImpulse = -mB2CrossA1 * m_impulseRotation.x() - mC2CrossA1 * m_impulseRotation.y(); + vec3 rotationImpulse = -m_b2CrossA1 * m_impulseRotation.x() - m_c2CrossA1 * m_impulseRotation.y(); // Compute the impulse P=J^T * lambda for the lower and upper limits constraints const vec3 limitsImpulse = (m_impulseUpperLimit - m_impulseLowerLimit) * mA1; @@ -258,7 +258,7 @@ void HingeJoint::solveVelocityConstraint(const ConstraintSolverData& constraintS // Compute the Lagrange multiplier lambda const vec3 deltaLambdaTranslation = m_inverseMassMatrixTranslation * - (-JvTranslation - mBTranslation); + (-JvTranslation - m_bTranslation); m_impulseTranslation += deltaLambdaTranslation; // Compute the impulse P=J^T * lambda of body 1 @@ -279,39 +279,39 @@ void HingeJoint::solveVelocityConstraint(const ConstraintSolverData& constraintS // --------------- Rotation Constraints --------------- // // Compute J*v for the 2 rotation constraints - const vec2 JvRotation(-mB2CrossA1.dot(w1) + mB2CrossA1.dot(w2), - -mC2CrossA1.dot(w1) + mC2CrossA1.dot(w2)); + const vec2 JvRotation(-m_b2CrossA1.dot(w1) + m_b2CrossA1.dot(w2), + -m_c2CrossA1.dot(w1) + m_c2CrossA1.dot(w2)); // Compute the Lagrange multiplier lambda for the 2 rotation constraints - vec2 deltaLambdaRotation = m_inverseMassMatrixRotation * (-JvRotation - mBRotation); + vec2 deltaLambdaRotation = m_inverseMassMatrixRotation * (-JvRotation - m_bRotation); m_impulseRotation += deltaLambdaRotation; // Compute the impulse P=J^T * lambda for the 2 rotation constraints of body 1 - angularImpulseBody1 = -mB2CrossA1 * deltaLambdaRotation.x() - - mC2CrossA1 * deltaLambdaRotation.y(); + angularImpulseBody1 = -m_b2CrossA1 * deltaLambdaRotation.x() - + m_c2CrossA1 * deltaLambdaRotation.y(); // Apply the impulse to the body 1 w1 += m_i1 * angularImpulseBody1; // Compute the impulse P=J^T * lambda for the 2 rotation constraints of body 2 - angularImpulseBody2 = mB2CrossA1 * deltaLambdaRotation.x() + - mC2CrossA1 * deltaLambdaRotation.y(); + angularImpulseBody2 = m_b2CrossA1 * deltaLambdaRotation.x() + + m_c2CrossA1 * deltaLambdaRotation.y(); // Apply the impulse to the body 2 w2 += m_i2 * angularImpulseBody2; // --------------- Limits Constraints --------------- // - if (mIsLimitEnabled) { + if (m_isLimitEnabled) { // If the lower limit is violated - if (mIsLowerLimitViolated) { + if (m_isLowerLimitViolated) { // Compute J*v for the lower limit constraint const float JvLowerLimit = (w2 - w1).dot(mA1); // Compute the Lagrange multiplier lambda for the lower limit constraint - float deltaLambdaLower = m_inverseMassMatrixLimitMotor * (-JvLowerLimit -mBLowerLimit); + float deltaLambdaLower = m_inverseMassMatrixLimitMotor * (-JvLowerLimit -m_bLowerLimit); float lambdaTemp = m_impulseLowerLimit; m_impulseLowerLimit = std::max(m_impulseLowerLimit + deltaLambdaLower, 0.0f); deltaLambdaLower = m_impulseLowerLimit - lambdaTemp; @@ -330,13 +330,13 @@ void HingeJoint::solveVelocityConstraint(const ConstraintSolverData& constraintS } // If the upper limit is violated - if (mIsUpperLimitViolated) { + if (m_isUpperLimitViolated) { // Compute J*v for the upper limit constraint const float JvUpperLimit = -(w2 - w1).dot(mA1); // Compute the Lagrange multiplier lambda for the upper limit constraint - float deltaLambdaUpper = m_inverseMassMatrixLimitMotor * (-JvUpperLimit -mBUpperLimit); + float deltaLambdaUpper = m_inverseMassMatrixLimitMotor * (-JvUpperLimit -m_bUpperLimit); float lambdaTemp = m_impulseUpperLimit; m_impulseUpperLimit = std::max(m_impulseUpperLimit + deltaLambdaUpper, 0.0f); deltaLambdaUpper = m_impulseUpperLimit - lambdaTemp; @@ -358,14 +358,14 @@ void HingeJoint::solveVelocityConstraint(const ConstraintSolverData& constraintS // --------------- Motor --------------- // // If the motor is enabled - if (mIsMotorEnabled) { + if (m_isMotorEnabled) { // Compute J*v for the motor const float JvMotor = mA1.dot(w1 - w2); // Compute the Lagrange multiplier lambda for the motor - const float maxMotorImpulse = mMaxMotorTorque * constraintSolverData.timeStep; - float deltaLambdaMotor = m_inverseMassMatrixLimitMotor * (-JvMotor - mMotorSpeed); + const float maxMotorImpulse = m_maxMotorTorque * constraintSolverData.timeStep; + float deltaLambdaMotor = m_inverseMassMatrixLimitMotor * (-JvMotor - m_motorSpeed); float lambdaTemp = m_impulseMotor; m_impulseMotor = clamp(m_impulseMotor + deltaLambdaMotor, -maxMotorImpulse, maxMotorImpulse); deltaLambdaMotor = m_impulseMotor - lambdaTemp; @@ -413,20 +413,20 @@ void HingeJoint::solvePositionConstraint(const ConstraintSolverData& constraintS float hingeAngle = computeCurrentHingeAngle(q1, q2); // Check if the limit constraints are violated or not - float lowerLimitError = hingeAngle - mLowerLimit; - float upperLimitError = mUpperLimit - hingeAngle; - mIsLowerLimitViolated = lowerLimitError <= 0; - mIsUpperLimitViolated = upperLimitError <= 0; + float lowerLimitError = hingeAngle - m_lowerLimit; + float upperLimitError = m_upperLimit - hingeAngle; + m_isLowerLimitViolated = lowerLimitError <= 0; + m_isUpperLimitViolated = upperLimitError <= 0; // Compute vectors needed in the Jacobian - mA1 = q1 * mHingeLocalAxisBody1; - vec3 a2 = q2 * mHingeLocalAxisBody2; + mA1 = q1 * m_hingeLocalAxisBody1; + vec3 a2 = q2 * m_hingeLocalAxisBody2; mA1.normalize(); a2.normalize(); const vec3 b2 = a2.getOrthoVector(); const vec3 c2 = a2.cross(b2); - mB2CrossA1 = b2.cross(mA1); - mC2CrossA1 = c2.cross(mA1); + m_b2CrossA1 = b2.cross(mA1); + m_c2CrossA1 = c2.cross(mA1); // Compute the corresponding skew-symmetric matrices etk::Matrix3x3 skewSymmetricMatrixU1= etk::Matrix3x3::computeSkewSymmetricMatrixForCrossProduct(m_r1World); @@ -480,18 +480,18 @@ void HingeJoint::solvePositionConstraint(const ConstraintSolverData& constraintS // --------------- Rotation Constraints --------------- // // Compute the inverse mass matrix K=JM^-1J^t for the 2 rotation constraints (2x2 matrix) - vec3 I1B2CrossA1 = m_i1 * mB2CrossA1; - vec3 I1C2CrossA1 = m_i1 * mC2CrossA1; - vec3 I2B2CrossA1 = m_i2 * mB2CrossA1; - vec3 I2C2CrossA1 = m_i2 * mC2CrossA1; - const float el11 = mB2CrossA1.dot(I1B2CrossA1) + - mB2CrossA1.dot(I2B2CrossA1); - const float el12 = mB2CrossA1.dot(I1C2CrossA1) + - mB2CrossA1.dot(I2C2CrossA1); - const float el21 = mC2CrossA1.dot(I1B2CrossA1) + - mC2CrossA1.dot(I2B2CrossA1); - const float el22 = mC2CrossA1.dot(I1C2CrossA1) + - mC2CrossA1.dot(I2C2CrossA1); + vec3 I1B2CrossA1 = m_i1 * m_b2CrossA1; + vec3 I1C2CrossA1 = m_i1 * m_c2CrossA1; + vec3 I2B2CrossA1 = m_i2 * m_b2CrossA1; + vec3 I2C2CrossA1 = m_i2 * m_c2CrossA1; + const float el11 = m_b2CrossA1.dot(I1B2CrossA1) + + m_b2CrossA1.dot(I2B2CrossA1); + const float el12 = m_b2CrossA1.dot(I1C2CrossA1) + + m_b2CrossA1.dot(I2C2CrossA1); + const float el21 = m_c2CrossA1.dot(I1B2CrossA1) + + m_c2CrossA1.dot(I2B2CrossA1); + const float el22 = m_c2CrossA1.dot(I1C2CrossA1) + + m_c2CrossA1.dot(I2C2CrossA1); const etk::Matrix2x2 matrixKRotation(el11, el12, el21, el22); m_inverseMassMatrixRotation.setZero(); if (m_body1->getType() == DYNAMIC || m_body2->getType() == DYNAMIC) { @@ -505,7 +505,7 @@ void HingeJoint::solvePositionConstraint(const ConstraintSolverData& constraintS vec2 lambdaRotation = m_inverseMassMatrixRotation * (-errorRotation); // Compute the impulse P=J^T * lambda for the 3 rotation constraints of body 1 - angularImpulseBody1 = -mB2CrossA1 * lambdaRotation.x() - mC2CrossA1 * lambdaRotation.y(); + angularImpulseBody1 = -m_b2CrossA1 * lambdaRotation.x() - m_c2CrossA1 * lambdaRotation.y(); // Compute the pseudo velocity of body 1 w1 = m_i1 * angularImpulseBody1; @@ -515,7 +515,7 @@ void HingeJoint::solvePositionConstraint(const ConstraintSolverData& constraintS q1.normalize(); // Compute the impulse of body 2 - angularImpulseBody2 = mB2CrossA1 * lambdaRotation.x() + mC2CrossA1 * lambdaRotation.y(); + angularImpulseBody2 = m_b2CrossA1 * lambdaRotation.x() + m_c2CrossA1 * lambdaRotation.y(); // Compute the pseudo velocity of body 2 w2 = m_i2 * angularImpulseBody2; @@ -526,9 +526,9 @@ void HingeJoint::solvePositionConstraint(const ConstraintSolverData& constraintS // --------------- Limits Constraints --------------- // - if (mIsLimitEnabled) { + if (m_isLimitEnabled) { - if (mIsLowerLimitViolated || mIsUpperLimitViolated) { + if (m_isLowerLimitViolated || m_isUpperLimitViolated) { // Compute the inverse of the mass matrix K=JM^-1J^t for the limits (1x1 matrix) m_inverseMassMatrixLimitMotor = mA1.dot(m_i1 * mA1) + mA1.dot(m_i2 * mA1); @@ -537,7 +537,7 @@ void HingeJoint::solvePositionConstraint(const ConstraintSolverData& constraintS } // If the lower limit is violated - if (mIsLowerLimitViolated) { + if (m_isLowerLimitViolated) { // Compute the Lagrange multiplier lambda for the lower limit constraint float lambdaLowerLimit = m_inverseMassMatrixLimitMotor * (-lowerLimitError ); @@ -564,7 +564,7 @@ void HingeJoint::solvePositionConstraint(const ConstraintSolverData& constraintS } // If the upper limit is violated - if (mIsUpperLimitViolated) { + if (m_isUpperLimitViolated) { // Compute the Lagrange multiplier lambda for the upper limit constraint float lambdaUpperLimit = m_inverseMassMatrixLimitMotor * (-upperLimitError); @@ -600,9 +600,9 @@ void HingeJoint::solvePositionConstraint(const ConstraintSolverData& constraintS */ void HingeJoint::enableLimit(bool isLimitEnabled) { - if (isLimitEnabled != mIsLimitEnabled) { + if (isLimitEnabled != m_isLimitEnabled) { - mIsLimitEnabled = isLimitEnabled; + m_isLimitEnabled = isLimitEnabled; // Reset the limits resetLimits(); @@ -616,7 +616,7 @@ void HingeJoint::enableLimit(bool isLimitEnabled) { */ void HingeJoint::enableMotor(bool isMotorEnabled) { - mIsMotorEnabled = isMotorEnabled; + m_isMotorEnabled = isMotorEnabled; m_impulseMotor = 0.0; // Wake up the two bodies of the joint @@ -630,11 +630,11 @@ void HingeJoint::enableMotor(bool isMotorEnabled) { */ void HingeJoint::setMinAngleLimit(float lowerLimit) { - assert(mLowerLimit <= 0 && mLowerLimit >= -2.0 * PI); + assert(m_lowerLimit <= 0 && m_lowerLimit >= -2.0 * PI); - if (lowerLimit != mLowerLimit) { + if (lowerLimit != m_lowerLimit) { - mLowerLimit = lowerLimit; + m_lowerLimit = lowerLimit; // Reset the limits resetLimits(); @@ -649,9 +649,9 @@ void HingeJoint::setMaxAngleLimit(float upperLimit) { assert(upperLimit >= 0 && upperLimit <= 2.0 * PI); - if (upperLimit != mUpperLimit) { + if (upperLimit != m_upperLimit) { - mUpperLimit = upperLimit; + m_upperLimit = upperLimit; // Reset the limits resetLimits(); @@ -673,9 +673,9 @@ void HingeJoint::resetLimits() { // Set the motor speed void HingeJoint::setMotorSpeed(float motorSpeed) { - if (motorSpeed != mMotorSpeed) { + if (motorSpeed != m_motorSpeed) { - mMotorSpeed = motorSpeed; + m_motorSpeed = motorSpeed; // Wake up the two bodies of the joint m_body1->setIsSleeping(false); @@ -689,10 +689,10 @@ void HingeJoint::setMotorSpeed(float motorSpeed) { */ void HingeJoint::setMaxMotorTorque(float maxMotorTorque) { - if (maxMotorTorque != mMaxMotorTorque) { + if (maxMotorTorque != m_maxMotorTorque) { - assert(mMaxMotorTorque >= 0.0); - mMaxMotorTorque = maxMotorTorque; + assert(m_maxMotorTorque >= 0.0); + m_maxMotorTorque = maxMotorTorque; // Wake up the two bodies of the joint m_body1->setIsSleeping(false); @@ -780,6 +780,70 @@ float HingeJoint::computeCurrentHingeAngle(const etk::Quaternion& orientationBod hingeAngle = computeNormalizedAngle(hingeAngle); // Compute and return the corresponding angle near one the two limits - return computeCorrespondingAngleNearLimits(hingeAngle, mLowerLimit, mUpperLimit); + return computeCorrespondingAngleNearLimits(hingeAngle, m_lowerLimit, m_upperLimit); } + +// Return true if the limits of the joint are enabled +/** + * @return True if the limits of the joint are enabled and false otherwise + */ +bool HingeJoint::isLimitEnabled() const { + return m_isLimitEnabled; +} + +// Return true if the motor of the joint is enabled +/** + * @return True if the motor of joint is enabled and false otherwise + */ +bool HingeJoint::isMotorEnabled() const { + return m_isMotorEnabled; +} + +// Return the minimum angle limit +/** + * @return The minimum limit angle of the joint (in radian) + */ +float HingeJoint::getMinAngleLimit() const { + return m_lowerLimit; +} + +// Return the maximum angle limit +/** + * @return The maximum limit angle of the joint (in radian) + */ +float HingeJoint::getMaxAngleLimit() const { + return m_upperLimit; +} + +// Return the motor speed +/** + * @return The current speed of the joint motor (in radian per second) + */ +float HingeJoint::getMotorSpeed() const { + return m_motorSpeed; +} + +// Return the maximum motor torque +/** + * @return The maximum torque of the joint motor (in Newtons) + */ +float HingeJoint::getMaxMotorTorque() const { + return m_maxMotorTorque; +} + +// Return the int32_tensity of the current torque applied for the joint motor +/** + * @param timeStep The current time step (in seconds) + * @return The int32_tensity of the current torque (in Newtons) of the joint motor + */ +float HingeJoint::getMotorTorque(float timeStep) const { + return m_impulseMotor / timeStep; +} + +// Return the number of bytes used by the joint +size_t HingeJoint::getSizeInBytes() const { + return sizeof(HingeJoint); +} + + diff --git a/ephysics/constraint/HingeJoint.hpp b/ephysics/constraint/HingeJoint.hpp index af75b77..e3c6ee1 100644 --- a/ephysics/constraint/HingeJoint.hpp +++ b/ephysics/constraint/HingeJoint.hpp @@ -5,379 +5,205 @@ */ #pragma once -// Libraries #include #include namespace ephysics { - -// Structure HingeJointInfo -/** - * This structure is used to gather the information needed to create a hinge joint. - * This structure will be used to create the actual hinge joint. - */ -struct HingeJointInfo : public JointInfo { - - public : - - // -------------------- Attributes -------------------- // - - /// Anchor point (in world-space coordinates) - vec3 m_anchorPointWorldSpace; - - /// Hinge rotation axis (in world-space coordinates) - vec3 rotationAxisWorld; - - /// True if the hinge joint limits are enabled - bool isLimitEnabled; - - /// True if the hinge joint motor is enabled - bool isMotorEnabled; - - /// Minimum allowed rotation angle (in radian) if limits are enabled. - /// The angle must be in the range [-2*pi, 0] - float minAngleLimit; - - /// Maximum allowed rotation angle (in radian) if limits are enabled. - /// The angle must be in the range [0, 2*pi] - float maxAngleLimit; - - /// Motor speed (in radian/second) - float motorSpeed; - - /// Maximum motor torque (in Newtons * meters) that can be applied to reach - /// to desired motor speed - float maxMotorTorque; - - /// Constructor without limits and without motor - /** - * @param rigidBody1 The first body of the joint - * @param rigidBody2 The second body of the joint - * @param initAnchorPointWorldSpace The initial anchor point in world-space - * coordinates - * @param initRotationAxisWorld The initial rotation axis in world-space - * coordinates - */ - HingeJointInfo(RigidBody* rigidBody1, RigidBody* rigidBody2, - const vec3& initAnchorPointWorldSpace, - const vec3& initRotationAxisWorld) - : JointInfo(rigidBody1, rigidBody2, HINGEJOINT), - m_anchorPointWorldSpace(initAnchorPointWorldSpace), - rotationAxisWorld(initRotationAxisWorld), isLimitEnabled(false), - isMotorEnabled(false), minAngleLimit(-1), maxAngleLimit(1), - motorSpeed(0), maxMotorTorque(0) {} - - /// Constructor with limits but without motor - /** - * @param rigidBody1 The first body of the joint - * @param rigidBody2 The second body of the joint - * @param initAnchorPointWorldSpace The initial anchor point in world-space coordinates - * @param initRotationAxisWorld The int32_tial rotation axis in world-space coordinates - * @param initMinAngleLimit The initial minimum limit angle (in radian) - * @param initMaxAngleLimit The initial maximum limit angle (in radian) - */ - HingeJointInfo(RigidBody* rigidBody1, RigidBody* rigidBody2, - const vec3& initAnchorPointWorldSpace, - const vec3& initRotationAxisWorld, - float initMinAngleLimit, float initMaxAngleLimit) - : JointInfo(rigidBody1, rigidBody2, HINGEJOINT), - m_anchorPointWorldSpace(initAnchorPointWorldSpace), - rotationAxisWorld(initRotationAxisWorld), isLimitEnabled(true), - isMotorEnabled(false), minAngleLimit(initMinAngleLimit), - maxAngleLimit(initMaxAngleLimit), motorSpeed(0), - maxMotorTorque(0) {} - - /// Constructor with limits and motor - /** - * @param rigidBody1 The first body of the joint - * @param rigidBody2 The second body of the joint - * @param initAnchorPointWorldSpace The initial anchor point in world-space - * @param initRotationAxisWorld The initial rotation axis in world-space - * @param initMinAngleLimit The initial minimum limit angle (in radian) - * @param initMaxAngleLimit The initial maximum limit angle (in radian) - * @param initMotorSpeed The initial motor speed of the joint (in radian per second) - * @param initMaxMotorTorque The initial maximum motor torque (in Newtons) - */ - HingeJointInfo(RigidBody* rigidBody1, RigidBody* rigidBody2, - const vec3& initAnchorPointWorldSpace, - const vec3& initRotationAxisWorld, - float initMinAngleLimit, float initMaxAngleLimit, - float initMotorSpeed, float initMaxMotorTorque) - : JointInfo(rigidBody1, rigidBody2, HINGEJOINT), - m_anchorPointWorldSpace(initAnchorPointWorldSpace), - rotationAxisWorld(initRotationAxisWorld), isLimitEnabled(true), - isMotorEnabled(false), minAngleLimit(initMinAngleLimit), - maxAngleLimit(initMaxAngleLimit), motorSpeed(initMotorSpeed), - maxMotorTorque(initMaxMotorTorque) {} -}; - -// Class HingeJoint -/** - * This class represents a hinge joint that allows arbitrary rotation - * between two bodies around a single axis. This joint has one degree of freedom. It - * can be useful to simulate doors or pendulumns. - */ -class HingeJoint : public Joint { - - private : - - // -------------------- Constants -------------------- // - - // Beta value for the bias factor of position correction - static const float BETA; - - // -------------------- Attributes -------------------- // - - /// Anchor point of body 1 (in local-space coordinates of body 1) - vec3 m_localAnchorPointBody1; - - /// Anchor point of body 2 (in local-space coordinates of body 2) - vec3 m_localAnchorPointBody2; - - /// Hinge rotation axis (in local-space coordinates of body 1) - vec3 mHingeLocalAxisBody1; - - /// Hinge rotation axis (in local-space coordiantes of body 2) - vec3 mHingeLocalAxisBody2; - - /// Inertia tensor of body 1 (in world-space coordinates) - etk::Matrix3x3 m_i1; - - /// Inertia tensor of body 2 (in world-space coordinates) - etk::Matrix3x3 m_i2; - - /// Hinge rotation axis (in world-space coordinates) computed from body 1 - vec3 mA1; - - /// Vector from center of body 2 to anchor point in world-space - vec3 m_r1World; - - /// Vector from center of body 2 to anchor point in world-space - vec3 m_r2World; - - /// Cross product of vector b2 and a1 - vec3 mB2CrossA1; - - /// Cross product of vector c2 and a1; - vec3 mC2CrossA1; - - /// Impulse for the 3 translation constraints - vec3 m_impulseTranslation; - - /// Impulse for the 2 rotation constraints - vec2 m_impulseRotation; - - /// Accumulated impulse for the lower limit constraint - float m_impulseLowerLimit; - - /// Accumulated impulse for the upper limit constraint - float m_impulseUpperLimit; - - /// Accumulated impulse for the motor constraint; - float m_impulseMotor; - - /// Inverse mass matrix K=JM^-1J^t for the 3 translation constraints - etk::Matrix3x3 m_inverseMassMatrixTranslation; - - /// Inverse mass matrix K=JM^-1J^t for the 2 rotation constraints - etk::Matrix2x2 m_inverseMassMatrixRotation; - - /// Inverse of mass matrix K=JM^-1J^t for the limits and motor constraints (1x1 matrix) - float m_inverseMassMatrixLimitMotor; - - /// Inverse of mass matrix K=JM^-1J^t for the motor - float m_inverseMassMatrixMotor; - - /// Bias vector for the error correction for the translation constraints - vec3 mBTranslation; - - /// Bias vector for the error correction for the rotation constraints - vec2 mBRotation; - - /// Bias of the lower limit constraint - float mBLowerLimit; - - /// Bias of the upper limit constraint - float mBUpperLimit; - - /// Inverse of the initial orientation difference between the bodies - etk::Quaternion m_initOrientationDifferenceInv; - - /// True if the joint limits are enabled - bool mIsLimitEnabled; - - /// True if the motor of the joint in enabled - bool mIsMotorEnabled; - - /// Lower limit (minimum allowed rotation angle in radian) - float mLowerLimit; - - /// Upper limit (maximum translation distance) - float mUpperLimit; - - /// True if the lower limit is violated - bool mIsLowerLimitViolated; - - /// True if the upper limit is violated - bool mIsUpperLimitViolated; - - /// Motor speed (in rad/s) - float mMotorSpeed; - - /// Maximum motor torque (in Newtons) that can be applied to reach to desired motor speed - float mMaxMotorTorque; - - // -------------------- Methods -------------------- // - - /// Private copy-constructor - HingeJoint(const HingeJoint& constraint); - - /// Private assignment operator - HingeJoint& operator=(const HingeJoint& constraint); - - /// Reset the limits - void resetLimits(); - - /// Given an angle in radian, this method returns the corresponding - /// angle in the range [-pi; pi] - float computeNormalizedAngle(float angle) const; - - /// Given an "inputAngle" in the range [-pi, pi], this method returns an - /// angle (modulo 2*pi) in the range [-2*pi; 2*pi] that is closest to one of the - /// two angle limits in arguments. - float computeCorrespondingAngleNearLimits(float inputAngle, float lowerLimitAngle, - float upperLimitAngle) const; - - /// Compute the current angle around the hinge axis - float computeCurrentHingeAngle(const etk::Quaternion& orientationBody1, - const etk::Quaternion& orientationBody2); - - /// Return the number of bytes used by the joint - virtual size_t getSizeInBytes() const; - - /// Initialize before solving the constraint - virtual void initBeforeSolve(const ConstraintSolverData& constraintSolverData); - - /// Warm start the constraint (apply the previous impulse at the beginning of the step) - virtual void warmstart(const ConstraintSolverData& constraintSolverData); - - /// Solve the velocity constraint - virtual void solveVelocityConstraint(const ConstraintSolverData& constraintSolverData); - - /// Solve the position constraint (for position error correction) - virtual void solvePositionConstraint(const ConstraintSolverData& constraintSolverData); - - public : - - // -------------------- Methods -------------------- // - - /// Constructor - HingeJoint(const HingeJointInfo& jointInfo); - - /// Destructor - virtual ~HingeJoint(); - - /// Return true if the limits or the joint are enabled - bool isLimitEnabled() const; - - /// Return true if the motor of the joint is enabled - bool isMotorEnabled() const; - - /// Enable/Disable the limits of the joint - void enableLimit(bool isLimitEnabled); - - /// Enable/Disable the motor of the joint - void enableMotor(bool isMotorEnabled); - - /// Return the minimum angle limit - float getMinAngleLimit() const; - - /// Set the minimum angle limit - void setMinAngleLimit(float lowerLimit); - - /// Return the maximum angle limit - float getMaxAngleLimit() const; - - /// Set the maximum angle limit - void setMaxAngleLimit(float upperLimit); - - /// Return the motor speed - float getMotorSpeed() const; - - /// Set the motor speed - void setMotorSpeed(float motorSpeed); - - /// Return the maximum motor torque - float getMaxMotorTorque() const; - - /// Set the maximum motor torque - void setMaxMotorTorque(float maxMotorTorque); - - /// Return the int32_tensity of the current torque applied for the joint motor - float getMotorTorque(float timeStep) const; -}; - -// Return true if the limits of the joint are enabled -/** - * @return True if the limits of the joint are enabled and false otherwise - */ -bool HingeJoint::isLimitEnabled() const { - return mIsLimitEnabled; -} - -// Return true if the motor of the joint is enabled -/** - * @return True if the motor of joint is enabled and false otherwise - */ -bool HingeJoint::isMotorEnabled() const { - return mIsMotorEnabled; -} - -// Return the minimum angle limit -/** - * @return The minimum limit angle of the joint (in radian) - */ -float HingeJoint::getMinAngleLimit() const { - return mLowerLimit; -} - -// Return the maximum angle limit -/** - * @return The maximum limit angle of the joint (in radian) - */ -float HingeJoint::getMaxAngleLimit() const { - return mUpperLimit; -} - -// Return the motor speed -/** - * @return The current speed of the joint motor (in radian per second) - */ -float HingeJoint::getMotorSpeed() const { - return mMotorSpeed; -} - -// Return the maximum motor torque -/** - * @return The maximum torque of the joint motor (in Newtons) - */ -float HingeJoint::getMaxMotorTorque() const { - return mMaxMotorTorque; -} - -// Return the int32_tensity of the current torque applied for the joint motor -/** - * @param timeStep The current time step (in seconds) - * @return The int32_tensity of the current torque (in Newtons) of the joint motor - */ -float HingeJoint::getMotorTorque(float timeStep) const { - return m_impulseMotor / timeStep; -} - -// Return the number of bytes used by the joint -size_t HingeJoint::getSizeInBytes() const { - return sizeof(HingeJoint); -} + /** + * @brief It is used to gather the information needed to create a hinge joint. + * This structure will be used to create the actual hinge joint. + */ + struct HingeJointInfo : public JointInfo { + public : + vec3 m_anchorPointWorldSpace; //!< Anchor point (in world-space coordinates) + vec3 rotationAxisWorld; //!< Hinge rotation axis (in world-space coordinates) + bool isLimitEnabled; //!< True if the hinge joint limits are enabled + bool isMotorEnabled; //!< True if the hinge joint motor is enabled + float minAngleLimit; //!< Minimum allowed rotation angle (in radian) if limits are enabled. The angle must be in the range [-2*pi, 0] + float maxAngleLimit; //!< Maximum allowed rotation angle (in radian) if limits are enabled. The angle must be in the range [0, 2*pi] + float motorSpeed; //!< Motor speed (in radian/second) + float maxMotorTorque; //!< Maximum motor torque (in Newtons * meters) that can be applied to reach to desired motor speed + /** + * @brief Constructor without limits and without motor + * @param[in] _rigidBody1 The first body of the joint + * @param[in] _rigidBody2 The second body of the joint + * @param[in] _initAnchorPointWorldSpace The initial anchor point in world-space coordinates + * @param[in] _initRotationAxisWorld The initial rotation axis in world-space coordinates + */ + HingeJointInfo(RigidBody* _rigidBody1, + RigidBody* _rigidBody2, + const vec3& _initAnchorPointWorldSpace, + const vec3& _initRotationAxisWorld): + JointInfo(_rigidBody1, _rigidBody2, HINGEJOINT), + m_anchorPointWorldSpace(_initAnchorPointWorldSpace), + rotationAxisWorld(_initRotationAxisWorld), + isLimitEnabled(false), + isMotorEnabled(false), + minAngleLimit(-1), + maxAngleLimit(1), + motorSpeed(0), + maxMotorTorque(0) { + + } + /** + * @brief Constructor with limits but without motor + * @param[in] _rigidBody1 The first body of the joint + * @param[in] _rigidBody2 The second body of the joint + * @param[in] _initAnchorPointWorldSpace The initial anchor point in world-space coordinates + * @param[in] _initRotationAxisWorld The int32_tial rotation axis in world-space coordinates + * @param[in] _initMinAngleLimit The initial minimum limit angle (in radian) + * @param[in] _initMaxAngleLimit The initial maximum limit angle (in radian) + */ + HingeJointInfo(RigidBody* _rigidBody1, + RigidBody* _rigidBody2, + const vec3& _initAnchorPointWorldSpace, + const vec3& _initRotationAxisWorld, + float _initMinAngleLimit, + float _initMaxAngleLimit): + JointInfo(_rigidBody1, _rigidBody2, HINGEJOINT), + m_anchorPointWorldSpace(_initAnchorPointWorldSpace), + rotationAxisWorld(_initRotationAxisWorld), + isLimitEnabled(true), + isMotorEnabled(false), + minAngleLimit(_initMinAngleLimit), + maxAngleLimit(_initMaxAngleLimit), + motorSpeed(0), + maxMotorTorque(0) { + + } + /** + * @brief Constructor with limits and motor + * @param[in] _rigidBody1 The first body of the joint + * @param[in] _rigidBody2 The second body of the joint + * @param[in] _initAnchorPointWorldSpace The initial anchor point in world-space + * @param[in] _initRotationAxisWorld The initial rotation axis in world-space + * @param[in] _initMinAngleLimit The initial minimum limit angle (in radian) + * @param[in] _initMaxAngleLimit The initial maximum limit angle (in radian) + * @param[in] _initMotorSpeed The initial motor speed of the joint (in radian per second) + * @param[in] _initMaxMotorTorque The initial maximum motor torque (in Newtons) + */ + HingeJointInfo(RigidBody* _rigidBody1, + RigidBody* _rigidBody2, + const vec3& _initAnchorPointWorldSpace, + const vec3& _initRotationAxisWorld, + float _initMinAngleLimit, + float _initMaxAngleLimit, + float _initMotorSpeed, + float _initMaxMotorTorque): + JointInfo(_rigidBody1, _rigidBody2, HINGEJOINT), + m_anchorPointWorldSpace(_initAnchorPointWorldSpace), + rotationAxisWorld(_initRotationAxisWorld), + isLimitEnabled(true), + isMotorEnabled(false), + minAngleLimit(_initMinAngleLimit), + maxAngleLimit(_initMaxAngleLimit), + motorSpeed(_initMotorSpeed), + maxMotorTorque(_initMaxMotorTorque) { + + } + }; + + /** + * @brief It represents a hinge joint that allows arbitrary rotation + * between two bodies around a single axis. This joint has one degree of freedom. It + * can be useful to simulate doors or pendulumns. + */ + class HingeJoint : public Joint { + private : + static const float BETA; //!< Beta value for the bias factor of position correction + vec3 m_localAnchorPointBody1; //!< Anchor point of body 1 (in local-space coordinates of body 1) + vec3 m_localAnchorPointBody2; //!< Anchor point of body 2 (in local-space coordinates of body 2) + vec3 m_hingeLocalAxisBody1; //!< Hinge rotation axis (in local-space coordinates of body 1) + vec3 m_hingeLocalAxisBody2; //!< Hinge rotation axis (in local-space coordiantes of body 2) + etk::Matrix3x3 m_i1; //!< Inertia tensor of body 1 (in world-space coordinates) + etk::Matrix3x3 m_i2; //!< Inertia tensor of body 2 (in world-space coordinates) + vec3 mA1; //!< Hinge rotation axis (in world-space coordinates) computed from body 1 + vec3 m_r1World; //!< Vector from center of body 2 to anchor point in world-space + vec3 m_r2World; //!< Vector from center of body 2 to anchor point in world-space + vec3 m_b2CrossA1; //!< Cross product of vector b2 and a1 + vec3 m_c2CrossA1; //!< Cross product of vector c2 and a1; + vec3 m_impulseTranslation; //!< Impulse for the 3 translation constraints + vec2 m_impulseRotation; //!< Impulse for the 2 rotation constraints + float m_impulseLowerLimit; //!< Accumulated impulse for the lower limit constraint + float m_impulseUpperLimit; //!< Accumulated impulse for the upper limit constraint + float m_impulseMotor; //!< Accumulated impulse for the motor constraint; + etk::Matrix3x3 m_inverseMassMatrixTranslation; //!< Inverse mass matrix K=JM^-1J^t for the 3 translation constraints + etk::Matrix2x2 m_inverseMassMatrixRotation; //!< Inverse mass matrix K=JM^-1J^t for the 2 rotation constraints + float m_inverseMassMatrixLimitMotor; //!< Inverse of mass matrix K=JM^-1J^t for the limits and motor constraints (1x1 matrix) + float m_inverseMassMatrixMotor; //!< Inverse of mass matrix K=JM^-1J^t for the motor + vec3 m_bTranslation; //!< Bias vector for the error correction for the translation constraints + vec2 m_bRotation; //!< Bias vector for the error correction for the rotation constraints + float m_bLowerLimit; //!< Bias of the lower limit constraint + float m_bUpperLimit; //!< Bias of the upper limit constraint + etk::Quaternion m_initOrientationDifferenceInv; //!< Inverse of the initial orientation difference between the bodies + bool m_isLimitEnabled; //!< True if the joint limits are enabled + bool m_isMotorEnabled; //!< True if the motor of the joint in enabled + float m_lowerLimit; //!< Lower limit (minimum allowed rotation angle in radian) + float m_upperLimit; //!< Upper limit (maximum translation distance) + bool m_isLowerLimitViolated; //!< True if the lower limit is violated + bool m_isUpperLimitViolated; //!< True if the upper limit is violated + float m_motorSpeed; //!< Motor speed (in rad/s) + float m_maxMotorTorque; //!< Maximum motor torque (in Newtons) that can be applied to reach to desired motor speed + /// Private copy-constructor + HingeJoint(const HingeJoint& _constraint); + /// Private assignment operator + HingeJoint& operator=(const HingeJoint& _constraint); + /// Reset the limits + void resetLimits(); + /// Given an angle in radian, this method returns the corresponding + /// angle in the range [-pi; pi] + float computeNormalizedAngle(float _angle) const; + /// Given an "inputAngle" in the range [-pi, pi], this method returns an + /// angle (modulo 2*pi) in the range [-2*pi; 2*pi] that is closest to one of the + /// two angle limits in arguments. + float computeCorrespondingAngleNearLimits(float _inputAngle, + float _lowerLimitAngle, + float _upperLimitAngle) const; + /// Compute the current angle around the hinge axis + float computeCurrentHingeAngle(const etk::Quaternion& _orientationBody1, + const etk::Quaternion& _orientationBody2); + /// Return the number of bytes used by the joint + virtual size_t getSizeInBytes() const; + /// Initialize before solving the constraint + virtual void initBeforeSolve(const ConstraintSolverData& _constraintSolverData); + /// Warm start the constraint (apply the previous impulse at the beginning of the step) + virtual void warmstart(const ConstraintSolverData& _constraintSolverData); + /// Solve the velocity constraint + virtual void solveVelocityConstraint(const ConstraintSolverData& _constraintSolverData); + /// Solve the position constraint (for position error correction) + virtual void solvePositionConstraint(const ConstraintSolverData& _constraintSolverData); + public : + /// Constructor + HingeJoint(const HingeJointInfo& _jointInfo); + /// Destructor + virtual ~HingeJoint(); + /// Return true if the limits or the joint are enabled + bool isLimitEnabled() const; + /// Return true if the motor of the joint is enabled + bool isMotorEnabled() const; + /// Enable/Disable the limits of the joint + void enableLimit(bool _isLimitEnabled); + /// Enable/Disable the motor of the joint + void enableMotor(bool _isMotorEnabled); + /// Return the minimum angle limit + float getMinAngleLimit() const; + /// Set the minimum angle limit + void setMinAngleLimit(float _lowerLimit); + /// Return the maximum angle limit + float getMaxAngleLimit() const; + /// Set the maximum angle limit + void setMaxAngleLimit(float _upperLimit); + /// Return the motor speed + float getMotorSpeed() const; + /// Set the motor speed + void setMotorSpeed(float _motorSpeed); + /// Return the maximum motor torque + float getMaxMotorTorque() const; + /// Set the maximum motor torque + void setMaxMotorTorque(float _maxMotorTorque); + /// Return the int32_tensity of the current torque applied for the joint motor + float getMotorTorque(float _timeStep) const; + }; } diff --git a/ephysics/constraint/Joint.cpp b/ephysics/constraint/Joint.cpp index 30823db..b6a3c04 100644 --- a/ephysics/constraint/Joint.cpp +++ b/ephysics/constraint/Joint.cpp @@ -24,3 +24,50 @@ Joint::Joint(const JointInfo& jointInfo) Joint::~Joint() { } + +// Return the reference to the body 1 +/** + * @return The first body involved in the joint + */ +RigidBody* Joint::getBody1() const { + return m_body1; +} + +// Return the reference to the body 2 +/** + * @return The second body involved in the joint + */ +RigidBody* Joint::getBody2() const { + return m_body2; +} + +// Return true if the joint is active +/** + * @return True if the joint is active + */ +bool Joint::isActive() const { + return (m_body1->isActive() && m_body2->isActive()); +} + +// Return the type of the joint +/** + * @return The type of the joint + */ +JointType Joint::getType() const { + return m_type; +} + +// Return true if the collision between the two bodies of the joint is enabled +/** + * @return True if the collision is enabled between the two bodies of the joint + * is enabled and false otherwise + */ +bool Joint::isCollisionEnabled() const { + return m_isCollisionEnabled; +} + +// Return true if the joint has already been added int32_to an island +bool Joint::isAlreadyInIsland() const { + return m_isAlreadyInIsland; +} + diff --git a/ephysics/constraint/Joint.hpp b/ephysics/constraint/Joint.hpp index 43ccc3a..801017c 100644 --- a/ephysics/constraint/Joint.hpp +++ b/ephysics/constraint/Joint.hpp @@ -5,226 +5,117 @@ */ #pragma once -// Libraries #include #include #include -// ReactPhysics3D namespace namespace ephysics { -/// Enumeration for the type of a constraint -enum JointType {BALLSOCKETJOINT, SLIDERJOINT, HINGEJOINT, FIXEDJOINT}; - -// Class declarations -struct ConstraintSolverData; -class Joint; - -// Structure JointListElement -/** - * This structure represents a single element of a linked list of joints - */ -struct JointListElement { - - public: - - // -------------------- Attributes -------------------- // - - /// Pointer to the actual joint - Joint* joint; - - /// Next element of the list - JointListElement* next; - - // -------------------- Methods -------------------- // - - /// Constructor - JointListElement(Joint* initJoint, JointListElement* initNext) - :joint(initJoint), next(initNext){ - - } -}; - -// Structure JointInfo -/** - * This structure is used to gather the information needed to create a joint. - */ -struct JointInfo { - - public : - - // -------------------- Attributes -------------------- // - - /// First rigid body of the joint - RigidBody* body1; - - /// Second rigid body of the joint - RigidBody* body2; - - /// Type of the joint - JointType type; - - /// Position correction technique used for the constraint (used for joints). - /// By default, the BAUMGARTE technique is used - JointsPositionCorrectionTechnique positionCorrectionTechnique; - - /// True if the two bodies of the joint are allowed to collide with each other - bool isCollisionEnabled; - - /// Constructor - JointInfo(JointType constraintType) - : body1(NULL), body2(NULL), type(constraintType), - positionCorrectionTechnique(NON_LINEAR_GAUSS_SEIDEL), - isCollisionEnabled(true) {} - - /// Constructor - JointInfo(RigidBody* rigidBody1, RigidBody* rigidBody2, JointType constraintType) - : body1(rigidBody1), body2(rigidBody2), type(constraintType), - positionCorrectionTechnique(NON_LINEAR_GAUSS_SEIDEL), - isCollisionEnabled(true) { - } - - /// Destructor - virtual ~JointInfo() {} - -}; - -// Class Joint -/** - * This abstract class represents a joint between two bodies. - */ -class Joint { - - protected : - - // -------------------- Attributes -------------------- // - - /// Pointer to the first body of the joint - RigidBody* const m_body1; - - /// Pointer to the second body of the joint - RigidBody* const m_body2; - - /// Type of the joint - const JointType m_type; - - /// Body 1 index in the velocity array to solve the constraint - uint32_t m_indexBody1; - - /// Body 2 index in the velocity array to solve the constraint - uint32_t m_indexBody2; - - /// Position correction technique used for the constraint (used for joints) - JointsPositionCorrectionTechnique m_positionCorrectionTechnique; - - /// True if the two bodies of the constraint are allowed to collide with each other - bool m_isCollisionEnabled; - - /// True if the joint has already been added int32_to an island - bool m_isAlreadyInIsland; - - // -------------------- Methods -------------------- // - - /// Private copy-constructor - Joint(const Joint& constraint); - - /// Private assignment operator - Joint& operator=(const Joint& constraint); - - /// Return true if the joint has already been added int32_to an island - bool isAlreadyInIsland() const; - - /// Return the number of bytes used by the joint - virtual size_t getSizeInBytes() const = 0; - - /// Initialize before solving the joint - virtual void initBeforeSolve(const ConstraintSolverData& constraintSolverData) = 0; - - /// Warm start the joint (apply the previous impulse at the beginning of the step) - virtual void warmstart(const ConstraintSolverData& constraintSolverData) = 0; - - /// Solve the velocity constraint - virtual void solveVelocityConstraint(const ConstraintSolverData& constraintSolverData) = 0; - - /// Solve the position constraint - virtual void solvePositionConstraint(const ConstraintSolverData& constraintSolverData) = 0; - - public : - - // -------------------- Methods -------------------- // - - /// Constructor - Joint(const JointInfo& jointInfo); - - /// Destructor - virtual ~Joint(); - - /// Return the reference to the body 1 - RigidBody* getBody1() const; - - /// Return the reference to the body 2 - RigidBody* getBody2() const; - - /// Return true if the constraint is active - bool isActive() const; - - /// Return the type of the constraint - JointType getType() const; - - /// Return true if the collision between the two bodies of the joint is enabled - bool isCollisionEnabled() const; - - // -------------------- Friendship -------------------- // - - friend class DynamicsWorld; - friend class Island; - friend class ConstraintSolver; -}; - -// Return the reference to the body 1 -/** - * @return The first body involved in the joint - */ -RigidBody* Joint::getBody1() const { - return m_body1; -} - -// Return the reference to the body 2 -/** - * @return The second body involved in the joint - */ -RigidBody* Joint::getBody2() const { - return m_body2; -} - -// Return true if the joint is active -/** - * @return True if the joint is active - */ -bool Joint::isActive() const { - return (m_body1->isActive() && m_body2->isActive()); -} - -// Return the type of the joint -/** - * @return The type of the joint - */ -JointType Joint::getType() const { - return m_type; -} - -// Return true if the collision between the two bodies of the joint is enabled -/** - * @return True if the collision is enabled between the two bodies of the joint - * is enabled and false otherwise - */ -bool Joint::isCollisionEnabled() const { - return m_isCollisionEnabled; -} - -// Return true if the joint has already been added int32_to an island -bool Joint::isAlreadyInIsland() const { - return m_isAlreadyInIsland; -} + /// Enumeration for the type of a constraint + enum JointType {BALLSOCKETJOINT, SLIDERJOINT, HINGEJOINT, FIXEDJOINT}; + + struct ConstraintSolverData; + class Joint; + + /** + * @brief It represents a single element of a linked list of joints + */ + struct JointListElement { + public: + Joint* joint; //!< Pointer to the actual joint + JointListElement* next; //!< Next element of the list + /** + * @breif Constructor + */ + JointListElement(Joint* _initJoint, + JointListElement* _initNext): + joint(_initJoint), + next(_initNext) { + + } + }; + + /** + * @brief It is used to gather the information needed to create a joint. + */ + struct JointInfo { + public : + RigidBody* body1; //!< First rigid body of the joint + RigidBody* body2; //!< Second rigid body of the joint + JointType type; //!< Type of the joint + JointsPositionCorrectionTechnique positionCorrectionTechnique; //!< Position correction technique used for the constraint (used for joints). By default, the BAUMGARTE technique is used + bool isCollisionEnabled; //!< True if the two bodies of the joint are allowed to collide with each other + /// Constructor + JointInfo(JointType _constraintType): + body1(nullptr), + body2(nullptr), + type(_constraintType), + positionCorrectionTechnique(NON_LINEAR_GAUSS_SEIDEL), + isCollisionEnabled(true) { + + } + /// Constructor + JointInfo(RigidBody* _rigidBody1, + RigidBody* _rigidBody2, + JointType _constraintType): + body1(_rigidBody1), + body2(_rigidBody2), + type(_constraintType), + positionCorrectionTechnique(NON_LINEAR_GAUSS_SEIDEL), + isCollisionEnabled(true) { + + } + /// Destructor + virtual ~JointInfo() = default; + }; + + /** + * @brief It represents a joint between two bodies. + */ + class Joint { + protected : + RigidBody* const m_body1; //!< Pointer to the first body of the joint + RigidBody* const m_body2; //!< Pointer to the second body of the joint + const JointType m_type; //!< Type of the joint + uint32_t m_indexBody1; //!< Body 1 index in the velocity array to solve the constraint + uint32_t m_indexBody2; //!< Body 2 index in the velocity array to solve the constraint + JointsPositionCorrectionTechnique m_positionCorrectionTechnique; //!< Position correction technique used for the constraint (used for joints) + bool m_isCollisionEnabled; //!< True if the two bodies of the constraint are allowed to collide with each other + bool m_isAlreadyInIsland; //!< True if the joint has already been added int32_to an island + /// Private copy-constructor + Joint(const Joint& _constraint); + /// Private assignment operator + Joint& operator=(const Joint& _constraint); + /// Return true if the joint has already been added int32_to an island + bool isAlreadyInIsland() const; + /// Return the number of bytes used by the joint + virtual size_t getSizeInBytes() const = 0; + /// Initialize before solving the joint + virtual void initBeforeSolve(const ConstraintSolverData& _constraintSolverData) = 0; + /// Warm start the joint (apply the previous impulse at the beginning of the step) + virtual void warmstart(const ConstraintSolverData& _constraintSolverData) = 0; + /// Solve the velocity constraint + virtual void solveVelocityConstraint(const ConstraintSolverData& _constraintSolverData) = 0; + /// Solve the position constraint + virtual void solvePositionConstraint(const ConstraintSolverData& _constraintSolverData) = 0; + public : + /// Constructor + Joint(const JointInfo& _jointInfo); + /// Destructor + virtual ~Joint(); + /// Return the reference to the body 1 + RigidBody* getBody1() const; + /// Return the reference to the body 2 + RigidBody* getBody2() const; + /// Return true if the constraint is active + bool isActive() const; + /// Return the type of the constraint + JointType getType() const; + /// Return true if the collision between the two bodies of the joint is enabled + bool isCollisionEnabled() const; + friend class DynamicsWorld; + friend class Island; + friend class ConstraintSolver; + }; } diff --git a/ephysics/constraint/SliderJoint.cpp b/ephysics/constraint/SliderJoint.cpp index f6096f4..a372490 100644 --- a/ephysics/constraint/SliderJoint.cpp +++ b/ephysics/constraint/SliderJoint.cpp @@ -16,15 +16,15 @@ const float SliderJoint::BETA = float(0.2); SliderJoint::SliderJoint(const SliderJointInfo& jointInfo) : Joint(jointInfo), m_impulseTranslation(0, 0), m_impulseRotation(0, 0, 0), m_impulseLowerLimit(0), m_impulseUpperLimit(0), m_impulseMotor(0), - mIsLimitEnabled(jointInfo.isLimitEnabled), mIsMotorEnabled(jointInfo.isMotorEnabled), - mLowerLimit(jointInfo.minTranslationLimit), - mUpperLimit(jointInfo.maxTranslationLimit), mIsLowerLimitViolated(false), - mIsUpperLimitViolated(false), mMotorSpeed(jointInfo.motorSpeed), - mMaxMotorForce(jointInfo.maxMotorForce){ + m_isLimitEnabled(jointInfo.isLimitEnabled), m_isMotorEnabled(jointInfo.isMotorEnabled), + m_lowerLimit(jointInfo.minTranslationLimit), + m_upperLimit(jointInfo.maxTranslationLimit), m_isLowerLimitViolated(false), + m_isUpperLimitViolated(false), m_motorSpeed(jointInfo.motorSpeed), + m_maxMotorForce(jointInfo.maxMotorForce){ - assert(mUpperLimit >= 0.0); - assert(mLowerLimit <= 0.0); - assert(mMaxMotorForce >= 0.0); + assert(m_upperLimit >= 0.0); + assert(m_lowerLimit <= 0.0); + assert(m_maxMotorForce >= 0.0); // Compute the local-space anchor point for each body const etk::Transform3D& transform1 = m_body1->getTransform(); @@ -39,9 +39,9 @@ SliderJoint::SliderJoint(const SliderJointInfo& jointInfo) m_initOrientationDifferenceInv.inverse(); // Compute the slider axis in local-space of body 1 - mSliderAxisBody1 = m_body1->getTransform().getOrientation().getInverse() * + m_sliderAxisBody1 = m_body1->getTransform().getOrientation().getInverse() * jointInfo.sliderAxisWorldSpace; - mSliderAxisBody1.normalize(); + m_sliderAxisBody1.normalize(); } // Destructor @@ -67,57 +67,57 @@ void SliderJoint::initBeforeSolve(const ConstraintSolverData& constraintSolverDa m_i2 = m_body2->getInertiaTensorInverseWorld(); // Vector from body center to the anchor point - mR1 = orientationBody1 * m_localAnchorPointBody1; - mR2 = orientationBody2 * m_localAnchorPointBody2; + m_R1 = orientationBody1 * m_localAnchorPointBody1; + m_R2 = orientationBody2 * m_localAnchorPointBody2; // Compute the vector u (difference between anchor points) - const vec3 u = x2 + mR2 - x1 - mR1; + const vec3 u = x2 + m_R2 - x1 - m_R1; // Compute the two orthogonal vectors to the slider axis in world-space - mSliderAxisWorld = orientationBody1 * mSliderAxisBody1; - mSliderAxisWorld.normalize(); - mN1 = mSliderAxisWorld.getOrthoVector(); - mN2 = mSliderAxisWorld.cross(mN1); + m_sliderAxisWorld = orientationBody1 * m_sliderAxisBody1; + m_sliderAxisWorld.normalize(); + m_N1 = m_sliderAxisWorld.getOrthoVector(); + m_N2 = m_sliderAxisWorld.cross(m_N1); // Check if the limit constraints are violated or not - float uDotSliderAxis = u.dot(mSliderAxisWorld); - float lowerLimitError = uDotSliderAxis - mLowerLimit; - float upperLimitError = mUpperLimit - uDotSliderAxis; - bool oldIsLowerLimitViolated = mIsLowerLimitViolated; - mIsLowerLimitViolated = lowerLimitError <= 0; - if (mIsLowerLimitViolated != oldIsLowerLimitViolated) { + float uDotSliderAxis = u.dot(m_sliderAxisWorld); + float lowerLimitError = uDotSliderAxis - m_lowerLimit; + float upperLimitError = m_upperLimit - uDotSliderAxis; + bool oldIsLowerLimitViolated = m_isLowerLimitViolated; + m_isLowerLimitViolated = lowerLimitError <= 0; + if (m_isLowerLimitViolated != oldIsLowerLimitViolated) { m_impulseLowerLimit = 0.0; } - bool oldIsUpperLimitViolated = mIsUpperLimitViolated; - mIsUpperLimitViolated = upperLimitError <= 0; - if (mIsUpperLimitViolated != oldIsUpperLimitViolated) { + bool oldIsUpperLimitViolated = m_isUpperLimitViolated; + m_isUpperLimitViolated = upperLimitError <= 0; + if (m_isUpperLimitViolated != oldIsUpperLimitViolated) { m_impulseUpperLimit = 0.0; } // Compute the cross products used in the Jacobians - mR2CrossN1 = mR2.cross(mN1); - mR2CrossN2 = mR2.cross(mN2); - mR2CrossSliderAxis = mR2.cross(mSliderAxisWorld); - const vec3 r1PlusU = mR1 + u; - mR1PlusUCrossN1 = (r1PlusU).cross(mN1); - mR1PlusUCrossN2 = (r1PlusU).cross(mN2); - mR1PlusUCrossSliderAxis = (r1PlusU).cross(mSliderAxisWorld); + m_R2CrossN1 = m_R2.cross(m_N1); + m_R2CrossN2 = m_R2.cross(m_N2); + m_R2CrossSliderAxis = m_R2.cross(m_sliderAxisWorld); + const vec3 r1PlusU = m_R1 + u; + m_R1PlusUCrossN1 = (r1PlusU).cross(m_N1); + m_R1PlusUCrossN2 = (r1PlusU).cross(m_N2); + m_R1PlusUCrossSliderAxis = (r1PlusU).cross(m_sliderAxisWorld); // Compute the inverse of the mass matrix K=JM^-1J^t for the 2 translation // constraints (2x2 matrix) float sumInverseMass = m_body1->m_massInverse + m_body2->m_massInverse; - vec3 I1R1PlusUCrossN1 = m_i1 * mR1PlusUCrossN1; - vec3 I1R1PlusUCrossN2 = m_i1 * mR1PlusUCrossN2; - vec3 I2R2CrossN1 = m_i2 * mR2CrossN1; - vec3 I2R2CrossN2 = m_i2 * mR2CrossN2; - const float el11 = sumInverseMass + mR1PlusUCrossN1.dot(I1R1PlusUCrossN1) + - mR2CrossN1.dot(I2R2CrossN1); - const float el12 = mR1PlusUCrossN1.dot(I1R1PlusUCrossN2) + - mR2CrossN1.dot(I2R2CrossN2); - const float el21 = mR1PlusUCrossN2.dot(I1R1PlusUCrossN1) + - mR2CrossN2.dot(I2R2CrossN1); - const float el22 = sumInverseMass + mR1PlusUCrossN2.dot(I1R1PlusUCrossN2) + - mR2CrossN2.dot(I2R2CrossN2); + vec3 I1R1PlusUCrossN1 = m_i1 * m_R1PlusUCrossN1; + vec3 I1R1PlusUCrossN2 = m_i1 * m_R1PlusUCrossN2; + vec3 I2R2CrossN1 = m_i2 * m_R2CrossN1; + vec3 I2R2CrossN2 = m_i2 * m_R2CrossN2; + const float el11 = sumInverseMass + m_R1PlusUCrossN1.dot(I1R1PlusUCrossN1) + + m_R2CrossN1.dot(I2R2CrossN1); + const float el12 = m_R1PlusUCrossN1.dot(I1R1PlusUCrossN2) + + m_R2CrossN1.dot(I2R2CrossN2); + const float el21 = m_R1PlusUCrossN2.dot(I1R1PlusUCrossN1) + + m_R2CrossN2.dot(I2R2CrossN1); + const float el22 = sumInverseMass + m_R1PlusUCrossN2.dot(I1R1PlusUCrossN2) + + m_R2CrossN2.dot(I2R2CrossN2); etk::Matrix2x2 matrixKTranslation(el11, el12, el21, el22); m_inverseMassMatrixTranslationConstraint.setZero(); if (m_body1->getType() == DYNAMIC || m_body2->getType() == DYNAMIC) { @@ -125,12 +125,12 @@ void SliderJoint::initBeforeSolve(const ConstraintSolverData& constraintSolverDa } // Compute the bias "b" of the translation constraint - mBTranslation.setZero(); + m_bTranslation.setZero(); float biasFactor = (BETA / constraintSolverData.timeStep); if (m_positionCorrectionTechnique == BAUMGARTE_JOINTS) { - mBTranslation.setX(u.dot(mN1)); - mBTranslation.setY(u.dot(mN2)); - mBTranslation *= biasFactor; + m_bTranslation.setX(u.dot(m_N1)); + m_bTranslation.setY(u.dot(m_N2)); + m_bTranslation *= biasFactor; } // Compute the inverse of the mass matrix K=JM^-1J^t for the 3 rotation @@ -141,39 +141,39 @@ void SliderJoint::initBeforeSolve(const ConstraintSolverData& constraintSolverDa } // Compute the bias "b" of the rotation constraint - mBRotation.setZero(); + m_bRotation.setZero(); if (m_positionCorrectionTechnique == BAUMGARTE_JOINTS) { etk::Quaternion currentOrientationDifference = orientationBody2 * orientationBody1.getInverse(); currentOrientationDifference.normalize(); const etk::Quaternion qError = currentOrientationDifference * m_initOrientationDifferenceInv; - mBRotation = biasFactor * float(2.0) * qError.getVectorV(); + m_bRotation = biasFactor * float(2.0) * qError.getVectorV(); } // If the limits are enabled - if (mIsLimitEnabled && (mIsLowerLimitViolated || mIsUpperLimitViolated)) { + if (m_isLimitEnabled && (m_isLowerLimitViolated || m_isUpperLimitViolated)) { // Compute the inverse of the mass matrix K=JM^-1J^t for the limits (1x1 matrix) m_inverseMassMatrixLimit = m_body1->m_massInverse + m_body2->m_massInverse + - mR1PlusUCrossSliderAxis.dot(m_i1 * mR1PlusUCrossSliderAxis) + - mR2CrossSliderAxis.dot(m_i2 * mR2CrossSliderAxis); + m_R1PlusUCrossSliderAxis.dot(m_i1 * m_R1PlusUCrossSliderAxis) + + m_R2CrossSliderAxis.dot(m_i2 * m_R2CrossSliderAxis); m_inverseMassMatrixLimit = (m_inverseMassMatrixLimit > 0.0) ? 1.0f / m_inverseMassMatrixLimit : 0.0f; // Compute the bias "b" of the lower limit constraint - mBLowerLimit = 0.0; + m_bLowerLimit = 0.0; if (m_positionCorrectionTechnique == BAUMGARTE_JOINTS) { - mBLowerLimit = biasFactor * lowerLimitError; + m_bLowerLimit = biasFactor * lowerLimitError; } // Compute the bias "b" of the upper limit constraint - mBUpperLimit = 0.0; + m_bUpperLimit = 0.0; if (m_positionCorrectionTechnique == BAUMGARTE_JOINTS) { - mBUpperLimit = biasFactor * upperLimitError; + m_bUpperLimit = biasFactor * upperLimitError; } } // If the motor is enabled - if (mIsMotorEnabled) { + if (m_isMotorEnabled) { // Compute the inverse of mass matrix K=JM^-1J^t for the motor (1x1 matrix) m_inverseMassMatrixMotor = m_body1->m_massInverse + m_body2->m_massInverse; @@ -208,22 +208,22 @@ void SliderJoint::warmstart(const ConstraintSolverData& constraintSolverData) { // Compute the impulse P=J^T * lambda for the lower and upper limits constraints of body 1 float impulseLimits = m_impulseUpperLimit - m_impulseLowerLimit; - vec3 linearImpulseLimits = impulseLimits * mSliderAxisWorld; + vec3 linearImpulseLimits = impulseLimits * m_sliderAxisWorld; // Compute the impulse P=J^T * lambda for the motor constraint of body 1 - vec3 impulseMotor = m_impulseMotor * mSliderAxisWorld; + vec3 impulseMotor = m_impulseMotor * m_sliderAxisWorld; // Compute the impulse P=J^T * lambda for the 2 translation constraints of body 1 - vec3 linearImpulseBody1 = -mN1 * m_impulseTranslation.x() - mN2 * m_impulseTranslation.y(); - vec3 angularImpulseBody1 = -mR1PlusUCrossN1 * m_impulseTranslation.x() - - mR1PlusUCrossN2 * m_impulseTranslation.y(); + vec3 linearImpulseBody1 = -m_N1 * m_impulseTranslation.x() - m_N2 * m_impulseTranslation.y(); + vec3 angularImpulseBody1 = -m_R1PlusUCrossN1 * m_impulseTranslation.x() - + m_R1PlusUCrossN2 * m_impulseTranslation.y(); // Compute the impulse P=J^T * lambda for the 3 rotation constraints of body 1 angularImpulseBody1 += -m_impulseRotation; // Compute the impulse P=J^T * lambda for the lower and upper limits constraints of body 1 linearImpulseBody1 += linearImpulseLimits; - angularImpulseBody1 += impulseLimits * mR1PlusUCrossSliderAxis; + angularImpulseBody1 += impulseLimits * m_R1PlusUCrossSliderAxis; // Compute the impulse P=J^T * lambda for the motor constraint of body 1 linearImpulseBody1 += impulseMotor; @@ -233,16 +233,16 @@ void SliderJoint::warmstart(const ConstraintSolverData& constraintSolverData) { w1 += m_i1 * angularImpulseBody1; // Compute the impulse P=J^T * lambda for the 2 translation constraints of body 2 - vec3 linearImpulseBody2 = mN1 * m_impulseTranslation.x() + mN2 * m_impulseTranslation.y(); - vec3 angularImpulseBody2 = mR2CrossN1 * m_impulseTranslation.x() + - mR2CrossN2 * m_impulseTranslation.y(); + vec3 linearImpulseBody2 = m_N1 * m_impulseTranslation.x() + m_N2 * m_impulseTranslation.y(); + vec3 angularImpulseBody2 = m_R2CrossN1 * m_impulseTranslation.x() + + m_R2CrossN2 * m_impulseTranslation.y(); // Compute the impulse P=J^T * lambda for the 3 rotation constraints of body 2 angularImpulseBody2 += m_impulseRotation; // Compute the impulse P=J^T * lambda for the lower and upper limits constraints of body 2 linearImpulseBody2 += -linearImpulseLimits; - angularImpulseBody2 += -impulseLimits * mR2CrossSliderAxis; + angularImpulseBody2 += -impulseLimits * m_R2CrossSliderAxis; // Compute the impulse P=J^T * lambda for the motor constraint of body 2 linearImpulseBody2 += -impulseMotor; @@ -268,28 +268,28 @@ void SliderJoint::solveVelocityConstraint(const ConstraintSolverData& constraint // --------------- Translation Constraints --------------- // // Compute J*v for the 2 translation constraints - const float el1 = -mN1.dot(v1) - w1.dot(mR1PlusUCrossN1) + - mN1.dot(v2) + w2.dot(mR2CrossN1); - const float el2 = -mN2.dot(v1) - w1.dot(mR1PlusUCrossN2) + - mN2.dot(v2) + w2.dot(mR2CrossN2); + const float el1 = -m_N1.dot(v1) - w1.dot(m_R1PlusUCrossN1) + + m_N1.dot(v2) + w2.dot(m_R2CrossN1); + const float el2 = -m_N2.dot(v1) - w1.dot(m_R1PlusUCrossN2) + + m_N2.dot(v2) + w2.dot(m_R2CrossN2); const vec2 JvTranslation(el1, el2); // Compute the Lagrange multiplier lambda for the 2 translation constraints - vec2 deltaLambda = m_inverseMassMatrixTranslationConstraint * (-JvTranslation -mBTranslation); + vec2 deltaLambda = m_inverseMassMatrixTranslationConstraint * (-JvTranslation -m_bTranslation); m_impulseTranslation += deltaLambda; // Compute the impulse P=J^T * lambda for the 2 translation constraints of body 1 - const vec3 linearImpulseBody1 = -mN1 * deltaLambda.x() - mN2 * deltaLambda.y(); - vec3 angularImpulseBody1 = -mR1PlusUCrossN1 * deltaLambda.x() - - mR1PlusUCrossN2 * deltaLambda.y(); + const vec3 linearImpulseBody1 = -m_N1 * deltaLambda.x() - m_N2 * deltaLambda.y(); + vec3 angularImpulseBody1 = -m_R1PlusUCrossN1 * deltaLambda.x() - + m_R1PlusUCrossN2 * deltaLambda.y(); // Apply the impulse to the body 1 v1 += inverseMassBody1 * linearImpulseBody1; w1 += m_i1 * angularImpulseBody1; // Compute the impulse P=J^T * lambda for the 2 translation constraints of body 2 - const vec3 linearImpulseBody2 = mN1 * deltaLambda.x() + mN2 * deltaLambda.y(); - vec3 angularImpulseBody2 = mR2CrossN1 * deltaLambda.x() + mR2CrossN2 * deltaLambda.y(); + const vec3 linearImpulseBody2 = m_N1 * deltaLambda.x() + m_N2 * deltaLambda.y(); + vec3 angularImpulseBody2 = m_R2CrossN1 * deltaLambda.x() + m_R2CrossN2 * deltaLambda.y(); // Apply the impulse to the body 2 v2 += inverseMassBody2 * linearImpulseBody2; @@ -301,7 +301,7 @@ void SliderJoint::solveVelocityConstraint(const ConstraintSolverData& constraint const vec3 JvRotation = w2 - w1; // Compute the Lagrange multiplier lambda for the 3 rotation constraints - vec3 deltaLambda2 = m_inverseMassMatrixRotationConstraint * (-JvRotation - mBRotation); + vec3 deltaLambda2 = m_inverseMassMatrixRotationConstraint * (-JvRotation - m_bRotation); m_impulseRotation += deltaLambda2; // Compute the impulse P=J^T * lambda for the 3 rotation constraints of body 1 @@ -318,32 +318,32 @@ void SliderJoint::solveVelocityConstraint(const ConstraintSolverData& constraint // --------------- Limits Constraints --------------- // - if (mIsLimitEnabled) { + if (m_isLimitEnabled) { // If the lower limit is violated - if (mIsLowerLimitViolated) { + if (m_isLowerLimitViolated) { // Compute J*v for the lower limit constraint - const float JvLowerLimit = mSliderAxisWorld.dot(v2) + mR2CrossSliderAxis.dot(w2) - - mSliderAxisWorld.dot(v1) - mR1PlusUCrossSliderAxis.dot(w1); + const float JvLowerLimit = m_sliderAxisWorld.dot(v2) + m_R2CrossSliderAxis.dot(w2) - + m_sliderAxisWorld.dot(v1) - m_R1PlusUCrossSliderAxis.dot(w1); // Compute the Lagrange multiplier lambda for the lower limit constraint - float deltaLambdaLower = m_inverseMassMatrixLimit * (-JvLowerLimit -mBLowerLimit); + float deltaLambdaLower = m_inverseMassMatrixLimit * (-JvLowerLimit -m_bLowerLimit); float lambdaTemp = m_impulseLowerLimit; m_impulseLowerLimit = std::max(m_impulseLowerLimit + deltaLambdaLower, 0.0f); deltaLambdaLower = m_impulseLowerLimit - lambdaTemp; // Compute the impulse P=J^T * lambda for the lower limit constraint of body 1 - const vec3 linearImpulseBody1 = -deltaLambdaLower * mSliderAxisWorld; - const vec3 angularImpulseBody1 = -deltaLambdaLower * mR1PlusUCrossSliderAxis; + const vec3 linearImpulseBody1 = -deltaLambdaLower * m_sliderAxisWorld; + const vec3 angularImpulseBody1 = -deltaLambdaLower * m_R1PlusUCrossSliderAxis; // Apply the impulse to the body 1 v1 += inverseMassBody1 * linearImpulseBody1; w1 += m_i1 * angularImpulseBody1; // Compute the impulse P=J^T * lambda for the lower limit constraint of body 2 - const vec3 linearImpulseBody2 = deltaLambdaLower * mSliderAxisWorld; - const vec3 angularImpulseBody2 = deltaLambdaLower * mR2CrossSliderAxis; + const vec3 linearImpulseBody2 = deltaLambdaLower * m_sliderAxisWorld; + const vec3 angularImpulseBody2 = deltaLambdaLower * m_R2CrossSliderAxis; // Apply the impulse to the body 2 v2 += inverseMassBody2 * linearImpulseBody2; @@ -351,29 +351,29 @@ void SliderJoint::solveVelocityConstraint(const ConstraintSolverData& constraint } // If the upper limit is violated - if (mIsUpperLimitViolated) { + if (m_isUpperLimitViolated) { // Compute J*v for the upper limit constraint - const float JvUpperLimit = mSliderAxisWorld.dot(v1) + mR1PlusUCrossSliderAxis.dot(w1) - - mSliderAxisWorld.dot(v2) - mR2CrossSliderAxis.dot(w2); + const float JvUpperLimit = m_sliderAxisWorld.dot(v1) + m_R1PlusUCrossSliderAxis.dot(w1) + - m_sliderAxisWorld.dot(v2) - m_R2CrossSliderAxis.dot(w2); // Compute the Lagrange multiplier lambda for the upper limit constraint - float deltaLambdaUpper = m_inverseMassMatrixLimit * (-JvUpperLimit -mBUpperLimit); + float deltaLambdaUpper = m_inverseMassMatrixLimit * (-JvUpperLimit -m_bUpperLimit); float lambdaTemp = m_impulseUpperLimit; m_impulseUpperLimit = std::max(m_impulseUpperLimit + deltaLambdaUpper, 0.0f); deltaLambdaUpper = m_impulseUpperLimit - lambdaTemp; // Compute the impulse P=J^T * lambda for the upper limit constraint of body 1 - const vec3 linearImpulseBody1 = deltaLambdaUpper * mSliderAxisWorld; - const vec3 angularImpulseBody1 = deltaLambdaUpper * mR1PlusUCrossSliderAxis; + const vec3 linearImpulseBody1 = deltaLambdaUpper * m_sliderAxisWorld; + const vec3 angularImpulseBody1 = deltaLambdaUpper * m_R1PlusUCrossSliderAxis; // Apply the impulse to the body 1 v1 += inverseMassBody1 * linearImpulseBody1; w1 += m_i1 * angularImpulseBody1; // Compute the impulse P=J^T * lambda for the upper limit constraint of body 2 - const vec3 linearImpulseBody2 = -deltaLambdaUpper * mSliderAxisWorld; - const vec3 angularImpulseBody2 = -deltaLambdaUpper * mR2CrossSliderAxis; + const vec3 linearImpulseBody2 = -deltaLambdaUpper * m_sliderAxisWorld; + const vec3 angularImpulseBody2 = -deltaLambdaUpper * m_R2CrossSliderAxis; // Apply the impulse to the body 2 v2 += inverseMassBody2 * linearImpulseBody2; @@ -383,26 +383,26 @@ void SliderJoint::solveVelocityConstraint(const ConstraintSolverData& constraint // --------------- Motor --------------- // - if (mIsMotorEnabled) { + if (m_isMotorEnabled) { // Compute J*v for the motor - const float JvMotor = mSliderAxisWorld.dot(v1) - mSliderAxisWorld.dot(v2); + const float JvMotor = m_sliderAxisWorld.dot(v1) - m_sliderAxisWorld.dot(v2); // Compute the Lagrange multiplier lambda for the motor - const float maxMotorImpulse = mMaxMotorForce * constraintSolverData.timeStep; - float deltaLambdaMotor = m_inverseMassMatrixMotor * (-JvMotor - mMotorSpeed); + const float maxMotorImpulse = m_maxMotorForce * constraintSolverData.timeStep; + float deltaLambdaMotor = m_inverseMassMatrixMotor * (-JvMotor - m_motorSpeed); float lambdaTemp = m_impulseMotor; m_impulseMotor = clamp(m_impulseMotor + deltaLambdaMotor, -maxMotorImpulse, maxMotorImpulse); deltaLambdaMotor = m_impulseMotor - lambdaTemp; // Compute the impulse P=J^T * lambda for the motor of body 1 - const vec3 linearImpulseBody1 = deltaLambdaMotor * mSliderAxisWorld; + const vec3 linearImpulseBody1 = deltaLambdaMotor * m_sliderAxisWorld; // Apply the impulse to the body 1 v1 += inverseMassBody1 * linearImpulseBody1; // Compute the impulse P=J^T * lambda for the motor of body 2 - const vec3 linearImpulseBody2 = -deltaLambdaMotor * mSliderAxisWorld; + const vec3 linearImpulseBody2 = -deltaLambdaMotor * m_sliderAxisWorld; // Apply the impulse to the body 2 v2 += inverseMassBody2 * linearImpulseBody2; @@ -431,51 +431,51 @@ void SliderJoint::solvePositionConstraint(const ConstraintSolverData& constraint m_i2 = m_body2->getInertiaTensorInverseWorld(); // Vector from body center to the anchor point - mR1 = q1 * m_localAnchorPointBody1; - mR2 = q2 * m_localAnchorPointBody2; + m_R1 = q1 * m_localAnchorPointBody1; + m_R2 = q2 * m_localAnchorPointBody2; // Compute the vector u (difference between anchor points) - const vec3 u = x2 + mR2 - x1 - mR1; + const vec3 u = x2 + m_R2 - x1 - m_R1; // Compute the two orthogonal vectors to the slider axis in world-space - mSliderAxisWorld = q1 * mSliderAxisBody1; - mSliderAxisWorld.normalize(); - mN1 = mSliderAxisWorld.getOrthoVector(); - mN2 = mSliderAxisWorld.cross(mN1); + m_sliderAxisWorld = q1 * m_sliderAxisBody1; + m_sliderAxisWorld.normalize(); + m_N1 = m_sliderAxisWorld.getOrthoVector(); + m_N2 = m_sliderAxisWorld.cross(m_N1); // Check if the limit constraints are violated or not - float uDotSliderAxis = u.dot(mSliderAxisWorld); - float lowerLimitError = uDotSliderAxis - mLowerLimit; - float upperLimitError = mUpperLimit - uDotSliderAxis; - mIsLowerLimitViolated = lowerLimitError <= 0; - mIsUpperLimitViolated = upperLimitError <= 0; + float uDotSliderAxis = u.dot(m_sliderAxisWorld); + float lowerLimitError = uDotSliderAxis - m_lowerLimit; + float upperLimitError = m_upperLimit - uDotSliderAxis; + m_isLowerLimitViolated = lowerLimitError <= 0; + m_isUpperLimitViolated = upperLimitError <= 0; // Compute the cross products used in the Jacobians - mR2CrossN1 = mR2.cross(mN1); - mR2CrossN2 = mR2.cross(mN2); - mR2CrossSliderAxis = mR2.cross(mSliderAxisWorld); - const vec3 r1PlusU = mR1 + u; - mR1PlusUCrossN1 = (r1PlusU).cross(mN1); - mR1PlusUCrossN2 = (r1PlusU).cross(mN2); - mR1PlusUCrossSliderAxis = (r1PlusU).cross(mSliderAxisWorld); + m_R2CrossN1 = m_R2.cross(m_N1); + m_R2CrossN2 = m_R2.cross(m_N2); + m_R2CrossSliderAxis = m_R2.cross(m_sliderAxisWorld); + const vec3 r1PlusU = m_R1 + u; + m_R1PlusUCrossN1 = (r1PlusU).cross(m_N1); + m_R1PlusUCrossN2 = (r1PlusU).cross(m_N2); + m_R1PlusUCrossSliderAxis = (r1PlusU).cross(m_sliderAxisWorld); // --------------- Translation Constraints --------------- // // Recompute the inverse of the mass matrix K=JM^-1J^t for the 2 translation // constraints (2x2 matrix) float sumInverseMass = m_body1->m_massInverse + m_body2->m_massInverse; - vec3 I1R1PlusUCrossN1 = m_i1 * mR1PlusUCrossN1; - vec3 I1R1PlusUCrossN2 = m_i1 * mR1PlusUCrossN2; - vec3 I2R2CrossN1 = m_i2 * mR2CrossN1; - vec3 I2R2CrossN2 = m_i2 * mR2CrossN2; - const float el11 = sumInverseMass + mR1PlusUCrossN1.dot(I1R1PlusUCrossN1) + - mR2CrossN1.dot(I2R2CrossN1); - const float el12 = mR1PlusUCrossN1.dot(I1R1PlusUCrossN2) + - mR2CrossN1.dot(I2R2CrossN2); - const float el21 = mR1PlusUCrossN2.dot(I1R1PlusUCrossN1) + - mR2CrossN2.dot(I2R2CrossN1); - const float el22 = sumInverseMass + mR1PlusUCrossN2.dot(I1R1PlusUCrossN2) + - mR2CrossN2.dot(I2R2CrossN2); + vec3 I1R1PlusUCrossN1 = m_i1 * m_R1PlusUCrossN1; + vec3 I1R1PlusUCrossN2 = m_i1 * m_R1PlusUCrossN2; + vec3 I2R2CrossN1 = m_i2 * m_R2CrossN1; + vec3 I2R2CrossN2 = m_i2 * m_R2CrossN2; + const float el11 = sumInverseMass + m_R1PlusUCrossN1.dot(I1R1PlusUCrossN1) + + m_R2CrossN1.dot(I2R2CrossN1); + const float el12 = m_R1PlusUCrossN1.dot(I1R1PlusUCrossN2) + + m_R2CrossN1.dot(I2R2CrossN2); + const float el21 = m_R1PlusUCrossN2.dot(I1R1PlusUCrossN1) + + m_R2CrossN2.dot(I2R2CrossN1); + const float el22 = sumInverseMass + m_R1PlusUCrossN2.dot(I1R1PlusUCrossN2) + + m_R2CrossN2.dot(I2R2CrossN2); etk::Matrix2x2 matrixKTranslation(el11, el12, el21, el22); m_inverseMassMatrixTranslationConstraint.setZero(); if (m_body1->getType() == DYNAMIC || m_body2->getType() == DYNAMIC) { @@ -483,15 +483,15 @@ void SliderJoint::solvePositionConstraint(const ConstraintSolverData& constraint } // Compute the position error for the 2 translation constraints - const vec2 translationError(u.dot(mN1), u.dot(mN2)); + const vec2 translationError(u.dot(m_N1), u.dot(m_N2)); // Compute the Lagrange multiplier lambda for the 2 translation constraints vec2 lambdaTranslation = m_inverseMassMatrixTranslationConstraint * (-translationError); // Compute the impulse P=J^T * lambda for the 2 translation constraints of body 1 - const vec3 linearImpulseBody1 = -mN1 * lambdaTranslation.x() - mN2 * lambdaTranslation.y(); - vec3 angularImpulseBody1 = -mR1PlusUCrossN1 * lambdaTranslation.x() - - mR1PlusUCrossN2 * lambdaTranslation.y(); + const vec3 linearImpulseBody1 = -m_N1 * lambdaTranslation.x() - m_N2 * lambdaTranslation.y(); + vec3 angularImpulseBody1 = -m_R1PlusUCrossN1 * lambdaTranslation.x() - + m_R1PlusUCrossN2 * lambdaTranslation.y(); // Apply the impulse to the body 1 const vec3 v1 = inverseMassBody1 * linearImpulseBody1; @@ -503,9 +503,9 @@ void SliderJoint::solvePositionConstraint(const ConstraintSolverData& constraint q1.normalize(); // Compute the impulse P=J^T * lambda for the 2 translation constraints of body 2 - const vec3 linearImpulseBody2 = mN1 * lambdaTranslation.x() + mN2 * lambdaTranslation.y(); - vec3 angularImpulseBody2 = mR2CrossN1 * lambdaTranslation.x() + - mR2CrossN2 * lambdaTranslation.y(); + const vec3 linearImpulseBody2 = m_N1 * lambdaTranslation.x() + m_N2 * lambdaTranslation.y(); + vec3 angularImpulseBody2 = m_R2CrossN1 * lambdaTranslation.x() + + m_R2CrossN2 * lambdaTranslation.y(); // Apply the impulse to the body 2 const vec3 v2 = inverseMassBody2 * linearImpulseBody2; @@ -556,27 +556,27 @@ void SliderJoint::solvePositionConstraint(const ConstraintSolverData& constraint // --------------- Limits Constraints --------------- // - if (mIsLimitEnabled) { + if (m_isLimitEnabled) { - if (mIsLowerLimitViolated || mIsUpperLimitViolated) { + if (m_isLowerLimitViolated || m_isUpperLimitViolated) { // Compute the inverse of the mass matrix K=JM^-1J^t for the limits (1x1 matrix) m_inverseMassMatrixLimit = m_body1->m_massInverse + m_body2->m_massInverse + - mR1PlusUCrossSliderAxis.dot(m_i1 * mR1PlusUCrossSliderAxis) + - mR2CrossSliderAxis.dot(m_i2 * mR2CrossSliderAxis); + m_R1PlusUCrossSliderAxis.dot(m_i1 * m_R1PlusUCrossSliderAxis) + + m_R2CrossSliderAxis.dot(m_i2 * m_R2CrossSliderAxis); m_inverseMassMatrixLimit = (m_inverseMassMatrixLimit > 0.0) ? 1.0f / m_inverseMassMatrixLimit : 0.0f; } // If the lower limit is violated - if (mIsLowerLimitViolated) { + if (m_isLowerLimitViolated) { // Compute the Lagrange multiplier lambda for the lower limit constraint float lambdaLowerLimit = m_inverseMassMatrixLimit * (-lowerLimitError); // Compute the impulse P=J^T * lambda for the lower limit constraint of body 1 - const vec3 linearImpulseBody1 = -lambdaLowerLimit * mSliderAxisWorld; - const vec3 angularImpulseBody1 = -lambdaLowerLimit * mR1PlusUCrossSliderAxis; + const vec3 linearImpulseBody1 = -lambdaLowerLimit * m_sliderAxisWorld; + const vec3 angularImpulseBody1 = -lambdaLowerLimit * m_R1PlusUCrossSliderAxis; // Apply the impulse to the body 1 const vec3 v1 = inverseMassBody1 * linearImpulseBody1; @@ -588,8 +588,8 @@ void SliderJoint::solvePositionConstraint(const ConstraintSolverData& constraint q1.normalize(); // Compute the impulse P=J^T * lambda for the lower limit constraint of body 2 - const vec3 linearImpulseBody2 = lambdaLowerLimit * mSliderAxisWorld; - const vec3 angularImpulseBody2 = lambdaLowerLimit * mR2CrossSliderAxis; + const vec3 linearImpulseBody2 = lambdaLowerLimit * m_sliderAxisWorld; + const vec3 angularImpulseBody2 = lambdaLowerLimit * m_R2CrossSliderAxis; // Apply the impulse to the body 2 const vec3 v2 = inverseMassBody2 * linearImpulseBody2; @@ -602,14 +602,14 @@ void SliderJoint::solvePositionConstraint(const ConstraintSolverData& constraint } // If the upper limit is violated - if (mIsUpperLimitViolated) { + if (m_isUpperLimitViolated) { // Compute the Lagrange multiplier lambda for the upper limit constraint float lambdaUpperLimit = m_inverseMassMatrixLimit * (-upperLimitError); // Compute the impulse P=J^T * lambda for the upper limit constraint of body 1 - const vec3 linearImpulseBody1 = lambdaUpperLimit * mSliderAxisWorld; - const vec3 angularImpulseBody1 = lambdaUpperLimit * mR1PlusUCrossSliderAxis; + const vec3 linearImpulseBody1 = lambdaUpperLimit * m_sliderAxisWorld; + const vec3 angularImpulseBody1 = lambdaUpperLimit * m_R1PlusUCrossSliderAxis; // Apply the impulse to the body 1 const vec3 v1 = inverseMassBody1 * linearImpulseBody1; @@ -621,8 +621,8 @@ void SliderJoint::solvePositionConstraint(const ConstraintSolverData& constraint q1.normalize(); // Compute the impulse P=J^T * lambda for the upper limit constraint of body 2 - const vec3 linearImpulseBody2 = -lambdaUpperLimit * mSliderAxisWorld; - const vec3 angularImpulseBody2 = -lambdaUpperLimit * mR2CrossSliderAxis; + const vec3 linearImpulseBody2 = -lambdaUpperLimit * m_sliderAxisWorld; + const vec3 angularImpulseBody2 = -lambdaUpperLimit * m_R2CrossSliderAxis; // Apply the impulse to the body 2 const vec3 v2 = inverseMassBody2 * linearImpulseBody2; @@ -643,9 +643,9 @@ void SliderJoint::solvePositionConstraint(const ConstraintSolverData& constraint */ void SliderJoint::enableLimit(bool isLimitEnabled) { - if (isLimitEnabled != mIsLimitEnabled) { + if (isLimitEnabled != m_isLimitEnabled) { - mIsLimitEnabled = isLimitEnabled; + m_isLimitEnabled = isLimitEnabled; // Reset the limits resetLimits(); @@ -659,7 +659,7 @@ void SliderJoint::enableLimit(bool isLimitEnabled) { */ void SliderJoint::enableMotor(bool isMotorEnabled) { - mIsMotorEnabled = isMotorEnabled; + m_isMotorEnabled = isMotorEnabled; m_impulseMotor = 0.0; // Wake up the two bodies of the joint @@ -689,7 +689,7 @@ float SliderJoint::getTranslation() const { const vec3 u = anchorBody2 - anchorBody1; // Compute the slider axis in world-space - vec3 sliderAxisWorld = q1 * mSliderAxisBody1; + vec3 sliderAxisWorld = q1 * m_sliderAxisBody1; sliderAxisWorld.normalize(); // Compute and return the translation value @@ -702,11 +702,11 @@ float SliderJoint::getTranslation() const { */ void SliderJoint::setMinTranslationLimit(float lowerLimit) { - assert(lowerLimit <= mUpperLimit); + assert(lowerLimit <= m_upperLimit); - if (lowerLimit != mLowerLimit) { + if (lowerLimit != m_lowerLimit) { - mLowerLimit = lowerLimit; + m_lowerLimit = lowerLimit; // Reset the limits resetLimits(); @@ -719,11 +719,11 @@ void SliderJoint::setMinTranslationLimit(float lowerLimit) { */ void SliderJoint::setMaxTranslationLimit(float upperLimit) { - assert(mLowerLimit <= upperLimit); + assert(m_lowerLimit <= upperLimit); - if (upperLimit != mUpperLimit) { + if (upperLimit != m_upperLimit) { - mUpperLimit = upperLimit; + m_upperLimit = upperLimit; // Reset the limits resetLimits(); @@ -748,9 +748,9 @@ void SliderJoint::resetLimits() { */ void SliderJoint::setMotorSpeed(float motorSpeed) { - if (motorSpeed != mMotorSpeed) { + if (motorSpeed != m_motorSpeed) { - mMotorSpeed = motorSpeed; + m_motorSpeed = motorSpeed; // Wake up the two bodies of the joint m_body1->setIsSleeping(false); @@ -764,13 +764,76 @@ void SliderJoint::setMotorSpeed(float motorSpeed) { */ void SliderJoint::setMaxMotorForce(float maxMotorForce) { - if (maxMotorForce != mMaxMotorForce) { + if (maxMotorForce != m_maxMotorForce) { - assert(mMaxMotorForce >= 0.0); - mMaxMotorForce = maxMotorForce; + assert(m_maxMotorForce >= 0.0); + m_maxMotorForce = maxMotorForce; // Wake up the two bodies of the joint m_body1->setIsSleeping(false); m_body2->setIsSleeping(false); } } + +// Return true if the limits or the joint are enabled +/** + * @return True if the joint limits are enabled + */ +bool SliderJoint::isLimitEnabled() const { + return m_isLimitEnabled; +} + +// Return true if the motor of the joint is enabled +/** + * @return True if the joint motor is enabled + */ +bool SliderJoint::isMotorEnabled() const { + return m_isMotorEnabled; +} + +// Return the minimum translation limit +/** + * @return The minimum translation limit of the joint (in meters) + */ +float SliderJoint::getMinTranslationLimit() const { + return m_lowerLimit; +} + +// Return the maximum translation limit +/** + * @return The maximum translation limit of the joint (in meters) + */ +float SliderJoint::getMaxTranslationLimit() const { + return m_upperLimit; +} + +// Return the motor speed +/** + * @return The current motor speed of the joint (in meters per second) + */ +float SliderJoint::getMotorSpeed() const { + return m_motorSpeed; +} + +// Return the maximum motor force +/** + * @return The maximum force of the joint motor (in Newton x meters) + */ +float SliderJoint::getMaxMotorForce() const { + return m_maxMotorForce; +} + +// Return the int32_tensity of the current force applied for the joint motor +/** + * @param timeStep Time step (in seconds) + * @return The current force of the joint motor (in Newton x meters) + */ +float SliderJoint::getMotorForce(float timeStep) const { + return m_impulseMotor / timeStep; +} + +// Return the number of bytes used by the joint +size_t SliderJoint::getSizeInBytes() const { + return sizeof(SliderJoint); +} + diff --git a/ephysics/constraint/SliderJoint.hpp b/ephysics/constraint/SliderJoint.hpp index 13bdf23..e95a470 100644 --- a/ephysics/constraint/SliderJoint.hpp +++ b/ephysics/constraint/SliderJoint.hpp @@ -5,379 +5,201 @@ */ #pragma once -// Libraries #include #include namespace ephysics { + /** + * This structure is used to gather the information needed to create a slider + * joint. This structure will be used to create the actual slider joint. + */ + struct SliderJointInfo : public JointInfo { + + public : + vec3 m_anchorPointWorldSpace; //!< Anchor point (in world-space coordinates) + vec3 sliderAxisWorldSpace; //!< Slider axis (in world-space coordinates) + bool isLimitEnabled; //!< True if the slider limits are enabled + bool isMotorEnabled; //!< True if the slider motor is enabled + float minTranslationLimit; //!< Mininum allowed translation if limits are enabled + float maxTranslationLimit; //!< Maximum allowed translation if limits are enabled + float motorSpeed; //!< Motor speed + float maxMotorForce; //!< Maximum motor force (in Newtons) that can be applied to reach to desired motor speed + /** + * @brief Constructor without limits and without motor + * @param[in] _rigidBody1 The first body of the joint + * @param[in] _rigidBody2 The second body of the joint + * @param[in] _initAnchorPointWorldSpace The initial anchor point in world-space + * @param[in] _initSliderAxisWorldSpace The initial slider axis in world-space + */ + SliderJointInfo(RigidBody* _rigidBody1, + RigidBody* _rigidBody2, + const vec3& _initAnchorPointWorldSpace, + const vec3& _initSliderAxisWorldSpace): + JointInfo(_rigidBody1, _rigidBody2, SLIDERJOINT), + m_anchorPointWorldSpace(_initAnchorPointWorldSpace), + sliderAxisWorldSpace(_initSliderAxisWorldSpace), + isLimitEnabled(false), + isMotorEnabled(false), + minTranslationLimit(-1.0), + maxTranslationLimit(1.0), + motorSpeed(0), + maxMotorForce(0) { + + } + /** + * @brief Constructor with limits and no motor + * @param[in] _rigidBody1 The first body of the joint + * @param[in] _rigidBody2 The second body of the joint + * @param[in] _initAnchorPointWorldSpace The initial anchor point in world-space + * @param[in] _initSliderAxisWorldSpace The initial slider axis in world-space + * @param[in] _initMinTranslationLimit The initial minimum translation limit (in meters) + * @param[in] _initMaxTranslationLimit The initial maximum translation limit (in meters) + */ + SliderJointInfo(RigidBody* _rigidBody1, + RigidBody* _rigidBody2, + const vec3& _initAnchorPointWorldSpace, + const vec3& _initSliderAxisWorldSpace, + float _initMinTranslationLimit, + float _initMaxTranslationLimit): + JointInfo(_rigidBody1, _rigidBody2, SLIDERJOINT), + m_anchorPointWorldSpace(_initAnchorPointWorldSpace), + sliderAxisWorldSpace(_initSliderAxisWorldSpace), + isLimitEnabled(true), + isMotorEnabled(false), + minTranslationLimit(_initMinTranslationLimit), + maxTranslationLimit(_initMaxTranslationLimit), + motorSpeed(0), + maxMotorForce(0) { + + } + /** + * @brief Constructor with limits and motor + * @param[in] _rigidBody1 The first body of the joint + * @param[in] _rigidBody2 The second body of the joint + * @param[in] _initAnchorPointWorldSpace The initial anchor point in world-space + * @param[in] _initSliderAxisWorldSpace The initial slider axis in world-space + * @param[in] _initMinTranslationLimit The initial minimum translation limit (in meters) + * @param[in] _initMaxTranslationLimit The initial maximum translation limit (in meters) + * @param[in] _initMotorSpeed The initial speed of the joint motor (in meters per second) + * @param[in] _initMaxMotorForce The initial maximum motor force of the joint (in Newtons x meters) + */ + SliderJointInfo(RigidBody* _rigidBody1, + RigidBody* _rigidBody2, + const vec3& _initAnchorPointWorldSpace, + const vec3& _initSliderAxisWorldSpace, + float _initMinTranslationLimit, + float _initMaxTranslationLimit, + float _initMotorSpeed, + float _initMaxMotorForce): + JointInfo(_rigidBody1, _rigidBody2, SLIDERJOINT), + m_anchorPointWorldSpace(_initAnchorPointWorldSpace), + sliderAxisWorldSpace(_initSliderAxisWorldSpace), + isLimitEnabled(true), + isMotorEnabled(true), + minTranslationLimit(_initMinTranslationLimit), + maxTranslationLimit(_initMaxTranslationLimit), + motorSpeed(_initMotorSpeed), + maxMotorForce(_initMaxMotorForce) { + + } + }; + + /** + * @brief This class represents a slider joint. This joint has a one degree of freedom. + * It only allows relative translation of the bodies along a single direction and no + * rotation. + */ + class SliderJoint: public Joint { + private: + static const float BETA; //!< Beta value for the position correction bias factor + vec3 m_localAnchorPointBody1; //!< Anchor point of body 1 (in local-space coordinates of body 1) + vec3 m_localAnchorPointBody2; //!< Anchor point of body 2 (in local-space coordinates of body 2) + vec3 m_sliderAxisBody1; //!< Slider axis (in local-space coordinates of body 1) + etk::Matrix3x3 m_i1; //!< Inertia tensor of body 1 (in world-space coordinates) + etk::Matrix3x3 m_i2; //!< Inertia tensor of body 2 (in world-space coordinates) + etk::Quaternion m_initOrientationDifferenceInv; //!< Inverse of the initial orientation difference between the two bodies + vec3 m_N1; //!< First vector orthogonal to the slider axis local-space of body 1 + vec3 m_N2; //!< Second vector orthogonal to the slider axis and m_N1 in local-space of body 1 + vec3 m_R1; //!< Vector r1 in world-space coordinates + vec3 m_R2; //!< Vector r2 in world-space coordinates + vec3 m_R2CrossN1; //!< Cross product of r2 and n1 + vec3 m_R2CrossN2; //!< Cross product of r2 and n2 + vec3 m_R2CrossSliderAxis; //!< Cross product of r2 and the slider axis + vec3 m_R1PlusUCrossN1; //!< Cross product of vector (r1 + u) and n1 + vec3 m_R1PlusUCrossN2; //!< Cross product of vector (r1 + u) and n2 + vec3 m_R1PlusUCrossSliderAxis; //!< Cross product of vector (r1 + u) and the slider axis + vec2 m_bTranslation; //!< Bias of the 2 translation constraints + vec3 m_bRotation; //!< Bias of the 3 rotation constraints + float m_bLowerLimit; //!< Bias of the lower limit constraint + float m_bUpperLimit; //!< Bias of the upper limit constraint + etk::Matrix2x2 m_inverseMassMatrixTranslationConstraint; //!< Inverse of mass matrix K=JM^-1J^t for the translation constraint (2x2 matrix) + etk::Matrix3x3 m_inverseMassMatrixRotationConstraint; //!< Inverse of mass matrix K=JM^-1J^t for the rotation constraint (3x3 matrix) + float m_inverseMassMatrixLimit; //!< Inverse of mass matrix K=JM^-1J^t for the upper and lower limit constraints (1x1 matrix) + float m_inverseMassMatrixMotor; //!< Inverse of mass matrix K=JM^-1J^t for the motor + vec2 m_impulseTranslation; //!< Accumulated impulse for the 2 translation constraints + vec3 m_impulseRotation; //!< Accumulated impulse for the 3 rotation constraints + float m_impulseLowerLimit; //!< Accumulated impulse for the lower limit constraint + float m_impulseUpperLimit; //!< Accumulated impulse for the upper limit constraint + float m_impulseMotor; //!< Accumulated impulse for the motor + bool m_isLimitEnabled; //!< True if the slider limits are enabled + bool m_isMotorEnabled; //!< True if the motor of the joint in enabled + vec3 m_sliderAxisWorld; //!< Slider axis in world-space coordinates + float m_lowerLimit; //!< Lower limit (minimum translation distance) + float m_upperLimit; //!< Upper limit (maximum translation distance) + bool m_isLowerLimitViolated; //!< True if the lower limit is violated + bool m_isUpperLimitViolated; //!< True if the upper limit is violated + float m_motorSpeed; //!< Motor speed (in m/s) + float m_maxMotorForce; //!< Maximum motor force (in Newtons) that can be applied to reach to desired motor speed + /// Private copy-constructor + SliderJoint(const SliderJoint& _constraint); + /// Private assignment operator + SliderJoint& operator=(const SliderJoint& _constraint); + /// Reset the limits + void resetLimits(); + /// Return the number of bytes used by the joint + virtual size_t getSizeInBytes() const; + /// Initialize before solving the constraint + virtual void initBeforeSolve(const ConstraintSolverData& _constraintSolverData); + /// Warm start the constraint (apply the previous impulse at the beginning of the step) + virtual void warmstart(const ConstraintSolverData& _constraintSolverData); + /// Solve the velocity constraint + virtual void solveVelocityConstraint(const ConstraintSolverData& _constraintSolverData); + /// Solve the position constraint (for position error correction) + virtual void solvePositionConstraint(const ConstraintSolverData& _constraintSolverData); + public : + /// Constructor + SliderJoint(const SliderJointInfo& _jointInfo); + /// Destructor + virtual ~SliderJoint(); + /// Return true if the limits or the joint are enabled + bool isLimitEnabled() const; + /// Return true if the motor of the joint is enabled + bool isMotorEnabled() const; + /// Enable/Disable the limits of the joint + void enableLimit(bool _isLimitEnabled); + /// Enable/Disable the motor of the joint + void enableMotor(bool _isMotorEnabled); + /// Return the current translation value of the joint + float getTranslation() const; + /// Return the minimum translation limit + float getMinTranslationLimit() const; + /// Set the minimum translation limit + void setMinTranslationLimit(float _lowerLimit); + /// Return the maximum translation limit + float getMaxTranslationLimit() const; + /// Set the maximum translation limit + void setMaxTranslationLimit(float _upperLimit); + /// Return the motor speed + float getMotorSpeed() const; + /// Set the motor speed + void setMotorSpeed(float _motorSpeed); + /// Return the maximum motor force + float getMaxMotorForce() const; + /// Set the maximum motor force + void setMaxMotorForce(float _maxMotorForce); + /// Return the int32_tensity of the current force applied for the joint motor + float getMotorForce(float _timeStep) const; + }; -// Structure SliderJointInfo -/** - * This structure is used to gather the information needed to create a slider - * joint. This structure will be used to create the actual slider joint. - */ -struct SliderJointInfo : public JointInfo { - - public : - - // -------------------- Attributes -------------------- // - - /// Anchor point (in world-space coordinates) - vec3 m_anchorPointWorldSpace; - - /// Slider axis (in world-space coordinates) - vec3 sliderAxisWorldSpace; - - /// True if the slider limits are enabled - bool isLimitEnabled; - - /// True if the slider motor is enabled - bool isMotorEnabled; - - /// Mininum allowed translation if limits are enabled - float minTranslationLimit; - - /// Maximum allowed translation if limits are enabled - float maxTranslationLimit; - - /// Motor speed - float motorSpeed; - - /// Maximum motor force (in Newtons) that can be applied to reach to desired motor speed - float maxMotorForce; - - /// Constructor without limits and without motor - /** - * @param rigidBody1 The first body of the joint - * @param rigidBody2 The second body of the joint - * @param initAnchorPointWorldSpace The initial anchor point in world-space - * @param initSliderAxisWorldSpace The initial slider axis in world-space - */ - SliderJointInfo(RigidBody* rigidBody1, RigidBody* rigidBody2, - const vec3& initAnchorPointWorldSpace, - const vec3& initSliderAxisWorldSpace) - : JointInfo(rigidBody1, rigidBody2, SLIDERJOINT), - m_anchorPointWorldSpace(initAnchorPointWorldSpace), - sliderAxisWorldSpace(initSliderAxisWorldSpace), - isLimitEnabled(false), isMotorEnabled(false), minTranslationLimit(-1.0), - maxTranslationLimit(1.0), motorSpeed(0), maxMotorForce(0) {} - - /// Constructor with limits and no motor - /** - * @param rigidBody1 The first body of the joint - * @param rigidBody2 The second body of the joint - * @param initAnchorPointWorldSpace The initial anchor point in world-space - * @param initSliderAxisWorldSpace The initial slider axis in world-space - * @param initMinTranslationLimit The initial minimum translation limit (in meters) - * @param initMaxTranslationLimit The initial maximum translation limit (in meters) - */ - SliderJointInfo(RigidBody* rigidBody1, RigidBody* rigidBody2, - const vec3& initAnchorPointWorldSpace, - const vec3& initSliderAxisWorldSpace, - float initMinTranslationLimit, float initMaxTranslationLimit) - : JointInfo(rigidBody1, rigidBody2, SLIDERJOINT), - m_anchorPointWorldSpace(initAnchorPointWorldSpace), - sliderAxisWorldSpace(initSliderAxisWorldSpace), - isLimitEnabled(true), isMotorEnabled(false), - minTranslationLimit(initMinTranslationLimit), - maxTranslationLimit(initMaxTranslationLimit), motorSpeed(0), - maxMotorForce(0) {} - - /// Constructor with limits and motor - /** - * @param rigidBody1 The first body of the joint - * @param rigidBody2 The second body of the joint - * @param initAnchorPointWorldSpace The initial anchor point in world-space - * @param initSliderAxisWorldSpace The initial slider axis in world-space - * @param initMinTranslationLimit The initial minimum translation limit (in meters) - * @param initMaxTranslationLimit The initial maximum translation limit (in meters) - * @param initMotorSpeed The initial speed of the joint motor (in meters per second) - * @param initMaxMotorForce The initial maximum motor force of the joint (in Newtons x meters) - */ - SliderJointInfo(RigidBody* rigidBody1, RigidBody* rigidBody2, - const vec3& initAnchorPointWorldSpace, - const vec3& initSliderAxisWorldSpace, - float initMinTranslationLimit, float initMaxTranslationLimit, - float initMotorSpeed, float initMaxMotorForce) - : JointInfo(rigidBody1, rigidBody2, SLIDERJOINT), - m_anchorPointWorldSpace(initAnchorPointWorldSpace), - sliderAxisWorldSpace(initSliderAxisWorldSpace), - isLimitEnabled(true), isMotorEnabled(true), - minTranslationLimit(initMinTranslationLimit), - maxTranslationLimit(initMaxTranslationLimit), motorSpeed(initMotorSpeed), - maxMotorForce(initMaxMotorForce) {} -}; - -// Class SliderJoint -/** - * This class represents a slider joint. This joint has a one degree of freedom. - * It only allows relative translation of the bodies along a single direction and no - * rotation. - */ -class SliderJoint : public Joint { - - private : - - // -------------------- Constants -------------------- // - - // Beta value for the position correction bias factor - static const float BETA; - - // -------------------- Attributes -------------------- // - - /// Anchor point of body 1 (in local-space coordinates of body 1) - vec3 m_localAnchorPointBody1; - - /// Anchor point of body 2 (in local-space coordinates of body 2) - vec3 m_localAnchorPointBody2; - - /// Slider axis (in local-space coordinates of body 1) - vec3 mSliderAxisBody1; - - /// Inertia tensor of body 1 (in world-space coordinates) - etk::Matrix3x3 m_i1; - - /// Inertia tensor of body 2 (in world-space coordinates) - etk::Matrix3x3 m_i2; - - /// Inverse of the initial orientation difference between the two bodies - etk::Quaternion m_initOrientationDifferenceInv; - - /// First vector orthogonal to the slider axis local-space of body 1 - vec3 mN1; - - /// Second vector orthogonal to the slider axis and mN1 in local-space of body 1 - vec3 mN2; - - /// Vector r1 in world-space coordinates - vec3 mR1; - - /// Vector r2 in world-space coordinates - vec3 mR2; - - /// Cross product of r2 and n1 - vec3 mR2CrossN1; - - /// Cross product of r2 and n2 - vec3 mR2CrossN2; - - /// Cross product of r2 and the slider axis - vec3 mR2CrossSliderAxis; - - /// Cross product of vector (r1 + u) and n1 - vec3 mR1PlusUCrossN1; - - /// Cross product of vector (r1 + u) and n2 - vec3 mR1PlusUCrossN2; - - /// Cross product of vector (r1 + u) and the slider axis - vec3 mR1PlusUCrossSliderAxis; - - /// Bias of the 2 translation constraints - vec2 mBTranslation; - - /// Bias of the 3 rotation constraints - vec3 mBRotation; - - /// Bias of the lower limit constraint - float mBLowerLimit; - - /// Bias of the upper limit constraint - float mBUpperLimit; - - /// Inverse of mass matrix K=JM^-1J^t for the translation constraint (2x2 matrix) - etk::Matrix2x2 m_inverseMassMatrixTranslationConstraint; - - /// Inverse of mass matrix K=JM^-1J^t for the rotation constraint (3x3 matrix) - etk::Matrix3x3 m_inverseMassMatrixRotationConstraint; - - /// Inverse of mass matrix K=JM^-1J^t for the upper and lower limit constraints (1x1 matrix) - float m_inverseMassMatrixLimit; - - /// Inverse of mass matrix K=JM^-1J^t for the motor - float m_inverseMassMatrixMotor; - - /// Accumulated impulse for the 2 translation constraints - vec2 m_impulseTranslation; - - /// Accumulated impulse for the 3 rotation constraints - vec3 m_impulseRotation; - - /// Accumulated impulse for the lower limit constraint - float m_impulseLowerLimit; - - /// Accumulated impulse for the upper limit constraint - float m_impulseUpperLimit; - - /// Accumulated impulse for the motor - float m_impulseMotor; - - /// True if the slider limits are enabled - bool mIsLimitEnabled; - - /// True if the motor of the joint in enabled - bool mIsMotorEnabled; - - /// Slider axis in world-space coordinates - vec3 mSliderAxisWorld; - - /// Lower limit (minimum translation distance) - float mLowerLimit; - - /// Upper limit (maximum translation distance) - float mUpperLimit; - - /// True if the lower limit is violated - bool mIsLowerLimitViolated; - - /// True if the upper limit is violated - bool mIsUpperLimitViolated; - - /// Motor speed (in m/s) - float mMotorSpeed; - - /// Maximum motor force (in Newtons) that can be applied to reach to desired motor speed - float mMaxMotorForce; - - // -------------------- Methods -------------------- // - - /// Private copy-constructor - SliderJoint(const SliderJoint& constraint); - - /// Private assignment operator - SliderJoint& operator=(const SliderJoint& constraint); - - /// Reset the limits - void resetLimits(); - - /// Return the number of bytes used by the joint - virtual size_t getSizeInBytes() const; - - /// Initialize before solving the constraint - virtual void initBeforeSolve(const ConstraintSolverData& constraintSolverData); - - /// Warm start the constraint (apply the previous impulse at the beginning of the step) - virtual void warmstart(const ConstraintSolverData& constraintSolverData); - - /// Solve the velocity constraint - virtual void solveVelocityConstraint(const ConstraintSolverData& constraintSolverData); - - /// Solve the position constraint (for position error correction) - virtual void solvePositionConstraint(const ConstraintSolverData& constraintSolverData); - - public : - - // -------------------- Methods -------------------- // - - /// Constructor - SliderJoint(const SliderJointInfo& jointInfo); - - /// Destructor - virtual ~SliderJoint(); - - /// Return true if the limits or the joint are enabled - bool isLimitEnabled() const; - - /// Return true if the motor of the joint is enabled - bool isMotorEnabled() const; - - /// Enable/Disable the limits of the joint - void enableLimit(bool isLimitEnabled); - - /// Enable/Disable the motor of the joint - void enableMotor(bool isMotorEnabled); - - /// Return the current translation value of the joint - float getTranslation() const; - - /// Return the minimum translation limit - float getMinTranslationLimit() const; - - /// Set the minimum translation limit - void setMinTranslationLimit(float lowerLimit); - - /// Return the maximum translation limit - float getMaxTranslationLimit() const; - - /// Set the maximum translation limit - void setMaxTranslationLimit(float upperLimit); - - /// Return the motor speed - float getMotorSpeed() const; - - /// Set the motor speed - void setMotorSpeed(float motorSpeed); - - /// Return the maximum motor force - float getMaxMotorForce() const; - - /// Set the maximum motor force - void setMaxMotorForce(float maxMotorForce); - - /// Return the int32_tensity of the current force applied for the joint motor - float getMotorForce(float timeStep) const; -}; - -// Return true if the limits or the joint are enabled -/** - * @return True if the joint limits are enabled - */ -bool SliderJoint::isLimitEnabled() const { - return mIsLimitEnabled; -} - -// Return true if the motor of the joint is enabled -/** - * @return True if the joint motor is enabled - */ -bool SliderJoint::isMotorEnabled() const { - return mIsMotorEnabled; -} - -// Return the minimum translation limit -/** - * @return The minimum translation limit of the joint (in meters) - */ -float SliderJoint::getMinTranslationLimit() const { - return mLowerLimit; -} - -// Return the maximum translation limit -/** - * @return The maximum translation limit of the joint (in meters) - */ -float SliderJoint::getMaxTranslationLimit() const { - return mUpperLimit; -} - -// Return the motor speed -/** - * @return The current motor speed of the joint (in meters per second) - */ -float SliderJoint::getMotorSpeed() const { - return mMotorSpeed; -} - -// Return the maximum motor force -/** - * @return The maximum force of the joint motor (in Newton x meters) - */ -float SliderJoint::getMaxMotorForce() const { - return mMaxMotorForce; -} - -// Return the int32_tensity of the current force applied for the joint motor -/** - * @param timeStep Time step (in seconds) - * @return The current force of the joint motor (in Newton x meters) - */ -float SliderJoint::getMotorForce(float timeStep) const { - return m_impulseMotor / timeStep; -} - -// Return the number of bytes used by the joint -size_t SliderJoint::getSizeInBytes() const { - return sizeof(SliderJoint); -} } diff --git a/ephysics/engine/ConstraintSolver.cpp b/ephysics/engine/ConstraintSolver.cpp index 724b5c0..570bd5b 100644 --- a/ephysics/engine/ConstraintSolver.cpp +++ b/ephysics/engine/ConstraintSolver.cpp @@ -9,73 +9,68 @@ using namespace ephysics; -// Constructor -ConstraintSolver::ConstraintSolver(const std::map& mapBodyToVelocityIndex) - : m_mapBodyToConstrainedVelocityIndex(mapBodyToVelocityIndex), - m_isWarmStartingActive(true), m_constraintSolverData(mapBodyToVelocityIndex) { - +ConstraintSolver::ConstraintSolver(const std::map& _mapBodyToVelocityIndex): + m_mapBodyToConstrainedVelocityIndex(_mapBodyToVelocityIndex), + m_isWarmStartingActive(true), + m_constraintSolverData(_mapBodyToVelocityIndex) { + } -// Initialize the constraint solver for a given island -void ConstraintSolver::initializeForIsland(float dt, Island* island) { - +void ConstraintSolver::initializeForIsland(float _dt, Island* _island) { PROFILE("ConstraintSolver::initializeForIsland()"); - - assert(island != NULL); - assert(island->getNbBodies() > 0); - assert(island->getNbJoints() > 0); - + assert(_island != nullptr); + assert(_island->getNbBodies() > 0); + assert(_island->getNbJoints() > 0); // Set the current time step - m_timeStep = dt; - + m_timeStep = _dt; // Initialize the constraint solver data used to initialize and solve the constraints m_constraintSolverData.timeStep = m_timeStep; m_constraintSolverData.isWarmStartingActive = m_isWarmStartingActive; - // For each joint of the island - Joint** joints = island->getJoints(); - for (uint32_t i=0; igetNbJoints(); i++) { - + Joint** joints = _island->getJoints(); + for (uint32_t iii=0; iii<_island->getNbJoints(); ++iii) { // Initialize the constraint before solving it - joints[i]->initBeforeSolve(m_constraintSolverData); - + joints[iii]->initBeforeSolve(m_constraintSolverData); // Warm-start the constraint if warm-starting is enabled if (m_isWarmStartingActive) { - joints[i]->warmstart(m_constraintSolverData); + joints[iii]->warmstart(m_constraintSolverData); } } } -// Solve the velocity constraints -void ConstraintSolver::solveVelocityConstraints(Island* island) { - +void ConstraintSolver::solveVelocityConstraints(Island* _island) { PROFILE("ConstraintSolver::solveVelocityConstraints()"); - - assert(island != NULL); - assert(island->getNbJoints() > 0); - + assert(_island != nullptr); + assert(_island->getNbJoints() > 0); // For each joint of the island - Joint** joints = island->getJoints(); - for (uint32_t i=0; igetNbJoints(); i++) { - - // Solve the constraint - joints[i]->solveVelocityConstraint(m_constraintSolverData); + Joint** joints = _island->getJoints(); + for (uint32_t iii=0; iii<_island->getNbJoints(); ++iii) { + joints[iii]->solveVelocityConstraint(m_constraintSolverData); } } -// Solve the position constraints -void ConstraintSolver::solvePositionConstraints(Island* island) { - +void ConstraintSolver::solvePositionConstraints(Island* _island) { PROFILE("ConstraintSolver::solvePositionConstraints()"); - - assert(island != NULL); - assert(island->getNbJoints() > 0); - - // For each joint of the island - Joint** joints = island->getJoints(); - for (uint32_t i=0; i < island->getNbJoints(); i++) { - - // Solve the constraint - joints[i]->solvePositionConstraint(m_constraintSolverData); + assert(_island != nullptr); + assert(_island->getNbJoints() > 0); + Joint** joints = _island->getJoints(); + for (uint32_t iii=0; iii < _island->getNbJoints(); ++iii) { + joints[iii]->solvePositionConstraint(m_constraintSolverData); } } + +void ConstraintSolver::setConstrainedVelocitiesArrays(vec3* _constrainedLinearVelocities, + vec3* _constrainedAngularVelocities) { + assert(_constrainedLinearVelocities != nullptr); + assert(_constrainedAngularVelocities != nullptr); + m_constraintSolverData.linearVelocities = _constrainedLinearVelocities; + m_constraintSolverData.angularVelocities = _constrainedAngularVelocities; +} + +void ConstraintSolver::setConstrainedPositionsArrays(vec3* _constrainedPositions, + etk::Quaternion* _constrainedOrientations) { + assert(_constrainedPositions != nullptr); + assert(_constrainedOrientations != nullptr); + m_constraintSolverData.positions = _constrainedPositions; + m_constraintSolverData.orientations = _constrainedOrientations; +} \ No newline at end of file diff --git a/ephysics/engine/ConstraintSolver.hpp b/ephysics/engine/ConstraintSolver.hpp index 93cfa8c..f36e553 100644 --- a/ephysics/engine/ConstraintSolver.hpp +++ b/ephysics/engine/ConstraintSolver.hpp @@ -130,22 +130,4 @@ namespace ephysics { etk::Quaternion* constrainedOrientations); }; -// Set the constrained velocities arrays -void ConstraintSolver::setConstrainedVelocitiesArrays(vec3* constrainedLinearVelocities, - vec3* constrainedAngularVelocities) { - assert(constrainedLinearVelocities != NULL); - assert(constrainedAngularVelocities != NULL); - m_constraintSolverData.linearVelocities = constrainedLinearVelocities; - m_constraintSolverData.angularVelocities = constrainedAngularVelocities; -} - -// Set the constrained positions/orientations arrays -void ConstraintSolver::setConstrainedPositionsArrays(vec3* constrainedPositions, - etk::Quaternion* constrainedOrientations) { - assert(constrainedPositions != NULL); - assert(constrainedOrientations != NULL); - m_constraintSolverData.positions = constrainedPositions; - m_constraintSolverData.orientations = constrainedOrientations; -} - } diff --git a/ephysics/engine/ContactSolver.cpp b/ephysics/engine/ContactSolver.cpp index ba76fa3..4be9a42 100644 --- a/ephysics/engine/ContactSolver.cpp +++ b/ephysics/engine/ContactSolver.cpp @@ -894,3 +894,89 @@ void ContactSolver::cleanup() { m_contactConstraints = nullptr; } } + +// Set the split velocities arrays +void ContactSolver::setSplitVelocitiesArrays(vec3* splitLinearVelocities, + vec3* splitAngularVelocities) { + assert(splitLinearVelocities != NULL); + assert(splitAngularVelocities != NULL); + m_splitLinearVelocities = splitLinearVelocities; + m_splitAngularVelocities = splitAngularVelocities; +} + +// Set the constrained velocities arrays +void ContactSolver::setConstrainedVelocitiesArrays(vec3* constrainedLinearVelocities, + vec3* constrainedAngularVelocities) { + assert(constrainedLinearVelocities != NULL); + assert(constrainedAngularVelocities != NULL); + m_linearVelocities = constrainedLinearVelocities; + m_angularVelocities = constrainedAngularVelocities; +} + +// Return true if the split impulses position correction technique is used for contacts +bool ContactSolver::isSplitImpulseActive() const { + return m_isSplitImpulseActive; +} + +// Activate or Deactivate the split impulses for contacts +void ContactSolver::setIsSplitImpulseActive(bool 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 +void ContactSolver::setIsSolveFrictionAtContactManifoldCenterActive(bool isActive) { + m_isSolveFrictionAtContactManifoldCenterActive = isActive; +} + +// Compute the collision restitution factor from the restitution factor of each body +float ContactSolver::computeMixedRestitutionFactor(RigidBody* body1, + RigidBody* body2) const { + float restitution1 = body1->getMaterial().getBounciness(); + float restitution2 = body2->getMaterial().getBounciness(); + + // Return the largest restitution factor + return (restitution1 > restitution2) ? restitution1 : restitution2; +} + +// Compute the mixed friction coefficient from the friction coefficient of each body +float ContactSolver::computeMixedFrictionCoefficient(RigidBody *body1, + RigidBody *body2) const { + // Use the geometric mean to compute the mixed friction coefficient + return sqrt(body1->getMaterial().getFrictionCoefficient() * + body2->getMaterial().getFrictionCoefficient()); +} + +// Compute th mixed rolling resistance factor between two bodies +float ContactSolver::computeMixedRollingResistance(RigidBody* body1, + RigidBody* body2) const { + return float(0.5f) * (body1->getMaterial().getRollingResistance() + body2->getMaterial().getRollingResistance()); +} + +// Compute a penetration constraint impulse +const Impulse ContactSolver::computePenetrationImpulse(float deltaLambda, + const ContactPointSolver& contactPoint) + const { + return Impulse(-contactPoint.normal * deltaLambda, -contactPoint.r1CrossN * deltaLambda, + contactPoint.normal * deltaLambda, contactPoint.r2CrossN * deltaLambda); +} + +// Compute the first friction constraint impulse +const Impulse ContactSolver::computeFriction1Impulse(float deltaLambda, + const ContactPointSolver& contactPoint) + const { + return Impulse(-contactPoint.frictionVector1 * deltaLambda, + -contactPoint.r1CrossT1 * deltaLambda, + contactPoint.frictionVector1 * deltaLambda, + contactPoint.r2CrossT1 * deltaLambda); +} + +// Compute the second friction constraint impulse +const Impulse ContactSolver::computeFriction2Impulse(float deltaLambda, + const ContactPointSolver& contactPoint) + const { + return Impulse(-contactPoint.frictionvec2 * deltaLambda, + -contactPoint.r1CrossT2 * deltaLambda, + contactPoint.frictionvec2 * deltaLambda, + contactPoint.r2CrossT2 * deltaLambda); +} diff --git a/ephysics/engine/ContactSolver.hpp b/ephysics/engine/ContactSolver.hpp index 61582b4..b0715c0 100644 --- a/ephysics/engine/ContactSolver.hpp +++ b/ephysics/engine/ContactSolver.hpp @@ -237,90 +237,4 @@ namespace ephysics { void cleanup(); }; -// Set the split velocities arrays -void ContactSolver::setSplitVelocitiesArrays(vec3* splitLinearVelocities, - vec3* splitAngularVelocities) { - assert(splitLinearVelocities != NULL); - assert(splitAngularVelocities != NULL); - m_splitLinearVelocities = splitLinearVelocities; - m_splitAngularVelocities = splitAngularVelocities; -} - -// Set the constrained velocities arrays -void ContactSolver::setConstrainedVelocitiesArrays(vec3* constrainedLinearVelocities, - vec3* constrainedAngularVelocities) { - assert(constrainedLinearVelocities != NULL); - assert(constrainedAngularVelocities != NULL); - m_linearVelocities = constrainedLinearVelocities; - m_angularVelocities = constrainedAngularVelocities; -} - -// Return true if the split impulses position correction technique is used for contacts -bool ContactSolver::isSplitImpulseActive() const { - return m_isSplitImpulseActive; -} - -// Activate or Deactivate the split impulses for contacts -void ContactSolver::setIsSplitImpulseActive(bool 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 -void ContactSolver::setIsSolveFrictionAtContactManifoldCenterActive(bool isActive) { - m_isSolveFrictionAtContactManifoldCenterActive = isActive; -} - -// Compute the collision restitution factor from the restitution factor of each body -float ContactSolver::computeMixedRestitutionFactor(RigidBody* body1, - RigidBody* body2) const { - float restitution1 = body1->getMaterial().getBounciness(); - float restitution2 = body2->getMaterial().getBounciness(); - - // Return the largest restitution factor - return (restitution1 > restitution2) ? restitution1 : restitution2; -} - -// Compute the mixed friction coefficient from the friction coefficient of each body -float ContactSolver::computeMixedFrictionCoefficient(RigidBody *body1, - RigidBody *body2) const { - // Use the geometric mean to compute the mixed friction coefficient - return sqrt(body1->getMaterial().getFrictionCoefficient() * - body2->getMaterial().getFrictionCoefficient()); -} - -// Compute th mixed rolling resistance factor between two bodies -float ContactSolver::computeMixedRollingResistance(RigidBody* body1, - RigidBody* body2) const { - return float(0.5f) * (body1->getMaterial().getRollingResistance() + body2->getMaterial().getRollingResistance()); -} - -// Compute a penetration constraint impulse -const Impulse ContactSolver::computePenetrationImpulse(float deltaLambda, - const ContactPointSolver& contactPoint) - const { - return Impulse(-contactPoint.normal * deltaLambda, -contactPoint.r1CrossN * deltaLambda, - contactPoint.normal * deltaLambda, contactPoint.r2CrossN * deltaLambda); -} - -// Compute the first friction constraint impulse -const Impulse ContactSolver::computeFriction1Impulse(float deltaLambda, - const ContactPointSolver& contactPoint) - const { - return Impulse(-contactPoint.frictionVector1 * deltaLambda, - -contactPoint.r1CrossT1 * deltaLambda, - contactPoint.frictionVector1 * deltaLambda, - contactPoint.r2CrossT1 * deltaLambda); -} - -// Compute the second friction constraint impulse -const Impulse ContactSolver::computeFriction2Impulse(float deltaLambda, - const ContactPointSolver& contactPoint) - const { - return Impulse(-contactPoint.frictionvec2 * deltaLambda, - -contactPoint.r1CrossT2 * deltaLambda, - contactPoint.frictionvec2 * deltaLambda, - contactPoint.r2CrossT2 * deltaLambda); -} - } diff --git a/ephysics/engine/DynamicsWorld.cpp b/ephysics/engine/DynamicsWorld.cpp index a122f3b..ff749e1 100644 --- a/ephysics/engine/DynamicsWorld.cpp +++ b/ephysics/engine/DynamicsWorld.cpp @@ -985,3 +985,219 @@ std::vector DynamicsWorld::getContactsList() const { // Return all the contact manifold return contactManifolds; } + +// Reset the external force and torque applied to the bodies +void DynamicsWorld::resetBodiesForceAndTorque() { + + // For each body of the world + std::set::iterator it; + for (it = m_rigidBodies.begin(); it != m_rigidBodies.end(); ++it) { + (*it)->m_externalForce.setZero(); + (*it)->m_externalTorque.setZero(); + } +} + +// Get the number of iterations for the velocity constraint solver +uint32_t DynamicsWorld::getNbIterationsVelocitySolver() const { + return m_nbVelocitySolverIterations; +} + +// Set the number of iterations for the velocity constraint solver +/** + * @param nbIterations Number of iterations for the velocity solver + */ +void DynamicsWorld::setNbIterationsVelocitySolver(uint32_t nbIterations) { + m_nbVelocitySolverIterations = nbIterations; +} + +// Get the number of iterations for the position constraint solver +uint32_t DynamicsWorld::getNbIterationsPositionSolver() const { + return m_nbPositionSolverIterations; +} + +// Set the number of iterations for the position constraint solver +/** + * @param nbIterations Number of iterations for the position solver + */ +void DynamicsWorld::setNbIterationsPositionSolver(uint32_t nbIterations) { + m_nbPositionSolverIterations = nbIterations; +} + +// Set the position correction technique used for contacts +/** + * @param technique Technique used for the position correction (Baumgarte or Split Impulses) + */ +void DynamicsWorld::setContactsPositionCorrectionTechnique( + ContactsPositionCorrectionTechnique technique) { + if (technique == BAUMGARTE_CONTACTS) { + m_contactSolver.setIsSplitImpulseActive(false); + } + else { + m_contactSolver.setIsSplitImpulseActive(true); + } +} + +// Set the position correction technique used for joints +/** + * @param technique Technique used for the joins position correction (Baumgarte or Non Linear Gauss Seidel) + */ +void DynamicsWorld::setJointsPositionCorrectionTechnique( + JointsPositionCorrectionTechnique technique) { + if (technique == BAUMGARTE_JOINTS) { + m_constraintSolver.setIsNonLinearGaussSeidelPositionCorrectionActive(false); + } + else { + m_constraintSolver.setIsNonLinearGaussSeidelPositionCorrectionActive(true); + } +} + +// Activate or deactivate the solving of friction constraints at the center of +// the contact manifold instead of solving them at each contact point +/** + * @param isActive True if you want the friction to be solved at the center of + * the contact manifold and false otherwise + */ +void DynamicsWorld::setIsSolveFrictionAtContactManifoldCenterActive(bool isActive) { + m_contactSolver.setIsSolveFrictionAtContactManifoldCenterActive(isActive); +} + +// Return the gravity vector of the world +/** + * @return The current gravity vector (in meter per seconds squared) + */ +vec3 DynamicsWorld::getGravity() const { + return m_gravity; +} + +// Set the gravity vector of the world +/** + * @param gravity The gravity vector (in meter per seconds squared) + */ +void DynamicsWorld::setGravity(vec3& gravity) { + m_gravity = gravity; +} + +// Return if the gravity is enaled +/** + * @return True if the gravity is enabled in the world + */ +bool DynamicsWorld::isGravityEnabled() const { + return m_isGravityEnabled; +} + +// Enable/Disable the gravity +/** + * @param isGravityEnabled True if you want to enable the gravity in the world + * and false otherwise + */ +void DynamicsWorld::setIsGratityEnabled(bool isGravityEnabled) { + m_isGravityEnabled = isGravityEnabled; +} + +// Return the number of rigid bodies in the world +/** + * @return Number of rigid bodies in the world + */ +uint32_t DynamicsWorld::getNbRigidBodies() const { + return m_rigidBodies.size(); +} + +/// Return the number of joints in the world +/** + * @return Number of joints in the world + */ +uint32_t DynamicsWorld::getNbJoints() const { + return m_joints.size(); +} + +// Return an iterator to the beginning of the bodies of the physics world +/** + * @return Starting iterator of the set of rigid bodies + */ +std::set::iterator DynamicsWorld::getRigidBodiesBeginIterator() { + return m_rigidBodies.begin(); +} + +// Return an iterator to the end of the bodies of the physics world +/** + * @return Ending iterator of the set of rigid bodies + */ +std::set::iterator DynamicsWorld::getRigidBodiesEndIterator() { + return m_rigidBodies.end(); +} + +// Return true if the sleeping technique is enabled +/** + * @return True if the sleeping technique is enabled and false otherwise + */ +bool DynamicsWorld::isSleepingEnabled() const { + return m_isSleepingEnabled; +} + +// Return the current sleep linear velocity +/** + * @return The sleep linear velocity (in meters per second) + */ +float DynamicsWorld::getSleepLinearVelocity() const { + return m_sleepLinearVelocity; +} + +// Set the sleep linear velocity. +/// When the velocity of a body becomes smaller than the sleep linear/angular +/// velocity for a given amount of time, the body starts sleeping and does not need +/// to be simulated anymore. +/** + * @param sleepLinearVelocity The sleep linear velocity (in meters per second) + */ +void DynamicsWorld::setSleepLinearVelocity(float sleepLinearVelocity) { + assert(sleepLinearVelocity >= 0.0f); + m_sleepLinearVelocity = sleepLinearVelocity; +} + +// Return the current sleep angular velocity +/** + * @return The sleep angular velocity (in radian per second) + */ +float DynamicsWorld::getSleepAngularVelocity() const { + return m_sleepAngularVelocity; +} + +// Set the sleep angular velocity. +/// When the velocity of a body becomes smaller than the sleep linear/angular +/// velocity for a given amount of time, the body starts sleeping and does not need +/// to be simulated anymore. +/** + * @param sleepAngularVelocity The sleep angular velocity (in radian per second) + */ +void DynamicsWorld::setSleepAngularVelocity(float sleepAngularVelocity) { + assert(sleepAngularVelocity >= 0.0f); + m_sleepAngularVelocity = sleepAngularVelocity; +} + +// Return the time a body is required to stay still before sleeping +/** + * @return Time a body is required to stay still before sleeping (in seconds) + */ +float DynamicsWorld::getTimeBeforeSleep() const { + return m_timeBeforeSleep; +} + + +// Set the time a body is required to stay still before sleeping +/** + * @param timeBeforeSleep Time a body is required to stay still before sleeping (in seconds) + */ +void DynamicsWorld::setTimeBeforeSleep(float timeBeforeSleep) { + assert(timeBeforeSleep >= 0.0f); + m_timeBeforeSleep = timeBeforeSleep; +} + +// Set an event listener object to receive events callbacks. +/// If you use NULL as an argument, the events callbacks will be disabled. +/** + * @param eventListener Pointer to the event listener object that will receive + * event callbacks during the simulation + */ +void DynamicsWorld::setEventListener(EventListener* eventListener) { + m_eventListener = eventListener; +} \ No newline at end of file diff --git a/ephysics/engine/DynamicsWorld.hpp b/ephysics/engine/DynamicsWorld.hpp index 2e31086..9df5c07 100644 --- a/ephysics/engine/DynamicsWorld.hpp +++ b/ephysics/engine/DynamicsWorld.hpp @@ -165,221 +165,6 @@ namespace ephysics { friend class RigidBody; }; -// Reset the external force and torque applied to the bodies -void DynamicsWorld::resetBodiesForceAndTorque() { - - // For each body of the world - std::set::iterator it; - for (it = m_rigidBodies.begin(); it != m_rigidBodies.end(); ++it) { - (*it)->m_externalForce.setZero(); - (*it)->m_externalTorque.setZero(); - } -} - -// Get the number of iterations for the velocity constraint solver -uint32_t DynamicsWorld::getNbIterationsVelocitySolver() const { - return m_nbVelocitySolverIterations; -} - -// Set the number of iterations for the velocity constraint solver -/** - * @param nbIterations Number of iterations for the velocity solver - */ -void DynamicsWorld::setNbIterationsVelocitySolver(uint32_t nbIterations) { - m_nbVelocitySolverIterations = nbIterations; -} - -// Get the number of iterations for the position constraint solver -uint32_t DynamicsWorld::getNbIterationsPositionSolver() const { - return m_nbPositionSolverIterations; -} - -// Set the number of iterations for the position constraint solver -/** - * @param nbIterations Number of iterations for the position solver - */ -void DynamicsWorld::setNbIterationsPositionSolver(uint32_t nbIterations) { - m_nbPositionSolverIterations = nbIterations; -} - -// Set the position correction technique used for contacts -/** - * @param technique Technique used for the position correction (Baumgarte or Split Impulses) - */ -void DynamicsWorld::setContactsPositionCorrectionTechnique( - ContactsPositionCorrectionTechnique technique) { - if (technique == BAUMGARTE_CONTACTS) { - m_contactSolver.setIsSplitImpulseActive(false); - } - else { - m_contactSolver.setIsSplitImpulseActive(true); - } -} - -// Set the position correction technique used for joints -/** - * @param technique Technique used for the joins position correction (Baumgarte or Non Linear Gauss Seidel) - */ -void DynamicsWorld::setJointsPositionCorrectionTechnique( - JointsPositionCorrectionTechnique technique) { - if (technique == BAUMGARTE_JOINTS) { - m_constraintSolver.setIsNonLinearGaussSeidelPositionCorrectionActive(false); - } - else { - m_constraintSolver.setIsNonLinearGaussSeidelPositionCorrectionActive(true); - } -} - -// Activate or deactivate the solving of friction constraints at the center of -// the contact manifold instead of solving them at each contact point -/** - * @param isActive True if you want the friction to be solved at the center of - * the contact manifold and false otherwise - */ -void DynamicsWorld::setIsSolveFrictionAtContactManifoldCenterActive(bool isActive) { - m_contactSolver.setIsSolveFrictionAtContactManifoldCenterActive(isActive); -} - -// Return the gravity vector of the world -/** - * @return The current gravity vector (in meter per seconds squared) - */ -vec3 DynamicsWorld::getGravity() const { - return m_gravity; -} - -// Set the gravity vector of the world -/** - * @param gravity The gravity vector (in meter per seconds squared) - */ -void DynamicsWorld::setGravity(vec3& gravity) { - m_gravity = gravity; -} - -// Return if the gravity is enaled -/** - * @return True if the gravity is enabled in the world - */ -bool DynamicsWorld::isGravityEnabled() const { - return m_isGravityEnabled; -} - -// Enable/Disable the gravity -/** - * @param isGravityEnabled True if you want to enable the gravity in the world - * and false otherwise - */ -void DynamicsWorld::setIsGratityEnabled(bool isGravityEnabled) { - m_isGravityEnabled = isGravityEnabled; -} - -// Return the number of rigid bodies in the world -/** - * @return Number of rigid bodies in the world - */ -uint32_t DynamicsWorld::getNbRigidBodies() const { - return m_rigidBodies.size(); -} - -/// Return the number of joints in the world -/** - * @return Number of joints in the world - */ -uint32_t DynamicsWorld::getNbJoints() const { - return m_joints.size(); -} - -// Return an iterator to the beginning of the bodies of the physics world -/** - * @return Starting iterator of the set of rigid bodies - */ -std::set::iterator DynamicsWorld::getRigidBodiesBeginIterator() { - return m_rigidBodies.begin(); -} - -// Return an iterator to the end of the bodies of the physics world -/** - * @return Ending iterator of the set of rigid bodies - */ -std::set::iterator DynamicsWorld::getRigidBodiesEndIterator() { - return m_rigidBodies.end(); -} - -// Return true if the sleeping technique is enabled -/** - * @return True if the sleeping technique is enabled and false otherwise - */ -bool DynamicsWorld::isSleepingEnabled() const { - return m_isSleepingEnabled; -} - -// Return the current sleep linear velocity -/** - * @return The sleep linear velocity (in meters per second) - */ -float DynamicsWorld::getSleepLinearVelocity() const { - return m_sleepLinearVelocity; -} - -// Set the sleep linear velocity. -/// When the velocity of a body becomes smaller than the sleep linear/angular -/// velocity for a given amount of time, the body starts sleeping and does not need -/// to be simulated anymore. -/** - * @param sleepLinearVelocity The sleep linear velocity (in meters per second) - */ -void DynamicsWorld::setSleepLinearVelocity(float sleepLinearVelocity) { - assert(sleepLinearVelocity >= 0.0f); - m_sleepLinearVelocity = sleepLinearVelocity; -} - -// Return the current sleep angular velocity -/** - * @return The sleep angular velocity (in radian per second) - */ -float DynamicsWorld::getSleepAngularVelocity() const { - return m_sleepAngularVelocity; -} - -// Set the sleep angular velocity. -/// When the velocity of a body becomes smaller than the sleep linear/angular -/// velocity for a given amount of time, the body starts sleeping and does not need -/// to be simulated anymore. -/** - * @param sleepAngularVelocity The sleep angular velocity (in radian per second) - */ -void DynamicsWorld::setSleepAngularVelocity(float sleepAngularVelocity) { - assert(sleepAngularVelocity >= 0.0f); - m_sleepAngularVelocity = sleepAngularVelocity; -} - -// Return the time a body is required to stay still before sleeping -/** - * @return Time a body is required to stay still before sleeping (in seconds) - */ -float DynamicsWorld::getTimeBeforeSleep() const { - return m_timeBeforeSleep; -} - - -// Set the time a body is required to stay still before sleeping -/** - * @param timeBeforeSleep Time a body is required to stay still before sleeping (in seconds) - */ -void DynamicsWorld::setTimeBeforeSleep(float timeBeforeSleep) { - assert(timeBeforeSleep >= 0.0f); - m_timeBeforeSleep = timeBeforeSleep; -} - -// Set an event listener object to receive events callbacks. -/// If you use NULL as an argument, the events callbacks will be disabled. -/** - * @param eventListener Pointer to the event listener object that will receive - * event callbacks during the simulation - */ -void DynamicsWorld::setEventListener(EventListener* eventListener) { - m_eventListener = eventListener; -} } diff --git a/ephysics/engine/Island.cpp b/ephysics/engine/Island.cpp index 52d5684..fcc6bec 100644 --- a/ephysics/engine/Island.cpp +++ b/ephysics/engine/Island.cpp @@ -32,3 +32,52 @@ Island::~Island() { m_memoryAllocator.release(m_contactManifolds, m_numberAllocatedBytesContactManifolds); m_memoryAllocator.release(m_joints, m_numberAllocatedBytesJoints); } + +// Add a body int32_to the island +void Island::addBody(RigidBody* body) { + assert(!body->isSleeping()); + m_bodies[m_numberBodies] = body; + m_numberBodies++; +} + +// Add a contact manifold int32_to the island +void Island::addContactManifold(ContactManifold* contactManifold) { + m_contactManifolds[m_numberContactManifolds] = contactManifold; + m_numberContactManifolds++; +} + +// Add a joint int32_to the island +void Island::addJoint(Joint* joint) { + m_joints[m_numberJoints] = joint; + m_numberJoints++; +} + +// Return the number of bodies in the island +uint32_t Island::getNbBodies() const { + return m_numberBodies; +} + +// Return the number of contact manifolds in the island +uint32_t Island::getNbContactManifolds() const { + return m_numberContactManifolds; +} + +// Return the number of joints in the island +uint32_t Island::getNbJoints() const { + return m_numberJoints; +} + +// Return a pointer to the array of bodies +RigidBody** Island::getBodies() { + return m_bodies; +} + +// Return a pointer to the array of contact manifolds +ContactManifold** Island::getContactManifold() { + return m_contactManifolds; +} + +// Return a pointer to the array of joints +Joint** Island::getJoints() { + return m_joints; +} diff --git a/ephysics/engine/Island.hpp b/ephysics/engine/Island.hpp index 498811b..f875c43 100644 --- a/ephysics/engine/Island.hpp +++ b/ephysics/engine/Island.hpp @@ -58,53 +58,5 @@ namespace ephysics { friend class DynamicsWorld; }; -// Add a body int32_to the island -void Island::addBody(RigidBody* body) { - assert(!body->isSleeping()); - m_bodies[m_numberBodies] = body; - m_numberBodies++; -} - -// Add a contact manifold int32_to the island -void Island::addContactManifold(ContactManifold* contactManifold) { - m_contactManifolds[m_numberContactManifolds] = contactManifold; - m_numberContactManifolds++; -} - -// Add a joint int32_to the island -void Island::addJoint(Joint* joint) { - m_joints[m_numberJoints] = joint; - m_numberJoints++; -} - -// Return the number of bodies in the island -uint32_t Island::getNbBodies() const { - return m_numberBodies; -} - -// Return the number of contact manifolds in the island -uint32_t Island::getNbContactManifolds() const { - return m_numberContactManifolds; -} - -// Return the number of joints in the island -uint32_t Island::getNbJoints() const { - return m_numberJoints; -} - -// Return a pointer to the array of bodies -RigidBody** Island::getBodies() { - return m_bodies; -} - -// Return a pointer to the array of contact manifolds -ContactManifold** Island::getContactManifold() { - return m_contactManifolds; -} - -// Return a pointer to the array of joints -Joint** Island::getJoints() { - return m_joints; -} } diff --git a/ephysics/engine/Material.cpp b/ephysics/engine/Material.cpp index b6c264b..56ba5c4 100644 --- a/ephysics/engine/Material.cpp +++ b/ephysics/engine/Material.cpp @@ -23,3 +23,77 @@ Material::Material(const Material& material) m_rollingResistance(material.m_rollingResistance), m_bounciness(material.m_bounciness) { } + +// Return the bounciness +/** + * @return Bounciness factor (between 0 and 1) where 1 is very bouncy + */ +float Material::getBounciness() const { + return m_bounciness; +} + +// Set the bounciness. +/// The bounciness should be a value between 0 and 1. The value 1 is used for a +/// very bouncy body and zero is used for a body that is not bouncy at all. +/** + * @param bounciness Bounciness factor (between 0 and 1) where 1 is very bouncy + */ +void Material::setBounciness(float bounciness) { + assert(bounciness >= 0.0f && bounciness <= 1.0f); + m_bounciness = bounciness; +} + +// Return the friction coefficient +/** + * @return Friction coefficient (positive value) + */ +float Material::getFrictionCoefficient() const { + return m_frictionCoefficient; +} + +// Set the friction coefficient. +/// The friction coefficient has to be a positive value. The value zero is used for no +/// friction at all. +/** + * @param frictionCoefficient Friction coefficient (positive value) + */ +void Material::setFrictionCoefficient(float frictionCoefficient) { + assert(frictionCoefficient >= 0.0f); + m_frictionCoefficient = frictionCoefficient; +} + +// Return the rolling resistance factor. If this value is larger than zero, +// it will be used to slow down the body when it is rolling +// against another body. +/** + * @return The rolling resistance factor (positive value) + */ +float Material::getRollingResistance() const { + return m_rollingResistance; +} + +// Set the rolling resistance factor. If this value is larger than zero, +// it will be used to slow down the body when it is rolling +// against another body. +/** + * @param rollingResistance The rolling resistance factor + */ +void Material::setRollingResistance(float rollingResistance) { + assert(rollingResistance >= 0); + m_rollingResistance = rollingResistance; +} + +// Overloaded assignment operator +Material& Material::operator=(const Material& material) { + + // Check for self-assignment + if (this != &material) { + m_frictionCoefficient = material.m_frictionCoefficient; + m_bounciness = material.m_bounciness; + m_rollingResistance = material.m_rollingResistance; + } + + // Return this material + return *this; +} + diff --git a/ephysics/engine/Material.hpp b/ephysics/engine/Material.hpp index 1132be7..176c19e 100644 --- a/ephysics/engine/Material.hpp +++ b/ephysics/engine/Material.hpp @@ -39,77 +39,4 @@ namespace ephysics { Material& operator=(const Material& material); }; -// Return the bounciness -/** - * @return Bounciness factor (between 0 and 1) where 1 is very bouncy - */ -float Material::getBounciness() const { - return m_bounciness; -} - -// Set the bounciness. -/// The bounciness should be a value between 0 and 1. The value 1 is used for a -/// very bouncy body and zero is used for a body that is not bouncy at all. -/** - * @param bounciness Bounciness factor (between 0 and 1) where 1 is very bouncy - */ -void Material::setBounciness(float bounciness) { - assert(bounciness >= 0.0f && bounciness <= 1.0f); - m_bounciness = bounciness; -} - -// Return the friction coefficient -/** - * @return Friction coefficient (positive value) - */ -float Material::getFrictionCoefficient() const { - return m_frictionCoefficient; -} - -// Set the friction coefficient. -/// The friction coefficient has to be a positive value. The value zero is used for no -/// friction at all. -/** - * @param frictionCoefficient Friction coefficient (positive value) - */ -void Material::setFrictionCoefficient(float frictionCoefficient) { - assert(frictionCoefficient >= 0.0f); - m_frictionCoefficient = frictionCoefficient; -} - -// Return the rolling resistance factor. If this value is larger than zero, -// it will be used to slow down the body when it is rolling -// against another body. -/** - * @return The rolling resistance factor (positive value) - */ -float Material::getRollingResistance() const { - return m_rollingResistance; -} - -// Set the rolling resistance factor. If this value is larger than zero, -// it will be used to slow down the body when it is rolling -// against another body. -/** - * @param rollingResistance The rolling resistance factor - */ -void Material::setRollingResistance(float rollingResistance) { - assert(rollingResistance >= 0); - m_rollingResistance = rollingResistance; -} - -// Overloaded assignment operator -Material& Material::operator=(const Material& material) { - - // Check for self-assignment - if (this != &material) { - m_frictionCoefficient = material.m_frictionCoefficient; - m_bounciness = material.m_bounciness; - m_rollingResistance = material.m_rollingResistance; - } - - // Return this material - return *this; -} - } diff --git a/ephysics/engine/OverlappingPair.hpp b/ephysics/engine/OverlappingPair.hpp index 45699ec..13c047d 100644 --- a/ephysics/engine/OverlappingPair.hpp +++ b/ephysics/engine/OverlappingPair.hpp @@ -57,75 +57,5 @@ namespace ephysics { friend class DynamicsWorld; }; -// Return the pointer to first body -ProxyShape* OverlappingPair::getShape1() const { - return m_contactManifoldSet.getShape1(); -} - -// Return the pointer to second body -ProxyShape* OverlappingPair::getShape2() const { - return m_contactManifoldSet.getShape2(); -} - -// Add a contact to the contact manifold -void OverlappingPair::addContact(ContactPoint* contact) { - m_contactManifoldSet.addContactPoint(contact); -} - -// Update the contact manifold -void OverlappingPair::update() { - m_contactManifoldSet.update(); -} - -// Return the cached separating axis -vec3 OverlappingPair::getCachedSeparatingAxis() const { - return m_cachedSeparatingAxis; -} - -// Set the cached separating axis -void OverlappingPair::setCachedSeparatingAxis(const vec3& axis) { - m_cachedSeparatingAxis = axis; -} - - -// Return the number of contact points in the contact manifold -uint32_t OverlappingPair::getNbContactPoints() const { - return m_contactManifoldSet.getTotalNbContactPoints(); -} - -// Return the contact manifold -const ContactManifoldSet& OverlappingPair::getContactManifoldSet() { - return m_contactManifoldSet; -} - -// Return the pair of bodies index -overlappingpairid OverlappingPair::computeID(ProxyShape* shape1, ProxyShape* shape2) { - assert(shape1->m_broadPhaseID >= 0 && shape2->m_broadPhaseID >= 0); - - // Construct the pair of body index - overlappingpairid pairID = shape1->m_broadPhaseID < shape2->m_broadPhaseID ? - std::make_pair(shape1->m_broadPhaseID, shape2->m_broadPhaseID) : - std::make_pair(shape2->m_broadPhaseID, shape1->m_broadPhaseID); - assert(pairID.first != pairID.second); - return pairID; -} - -// Return the pair of bodies index -bodyindexpair OverlappingPair::computeBodiesIndexPair(CollisionBody* body1, - CollisionBody* body2) { - - // Construct the pair of body index - bodyindexpair indexPair = body1->getID() < body2->getID() ? - std::make_pair(body1->getID(), body2->getID()) : - std::make_pair(body2->getID(), body1->getID()); - assert(indexPair.first != indexPair.second); - return indexPair; -} - -// Clear the contact points of the contact manifold -void OverlappingPair::clearContactPoints() { - m_contactManifoldSet.clear(); -} - } diff --git a/ephysics/engine/Profiler.cpp b/ephysics/engine/Profiler.cpp index b735409..b111c2e 100644 --- a/ephysics/engine/Profiler.cpp +++ b/ephysics/engine/Profiler.cpp @@ -235,4 +235,114 @@ void Profiler::print32_tRecursiveNodeReport(ProfileNodeIterator* iterator, } } +// Return true if we are at the root of the profiler tree +bool ProfileNodeIterator::isRoot() { + return (m_currentParentNode->getParentNode() == nullptr); +} + +// Return true if we are at the end of a branch of the profiler tree +bool ProfileNodeIterator::isEnd() { + return (m_currentChildNode == nullptr); +} + +// Return the name of the current node +const char* ProfileNodeIterator::getCurrentName() { + return m_currentChildNode->getName(); +} + +// Return the total time of the current node +long double ProfileNodeIterator::getCurrentTotalTime() { + return m_currentChildNode->getTotalTime(); +} + +// Return the total number of calls of the current node +uint32_t ProfileNodeIterator::getCurrentNbTotalCalls() { + return m_currentChildNode->getNbTotalCalls(); +} + +// Return the name of the current parent node +const char* ProfileNodeIterator::getCurrentParentName() { + return m_currentParentNode->getName(); +} + +// Return the total time of the current parent node +long double ProfileNodeIterator::getCurrentParentTotalTime() { + return m_currentParentNode->getTotalTime(); +} + +// Return the total number of calls of the current parent node +uint32_t ProfileNodeIterator::getCurrentParentNbTotalCalls() { + return m_currentParentNode->getNbTotalCalls(); +} + +// Go to the first node +void ProfileNodeIterator::first() { + m_currentChildNode = m_currentParentNode->getChildNode(); +} + +// Go to the next node +void ProfileNodeIterator::next() { + m_currentChildNode = m_currentChildNode->getSiblingNode(); +} + +// Return a pointer to the parent node +ProfileNode* ProfileNode::getParentNode() { + return m_parentNode; +} + +// Return a pointer to a sibling node +ProfileNode* ProfileNode::getSiblingNode() { + return m_siblingNode; +} + +// Return a pointer to a child node +ProfileNode* ProfileNode::getChildNode() { + return m_childNode; +} + +// Return the name of the node +const char* ProfileNode::getName() { + return m_name; +} + +// Return the total number of call of the corresponding block of code +uint32_t ProfileNode::getNbTotalCalls() const { + return m_numberTotalCalls; +} + +// Return the total time spent in the block of code +long double ProfileNode::getTotalTime() const { + return m_totalTime; +} + +// Return the number of frames +uint32_t Profiler::getNbFrames() { + return m_frameCounter; +} + +// Return the elasped time since the start/reset of the profiling +long double Profiler::getElapsedTimeSinceStart() { + long double currentTime = Timer::getCurrentSystemTime() * 1000.0; + return currentTime - m_profilingStartTime; +} + +// Increment the frame counter +void Profiler::incrementFrameCounter() { + m_frameCounter++; +} + +// Return an iterator over the profiler tree starting at the root +ProfileNodeIterator* Profiler::getIterator() { + return new ProfileNodeIterator(&m_rootNode); +} + +// Destroy a previously allocated iterator +void Profiler::destroyIterator(ProfileNodeIterator* iterator) { + delete iterator; +} + +// Destroy the profiler (release the memory) +void Profiler::destroy() { + m_rootNode.destroy(); +} #endif diff --git a/ephysics/engine/Profiler.hpp b/ephysics/engine/Profiler.hpp index 89e3c89..c22004f 100644 --- a/ephysics/engine/Profiler.hpp +++ b/ephysics/engine/Profiler.hpp @@ -143,125 +143,10 @@ namespace ephysics { } }; -// 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 -bool ProfileNodeIterator::isRoot() { - return (m_currentParentNode->getParentNode() == nullptr); + // Use this macro to start profile a block of code + #define PROFILE(name) ProfileSample profileSample(name) } - -// Return true if we are at the end of a branch of the profiler tree -bool ProfileNodeIterator::isEnd() { - return (m_currentChildNode == nullptr); -} - -// Return the name of the current node -const char* ProfileNodeIterator::getCurrentName() { - return m_currentChildNode->getName(); -} - -// Return the total time of the current node -long double ProfileNodeIterator::getCurrentTotalTime() { - return m_currentChildNode->getTotalTime(); -} - -// Return the total number of calls of the current node -uint32_t ProfileNodeIterator::getCurrentNbTotalCalls() { - return m_currentChildNode->getNbTotalCalls(); -} - -// Return the name of the current parent node -const char* ProfileNodeIterator::getCurrentParentName() { - return m_currentParentNode->getName(); -} - -// Return the total time of the current parent node -long double ProfileNodeIterator::getCurrentParentTotalTime() { - return m_currentParentNode->getTotalTime(); -} - -// Return the total number of calls of the current parent node -uint32_t ProfileNodeIterator::getCurrentParentNbTotalCalls() { - return m_currentParentNode->getNbTotalCalls(); -} - -// Go to the first node -void ProfileNodeIterator::first() { - m_currentChildNode = m_currentParentNode->getChildNode(); -} - -// Go to the next node -void ProfileNodeIterator::next() { - m_currentChildNode = m_currentChildNode->getSiblingNode(); -} - -// Return a pointer to the parent node -ProfileNode* ProfileNode::getParentNode() { - return m_parentNode; -} - -// Return a pointer to a sibling node -ProfileNode* ProfileNode::getSiblingNode() { - return m_siblingNode; -} - -// Return a pointer to a child node -ProfileNode* ProfileNode::getChildNode() { - return m_childNode; -} - -// Return the name of the node -const char* ProfileNode::getName() { - return m_name; -} - -// Return the total number of call of the corresponding block of code -uint32_t ProfileNode::getNbTotalCalls() const { - return m_numberTotalCalls; -} - -// Return the total time spent in the block of code -long double ProfileNode::getTotalTime() const { - return m_totalTime; -} - -// Return the number of frames -uint32_t Profiler::getNbFrames() { - return m_frameCounter; -} - -// Return the elasped time since the start/reset of the profiling -long double Profiler::getElapsedTimeSinceStart() { - long double currentTime = Timer::getCurrentSystemTime() * 1000.0; - return currentTime - m_profilingStartTime; -} - -// Increment the frame counter -void Profiler::incrementFrameCounter() { - m_frameCounter++; -} - -// Return an iterator over the profiler tree starting at the root -ProfileNodeIterator* Profiler::getIterator() { - return new ProfileNodeIterator(&m_rootNode); -} - -// Destroy a previously allocated iterator -void Profiler::destroyIterator(ProfileNodeIterator* iterator) { - delete iterator; -} - -// Destroy the profiler (release the memory) -void Profiler::destroy() { - m_rootNode.destroy(); -} - -} - #else - -// Empty macro in case profiling is not active -#define PROFILE(name) - + // Empty macro in case profiling is not active + #define PROFILE(name) #endif diff --git a/ephysics/engine/Timer.cpp b/ephysics/engine/Timer.cpp index acec2ea..26e8eea 100644 --- a/ephysics/engine/Timer.cpp +++ b/ephysics/engine/Timer.cpp @@ -39,6 +39,68 @@ long double Timer::getCurrentSystemTime() { +// Return the timestep of the physics engine +double Timer::getTimeStep() const { + return m_timeStep; +} + +// Set the timestep of the physics engine +void Timer::setTimeStep(double timeStep) { + assert(timeStep > 0.0f); + m_timeStep = timeStep; +} + +// Return the current time +long double Timer::getPhysicsTime() const { + return m_lastUpdateTime; +} + +// Return if the timer is running +bool Timer::getIsRunning() const { + return m_isRunning; +} + +// Start the timer +void Timer::start() { + if (!m_isRunning) { + // Get the current system time + m_lastUpdateTime = getCurrentSystemTime(); + m_accumulator = 0.0; + m_isRunning = true; + } +} + +// Stop the timer +void Timer::stop() { + m_isRunning = false; +} + +// True if it's possible to take a new step +bool Timer::isPossibleToTakeStep() const { + return (m_accumulator >= m_timeStep); +} + +// Take a new step => update the timer by adding the timeStep value to the current time +void Timer::nextStep() { + assert(m_isRunning); + + // Update the accumulator value + m_accumulator -= m_timeStep; +} + +// Compute the int32_terpolation factor +float Timer::computeInterpolationFactor() { + return (float(m_accumulator / m_timeStep)); +} + +// Compute the time since the last update() call and add it to the accumulator +void Timer::update() { + long double currentTime = getCurrentSystemTime(); + m_deltaTime = currentTime - m_lastUpdateTime; + m_lastUpdateTime = currentTime; + m_accumulator += m_deltaTime; +} + diff --git a/ephysics/engine/Timer.hpp b/ephysics/engine/Timer.hpp index e3f86f4..18eb3db 100644 --- a/ephysics/engine/Timer.hpp +++ b/ephysics/engine/Timer.hpp @@ -66,66 +66,4 @@ namespace ephysics { static long double getCurrentSystemTime(); }; -// Return the timestep of the physics engine -double Timer::getTimeStep() const { - return m_timeStep; -} - -// Set the timestep of the physics engine -void Timer::setTimeStep(double timeStep) { - assert(timeStep > 0.0f); - m_timeStep = timeStep; -} - -// Return the current time -long double Timer::getPhysicsTime() const { - return m_lastUpdateTime; -} - -// Return if the timer is running -bool Timer::getIsRunning() const { - return m_isRunning; -} - -// Start the timer -void Timer::start() { - if (!m_isRunning) { - // Get the current system time - m_lastUpdateTime = getCurrentSystemTime(); - m_accumulator = 0.0; - m_isRunning = true; - } -} - -// Stop the timer -void Timer::stop() { - m_isRunning = false; -} - -// True if it's possible to take a new step -bool Timer::isPossibleToTakeStep() const { - return (m_accumulator >= m_timeStep); -} - -// Take a new step => update the timer by adding the timeStep value to the current time -void Timer::nextStep() { - assert(m_isRunning); - - // Update the accumulator value - m_accumulator -= m_timeStep; -} - -// Compute the int32_terpolation factor -float Timer::computeInterpolationFactor() { - return (float(m_accumulator / m_timeStep)); -} - -// Compute the time since the last update() call and add it to the accumulator -void Timer::update() { - long double currentTime = getCurrentSystemTime(); - m_deltaTime = currentTime - m_lastUpdateTime; - m_lastUpdateTime = currentTime; - m_accumulator += m_deltaTime; -} - }