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

[DEV] continue rework

Browse Source
This commit is contained in:
Edouard DUPIN 2017-06-22 21:51:42 +02:00
parent f9975bc8dc
commit aa25f1fd11
71 changed files with 3888 additions and 4764 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

285
ephysics/body/Body.hpp
View File

@ -10,154 +10,147 @@
#include <ephysics/configuration.hpp> #include <ephysics/configuration.hpp>
namespace ephysics { namespace ephysics {
/** /**
* @brief Represent a body of the physics engine. You should not * @brief Represent a body of the physics engine. You should not
* instantiante this class but instantiate the CollisionBody or RigidBody * instantiante this class but instantiate the CollisionBody or RigidBody
* classes instead. * classes instead.
*/ */
class Body { class Body {
protected : protected :
bodyindex m_id; //!< ID of the body 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_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 bool m_isAllowedToSleep; //!< True if the body is allowed to go to sleep for better efficiency
/** /**
* @brief True if the body is active. * @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. * 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. * 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. * 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. * A joint connected to an inactive body will also be inactive.
*/ */
bool m_isActive; bool m_isActive;
bool m_isSleeping; //!< True if the body is sleeping (for sleeping technique) 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 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 void* m_userData; //!< Pointer that can be used to attach user data to the body
/// Private copy-constructor /// Private copy-constructor
Body(const Body& body); Body(const Body& body);
/// Private assignment operator /// Private assignment operator
Body& operator=(const Body& body); Body& operator=(const Body& body);
public : public :
/** /**
* @brief Constructor * @brief Constructor
* @param[in] _id ID of the new body * @param[in] _id ID of the new body
*/ */
Body(bodyindex _id); Body(bodyindex _id);
/** /**
* @brtief Virtualize Destructor * @brtief Virtualize Destructor
*/ */
virtual ~Body() = default; virtual ~Body() = default;
/** /**
* @brief Return the id of the body * @brief Return the id of the body
* @return The ID of the body * @return The ID of the body
*/ */
bodyindex getID() const { bodyindex getID() const {
return m_id; 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 Return whether or not the body is allowed to sleep
* @brief Return whether or not the body is sleeping * @return True if the body is allowed to sleep and false otherwise
* @return True if the body is currently sleeping and false otherwise */
*/ bool isAllowedToSleep() const {
bool isSleeping() const { return m_isAllowedToSleep;
return m_isSleeping; }
} /**
/** * @brief Set whether or not the body is allowed to go to sleep
* @brief Return true if the body is active * @param[in] _isAllowedToSleep True if the body is allowed to sleep
* @return True if the body currently active and false otherwise */
*/ void setIsAllowedToSleep(bool _isAllowedToSleep) {
bool isActive() const { m_isAllowedToSleep = _isAllowedToSleep;
return m_isActive; if (!m_isAllowedToSleep) {
} setIsSleeping(false);
/**
* @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 whether or not the body is sleeping
/** * @return True if the body is currently sleeping and false otherwise
* @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 bool isSleeping() const {
*/ return m_isSleeping;
void* getUserData() const { }
return m_userData; /**
} * @brief Return true if the body is active
/** * @return True if the body currently active and false otherwise
* @brief Attach user data to this body */
* @param[in] _userData A pointer to the user data you want to attach to the body bool isActive() const {
*/ return m_isActive;
void setUserData(void* _userData) { }
m_userData = _userData; /**
} * @brief Set whether or not the body is active
/** * @param[in] _isActive True if you want to activate the body
* @brief Smaller than operator */
* @param[in] _obj Other object to compare void setIsActive(bool _isActive) {
* @return true if the current element is smaller m_isActive = _isActive;
*/ }
bool operator<(const Body& _obj) const { /**
return (m_id < _obj.m_id); * @brief Set the variable to know whether or not the body is sleeping
} * @param[in] _isSleeping Set the new status
/** */
* @brief Larger than operator void setIsSleeping(bool _isSleeping) {
* @param[in] _obj Other object to compare if (_isSleeping) {
* @return true if the current element is Bigger m_sleepTime = 0.0f;
*/ } else {
bool operator>(const Body& _obj) const { if (m_isSleeping) {
return (m_id > _obj.m_id); m_sleepTime = 0.0f;
} }
/** }
* @brief Equal operator m_isSleeping = _isSleeping;
* @param[in] _obj Other object to compare }
* @return true if the curretn element is equal /**
*/ * @brief Return a pointer to the user data attached to this body
bool operator==(const Body& _obj) const { * @return A pointer to the user data you have attached to the body
return (m_id == _obj.m_id); */
} void* getUserData() const {
/** return m_userData;
* @brief Not equal operator }
* @param[in] _obj Other object to compare /**
* @return true if the curretn element is NOT equal * @brief Attach user data to this body
*/ * @param[in] _userData A pointer to the user data you want to attach to the body
bool operator!=(const Body& _obj) const { */
return (m_id != _obj.m_id); void setUserData(void* _userData) {
} m_userData = _userData;
friend class DynamicsWorld; }
}; /**
* @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;
};
} }

83
ephysics/collision/CollisionDetection.cpp
View File

@ -511,3 +511,86 @@ void TestCollisionBetweenShapesCallback::notifyContact(OverlappingPair* overlapp
const ContactPointInfo& contactInfo) { const ContactPointInfo& contactInfo) {
m_collisionCallback->notifyContact(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;
}

81
ephysics/collision/CollisionDetection.hpp
View File

@ -140,85 +140,4 @@ namespace ephysics {
friend class ConvexMeshShape; 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;
}
} }

66
ephysics/collision/CollisionShapeInfo.hpp
View File

@ -5,49 +5,35 @@
*/ */
#pragma once #pragma once
// Libraries
#include <ephysics/collision/shapes/CollisionShape.hpp> #include <ephysics/collision/shapes/CollisionShape.hpp>
/// Namespace ReactPhysics3D
namespace ephysics { namespace ephysics {
class OverlappingPair;
class OverlappingPair; /**
* @brief It regroups different things about a collision shape. This is
// Class CollisionShapeInfo * used to pass information about a collision shape to a collision algorithm.
/** */
* This structure regroups different things about a collision shape. This is struct CollisionShapeInfo {
* used to pass information about a collision shape to a collision algorithm. public:
*/ OverlappingPair* overlappingPair; //!< Broadphase overlapping pair
struct CollisionShapeInfo { ProxyShape* proxyShape; //!< Proxy shape
const CollisionShape* collisionShape; //!< Pointer to the collision shape
public: etk::Transform3D shapeToWorldTransform; //!< etk::Transform3D that maps from collision shape local-space to world-space
void** cachedCollisionData; //!< Cached collision data of the proxy shape
/// Broadphase overlapping pair /// Constructor
OverlappingPair* overlappingPair; CollisionShapeInfo(ProxyShape* _proxyCollisionShape,
const CollisionShape* _shape,
/// Proxy shape const etk::Transform3D& _shapeLocalToWorldTransform,
ProxyShape* proxyShape; OverlappingPair* _pair,
void** _cachedData):
/// Pointer to the collision shape overlappingPair(_pair),
const CollisionShape* collisionShape; proxyShape(_proxyCollisionShape),
collisionShape(_shape),
/// etk::Transform3D that maps from collision shape local-space to world-space shapeToWorldTransform(_shapeLocalToWorldTransform),
etk::Transform3D shapeToWorldTransform; cachedCollisionData(_cachedData) {
/// 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) {
}
};
} }

117
ephysics/collision/ContactManifold.cpp
View File

@ -243,3 +243,120 @@ void ContactManifold::clear() {
} }
m_nbContactPoints = 0; 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; i<m_nbContactPoints; i++) {
averageNormal += m_contactPoints[i]->getNormal();
}
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; i<m_nbContactPoints; i++) {
float depth = m_contactPoints[i]->getPenetrationDepth();
if (depth > largestDepth) {
largestDepth = depth;
}
}
return largestDepth;
}

117
ephysics/collision/ContactManifold.hpp
View File

@ -135,123 +135,6 @@ namespace ephysics {
friend class CollisionBody; 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; i<m_nbContactPoints; i++) {
averageNormal += m_contactPoints[i]->getNormal();
}
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; i<m_nbContactPoints; i++) {
float depth = m_contactPoints[i]->getPenetrationDepth();
if (depth > largestDepth) {
largestDepth = depth;
}
}
return largestDepth;
}
} }

30
ephysics/collision/ContactManifoldSet.cpp
View File

@ -215,3 +215,33 @@ void ContactManifoldSet::removeManifold(int32_t index) {
m_nbManifolds--; 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; i<m_nbManifolds; i++) {
nbPoints += m_manifolds[i]->getNbContactPoints();
}
return nbPoints;
}

30
ephysics/collision/ContactManifoldSet.hpp
View File

@ -60,35 +60,5 @@ namespace ephysics {
int32_t getTotalNbContactPoints() const; 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; i<m_nbManifolds; i++) {
nbPoints += m_manifolds[i]->getNbContactPoints();
}
return nbPoints;
}
} }

146
ephysics/collision/ProxyShape.cpp
View File

@ -25,7 +25,6 @@ ProxyShape::ProxyShape(CollisionBody* body, CollisionShape* shape, const etk::Tr
// Destructor // Destructor
ProxyShape::~ProxyShape() { ProxyShape::~ProxyShape() {
// Release the cached collision data memory // Release the cached collision data memory
if (m_cachedCollisionData != NULL) { if (m_cachedCollisionData != NULL) {
free(m_cachedCollisionData); free(m_cachedCollisionData);
@ -71,3 +70,148 @@ bool ProxyShape::raycast(const Ray& ray, RaycastInfo& raycastInfo) {
return isHit; 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);
}

409
ephysics/collision/ProxyShape.hpp
View File

@ -5,301 +5,128 @@
*/ */
#pragma once #pragma once
// Libraries
#include <ephysics/body/CollisionBody.hpp> #include <ephysics/body/CollisionBody.hpp>
#include <ephysics/collision/shapes/CollisionShape.hpp> #include <ephysics/collision/shapes/CollisionShape.hpp>
namespace ephysics { namespace ephysics {
/**
// Class ProxyShape * @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
* The CollisionShape instances are supposed to be unique for memory optimization. For instance, * a unique instance of SphereShape but we need to differentiate between the two instances during
* consider two rigid bodies with the same sphere collision shape. In this situation, we will have * the collision detection. They do not have the same position in the world and they do not
* a unique instance of SphereShape but we need to differentiate between the two instances during * belong to the same rigid body. The ProxyShape class is used for that purpose by attaching a
* the collision detection. They do not have the same position in the world and they do not * rigid body with one of its collision shape. A body can have multiple proxy shapes (one for
* belong to the same rigid body. The ProxyShape class is used for that purpose by attaching a * each collision shape attached to the body).
* 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:
class ProxyShape { CollisionBody* m_body; //!< Pointer to the parent body
CollisionShape* m_collisionShape; //!< Internal collision shape
protected: 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
// -------------------- Attributes -------------------- // 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)
/// Pointer to the parent body void* m_cachedCollisionData; //!< Cached collision data
CollisionBody* m_body; void* m_userData; //!< Pointer to user data
/**
/// Internal collision shape * @brief Bits used to define the collision category of this shape.
CollisionShape* m_collisionShape; * 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
/// Local-space to parent body-space transform (does not change over time) * together with the m_collideWithMaskBits variable so that given
etk::Transform3D m_localToBodyTransform; * categories of shapes collide with each other and do not collide with
* other categories.
/// Mass (in kilogramms) of the corresponding collision shape */
float m_mass; unsigned short m_collisionCategoryBits;
/**
/// Pointer to the next proxy shape of the body (linked list) * @brief Bits mask used to state which collision categories this shape can
ProxyShape* m_next; * collide with. This value is 0xFFFF by default. It means that this
* proxy shape will collide with every collision categories by default.
/// Broad-phase ID (node ID in the dynamic AABB tree) */
int32_t m_broadPhaseID; unsigned short m_collideWithMaskBits;
/// Private copy-constructor
/// Cached collision data ProxyShape(const ProxyShape&) = delete;
void* m_cachedCollisionData; /// Private assignment operator
ProxyShape& operator=(const ProxyShape&) = delete;
/// Pointer to user data public:
void* m_userData; /// Constructor
ProxyShape(CollisionBody* _body,
/// Bits used to define the collision category of this shape. CollisionShape* _shape,
/// You can set a single bit to one to define a category value for this const etk::Transform3D& _transform,
/// shape. This value is one (0x0001) by default. This variable can be used float _mass);
/// together with the m_collideWithMaskBits variable so that given
/// categories of shapes collide with each other and do not collide with /// Destructor
/// other categories. virtual ~ProxyShape();
unsigned short m_collisionCategoryBits;
/// Return the collision shape
/// Bits mask used to state which collision categories this shape can const CollisionShape* getCollisionShape() const;
/// collide with. This value is 0xFFFF by default. It means that this
/// proxy shape will collide with every collision categories by default. /// Return the parent body
unsigned short m_collideWithMaskBits; CollisionBody* getBody() const;
// -------------------- Methods -------------------- // /// Return the mass of the collision shape
float getMass() const;
/// Private copy-constructor
ProxyShape(const ProxyShape& proxyShape); /// Return a pointer to the user data attached to this body
void* getUserData() const;
/// Private assignment operator
ProxyShape& operator=(const ProxyShape& proxyShape); /// Attach user data to this body
void setUserData(void* _userData);
public:
/// Return the local to parent body transform
// -------------------- Methods -------------------- // const etk::Transform3D& getLocalToBodyTransform() const;
/// Constructor /// Set the local to parent body transform
ProxyShape(CollisionBody* body, CollisionShape* shape, void setLocalToBodyTransform(const etk::Transform3D& _transform);
const etk::Transform3D& transform, float mass);
/// Return the local to world transform
/// Destructor const etk::Transform3D getLocalToWorldTransform() const;
virtual ~ProxyShape();
/// Return true if a point is inside the collision shape
/// Return the collision shape bool testPointInside(const vec3& _worldPoint);
const CollisionShape* getCollisionShape() const;
/// Raycast method with feedback information
/// Return the parent body bool raycast(const Ray& _ray, RaycastInfo& _raycastInfo);
CollisionBody* getBody() const;
/// Return the collision bits mask
/// Return the mass of the collision shape unsigned short getCollideWithMaskBits() const;
float getMass() const;
/// Set the collision bits mask
/// Return a pointer to the user data attached to this body void setCollideWithMaskBits(unsigned short _collideWithMaskBits);
void* getUserData() const;
/// Return the collision category bits
/// Attach user data to this body unsigned short getCollisionCategoryBits() const;
void setUserData(void* userData);
/// Set the collision category bits
/// Return the local to parent body transform void setCollisionCategoryBits(unsigned short _collisionCategoryBits);
const etk::Transform3D& getLocalToBodyTransform() const;
/// Return the next proxy shape in the linked list of proxy shapes
/// Set the local to parent body transform ProxyShape* getNext();
void setLocalToBodyTransform(const etk::Transform3D& transform);
/// Return the next proxy shape in the linked list of proxy shapes
/// Return the local to world transform const ProxyShape* getNext() const;
const etk::Transform3D getLocalToWorldTransform() const;
/// Return the pointer to the cached collision data
/// Return true if a point is inside the collision shape void** getCachedCollisionData();
bool testPointInside(const vec3& worldPoint);
/// Return the local scaling vector of the collision shape
/// Raycast method with feedback information vec3 getLocalScaling() const;
bool raycast(const Ray& ray, RaycastInfo& raycastInfo);
/// Set the local scaling vector of the collision shape
/// Return the collision bits mask virtual void setLocalScaling(const vec3& _scaling);
unsigned short getCollideWithMaskBits() const;
friend class OverlappingPair;
/// Set the collision bits mask friend class CollisionBody;
void setCollideWithMaskBits(unsigned short collideWithMaskBits); friend class RigidBody;
friend class BroadPhaseAlgorithm;
/// Return the collision category bits friend class DynamicAABBTree;
unsigned short getCollisionCategoryBits() const; friend class CollisionDetection;
friend class CollisionWorld;
/// Set the collision category bits friend class DynamicsWorld;
void setCollisionCategoryBits(unsigned short collisionCategoryBits); friend class EPAAlgorithm;
friend class GJKAlgorithm;
/// Return the next proxy shape in the linked list of proxy shapes friend class ConvexMeshShape;
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);
}
} }

187
ephysics/collision/RaycastInfo.hpp
View File

@ -5,126 +5,81 @@
*/ */
#pragma once #pragma once
// Libraries
#include <etk/math/Vector3D.hpp> #include <etk/math/Vector3D.hpp>
#include <ephysics/mathematics/Ray.hpp> #include <ephysics/mathematics/Ray.hpp>
/// ReactPhysics3D namespace
namespace ephysics { namespace ephysics {
class CollisionBody;
// Declarations class ProxyShape;
class CollisionBody; class CollisionShape;
class ProxyShape; /**
class CollisionShape; * @brief It contains the information about a raycast hit.
*/
// Structure RaycastInfo struct RaycastInfo {
/** private:
* This structure contains the information about a raycast hit. /// Private copy constructor
*/ RaycastInfo(const RaycastInfo&) = delete;
struct RaycastInfo { /// Private assignment operator
RaycastInfo& operator=(const RaycastInfo&) = delete;
private: public:
vec3 worldPoint; //!< Hit point in world-space coordinates
// -------------------- Methods -------------------- // 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)
/// Private copy constructor int32_t meshSubpart; //!< Mesh subpart index that has been hit (only used for triangles mesh and -1 otherwise)
RaycastInfo(const RaycastInfo& raycastInfo); int32_t triangleIndex; //!< Hit triangle index (only used for triangles mesh and -1 otherwise)
CollisionBody* body; //!< Pointer to the hit collision body
/// Private assignment operator ProxyShape* proxyShape; //!< Pointer to the hit proxy collision shape
RaycastInfo& operator=(const RaycastInfo& raycastInfo); /// Constructor
RaycastInfo() :
public: meshSubpart(-1),
triangleIndex(-1),
// -------------------- Attributes -------------------- // body(nullptr),
proxyShape(nullptr) {
/// Hit point in world-space coordinates
vec3 worldPoint; }
/// Surface normal at hit point in world-space coordinates /// Destructor
vec3 worldNormal; virtual ~RaycastInfo() = default;
};
/// 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; * @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
/// Mesh subpart index that has been hit (only used for triangles mesh and -1 otherwise) * the notifyRaycastHit() method. This method will be called for each ProxyShape
int32_t meshSubpart; * that is hit by the ray.
*/
/// Hit triangle index (only used for triangles mesh and -1 otherwise) class RaycastCallback {
int32_t triangleIndex; public:
/// Destructor
/// Pointer to the hit collision body virtual ~RaycastCallback() = default;
CollisionBody* body; /**
* @brief This method will be called for each ProxyShape that is hit by the
/// Pointer to the hit proxy collision shape * ray. You cannot make any assumptions about the order of the
ProxyShape* proxyShape; * calls. You should use the return value to control the continuation
* of the ray. The returned value is the next maxFraction value to use.
// -------------------- Methods -------------------- // * 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
/// Constructor * ray is not clipped and the ray cast should continue as if no hit
RaycastInfo() : meshSubpart(-1), triangleIndex(-1), body(NULL), proxyShape(NULL) { * 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.
/// Destructor * @param[in] _raycastInfo Information about the raycast hit
virtual ~RaycastInfo() { * @return Value that controls the continuation of the ray after a hit
*/
} virtual float notifyRaycastHit(const RaycastInfo& _raycastInfo)=0;
}; };
// Class RaycastCallback struct RaycastTest {
/** public:
* This class can be used to register a callback for ray casting queries. RaycastCallback* userCallback; //!< User callback class
* You should implement your own class inherited from this one and implement /// Constructor
* the notifyRaycastHit() method. This method will be called for each ProxyShape RaycastTest(RaycastCallback* _callback) {
* that is hit by the ray. userCallback = _callback;
*/ }
class RaycastCallback { /// Ray cast test against a proxy shape
float raycastAgainstShape(ProxyShape* _shape, const Ray& _ray);
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);
};
} }

40
ephysics/collision/TriangleVertexArray.cpp
View File

@ -40,3 +40,43 @@ TriangleVertexArray::TriangleVertexArray(uint32_t nbVertices, void* verticesStar
TriangleVertexArray::~TriangleVertexArray() { 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;
}

146
ephysics/collision/TriangleVertexArray.hpp
View File

@ -8,105 +8,55 @@
#include <ephysics/configuration.hpp> #include <ephysics/configuration.hpp>
namespace ephysics { namespace ephysics {
/** /**
* This class is used to describe the vertices and faces of a triangular mesh. * This class is used to describe the vertices and faces of a triangular mesh.
* A TriangleVertexArray represents a continuous array of vertices and indexes * A TriangleVertexArray represents a continuous array of vertices and indexes
* of a triangular mesh. When you create a TriangleVertexArray, no data is copied * 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 * 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 * the user to share vertices data between the physics engine and the rendering
* part. Therefore, make sure that the data pointed by a TriangleVertexArray * part. Therefore, make sure that the data pointed by a TriangleVertexArray
* remains valid during the TriangleVertexArray life. * remains valid during the TriangleVertexArray life.
*/ */
class TriangleVertexArray { class TriangleVertexArray {
public: public:
/// Data type for the vertices in the array /// Data type for the vertices in the array
enum VertexDataType {VERTEX_FLOAT_TYPE, VERTEX_DOUBLE_TYPE}; 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;
}
} }

2
ephysics/collision/broadphase/BroadPhaseAlgorithm.cpp
View File

@ -254,7 +254,7 @@ float BroadPhaseRaycastCallback::raycastBroadPhaseShape(int32_t nodeId, const Ra
return hitFraction; return hitFraction;
} }
bool BroadPhaseAlgorithm::testOverlappingShapes(const _ProxyShape* shape1, bool BroadPhaseAlgorithm::testOverlappingShapes(const ProxyShape* _shape1,
const ProxyShape* _shape2) const { const ProxyShape* _shape2) const {
// Get the two AABBs of the collision shapes // Get the two AABBs of the collision shapes
const AABB& aabb1 = m_dynamicAABBTree.getFatAABB(_shape1->m_broadPhaseID); const AABB& aabb1 = m_dynamicAABBTree.getFatAABB(_shape1->m_broadPhaseID);

59
ephysics/collision/broadphase/DynamicAABBTree.cpp
View File

@ -4,7 +4,6 @@
* @license BSD 3 clauses (see license file) * @license BSD 3 clauses (see license file)
*/ */
// Libraries
#include <ephysics/collision/broadphase/DynamicAABBTree.hpp> #include <ephysics/collision/broadphase/DynamicAABBTree.hpp>
#include <ephysics/collision/broadphase/BroadPhaseAlgorithm.hpp> #include <ephysics/collision/broadphase/BroadPhaseAlgorithm.hpp>
#include <ephysics/memory/Stack.hpp> #include <ephysics/memory/Stack.hpp>
@ -13,19 +12,15 @@
using namespace ephysics; using namespace ephysics;
// Initialization of static variables
const int32_t TreeNode::NULL_TREE_NODE = -1; const int32_t TreeNode::NULL_TREE_NODE = -1;
// Constructor
DynamicAABBTree::DynamicAABBTree(float extraAABBGap) : m_extraAABBGap(extraAABBGap) { DynamicAABBTree::DynamicAABBTree(float extraAABBGap) : m_extraAABBGap(extraAABBGap) {
init(); init();
} }
// Destructor
DynamicAABBTree::~DynamicAABBTree() { DynamicAABBTree::~DynamicAABBTree() {
// Free the allocated memory for the nodes
free(m_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 #ifndef NDEBUG
// Check if the tree structure is valid (for debugging purpose) // Check if the tree structure is valid (for debugging purpose)

374
ephysics/collision/broadphase/DynamicAABBTree.hpp
View File

@ -5,267 +5,135 @@
*/ */
#pragma once #pragma once
// Libraries
#include <ephysics/configuration.hpp> #include <ephysics/configuration.hpp>
#include <ephysics/collision/shapes/AABB.hpp> #include <ephysics/collision/shapes/AABB.hpp>
#include <ephysics/body/CollisionBody.hpp> #include <ephysics/body/CollisionBody.hpp>
/// Namespace ReactPhysics3D
namespace ephysics { namespace ephysics {
class BroadPhaseAlgorithm;
// Declarations class BroadPhaseRaycastTestCallback;
class BroadPhaseAlgorithm; class DynamicAABBTreeOverlapCallback;
class BroadPhaseRaycastTestCallback; struct RaycastTest;
class DynamicAABBTreeOverlapCallback; /**
struct RaycastTest; * @brief It represents a node of the dynamic AABB tree.
*/
// Structure TreeNode struct TreeNode {
/** const static int32_t NULL_TREE_NODE; //!< Null tree node constant
* This structure represents a node of the dynamic AABB tree. /**
*/ * @brief A node is either in the tree (has a parent) or in the free nodes list (has a next node)
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)
union { union {
int32_t dataInt[2]; int32_t parentID; //!< Parent node ID
void* dataPointer; 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;
}
} }

5
ephysics/collision/narrowphase/ConcaveVsConvexAlgorithm.hpp
View File

@ -122,9 +122,8 @@ namespace ephysics {
std::vector<SmoothMeshContactInfo> _contactPoints, std::vector<SmoothMeshContactInfo> _contactPoints,
NarrowPhaseCallback* _narrowPhaseCallback); NarrowPhaseCallback* _narrowPhaseCallback);
/// Add a triangle vertex int32_to the set of processed triangles /// Add a triangle vertex int32_to the set of processed triangles
void addProcessedVertex(std::unordered_multimap<int32_t, vec3>& _processTriangleVertices, void addProcessedVertex(std::unordered_multimap<int32_t, vec3>& _processTriangleVertices, const vec3& _vertex) {
const vec3& _vertex) { _processTriangleVertices.insert(std::make_pair(int32_t(_vertex.x() * _vertex.y() * _vertex.z()), _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 /// Return true if the vertex is in the set of already processed vertices
bool hasVertexBeenProcessed(const std::unordered_multimap<int32_t, vec3>& _processTriangleVertices, bool hasVertexBeenProcessed(const std::unordered_multimap<int32_t, vec3>& _processTriangleVertices,

28
ephysics/collision/narrowphase/DefaultCollisionDispatch.hpp
View File

@ -5,48 +5,32 @@
*/ */
#pragma once #pragma once
// Libraries
#include <ephysics/collision/narrowphase/CollisionDispatch.hpp> #include <ephysics/collision/narrowphase/CollisionDispatch.hpp>
#include <ephysics/collision/narrowphase/ConcaveVsConvexAlgorithm.hpp> #include <ephysics/collision/narrowphase/ConcaveVsConvexAlgorithm.hpp>
#include <ephysics/collision/narrowphase/SphereVsSphereAlgorithm.hpp> #include <ephysics/collision/narrowphase/SphereVsSphereAlgorithm.hpp>
#include <ephysics/collision/narrowphase/GJK/GJKAlgorithm.hpp> #include <ephysics/collision/narrowphase/GJK/GJKAlgorithm.hpp>
namespace ephysics { 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 * Collision dispatching decides which collision
* algorithm to use given two types of proxy collision shapes. * algorithm to use given two types of proxy collision shapes.
*/ */
class DefaultCollisionDispatch : public CollisionDispatch { class DefaultCollisionDispatch : public CollisionDispatch {
protected: protected:
SphereVsSphereAlgorithm mSphereVsSphereAlgorithm; //!< Sphere vs Sphere collision algorithm
/// Sphere vs Sphere collision algorithm ConcaveVsConvexAlgorithm mConcaveVsConvexAlgorithm; //!< Concave vs Convex collision algorithm
SphereVsSphereAlgorithm mSphereVsSphereAlgorithm; GJKAlgorithm mGJKAlgorithm; //!< GJK Algorithm
/// Concave vs Convex collision algorithm
ConcaveVsConvexAlgorithm mConcaveVsConvexAlgorithm;
/// GJK Algorithm
GJKAlgorithm mGJKAlgorithm;
public: public:
/// Constructor /// Constructor
DefaultCollisionDispatch(); DefaultCollisionDispatch();
/// Destructor /// Destructor
virtual ~DefaultCollisionDispatch(); virtual ~DefaultCollisionDispatch();
/// Initialize the collision dispatch configuration /// Initialize the collision dispatch configuration
virtual void init(CollisionDetection* collisionDetection, virtual void init(CollisionDetection* _collisionDetection, MemoryAllocator* _memoryAllocator);
MemoryAllocator* memoryAllocator);
/// Select and return the narrow-phase collision detection algorithm to /// Select and return the narrow-phase collision detection algorithm to
/// use between two types of collision shapes. /// 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);
}; };
} }

4
ephysics/collision/narrowphase/GJK/GJKAlgorithm.cpp
View File

@ -82,7 +82,7 @@ void GJKAlgorithm::testCollision(const CollisionShapeInfo& shape1Info,
Simplex simplex; Simplex simplex;
// Get the previous point V (last cached separating axis) // 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 // Initialize the upper bound for the square distance
float distSquare = DECIMAL_LARGEST; float distSquare = DECIMAL_LARGEST;
@ -103,7 +103,7 @@ void GJKAlgorithm::testCollision(const CollisionShapeInfo& shape1Info,
if (vDotw > 0.0 && vDotw * vDotw > distSquare * marginSquare) { if (vDotw > 0.0 && vDotw * vDotw > distSquare * marginSquare) {
// Cache the current separating axis for frame coherence // Cache the current separating axis for frame coherence
mCurrentOverlappingPair->setCachedSeparatingAxis(v); m_currentOverlappingPair->setCachedSeparatingAxis(v);
// No int32_tersection, we return // No int32_tersection, we return
return; return;

11
ephysics/collision/narrowphase/NarrowPhaseAlgorithm.cpp
View File

@ -4,25 +4,24 @@
* @license BSD 3 clauses (see license file) * @license BSD 3 clauses (see license file)
*/ */
// Libraries
#include <ephysics/collision/narrowphase/NarrowPhaseAlgorithm.hpp> #include <ephysics/collision/narrowphase/NarrowPhaseAlgorithm.hpp>
// We want to use the ReactPhysics3D namespace
using namespace ephysics; using namespace ephysics;
// Constructor
NarrowPhaseAlgorithm::NarrowPhaseAlgorithm() NarrowPhaseAlgorithm::NarrowPhaseAlgorithm()
: m_memoryAllocator(NULL), mCurrentOverlappingPair(NULL) { : m_memoryAllocator(NULL), m_currentOverlappingPair(NULL) {
} }
// Destructor
NarrowPhaseAlgorithm::~NarrowPhaseAlgorithm() { NarrowPhaseAlgorithm::~NarrowPhaseAlgorithm() {
} }
// Initalize the algorithm
void NarrowPhaseAlgorithm::init(CollisionDetection* collisionDetection, MemoryAllocator* memoryAllocator) { void NarrowPhaseAlgorithm::init(CollisionDetection* collisionDetection, MemoryAllocator* memoryAllocator) {
m_collisionDetection = collisionDetection; m_collisionDetection = collisionDetection;
m_memoryAllocator = memoryAllocator; m_memoryAllocator = memoryAllocator;
} }
void NarrowPhaseAlgorithm::setCurrentOverlappingPair(OverlappingPair* _overlappingPair) {
m_currentOverlappingPair = _overlappingPair;
}

120
ephysics/collision/narrowphase/NarrowPhaseAlgorithm.hpp
View File

@ -5,90 +5,58 @@
*/ */
#pragma once #pragma once
// Libraries
#include <ephysics/body/Body.hpp> #include <ephysics/body/Body.hpp>
#include <ephysics/constraint/ContactPoint.hpp> #include <ephysics/constraint/ContactPoint.hpp>
#include <ephysics/memory/MemoryAllocator.hpp> #include <ephysics/memory/MemoryAllocator.hpp>
#include <ephysics/engine/OverlappingPair.hpp> #include <ephysics/engine/OverlappingPair.hpp>
#include <ephysics/collision/CollisionShapeInfo.hpp> #include <ephysics/collision/CollisionShapeInfo.hpp>
/// Namespace ReactPhysics3D
namespace ephysics { namespace ephysics {
class CollisionDetection; class CollisionDetection;
// Class NarrowPhaseCallback /**
/** * @brief It is the base class for a narrow-phase collision
* This abstract class is the base class for a narrow-phase collision * callback class.
* callback class. */
*/ class NarrowPhaseCallback {
class NarrowPhaseCallback { public:
virtual ~NarrowPhaseCallback() = default;
public: /// Called by a narrow-phase collision algorithm when a new contact has been found
virtual ~NarrowPhaseCallback() = default; virtual void notifyContact(OverlappingPair* _overlappingPair,
const ContactPointInfo& _contactInfo) = 0;
/// 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
// Class NarrowPhaseAlgorithm * compute information about the contact between two proxy shapes.
/** */
* This abstract class is the base class for a narrow-phase collision class NarrowPhaseAlgorithm {
* detection algorithm. The goal of the narrow phase algorithm is to protected :
* compute information about the contact between two proxy shapes. CollisionDetection* m_collisionDetection; //!< Pointer to the collision detection object
*/ MemoryAllocator* m_memoryAllocator; //!< Pointer to the memory allocator
class NarrowPhaseAlgorithm { OverlappingPair* m_currentOverlappingPair; //!< Overlapping pair of the bodies currently tested for collision
/// Private copy-constructor
protected : NarrowPhaseAlgorithm(const NarrowPhaseAlgorithm& algorithm) = delete;
/// Private assignment operator
// -------------------- Attributes -------------------- // NarrowPhaseAlgorithm& operator=(const NarrowPhaseAlgorithm& algorithm) = delete;
public :
/// Pointer to the collision detection object /// Constructor
CollisionDetection* m_collisionDetection; NarrowPhaseAlgorithm();
/// Destructor
/// Pointer to the memory allocator virtual ~NarrowPhaseAlgorithm();
MemoryAllocator* m_memoryAllocator; /// Initalize the algorithm
virtual void init(CollisionDetection* _collisionDetection,
/// Overlapping pair of the bodies currently tested for collision MemoryAllocator* _memoryAllocator);
OverlappingPair* mCurrentOverlappingPair; /// Set the current overlapping pair of bodies
void setCurrentOverlappingPair(OverlappingPair* _overlappingPair);
// -------------------- Methods -------------------- // /// Compute a contact info if the two bounding volume collide
virtual void testCollision(const CollisionShapeInfo& _shape1Info,
/// Private copy-constructor const CollisionShapeInfo& _shape2Info,
NarrowPhaseAlgorithm(const NarrowPhaseAlgorithm& algorithm); NarrowPhaseCallback* _narrowPhaseCallback) = 0;
};
/// 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;
}
} }

4
ephysics/collision/narrowphase/SphereVsSphereAlgorithm.cpp
View File

@ -25,7 +25,7 @@ void ephysics::SphereVsSphereAlgorithm::testCollision(const ephysics::CollisionS
vec3 vectorBetweenCenters = transform2.getPosition() - transform1.getPosition(); vec3 vectorBetweenCenters = transform2.getPosition() - transform1.getPosition();
float squaredDistanceBetweenCenters = vectorBetweenCenters.length2(); float squaredDistanceBetweenCenters = vectorBetweenCenters.length2();
// Compute the sum of the radius // 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 the sphere collision shapes intersect
if (squaredDistanceBetweenCenters <= sumRadius * sumRadius) { if (squaredDistanceBetweenCenters <= sumRadius * sumRadius) {
vec3 centerSphere2InBody1LocalSpace = transform1.getInverse() * transform2.getPosition(); vec3 centerSphere2InBody1LocalSpace = transform1.getInverse() * transform2.getPosition();
@ -44,6 +44,6 @@ void ephysics::SphereVsSphereAlgorithm::testCollision(const ephysics::CollisionS
intersectionOnBody1, intersectionOnBody1,
intersectionOnBody2); intersectionOnBody2);
// Notify about the new contact // Notify about the new contact
_narrowPhaseCallback->notifyContact(shape1Info.overlappingPair, contactInfo); _narrowPhaseCallback->notifyContact(_shape1Info.overlappingPair, contactInfo);
} }
} }

49
ephysics/collision/narrowphase/SphereVsSphereAlgorithm.hpp
View File

@ -10,31 +10,30 @@
#include <ephysics/collision/narrowphase/NarrowPhaseAlgorithm.hpp> #include <ephysics/collision/narrowphase/NarrowPhaseAlgorithm.hpp>
namespace ephysics { namespace ephysics {
/** /**
* @brief It is used to compute the narrow-phase collision detection * @brief It is used to compute the narrow-phase collision detection
* between two sphere collision shapes. * between two sphere collision shapes.
*/ */
class SphereVsSphereAlgorithm : public NarrowPhaseAlgorithm { class SphereVsSphereAlgorithm : public NarrowPhaseAlgorithm {
protected : protected :
SphereVsSphereAlgorithm(const SphereVsSphereAlgorithm& algorithm) = delete; SphereVsSphereAlgorithm(const SphereVsSphereAlgorithm&) = delete;
SphereVsSphereAlgorithm& operator=(const SphereVsSphereAlgorithm& algorithm) = delete; SphereVsSphereAlgorithm& operator=(const SphereVsSphereAlgorithm&) = delete;
public : public :
/** /**
* @brief Constructor * @brief Constructor
*/ */
SphereVsSphereAlgorithm(); SphereVsSphereAlgorithm();
/** /**
* @brief Destructor * @brief Destructor
*/ */
virtual ~SphereVsSphereAlgorithm() = default; virtual ~SphereVsSphereAlgorithm() = default;
/** /**
* @brief Compute a contact info if the two bounding volume collide * @brief Compute a contact info if the two bounding volume collide
*/ */
virtual void testCollision(const CollisionShapeInfo& _shape1Info, virtual void testCollision(const CollisionShapeInfo& _shape1Info,
const CollisionShapeInfo& _shape2Info, const CollisionShapeInfo& _shape2Info,
NarrowPhaseCallback* _narrowPhaseCallback); NarrowPhaseCallback* _narrowPhaseCallback);
}; };
} }

2
ephysics/collision/shapes/AABB.cpp
View File

@ -190,7 +190,7 @@ bool AABB::testCollisionTriangleAABB(const vec3* _trianglePoints) const {
bool AABB::contains(const vec3& _point) const { bool AABB::contains(const vec3& _point) const {
return _point.x() >= m_minCoordinates.x() - MACHINE_EPSILON && _point.x() <= m_maxCoordinates.x() + MACHINE_EPSILON 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.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) { AABB& AABB::operator=(const AABB& _aabb) {

56
ephysics/collision/shapes/BoxShape.cpp
View File

@ -107,3 +107,59 @@ bool BoxShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* pro
return true; 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]);
}

52
ephysics/collision/shapes/BoxShape.hpp
View File

@ -56,56 +56,4 @@ class BoxShape : public ConvexShape {
virtual void computeLocalInertiaTensor(etk::Matrix3x3& tensor, float mass) const; 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]);
}
} }

76
ephysics/collision/shapes/CapsuleShape.cpp
View File

@ -261,3 +261,79 @@ bool CapsuleShape::raycastWithSphereEndCap(const vec3& point1, const vec3& point
return false; 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);
}
}

89
ephysics/collision/shapes/CapsuleShape.hpp
View File

@ -10,7 +10,6 @@
#include <ephysics/mathematics/mathematics.hpp> #include <ephysics/mathematics/mathematics.hpp>
namespace ephysics { namespace ephysics {
/** /**
* @brief It represents a capsule collision shape that is defined around the Y axis. * @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. * 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) float m_halfHeight; //!< Half height of the capsule (height = distance between the centers of the two spheres)
/// Private copy-constructor /// Private copy-constructor
CapsuleShape(const CapsuleShape& shape); CapsuleShape(const CapsuleShape& shape);
/// Private assignment operator /// Private assignment operator
CapsuleShape& operator=(const CapsuleShape& shape); CapsuleShape& operator=(const CapsuleShape& shape);
/// Return a local support point in a given direction without the object margin /// Return a local support point in a given direction without the object margin
virtual vec3 getLocalSupportPointWithoutMargin(const vec3& direction, virtual vec3 getLocalSupportPointWithoutMargin(const vec3& direction,
void** cachedCollisionData) const; void** cachedCollisionData) const;
/// Return true if a point is inside the collision shape /// Return true if a point is inside the collision shape
virtual bool testPointInside(const vec3& localPoint, ProxyShape* proxyShape) const; virtual bool testPointInside(const vec3& localPoint, ProxyShape* proxyShape) const;
/// Raycast method with feedback information /// Raycast method with feedback information
virtual bool raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape) const; 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 /// Raycasting method between a ray one of the two spheres end cap of the capsule
bool raycastWithSphereEndCap(const vec3& point1, const vec3& point2, bool raycastWithSphereEndCap(const vec3& point1, const vec3& point2,
const vec3& sphereCenter, float maxFraction, const vec3& sphereCenter, float maxFraction,
vec3& hitLocalPoint, float& hitFraction) const; vec3& hitLocalPoint, float& hitFraction) const;
/// Return the number of bytes used by the collision shape /// Return the number of bytes used by the collision shape
virtual size_t getSizeInBytes() const; virtual size_t getSizeInBytes() const;
public : public :
/// Constructor /// Constructor
CapsuleShape(float _radius, float _height); CapsuleShape(float _radius, float _height);
/// Destructor /// Destructor
virtual ~CapsuleShape(); virtual ~CapsuleShape();
/// Return the radius of the capsule /// Return the radius of the capsule
float getRadius() const; float getRadius() const;
/// Return the height of the capsule /// Return the height of the capsule
float getHeight() const; float getHeight() const;
/// Set the scaling vector of the collision shape /// Set the scaling vector of the collision shape
virtual void setLocalScaling(const vec3& scaling); virtual void setLocalScaling(const vec3& scaling);
/// Return the local bounds of the shape in x, y and z directions /// Return the local bounds of the shape in x, y and z directions
virtual void getLocalBounds(vec3& min, vec3& max) const; virtual void getLocalBounds(vec3& min, vec3& max) const;
/// Return the local inertia tensor of the collision shape /// Return the local inertia tensor of the collision shape
virtual void computeLocalInertiaTensor(etk::Matrix3x3& tensor, float mass) const; 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);
}
}
} }

11
ephysics/collision/shapes/CollisionShape.cpp
View File

@ -54,3 +54,14 @@ void CollisionShape::computeAABB(AABB& aabb, const etk::Transform3D& transform)
aabb.setMin(minCoordinates); aabb.setMin(minCoordinates);
aabb.setMax(maxCoordinates); 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;
}
}

88
ephysics/collision/shapes/CollisionShape.hpp
View File

@ -42,67 +42,57 @@ class CollisionShape {
virtual size_t getSizeInBytes() const = 0; virtual size_t getSizeInBytes() const = 0;
public : public :
/// Constructor /// Constructor
CollisionShape(CollisionShapeType type); CollisionShape(CollisionShapeType _type);
/// Destructor /// Destructor
virtual ~CollisionShape(); virtual ~CollisionShape();
/// Return the type of the collision shapes /**
CollisionShapeType getType() const; * @brief Get the type of the collision shapes
/// Return true if the collision shape is convex, false if it is concave * @return The type of the collision shape (box, sphere, cylinder, ...)
virtual bool isConvex() const=0; */
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 /// 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 /// 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 /// 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 /// 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 /// Compute the world-space AABB of the collision shape given a transform
virtual void computeAABB(AABB& aabb, const etk::Transform3D& transform) const; 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); * @brief Check if the shape is convex
/// Return the maximum number of contact manifolds in an overlapping pair given two shape types * @param[in] _shapeType shape type
static int32_t computeNbMaxContactManifolds(CollisionShapeType shapeType1, * @return true If the collision shape is convex
CollisionShapeType shapeType2); * @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 ProxyShape;
friend class CollisionWorld; 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;
}
}
} }

66
ephysics/collision/shapes/ConcaveMeshShape.cpp
View File

@ -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);
}

67
ephysics/collision/shapes/ConcaveMeshShape.hpp
View File

@ -12,7 +12,6 @@
#include <ephysics/engine/Profiler.hpp> #include <ephysics/engine/Profiler.hpp>
namespace ephysics { namespace ephysics {
class ConcaveMeshShape; class ConcaveMeshShape;
class ConvexTriangleAABBOverlapCallback : public DynamicAABBTreeOverlapCallback { class ConvexTriangleAABBOverlapCallback : public DynamicAABBTreeOverlapCallback {
private: private:
@ -32,10 +31,8 @@ namespace ephysics {
// Called when a overlapping node has been found during the call to // Called when a overlapping node has been found during the call to
// DynamicAABBTree:reportAllShapesOverlappingWithAABB() // DynamicAABBTree:reportAllShapesOverlappingWithAABB()
virtual void notifyOverlappingNode(int32_t _nodeId); virtual void notifyOverlappingNode(int32_t _nodeId);
}; };
/// Class ConcaveMeshRaycastCallback
class ConcaveMeshRaycastCallback : public DynamicAABBTreeRaycastCallback { class ConcaveMeshRaycastCallback : public DynamicAABBTreeRaycastCallback {
private : private :
std::vector<int32_t> m_hitAABBNodes; std::vector<int32_t> m_hitAABBNodes;
@ -110,68 +107,4 @@ namespace ephysics {
friend class ConcaveMeshRaycastCallback; 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);
}
} }

41
ephysics/collision/shapes/ConcaveShape.cpp
View File

@ -22,3 +22,44 @@ ConcaveShape::ConcaveShape(CollisionShapeType type)
ConcaveShape::~ConcaveShape() { 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;
}

41
ephysics/collision/shapes/ConcaveShape.hpp
View File

@ -56,47 +56,6 @@ namespace ephysics {
void setIsSmoothMeshCollisionEnabled(bool _isEnabled); 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;
}
} }

70
ephysics/collision/shapes/ConeShape.cpp
View File

@ -190,3 +190,73 @@ bool ConeShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* pr
return true; 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);
}

71
ephysics/collision/shapes/ConeShape.hpp
View File

@ -56,75 +56,4 @@ namespace ephysics {
/// Return the local inertia tensor of the collision shape /// Return the local inertia tensor of the collision shape
virtual void computeLocalInertiaTensor(etk::Matrix3x3& _tensor, float _mass) const; 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);
}
} }

124
ephysics/collision/shapes/ConvexMeshShape.cpp
View File

@ -257,3 +257,127 @@ void ConvexMeshShape::recalculateBounds() {
bool ConvexMeshShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape) const { 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); 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<uint32_t>()));
}
// 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<uint32_t>()));
}
// 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);
}

300
ephysics/collision/shapes/ConvexMeshShape.hpp
View File

@ -5,7 +5,6 @@
*/ */
#pragma once #pragma once
// Libraries
#include <ephysics/collision/shapes/ConvexShape.hpp> #include <ephysics/collision/shapes/ConvexShape.hpp>
#include <ephysics/engine/CollisionWorld.hpp> #include <ephysics/engine/CollisionWorld.hpp>
#include <ephysics/mathematics/mathematics.hpp> #include <ephysics/mathematics/mathematics.hpp>
@ -15,240 +14,73 @@
#include <set> #include <set>
#include <map> #include <map>
/// ReactPhysics3D namespace
namespace ephysics { namespace ephysics {
class CollisionWorld;
// Declaration /**
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
// Class ConvexMeshShape * 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
* This class represents a convex mesh shape. In order to create a convex mesh shape, you * mesh. The center of mass of the shape will be at the origin of the local-space geometry
* need to indicate the local-space position of the mesh vertices. You do it either by * that you use to create the mesh. The method used for collision detection with a convex
* passing a vertices array to the constructor or using the addVertex() method. Make sure * mesh shape has an O(n) running time with "n" beeing the number of vertices in the mesh.
* that the set of vertices that you use to create the shape are indeed part of a convex * Therefore, you should try not to use too many vertices. However, it is possible to speed
* mesh. The center of mass of the shape will be at the origin of the local-space geometry * up the collision detection by using the edges information of your mesh. The running time
* that you use to create the mesh. The method used for collision detection with a convex * of the collision detection that uses the edges is almost O(1) constant time at the cost
* mesh shape has an O(n) running time with "n" beeing the number of vertices in the mesh. * of additional memory used to store the vertices. You can indicate edges information
* Therefore, you should try not to use too many vertices. However, it is possible to speed * with the addEdge() method. Then, you must use the setIsEdgesInformationUsed(true) method
* up the collision detection by using the edges information of your mesh. The running time * in order to use the edges information for collision detection.
* 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 class ConvexMeshShape : public ConvexShape {
* with the addEdge() method. Then, you must use the setIsEdgesInformationUsed(true) method protected :
* in order to use the edges information for collision detection. std::vector<vec3> m_vertices; //!< Array with the vertices of the mesh
*/ uint32_t m_numberVertices; //!< Number of vertices in the mesh
class ConvexMeshShape : public ConvexShape { 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
protected : bool m_isEdgesInformationUsed; //!< True if the shape contains the edges of the convex mesh in order to make the collision detection faster
std::map<uint32_t, std::set<uint32_t> > m_edgesAdjacencyList; //!< Adjacency list representing the edges of the mesh
// -------------------- Attributes -------------------- // /// Private copy-constructor
ConvexMeshShape(const ConvexMeshShape& shape);
/// Array with the vertices of the mesh /// Private assignment operator
std::vector<vec3> m_vertices; ConvexMeshShape& operator=(const ConvexMeshShape& shape);
/// Recompute the bounds of the mesh
/// Number of vertices in the mesh void recalculateBounds();
uint32_t m_numberVertices; /// Set the scaling vector of the collision shape
virtual void setLocalScaling(const vec3& scaling);
/// Mesh minimum bounds in the three local x, y and z directions /// Return a local support point in a given direction without the object margin.
vec3 m_minBounds; virtual vec3 getLocalSupportPointWithoutMargin(const vec3& direction,
void** cachedCollisionData) const;
/// Mesh maximum bounds in the three local x, y and z directions /// Return true if a point is inside the collision shape
vec3 m_maxBounds; virtual bool testPointInside(const vec3& localPoint, ProxyShape* proxyShape) const;
/// Raycast method with feedback information
/// True if the shape contains the edges of the convex mesh in order to virtual bool raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape) const;
/// make the collision detection faster /// Return the number of bytes used by the collision shape
bool m_isEdgesInformationUsed; virtual size_t getSizeInBytes() const;
public :
/// Adjacency list representing the edges of the mesh /// Constructor to initialize with an array of 3D vertices.
std::map<uint32_t, std::set<uint32_t> > m_edgesAdjacencyList; ConvexMeshShape(const float* arrayVertices, uint32_t nbVertices, int32_t stride,
float margin = OBJECT_MARGIN);
// -------------------- Methods -------------------- // /// Constructor to initialize with a triangle vertex array
ConvexMeshShape(TriangleVertexArray* triangleVertexArray, bool isEdgesInformationUsed = true,
/// Private copy-constructor float margin = OBJECT_MARGIN);
ConvexMeshShape(const ConvexMeshShape& shape); /// Constructor.
ConvexMeshShape(float margin = OBJECT_MARGIN);
/// Private assignment operator /// Destructor
ConvexMeshShape& operator=(const ConvexMeshShape& shape); virtual ~ConvexMeshShape();
/// Return the local bounds of the shape in x, y and z directions
/// Recompute the bounds of the mesh virtual void getLocalBounds(vec3& min, vec3& max) const;
void recalculateBounds(); /// Return the local inertia tensor of the collision shape.
virtual void computeLocalInertiaTensor(etk::Matrix3x3& tensor, float mass) const;
/// Set the scaling vector of the collision shape /// Add a vertex int32_to the convex mesh
virtual void setLocalScaling(const vec3& scaling); void addVertex(const vec3& vertex);
/// Add an edge int32_to the convex mesh by specifying the two vertex indices of the edge.
/// Return a local support point in a given direction without the object margin. void addEdge(uint32_t v1, uint32_t v2);
virtual vec3 getLocalSupportPointWithoutMargin(const vec3& direction, /// Return true if the edges information is used to speed up the collision detection
void** cachedCollisionData) const; bool isEdgesInformationUsed() const;
/// Set the variable to know if the edges information is used to speed up the
/// Return true if a point is inside the collision shape /// collision detection
virtual bool testPointInside(const vec3& localPoint, ProxyShape* proxyShape) const; void setIsEdgesInformationUsed(bool isEdgesUsed);
};
/// 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();
} }
// 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<uint32_t>()));
}
// 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<uint32_t>()));
}
// 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);
}
}

58
ephysics/collision/shapes/ConvexShape.hpp
View File

@ -5,80 +5,46 @@
*/ */
#pragma once #pragma once
// Libraries
#include <ephysics/collision/shapes/CollisionShape.hpp> #include <ephysics/collision/shapes/CollisionShape.hpp>
/// ReactPhysics3D namespace
namespace ephysics { 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. * body that is used during the narrow-phase collision detection.
*/ */
class ConvexShape : public CollisionShape { class ConvexShape : public CollisionShape {
protected : protected :
float m_margin; //!< Margin used for the GJK collision detection algorithm
// -------------------- Attributes -------------------- //
/// Margin used for the GJK collision detection algorithm
float m_margin;
// -------------------- Methods -------------------- //
/// Private copy-constructor /// Private copy-constructor
ConvexShape(const ConvexShape& shape); ConvexShape(const ConvexShape& shape);
/// Private assignment operator /// Private assignment operator
ConvexShape& operator=(const ConvexShape& shape); ConvexShape& operator=(const ConvexShape& shape);
// Return a local support point in a given direction with the object margin // Return a local support point in a given direction with the object margin
vec3 getLocalSupportPointWithMargin(const vec3& direction, vec3 getLocalSupportPointWithMargin(const vec3& direction,
void** cachedCollisionData) const; void** cachedCollisionData) const;
/// Return a local support point in a given direction without the object margin /// Return a local support point in a given direction without the object margin
virtual vec3 getLocalSupportPointWithoutMargin(const vec3& direction, virtual vec3 getLocalSupportPointWithoutMargin(const vec3& direction,
void** cachedCollisionData) const=0; void** cachedCollisionData) const=0;
/// Return true if a point is inside the collision shape /// Return true if a point is inside the collision shape
virtual bool testPointInside(const vec3& worldPoint, ProxyShape* proxyShape) const=0; virtual bool testPointInside(const vec3& worldPoint, ProxyShape* proxyShape) const=0;
public : public :
// -------------------- Methods -------------------- //
/// Constructor /// Constructor
ConvexShape(CollisionShapeType type, float margin); ConvexShape(CollisionShapeType type, float margin);
/// Destructor /// Destructor
virtual ~ConvexShape(); virtual ~ConvexShape();
/**
/// Return the current object margin * @brief Get the current object margin
float getMargin() const; * @return The margin (in meters) around the collision shape
*/
/// Return true if the collision shape is convex, false if it is concave float getMargin() const {
virtual bool isConvex() const; return m_margin;
}
// -------------------- Friendship -------------------- // virtual bool isConvex() const override {
return true;
}
friend class GJKAlgorithm; friend class GJKAlgorithm;
friend class EPAAlgorithm; 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;
}
} }

67
ephysics/collision/shapes/CylinderShape.cpp
View File

@ -216,3 +216,70 @@ bool CylinderShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape
return true; 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);
}

103
ephysics/collision/shapes/CylinderShape.hpp
View File

@ -5,18 +5,13 @@
*/ */
#pragma once #pragma once
// Libraries
#include <ephysics/collision/shapes/ConvexShape.hpp> #include <ephysics/collision/shapes/ConvexShape.hpp>
#include <ephysics/body/CollisionBody.hpp> #include <ephysics/body/CollisionBody.hpp>
#include <ephysics/mathematics/mathematics.hpp> #include <ephysics/mathematics/mathematics.hpp>
/// ReactPhysics3D namespace
namespace ephysics { 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 centered at the origin. The cylinder is defined by its height
* and the radius of its base. The "transform" of the corresponding * and the radius of its base. The "transform" of the corresponding
* rigid body gives an orientation and a position to the cylinder. * 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. * default margin distance by not using the "margin" parameter in the constructor.
*/ */
class CylinderShape : public ConvexShape { class CylinderShape : public ConvexShape {
protected : protected :
float mRadius; //!< Radius of the base
// -------------------- Attributes -------------------- // float m_halfHeight; //!< Half height of the cylinder
/// Radius of the base
float mRadius;
/// Half height of the cylinder
float m_halfHeight;
// -------------------- Methods -------------------- //
/// Private copy-constructor /// Private copy-constructor
CylinderShape(const CylinderShape& shape); CylinderShape(const CylinderShape& shape);
/// Private assignment operator /// Private assignment operator
CylinderShape& operator=(const CylinderShape& shape); CylinderShape& operator=(const CylinderShape& shape);
/// Return a local support point in a given direction without the object margin /// Return a local support point in a given direction without the object margin
virtual vec3 getLocalSupportPointWithoutMargin(const vec3& direction, virtual vec3 getLocalSupportPointWithoutMargin(const vec3& direction,
void** cachedCollisionData) const; void** cachedCollisionData) const;
/// Return true if a point is inside the collision shape /// Return true if a point is inside the collision shape
virtual bool testPointInside(const vec3& localPoint, ProxyShape* proxyShape) const; virtual bool testPointInside(const vec3& localPoint, ProxyShape* proxyShape) const;
/// Raycast method with feedback information /// Raycast method with feedback information
virtual bool raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape) const; virtual bool raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape) const;
/// Return the number of bytes used by the collision shape /// Return the number of bytes used by the collision shape
virtual size_t getSizeInBytes() const; virtual size_t getSizeInBytes() const;
public : public :
// -------------------- Methods -------------------- //
/// Constructor /// Constructor
CylinderShape(float radius, float height, float margin = OBJECT_MARGIN); CylinderShape(float radius, float height, float margin = OBJECT_MARGIN);
/// Destructor /// Destructor
virtual ~CylinderShape(); virtual ~CylinderShape();
/// Return the radius /// Return the radius
float getRadius() const; float getRadius() const;
/// Return the height /// Return the height
float getHeight() const; float getHeight() const;
/// Set the scaling vector of the collision shape /// Set the scaling vector of the collision shape
virtual void setLocalScaling(const vec3& scaling); virtual void setLocalScaling(const vec3& scaling);
/// Return the local bounds of the shape in x, y and z directions /// Return the local bounds of the shape in x, y and z directions
virtual void getLocalBounds(vec3& min, vec3& max) const; virtual void getLocalBounds(vec3& min, vec3& max) const;
/// Return the local inertia tensor of the collision shape /// Return the local inertia tensor of the collision shape
virtual void computeLocalInertiaTensor(etk::Matrix3x3& tensor, float mass) const; 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);
}
} }

58
ephysics/collision/shapes/HeightFieldShape.cpp
View File

@ -246,3 +246,61 @@ void TriangleOverlapCallback::testTriangle(const vec3* trianglePoints) {
m_isHit = true; 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);
}

266
ephysics/collision/shapes/HeightFieldShape.hpp
View File

@ -5,7 +5,6 @@
*/ */
#pragma once #pragma once
// Libraries
#include <ephysics/collision/shapes/ConcaveShape.hpp> #include <ephysics/collision/shapes/ConcaveShape.hpp>
#include <ephysics/collision/shapes/TriangleShape.hpp> #include <ephysics/collision/shapes/TriangleShape.hpp>
#include <ephysics/engine/Profiler.hpp> #include <ephysics/engine/Profiler.hpp>
@ -43,188 +42,89 @@ namespace ephysics {
}; };
/** /**
* @brief This class represents a static height field that can be used to represent * @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 * 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 * 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. * 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 * 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 * 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 * 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. * minimum height of the field in the simulation will be -300 and the maximum height will be 300.
*/ */
class HeightFieldShape : public ConcaveShape { class HeightFieldShape : public ConcaveShape {
public:
public: /**
* @brief Data type for the height data of the height field
/// Data type for the height data of the height field */
enum HeightDataType {HEIGHT_FLOAT_TYPE, HEIGHT_DOUBLE_TYPE, HEIGHT_INT_TYPE}; enum HeightDataType {
HEIGHT_FLOAT_TYPE,
protected: HEIGHT_DOUBLE_TYPE,
HEIGHT_INT_TYPE
// -------------------- Attributes -------------------- // };
protected:
/// Number of columns in the grid of the height field int32_t m_numberColumns; //!< Number of columns in the grid of the height field
int32_t m_numberColumns; int32_t m_numberRows; //!< Number of rows in the grid of the height field
float m_width; //!< Height field width
/// Number of rows in the grid of the height field float m_length; //!< Height field length
int32_t m_numberRows; float m_minHeight; //!< Minimum height of the height field
float m_maxHeight; //!< Maximum height of the height field
/// Height field width int32_t m_upAxis; //!< Up axis direction (0 => x, 1 => y, 2 => z)
float m_width; float m_integerHeightScale; //!< Height values scale for height field with int32_teger height values
HeightDataType m_heightDataType; //!< Data type of the height values
/// Height field length const void* m_heightFieldData; //!< Array of data with all the height values of the height field
float m_length; AABB m_AABB; //!< Local AABB of the height field (without scaling)
/// Private copy-constructor
/// Minimum height of the height field HeightFieldShape(const HeightFieldShape& _shape);
float m_minHeight; /// Private assignment operator
HeightFieldShape& operator=(const HeightFieldShape& _shape);
/// Maximum height of the height field /// Raycast method with feedback information
float m_maxHeight; virtual bool raycast(const Ray& _ray, RaycastInfo& _raycastInfo, ProxyShape* _proxyShape) const;
/// Return the number of bytes used by the collision shape
/// Up axis direction (0 => x, 1 => y, 2 => z) virtual size_t getSizeInBytes() const;
int32_t m_upAxis; /// Insert all the triangles int32_to the dynamic AABB tree
void initBVHTree();
/// Height values scale for height field with int32_teger height values /// Return the three vertices coordinates (in the array outTriangleVertices) of a triangle
float m_integerHeightScale; /// given the start vertex index pointer of the triangle.
void getTriangleVerticesWithIndexPointer(int32_t _subPart,
/// Data type of the height values int32_t _triangleIndex,
HeightDataType m_heightDataType; vec3* _outTriangleVertices) const;
/// Return the vertex (local-coordinates) of the height field at a given (x,y) position
/// Array of data with all the height values of the height field vec3 getVertexAt(int32_t _x, int32_t _y) const;
const void* m_heightFieldData; /// Return the height of a given (x,y) point in the height field
float getHeightAt(int32_t _x, int32_t _y) const;
/// Local AABB of the height field (without scaling) /// Return the closest inside int32_teger grid value of a given floating grid value
AABB m_AABB; 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
// -------------------- Methods -------------------- // void computeMinMaxGridCoordinates(int32_t* _minCoords, int32_t* _maxCoords, const AABB& _aabbToCollide) const;
public:
/// Private copy-constructor /// Constructor
HeightFieldShape(const HeightFieldShape& shape); HeightFieldShape(int32_t _nbGridColumns,
int32_t _nbGridRows,
/// Private assignment operator float _minHeight,
HeightFieldShape& operator=(const HeightFieldShape& shape); float _maxHeight,
const void* _heightFieldData,
/// Raycast method with feedback information HeightDataType _dataType,
virtual bool raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape) const; int32_t _upAxis = 1, float _integerHeightScale = 1.0f);
/// Destructor
/// Return the number of bytes used by the collision shape ~HeightFieldShape();
virtual size_t getSizeInBytes() const; /// Return the number of rows in the height field
int32_t getNbRows() const;
/// Insert all the triangles int32_to the dynamic AABB tree /// Return the number of columns in the height field
void initBVHTree(); int32_t getNbColumns() const;
/// Return the type of height value in the height field
/// Return the three vertices coordinates (in the array outTriangleVertices) of a triangle HeightDataType getHeightDataType() const;
/// given the start vertex index pointer of the triangle. /// Return the local bounds of the shape in x, y and z directions.
void getTriangleVerticesWithIndexPointer(int32_t subPart, int32_t triangleIndex, virtual void getLocalBounds(vec3& _min, vec3& _max) const;
vec3* outTriangleVertices) const; /// Set the local scaling vector of the collision shape
virtual void setLocalScaling(const vec3& _scaling);
/// Return the vertex (local-coordinates) of the height field at a given (x,y) position /// Return the local inertia tensor of the collision shape
vec3 getVertexAt(int32_t x, int32_t y) const; virtual void computeLocalInertiaTensor(etk::Matrix3x3& _tensor, float _mass) const;
/// Use a callback method on all triangles of the concave shape inside a given AABB
/// Return the height of a given (x,y) point in the height field virtual void testAllTriangles(TriangleCallback& _callback, const AABB& _localAABB) const;
float getHeightAt(int32_t x, int32_t y) const; friend class ConvexTriangleAABBOverlapCallback;
friend class ConcaveMeshRaycastCallback;
/// 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);
}
} }

33
ephysics/collision/shapes/SphereShape.cpp
View File

@ -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 // Raycast method with feedback information
bool SphereShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape) const { bool SphereShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape) const {

195
ephysics/collision/shapes/SphereShape.hpp
View File

@ -11,127 +11,78 @@
namespace ephysics { namespace ephysics {
/** /**
* @brief Represents a sphere collision shape that is centered * @brief Represents a sphere collision shape that is centered
* at the origin and defined by its radius. This collision shape does not * at the origin and defined by its radius. This collision shape does not
* have an explicit object margin distance. The margin is implicitly the * have an explicit object margin distance. The margin is implicitly the
* radius of the sphere. Therefore, no need to specify an object margin * radius of the sphere. Therefore, no need to specify an object margin
* for a sphere shape. * for a sphere shape.
*/ */
class SphereShape : public ConvexShape { class SphereShape : public ConvexShape {
protected : protected :
SphereShape(const SphereShape& shape); SphereShape(const SphereShape& _shape);
SphereShape& operator=(const SphereShape& shape) = delete; 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, * @brief Get a local support point in a given direction without the object margin
void** cachedCollisionData) const; * @param[in] _direction
/// Return true if a point is inside the collision shape * @param[in] _cachedCollisionData
virtual bool testPointInside(const vec3& localPoint, ProxyShape* proxyShape) const; * @return the center of the sphere (the radius is taken int32_to account in the object margin)
/// Raycast method with feedback information */
virtual bool raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape) const; vec3 getLocalSupportPointWithoutMargin(const vec3& _direction, void** _cachedCollisionData) const {
/// Return the number of bytes used by the collision shape return vec3(0.0, 0.0, 0.0);
virtual size_t getSizeInBytes() const; }
public : /**
/// Constructor * @brief Test if a point is inside a shape
SphereShape(float radius); * @param[in] _localPoint Point to check
/// Destructor * @param[in] _proxyShape Shape to check
virtual ~SphereShape(); * @return true if a point is inside the collision shape
/// Return the radius of the sphere */
float getRadius() const; bool testPointInside(const vec3& _localPoint, ProxyShape* _proxyShape) const {
/// Set the scaling vector of the collision shape return (_localPoint.length2() < m_margin * m_margin);
virtual void setLocalScaling(const vec3& scaling); }
/// Return the local bounds of the shape in x, y and z directions. /// Raycast method with feedback information
virtual void getLocalBounds(vec3& min, vec3& max) const; virtual bool raycast(const Ray& _ray, RaycastInfo& _raycastInfo, ProxyShape* _proxyShape) const;
/// Return the local inertia tensor of the collision shape /**
virtual void computeLocalInertiaTensor(etk::Matrix3x3& tensor, float mass) const; * @brief Return the number of bytes used by the collision shape
/// Update the AABB of a body using its collision shape */
virtual void computeAABB(AABB& aabb, const etk::Transform3D& transform) const; virtual size_t getSizeInBytes() const {
}; return sizeof(SphereShape);
}
// Get the radius of the sphere public :
/** /// Constructor
* @brief SphereShape(float _radius);
* @return Radius of the sphere (in meters) /// Destructor
*/ virtual ~SphereShape();
float SphereShape::getRadius() const { /**
return m_margin; * @brief Get the radius of the sphere
} * @return Radius of the sphere (in meters)
*/
* @brief float getRadius() const {
// Set the scaling vector of the collision shape return m_margin;
void SphereShape::setLocalScaling(const vec3& scaling) { }
/**
m_margin = (m_margin / m_scaling.x()) * scaling.x(); * @brief Set the scaling vector of the collision shape
*/
CollisionShape::setLocalScaling(scaling); virtual void setLocalScaling(const vec3& _scaling);
} /**
* @brief Get the local bounds of the shape in x, y and z directions.
* @brief * This method is used to compute the AABB of the box
// Return the number of bytes used by the collision shape * @param _min The minimum bounds of the shape in local-space coordinates
size_t SphereShape::getSizeInBytes() const { * @param _max The maximum bounds of the shape in local-space coordinates
return sizeof(SphereShape); */
} virtual void getLocalBounds(vec3& _min, vec3& _max) const;
/**
/** * @brief Compute the local inertia tensor of the sphere
* @brief Get a local support point in a given direction without the object margin * @param[out] _tensor The 3x3 inertia tensor matrix of the shape in local-space coordinates
* @param[in] _direction * @param[in] _mass Mass to use to compute the inertia tensor of the collision shape
* @param[in] _cachedCollisionData */
* @return the center of the sphere (the radius is taken int32_to account in the object margin) virtual void computeLocalInertiaTensor(etk::Matrix3x3& _tensor, float _mass) const;
*/ /**
vec3 SphereShape::getLocalSupportPointWithoutMargin(const vec3& _direction, void** _cachedCollisionData) const { * @brief Update the AABB of a body using its collision shape
return vec3(0.0, 0.0, 0.0); * @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;
* @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);
}
} }

96
ephysics/collision/shapes/TriangleShape.cpp
View File

@ -106,3 +106,99 @@ bool TriangleShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape
return true; 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];
}

218
ephysics/collision/shapes/TriangleShape.hpp
View File

@ -9,170 +9,58 @@
#include <ephysics/collision/shapes/ConvexShape.hpp> #include <ephysics/collision/shapes/ConvexShape.hpp>
namespace ephysics { namespace ephysics {
/**
/** * @brief Raycast test side for the triangle
* @brief Raycast test side for the triangle */
*/ enum TriangleRaycastSide {
enum TriangleRaycastSide { FRONT, //!< Raycast against front triangle
FRONT, //!< Raycast against front triangle BACK, //!< Raycast against back triangle
BACK, //!< Raycast against back triangle FRONT_AND_BACK //!< Raycast against front and back triangle
FRONT_AND_BACK //!< Raycast against front and back triangle };
};
/**
/** * This class represents a triangle collision shape that is centered
* This class represents a triangle collision shape that is centered * at the origin and defined three points.
* at the origin and defined three points. */
*/ class TriangleShape : public ConvexShape {
class TriangleShape : public ConvexShape { protected:
protected: vec3 m_points[3]; //!< Three points of the triangle
vec3 m_points[3]; //!< Three points of the triangle TriangleRaycastSide m_raycastTestType; //!< Raycast test type for the triangle (front, back, front-back)
TriangleRaycastSide m_raycastTestType; //!< Raycast test type for the triangle (front, back, front-back) /// Private copy-constructor
/// Private copy-constructor TriangleShape(const TriangleShape& shape);
TriangleShape(const TriangleShape& shape); /// Private assignment operator
TriangleShape& operator=(const TriangleShape& shape);
/// Private assignment operator /// Return a local support point in a given direction without the object margin
TriangleShape& operator=(const TriangleShape& shape); virtual vec3 getLocalSupportPointWithoutMargin(const vec3& direction,
void** cachedCollisionData) const;
/// Return a local support point in a given direction without the object margin /// Return true if a point is inside the collision shape
virtual vec3 getLocalSupportPointWithoutMargin(const vec3& direction, virtual bool testPointInside(const vec3& localPoint, ProxyShape* proxyShape) const;
void** cachedCollisionData) const; /// Raycast method with feedback information
virtual bool raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape) const;
/// Return true if a point is inside the collision shape /// Return the number of bytes used by the collision shape
virtual bool testPointInside(const vec3& localPoint, ProxyShape* proxyShape) const; virtual size_t getSizeInBytes() const;
public:
/// Raycast method with feedback information /// Constructor
virtual bool raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape) const; TriangleShape(const vec3& point1, const vec3& point2, const vec3& point3,
float margin = OBJECT_MARGIN);
/// Return the number of bytes used by the collision shape /// Destructor
virtual size_t getSizeInBytes() const; virtual ~TriangleShape();
/// Return the local bounds of the shape in x, y and z directions.
public: virtual void getLocalBounds(vec3& min, vec3& max) const;
/// Constructor /// Set the local scaling vector of the collision shape
TriangleShape(const vec3& point1, const vec3& point2, const vec3& point3, virtual void setLocalScaling(const vec3& scaling);
float margin = OBJECT_MARGIN); /// Return the local inertia tensor of the collision shape
virtual void computeLocalInertiaTensor(etk::Matrix3x3& tensor, float mass) const;
/// Destructor /// Update the AABB of a body using its collision shape
virtual ~TriangleShape(); virtual void computeAABB(AABB& aabb, const etk::Transform3D& transform) const;
/// Return the raycast test type (front, back, front-back)
/// Return the local bounds of the shape in x, y and z directions. TriangleRaycastSide getRaycastTestType() const;
virtual void getLocalBounds(vec3& min, vec3& max) const; // Set the raycast test type (front, back, front-back)
void setRaycastTestType(TriangleRaycastSide testType);
/// Set the local scaling vector of the collision shape /// Return the coordinates of a given vertex of the triangle
virtual void setLocalScaling(const vec3& scaling); vec3 getVertex(int32_t index) const;
friend class ConcaveMeshRaycastCallback;
/// Return the local inertia tensor of the collision shape friend class TriangleOverlapCallback;
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];
}
} }

130
ephysics/constraint/ContactPoint.cpp
View File

@ -37,3 +37,133 @@ ContactPoint::ContactPoint(const ContactPointInfo& contactInfo)
ContactPoint::~ContactPoint() { 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);
}

155
ephysics/constraint/ContactPoint.hpp
View File

@ -21,23 +21,14 @@ namespace ephysics {
*/ */
struct ContactPointInfo { struct ContactPointInfo {
public: public:
/// First proxy shape of the contact ProxyShape* shape1; //!< First proxy shape of the contact
ProxyShape* shape1; ProxyShape* shape2; //!< Second proxy shape of the contact
/// Second proxy shape of the contact const CollisionShape* collisionShape1; //!< First collision shape
ProxyShape* shape2; const CollisionShape* collisionShape2; //!< Second collision shape
/// First collision shape vec3 normal; //!< Normalized normal vector of the collision contact in world space
const CollisionShape* collisionShape1; float penetrationDepth; //!< Penetration depth of the contact
/// Second collision shape vec3 localPoint1; //!< Contact point of body 1 in local space of body 1
const CollisionShape* collisionShape2; vec3 localPoint2; //!< Contact point of body 2 in local space of body 2
/// 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
ContactPointInfo(ProxyShape* _proxyShape1, ContactPointInfo(ProxyShape* _proxyShape1,
ProxyShape* _proxyShape2, ProxyShape* _proxyShape2,
const CollisionShape* _collShape1, const CollisionShape* _collShape1,
@ -141,134 +132,4 @@ namespace ephysics {
size_t getSizeInBytes() const; 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);
}
} }

184
ephysics/constraint/FixedJoint.hpp
View File

@ -11,125 +11,69 @@
namespace ephysics { namespace ephysics {
// Structure FixedJointInfo /**
/** * This structure is used to gather the information needed to create a fixed
* 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.
* joint. This structure will be used to create the actual fixed joint. */
*/ struct FixedJointInfo : public JointInfo {
struct FixedJointInfo : public JointInfo { public :
vec3 m_anchorPointWorldSpace; //!< Anchor point (in world-space coordinates)
public : /**
* @breif Contructor
// -------------------- Attributes -------------------- // * @param _rigidBody1 The first body of the joint
* @param _rigidBody2 The second body of the joint
/// Anchor point (in world-space coordinates) * @param _initAnchorPointWorldSpace The initial anchor point of the joint in world-space coordinates
vec3 m_anchorPointWorldSpace; */
FixedJointInfo(RigidBody* _rigidBody1,
/// Constructor RigidBody* _rigidBody2,
/** const vec3& _initAnchorPointWorldSpace):
* @param rigidBody1 The first body of the joint JointInfo(_rigidBody1, _rigidBody2, FIXEDJOINT),
* @param rigidBody2 The second body of the joint m_anchorPointWorldSpace(_initAnchorPointWorldSpace){
* @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), * @breif It represents a fixed joint that is used to forbid any translation or rotation
m_anchorPointWorldSpace(initAnchorPointWorldSpace){} * between two bodies.
}; */
class FixedJoint : public Joint {
// Class FixedJoint private :
/** static const float BETA; //!< Beta value for the bias factor of position correction
* This class represents a fixed joint that is used to forbid any translation or rotation vec3 m_localAnchorPointBody1; //!< Anchor point of body 1 (in local-space coordinates of body 1)
* between two bodies. 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
class FixedJoint : public Joint { 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)
private : etk::Matrix3x3 m_i2; //!< Inertia tensor of body 2 (in world-space coordinates)
vec3 m_impulseTranslation; //!< Accumulated impulse for the 3 translation constraints
// -------------------- Constants -------------------- // 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)
// Beta value for the bias factor of position correction etk::Matrix3x3 m_inverseMassMatrixRotation; //!< Inverse mass matrix K=JM^-1J^-t of the 3 rotation constraints (3x3 matrix)
static const float BETA; vec3 m_biasTranslation; //!< Bias vector for the 3 translation constraints
vec3 m_biasRotation; //!< Bias vector for the 3 rotation constraints
// -------------------- Attributes -------------------- // etk::Quaternion m_initOrientationDifferenceInv; //!< Inverse of the initial orientation difference between the two bodies
/// Private copy-constructor
/// Anchor point of body 1 (in local-space coordinates of body 1) FixedJoint(const FixedJoint& constraint);
vec3 m_localAnchorPointBody1; /// Private assignment operator
FixedJoint& operator=(const FixedJoint& constraint);
/// Anchor point of body 2 (in local-space coordinates of body 2) /// Return the number of bytes used by the joint
vec3 m_localAnchorPointBody2; virtual size_t getSizeInBytes() const override {
return sizeof(FixedJoint);
/// Vector from center of body 2 to anchor point in world-space }
vec3 m_r1World; /// Initialize before solving the constraint
virtual void initBeforeSolve(const ConstraintSolverData& constraintSolverData);
/// Vector from center of body 2 to anchor point in world-space /// Warm start the constraint (apply the previous impulse at the beginning of the step)
vec3 m_r2World; virtual void warmstart(const ConstraintSolverData& constraintSolverData);
/// Solve the velocity constraint
/// Inertia tensor of body 1 (in world-space coordinates) virtual void solveVelocityConstraint(const ConstraintSolverData& constraintSolverData);
etk::Matrix3x3 m_i1; /// Solve the position constraint (for position error correction)
virtual void solvePositionConstraint(const ConstraintSolverData& constraintSolverData);
/// Inertia tensor of body 2 (in world-space coordinates) public :
etk::Matrix3x3 m_i2; /// Constructor
FixedJoint(const FixedJointInfo& jointInfo);
/// Accumulated impulse for the 3 translation constraints /// Destructor
vec3 m_impulseTranslation; virtual ~FixedJoint();
};
/// 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);
} }
}

266
ephysics/constraint/HingeJoint.cpp
View File

@ -18,13 +18,13 @@ const float HingeJoint::BETA = float(0.2);
HingeJoint::HingeJoint(const HingeJointInfo& jointInfo) HingeJoint::HingeJoint(const HingeJointInfo& jointInfo)
: Joint(jointInfo), m_impulseTranslation(0, 0, 0), m_impulseRotation(0, 0), : Joint(jointInfo), m_impulseTranslation(0, 0, 0), m_impulseRotation(0, 0),
m_impulseLowerLimit(0), m_impulseUpperLimit(0), m_impulseMotor(0), m_impulseLowerLimit(0), m_impulseUpperLimit(0), m_impulseMotor(0),
mIsLimitEnabled(jointInfo.isLimitEnabled), mIsMotorEnabled(jointInfo.isMotorEnabled), m_isLimitEnabled(jointInfo.isLimitEnabled), m_isMotorEnabled(jointInfo.isMotorEnabled),
mLowerLimit(jointInfo.minAngleLimit), mUpperLimit(jointInfo.maxAngleLimit), m_lowerLimit(jointInfo.minAngleLimit), m_upperLimit(jointInfo.maxAngleLimit),
mIsLowerLimitViolated(false), mIsUpperLimitViolated(false), m_isLowerLimitViolated(false), m_isUpperLimitViolated(false),
mMotorSpeed(jointInfo.motorSpeed), mMaxMotorTorque(jointInfo.maxMotorTorque) { m_motorSpeed(jointInfo.motorSpeed), m_maxMotorTorque(jointInfo.maxMotorTorque) {
assert(mLowerLimit <= 0 && mLowerLimit >= -2.0 * PI); assert(m_lowerLimit <= 0 && m_lowerLimit >= -2.0 * PI);
assert(mUpperLimit >= 0 && mUpperLimit <= 2.0 * PI); assert(m_upperLimit >= 0 && m_upperLimit <= 2.0 * PI);
// Compute the local-space anchor point for each body // Compute the local-space anchor point for each body
etk::Transform3D transform1 = m_body1->getTransform(); etk::Transform3D transform1 = m_body1->getTransform();
@ -33,10 +33,10 @@ HingeJoint::HingeJoint(const HingeJointInfo& jointInfo)
m_localAnchorPointBody2 = transform2.getInverse() * jointInfo.m_anchorPointWorldSpace; m_localAnchorPointBody2 = transform2.getInverse() * jointInfo.m_anchorPointWorldSpace;
// Compute the local-space hinge axis // Compute the local-space hinge axis
mHingeLocalAxisBody1 = transform1.getOrientation().getInverse() * jointInfo.rotationAxisWorld; m_hingeLocalAxisBody1 = transform1.getOrientation().getInverse() * jointInfo.rotationAxisWorld;
mHingeLocalAxisBody2 = transform2.getOrientation().getInverse() * jointInfo.rotationAxisWorld; m_hingeLocalAxisBody2 = transform2.getOrientation().getInverse() * jointInfo.rotationAxisWorld;
mHingeLocalAxisBody1.normalize(); m_hingeLocalAxisBody1.normalize();
mHingeLocalAxisBody2.normalize(); m_hingeLocalAxisBody2.normalize();
// Compute the inverse of the initial orientation difference between the two bodies // Compute the inverse of the initial orientation difference between the two bodies
m_initOrientationDifferenceInv = transform2.getOrientation() * m_initOrientationDifferenceInv = transform2.getOrientation() *
@ -75,28 +75,28 @@ void HingeJoint::initBeforeSolve(const ConstraintSolverData& constraintSolverDat
float hingeAngle = computeCurrentHingeAngle(orientationBody1, orientationBody2); float hingeAngle = computeCurrentHingeAngle(orientationBody1, orientationBody2);
// Check if the limit constraints are violated or not // Check if the limit constraints are violated or not
float lowerLimitError = hingeAngle - mLowerLimit; float lowerLimitError = hingeAngle - m_lowerLimit;
float upperLimitError = mUpperLimit - hingeAngle; float upperLimitError = m_upperLimit - hingeAngle;
bool oldIsLowerLimitViolated = mIsLowerLimitViolated; bool oldIsLowerLimitViolated = m_isLowerLimitViolated;
mIsLowerLimitViolated = lowerLimitError <= 0; m_isLowerLimitViolated = lowerLimitError <= 0;
if (mIsLowerLimitViolated != oldIsLowerLimitViolated) { if (m_isLowerLimitViolated != oldIsLowerLimitViolated) {
m_impulseLowerLimit = 0.0; m_impulseLowerLimit = 0.0;
} }
bool oldIsUpperLimitViolated = mIsUpperLimitViolated; bool oldIsUpperLimitViolated = m_isUpperLimitViolated;
mIsUpperLimitViolated = upperLimitError <= 0; m_isUpperLimitViolated = upperLimitError <= 0;
if (mIsUpperLimitViolated != oldIsUpperLimitViolated) { if (m_isUpperLimitViolated != oldIsUpperLimitViolated) {
m_impulseUpperLimit = 0.0; m_impulseUpperLimit = 0.0;
} }
// Compute vectors needed in the Jacobian // Compute vectors needed in the Jacobian
mA1 = orientationBody1 * mHingeLocalAxisBody1; mA1 = orientationBody1 * m_hingeLocalAxisBody1;
vec3 a2 = orientationBody2 * mHingeLocalAxisBody2; vec3 a2 = orientationBody2 * m_hingeLocalAxisBody2;
mA1.normalize(); mA1.normalize();
a2.normalize(); a2.normalize();
const vec3 b2 = a2.getOrthoVector(); const vec3 b2 = a2.getOrthoVector();
const vec3 c2 = a2.cross(b2); const vec3 c2 = a2.cross(b2);
mB2CrossA1 = b2.cross(mA1); m_b2CrossA1 = b2.cross(mA1);
mC2CrossA1 = c2.cross(mA1); m_c2CrossA1 = c2.cross(mA1);
// Compute the corresponding skew-symmetric matrices // Compute the corresponding skew-symmetric matrices
etk::Matrix3x3 skewSymmetricMatrixU1= etk::Matrix3x3::computeSkewSymmetricMatrixForCrossProduct(m_r1World); 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 // Compute the bias "b" of the translation constraints
mBTranslation.setZero(); m_bTranslation.setZero();
float biasFactor = (BETA / constraintSolverData.timeStep); float biasFactor = (BETA / constraintSolverData.timeStep);
if (m_positionCorrectionTechnique == BAUMGARTE_JOINTS) { 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) // Compute the inverse mass matrix K=JM^-1J^t for the 2 rotation constraints (2x2 matrix)
vec3 I1B2CrossA1 = m_i1 * mB2CrossA1; vec3 I1B2CrossA1 = m_i1 * m_b2CrossA1;
vec3 I1C2CrossA1 = m_i1 * mC2CrossA1; vec3 I1C2CrossA1 = m_i1 * m_c2CrossA1;
vec3 I2B2CrossA1 = m_i2 * mB2CrossA1; vec3 I2B2CrossA1 = m_i2 * m_b2CrossA1;
vec3 I2C2CrossA1 = m_i2 * mC2CrossA1; vec3 I2C2CrossA1 = m_i2 * m_c2CrossA1;
const float el11 = mB2CrossA1.dot(I1B2CrossA1) + const float el11 = m_b2CrossA1.dot(I1B2CrossA1) +
mB2CrossA1.dot(I2B2CrossA1); m_b2CrossA1.dot(I2B2CrossA1);
const float el12 = mB2CrossA1.dot(I1C2CrossA1) + const float el12 = m_b2CrossA1.dot(I1C2CrossA1) +
mB2CrossA1.dot(I2C2CrossA1); m_b2CrossA1.dot(I2C2CrossA1);
const float el21 = mC2CrossA1.dot(I1B2CrossA1) + const float el21 = m_c2CrossA1.dot(I1B2CrossA1) +
mC2CrossA1.dot(I2B2CrossA1); m_c2CrossA1.dot(I2B2CrossA1);
const float el22 = mC2CrossA1.dot(I1C2CrossA1) + const float el22 = m_c2CrossA1.dot(I1C2CrossA1) +
mC2CrossA1.dot(I2C2CrossA1); m_c2CrossA1.dot(I2C2CrossA1);
const etk::Matrix2x2 matrixKRotation(el11, el12, el21, el22); const etk::Matrix2x2 matrixKRotation(el11, el12, el21, el22);
m_inverseMassMatrixRotation.setZero(); m_inverseMassMatrixRotation.setZero();
if (m_body1->getType() == DYNAMIC || m_body2->getType() == DYNAMIC) { 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 // Compute the bias "b" of the rotation constraints
mBRotation.setZero(); m_bRotation.setZero();
if (m_positionCorrectionTechnique == BAUMGARTE_JOINTS) { 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 // If warm-starting is not enabled
@ -158,25 +158,25 @@ void HingeJoint::initBeforeSolve(const ConstraintSolverData& constraintSolverDat
} }
// If the motor or limits are enabled // 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) // 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 = mA1.dot(m_i1 * mA1) + mA1.dot(m_i2 * mA1);
m_inverseMassMatrixLimitMotor = (m_inverseMassMatrixLimitMotor > 0.0) ? m_inverseMassMatrixLimitMotor = (m_inverseMassMatrixLimitMotor > 0.0) ?
1.0f / m_inverseMassMatrixLimitMotor : 0.0f; 1.0f / m_inverseMassMatrixLimitMotor : 0.0f;
if (mIsLimitEnabled) { if (m_isLimitEnabled) {
// Compute the bias "b" of the lower limit constraint // Compute the bias "b" of the lower limit constraint
mBLowerLimit = 0.0; m_bLowerLimit = 0.0;
if (m_positionCorrectionTechnique == BAUMGARTE_JOINTS) { if (m_positionCorrectionTechnique == BAUMGARTE_JOINTS) {
mBLowerLimit = biasFactor * lowerLimitError; m_bLowerLimit = biasFactor * lowerLimitError;
} }
// Compute the bias "b" of the upper limit constraint // Compute the bias "b" of the upper limit constraint
mBUpperLimit = 0.0; m_bUpperLimit = 0.0;
if (m_positionCorrectionTechnique == BAUMGARTE_JOINTS) { 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; const float inverseMassBody2 = m_body2->m_massInverse;
// Compute the impulse P=J^T * lambda for the 2 rotation constraints // 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 // Compute the impulse P=J^T * lambda for the lower and upper limits constraints
const vec3 limitsImpulse = (m_impulseUpperLimit - m_impulseLowerLimit) * mA1; const vec3 limitsImpulse = (m_impulseUpperLimit - m_impulseLowerLimit) * mA1;
@ -258,7 +258,7 @@ void HingeJoint::solveVelocityConstraint(const ConstraintSolverData& constraintS
// Compute the Lagrange multiplier lambda // Compute the Lagrange multiplier lambda
const vec3 deltaLambdaTranslation = m_inverseMassMatrixTranslation * const vec3 deltaLambdaTranslation = m_inverseMassMatrixTranslation *
(-JvTranslation - mBTranslation); (-JvTranslation - m_bTranslation);
m_impulseTranslation += deltaLambdaTranslation; m_impulseTranslation += deltaLambdaTranslation;
// Compute the impulse P=J^T * lambda of body 1 // Compute the impulse P=J^T * lambda of body 1
@ -279,39 +279,39 @@ void HingeJoint::solveVelocityConstraint(const ConstraintSolverData& constraintS
// --------------- Rotation Constraints --------------- // // --------------- Rotation Constraints --------------- //
// Compute J*v for the 2 rotation constraints // Compute J*v for the 2 rotation constraints
const vec2 JvRotation(-mB2CrossA1.dot(w1) + mB2CrossA1.dot(w2), const vec2 JvRotation(-m_b2CrossA1.dot(w1) + m_b2CrossA1.dot(w2),
-mC2CrossA1.dot(w1) + mC2CrossA1.dot(w2)); -m_c2CrossA1.dot(w1) + m_c2CrossA1.dot(w2));
// Compute the Lagrange multiplier lambda for the 2 rotation constraints // 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; m_impulseRotation += deltaLambdaRotation;
// Compute the impulse P=J^T * lambda for the 2 rotation constraints of body 1 // Compute the impulse P=J^T * lambda for the 2 rotation constraints of body 1
angularImpulseBody1 = -mB2CrossA1 * deltaLambdaRotation.x() - angularImpulseBody1 = -m_b2CrossA1 * deltaLambdaRotation.x() -
mC2CrossA1 * deltaLambdaRotation.y(); m_c2CrossA1 * deltaLambdaRotation.y();
// Apply the impulse to the body 1 // Apply the impulse to the body 1
w1 += m_i1 * angularImpulseBody1; w1 += m_i1 * angularImpulseBody1;
// Compute the impulse P=J^T * lambda for the 2 rotation constraints of body 2 // Compute the impulse P=J^T * lambda for the 2 rotation constraints of body 2
angularImpulseBody2 = mB2CrossA1 * deltaLambdaRotation.x() + angularImpulseBody2 = m_b2CrossA1 * deltaLambdaRotation.x() +
mC2CrossA1 * deltaLambdaRotation.y(); m_c2CrossA1 * deltaLambdaRotation.y();
// Apply the impulse to the body 2 // Apply the impulse to the body 2
w2 += m_i2 * angularImpulseBody2; w2 += m_i2 * angularImpulseBody2;
// --------------- Limits Constraints --------------- // // --------------- Limits Constraints --------------- //
if (mIsLimitEnabled) { if (m_isLimitEnabled) {
// If the lower limit is violated // If the lower limit is violated
if (mIsLowerLimitViolated) { if (m_isLowerLimitViolated) {
// Compute J*v for the lower limit constraint // Compute J*v for the lower limit constraint
const float JvLowerLimit = (w2 - w1).dot(mA1); const float JvLowerLimit = (w2 - w1).dot(mA1);
// Compute the Lagrange multiplier lambda for the lower limit constraint // 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; float lambdaTemp = m_impulseLowerLimit;
m_impulseLowerLimit = std::max(m_impulseLowerLimit + deltaLambdaLower, 0.0f); m_impulseLowerLimit = std::max(m_impulseLowerLimit + deltaLambdaLower, 0.0f);
deltaLambdaLower = m_impulseLowerLimit - lambdaTemp; deltaLambdaLower = m_impulseLowerLimit - lambdaTemp;
@ -330,13 +330,13 @@ void HingeJoint::solveVelocityConstraint(const ConstraintSolverData& constraintS
} }
// If the upper limit is violated // If the upper limit is violated
if (mIsUpperLimitViolated) { if (m_isUpperLimitViolated) {
// Compute J*v for the upper limit constraint // Compute J*v for the upper limit constraint
const float JvUpperLimit = -(w2 - w1).dot(mA1); const float JvUpperLimit = -(w2 - w1).dot(mA1);
// Compute the Lagrange multiplier lambda for the upper limit constraint // 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; float lambdaTemp = m_impulseUpperLimit;
m_impulseUpperLimit = std::max(m_impulseUpperLimit + deltaLambdaUpper, 0.0f); m_impulseUpperLimit = std::max(m_impulseUpperLimit + deltaLambdaUpper, 0.0f);
deltaLambdaUpper = m_impulseUpperLimit - lambdaTemp; deltaLambdaUpper = m_impulseUpperLimit - lambdaTemp;
@ -358,14 +358,14 @@ void HingeJoint::solveVelocityConstraint(const ConstraintSolverData& constraintS
// --------------- Motor --------------- // // --------------- Motor --------------- //
// If the motor is enabled // If the motor is enabled
if (mIsMotorEnabled) { if (m_isMotorEnabled) {
// Compute J*v for the motor // Compute J*v for the motor
const float JvMotor = mA1.dot(w1 - w2); const float JvMotor = mA1.dot(w1 - w2);
// Compute the Lagrange multiplier lambda for the motor // Compute the Lagrange multiplier lambda for the motor
const float maxMotorImpulse = mMaxMotorTorque * constraintSolverData.timeStep; const float maxMotorImpulse = m_maxMotorTorque * constraintSolverData.timeStep;
float deltaLambdaMotor = m_inverseMassMatrixLimitMotor * (-JvMotor - mMotorSpeed); float deltaLambdaMotor = m_inverseMassMatrixLimitMotor * (-JvMotor - m_motorSpeed);
float lambdaTemp = m_impulseMotor; float lambdaTemp = m_impulseMotor;
m_impulseMotor = clamp(m_impulseMotor + deltaLambdaMotor, -maxMotorImpulse, maxMotorImpulse); m_impulseMotor = clamp(m_impulseMotor + deltaLambdaMotor, -maxMotorImpulse, maxMotorImpulse);
deltaLambdaMotor = m_impulseMotor - lambdaTemp; deltaLambdaMotor = m_impulseMotor - lambdaTemp;
@ -413,20 +413,20 @@ void HingeJoint::solvePositionConstraint(const ConstraintSolverData& constraintS
float hingeAngle = computeCurrentHingeAngle(q1, q2); float hingeAngle = computeCurrentHingeAngle(q1, q2);
// Check if the limit constraints are violated or not // Check if the limit constraints are violated or not
float lowerLimitError = hingeAngle - mLowerLimit; float lowerLimitError = hingeAngle - m_lowerLimit;
float upperLimitError = mUpperLimit - hingeAngle; float upperLimitError = m_upperLimit - hingeAngle;
mIsLowerLimitViolated = lowerLimitError <= 0; m_isLowerLimitViolated = lowerLimitError <= 0;
mIsUpperLimitViolated = upperLimitError <= 0; m_isUpperLimitViolated = upperLimitError <= 0;
// Compute vectors needed in the Jacobian // Compute vectors needed in the Jacobian
mA1 = q1 * mHingeLocalAxisBody1; mA1 = q1 * m_hingeLocalAxisBody1;
vec3 a2 = q2 * mHingeLocalAxisBody2; vec3 a2 = q2 * m_hingeLocalAxisBody2;
mA1.normalize(); mA1.normalize();
a2.normalize(); a2.normalize();
const vec3 b2 = a2.getOrthoVector(); const vec3 b2 = a2.getOrthoVector();
const vec3 c2 = a2.cross(b2); const vec3 c2 = a2.cross(b2);
mB2CrossA1 = b2.cross(mA1); m_b2CrossA1 = b2.cross(mA1);
mC2CrossA1 = c2.cross(mA1); m_c2CrossA1 = c2.cross(mA1);
// Compute the corresponding skew-symmetric matrices // Compute the corresponding skew-symmetric matrices
etk::Matrix3x3 skewSymmetricMatrixU1= etk::Matrix3x3::computeSkewSymmetricMatrixForCrossProduct(m_r1World); etk::Matrix3x3 skewSymmetricMatrixU1= etk::Matrix3x3::computeSkewSymmetricMatrixForCrossProduct(m_r1World);
@ -480,18 +480,18 @@ void HingeJoint::solvePositionConstraint(const ConstraintSolverData& constraintS
// --------------- Rotation Constraints --------------- // // --------------- Rotation Constraints --------------- //
// Compute the inverse mass matrix K=JM^-1J^t for the 2 rotation constraints (2x2 matrix) // Compute the inverse mass matrix K=JM^-1J^t for the 2 rotation constraints (2x2 matrix)
vec3 I1B2CrossA1 = m_i1 * mB2CrossA1; vec3 I1B2CrossA1 = m_i1 * m_b2CrossA1;
vec3 I1C2CrossA1 = m_i1 * mC2CrossA1; vec3 I1C2CrossA1 = m_i1 * m_c2CrossA1;
vec3 I2B2CrossA1 = m_i2 * mB2CrossA1; vec3 I2B2CrossA1 = m_i2 * m_b2CrossA1;
vec3 I2C2CrossA1 = m_i2 * mC2CrossA1; vec3 I2C2CrossA1 = m_i2 * m_c2CrossA1;
const float el11 = mB2CrossA1.dot(I1B2CrossA1) + const float el11 = m_b2CrossA1.dot(I1B2CrossA1) +
mB2CrossA1.dot(I2B2CrossA1); m_b2CrossA1.dot(I2B2CrossA1);
const float el12 = mB2CrossA1.dot(I1C2CrossA1) + const float el12 = m_b2CrossA1.dot(I1C2CrossA1) +
mB2CrossA1.dot(I2C2CrossA1); m_b2CrossA1.dot(I2C2CrossA1);
const float el21 = mC2CrossA1.dot(I1B2CrossA1) + const float el21 = m_c2CrossA1.dot(I1B2CrossA1) +
mC2CrossA1.dot(I2B2CrossA1); m_c2CrossA1.dot(I2B2CrossA1);
const float el22 = mC2CrossA1.dot(I1C2CrossA1) + const float el22 = m_c2CrossA1.dot(I1C2CrossA1) +
mC2CrossA1.dot(I2C2CrossA1); m_c2CrossA1.dot(I2C2CrossA1);
const etk::Matrix2x2 matrixKRotation(el11, el12, el21, el22); const etk::Matrix2x2 matrixKRotation(el11, el12, el21, el22);
m_inverseMassMatrixRotation.setZero(); m_inverseMassMatrixRotation.setZero();
if (m_body1->getType() == DYNAMIC || m_body2->getType() == DYNAMIC) { if (m_body1->getType() == DYNAMIC || m_body2->getType() == DYNAMIC) {
@ -505,7 +505,7 @@ void HingeJoint::solvePositionConstraint(const ConstraintSolverData& constraintS
vec2 lambdaRotation = m_inverseMassMatrixRotation * (-errorRotation); vec2 lambdaRotation = m_inverseMassMatrixRotation * (-errorRotation);
// Compute the impulse P=J^T * lambda for the 3 rotation constraints of body 1 // 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 // Compute the pseudo velocity of body 1
w1 = m_i1 * angularImpulseBody1; w1 = m_i1 * angularImpulseBody1;
@ -515,7 +515,7 @@ void HingeJoint::solvePositionConstraint(const ConstraintSolverData& constraintS
q1.normalize(); q1.normalize();
// Compute the impulse of body 2 // 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 // Compute the pseudo velocity of body 2
w2 = m_i2 * angularImpulseBody2; w2 = m_i2 * angularImpulseBody2;
@ -526,9 +526,9 @@ void HingeJoint::solvePositionConstraint(const ConstraintSolverData& constraintS
// --------------- Limits 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) // 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); 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 the lower limit is violated
if (mIsLowerLimitViolated) { if (m_isLowerLimitViolated) {
// Compute the Lagrange multiplier lambda for the lower limit constraint // Compute the Lagrange multiplier lambda for the lower limit constraint
float lambdaLowerLimit = m_inverseMassMatrixLimitMotor * (-lowerLimitError ); float lambdaLowerLimit = m_inverseMassMatrixLimitMotor * (-lowerLimitError );
@ -564,7 +564,7 @@ void HingeJoint::solvePositionConstraint(const ConstraintSolverData& constraintS
} }
// If the upper limit is violated // If the upper limit is violated
if (mIsUpperLimitViolated) { if (m_isUpperLimitViolated) {
// Compute the Lagrange multiplier lambda for the upper limit constraint // Compute the Lagrange multiplier lambda for the upper limit constraint
float lambdaUpperLimit = m_inverseMassMatrixLimitMotor * (-upperLimitError); float lambdaUpperLimit = m_inverseMassMatrixLimitMotor * (-upperLimitError);
@ -600,9 +600,9 @@ void HingeJoint::solvePositionConstraint(const ConstraintSolverData& constraintS
*/ */
void HingeJoint::enableLimit(bool isLimitEnabled) { void HingeJoint::enableLimit(bool isLimitEnabled) {
if (isLimitEnabled != mIsLimitEnabled) { if (isLimitEnabled != m_isLimitEnabled) {
mIsLimitEnabled = isLimitEnabled; m_isLimitEnabled = isLimitEnabled;
// Reset the limits // Reset the limits
resetLimits(); resetLimits();
@ -616,7 +616,7 @@ void HingeJoint::enableLimit(bool isLimitEnabled) {
*/ */
void HingeJoint::enableMotor(bool isMotorEnabled) { void HingeJoint::enableMotor(bool isMotorEnabled) {
mIsMotorEnabled = isMotorEnabled; m_isMotorEnabled = isMotorEnabled;
m_impulseMotor = 0.0; m_impulseMotor = 0.0;
// Wake up the two bodies of the joint // Wake up the two bodies of the joint
@ -630,11 +630,11 @@ void HingeJoint::enableMotor(bool isMotorEnabled) {
*/ */
void HingeJoint::setMinAngleLimit(float lowerLimit) { 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 // Reset the limits
resetLimits(); resetLimits();
@ -649,9 +649,9 @@ void HingeJoint::setMaxAngleLimit(float upperLimit) {
assert(upperLimit >= 0 && upperLimit <= 2.0 * PI); assert(upperLimit >= 0 && upperLimit <= 2.0 * PI);
if (upperLimit != mUpperLimit) { if (upperLimit != m_upperLimit) {
mUpperLimit = upperLimit; m_upperLimit = upperLimit;
// Reset the limits // Reset the limits
resetLimits(); resetLimits();
@ -673,9 +673,9 @@ void HingeJoint::resetLimits() {
// Set the motor speed // Set the motor speed
void HingeJoint::setMotorSpeed(float motorSpeed) { 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 // Wake up the two bodies of the joint
m_body1->setIsSleeping(false); m_body1->setIsSleeping(false);
@ -689,10 +689,10 @@ void HingeJoint::setMotorSpeed(float motorSpeed) {
*/ */
void HingeJoint::setMaxMotorTorque(float maxMotorTorque) { void HingeJoint::setMaxMotorTorque(float maxMotorTorque) {
if (maxMotorTorque != mMaxMotorTorque) { if (maxMotorTorque != m_maxMotorTorque) {
assert(mMaxMotorTorque >= 0.0); assert(m_maxMotorTorque >= 0.0);
mMaxMotorTorque = maxMotorTorque; m_maxMotorTorque = maxMotorTorque;
// Wake up the two bodies of the joint // Wake up the two bodies of the joint
m_body1->setIsSleeping(false); m_body1->setIsSleeping(false);
@ -780,6 +780,70 @@ float HingeJoint::computeCurrentHingeAngle(const etk::Quaternion& orientationBod
hingeAngle = computeNormalizedAngle(hingeAngle); hingeAngle = computeNormalizedAngle(hingeAngle);
// Compute and return the corresponding angle near one the two limits // 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);
}

564
ephysics/constraint/HingeJoint.hpp
View File

@ -5,379 +5,205 @@
*/ */
#pragma once #pragma once
// Libraries
#include <ephysics/constraint/Joint.hpp> #include <ephysics/constraint/Joint.hpp>
#include <ephysics/mathematics/mathematics.hpp> #include <ephysics/mathematics/mathematics.hpp>
namespace ephysics { namespace ephysics {
/**
// Structure HingeJointInfo * @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.
* 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 :
struct HingeJointInfo : public JointInfo { vec3 m_anchorPointWorldSpace; //!< Anchor point (in world-space coordinates)
vec3 rotationAxisWorld; //!< Hinge rotation axis (in world-space coordinates)
public : bool isLimitEnabled; //!< True if the hinge joint limits are enabled
bool isMotorEnabled; //!< True if the hinge joint motor is enabled
// -------------------- Attributes -------------------- // 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]
/// Anchor point (in world-space coordinates) float motorSpeed; //!< Motor speed (in radian/second)
vec3 m_anchorPointWorldSpace; float maxMotorTorque; //!< Maximum motor torque (in Newtons * meters) that can be applied to reach to desired motor speed
/**
/// Hinge rotation axis (in world-space coordinates) * @brief Constructor without limits and without motor
vec3 rotationAxisWorld; * @param[in] _rigidBody1 The first body of the joint
* @param[in] _rigidBody2 The second body of the joint
/// True if the hinge joint limits are enabled * @param[in] _initAnchorPointWorldSpace The initial anchor point in world-space coordinates
bool isLimitEnabled; * @param[in] _initRotationAxisWorld The initial rotation axis in world-space coordinates
*/
/// True if the hinge joint motor is enabled HingeJointInfo(RigidBody* _rigidBody1,
bool isMotorEnabled; RigidBody* _rigidBody2,
const vec3& _initAnchorPointWorldSpace,
/// Minimum allowed rotation angle (in radian) if limits are enabled. const vec3& _initRotationAxisWorld):
/// The angle must be in the range [-2*pi, 0] JointInfo(_rigidBody1, _rigidBody2, HINGEJOINT),
float minAngleLimit; m_anchorPointWorldSpace(_initAnchorPointWorldSpace),
rotationAxisWorld(_initRotationAxisWorld),
/// Maximum allowed rotation angle (in radian) if limits are enabled. isLimitEnabled(false),
/// The angle must be in the range [0, 2*pi] isMotorEnabled(false),
float maxAngleLimit; minAngleLimit(-1),
maxAngleLimit(1),
/// Motor speed (in radian/second) motorSpeed(0),
float motorSpeed; maxMotorTorque(0) {
/// Maximum motor torque (in Newtons * meters) that can be applied to reach }
/// to desired motor speed /**
float maxMotorTorque; * @brief Constructor with limits but without motor
* @param[in] _rigidBody1 The first body of the joint
/// Constructor without limits and without motor * @param[in] _rigidBody2 The second body of the joint
/** * @param[in] _initAnchorPointWorldSpace The initial anchor point in world-space coordinates
* @param rigidBody1 The first body of the joint * @param[in] _initRotationAxisWorld The int32_tial rotation axis in world-space coordinates
* @param rigidBody2 The second body of the joint * @param[in] _initMinAngleLimit The initial minimum limit angle (in radian)
* @param initAnchorPointWorldSpace The initial anchor point in world-space * @param[in] _initMaxAngleLimit The initial maximum limit angle (in radian)
* coordinates */
* @param initRotationAxisWorld The initial rotation axis in world-space HingeJointInfo(RigidBody* _rigidBody1,
* coordinates RigidBody* _rigidBody2,
*/ const vec3& _initAnchorPointWorldSpace,
HingeJointInfo(RigidBody* rigidBody1, RigidBody* rigidBody2, const vec3& _initRotationAxisWorld,
const vec3& initAnchorPointWorldSpace, float _initMinAngleLimit,
const vec3& initRotationAxisWorld) float _initMaxAngleLimit):
: JointInfo(rigidBody1, rigidBody2, HINGEJOINT), JointInfo(_rigidBody1, _rigidBody2, HINGEJOINT),
m_anchorPointWorldSpace(initAnchorPointWorldSpace), m_anchorPointWorldSpace(_initAnchorPointWorldSpace),
rotationAxisWorld(initRotationAxisWorld), isLimitEnabled(false), rotationAxisWorld(_initRotationAxisWorld),
isMotorEnabled(false), minAngleLimit(-1), maxAngleLimit(1), isLimitEnabled(true),
motorSpeed(0), maxMotorTorque(0) {} isMotorEnabled(false),
minAngleLimit(_initMinAngleLimit),
/// Constructor with limits but without motor maxAngleLimit(_initMaxAngleLimit),
/** motorSpeed(0),
* @param rigidBody1 The first body of the joint maxMotorTorque(0) {
* @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) * @brief Constructor with limits and motor
* @param initMaxAngleLimit The initial maximum limit angle (in radian) * @param[in] _rigidBody1 The first body of the joint
*/ * @param[in] _rigidBody2 The second body of the joint
HingeJointInfo(RigidBody* rigidBody1, RigidBody* rigidBody2, * @param[in] _initAnchorPointWorldSpace The initial anchor point in world-space
const vec3& initAnchorPointWorldSpace, * @param[in] _initRotationAxisWorld The initial rotation axis in world-space
const vec3& initRotationAxisWorld, * @param[in] _initMinAngleLimit The initial minimum limit angle (in radian)
float initMinAngleLimit, float initMaxAngleLimit) * @param[in] _initMaxAngleLimit The initial maximum limit angle (in radian)
: JointInfo(rigidBody1, rigidBody2, HINGEJOINT), * @param[in] _initMotorSpeed The initial motor speed of the joint (in radian per second)
m_anchorPointWorldSpace(initAnchorPointWorldSpace), * @param[in] _initMaxMotorTorque The initial maximum motor torque (in Newtons)
rotationAxisWorld(initRotationAxisWorld), isLimitEnabled(true), */
isMotorEnabled(false), minAngleLimit(initMinAngleLimit), HingeJointInfo(RigidBody* _rigidBody1,
maxAngleLimit(initMaxAngleLimit), motorSpeed(0), RigidBody* _rigidBody2,
maxMotorTorque(0) {} const vec3& _initAnchorPointWorldSpace,
const vec3& _initRotationAxisWorld,
/// Constructor with limits and motor float _initMinAngleLimit,
/** float _initMaxAngleLimit,
* @param rigidBody1 The first body of the joint float _initMotorSpeed,
* @param rigidBody2 The second body of the joint float _initMaxMotorTorque):
* @param initAnchorPointWorldSpace The initial anchor point in world-space JointInfo(_rigidBody1, _rigidBody2, HINGEJOINT),
* @param initRotationAxisWorld The initial rotation axis in world-space m_anchorPointWorldSpace(_initAnchorPointWorldSpace),
* @param initMinAngleLimit The initial minimum limit angle (in radian) rotationAxisWorld(_initRotationAxisWorld),
* @param initMaxAngleLimit The initial maximum limit angle (in radian) isLimitEnabled(true),
* @param initMotorSpeed The initial motor speed of the joint (in radian per second) isMotorEnabled(false),
* @param initMaxMotorTorque The initial maximum motor torque (in Newtons) minAngleLimit(_initMinAngleLimit),
*/ maxAngleLimit(_initMaxAngleLimit),
HingeJointInfo(RigidBody* rigidBody1, RigidBody* rigidBody2, motorSpeed(_initMotorSpeed),
const vec3& initAnchorPointWorldSpace, maxMotorTorque(_initMaxMotorTorque) {
const vec3& initRotationAxisWorld,
float initMinAngleLimit, float initMaxAngleLimit, }
float initMotorSpeed, float initMaxMotorTorque) };
: JointInfo(rigidBody1, rigidBody2, HINGEJOINT),
m_anchorPointWorldSpace(initAnchorPointWorldSpace), /**
rotationAxisWorld(initRotationAxisWorld), isLimitEnabled(true), * @brief It represents a hinge joint that allows arbitrary rotation
isMotorEnabled(false), minAngleLimit(initMinAngleLimit), * between two bodies around a single axis. This joint has one degree of freedom. It
maxAngleLimit(initMaxAngleLimit), motorSpeed(initMotorSpeed), * can be useful to simulate doors or pendulumns.
maxMotorTorque(initMaxMotorTorque) {} */
}; class HingeJoint : public Joint {
private :
// Class HingeJoint 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)
* This class represents a hinge joint that allows arbitrary rotation vec3 m_localAnchorPointBody2; //!< Anchor point of body 2 (in local-space coordinates of body 2)
* between two bodies around a single axis. This joint has one degree of freedom. It vec3 m_hingeLocalAxisBody1; //!< Hinge rotation axis (in local-space coordinates of body 1)
* can be useful to simulate doors or pendulumns. 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)
class HingeJoint : public Joint { 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
private : 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
// -------------------- Constants -------------------- // vec3 m_b2CrossA1; //!< Cross product of vector b2 and a1
vec3 m_c2CrossA1; //!< Cross product of vector c2 and a1;
// Beta value for the bias factor of position correction vec3 m_impulseTranslation; //!< Impulse for the 3 translation constraints
static const float BETA; vec2 m_impulseRotation; //!< Impulse for the 2 rotation constraints
float m_impulseLowerLimit; //!< Accumulated impulse for the lower limit constraint
// -------------------- Attributes -------------------- // float m_impulseUpperLimit; //!< Accumulated impulse for the upper limit constraint
float m_impulseMotor; //!< Accumulated impulse for the motor constraint;
/// Anchor point of body 1 (in local-space coordinates of body 1) etk::Matrix3x3 m_inverseMassMatrixTranslation; //!< Inverse mass matrix K=JM^-1J^t for the 3 translation constraints
vec3 m_localAnchorPointBody1; 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)
/// Anchor point of body 2 (in local-space coordinates of body 2) float m_inverseMassMatrixMotor; //!< Inverse of mass matrix K=JM^-1J^t for the motor
vec3 m_localAnchorPointBody2; 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
/// Hinge rotation axis (in local-space coordinates of body 1) float m_bLowerLimit; //!< Bias of the lower limit constraint
vec3 mHingeLocalAxisBody1; float m_bUpperLimit; //!< Bias of the upper limit constraint
etk::Quaternion m_initOrientationDifferenceInv; //!< Inverse of the initial orientation difference between the bodies
/// Hinge rotation axis (in local-space coordiantes of body 2) bool m_isLimitEnabled; //!< True if the joint limits are enabled
vec3 mHingeLocalAxisBody2; bool m_isMotorEnabled; //!< True if the motor of the joint in enabled
float m_lowerLimit; //!< Lower limit (minimum allowed rotation angle in radian)
/// Inertia tensor of body 1 (in world-space coordinates) float m_upperLimit; //!< Upper limit (maximum translation distance)
etk::Matrix3x3 m_i1; bool m_isLowerLimitViolated; //!< True if the lower limit is violated
bool m_isUpperLimitViolated; //!< True if the upper limit is violated
/// Inertia tensor of body 2 (in world-space coordinates) float m_motorSpeed; //!< Motor speed (in rad/s)
etk::Matrix3x3 m_i2; float m_maxMotorTorque; //!< Maximum motor torque (in Newtons) that can be applied to reach to desired motor speed
/// Private copy-constructor
/// Hinge rotation axis (in world-space coordinates) computed from body 1 HingeJoint(const HingeJoint& _constraint);
vec3 mA1; /// Private assignment operator
HingeJoint& operator=(const HingeJoint& _constraint);
/// Vector from center of body 2 to anchor point in world-space /// Reset the limits
vec3 m_r1World; void resetLimits();
/// Given an angle in radian, this method returns the corresponding
/// Vector from center of body 2 to anchor point in world-space /// angle in the range [-pi; pi]
vec3 m_r2World; float computeNormalizedAngle(float _angle) const;
/// Given an "inputAngle" in the range [-pi, pi], this method returns an
/// Cross product of vector b2 and a1 /// angle (modulo 2*pi) in the range [-2*pi; 2*pi] that is closest to one of the
vec3 mB2CrossA1; /// two angle limits in arguments.
float computeCorrespondingAngleNearLimits(float _inputAngle,
/// Cross product of vector c2 and a1; float _lowerLimitAngle,
vec3 mC2CrossA1; float _upperLimitAngle) const;
/// Compute the current angle around the hinge axis
/// Impulse for the 3 translation constraints float computeCurrentHingeAngle(const etk::Quaternion& _orientationBody1,
vec3 m_impulseTranslation; const etk::Quaternion& _orientationBody2);
/// Return the number of bytes used by the joint
/// Impulse for the 2 rotation constraints virtual size_t getSizeInBytes() const;
vec2 m_impulseRotation; /// Initialize before solving the constraint
virtual void initBeforeSolve(const ConstraintSolverData& _constraintSolverData);
/// Accumulated impulse for the lower limit constraint /// Warm start the constraint (apply the previous impulse at the beginning of the step)
float m_impulseLowerLimit; virtual void warmstart(const ConstraintSolverData& _constraintSolverData);
/// Solve the velocity constraint
/// Accumulated impulse for the upper limit constraint virtual void solveVelocityConstraint(const ConstraintSolverData& _constraintSolverData);
float m_impulseUpperLimit; /// Solve the position constraint (for position error correction)
virtual void solvePositionConstraint(const ConstraintSolverData& _constraintSolverData);
/// Accumulated impulse for the motor constraint; public :
float m_impulseMotor; /// Constructor
HingeJoint(const HingeJointInfo& _jointInfo);
/// Inverse mass matrix K=JM^-1J^t for the 3 translation constraints /// Destructor
etk::Matrix3x3 m_inverseMassMatrixTranslation; virtual ~HingeJoint();
/// Return true if the limits or the joint are enabled
/// Inverse mass matrix K=JM^-1J^t for the 2 rotation constraints bool isLimitEnabled() const;
etk::Matrix2x2 m_inverseMassMatrixRotation; /// Return true if the motor of the joint is enabled
bool isMotorEnabled() const;
/// Inverse of mass matrix K=JM^-1J^t for the limits and motor constraints (1x1 matrix) /// Enable/Disable the limits of the joint
float m_inverseMassMatrixLimitMotor; void enableLimit(bool _isLimitEnabled);
/// Enable/Disable the motor of the joint
/// Inverse of mass matrix K=JM^-1J^t for the motor void enableMotor(bool _isMotorEnabled);
float m_inverseMassMatrixMotor; /// Return the minimum angle limit
float getMinAngleLimit() const;
/// Bias vector for the error correction for the translation constraints /// Set the minimum angle limit
vec3 mBTranslation; void setMinAngleLimit(float _lowerLimit);
/// Return the maximum angle limit
/// Bias vector for the error correction for the rotation constraints float getMaxAngleLimit() const;
vec2 mBRotation; /// Set the maximum angle limit
void setMaxAngleLimit(float _upperLimit);
/// Bias of the lower limit constraint /// Return the motor speed
float mBLowerLimit; float getMotorSpeed() const;
/// Set the motor speed
/// Bias of the upper limit constraint void setMotorSpeed(float _motorSpeed);
float mBUpperLimit; /// Return the maximum motor torque
float getMaxMotorTorque() const;
/// Inverse of the initial orientation difference between the bodies /// Set the maximum motor torque
etk::Quaternion m_initOrientationDifferenceInv; void setMaxMotorTorque(float _maxMotorTorque);
/// Return the int32_tensity of the current torque applied for the joint motor
/// True if the joint limits are enabled float getMotorTorque(float _timeStep) const;
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);
}
} }

47
ephysics/constraint/Joint.cpp
View File

@ -24,3 +24,50 @@ Joint::Joint(const JointInfo& jointInfo)
Joint::~Joint() { 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;
}

321
ephysics/constraint/Joint.hpp
View File

@ -5,226 +5,117 @@
*/ */
#pragma once #pragma once
// Libraries
#include <ephysics/configuration.hpp> #include <ephysics/configuration.hpp>
#include <ephysics/body/RigidBody.hpp> #include <ephysics/body/RigidBody.hpp>
#include <ephysics/mathematics/mathematics.hpp> #include <ephysics/mathematics/mathematics.hpp>
// ReactPhysics3D namespace
namespace ephysics { namespace ephysics {
/// Enumeration for the type of a constraint /// Enumeration for the type of a constraint
enum JointType {BALLSOCKETJOINT, SLIDERJOINT, HINGEJOINT, FIXEDJOINT}; enum JointType {BALLSOCKETJOINT, SLIDERJOINT, HINGEJOINT, FIXEDJOINT};
// Class declarations struct ConstraintSolverData;
struct ConstraintSolverData; class Joint;
class Joint;
/**
// Structure JointListElement * @brief It represents a single element of a linked list of joints
/** */
* This structure represents a single element of a linked list of joints struct JointListElement {
*/ public:
struct JointListElement { Joint* joint; //!< Pointer to the actual joint
JointListElement* next; //!< Next element of the list
public: /**
* @breif Constructor
// -------------------- Attributes -------------------- // */
JointListElement(Joint* _initJoint,
/// Pointer to the actual joint JointListElement* _initNext):
Joint* joint; joint(_initJoint),
next(_initNext) {
/// Next element of the list
JointListElement* next; }
};
// -------------------- Methods -------------------- //
/**
/// Constructor * @brief It is used to gather the information needed to create a joint.
JointListElement(Joint* initJoint, JointListElement* initNext) */
:joint(initJoint), next(initNext){ 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
// Structure JointInfo 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
* This structure is used to gather the information needed to create a joint. /// Constructor
*/ JointInfo(JointType _constraintType):
struct JointInfo { body1(nullptr),
body2(nullptr),
public : type(_constraintType),
positionCorrectionTechnique(NON_LINEAR_GAUSS_SEIDEL),
// -------------------- Attributes -------------------- // isCollisionEnabled(true) {
/// First rigid body of the joint }
RigidBody* body1; /// Constructor
JointInfo(RigidBody* _rigidBody1,
/// Second rigid body of the joint RigidBody* _rigidBody2,
RigidBody* body2; JointType _constraintType):
body1(_rigidBody1),
/// Type of the joint body2(_rigidBody2),
JointType type; type(_constraintType),
positionCorrectionTechnique(NON_LINEAR_GAUSS_SEIDEL),
/// Position correction technique used for the constraint (used for joints). isCollisionEnabled(true) {
/// By default, the BAUMGARTE technique is used
JointsPositionCorrectionTechnique positionCorrectionTechnique; }
/// Destructor
/// True if the two bodies of the joint are allowed to collide with each other virtual ~JointInfo() = default;
bool isCollisionEnabled; };
/// Constructor /**
JointInfo(JointType constraintType) * @brief It represents a joint between two bodies.
: body1(NULL), body2(NULL), type(constraintType), */
positionCorrectionTechnique(NON_LINEAR_GAUSS_SEIDEL), class Joint {
isCollisionEnabled(true) {} protected :
RigidBody* const m_body1; //!< Pointer to the first body of the joint
/// Constructor RigidBody* const m_body2; //!< Pointer to the second body of the joint
JointInfo(RigidBody* rigidBody1, RigidBody* rigidBody2, JointType constraintType) const JointType m_type; //!< Type of the joint
: body1(rigidBody1), body2(rigidBody2), type(constraintType), uint32_t m_indexBody1; //!< Body 1 index in the velocity array to solve the constraint
positionCorrectionTechnique(NON_LINEAR_GAUSS_SEIDEL), uint32_t m_indexBody2; //!< Body 2 index in the velocity array to solve the constraint
isCollisionEnabled(true) { 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
/// Destructor /// Private copy-constructor
virtual ~JointInfo() {} 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
// Class Joint bool isAlreadyInIsland() const;
/** /// Return the number of bytes used by the joint
* This abstract class represents a joint between two bodies. virtual size_t getSizeInBytes() const = 0;
*/ /// Initialize before solving the joint
class Joint { virtual void initBeforeSolve(const ConstraintSolverData& _constraintSolverData) = 0;
/// Warm start the joint (apply the previous impulse at the beginning of the step)
protected : virtual void warmstart(const ConstraintSolverData& _constraintSolverData) = 0;
/// Solve the velocity constraint
// -------------------- Attributes -------------------- // virtual void solveVelocityConstraint(const ConstraintSolverData& _constraintSolverData) = 0;
/// Solve the position constraint
/// Pointer to the first body of the joint virtual void solvePositionConstraint(const ConstraintSolverData& _constraintSolverData) = 0;
RigidBody* const m_body1; public :
/// Constructor
/// Pointer to the second body of the joint Joint(const JointInfo& _jointInfo);
RigidBody* const m_body2; /// Destructor
virtual ~Joint();
/// Type of the joint /// Return the reference to the body 1
const JointType m_type; RigidBody* getBody1() const;
/// Return the reference to the body 2
/// Body 1 index in the velocity array to solve the constraint RigidBody* getBody2() const;
uint32_t m_indexBody1; /// Return true if the constraint is active
bool isActive() const;
/// Body 2 index in the velocity array to solve the constraint /// Return the type of the constraint
uint32_t m_indexBody2; JointType getType() const;
/// Return true if the collision between the two bodies of the joint is enabled
/// Position correction technique used for the constraint (used for joints) bool isCollisionEnabled() const;
JointsPositionCorrectionTechnique m_positionCorrectionTechnique; friend class DynamicsWorld;
friend class Island;
/// True if the two bodies of the constraint are allowed to collide with each other friend class ConstraintSolver;
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;
}
} }

403
ephysics/constraint/SliderJoint.cpp
View File

@ -16,15 +16,15 @@ const float SliderJoint::BETA = float(0.2);
SliderJoint::SliderJoint(const SliderJointInfo& jointInfo) SliderJoint::SliderJoint(const SliderJointInfo& jointInfo)
: Joint(jointInfo), m_impulseTranslation(0, 0), m_impulseRotation(0, 0, 0), : Joint(jointInfo), m_impulseTranslation(0, 0), m_impulseRotation(0, 0, 0),
m_impulseLowerLimit(0), m_impulseUpperLimit(0), m_impulseMotor(0), m_impulseLowerLimit(0), m_impulseUpperLimit(0), m_impulseMotor(0),
mIsLimitEnabled(jointInfo.isLimitEnabled), mIsMotorEnabled(jointInfo.isMotorEnabled), m_isLimitEnabled(jointInfo.isLimitEnabled), m_isMotorEnabled(jointInfo.isMotorEnabled),
mLowerLimit(jointInfo.minTranslationLimit), m_lowerLimit(jointInfo.minTranslationLimit),
mUpperLimit(jointInfo.maxTranslationLimit), mIsLowerLimitViolated(false), m_upperLimit(jointInfo.maxTranslationLimit), m_isLowerLimitViolated(false),
mIsUpperLimitViolated(false), mMotorSpeed(jointInfo.motorSpeed), m_isUpperLimitViolated(false), m_motorSpeed(jointInfo.motorSpeed),
mMaxMotorForce(jointInfo.maxMotorForce){ m_maxMotorForce(jointInfo.maxMotorForce){
assert(mUpperLimit >= 0.0); assert(m_upperLimit >= 0.0);
assert(mLowerLimit <= 0.0); assert(m_lowerLimit <= 0.0);
assert(mMaxMotorForce >= 0.0); assert(m_maxMotorForce >= 0.0);
// Compute the local-space anchor point for each body // Compute the local-space anchor point for each body
const etk::Transform3D& transform1 = m_body1->getTransform(); const etk::Transform3D& transform1 = m_body1->getTransform();
@ -39,9 +39,9 @@ SliderJoint::SliderJoint(const SliderJointInfo& jointInfo)
m_initOrientationDifferenceInv.inverse(); m_initOrientationDifferenceInv.inverse();
// Compute the slider axis in local-space of body 1 // 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; jointInfo.sliderAxisWorldSpace;
mSliderAxisBody1.normalize(); m_sliderAxisBody1.normalize();
} }
// Destructor // Destructor
@ -67,57 +67,57 @@ void SliderJoint::initBeforeSolve(const ConstraintSolverData& constraintSolverDa
m_i2 = m_body2->getInertiaTensorInverseWorld(); m_i2 = m_body2->getInertiaTensorInverseWorld();
// Vector from body center to the anchor point // Vector from body center to the anchor point
mR1 = orientationBody1 * m_localAnchorPointBody1; m_R1 = orientationBody1 * m_localAnchorPointBody1;
mR2 = orientationBody2 * m_localAnchorPointBody2; m_R2 = orientationBody2 * m_localAnchorPointBody2;
// Compute the vector u (difference between anchor points) // 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 // Compute the two orthogonal vectors to the slider axis in world-space
mSliderAxisWorld = orientationBody1 * mSliderAxisBody1; m_sliderAxisWorld = orientationBody1 * m_sliderAxisBody1;
mSliderAxisWorld.normalize(); m_sliderAxisWorld.normalize();
mN1 = mSliderAxisWorld.getOrthoVector(); m_N1 = m_sliderAxisWorld.getOrthoVector();
mN2 = mSliderAxisWorld.cross(mN1); m_N2 = m_sliderAxisWorld.cross(m_N1);
// Check if the limit constraints are violated or not // Check if the limit constraints are violated or not
float uDotSliderAxis = u.dot(mSliderAxisWorld); float uDotSliderAxis = u.dot(m_sliderAxisWorld);
float lowerLimitError = uDotSliderAxis - mLowerLimit; float lowerLimitError = uDotSliderAxis - m_lowerLimit;
float upperLimitError = mUpperLimit - uDotSliderAxis; float upperLimitError = m_upperLimit - uDotSliderAxis;
bool oldIsLowerLimitViolated = mIsLowerLimitViolated; bool oldIsLowerLimitViolated = m_isLowerLimitViolated;
mIsLowerLimitViolated = lowerLimitError <= 0; m_isLowerLimitViolated = lowerLimitError <= 0;
if (mIsLowerLimitViolated != oldIsLowerLimitViolated) { if (m_isLowerLimitViolated != oldIsLowerLimitViolated) {
m_impulseLowerLimit = 0.0; m_impulseLowerLimit = 0.0;
} }
bool oldIsUpperLimitViolated = mIsUpperLimitViolated; bool oldIsUpperLimitViolated = m_isUpperLimitViolated;
mIsUpperLimitViolated = upperLimitError <= 0; m_isUpperLimitViolated = upperLimitError <= 0;
if (mIsUpperLimitViolated != oldIsUpperLimitViolated) { if (m_isUpperLimitViolated != oldIsUpperLimitViolated) {
m_impulseUpperLimit = 0.0; m_impulseUpperLimit = 0.0;
} }
// Compute the cross products used in the Jacobians // Compute the cross products used in the Jacobians
mR2CrossN1 = mR2.cross(mN1); m_R2CrossN1 = m_R2.cross(m_N1);
mR2CrossN2 = mR2.cross(mN2); m_R2CrossN2 = m_R2.cross(m_N2);
mR2CrossSliderAxis = mR2.cross(mSliderAxisWorld); m_R2CrossSliderAxis = m_R2.cross(m_sliderAxisWorld);
const vec3 r1PlusU = mR1 + u; const vec3 r1PlusU = m_R1 + u;
mR1PlusUCrossN1 = (r1PlusU).cross(mN1); m_R1PlusUCrossN1 = (r1PlusU).cross(m_N1);
mR1PlusUCrossN2 = (r1PlusU).cross(mN2); m_R1PlusUCrossN2 = (r1PlusU).cross(m_N2);
mR1PlusUCrossSliderAxis = (r1PlusU).cross(mSliderAxisWorld); m_R1PlusUCrossSliderAxis = (r1PlusU).cross(m_sliderAxisWorld);
// Compute the inverse of the mass matrix K=JM^-1J^t for the 2 translation // Compute the inverse of the mass matrix K=JM^-1J^t for the 2 translation
// constraints (2x2 matrix) // constraints (2x2 matrix)
float sumInverseMass = m_body1->m_massInverse + m_body2->m_massInverse; float sumInverseMass = m_body1->m_massInverse + m_body2->m_massInverse;
vec3 I1R1PlusUCrossN1 = m_i1 * mR1PlusUCrossN1; vec3 I1R1PlusUCrossN1 = m_i1 * m_R1PlusUCrossN1;
vec3 I1R1PlusUCrossN2 = m_i1 * mR1PlusUCrossN2; vec3 I1R1PlusUCrossN2 = m_i1 * m_R1PlusUCrossN2;
vec3 I2R2CrossN1 = m_i2 * mR2CrossN1; vec3 I2R2CrossN1 = m_i2 * m_R2CrossN1;
vec3 I2R2CrossN2 = m_i2 * mR2CrossN2; vec3 I2R2CrossN2 = m_i2 * m_R2CrossN2;
const float el11 = sumInverseMass + mR1PlusUCrossN1.dot(I1R1PlusUCrossN1) + const float el11 = sumInverseMass + m_R1PlusUCrossN1.dot(I1R1PlusUCrossN1) +
mR2CrossN1.dot(I2R2CrossN1); m_R2CrossN1.dot(I2R2CrossN1);
const float el12 = mR1PlusUCrossN1.dot(I1R1PlusUCrossN2) + const float el12 = m_R1PlusUCrossN1.dot(I1R1PlusUCrossN2) +
mR2CrossN1.dot(I2R2CrossN2); m_R2CrossN1.dot(I2R2CrossN2);
const float el21 = mR1PlusUCrossN2.dot(I1R1PlusUCrossN1) + const float el21 = m_R1PlusUCrossN2.dot(I1R1PlusUCrossN1) +
mR2CrossN2.dot(I2R2CrossN1); m_R2CrossN2.dot(I2R2CrossN1);
const float el22 = sumInverseMass + mR1PlusUCrossN2.dot(I1R1PlusUCrossN2) + const float el22 = sumInverseMass + m_R1PlusUCrossN2.dot(I1R1PlusUCrossN2) +
mR2CrossN2.dot(I2R2CrossN2); m_R2CrossN2.dot(I2R2CrossN2);
etk::Matrix2x2 matrixKTranslation(el11, el12, el21, el22); etk::Matrix2x2 matrixKTranslation(el11, el12, el21, el22);
m_inverseMassMatrixTranslationConstraint.setZero(); m_inverseMassMatrixTranslationConstraint.setZero();
if (m_body1->getType() == DYNAMIC || m_body2->getType() == DYNAMIC) { 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 // Compute the bias "b" of the translation constraint
mBTranslation.setZero(); m_bTranslation.setZero();
float biasFactor = (BETA / constraintSolverData.timeStep); float biasFactor = (BETA / constraintSolverData.timeStep);
if (m_positionCorrectionTechnique == BAUMGARTE_JOINTS) { if (m_positionCorrectionTechnique == BAUMGARTE_JOINTS) {
mBTranslation.setX(u.dot(mN1)); m_bTranslation.setX(u.dot(m_N1));
mBTranslation.setY(u.dot(mN2)); m_bTranslation.setY(u.dot(m_N2));
mBTranslation *= biasFactor; m_bTranslation *= biasFactor;
} }
// Compute the inverse of the mass matrix K=JM^-1J^t for the 3 rotation // 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 // Compute the bias "b" of the rotation constraint
mBRotation.setZero(); m_bRotation.setZero();
if (m_positionCorrectionTechnique == BAUMGARTE_JOINTS) { if (m_positionCorrectionTechnique == BAUMGARTE_JOINTS) {
etk::Quaternion currentOrientationDifference = orientationBody2 * orientationBody1.getInverse(); etk::Quaternion currentOrientationDifference = orientationBody2 * orientationBody1.getInverse();
currentOrientationDifference.normalize(); currentOrientationDifference.normalize();
const etk::Quaternion qError = currentOrientationDifference * m_initOrientationDifferenceInv; 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 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) // 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 + m_inverseMassMatrixLimit = m_body1->m_massInverse + m_body2->m_massInverse +
mR1PlusUCrossSliderAxis.dot(m_i1 * mR1PlusUCrossSliderAxis) + m_R1PlusUCrossSliderAxis.dot(m_i1 * m_R1PlusUCrossSliderAxis) +
mR2CrossSliderAxis.dot(m_i2 * mR2CrossSliderAxis); m_R2CrossSliderAxis.dot(m_i2 * m_R2CrossSliderAxis);
m_inverseMassMatrixLimit = (m_inverseMassMatrixLimit > 0.0) ? m_inverseMassMatrixLimit = (m_inverseMassMatrixLimit > 0.0) ?
1.0f / m_inverseMassMatrixLimit : 0.0f; 1.0f / m_inverseMassMatrixLimit : 0.0f;
// Compute the bias "b" of the lower limit constraint // Compute the bias "b" of the lower limit constraint
mBLowerLimit = 0.0; m_bLowerLimit = 0.0;
if (m_positionCorrectionTechnique == BAUMGARTE_JOINTS) { if (m_positionCorrectionTechnique == BAUMGARTE_JOINTS) {
mBLowerLimit = biasFactor * lowerLimitError; m_bLowerLimit = biasFactor * lowerLimitError;
} }
// Compute the bias "b" of the upper limit constraint // Compute the bias "b" of the upper limit constraint
mBUpperLimit = 0.0; m_bUpperLimit = 0.0;
if (m_positionCorrectionTechnique == BAUMGARTE_JOINTS) { if (m_positionCorrectionTechnique == BAUMGARTE_JOINTS) {
mBUpperLimit = biasFactor * upperLimitError; m_bUpperLimit = biasFactor * upperLimitError;
} }
} }
// If the motor is enabled // 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) // 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; 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 // Compute the impulse P=J^T * lambda for the lower and upper limits constraints of body 1
float impulseLimits = m_impulseUpperLimit - m_impulseLowerLimit; 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 // 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 // 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 linearImpulseBody1 = -m_N1 * m_impulseTranslation.x() - m_N2 * m_impulseTranslation.y();
vec3 angularImpulseBody1 = -mR1PlusUCrossN1 * m_impulseTranslation.x() - vec3 angularImpulseBody1 = -m_R1PlusUCrossN1 * m_impulseTranslation.x() -
mR1PlusUCrossN2 * m_impulseTranslation.y(); m_R1PlusUCrossN2 * m_impulseTranslation.y();
// Compute the impulse P=J^T * lambda for the 3 rotation constraints of body 1 // Compute the impulse P=J^T * lambda for the 3 rotation constraints of body 1
angularImpulseBody1 += -m_impulseRotation; angularImpulseBody1 += -m_impulseRotation;
// Compute the impulse P=J^T * lambda for the lower and upper limits constraints of body 1 // Compute the impulse P=J^T * lambda for the lower and upper limits constraints of body 1
linearImpulseBody1 += linearImpulseLimits; linearImpulseBody1 += linearImpulseLimits;
angularImpulseBody1 += impulseLimits * mR1PlusUCrossSliderAxis; angularImpulseBody1 += impulseLimits * m_R1PlusUCrossSliderAxis;
// Compute the impulse P=J^T * lambda for the motor constraint of body 1 // Compute the impulse P=J^T * lambda for the motor constraint of body 1
linearImpulseBody1 += impulseMotor; linearImpulseBody1 += impulseMotor;
@ -233,16 +233,16 @@ void SliderJoint::warmstart(const ConstraintSolverData& constraintSolverData) {
w1 += m_i1 * angularImpulseBody1; w1 += m_i1 * angularImpulseBody1;
// Compute the impulse P=J^T * lambda for the 2 translation constraints of body 2 // 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 linearImpulseBody2 = m_N1 * m_impulseTranslation.x() + m_N2 * m_impulseTranslation.y();
vec3 angularImpulseBody2 = mR2CrossN1 * m_impulseTranslation.x() + vec3 angularImpulseBody2 = m_R2CrossN1 * m_impulseTranslation.x() +
mR2CrossN2 * m_impulseTranslation.y(); m_R2CrossN2 * m_impulseTranslation.y();
// Compute the impulse P=J^T * lambda for the 3 rotation constraints of body 2 // Compute the impulse P=J^T * lambda for the 3 rotation constraints of body 2
angularImpulseBody2 += m_impulseRotation; angularImpulseBody2 += m_impulseRotation;
// Compute the impulse P=J^T * lambda for the lower and upper limits constraints of body 2 // Compute the impulse P=J^T * lambda for the lower and upper limits constraints of body 2
linearImpulseBody2 += -linearImpulseLimits; linearImpulseBody2 += -linearImpulseLimits;
angularImpulseBody2 += -impulseLimits * mR2CrossSliderAxis; angularImpulseBody2 += -impulseLimits * m_R2CrossSliderAxis;
// Compute the impulse P=J^T * lambda for the motor constraint of body 2 // Compute the impulse P=J^T * lambda for the motor constraint of body 2
linearImpulseBody2 += -impulseMotor; linearImpulseBody2 += -impulseMotor;
@ -268,28 +268,28 @@ void SliderJoint::solveVelocityConstraint(const ConstraintSolverData& constraint
// --------------- Translation Constraints --------------- // // --------------- Translation Constraints --------------- //
// Compute J*v for the 2 translation constraints // Compute J*v for the 2 translation constraints
const float el1 = -mN1.dot(v1) - w1.dot(mR1PlusUCrossN1) + const float el1 = -m_N1.dot(v1) - w1.dot(m_R1PlusUCrossN1) +
mN1.dot(v2) + w2.dot(mR2CrossN1); m_N1.dot(v2) + w2.dot(m_R2CrossN1);
const float el2 = -mN2.dot(v1) - w1.dot(mR1PlusUCrossN2) + const float el2 = -m_N2.dot(v1) - w1.dot(m_R1PlusUCrossN2) +
mN2.dot(v2) + w2.dot(mR2CrossN2); m_N2.dot(v2) + w2.dot(m_R2CrossN2);
const vec2 JvTranslation(el1, el2); const vec2 JvTranslation(el1, el2);
// Compute the Lagrange multiplier lambda for the 2 translation constraints // 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; m_impulseTranslation += deltaLambda;
// Compute the impulse P=J^T * lambda for the 2 translation constraints of body 1 // Compute the impulse P=J^T * lambda for the 2 translation constraints of body 1
const vec3 linearImpulseBody1 = -mN1 * deltaLambda.x() - mN2 * deltaLambda.y(); const vec3 linearImpulseBody1 = -m_N1 * deltaLambda.x() - m_N2 * deltaLambda.y();
vec3 angularImpulseBody1 = -mR1PlusUCrossN1 * deltaLambda.x() - vec3 angularImpulseBody1 = -m_R1PlusUCrossN1 * deltaLambda.x() -
mR1PlusUCrossN2 * deltaLambda.y(); m_R1PlusUCrossN2 * deltaLambda.y();
// Apply the impulse to the body 1 // Apply the impulse to the body 1
v1 += inverseMassBody1 * linearImpulseBody1; v1 += inverseMassBody1 * linearImpulseBody1;
w1 += m_i1 * angularImpulseBody1; w1 += m_i1 * angularImpulseBody1;
// Compute the impulse P=J^T * lambda for the 2 translation constraints of body 2 // Compute the impulse P=J^T * lambda for the 2 translation constraints of body 2
const vec3 linearImpulseBody2 = mN1 * deltaLambda.x() + mN2 * deltaLambda.y(); const vec3 linearImpulseBody2 = m_N1 * deltaLambda.x() + m_N2 * deltaLambda.y();
vec3 angularImpulseBody2 = mR2CrossN1 * deltaLambda.x() + mR2CrossN2 * deltaLambda.y(); vec3 angularImpulseBody2 = m_R2CrossN1 * deltaLambda.x() + m_R2CrossN2 * deltaLambda.y();
// Apply the impulse to the body 2 // Apply the impulse to the body 2
v2 += inverseMassBody2 * linearImpulseBody2; v2 += inverseMassBody2 * linearImpulseBody2;
@ -301,7 +301,7 @@ void SliderJoint::solveVelocityConstraint(const ConstraintSolverData& constraint
const vec3 JvRotation = w2 - w1; const vec3 JvRotation = w2 - w1;
// Compute the Lagrange multiplier lambda for the 3 rotation constraints // 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; m_impulseRotation += deltaLambda2;
// Compute the impulse P=J^T * lambda for the 3 rotation constraints of body 1 // 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 --------------- // // --------------- Limits Constraints --------------- //
if (mIsLimitEnabled) { if (m_isLimitEnabled) {
// If the lower limit is violated // If the lower limit is violated
if (mIsLowerLimitViolated) { if (m_isLowerLimitViolated) {
// Compute J*v for the lower limit constraint // Compute J*v for the lower limit constraint
const float JvLowerLimit = mSliderAxisWorld.dot(v2) + mR2CrossSliderAxis.dot(w2) - const float JvLowerLimit = m_sliderAxisWorld.dot(v2) + m_R2CrossSliderAxis.dot(w2) -
mSliderAxisWorld.dot(v1) - mR1PlusUCrossSliderAxis.dot(w1); m_sliderAxisWorld.dot(v1) - m_R1PlusUCrossSliderAxis.dot(w1);
// Compute the Lagrange multiplier lambda for the lower limit constraint // 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; float lambdaTemp = m_impulseLowerLimit;
m_impulseLowerLimit = std::max(m_impulseLowerLimit + deltaLambdaLower, 0.0f); m_impulseLowerLimit = std::max(m_impulseLowerLimit + deltaLambdaLower, 0.0f);
deltaLambdaLower = m_impulseLowerLimit - lambdaTemp; deltaLambdaLower = m_impulseLowerLimit - lambdaTemp;
// Compute the impulse P=J^T * lambda for the lower limit constraint of body 1 // Compute the impulse P=J^T * lambda for the lower limit constraint of body 1
const vec3 linearImpulseBody1 = -deltaLambdaLower * mSliderAxisWorld; const vec3 linearImpulseBody1 = -deltaLambdaLower * m_sliderAxisWorld;
const vec3 angularImpulseBody1 = -deltaLambdaLower * mR1PlusUCrossSliderAxis; const vec3 angularImpulseBody1 = -deltaLambdaLower * m_R1PlusUCrossSliderAxis;
// Apply the impulse to the body 1 // Apply the impulse to the body 1
v1 += inverseMassBody1 * linearImpulseBody1; v1 += inverseMassBody1 * linearImpulseBody1;
w1 += m_i1 * angularImpulseBody1; w1 += m_i1 * angularImpulseBody1;
// Compute the impulse P=J^T * lambda for the lower limit constraint of body 2 // Compute the impulse P=J^T * lambda for the lower limit constraint of body 2
const vec3 linearImpulseBody2 = deltaLambdaLower * mSliderAxisWorld; const vec3 linearImpulseBody2 = deltaLambdaLower * m_sliderAxisWorld;
const vec3 angularImpulseBody2 = deltaLambdaLower * mR2CrossSliderAxis; const vec3 angularImpulseBody2 = deltaLambdaLower * m_R2CrossSliderAxis;
// Apply the impulse to the body 2 // Apply the impulse to the body 2
v2 += inverseMassBody2 * linearImpulseBody2; v2 += inverseMassBody2 * linearImpulseBody2;
@ -351,29 +351,29 @@ void SliderJoint::solveVelocityConstraint(const ConstraintSolverData& constraint
} }
// If the upper limit is violated // If the upper limit is violated
if (mIsUpperLimitViolated) { if (m_isUpperLimitViolated) {
// Compute J*v for the upper limit constraint // Compute J*v for the upper limit constraint
const float JvUpperLimit = mSliderAxisWorld.dot(v1) + mR1PlusUCrossSliderAxis.dot(w1) const float JvUpperLimit = m_sliderAxisWorld.dot(v1) + m_R1PlusUCrossSliderAxis.dot(w1)
- mSliderAxisWorld.dot(v2) - mR2CrossSliderAxis.dot(w2); - m_sliderAxisWorld.dot(v2) - m_R2CrossSliderAxis.dot(w2);
// Compute the Lagrange multiplier lambda for the upper limit constraint // 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; float lambdaTemp = m_impulseUpperLimit;
m_impulseUpperLimit = std::max(m_impulseUpperLimit + deltaLambdaUpper, 0.0f); m_impulseUpperLimit = std::max(m_impulseUpperLimit + deltaLambdaUpper, 0.0f);
deltaLambdaUpper = m_impulseUpperLimit - lambdaTemp; deltaLambdaUpper = m_impulseUpperLimit - lambdaTemp;
// Compute the impulse P=J^T * lambda for the upper limit constraint of body 1 // Compute the impulse P=J^T * lambda for the upper limit constraint of body 1
const vec3 linearImpulseBody1 = deltaLambdaUpper * mSliderAxisWorld; const vec3 linearImpulseBody1 = deltaLambdaUpper * m_sliderAxisWorld;
const vec3 angularImpulseBody1 = deltaLambdaUpper * mR1PlusUCrossSliderAxis; const vec3 angularImpulseBody1 = deltaLambdaUpper * m_R1PlusUCrossSliderAxis;
// Apply the impulse to the body 1 // Apply the impulse to the body 1
v1 += inverseMassBody1 * linearImpulseBody1; v1 += inverseMassBody1 * linearImpulseBody1;
w1 += m_i1 * angularImpulseBody1; w1 += m_i1 * angularImpulseBody1;
// Compute the impulse P=J^T * lambda for the upper limit constraint of body 2 // Compute the impulse P=J^T * lambda for the upper limit constraint of body 2
const vec3 linearImpulseBody2 = -deltaLambdaUpper * mSliderAxisWorld; const vec3 linearImpulseBody2 = -deltaLambdaUpper * m_sliderAxisWorld;
const vec3 angularImpulseBody2 = -deltaLambdaUpper * mR2CrossSliderAxis; const vec3 angularImpulseBody2 = -deltaLambdaUpper * m_R2CrossSliderAxis;
// Apply the impulse to the body 2 // Apply the impulse to the body 2
v2 += inverseMassBody2 * linearImpulseBody2; v2 += inverseMassBody2 * linearImpulseBody2;
@ -383,26 +383,26 @@ void SliderJoint::solveVelocityConstraint(const ConstraintSolverData& constraint
// --------------- Motor --------------- // // --------------- Motor --------------- //
if (mIsMotorEnabled) { if (m_isMotorEnabled) {
// Compute J*v for the motor // 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 // Compute the Lagrange multiplier lambda for the motor
const float maxMotorImpulse = mMaxMotorForce * constraintSolverData.timeStep; const float maxMotorImpulse = m_maxMotorForce * constraintSolverData.timeStep;
float deltaLambdaMotor = m_inverseMassMatrixMotor * (-JvMotor - mMotorSpeed); float deltaLambdaMotor = m_inverseMassMatrixMotor * (-JvMotor - m_motorSpeed);
float lambdaTemp = m_impulseMotor; float lambdaTemp = m_impulseMotor;
m_impulseMotor = clamp(m_impulseMotor + deltaLambdaMotor, -maxMotorImpulse, maxMotorImpulse); m_impulseMotor = clamp(m_impulseMotor + deltaLambdaMotor, -maxMotorImpulse, maxMotorImpulse);
deltaLambdaMotor = m_impulseMotor - lambdaTemp; deltaLambdaMotor = m_impulseMotor - lambdaTemp;
// Compute the impulse P=J^T * lambda for the motor of body 1 // 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 // Apply the impulse to the body 1
v1 += inverseMassBody1 * linearImpulseBody1; v1 += inverseMassBody1 * linearImpulseBody1;
// Compute the impulse P=J^T * lambda for the motor of body 2 // 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 // Apply the impulse to the body 2
v2 += inverseMassBody2 * linearImpulseBody2; v2 += inverseMassBody2 * linearImpulseBody2;
@ -431,51 +431,51 @@ void SliderJoint::solvePositionConstraint(const ConstraintSolverData& constraint
m_i2 = m_body2->getInertiaTensorInverseWorld(); m_i2 = m_body2->getInertiaTensorInverseWorld();
// Vector from body center to the anchor point // Vector from body center to the anchor point
mR1 = q1 * m_localAnchorPointBody1; m_R1 = q1 * m_localAnchorPointBody1;
mR2 = q2 * m_localAnchorPointBody2; m_R2 = q2 * m_localAnchorPointBody2;
// Compute the vector u (difference between anchor points) // 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 // Compute the two orthogonal vectors to the slider axis in world-space
mSliderAxisWorld = q1 * mSliderAxisBody1; m_sliderAxisWorld = q1 * m_sliderAxisBody1;
mSliderAxisWorld.normalize(); m_sliderAxisWorld.normalize();
mN1 = mSliderAxisWorld.getOrthoVector(); m_N1 = m_sliderAxisWorld.getOrthoVector();
mN2 = mSliderAxisWorld.cross(mN1); m_N2 = m_sliderAxisWorld.cross(m_N1);
// Check if the limit constraints are violated or not // Check if the limit constraints are violated or not
float uDotSliderAxis = u.dot(mSliderAxisWorld); float uDotSliderAxis = u.dot(m_sliderAxisWorld);
float lowerLimitError = uDotSliderAxis - mLowerLimit; float lowerLimitError = uDotSliderAxis - m_lowerLimit;
float upperLimitError = mUpperLimit - uDotSliderAxis; float upperLimitError = m_upperLimit - uDotSliderAxis;
mIsLowerLimitViolated = lowerLimitError <= 0; m_isLowerLimitViolated = lowerLimitError <= 0;
mIsUpperLimitViolated = upperLimitError <= 0; m_isUpperLimitViolated = upperLimitError <= 0;
// Compute the cross products used in the Jacobians // Compute the cross products used in the Jacobians
mR2CrossN1 = mR2.cross(mN1); m_R2CrossN1 = m_R2.cross(m_N1);
mR2CrossN2 = mR2.cross(mN2); m_R2CrossN2 = m_R2.cross(m_N2);
mR2CrossSliderAxis = mR2.cross(mSliderAxisWorld); m_R2CrossSliderAxis = m_R2.cross(m_sliderAxisWorld);
const vec3 r1PlusU = mR1 + u; const vec3 r1PlusU = m_R1 + u;
mR1PlusUCrossN1 = (r1PlusU).cross(mN1); m_R1PlusUCrossN1 = (r1PlusU).cross(m_N1);
mR1PlusUCrossN2 = (r1PlusU).cross(mN2); m_R1PlusUCrossN2 = (r1PlusU).cross(m_N2);
mR1PlusUCrossSliderAxis = (r1PlusU).cross(mSliderAxisWorld); m_R1PlusUCrossSliderAxis = (r1PlusU).cross(m_sliderAxisWorld);
// --------------- Translation Constraints --------------- // // --------------- Translation Constraints --------------- //
// Recompute the inverse of the mass matrix K=JM^-1J^t for the 2 translation // Recompute the inverse of the mass matrix K=JM^-1J^t for the 2 translation
// constraints (2x2 matrix) // constraints (2x2 matrix)
float sumInverseMass = m_body1->m_massInverse + m_body2->m_massInverse; float sumInverseMass = m_body1->m_massInverse + m_body2->m_massInverse;
vec3 I1R1PlusUCrossN1 = m_i1 * mR1PlusUCrossN1; vec3 I1R1PlusUCrossN1 = m_i1 * m_R1PlusUCrossN1;
vec3 I1R1PlusUCrossN2 = m_i1 * mR1PlusUCrossN2; vec3 I1R1PlusUCrossN2 = m_i1 * m_R1PlusUCrossN2;
vec3 I2R2CrossN1 = m_i2 * mR2CrossN1; vec3 I2R2CrossN1 = m_i2 * m_R2CrossN1;
vec3 I2R2CrossN2 = m_i2 * mR2CrossN2; vec3 I2R2CrossN2 = m_i2 * m_R2CrossN2;
const float el11 = sumInverseMass + mR1PlusUCrossN1.dot(I1R1PlusUCrossN1) + const float el11 = sumInverseMass + m_R1PlusUCrossN1.dot(I1R1PlusUCrossN1) +
mR2CrossN1.dot(I2R2CrossN1); m_R2CrossN1.dot(I2R2CrossN1);
const float el12 = mR1PlusUCrossN1.dot(I1R1PlusUCrossN2) + const float el12 = m_R1PlusUCrossN1.dot(I1R1PlusUCrossN2) +
mR2CrossN1.dot(I2R2CrossN2); m_R2CrossN1.dot(I2R2CrossN2);
const float el21 = mR1PlusUCrossN2.dot(I1R1PlusUCrossN1) + const float el21 = m_R1PlusUCrossN2.dot(I1R1PlusUCrossN1) +
mR2CrossN2.dot(I2R2CrossN1); m_R2CrossN2.dot(I2R2CrossN1);
const float el22 = sumInverseMass + mR1PlusUCrossN2.dot(I1R1PlusUCrossN2) + const float el22 = sumInverseMass + m_R1PlusUCrossN2.dot(I1R1PlusUCrossN2) +
mR2CrossN2.dot(I2R2CrossN2); m_R2CrossN2.dot(I2R2CrossN2);
etk::Matrix2x2 matrixKTranslation(el11, el12, el21, el22); etk::Matrix2x2 matrixKTranslation(el11, el12, el21, el22);
m_inverseMassMatrixTranslationConstraint.setZero(); m_inverseMassMatrixTranslationConstraint.setZero();
if (m_body1->getType() == DYNAMIC || m_body2->getType() == DYNAMIC) { 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 // 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 // Compute the Lagrange multiplier lambda for the 2 translation constraints
vec2 lambdaTranslation = m_inverseMassMatrixTranslationConstraint * (-translationError); vec2 lambdaTranslation = m_inverseMassMatrixTranslationConstraint * (-translationError);
// Compute the impulse P=J^T * lambda for the 2 translation constraints of body 1 // Compute the impulse P=J^T * lambda for the 2 translation constraints of body 1
const vec3 linearImpulseBody1 = -mN1 * lambdaTranslation.x() - mN2 * lambdaTranslation.y(); const vec3 linearImpulseBody1 = -m_N1 * lambdaTranslation.x() - m_N2 * lambdaTranslation.y();
vec3 angularImpulseBody1 = -mR1PlusUCrossN1 * lambdaTranslation.x() - vec3 angularImpulseBody1 = -m_R1PlusUCrossN1 * lambdaTranslation.x() -
mR1PlusUCrossN2 * lambdaTranslation.y(); m_R1PlusUCrossN2 * lambdaTranslation.y();
// Apply the impulse to the body 1 // Apply the impulse to the body 1
const vec3 v1 = inverseMassBody1 * linearImpulseBody1; const vec3 v1 = inverseMassBody1 * linearImpulseBody1;
@ -503,9 +503,9 @@ void SliderJoint::solvePositionConstraint(const ConstraintSolverData& constraint
q1.normalize(); q1.normalize();
// Compute the impulse P=J^T * lambda for the 2 translation constraints of body 2 // Compute the impulse P=J^T * lambda for the 2 translation constraints of body 2
const vec3 linearImpulseBody2 = mN1 * lambdaTranslation.x() + mN2 * lambdaTranslation.y(); const vec3 linearImpulseBody2 = m_N1 * lambdaTranslation.x() + m_N2 * lambdaTranslation.y();
vec3 angularImpulseBody2 = mR2CrossN1 * lambdaTranslation.x() + vec3 angularImpulseBody2 = m_R2CrossN1 * lambdaTranslation.x() +
mR2CrossN2 * lambdaTranslation.y(); m_R2CrossN2 * lambdaTranslation.y();
// Apply the impulse to the body 2 // Apply the impulse to the body 2
const vec3 v2 = inverseMassBody2 * linearImpulseBody2; const vec3 v2 = inverseMassBody2 * linearImpulseBody2;
@ -556,27 +556,27 @@ void SliderJoint::solvePositionConstraint(const ConstraintSolverData& constraint
// --------------- Limits 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) // 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 + m_inverseMassMatrixLimit = m_body1->m_massInverse + m_body2->m_massInverse +
mR1PlusUCrossSliderAxis.dot(m_i1 * mR1PlusUCrossSliderAxis) + m_R1PlusUCrossSliderAxis.dot(m_i1 * m_R1PlusUCrossSliderAxis) +
mR2CrossSliderAxis.dot(m_i2 * mR2CrossSliderAxis); m_R2CrossSliderAxis.dot(m_i2 * m_R2CrossSliderAxis);
m_inverseMassMatrixLimit = (m_inverseMassMatrixLimit > 0.0) ? m_inverseMassMatrixLimit = (m_inverseMassMatrixLimit > 0.0) ?
1.0f / m_inverseMassMatrixLimit : 0.0f; 1.0f / m_inverseMassMatrixLimit : 0.0f;
} }
// If the lower limit is violated // If the lower limit is violated
if (mIsLowerLimitViolated) { if (m_isLowerLimitViolated) {
// Compute the Lagrange multiplier lambda for the lower limit constraint // Compute the Lagrange multiplier lambda for the lower limit constraint
float lambdaLowerLimit = m_inverseMassMatrixLimit * (-lowerLimitError); float lambdaLowerLimit = m_inverseMassMatrixLimit * (-lowerLimitError);
// Compute the impulse P=J^T * lambda for the lower limit constraint of body 1 // Compute the impulse P=J^T * lambda for the lower limit constraint of body 1
const vec3 linearImpulseBody1 = -lambdaLowerLimit * mSliderAxisWorld; const vec3 linearImpulseBody1 = -lambdaLowerLimit * m_sliderAxisWorld;
const vec3 angularImpulseBody1 = -lambdaLowerLimit * mR1PlusUCrossSliderAxis; const vec3 angularImpulseBody1 = -lambdaLowerLimit * m_R1PlusUCrossSliderAxis;
// Apply the impulse to the body 1 // Apply the impulse to the body 1
const vec3 v1 = inverseMassBody1 * linearImpulseBody1; const vec3 v1 = inverseMassBody1 * linearImpulseBody1;
@ -588,8 +588,8 @@ void SliderJoint::solvePositionConstraint(const ConstraintSolverData& constraint
q1.normalize(); q1.normalize();
// Compute the impulse P=J^T * lambda for the lower limit constraint of body 2 // Compute the impulse P=J^T * lambda for the lower limit constraint of body 2
const vec3 linearImpulseBody2 = lambdaLowerLimit * mSliderAxisWorld; const vec3 linearImpulseBody2 = lambdaLowerLimit * m_sliderAxisWorld;
const vec3 angularImpulseBody2 = lambdaLowerLimit * mR2CrossSliderAxis; const vec3 angularImpulseBody2 = lambdaLowerLimit * m_R2CrossSliderAxis;
// Apply the impulse to the body 2 // Apply the impulse to the body 2
const vec3 v2 = inverseMassBody2 * linearImpulseBody2; const vec3 v2 = inverseMassBody2 * linearImpulseBody2;
@ -602,14 +602,14 @@ void SliderJoint::solvePositionConstraint(const ConstraintSolverData& constraint
} }
// If the upper limit is violated // If the upper limit is violated
if (mIsUpperLimitViolated) { if (m_isUpperLimitViolated) {
// Compute the Lagrange multiplier lambda for the upper limit constraint // Compute the Lagrange multiplier lambda for the upper limit constraint
float lambdaUpperLimit = m_inverseMassMatrixLimit * (-upperLimitError); float lambdaUpperLimit = m_inverseMassMatrixLimit * (-upperLimitError);
// Compute the impulse P=J^T * lambda for the upper limit constraint of body 1 // Compute the impulse P=J^T * lambda for the upper limit constraint of body 1
const vec3 linearImpulseBody1 = lambdaUpperLimit * mSliderAxisWorld; const vec3 linearImpulseBody1 = lambdaUpperLimit * m_sliderAxisWorld;
const vec3 angularImpulseBody1 = lambdaUpperLimit * mR1PlusUCrossSliderAxis; const vec3 angularImpulseBody1 = lambdaUpperLimit * m_R1PlusUCrossSliderAxis;
// Apply the impulse to the body 1 // Apply the impulse to the body 1
const vec3 v1 = inverseMassBody1 * linearImpulseBody1; const vec3 v1 = inverseMassBody1 * linearImpulseBody1;
@ -621,8 +621,8 @@ void SliderJoint::solvePositionConstraint(const ConstraintSolverData& constraint
q1.normalize(); q1.normalize();
// Compute the impulse P=J^T * lambda for the upper limit constraint of body 2 // Compute the impulse P=J^T * lambda for the upper limit constraint of body 2
const vec3 linearImpulseBody2 = -lambdaUpperLimit * mSliderAxisWorld; const vec3 linearImpulseBody2 = -lambdaUpperLimit * m_sliderAxisWorld;
const vec3 angularImpulseBody2 = -lambdaUpperLimit * mR2CrossSliderAxis; const vec3 angularImpulseBody2 = -lambdaUpperLimit * m_R2CrossSliderAxis;
// Apply the impulse to the body 2 // Apply the impulse to the body 2
const vec3 v2 = inverseMassBody2 * linearImpulseBody2; const vec3 v2 = inverseMassBody2 * linearImpulseBody2;
@ -643,9 +643,9 @@ void SliderJoint::solvePositionConstraint(const ConstraintSolverData& constraint
*/ */
void SliderJoint::enableLimit(bool isLimitEnabled) { void SliderJoint::enableLimit(bool isLimitEnabled) {
if (isLimitEnabled != mIsLimitEnabled) { if (isLimitEnabled != m_isLimitEnabled) {
mIsLimitEnabled = isLimitEnabled; m_isLimitEnabled = isLimitEnabled;
// Reset the limits // Reset the limits
resetLimits(); resetLimits();
@ -659,7 +659,7 @@ void SliderJoint::enableLimit(bool isLimitEnabled) {
*/ */
void SliderJoint::enableMotor(bool isMotorEnabled) { void SliderJoint::enableMotor(bool isMotorEnabled) {
mIsMotorEnabled = isMotorEnabled; m_isMotorEnabled = isMotorEnabled;
m_impulseMotor = 0.0; m_impulseMotor = 0.0;
// Wake up the two bodies of the joint // Wake up the two bodies of the joint
@ -689,7 +689,7 @@ float SliderJoint::getTranslation() const {
const vec3 u = anchorBody2 - anchorBody1; const vec3 u = anchorBody2 - anchorBody1;
// Compute the slider axis in world-space // Compute the slider axis in world-space
vec3 sliderAxisWorld = q1 * mSliderAxisBody1; vec3 sliderAxisWorld = q1 * m_sliderAxisBody1;
sliderAxisWorld.normalize(); sliderAxisWorld.normalize();
// Compute and return the translation value // Compute and return the translation value
@ -702,11 +702,11 @@ float SliderJoint::getTranslation() const {
*/ */
void SliderJoint::setMinTranslationLimit(float lowerLimit) { 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 // Reset the limits
resetLimits(); resetLimits();
@ -719,11 +719,11 @@ void SliderJoint::setMinTranslationLimit(float lowerLimit) {
*/ */
void SliderJoint::setMaxTranslationLimit(float upperLimit) { 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 // Reset the limits
resetLimits(); resetLimits();
@ -748,9 +748,9 @@ void SliderJoint::resetLimits() {
*/ */
void SliderJoint::setMotorSpeed(float motorSpeed) { 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 // Wake up the two bodies of the joint
m_body1->setIsSleeping(false); m_body1->setIsSleeping(false);
@ -764,13 +764,76 @@ void SliderJoint::setMotorSpeed(float motorSpeed) {
*/ */
void SliderJoint::setMaxMotorForce(float maxMotorForce) { void SliderJoint::setMaxMotorForce(float maxMotorForce) {
if (maxMotorForce != mMaxMotorForce) { if (maxMotorForce != m_maxMotorForce) {
assert(mMaxMotorForce >= 0.0); assert(m_maxMotorForce >= 0.0);
mMaxMotorForce = maxMotorForce; m_maxMotorForce = maxMotorForce;
// Wake up the two bodies of the joint // Wake up the two bodies of the joint
m_body1->setIsSleeping(false); m_body1->setIsSleeping(false);
m_body2->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);
}

560
ephysics/constraint/SliderJoint.hpp
View File

@ -5,379 +5,201 @@
*/ */
#pragma once #pragma once
// Libraries
#include <ephysics/mathematics/mathematics.hpp> #include <ephysics/mathematics/mathematics.hpp>
#include <ephysics/engine/ConstraintSolver.hpp> #include <ephysics/engine/ConstraintSolver.hpp>
namespace ephysics { 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);
}
} }

89
ephysics/engine/ConstraintSolver.cpp
View File

@ -9,73 +9,68 @@
using namespace ephysics; using namespace ephysics;
// Constructor ConstraintSolver::ConstraintSolver(const std::map<RigidBody*, uint32_t>& _mapBodyToVelocityIndex):
ConstraintSolver::ConstraintSolver(const std::map<RigidBody*, uint32_t>& mapBodyToVelocityIndex) m_mapBodyToConstrainedVelocityIndex(_mapBodyToVelocityIndex),
: m_mapBodyToConstrainedVelocityIndex(mapBodyToVelocityIndex), m_isWarmStartingActive(true),
m_isWarmStartingActive(true), m_constraintSolverData(mapBodyToVelocityIndex) { 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()"); PROFILE("ConstraintSolver::initializeForIsland()");
assert(_island != nullptr);
assert(island != NULL); assert(_island->getNbBodies() > 0);
assert(island->getNbBodies() > 0); assert(_island->getNbJoints() > 0);
assert(island->getNbJoints() > 0);
// Set the current time step // Set the current time step
m_timeStep = dt; m_timeStep = _dt;
// Initialize the constraint solver data used to initialize and solve the constraints // Initialize the constraint solver data used to initialize and solve the constraints
m_constraintSolverData.timeStep = m_timeStep; m_constraintSolverData.timeStep = m_timeStep;
m_constraintSolverData.isWarmStartingActive = m_isWarmStartingActive; m_constraintSolverData.isWarmStartingActive = m_isWarmStartingActive;
// For each joint of the island // For each joint of the island
Joint** joints = island->getJoints(); Joint** joints = _island->getJoints();
for (uint32_t i=0; i<island->getNbJoints(); i++) { for (uint32_t iii=0; iii<_island->getNbJoints(); ++iii) {
// Initialize the constraint before solving it // 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 // Warm-start the constraint if warm-starting is enabled
if (m_isWarmStartingActive) { 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()"); PROFILE("ConstraintSolver::solveVelocityConstraints()");
assert(_island != nullptr);
assert(island != NULL); assert(_island->getNbJoints() > 0);
assert(island->getNbJoints() > 0);
// For each joint of the island // For each joint of the island
Joint** joints = island->getJoints(); Joint** joints = _island->getJoints();
for (uint32_t i=0; i<island->getNbJoints(); i++) { for (uint32_t iii=0; iii<_island->getNbJoints(); ++iii) {
joints[iii]->solveVelocityConstraint(m_constraintSolverData);
// Solve the constraint
joints[i]->solveVelocityConstraint(m_constraintSolverData);
} }
} }
// Solve the position constraints void ConstraintSolver::solvePositionConstraints(Island* _island) {
void ConstraintSolver::solvePositionConstraints(Island* island) {
PROFILE("ConstraintSolver::solvePositionConstraints()"); PROFILE("ConstraintSolver::solvePositionConstraints()");
assert(_island != nullptr);
assert(island != NULL); assert(_island->getNbJoints() > 0);
assert(island->getNbJoints() > 0); Joint** joints = _island->getJoints();
for (uint32_t iii=0; iii < _island->getNbJoints(); ++iii) {
// For each joint of the island joints[iii]->solvePositionConstraint(m_constraintSolverData);
Joint** joints = island->getJoints();
for (uint32_t i=0; i < island->getNbJoints(); i++) {
// Solve the constraint
joints[i]->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;
}

18
ephysics/engine/ConstraintSolver.hpp
View File

@ -130,22 +130,4 @@ namespace ephysics {
etk::Quaternion* constrainedOrientations); 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;
}
} }

86
ephysics/engine/ContactSolver.cpp
View File

@ -894,3 +894,89 @@ void ContactSolver::cleanup() {
m_contactConstraints = nullptr; 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);
}

86
ephysics/engine/ContactSolver.hpp
View File

@ -237,90 +237,4 @@ namespace ephysics {
void cleanup(); 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);
}
} }

216
ephysics/engine/DynamicsWorld.cpp
View File

@ -985,3 +985,219 @@ std::vector<const ContactManifold*> DynamicsWorld::getContactsList() const {
// Return all the contact manifold // Return all the contact manifold
return contactManifolds; return contactManifolds;
} }
// Reset the external force and torque applied to the bodies
void DynamicsWorld::resetBodiesForceAndTorque() {
// For each body of the world
std::set<RigidBody*>::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<RigidBody*>::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<RigidBody*>::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;
}

215
ephysics/engine/DynamicsWorld.hpp
View File

@ -165,221 +165,6 @@ namespace ephysics {
friend class RigidBody; friend class RigidBody;
}; };
// Reset the external force and torque applied to the bodies
void DynamicsWorld::resetBodiesForceAndTorque() {
// For each body of the world
std::set<RigidBody*>::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<RigidBody*>::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<RigidBody*>::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;
}
} }

49
ephysics/engine/Island.cpp
View File

@ -32,3 +32,52 @@ Island::~Island() {
m_memoryAllocator.release(m_contactManifolds, m_numberAllocatedBytesContactManifolds); m_memoryAllocator.release(m_contactManifolds, m_numberAllocatedBytesContactManifolds);
m_memoryAllocator.release(m_joints, m_numberAllocatedBytesJoints); 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;
}

48
ephysics/engine/Island.hpp
View File

@ -58,53 +58,5 @@ namespace ephysics {
friend class DynamicsWorld; 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;
}
} }

74
ephysics/engine/Material.cpp
View File

@ -23,3 +23,77 @@ Material::Material(const Material& material)
m_rollingResistance(material.m_rollingResistance), m_bounciness(material.m_bounciness) { m_rollingResistance(material.m_rollingResistance), m_bounciness(material.m_bounciness) {
} }
// 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;
}

73
ephysics/engine/Material.hpp
View File

@ -39,77 +39,4 @@ namespace ephysics {
Material& operator=(const Material& material); 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;
}
} }

70
ephysics/engine/OverlappingPair.hpp
View File

@ -57,75 +57,5 @@ namespace ephysics {
friend class DynamicsWorld; 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();
}
} }

110
ephysics/engine/Profiler.cpp
View File

@ -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 #endif

123
ephysics/engine/Profiler.hpp
View File

@ -143,125 +143,10 @@ namespace ephysics {
} }
}; };
// Use this macro to start profile a block of code // Use this macro to start profile a block of code
#define PROFILE(name) ProfileSample profileSample(name) #define PROFILE(name) ProfileSample profileSample(name)
// Return true if we are at the root of the profiler tree
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();
}
}
#else #else
// Empty macro in case profiling is not active
// Empty macro in case profiling is not active #define PROFILE(name)
#define PROFILE(name)
#endif #endif

62
ephysics/engine/Timer.cpp
View File

@ -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;
}

62
ephysics/engine/Timer.hpp
View File

@ -66,66 +66,4 @@ namespace ephysics {
static long double getCurrentSystemTime(); 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;
}
} }