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

[DEV] continue renaming

Browse Source
This commit is contained in:
Edouard DUPIN 2017-06-08 22:50:13 +02:00
parent 0f6adcf289
commit b8685d12c3
42 changed files with 656 additions and 656 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
ephysics/body/Body.cpp
View File

@ -16,8 +16,8 @@ using namespace reactphysics3d;
* @param id ID of the new body * @param id ID of the new body
*/ */
Body::Body(bodyindex id) Body::Body(bodyindex id)
: mID(id), mIsAlreadyInIsland(false), mIsAllowedToSleep(true), mIsActive(true), : m_id(id), m_isAlreadyInIsland(false), m_isAllowedToSleep(true), m_isActive(true),
mIsSleeping(false), mSleepTime(0), mUserData(NULL) { m_isSleeping(false), m_sleepTime(0), m_userData(NULL) {
} }

48
ephysics/body/Body.h
View File

@ -27,13 +27,13 @@ class Body {
// -------------------- Attributes -------------------- // // -------------------- Attributes -------------------- //
/// ID of the body /// ID of the body
bodyindex mID; bodyindex m_id;
/// True if the body has already been added in an island (for sleeping technique) /// True if the body has already been added in an island (for sleeping technique)
bool mIsAlreadyInIsland; bool m_isAlreadyInIsland;
/// True if the body is allowed to go to sleep for better efficiency /// True if the body is allowed to go to sleep for better efficiency
bool mIsAllowedToSleep; bool m_isAllowedToSleep;
/// True if the body is active. /// True if the body is active.
/// An inactive body does not participate in collision detection, /// An inactive body does not participate in collision detection,
@ -43,16 +43,16 @@ class Body {
/// removed from the broad-phase. If you set this value to "true", /// removed from the broad-phase. If you set this value to "true",
/// all the proxy shapes will be added to the broad-phase. A joint /// all the proxy shapes will be added to the broad-phase. A joint
/// connected to an inactive body will also be inactive. /// connected to an inactive body will also be inactive.
bool mIsActive; bool m_isActive;
/// True if the body is sleeping (for sleeping technique) /// True if the body is sleeping (for sleeping technique)
bool mIsSleeping; bool m_isSleeping;
/// Elapsed time since the body velocity was bellow the sleep velocity /// Elapsed time since the body velocity was bellow the sleep velocity
float mSleepTime; float m_sleepTime;
/// Pointer that can be used to attach user data to the body /// Pointer that can be used to attach user data to the body
void* mUserData; void* m_userData;
// -------------------- Methods -------------------- // // -------------------- Methods -------------------- //
@ -121,7 +121,7 @@ class Body {
* @return The ID of the body * @return The ID of the body
*/ */
inline bodyindex Body::getID() const { inline bodyindex Body::getID() const {
return mID; return m_id;
} }
// Return whether or not the body is allowed to sleep // Return whether or not the body is allowed to sleep
@ -129,7 +129,7 @@ inline bodyindex Body::getID() const {
* @return True if the body is allowed to sleep and false otherwise * @return True if the body is allowed to sleep and false otherwise
*/ */
inline bool Body::isAllowedToSleep() const { inline bool Body::isAllowedToSleep() const {
return mIsAllowedToSleep; return m_isAllowedToSleep;
} }
// Set whether or not the body is allowed to go to sleep // Set whether or not the body is allowed to go to sleep
@ -137,9 +137,9 @@ inline bool Body::isAllowedToSleep() const {
* @param isAllowedToSleep True if the body is allowed to sleep * @param isAllowedToSleep True if the body is allowed to sleep
*/ */
inline void Body::setIsAllowedToSleep(bool isAllowedToSleep) { inline void Body::setIsAllowedToSleep(bool isAllowedToSleep) {
mIsAllowedToSleep = isAllowedToSleep; m_isAllowedToSleep = isAllowedToSleep;
if (!mIsAllowedToSleep) setIsSleeping(false); if (!m_isAllowedToSleep) setIsSleeping(false);
} }
// Return whether or not the body is sleeping // Return whether or not the body is sleeping
@ -147,7 +147,7 @@ inline void Body::setIsAllowedToSleep(bool isAllowedToSleep) {
* @return True if the body is currently sleeping and false otherwise * @return True if the body is currently sleeping and false otherwise
*/ */
inline bool Body::isSleeping() const { inline bool Body::isSleeping() const {
return mIsSleeping; return m_isSleeping;
} }
// Return true if the body is active // Return true if the body is active
@ -155,7 +155,7 @@ inline bool Body::isSleeping() const {
* @return True if the body currently active and false otherwise * @return True if the body currently active and false otherwise
*/ */
inline bool Body::isActive() const { inline bool Body::isActive() const {
return mIsActive; return m_isActive;
} }
// Set whether or not the body is active // Set whether or not the body is active
@ -163,22 +163,22 @@ inline bool Body::isActive() const {
* @param isActive True if you want to activate the body * @param isActive True if you want to activate the body
*/ */
inline void Body::setIsActive(bool isActive) { inline void Body::setIsActive(bool isActive) {
mIsActive = isActive; m_isActive = isActive;
} }
// Set the variable to know whether or not the body is sleeping // Set the variable to know whether or not the body is sleeping
inline void Body::setIsSleeping(bool isSleeping) { inline void Body::setIsSleeping(bool isSleeping) {
if (isSleeping) { if (isSleeping) {
mSleepTime = float(0.0); m_sleepTime = float(0.0);
} }
else { else {
if (mIsSleeping) { if (m_isSleeping) {
mSleepTime = float(0.0); m_sleepTime = float(0.0);
} }
} }
mIsSleeping = isSleeping; m_isSleeping = isSleeping;
} }
// Return a pointer to the user data attached to this body // Return a pointer to the user data attached to this body
@ -186,7 +186,7 @@ inline void Body::setIsSleeping(bool isSleeping) {
* @return A pointer to the user data you have attached to the body * @return A pointer to the user data you have attached to the body
*/ */
inline void* Body::getUserData() const { inline void* Body::getUserData() const {
return mUserData; return m_userData;
} }
// Attach user data to this body // Attach user data to this body
@ -194,27 +194,27 @@ inline void* Body::getUserData() const {
* @param userData A pointer to the user data you want to attach to the body * @param userData A pointer to the user data you want to attach to the body
*/ */
inline void Body::setUserData(void* userData) { inline void Body::setUserData(void* userData) {
mUserData = userData; m_userData = userData;
} }
// Smaller than operator // Smaller than operator
inline bool Body::operator<(const Body& body2) const { inline bool Body::operator<(const Body& body2) const {
return (mID < body2.mID); return (m_id < body2.m_id);
} }
// Larger than operator // Larger than operator
inline bool Body::operator>(const Body& body2) const { inline bool Body::operator>(const Body& body2) const {
return (mID > body2.mID); return (m_id > body2.m_id);
} }
// Equal operator // Equal operator
inline bool Body::operator==(const Body& body2) const { inline bool Body::operator==(const Body& body2) const {
return (mID == body2.mID); return (m_id == body2.m_id);
} }
// Not equal operator // Not equal operator
inline bool Body::operator!=(const Body& body2) const { inline bool Body::operator!=(const Body& body2) const {
return (mID != body2.mID); return (m_id != body2.m_id);
} }
} }

34
ephysics/body/CollisionBody.cpp
View File

@ -69,7 +69,7 @@ ProxyShape* CollisionBody::addCollisionShape(CollisionShape* collisionShape,
collisionShape->computeAABB(aabb, mTransform * transform); collisionShape->computeAABB(aabb, mTransform * transform);
// Notify the collision detection about this new collision shape // Notify the collision detection about this new collision shape
mWorld.mCollisionDetection.addProxyCollisionShape(proxyShape, aabb); mWorld.m_collisionDetection.addProxyCollisionShape(proxyShape, aabb);
mNbCollisionShapes++; mNbCollisionShapes++;
@ -92,8 +92,8 @@ void CollisionBody::removeCollisionShape(const ProxyShape* proxyShape) {
if (current == proxyShape) { if (current == proxyShape) {
mProxyCollisionShapes = current->mNext; mProxyCollisionShapes = current->mNext;
if (mIsActive) { if (m_isActive) {
mWorld.mCollisionDetection.removeProxyCollisionShape(current); mWorld.m_collisionDetection.removeProxyCollisionShape(current);
} }
current->~ProxyShape(); current->~ProxyShape();
@ -112,8 +112,8 @@ void CollisionBody::removeCollisionShape(const ProxyShape* proxyShape) {
ProxyShape* elementToRemove = current->mNext; ProxyShape* elementToRemove = current->mNext;
current->mNext = elementToRemove->mNext; current->mNext = elementToRemove->mNext;
if (mIsActive) { if (m_isActive) {
mWorld.mCollisionDetection.removeProxyCollisionShape(elementToRemove); mWorld.m_collisionDetection.removeProxyCollisionShape(elementToRemove);
} }
elementToRemove->~ProxyShape(); elementToRemove->~ProxyShape();
@ -138,8 +138,8 @@ void CollisionBody::removeAllCollisionShapes() {
// Remove the proxy collision shape // Remove the proxy collision shape
ProxyShape* nextElement = current->mNext; ProxyShape* nextElement = current->mNext;
if (mIsActive) { if (m_isActive) {
mWorld.mCollisionDetection.removeProxyCollisionShape(current); mWorld.m_collisionDetection.removeProxyCollisionShape(current);
} }
current->~ProxyShape(); current->~ProxyShape();
@ -188,7 +188,7 @@ void CollisionBody::updateProxyShapeInBroadPhase(ProxyShape* proxyShape, bool fo
proxyShape->getCollisionShape()->computeAABB(aabb, mTransform * proxyShape->getLocalToBodyTransform()); proxyShape->getCollisionShape()->computeAABB(aabb, mTransform * proxyShape->getLocalToBodyTransform());
// Update the broad-phase state for the proxy collision shape // Update the broad-phase state for the proxy collision shape
mWorld.mCollisionDetection.updateProxyCollisionShape(proxyShape, aabb, Vector3(0, 0, 0), forceReinsert); mWorld.m_collisionDetection.updateProxyCollisionShape(proxyShape, aabb, Vector3(0, 0, 0), forceReinsert);
} }
// Set whether or not the body is active // Set whether or not the body is active
@ -198,7 +198,7 @@ void CollisionBody::updateProxyShapeInBroadPhase(ProxyShape* proxyShape, bool fo
void CollisionBody::setIsActive(bool isActive) { void CollisionBody::setIsActive(bool isActive) {
// If the state does not change // If the state does not change
if (mIsActive == isActive) return; if (m_isActive == isActive) return;
Body::setIsActive(isActive); Body::setIsActive(isActive);
@ -213,7 +213,7 @@ void CollisionBody::setIsActive(bool isActive) {
shape->getCollisionShape()->computeAABB(aabb, mTransform * shape->mLocalToBodyTransform); shape->getCollisionShape()->computeAABB(aabb, mTransform * shape->mLocalToBodyTransform);
// Add the proxy shape to the collision detection // Add the proxy shape to the collision detection
mWorld.mCollisionDetection.addProxyCollisionShape(shape, aabb); mWorld.m_collisionDetection.addProxyCollisionShape(shape, aabb);
} }
} }
else { // If we have to deactivate the body else { // If we have to deactivate the body
@ -222,7 +222,7 @@ void CollisionBody::setIsActive(bool isActive) {
for (ProxyShape* shape = mProxyCollisionShapes; shape != NULL; shape = shape->mNext) { for (ProxyShape* shape = mProxyCollisionShapes; shape != NULL; shape = shape->mNext) {
// Remove the proxy shape from the collision detection // Remove the proxy shape from the collision detection
mWorld.mCollisionDetection.removeProxyCollisionShape(shape); mWorld.m_collisionDetection.removeProxyCollisionShape(shape);
} }
// Reset the contact manifold list of the body // Reset the contact manifold list of the body
@ -237,23 +237,23 @@ void CollisionBody::askForBroadPhaseCollisionCheck() const {
// For all the proxy collision shapes of the body // For all the proxy collision shapes of the body
for (ProxyShape* shape = mProxyCollisionShapes; shape != NULL; shape = shape->mNext) { for (ProxyShape* shape = mProxyCollisionShapes; shape != NULL; shape = shape->mNext) {
mWorld.mCollisionDetection.askForBroadPhaseCollisionCheck(shape); mWorld.m_collisionDetection.askForBroadPhaseCollisionCheck(shape);
} }
} }
// Reset the mIsAlreadyInIsland variable of the body and contact manifolds. // Reset the m_isAlreadyInIsland variable of the body and contact manifolds.
/// This method also returns the number of contact manifolds of the body. /// This method also returns the number of contact manifolds of the body.
int32_t CollisionBody::resetIsAlreadyInIslandAndCountManifolds() { int32_t CollisionBody::resetIsAlreadyInIslandAndCountManifolds() {
mIsAlreadyInIsland = false; m_isAlreadyInIsland = false;
int32_t nbManifolds = 0; int32_t nbManifolds = 0;
// Reset the mIsAlreadyInIsland variable of the contact manifolds for // Reset the m_isAlreadyInIsland variable of the contact manifolds for
// this body // this body
ContactManifoldListElement* currentElement = mContactManifoldsList; ContactManifoldListElement* currentElement = mContactManifoldsList;
while (currentElement != NULL) { while (currentElement != NULL) {
currentElement->contactManifold->mIsAlreadyInIsland = false; currentElement->contactManifold->m_isAlreadyInIsland = false;
currentElement = currentElement->next; currentElement = currentElement->next;
nbManifolds++; nbManifolds++;
} }
@ -290,7 +290,7 @@ bool CollisionBody::testPointInside(const Vector3& worldPoint) const {
bool CollisionBody::raycast(const Ray& ray, RaycastInfo& raycastInfo) { bool CollisionBody::raycast(const Ray& ray, RaycastInfo& raycastInfo) {
// If the body is not active, it cannot be hit by rays // If the body is not active, it cannot be hit by rays
if (!mIsActive) return false; if (!m_isActive) return false;
bool isHit = false; bool isHit = false;
Ray rayTemp(ray); Ray rayTemp(ray);

2
ephysics/body/CollisionBody.h
View File

@ -88,7 +88,7 @@ class CollisionBody : public Body {
/// (as if the body has moved). /// (as if the body has moved).
void askForBroadPhaseCollisionCheck() const; void askForBroadPhaseCollisionCheck() const;
/// Reset the mIsAlreadyInIsland variable of the body and contact manifolds /// Reset the m_isAlreadyInIsland variable of the body and contact manifolds
int32_t resetIsAlreadyInIslandAndCountManifolds(); int32_t resetIsAlreadyInIslandAndCountManifolds();
public : public :

4
ephysics/body/RigidBody.cpp
View File

@ -215,7 +215,7 @@ ProxyShape* RigidBody::addCollisionShape(CollisionShape* collisionShape,
collisionShape->computeAABB(aabb, mTransform * transform); collisionShape->computeAABB(aabb, mTransform * transform);
// Notify the collision detection about this new collision shape // Notify the collision detection about this new collision shape
mWorld.mCollisionDetection.addProxyCollisionShape(proxyShape, aabb); mWorld.m_collisionDetection.addProxyCollisionShape(proxyShape, aabb);
mNbCollisionShapes++; mNbCollisionShapes++;
@ -389,7 +389,7 @@ void RigidBody::updateBroadPhaseState() const {
shape->getCollisionShape()->computeAABB(aabb, mTransform *shape->getLocalToBodyTransform()); shape->getCollisionShape()->computeAABB(aabb, mTransform *shape->getLocalToBodyTransform());
// Update the broad-phase state for the proxy collision shape // Update the broad-phase state for the proxy collision shape
mWorld.mCollisionDetection.updateProxyCollisionShape(shape, aabb, displacement); mWorld.m_collisionDetection.updateProxyCollisionShape(shape, aabb, displacement);
} }
} }

6
ephysics/body/RigidBody.h
View File

@ -389,7 +389,7 @@ inline void RigidBody::applyForceToCenterOfMass(const Vector3& force) {
if (mType != DYNAMIC) return; if (mType != DYNAMIC) return;
// Awake the body if it was sleeping // Awake the body if it was sleeping
if (mIsSleeping) { if (m_isSleeping) {
setIsSleeping(false); setIsSleeping(false);
} }
@ -414,7 +414,7 @@ inline void RigidBody::applyForce(const Vector3& force, const Vector3& point) {
if (mType != DYNAMIC) return; if (mType != DYNAMIC) return;
// Awake the body if it was sleeping // Awake the body if it was sleeping
if (mIsSleeping) { if (m_isSleeping) {
setIsSleeping(false); setIsSleeping(false);
} }
@ -437,7 +437,7 @@ inline void RigidBody::applyTorque(const Vector3& torque) {
if (mType != DYNAMIC) return; if (mType != DYNAMIC) return;
// Awake the body if it was sleeping // Awake the body if it was sleeping
if (mIsSleeping) { if (m_isSleeping) {
setIsSleeping(false); setIsSleeping(false);
} }

2
ephysics/collision/ContactManifold.cpp
View File

@ -16,7 +16,7 @@ ContactManifold::ContactManifold(ProxyShape* shape1, ProxyShape* shape2,
MemoryAllocator& memoryAllocator, short normalDirectionId) MemoryAllocator& memoryAllocator, short normalDirectionId)
: mShape1(shape1), mShape2(shape2), mNormalDirectionId(normalDirectionId), : mShape1(shape1), mShape2(shape2), mNormalDirectionId(normalDirectionId),
mNbContactPoints(0), mFrictionImpulse1(0.0), mFrictionImpulse2(0.0), mNbContactPoints(0), mFrictionImpulse1(0.0), mFrictionImpulse2(0.0),
mFrictionTwistImpulse(0.0), mIsAlreadyInIsland(false), mFrictionTwistImpulse(0.0), m_isAlreadyInIsland(false),
mMemoryAllocator(memoryAllocator) { mMemoryAllocator(memoryAllocator) {
} }

4
ephysics/collision/ContactManifold.h
View File

@ -102,7 +102,7 @@ class ContactManifold {
Vector3 mRollingResistanceImpulse; Vector3 mRollingResistanceImpulse;
/// True if the contact manifold has already been added int32_to an island /// True if the contact manifold has already been added int32_to an island
bool mIsAlreadyInIsland; bool m_isAlreadyInIsland;
/// Reference to the memory allocator /// Reference to the memory allocator
MemoryAllocator& mMemoryAllocator; MemoryAllocator& mMemoryAllocator;
@ -310,7 +310,7 @@ inline ContactPoint* ContactManifold::getContactPoint(uint32_t index) const {
// Return true if the contact manifold has already been added int32_to an island // Return true if the contact manifold has already been added int32_to an island
inline bool ContactManifold::isAlreadyInIsland() const { inline bool ContactManifold::isAlreadyInIsland() const {
return mIsAlreadyInIsland; return m_isAlreadyInIsland;
} }
// Return the normalized averaged normal vector // Return the normalized averaged normal vector

2
ephysics/collision/ProxyShape.cpp
View File

@ -18,7 +18,7 @@ using namespace reactphysics3d;
*/ */
ProxyShape::ProxyShape(CollisionBody* body, CollisionShape* shape, const Transform& transform, float mass) ProxyShape::ProxyShape(CollisionBody* body, CollisionShape* shape, const Transform& transform, float mass)
:mBody(body), mCollisionShape(shape), mLocalToBodyTransform(transform), mMass(mass), :mBody(body), mCollisionShape(shape), mLocalToBodyTransform(transform), mMass(mass),
mNext(NULL), mBroadPhaseID(-1), mCachedCollisionData(NULL), mUserData(NULL), mNext(NULL), mBroadPhaseID(-1), mCachedCollisionData(NULL), m_userData(NULL),
mCollisionCategoryBits(0x0001), mCollideWithMaskBits(0xFFFF) { mCollisionCategoryBits(0x0001), mCollideWithMaskBits(0xFFFF) {
} }

6
ephysics/collision/ProxyShape.h
View File

@ -49,7 +49,7 @@ class ProxyShape {
void* mCachedCollisionData; void* mCachedCollisionData;
/// Pointer to user data /// Pointer to user data
void* mUserData; void* m_userData;
/// Bits used to define the collision category of this shape. /// Bits used to define the collision category of this shape.
/// You can set a single bit to one to define a category value for this /// You can set a single bit to one to define a category value for this
@ -190,7 +190,7 @@ inline float ProxyShape::getMass() const {
* @return A pointer to the user data stored int32_to the proxy shape * @return A pointer to the user data stored int32_to the proxy shape
*/ */
inline void* ProxyShape::getUserData() const { inline void* ProxyShape::getUserData() const {
return mUserData; return m_userData;
} }
// Attach user data to this body // Attach user data to this body
@ -198,7 +198,7 @@ inline void* ProxyShape::getUserData() const {
* @param userData Pointer to the user data you want to store within the proxy shape * @param userData Pointer to the user data you want to store within the proxy shape
*/ */
inline void ProxyShape::setUserData(void* userData) { inline void ProxyShape::setUserData(void* userData) {
mUserData = userData; m_userData = userData;
} }
// Return the local to parent body transform // Return the local to parent body transform

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

@ -14,27 +14,27 @@ using namespace reactphysics3d;
// Constructor // Constructor
BroadPhaseAlgorithm::BroadPhaseAlgorithm(CollisionDetection& collisionDetection) BroadPhaseAlgorithm::BroadPhaseAlgorithm(CollisionDetection& collisionDetection)
:m_dynamicAABBTree(DYNAMIC_TREE_AABB_GAP), mNbMovedShapes(0), mNbAllocatedMovedShapes(8), :m_dynamicAABBTree(DYNAMIC_TREE_AABB_GAP), m_numberMovedShapes(0), m_numberAllocatedMovedShapes(8),
mNbNonUsedMovedShapes(0), mNbPotentialPairs(0), mNbAllocatedPotentialPairs(8), m_numberNonUsedMovedShapes(0), m_numberPotentialPairs(0), m_numberAllocatedPotentialPairs(8),
mCollisionDetection(collisionDetection) { m_collisionDetection(collisionDetection) {
// Allocate memory for the array of non-static proxy shapes IDs // Allocate memory for the array of non-static proxy shapes IDs
mMovedShapes = (int32_t*) malloc(mNbAllocatedMovedShapes * sizeof(int32_t)); m_movedShapes = (int32_t*) malloc(m_numberAllocatedMovedShapes * sizeof(int32_t));
assert(mMovedShapes != NULL); assert(m_movedShapes != NULL);
// Allocate memory for the array of potential overlapping pairs // Allocate memory for the array of potential overlapping pairs
mPotentialPairs = (BroadPhasePair*) malloc(mNbAllocatedPotentialPairs * sizeof(BroadPhasePair)); m_potentialPairs = (BroadPhasePair*) malloc(m_numberAllocatedPotentialPairs * sizeof(BroadPhasePair));
assert(mPotentialPairs != NULL); assert(m_potentialPairs != NULL);
} }
// Destructor // Destructor
BroadPhaseAlgorithm::~BroadPhaseAlgorithm() { BroadPhaseAlgorithm::~BroadPhaseAlgorithm() {
// Release the memory for the array of non-static proxy shapes IDs // Release the memory for the array of non-static proxy shapes IDs
free(mMovedShapes); free(m_movedShapes);
// Release the memory for the array of potential overlapping pairs // Release the memory for the array of potential overlapping pairs
free(mPotentialPairs); free(m_potentialPairs);
} }
// Add a collision shape in the array of shapes that have moved in the last simulation step // Add a collision shape in the array of shapes that have moved in the last simulation step
@ -42,54 +42,54 @@ BroadPhaseAlgorithm::~BroadPhaseAlgorithm() {
void BroadPhaseAlgorithm::addMovedCollisionShape(int32_t broadPhaseID) { void BroadPhaseAlgorithm::addMovedCollisionShape(int32_t broadPhaseID) {
// Allocate more elements in the array of shapes that have moved if necessary // Allocate more elements in the array of shapes that have moved if necessary
if (mNbAllocatedMovedShapes == mNbMovedShapes) { if (m_numberAllocatedMovedShapes == m_numberMovedShapes) {
mNbAllocatedMovedShapes *= 2; m_numberAllocatedMovedShapes *= 2;
int32_t* oldArray = mMovedShapes; int32_t* oldArray = m_movedShapes;
mMovedShapes = (int32_t*) malloc(mNbAllocatedMovedShapes * sizeof(int32_t)); m_movedShapes = (int32_t*) malloc(m_numberAllocatedMovedShapes * sizeof(int32_t));
assert(mMovedShapes != NULL); assert(m_movedShapes != NULL);
memcpy(mMovedShapes, oldArray, mNbMovedShapes * sizeof(int32_t)); memcpy(m_movedShapes, oldArray, m_numberMovedShapes * sizeof(int32_t));
free(oldArray); free(oldArray);
} }
// Store the broad-phase ID int32_to the array of shapes that have moved // Store the broad-phase ID int32_to the array of shapes that have moved
assert(mNbMovedShapes < mNbAllocatedMovedShapes); assert(m_numberMovedShapes < m_numberAllocatedMovedShapes);
assert(mMovedShapes != NULL); assert(m_movedShapes != NULL);
mMovedShapes[mNbMovedShapes] = broadPhaseID; m_movedShapes[m_numberMovedShapes] = broadPhaseID;
mNbMovedShapes++; m_numberMovedShapes++;
} }
// Remove a collision shape from the array of shapes that have moved in the last simulation step // Remove a collision shape from the array of shapes that have moved in the last simulation step
// and that need to be tested again for broad-phase overlapping. // and that need to be tested again for broad-phase overlapping.
void BroadPhaseAlgorithm::removeMovedCollisionShape(int32_t broadPhaseID) { void BroadPhaseAlgorithm::removeMovedCollisionShape(int32_t broadPhaseID) {
assert(mNbNonUsedMovedShapes <= mNbMovedShapes); assert(m_numberNonUsedMovedShapes <= m_numberMovedShapes);
// If less than the quarter of allocated elements of the non-static shapes IDs array // If less than the quarter of allocated elements of the non-static shapes IDs array
// are used, we release some allocated memory // are used, we release some allocated memory
if ((mNbMovedShapes - mNbNonUsedMovedShapes) < mNbAllocatedMovedShapes / 4 && if ((m_numberMovedShapes - m_numberNonUsedMovedShapes) < m_numberAllocatedMovedShapes / 4 &&
mNbAllocatedMovedShapes > 8) { m_numberAllocatedMovedShapes > 8) {
mNbAllocatedMovedShapes /= 2; m_numberAllocatedMovedShapes /= 2;
int32_t* oldArray = mMovedShapes; int32_t* oldArray = m_movedShapes;
mMovedShapes = (int32_t*) malloc(mNbAllocatedMovedShapes * sizeof(int32_t)); m_movedShapes = (int32_t*) malloc(m_numberAllocatedMovedShapes * sizeof(int32_t));
assert(mMovedShapes != NULL); assert(m_movedShapes != NULL);
uint32_t nbElements = 0; uint32_t nbElements = 0;
for (uint32_t i=0; i<mNbMovedShapes; i++) { for (uint32_t i=0; i<m_numberMovedShapes; i++) {
if (oldArray[i] != -1) { if (oldArray[i] != -1) {
mMovedShapes[nbElements] = oldArray[i]; m_movedShapes[nbElements] = oldArray[i];
nbElements++; nbElements++;
} }
} }
mNbMovedShapes = nbElements; m_numberMovedShapes = nbElements;
mNbNonUsedMovedShapes = 0; m_numberNonUsedMovedShapes = 0;
free(oldArray); free(oldArray);
} }
// Remove the broad-phase ID from the array // Remove the broad-phase ID from the array
for (uint32_t i=0; i<mNbMovedShapes; i++) { for (uint32_t i=0; i<m_numberMovedShapes; i++) {
if (mMovedShapes[i] == broadPhaseID) { if (m_movedShapes[i] == broadPhaseID) {
mMovedShapes[i] = -1; m_movedShapes[i] = -1;
mNbNonUsedMovedShapes++; m_numberNonUsedMovedShapes++;
break; break;
} }
} }
@ -147,12 +147,12 @@ void BroadPhaseAlgorithm::updateProxyCollisionShape(ProxyShape* proxyShape, cons
void BroadPhaseAlgorithm::computeOverlappingPairs() { void BroadPhaseAlgorithm::computeOverlappingPairs() {
// Reset the potential overlapping pairs // Reset the potential overlapping pairs
mNbPotentialPairs = 0; m_numberPotentialPairs = 0;
// For all collision shapes that have moved (or have been created) during the // For all collision shapes that have moved (or have been created) during the
// last simulation step // last simulation step
for (uint32_t i=0; i<mNbMovedShapes; i++) { for (uint32_t i=0; i<m_numberMovedShapes; i++) {
int32_t shapeID = mMovedShapes[i]; int32_t shapeID = m_movedShapes[i];
if (shapeID == -1) continue; if (shapeID == -1) continue;
@ -169,18 +169,18 @@ void BroadPhaseAlgorithm::computeOverlappingPairs() {
// Reset the array of collision shapes that have move (or have been created) during the // Reset the array of collision shapes that have move (or have been created) during the
// last simulation step // last simulation step
mNbMovedShapes = 0; m_numberMovedShapes = 0;
// Sort the array of potential overlapping pairs in order to remove duplicate pairs // Sort the array of potential overlapping pairs in order to remove duplicate pairs
std::sort(mPotentialPairs, mPotentialPairs + mNbPotentialPairs, BroadPhasePair::smallerThan); std::sort(m_potentialPairs, m_potentialPairs + m_numberPotentialPairs, BroadPhasePair::smallerThan);
// Check all the potential overlapping pairs avoiding duplicates to report unique // Check all the potential overlapping pairs avoiding duplicates to report unique
// overlapping pairs // overlapping pairs
uint32_t i=0; uint32_t i=0;
while (i < mNbPotentialPairs) { while (i < m_numberPotentialPairs) {
// Get a potential overlapping pair // Get a potential overlapping pair
BroadPhasePair* pair = mPotentialPairs + i; BroadPhasePair* pair = m_potentialPairs + i;
i++; i++;
assert(pair->collisionShape1ID != pair->collisionShape2ID); assert(pair->collisionShape1ID != pair->collisionShape2ID);
@ -190,13 +190,13 @@ void BroadPhaseAlgorithm::computeOverlappingPairs() {
ProxyShape* shape2 = static_cast<ProxyShape*>(m_dynamicAABBTree.getNodeDataPointer(pair->collisionShape2ID)); ProxyShape* shape2 = static_cast<ProxyShape*>(m_dynamicAABBTree.getNodeDataPointer(pair->collisionShape2ID));
// Notify the collision detection about the overlapping pair // Notify the collision detection about the overlapping pair
mCollisionDetection.broadPhaseNotifyOverlappingPair(shape1, shape2); m_collisionDetection.broadPhaseNotifyOverlappingPair(shape1, shape2);
// Skip the duplicate overlapping pairs // Skip the duplicate overlapping pairs
while (i < mNbPotentialPairs) { while (i < m_numberPotentialPairs) {
// Get the next pair // Get the next pair
BroadPhasePair* nextPair = mPotentialPairs + i; BroadPhasePair* nextPair = m_potentialPairs + i;
// If the next pair is different from the previous one, we stop skipping pairs // If the next pair is different from the previous one, we stop skipping pairs
if (nextPair->collisionShape1ID != pair->collisionShape1ID || if (nextPair->collisionShape1ID != pair->collisionShape1ID ||
@ -209,14 +209,14 @@ void BroadPhaseAlgorithm::computeOverlappingPairs() {
// If the number of potential overlapping pairs is less than the quarter of allocated // If the number of potential overlapping pairs is less than the quarter of allocated
// number of overlapping pairs // number of overlapping pairs
if (mNbPotentialPairs < mNbAllocatedPotentialPairs / 4 && mNbPotentialPairs > 8) { if (m_numberPotentialPairs < m_numberAllocatedPotentialPairs / 4 && m_numberPotentialPairs > 8) {
// Reduce the number of allocated potential overlapping pairs // Reduce the number of allocated potential overlapping pairs
BroadPhasePair* oldPairs = mPotentialPairs; BroadPhasePair* oldPairs = m_potentialPairs;
mNbAllocatedPotentialPairs /= 2; m_numberAllocatedPotentialPairs /= 2;
mPotentialPairs = (BroadPhasePair*) malloc(mNbAllocatedPotentialPairs * sizeof(BroadPhasePair)); m_potentialPairs = (BroadPhasePair*) malloc(m_numberAllocatedPotentialPairs * sizeof(BroadPhasePair));
assert(mPotentialPairs); assert(m_potentialPairs);
memcpy(mPotentialPairs, oldPairs, mNbPotentialPairs * sizeof(BroadPhasePair)); memcpy(m_potentialPairs, oldPairs, m_numberPotentialPairs * sizeof(BroadPhasePair));
free(oldPairs); free(oldPairs);
} }
} }
@ -228,28 +228,28 @@ void BroadPhaseAlgorithm::notifyOverlappingNodes(int32_t node1ID, int32_t node2I
if (node1ID == node2ID) return; if (node1ID == node2ID) return;
// If we need to allocate more memory for the array of potential overlapping pairs // If we need to allocate more memory for the array of potential overlapping pairs
if (mNbPotentialPairs == mNbAllocatedPotentialPairs) { if (m_numberPotentialPairs == m_numberAllocatedPotentialPairs) {
// Allocate more memory for the array of potential pairs // Allocate more memory for the array of potential pairs
BroadPhasePair* oldPairs = mPotentialPairs; BroadPhasePair* oldPairs = m_potentialPairs;
mNbAllocatedPotentialPairs *= 2; m_numberAllocatedPotentialPairs *= 2;
mPotentialPairs = (BroadPhasePair*) malloc(mNbAllocatedPotentialPairs * sizeof(BroadPhasePair)); m_potentialPairs = (BroadPhasePair*) malloc(m_numberAllocatedPotentialPairs * sizeof(BroadPhasePair));
assert(mPotentialPairs); assert(m_potentialPairs);
memcpy(mPotentialPairs, oldPairs, mNbPotentialPairs * sizeof(BroadPhasePair)); memcpy(m_potentialPairs, oldPairs, m_numberPotentialPairs * sizeof(BroadPhasePair));
free(oldPairs); free(oldPairs);
} }
// Add the new potential pair int32_to the array of potential overlapping pairs // Add the new potential pair int32_to the array of potential overlapping pairs
mPotentialPairs[mNbPotentialPairs].collisionShape1ID = std::min(node1ID, node2ID); m_potentialPairs[m_numberPotentialPairs].collisionShape1ID = std::min(node1ID, node2ID);
mPotentialPairs[mNbPotentialPairs].collisionShape2ID = std::max(node1ID, node2ID); m_potentialPairs[m_numberPotentialPairs].collisionShape2ID = std::max(node1ID, node2ID);
mNbPotentialPairs++; m_numberPotentialPairs++;
} }
// 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()
void AABBOverlapCallback::notifyOverlappingNode(int32_t nodeId) { void AABBOverlapCallback::notifyOverlappingNode(int32_t nodeId) {
mBroadPhaseAlgorithm.notifyOverlappingNodes(mReferenceNodeId, nodeId); mBroadPhaseAlgorithm.notifyOverlappingNodes(m_referenceNodeId, nodeId);
} }
// Called for a broad-phase shape that has to be tested for raycast // Called for a broad-phase shape that has to be tested for raycast

20
ephysics/collision/broadphase/BroadPhaseAlgorithm.h
View File

@ -47,13 +47,13 @@ class AABBOverlapCallback : public DynamicAABBTreeOverlapCallback {
BroadPhaseAlgorithm& mBroadPhaseAlgorithm; BroadPhaseAlgorithm& mBroadPhaseAlgorithm;
int32_t mReferenceNodeId; int32_t m_referenceNodeId;
public: public:
// Constructor // Constructor
AABBOverlapCallback(BroadPhaseAlgorithm& broadPhaseAlgo, int32_t referenceNodeId) AABBOverlapCallback(BroadPhaseAlgorithm& broadPhaseAlgo, int32_t referenceNodeId)
: mBroadPhaseAlgorithm(broadPhaseAlgo), mReferenceNodeId(referenceNodeId) { : mBroadPhaseAlgorithm(broadPhaseAlgo), m_referenceNodeId(referenceNodeId) {
} }
@ -113,31 +113,31 @@ class BroadPhaseAlgorithm {
/// Array with the broad-phase IDs of all collision shapes that have moved (or have been /// Array with the broad-phase IDs of all collision shapes that have moved (or have been
/// created) during the last simulation step. Those are the shapes that need to be tested /// created) during the last simulation step. Those are the shapes that need to be tested
/// for overlapping in the next simulation step. /// for overlapping in the next simulation step.
int32_t* mMovedShapes; int32_t* m_movedShapes;
/// Number of collision shapes in the array of shapes that have moved during the last /// Number of collision shapes in the array of shapes that have moved during the last
/// simulation step. /// simulation step.
uint32_t mNbMovedShapes; uint32_t m_numberMovedShapes;
/// Number of allocated elements for the array of shapes that have moved during the last /// Number of allocated elements for the array of shapes that have moved during the last
/// simulation step. /// simulation step.
uint32_t mNbAllocatedMovedShapes; uint32_t m_numberAllocatedMovedShapes;
/// Number of non-used elements in the array of shapes that have moved during the last /// Number of non-used elements in the array of shapes that have moved during the last
/// simulation step. /// simulation step.
uint32_t mNbNonUsedMovedShapes; uint32_t m_numberNonUsedMovedShapes;
/// Temporary array of potential overlapping pairs (with potential duplicates) /// Temporary array of potential overlapping pairs (with potential duplicates)
BroadPhasePair* mPotentialPairs; BroadPhasePair* m_potentialPairs;
/// Number of potential overlapping pairs /// Number of potential overlapping pairs
uint32_t mNbPotentialPairs; uint32_t m_numberPotentialPairs;
/// Number of allocated elements for the array of potential overlapping pairs /// Number of allocated elements for the array of potential overlapping pairs
uint32_t mNbAllocatedPotentialPairs; uint32_t m_numberAllocatedPotentialPairs;
/// Reference to the collision detection object /// Reference to the collision detection object
CollisionDetection& mCollisionDetection; CollisionDetection& m_collisionDetection;
// -------------------- Methods -------------------- // // -------------------- Methods -------------------- //

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

@ -169,27 +169,27 @@ bool DynamicAABBTree::updateObject(int32_t nodeID, const AABB& newAABB, const Ve
// Compute the fat AABB by inflating the AABB with a constant gap // Compute the fat AABB by inflating the AABB with a constant gap
mNodes[nodeID].aabb = newAABB; mNodes[nodeID].aabb = newAABB;
const Vector3 gap(mExtraAABBGap, mExtraAABBGap, mExtraAABBGap); const Vector3 gap(mExtraAABBGap, mExtraAABBGap, mExtraAABBGap);
mNodes[nodeID].aabb.mMinCoordinates -= gap; mNodes[nodeID].aabb.m_minCoordinates -= gap;
mNodes[nodeID].aabb.mMaxCoordinates += gap; mNodes[nodeID].aabb.m_maxCoordinates += gap;
// Inflate the fat AABB in direction of the linear motion of the AABB // Inflate the fat AABB in direction of the linear motion of the AABB
if (displacement.x < float(0.0)) { if (displacement.x < float(0.0)) {
mNodes[nodeID].aabb.mMinCoordinates.x += DYNAMIC_TREE_AABB_LIN_GAP_MULTIPLIER *displacement.x; mNodes[nodeID].aabb.m_minCoordinates.x += DYNAMIC_TREE_AABB_LIN_GAP_MULTIPLIER *displacement.x;
} }
else { else {
mNodes[nodeID].aabb.mMaxCoordinates.x += DYNAMIC_TREE_AABB_LIN_GAP_MULTIPLIER *displacement.x; mNodes[nodeID].aabb.m_maxCoordinates.x += DYNAMIC_TREE_AABB_LIN_GAP_MULTIPLIER *displacement.x;
} }
if (displacement.y < float(0.0)) { if (displacement.y < float(0.0)) {
mNodes[nodeID].aabb.mMinCoordinates.y += DYNAMIC_TREE_AABB_LIN_GAP_MULTIPLIER *displacement.y; mNodes[nodeID].aabb.m_minCoordinates.y += DYNAMIC_TREE_AABB_LIN_GAP_MULTIPLIER *displacement.y;
} }
else { else {
mNodes[nodeID].aabb.mMaxCoordinates.y += DYNAMIC_TREE_AABB_LIN_GAP_MULTIPLIER *displacement.y; mNodes[nodeID].aabb.m_maxCoordinates.y += DYNAMIC_TREE_AABB_LIN_GAP_MULTIPLIER *displacement.y;
} }
if (displacement.z < float(0.0)) { if (displacement.z < float(0.0)) {
mNodes[nodeID].aabb.mMinCoordinates.z += DYNAMIC_TREE_AABB_LIN_GAP_MULTIPLIER *displacement.z; mNodes[nodeID].aabb.m_minCoordinates.z += DYNAMIC_TREE_AABB_LIN_GAP_MULTIPLIER *displacement.z;
} }
else { else {
mNodes[nodeID].aabb.mMaxCoordinates.z += DYNAMIC_TREE_AABB_LIN_GAP_MULTIPLIER *displacement.z; mNodes[nodeID].aabb.m_maxCoordinates.z += DYNAMIC_TREE_AABB_LIN_GAP_MULTIPLIER *displacement.z;
} }
assert(mNodes[nodeID].aabb.contains(newAABB)); assert(mNodes[nodeID].aabb.contains(newAABB));

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

@ -50,7 +50,7 @@ void ConcaveVsConvexAlgorithm::testCollision(const CollisionShapeInfo& shape1Inf
// Set the parameters of the callback object // Set the parameters of the callback object
ConvexVsTriangleCallback convexVsTriangleCallback; ConvexVsTriangleCallback convexVsTriangleCallback;
convexVsTriangleCallback.setCollisionDetection(mCollisionDetection); convexVsTriangleCallback.setCollisionDetection(m_collisionDetection);
convexVsTriangleCallback.setConvexShape(convexShape); convexVsTriangleCallback.setConvexShape(convexShape);
convexVsTriangleCallback.setConcaveShape(concaveShape); convexVsTriangleCallback.setConcaveShape(concaveShape);
convexVsTriangleCallback.setProxyShapes(convexProxyShape, concaveProxyShape); convexVsTriangleCallback.setProxyShapes(convexProxyShape, concaveProxyShape);
@ -92,7 +92,7 @@ void ConvexVsTriangleCallback::testTriangle(const Vector3* trianglePoints) {
TriangleShape triangleShape(trianglePoints[0], trianglePoints[1], trianglePoints[2], margin); TriangleShape triangleShape(trianglePoints[0], trianglePoints[1], trianglePoints[2], margin);
// Select the collision algorithm to use between the triangle and the convex shape // Select the collision algorithm to use between the triangle and the convex shape
NarrowPhaseAlgorithm* algo = mCollisionDetection->getCollisionAlgorithm(triangleShape.getType(), NarrowPhaseAlgorithm* algo = m_collisionDetection->getCollisionAlgorithm(triangleShape.getType(),
mConvexShape->getType()); mConvexShape->getType());
// If there is no collision algorithm between those two kinds of shapes // If there is no collision algorithm between those two kinds of shapes

4
ephysics/collision/narrowphase/ConcaveVsConvexAlgorithm.h
View File

@ -25,7 +25,7 @@ class ConvexVsTriangleCallback : public TriangleCallback {
protected: protected:
/// Pointer to the collision detection object /// Pointer to the collision detection object
CollisionDetection* mCollisionDetection; CollisionDetection* m_collisionDetection;
/// Narrow-phase collision callback /// Narrow-phase collision callback
NarrowPhaseCallback* mNarrowPhaseCallback; NarrowPhaseCallback* mNarrowPhaseCallback;
@ -53,7 +53,7 @@ class ConvexVsTriangleCallback : public TriangleCallback {
/// Set the collision detection pointer /// Set the collision detection pointer
void setCollisionDetection(CollisionDetection* collisionDetection) { void setCollisionDetection(CollisionDetection* collisionDetection) {
mCollisionDetection = collisionDetection; m_collisionDetection = collisionDetection;
} }
/// Set the narrow-phase collision callback /// Set the narrow-phase collision callback

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

@ -23,6 +23,6 @@ NarrowPhaseAlgorithm::~NarrowPhaseAlgorithm() {
// Initalize the algorithm // Initalize the algorithm
void NarrowPhaseAlgorithm::init(CollisionDetection* collisionDetection, MemoryAllocator* memoryAllocator) { void NarrowPhaseAlgorithm::init(CollisionDetection* collisionDetection, MemoryAllocator* memoryAllocator) {
mCollisionDetection = collisionDetection; m_collisionDetection = collisionDetection;
mMemoryAllocator = memoryAllocator; mMemoryAllocator = memoryAllocator;
} }

2
ephysics/collision/narrowphase/NarrowPhaseAlgorithm.h
View File

@ -46,7 +46,7 @@ class NarrowPhaseAlgorithm {
// -------------------- Attributes -------------------- // // -------------------- Attributes -------------------- //
/// Pointer to the collision detection object /// Pointer to the collision detection object
CollisionDetection* mCollisionDetection; CollisionDetection* m_collisionDetection;
/// Pointer to the memory allocator /// Pointer to the memory allocator
MemoryAllocator* mMemoryAllocator; MemoryAllocator* mMemoryAllocator;

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

@ -20,13 +20,13 @@ AABB::AABB() {
// Constructor // Constructor
AABB::AABB(const Vector3& minCoordinates, const Vector3& maxCoordinates) AABB::AABB(const Vector3& minCoordinates, const Vector3& maxCoordinates)
:mMinCoordinates(minCoordinates), mMaxCoordinates(maxCoordinates) { :m_minCoordinates(minCoordinates), m_maxCoordinates(maxCoordinates) {
} }
// Copy-constructor // Copy-constructor
AABB::AABB(const AABB& aabb) AABB::AABB(const AABB& aabb)
: mMinCoordinates(aabb.mMinCoordinates), mMaxCoordinates(aabb.mMaxCoordinates) { : m_minCoordinates(aabb.m_minCoordinates), m_maxCoordinates(aabb.m_maxCoordinates) {
} }
@ -37,37 +37,37 @@ AABB::~AABB() {
// Merge the AABB in parameter with the current one // Merge the AABB in parameter with the current one
void AABB::mergeWithAABB(const AABB& aabb) { void AABB::mergeWithAABB(const AABB& aabb) {
mMinCoordinates.x = std::min(mMinCoordinates.x, aabb.mMinCoordinates.x); m_minCoordinates.x = std::min(m_minCoordinates.x, aabb.m_minCoordinates.x);
mMinCoordinates.y = std::min(mMinCoordinates.y, aabb.mMinCoordinates.y); m_minCoordinates.y = std::min(m_minCoordinates.y, aabb.m_minCoordinates.y);
mMinCoordinates.z = std::min(mMinCoordinates.z, aabb.mMinCoordinates.z); m_minCoordinates.z = std::min(m_minCoordinates.z, aabb.m_minCoordinates.z);
mMaxCoordinates.x = std::max(mMaxCoordinates.x, aabb.mMaxCoordinates.x); m_maxCoordinates.x = std::max(m_maxCoordinates.x, aabb.m_maxCoordinates.x);
mMaxCoordinates.y = std::max(mMaxCoordinates.y, aabb.mMaxCoordinates.y); m_maxCoordinates.y = std::max(m_maxCoordinates.y, aabb.m_maxCoordinates.y);
mMaxCoordinates.z = std::max(mMaxCoordinates.z, aabb.mMaxCoordinates.z); m_maxCoordinates.z = std::max(m_maxCoordinates.z, aabb.m_maxCoordinates.z);
} }
// Replace the current AABB with a new AABB that is the union of two AABBs in parameters // Replace the current AABB with a new AABB that is the union of two AABBs in parameters
void AABB::mergeTwoAABBs(const AABB& aabb1, const AABB& aabb2) { void AABB::mergeTwoAABBs(const AABB& aabb1, const AABB& aabb2) {
mMinCoordinates.x = std::min(aabb1.mMinCoordinates.x, aabb2.mMinCoordinates.x); m_minCoordinates.x = std::min(aabb1.m_minCoordinates.x, aabb2.m_minCoordinates.x);
mMinCoordinates.y = std::min(aabb1.mMinCoordinates.y, aabb2.mMinCoordinates.y); m_minCoordinates.y = std::min(aabb1.m_minCoordinates.y, aabb2.m_minCoordinates.y);
mMinCoordinates.z = std::min(aabb1.mMinCoordinates.z, aabb2.mMinCoordinates.z); m_minCoordinates.z = std::min(aabb1.m_minCoordinates.z, aabb2.m_minCoordinates.z);
mMaxCoordinates.x = std::max(aabb1.mMaxCoordinates.x, aabb2.mMaxCoordinates.x); m_maxCoordinates.x = std::max(aabb1.m_maxCoordinates.x, aabb2.m_maxCoordinates.x);
mMaxCoordinates.y = std::max(aabb1.mMaxCoordinates.y, aabb2.mMaxCoordinates.y); m_maxCoordinates.y = std::max(aabb1.m_maxCoordinates.y, aabb2.m_maxCoordinates.y);
mMaxCoordinates.z = std::max(aabb1.mMaxCoordinates.z, aabb2.mMaxCoordinates.z); m_maxCoordinates.z = std::max(aabb1.m_maxCoordinates.z, aabb2.m_maxCoordinates.z);
} }
// Return true if the current AABB contains the AABB given in parameter // Return true if the current AABB contains the AABB given in parameter
bool AABB::contains(const AABB& aabb) const { bool AABB::contains(const AABB& aabb) const {
bool isInside = true; bool isInside = true;
isInside = isInside && mMinCoordinates.x <= aabb.mMinCoordinates.x; isInside = isInside && m_minCoordinates.x <= aabb.m_minCoordinates.x;
isInside = isInside && mMinCoordinates.y <= aabb.mMinCoordinates.y; isInside = isInside && m_minCoordinates.y <= aabb.m_minCoordinates.y;
isInside = isInside && mMinCoordinates.z <= aabb.mMinCoordinates.z; isInside = isInside && m_minCoordinates.z <= aabb.m_minCoordinates.z;
isInside = isInside && mMaxCoordinates.x >= aabb.mMaxCoordinates.x; isInside = isInside && m_maxCoordinates.x >= aabb.m_maxCoordinates.x;
isInside = isInside && mMaxCoordinates.y >= aabb.mMaxCoordinates.y; isInside = isInside && m_maxCoordinates.y >= aabb.m_maxCoordinates.y;
isInside = isInside && mMaxCoordinates.z >= aabb.mMaxCoordinates.z; isInside = isInside && m_maxCoordinates.z >= aabb.m_maxCoordinates.z;
return isInside; return isInside;
} }
@ -102,9 +102,9 @@ AABB AABB::createAABBForTriangle(const Vector3* trianglePoints) {
bool AABB::testRayIntersect(const Ray& ray) const { bool AABB::testRayIntersect(const Ray& ray) const {
const Vector3 point2 = ray.point1 + ray.maxFraction * (ray.point2 - ray.point1); const Vector3 point2 = ray.point1 + ray.maxFraction * (ray.point2 - ray.point1);
const Vector3 e = mMaxCoordinates - mMinCoordinates; const Vector3 e = m_maxCoordinates - m_minCoordinates;
const Vector3 d = point2 - ray.point1; const Vector3 d = point2 - ray.point1;
const Vector3 m = ray.point1 + point2 - mMinCoordinates - mMaxCoordinates; const Vector3 m = ray.point1 + point2 - m_minCoordinates - m_maxCoordinates;
// Test if the AABB face normals are separating axis // Test if the AABB face normals are separating axis
float adx = std::abs(d.x); float adx = std::abs(d.x);

56
ephysics/collision/shapes/AABB.h
View File

@ -25,10 +25,10 @@ class AABB {
// -------------------- Attributes -------------------- // // -------------------- Attributes -------------------- //
/// Minimum world coordinates of the AABB on the x,y and z axis /// Minimum world coordinates of the AABB on the x,y and z axis
Vector3 mMinCoordinates; Vector3 m_minCoordinates;
/// Maximum world coordinates of the AABB on the x,y and z axis /// Maximum world coordinates of the AABB on the x,y and z axis
Vector3 mMaxCoordinates; Vector3 m_maxCoordinates;
public : public :
@ -104,68 +104,68 @@ class AABB {
// Return the center point of the AABB in world coordinates // Return the center point of the AABB in world coordinates
inline Vector3 AABB::getCenter() const { inline Vector3 AABB::getCenter() const {
return (mMinCoordinates + mMaxCoordinates) * float(0.5); return (m_minCoordinates + m_maxCoordinates) * float(0.5);
} }
// Return the minimum coordinates of the AABB // Return the minimum coordinates of the AABB
inline const Vector3& AABB::getMin() const { inline const Vector3& AABB::getMin() const {
return mMinCoordinates; return m_minCoordinates;
} }
// Set the minimum coordinates of the AABB // Set the minimum coordinates of the AABB
inline void AABB::setMin(const Vector3& min) { inline void AABB::setMin(const Vector3& min) {
mMinCoordinates = min; m_minCoordinates = min;
} }
// Return the maximum coordinates of the AABB // Return the maximum coordinates of the AABB
inline const Vector3& AABB::getMax() const { inline const Vector3& AABB::getMax() const {
return mMaxCoordinates; return m_maxCoordinates;
} }
// Set the maximum coordinates of the AABB // Set the maximum coordinates of the AABB
inline void AABB::setMax(const Vector3& max) { inline void AABB::setMax(const Vector3& max) {
mMaxCoordinates = max; m_maxCoordinates = max;
} }
// Return the size of the AABB in the three dimension x, y and z // Return the size of the AABB in the three dimension x, y and z
inline Vector3 AABB::getExtent() const { inline Vector3 AABB::getExtent() const {
return mMaxCoordinates - mMinCoordinates; return m_maxCoordinates - m_minCoordinates;
} }
// Inflate each side of the AABB by a given size // Inflate each side of the AABB by a given size
inline void AABB::inflate(float dx, float dy, float dz) { inline void AABB::inflate(float dx, float dy, float dz) {
mMaxCoordinates += Vector3(dx, dy, dz); m_maxCoordinates += Vector3(dx, dy, dz);
mMinCoordinates -= Vector3(dx, dy, dz); m_minCoordinates -= Vector3(dx, dy, dz);
} }
// Return true if the current AABB is overlapping with the AABB in argument. // Return true if the current AABB is overlapping with the AABB in argument.
/// Two AABBs overlap if they overlap in the three x, y and z axis at the same time /// Two AABBs overlap if they overlap in the three x, y and z axis at the same time
inline bool AABB::testCollision(const AABB& aabb) const { inline bool AABB::testCollision(const AABB& aabb) const {
if (mMaxCoordinates.x < aabb.mMinCoordinates.x || if (m_maxCoordinates.x < aabb.m_minCoordinates.x ||
aabb.mMaxCoordinates.x < mMinCoordinates.x) return false; aabb.m_maxCoordinates.x < m_minCoordinates.x) return false;
if (mMaxCoordinates.y < aabb.mMinCoordinates.y || if (m_maxCoordinates.y < aabb.m_minCoordinates.y ||
aabb.mMaxCoordinates.y < mMinCoordinates.y) return false; aabb.m_maxCoordinates.y < m_minCoordinates.y) return false;
if (mMaxCoordinates.z < aabb.mMinCoordinates.z|| if (m_maxCoordinates.z < aabb.m_minCoordinates.z||
aabb.mMaxCoordinates.z < mMinCoordinates.z) return false; aabb.m_maxCoordinates.z < m_minCoordinates.z) return false;
return true; return true;
} }
// Return the volume of the AABB // Return the volume of the AABB
inline float AABB::getVolume() const { inline float AABB::getVolume() const {
const Vector3 diff = mMaxCoordinates - mMinCoordinates; const Vector3 diff = m_maxCoordinates - m_minCoordinates;
return (diff.x * diff.y * diff.z); return (diff.x * diff.y * diff.z);
} }
// Return true if the AABB of a triangle int32_tersects the AABB // Return true if the AABB of a triangle int32_tersects the AABB
inline bool AABB::testCollisionTriangleAABB(const Vector3* trianglePoints) const { inline bool AABB::testCollisionTriangleAABB(const Vector3* trianglePoints) const {
if (min3(trianglePoints[0].x, trianglePoints[1].x, trianglePoints[2].x) > mMaxCoordinates.x) return false; if (min3(trianglePoints[0].x, trianglePoints[1].x, trianglePoints[2].x) > m_maxCoordinates.x) return false;
if (min3(trianglePoints[0].y, trianglePoints[1].y, trianglePoints[2].y) > mMaxCoordinates.y) return false; if (min3(trianglePoints[0].y, trianglePoints[1].y, trianglePoints[2].y) > m_maxCoordinates.y) return false;
if (min3(trianglePoints[0].z, trianglePoints[1].z, trianglePoints[2].z) > mMaxCoordinates.z) return false; if (min3(trianglePoints[0].z, trianglePoints[1].z, trianglePoints[2].z) > m_maxCoordinates.z) return false;
if (max3(trianglePoints[0].x, trianglePoints[1].x, trianglePoints[2].x) < mMinCoordinates.x) return false; if (max3(trianglePoints[0].x, trianglePoints[1].x, trianglePoints[2].x) < m_minCoordinates.x) return false;
if (max3(trianglePoints[0].y, trianglePoints[1].y, trianglePoints[2].y) < mMinCoordinates.y) return false; if (max3(trianglePoints[0].y, trianglePoints[1].y, trianglePoints[2].y) < m_minCoordinates.y) return false;
if (max3(trianglePoints[0].z, trianglePoints[1].z, trianglePoints[2].z) < mMinCoordinates.z) return false; if (max3(trianglePoints[0].z, trianglePoints[1].z, trianglePoints[2].z) < m_minCoordinates.z) return false;
return true; return true;
} }
@ -173,16 +173,16 @@ inline bool AABB::testCollisionTriangleAABB(const Vector3* trianglePoints) const
// Return true if a point is inside the AABB // Return true if a point is inside the AABB
inline bool AABB::contains(const Vector3& point) const { inline bool AABB::contains(const Vector3& point) const {
return (point.x >= mMinCoordinates.x - MACHINE_EPSILON && point.x <= mMaxCoordinates.x + MACHINE_EPSILON && return (point.x >= m_minCoordinates.x - MACHINE_EPSILON && point.x <= m_maxCoordinates.x + MACHINE_EPSILON &&
point.y >= mMinCoordinates.y - MACHINE_EPSILON && point.y <= mMaxCoordinates.y + MACHINE_EPSILON && point.y >= m_minCoordinates.y - MACHINE_EPSILON && point.y <= m_maxCoordinates.y + MACHINE_EPSILON &&
point.z >= mMinCoordinates.z - MACHINE_EPSILON && point.z <= mMaxCoordinates.z + MACHINE_EPSILON); point.z >= m_minCoordinates.z - MACHINE_EPSILON && point.z <= m_maxCoordinates.z + MACHINE_EPSILON);
} }
// Assignment operator // Assignment operator
inline AABB& AABB::operator=(const AABB& aabb) { inline AABB& AABB::operator=(const AABB& aabb) {
if (this != &aabb) { if (this != &aabb) {
mMinCoordinates = aabb.mMinCoordinates; m_minCoordinates = aabb.m_minCoordinates;
mMaxCoordinates = aabb.mMaxCoordinates; m_maxCoordinates = aabb.m_maxCoordinates;
} }
return *this; return *this;
} }

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

@ -19,7 +19,7 @@ using namespace reactphysics3d;
* @param margin The collision margin (in meters) around the collision shape * @param margin The collision margin (in meters) around the collision shape
*/ */
BoxShape::BoxShape(const Vector3& extent, float margin) BoxShape::BoxShape(const Vector3& extent, float margin)
: ConvexShape(BOX, margin), mExtent(extent - Vector3(margin, margin, margin)) { : ConvexShape(BOX, margin), m_extent(extent - Vector3(margin, margin, margin)) {
assert(extent.x > float(0.0) && extent.x > margin); assert(extent.x > float(0.0) && extent.x > margin);
assert(extent.y > float(0.0) && extent.y > margin); assert(extent.y > float(0.0) && extent.y > margin);
assert(extent.z > float(0.0) && extent.z > margin); assert(extent.z > float(0.0) && extent.z > margin);
@ -38,7 +38,7 @@ BoxShape::~BoxShape() {
*/ */
void BoxShape::computeLocalInertiaTensor(Matrix3x3& tensor, float mass) const { void BoxShape::computeLocalInertiaTensor(Matrix3x3& tensor, float mass) const {
float factor = (float(1.0) / float(3.0)) * mass; float factor = (float(1.0) / float(3.0)) * mass;
Vector3 realExtent = mExtent + Vector3(mMargin, mMargin, mMargin); Vector3 realExtent = m_extent + Vector3(mMargin, mMargin, mMargin);
float xSquare = realExtent.x * realExtent.x; float xSquare = realExtent.x * realExtent.x;
float ySquare = realExtent.y * realExtent.y; float ySquare = realExtent.y * realExtent.y;
float zSquare = realExtent.z * realExtent.z; float zSquare = realExtent.z * realExtent.z;
@ -63,17 +63,17 @@ bool BoxShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* pro
if (std::abs(rayDirection[i]) < MACHINE_EPSILON) { if (std::abs(rayDirection[i]) < MACHINE_EPSILON) {
// If the ray's origin is not inside the slab, there is no hit // If the ray's origin is not inside the slab, there is no hit
if (ray.point1[i] > mExtent[i] || ray.point1[i] < -mExtent[i]) return false; if (ray.point1[i] > m_extent[i] || ray.point1[i] < -m_extent[i]) return false;
} }
else { else {
// Compute the int32_tersection of the ray with the near and far plane of the slab // Compute the int32_tersection of the ray with the near and far plane of the slab
float oneOverD = float(1.0) / rayDirection[i]; float oneOverD = float(1.0) / rayDirection[i];
float t1 = (-mExtent[i] - ray.point1[i]) * oneOverD; float t1 = (-m_extent[i] - ray.point1[i]) * oneOverD;
float t2 = (mExtent[i] - ray.point1[i]) * oneOverD; float t2 = (m_extent[i] - ray.point1[i]) * oneOverD;
currentNormal[0] = (i == 0) ? -mExtent[i] : float(0.0); currentNormal[0] = (i == 0) ? -m_extent[i] : float(0.0);
currentNormal[1] = (i == 1) ? -mExtent[i] : float(0.0); currentNormal[1] = (i == 1) ? -m_extent[i] : float(0.0);
currentNormal[2] = (i == 2) ? -mExtent[i] : float(0.0); currentNormal[2] = (i == 2) ? -m_extent[i] : float(0.0);
// Swap t1 and t2 if need so that t1 is int32_tersection with near plane and // Swap t1 and t2 if need so that t1 is int32_tersection with near plane and
// t2 with far plane // t2 with far plane

20
ephysics/collision/shapes/BoxShape.h
View File

@ -36,7 +36,7 @@ class BoxShape : public ConvexShape {
// -------------------- Attributes -------------------- // // -------------------- Attributes -------------------- //
/// Extent sizes of the box in the x, y and z direction /// Extent sizes of the box in the x, y and z direction
Vector3 mExtent; Vector3 m_extent;
// -------------------- Methods -------------------- // // -------------------- Methods -------------------- //
@ -87,13 +87,13 @@ class BoxShape : public ConvexShape {
* @return The vector with the three extents of the box shape (in meters) * @return The vector with the three extents of the box shape (in meters)
*/ */
inline Vector3 BoxShape::getExtent() const { inline Vector3 BoxShape::getExtent() const {
return mExtent + Vector3(mMargin, mMargin, mMargin); return m_extent + Vector3(mMargin, mMargin, mMargin);
} }
// Set the scaling vector of the collision shape // Set the scaling vector of the collision shape
inline void BoxShape::setLocalScaling(const Vector3& scaling) { inline void BoxShape::setLocalScaling(const Vector3& scaling) {
mExtent = (mExtent / mScaling) * scaling; m_extent = (m_extent / mScaling) * scaling;
CollisionShape::setLocalScaling(scaling); CollisionShape::setLocalScaling(scaling);
} }
@ -107,7 +107,7 @@ inline void BoxShape::setLocalScaling(const Vector3& scaling) {
inline void BoxShape::getLocalBounds(Vector3& min, Vector3& max) const { inline void BoxShape::getLocalBounds(Vector3& min, Vector3& max) const {
// Maximum bounds // Maximum bounds
max = mExtent + Vector3(mMargin, mMargin, mMargin); max = m_extent + Vector3(mMargin, mMargin, mMargin);
// Minimum bounds // Minimum bounds
min = -max; min = -max;
@ -122,16 +122,16 @@ inline size_t BoxShape::getSizeInBytes() const {
inline Vector3 BoxShape::getLocalSupportPointWithoutMargin(const Vector3& direction, inline Vector3 BoxShape::getLocalSupportPointWithoutMargin(const Vector3& direction,
void** cachedCollisionData) const { void** cachedCollisionData) const {
return Vector3(direction.x < 0.0 ? -mExtent.x : mExtent.x, return Vector3(direction.x < 0.0 ? -m_extent.x : m_extent.x,
direction.y < 0.0 ? -mExtent.y : mExtent.y, direction.y < 0.0 ? -m_extent.y : m_extent.y,
direction.z < 0.0 ? -mExtent.z : mExtent.z); direction.z < 0.0 ? -m_extent.z : m_extent.z);
} }
// Return true if a point is inside the collision shape // Return true if a point is inside the collision shape
inline bool BoxShape::testPointInside(const Vector3& localPoint, ProxyShape* proxyShape) const { inline bool BoxShape::testPointInside(const Vector3& localPoint, ProxyShape* proxyShape) const {
return (localPoint.x < mExtent[0] && localPoint.x > -mExtent[0] && return (localPoint.x < m_extent[0] && localPoint.x > -m_extent[0] &&
localPoint.y < mExtent[1] && localPoint.y > -mExtent[1] && localPoint.y < m_extent[1] && localPoint.y > -m_extent[1] &&
localPoint.z < mExtent[2] && localPoint.z > -mExtent[2]); localPoint.z < m_extent[2] && localPoint.z > -m_extent[2]);
} }
} }

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

@ -244,6 +244,6 @@ void ConvexMeshShape::recalculateBounds() {
// Raycast method with feedback information // Raycast method with feedback information
bool ConvexMeshShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape) const { bool ConvexMeshShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape) const {
return proxyShape->mBody->mWorld.mCollisionDetection.mNarrowPhaseGJKAlgorithm.raycast( return proxyShape->mBody->mWorld.m_collisionDetection.mNarrowPhaseGJKAlgorithm.raycast(
ray, proxyShape, raycastInfo); ray, proxyShape, raycastInfo);
} }

2
ephysics/collision/shapes/ConvexMeshShape.h
View File

@ -235,7 +235,7 @@ inline bool ConvexMeshShape::testPointInside(const Vector3& localPoint,
ProxyShape* proxyShape) const { ProxyShape* proxyShape) const {
// Use the GJK algorithm to test if the point is inside the convex mesh // Use the GJK algorithm to test if the point is inside the convex mesh
return proxyShape->mBody->mWorld.mCollisionDetection. return proxyShape->mBody->mWorld.m_collisionDetection.
mNarrowPhaseGJKAlgorithm.testPointInside(localPoint, proxyShape); mNarrowPhaseGJKAlgorithm.testPointInside(localPoint, proxyShape);
} }

90
ephysics/constraint/BallAndSocketJoint.cpp
View File

@ -15,11 +15,11 @@ const float BallAndSocketJoint::BETA = float(0.2);
// Constructor // Constructor
BallAndSocketJoint::BallAndSocketJoint(const BallAndSocketJointInfo& jointInfo) BallAndSocketJoint::BallAndSocketJoint(const BallAndSocketJointInfo& jointInfo)
: Joint(jointInfo), mImpulse(Vector3(0, 0, 0)) { : Joint(jointInfo), m_impulse(Vector3(0, 0, 0)) {
// Compute the local-space anchor point for each body // Compute the local-space anchor point for each body
mLocalAnchorPointBody1 = mBody1->getTransform().getInverse() * jointInfo.anchorPointWorldSpace; m_localAnchorPointBody1 = mBody1->getTransform().getInverse() * jointInfo.m_m_m_m_anchorPointWorldSpace;
mLocalAnchorPointBody2 = mBody2->getTransform().getInverse() * jointInfo.anchorPointWorldSpace; m_localAnchorPointBody2 = mBody2->getTransform().getInverse() * jointInfo.m_m_m_m_anchorPointWorldSpace;
} }
// Destructor // Destructor
@ -41,43 +41,43 @@ void BallAndSocketJoint::initBeforeSolve(const ConstraintSolverData& constraintS
const Quaternion& orientationBody2 = mBody2->getTransform().getOrientation(); const Quaternion& orientationBody2 = mBody2->getTransform().getOrientation();
// Get the inertia tensor of bodies // Get the inertia tensor of bodies
mI1 = mBody1->getInertiaTensorInverseWorld(); m_i1 = mBody1->getInertiaTensorInverseWorld();
mI2 = mBody2->getInertiaTensorInverseWorld(); m_i2 = mBody2->getInertiaTensorInverseWorld();
// Compute the vector from body center to the anchor point in world-space // Compute the vector from body center to the anchor point in world-space
mR1World = orientationBody1 * mLocalAnchorPointBody1; m_r1World = orientationBody1 * m_localAnchorPointBody1;
mR2World = orientationBody2 * mLocalAnchorPointBody2; m_r2World = orientationBody2 * m_localAnchorPointBody2;
// Compute the corresponding skew-symmetric matrices // Compute the corresponding skew-symmetric matrices
Matrix3x3 skewSymmetricMatrixU1= Matrix3x3::computeSkewSymmetricMatrixForCrossProduct(mR1World); Matrix3x3 skewSymmetricMatrixU1= Matrix3x3::computeSkewSymmetricMatrixForCrossProduct(m_r1World);
Matrix3x3 skewSymmetricMatrixU2= Matrix3x3::computeSkewSymmetricMatrixForCrossProduct(mR2World); Matrix3x3 skewSymmetricMatrixU2= Matrix3x3::computeSkewSymmetricMatrixForCrossProduct(m_r2World);
// Compute the matrix K=JM^-1J^t (3x3 matrix) // Compute the matrix K=JM^-1J^t (3x3 matrix)
float inverseMassBodies = mBody1->mMassInverse + mBody2->mMassInverse; float inverseMassBodies = mBody1->mMassInverse + mBody2->mMassInverse;
Matrix3x3 massMatrix = Matrix3x3(inverseMassBodies, 0, 0, Matrix3x3 massMatrix = Matrix3x3(inverseMassBodies, 0, 0,
0, inverseMassBodies, 0, 0, inverseMassBodies, 0,
0, 0, inverseMassBodies) + 0, 0, inverseMassBodies) +
skewSymmetricMatrixU1 * mI1 * skewSymmetricMatrixU1.getTranspose() + skewSymmetricMatrixU1 * m_i1 * skewSymmetricMatrixU1.getTranspose() +
skewSymmetricMatrixU2 * mI2 * skewSymmetricMatrixU2.getTranspose(); skewSymmetricMatrixU2 * m_i2 * skewSymmetricMatrixU2.getTranspose();
// Compute the inverse mass matrix K^-1 // Compute the inverse mass matrix K^-1
mInverseMassMatrix.setToZero(); m_inverseMassMatrix.setToZero();
if (mBody1->getType() == DYNAMIC || mBody2->getType() == DYNAMIC) { if (mBody1->getType() == DYNAMIC || mBody2->getType() == DYNAMIC) {
mInverseMassMatrix = massMatrix.getInverse(); m_inverseMassMatrix = massMatrix.getInverse();
} }
// Compute the bias "b" of the constraint // Compute the bias "b" of the constraint
mBiasVector.setToZero(); m_biasVector.setToZero();
if (mPositionCorrectionTechnique == BAUMGARTE_JOINTS) { if (mPositionCorrectionTechnique == BAUMGARTE_JOINTS) {
float biasFactor = (BETA / constraintSolverData.timeStep); float biasFactor = (BETA / constraintSolverData.timeStep);
mBiasVector = biasFactor * (x2 + mR2World - x1 - mR1World); m_biasVector = biasFactor * (x2 + m_r2World - x1 - m_r1World);
} }
// If warm-starting is not enabled // If warm-starting is not enabled
if (!constraintSolverData.isWarmStartingActive) { if (!constraintSolverData.isWarmStartingActive) {
// Reset the accumulated impulse // Reset the accumulated impulse
mImpulse.setToZero(); m_impulse.setToZero();
} }
} }
@ -91,19 +91,19 @@ void BallAndSocketJoint::warmstart(const ConstraintSolverData& constraintSolverD
Vector3& w2 = constraintSolverData.angularVelocities[mIndexBody2]; Vector3& w2 = constraintSolverData.angularVelocities[mIndexBody2];
// Compute the impulse P=J^T * lambda for the body 1 // Compute the impulse P=J^T * lambda for the body 1
const Vector3 linearImpulseBody1 = -mImpulse; const Vector3 linearImpulseBody1 = -m_impulse;
const Vector3 angularImpulseBody1 = mImpulse.cross(mR1World); const Vector3 angularImpulseBody1 = m_impulse.cross(m_r1World);
// Apply the impulse to the body 1 // Apply the impulse to the body 1
v1 += mBody1->mMassInverse * linearImpulseBody1; v1 += mBody1->mMassInverse * linearImpulseBody1;
w1 += mI1 * angularImpulseBody1; w1 += m_i1 * angularImpulseBody1;
// Compute the impulse P=J^T * lambda for the body 2 // Compute the impulse P=J^T * lambda for the body 2
const Vector3 angularImpulseBody2 = -mImpulse.cross(mR2World); const Vector3 angularImpulseBody2 = -m_impulse.cross(m_r2World);
// Apply the impulse to the body to the body 2 // Apply the impulse to the body to the body 2
v2 += mBody2->mMassInverse * mImpulse; v2 += mBody2->mMassInverse * m_impulse;
w2 += mI2 * angularImpulseBody2; w2 += m_i2 * angularImpulseBody2;
} }
// Solve the velocity constraint // Solve the velocity constraint
@ -116,26 +116,26 @@ void BallAndSocketJoint::solveVelocityConstraint(const ConstraintSolverData& con
Vector3& w2 = constraintSolverData.angularVelocities[mIndexBody2]; Vector3& w2 = constraintSolverData.angularVelocities[mIndexBody2];
// Compute J*v // Compute J*v
const Vector3 Jv = v2 + w2.cross(mR2World) - v1 - w1.cross(mR1World); const Vector3 Jv = v2 + w2.cross(m_r2World) - v1 - w1.cross(m_r1World);
// Compute the Lagrange multiplier lambda // Compute the Lagrange multiplier lambda
const Vector3 deltaLambda = mInverseMassMatrix * (-Jv - mBiasVector); const Vector3 deltaLambda = m_inverseMassMatrix * (-Jv - m_biasVector);
mImpulse += deltaLambda; m_impulse += deltaLambda;
// Compute the impulse P=J^T * lambda for the body 1 // Compute the impulse P=J^T * lambda for the body 1
const Vector3 linearImpulseBody1 = -deltaLambda; const Vector3 linearImpulseBody1 = -deltaLambda;
const Vector3 angularImpulseBody1 = deltaLambda.cross(mR1World); const Vector3 angularImpulseBody1 = deltaLambda.cross(m_r1World);
// Apply the impulse to the body 1 // Apply the impulse to the body 1
v1 += mBody1->mMassInverse * linearImpulseBody1; v1 += mBody1->mMassInverse * linearImpulseBody1;
w1 += mI1 * angularImpulseBody1; w1 += m_i1 * angularImpulseBody1;
// Compute the impulse P=J^T * lambda for the body 2 // Compute the impulse P=J^T * lambda for the body 2
const Vector3 angularImpulseBody2 = -deltaLambda.cross(mR2World); const Vector3 angularImpulseBody2 = -deltaLambda.cross(m_r2World);
// Apply the impulse to the body 2 // Apply the impulse to the body 2
v2 += mBody2->mMassInverse * deltaLambda; v2 += mBody2->mMassInverse * deltaLambda;
w2 += mI2 * angularImpulseBody2; w2 += m_i2 * angularImpulseBody2;
} }
// Solve the position constraint (for position error correction) // Solve the position constraint (for position error correction)
@ -156,44 +156,44 @@ void BallAndSocketJoint::solvePositionConstraint(const ConstraintSolverData& con
float inverseMassBody2 = mBody2->mMassInverse; float inverseMassBody2 = mBody2->mMassInverse;
// Recompute the inverse inertia tensors // Recompute the inverse inertia tensors
mI1 = mBody1->getInertiaTensorInverseWorld(); m_i1 = mBody1->getInertiaTensorInverseWorld();
mI2 = mBody2->getInertiaTensorInverseWorld(); m_i2 = mBody2->getInertiaTensorInverseWorld();
// Compute the vector from body center to the anchor point in world-space // Compute the vector from body center to the anchor point in world-space
mR1World = q1 * mLocalAnchorPointBody1; m_r1World = q1 * m_localAnchorPointBody1;
mR2World = q2 * mLocalAnchorPointBody2; m_r2World = q2 * m_localAnchorPointBody2;
// Compute the corresponding skew-symmetric matrices // Compute the corresponding skew-symmetric matrices
Matrix3x3 skewSymmetricMatrixU1= Matrix3x3::computeSkewSymmetricMatrixForCrossProduct(mR1World); Matrix3x3 skewSymmetricMatrixU1= Matrix3x3::computeSkewSymmetricMatrixForCrossProduct(m_r1World);
Matrix3x3 skewSymmetricMatrixU2= Matrix3x3::computeSkewSymmetricMatrixForCrossProduct(mR2World); Matrix3x3 skewSymmetricMatrixU2= Matrix3x3::computeSkewSymmetricMatrixForCrossProduct(m_r2World);
// Recompute the inverse mass matrix K=J^TM^-1J of of the 3 translation constraints // Recompute the inverse mass matrix K=J^TM^-1J of of the 3 translation constraints
float inverseMassBodies = inverseMassBody1 + inverseMassBody2; float inverseMassBodies = inverseMassBody1 + inverseMassBody2;
Matrix3x3 massMatrix = Matrix3x3(inverseMassBodies, 0, 0, Matrix3x3 massMatrix = Matrix3x3(inverseMassBodies, 0, 0,
0, inverseMassBodies, 0, 0, inverseMassBodies, 0,
0, 0, inverseMassBodies) + 0, 0, inverseMassBodies) +
skewSymmetricMatrixU1 * mI1 * skewSymmetricMatrixU1.getTranspose() + skewSymmetricMatrixU1 * m_i1 * skewSymmetricMatrixU1.getTranspose() +
skewSymmetricMatrixU2 * mI2 * skewSymmetricMatrixU2.getTranspose(); skewSymmetricMatrixU2 * m_i2 * skewSymmetricMatrixU2.getTranspose();
mInverseMassMatrix.setToZero(); m_inverseMassMatrix.setToZero();
if (mBody1->getType() == DYNAMIC || mBody2->getType() == DYNAMIC) { if (mBody1->getType() == DYNAMIC || mBody2->getType() == DYNAMIC) {
mInverseMassMatrix = massMatrix.getInverse(); m_inverseMassMatrix = massMatrix.getInverse();
} }
// Compute the constraint error (value of the C(x) function) // Compute the constraint error (value of the C(x) function)
const Vector3 constraintError = (x2 + mR2World - x1 - mR1World); const Vector3 constraintError = (x2 + m_r2World - x1 - m_r1World);
// Compute the Lagrange multiplier lambda // Compute the Lagrange multiplier lambda
// TODO : Do not solve the system by computing the inverse each time and multiplying with the // TODO : Do not solve the system by computing the inverse each time and multiplying with the
// right-hand side vector but instead use a method to directly solve the linear system. // right-hand side vector but instead use a method to directly solve the linear system.
const Vector3 lambda = mInverseMassMatrix * (-constraintError); const Vector3 lambda = m_inverseMassMatrix * (-constraintError);
// Compute the impulse of body 1 // Compute the impulse of body 1
const Vector3 linearImpulseBody1 = -lambda; const Vector3 linearImpulseBody1 = -lambda;
const Vector3 angularImpulseBody1 = lambda.cross(mR1World); const Vector3 angularImpulseBody1 = lambda.cross(m_r1World);
// Compute the pseudo velocity of body 1 // Compute the pseudo velocity of body 1
const Vector3 v1 = inverseMassBody1 * linearImpulseBody1; const Vector3 v1 = inverseMassBody1 * linearImpulseBody1;
const Vector3 w1 = mI1 * angularImpulseBody1; const Vector3 w1 = m_i1 * angularImpulseBody1;
// Update the body center of mass and orientation of body 1 // Update the body center of mass and orientation of body 1
x1 += v1; x1 += v1;
@ -201,11 +201,11 @@ void BallAndSocketJoint::solvePositionConstraint(const ConstraintSolverData& con
q1.normalize(); q1.normalize();
// Compute the impulse of body 2 // Compute the impulse of body 2
const Vector3 angularImpulseBody2 = -lambda.cross(mR2World); const Vector3 angularImpulseBody2 = -lambda.cross(m_r2World);
// Compute the pseudo velocity of body 2 // Compute the pseudo velocity of body 2
const Vector3 v2 = inverseMassBody2 * lambda; const Vector3 v2 = inverseMassBody2 * lambda;
const Vector3 w2 = mI2 * angularImpulseBody2; const Vector3 w2 = m_i2 * angularImpulseBody2;
// Update the body position/orientation of body 2 // Update the body position/orientation of body 2
x2 += v2; x2 += v2;

22
ephysics/constraint/BallAndSocketJoint.h
View File

@ -23,7 +23,7 @@ struct BallAndSocketJointInfo : public JointInfo {
// -------------------- Attributes -------------------- // // -------------------- Attributes -------------------- //
/// Anchor point (in world-space coordinates) /// Anchor point (in world-space coordinates)
Vector3 anchorPointWorldSpace; Vector3 m_m_m_m_anchorPointWorldSpace;
/// Constructor /// Constructor
/** /**
@ -35,7 +35,7 @@ struct BallAndSocketJointInfo : public JointInfo {
BallAndSocketJointInfo(RigidBody* rigidBody1, RigidBody* rigidBody2, BallAndSocketJointInfo(RigidBody* rigidBody1, RigidBody* rigidBody2,
const Vector3& initAnchorPointWorldSpace) const Vector3& initAnchorPointWorldSpace)
: JointInfo(rigidBody1, rigidBody2, BALLSOCKETJOINT), : JointInfo(rigidBody1, rigidBody2, BALLSOCKETJOINT),
anchorPointWorldSpace(initAnchorPointWorldSpace) {} m_m_m_m_anchorPointWorldSpace(initAnchorPointWorldSpace) {}
}; };
// Class BallAndSocketJoint // Class BallAndSocketJoint
@ -56,31 +56,31 @@ class BallAndSocketJoint : public Joint {
// -------------------- Attributes -------------------- // // -------------------- Attributes -------------------- //
/// Anchor point of body 1 (in local-space coordinates of body 1) /// Anchor point of body 1 (in local-space coordinates of body 1)
Vector3 mLocalAnchorPointBody1; Vector3 m_localAnchorPointBody1;
/// Anchor point of body 2 (in local-space coordinates of body 2) /// Anchor point of body 2 (in local-space coordinates of body 2)
Vector3 mLocalAnchorPointBody2; Vector3 m_localAnchorPointBody2;
/// Vector from center of body 2 to anchor point in world-space /// Vector from center of body 2 to anchor point in world-space
Vector3 mR1World; Vector3 m_r1World;
/// Vector from center of body 2 to anchor point in world-space /// Vector from center of body 2 to anchor point in world-space
Vector3 mR2World; Vector3 m_r2World;
/// Inertia tensor of body 1 (in world-space coordinates) /// Inertia tensor of body 1 (in world-space coordinates)
Matrix3x3 mI1; Matrix3x3 m_i1;
/// Inertia tensor of body 2 (in world-space coordinates) /// Inertia tensor of body 2 (in world-space coordinates)
Matrix3x3 mI2; Matrix3x3 m_i2;
/// Bias vector for the constraint /// Bias vector for the constraint
Vector3 mBiasVector; Vector3 m_biasVector;
/// Inverse mass matrix K=JM^-1J^-t of the constraint /// Inverse mass matrix K=JM^-1J^-t of the constraint
Matrix3x3 mInverseMassMatrix; Matrix3x3 m_inverseMassMatrix;
/// Accumulated impulse /// Accumulated impulse
Vector3 mImpulse; Vector3 m_impulse;
// -------------------- Methods -------------------- // // -------------------- Methods -------------------- //

116
ephysics/constraint/FixedJoint.cpp
View File

@ -15,13 +15,13 @@ const float FixedJoint::BETA = float(0.2);
// Constructor // Constructor
FixedJoint::FixedJoint(const FixedJointInfo& jointInfo) FixedJoint::FixedJoint(const FixedJointInfo& jointInfo)
: Joint(jointInfo), mImpulseTranslation(0, 0, 0), mImpulseRotation(0, 0, 0) { : Joint(jointInfo), m_impulseTranslation(0, 0, 0), m_impulseRotation(0, 0, 0) {
// Compute the local-space anchor point for each body // Compute the local-space anchor point for each body
const Transform& transform1 = mBody1->getTransform(); const Transform& transform1 = mBody1->getTransform();
const Transform& transform2 = mBody2->getTransform(); const Transform& transform2 = mBody2->getTransform();
mLocalAnchorPointBody1 = transform1.getInverse() * jointInfo.anchorPointWorldSpace; m_localAnchorPointBody1 = transform1.getInverse() * jointInfo.m_m_m_m_anchorPointWorldSpace;
mLocalAnchorPointBody2 = transform2.getInverse() * jointInfo.anchorPointWorldSpace; m_localAnchorPointBody2 = transform2.getInverse() * jointInfo.m_m_m_m_anchorPointWorldSpace;
// Compute the inverse of the initial orientation difference between the two bodies // Compute the inverse of the initial orientation difference between the two bodies
mInitOrientationDifferenceInv = transform2.getOrientation() * mInitOrientationDifferenceInv = transform2.getOrientation() *
@ -49,43 +49,43 @@ void FixedJoint::initBeforeSolve(const ConstraintSolverData& constraintSolverDat
const Quaternion& orientationBody2 = mBody2->getTransform().getOrientation(); const Quaternion& orientationBody2 = mBody2->getTransform().getOrientation();
// Get the inertia tensor of bodies // Get the inertia tensor of bodies
mI1 = mBody1->getInertiaTensorInverseWorld(); m_i1 = mBody1->getInertiaTensorInverseWorld();
mI2 = mBody2->getInertiaTensorInverseWorld(); m_i2 = mBody2->getInertiaTensorInverseWorld();
// Compute the vector from body center to the anchor point in world-space // Compute the vector from body center to the anchor point in world-space
mR1World = orientationBody1 * mLocalAnchorPointBody1; m_r1World = orientationBody1 * m_localAnchorPointBody1;
mR2World = orientationBody2 * mLocalAnchorPointBody2; m_r2World = orientationBody2 * m_localAnchorPointBody2;
// Compute the corresponding skew-symmetric matrices // Compute the corresponding skew-symmetric matrices
Matrix3x3 skewSymmetricMatrixU1= Matrix3x3::computeSkewSymmetricMatrixForCrossProduct(mR1World); Matrix3x3 skewSymmetricMatrixU1= Matrix3x3::computeSkewSymmetricMatrixForCrossProduct(m_r1World);
Matrix3x3 skewSymmetricMatrixU2= Matrix3x3::computeSkewSymmetricMatrixForCrossProduct(mR2World); Matrix3x3 skewSymmetricMatrixU2= Matrix3x3::computeSkewSymmetricMatrixForCrossProduct(m_r2World);
// Compute the matrix K=JM^-1J^t (3x3 matrix) for the 3 translation constraints // Compute the matrix K=JM^-1J^t (3x3 matrix) for the 3 translation constraints
float inverseMassBodies = mBody1->mMassInverse + mBody2->mMassInverse; float inverseMassBodies = mBody1->mMassInverse + mBody2->mMassInverse;
Matrix3x3 massMatrix = Matrix3x3(inverseMassBodies, 0, 0, Matrix3x3 massMatrix = Matrix3x3(inverseMassBodies, 0, 0,
0, inverseMassBodies, 0, 0, inverseMassBodies, 0,
0, 0, inverseMassBodies) + 0, 0, inverseMassBodies) +
skewSymmetricMatrixU1 * mI1 * skewSymmetricMatrixU1.getTranspose() + skewSymmetricMatrixU1 * m_i1 * skewSymmetricMatrixU1.getTranspose() +
skewSymmetricMatrixU2 * mI2 * skewSymmetricMatrixU2.getTranspose(); skewSymmetricMatrixU2 * m_i2 * skewSymmetricMatrixU2.getTranspose();
// Compute the inverse mass matrix K^-1 for the 3 translation constraints // Compute the inverse mass matrix K^-1 for the 3 translation constraints
mInverseMassMatrixTranslation.setToZero(); m_inverseMassMatrixTranslation.setToZero();
if (mBody1->getType() == DYNAMIC || mBody2->getType() == DYNAMIC) { if (mBody1->getType() == DYNAMIC || mBody2->getType() == DYNAMIC) {
mInverseMassMatrixTranslation = massMatrix.getInverse(); m_inverseMassMatrixTranslation = massMatrix.getInverse();
} }
// Compute the bias "b" of the constraint for the 3 translation constraints // Compute the bias "b" of the constraint for the 3 translation constraints
float biasFactor = (BETA / constraintSolverData.timeStep); float biasFactor = (BETA / constraintSolverData.timeStep);
mBiasTranslation.setToZero(); mBiasTranslation.setToZero();
if (mPositionCorrectionTechnique == BAUMGARTE_JOINTS) { if (mPositionCorrectionTechnique == BAUMGARTE_JOINTS) {
mBiasTranslation = biasFactor * (x2 + mR2World - x1 - mR1World); mBiasTranslation = biasFactor * (x2 + m_r2World - x1 - m_r1World);
} }
// 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
// contraints (3x3 matrix) // contraints (3x3 matrix)
mInverseMassMatrixRotation = mI1 + mI2; m_inverseMassMatrixRotation = m_i1 + m_i2;
if (mBody1->getType() == DYNAMIC || mBody2->getType() == DYNAMIC) { if (mBody1->getType() == DYNAMIC || mBody2->getType() == DYNAMIC) {
mInverseMassMatrixRotation = mInverseMassMatrixRotation.getInverse(); m_inverseMassMatrixRotation = m_inverseMassMatrixRotation.getInverse();
} }
// Compute the bias "b" for the 3 rotation constraints // Compute the bias "b" for the 3 rotation constraints
@ -101,8 +101,8 @@ void FixedJoint::initBeforeSolve(const ConstraintSolverData& constraintSolverDat
if (!constraintSolverData.isWarmStartingActive) { if (!constraintSolverData.isWarmStartingActive) {
// Reset the accumulated impulses // Reset the accumulated impulses
mImpulseTranslation.setToZero(); m_impulseTranslation.setToZero();
mImpulseRotation.setToZero(); m_impulseRotation.setToZero();
} }
} }
@ -120,25 +120,25 @@ void FixedJoint::warmstart(const ConstraintSolverData& constraintSolverData) {
const float inverseMassBody2 = mBody2->mMassInverse; const float inverseMassBody2 = mBody2->mMassInverse;
// Compute the impulse P=J^T * lambda for the 3 translation constraints for body 1 // Compute the impulse P=J^T * lambda for the 3 translation constraints for body 1
Vector3 linearImpulseBody1 = -mImpulseTranslation; Vector3 linearImpulseBody1 = -m_impulseTranslation;
Vector3 angularImpulseBody1 = mImpulseTranslation.cross(mR1World); Vector3 angularImpulseBody1 = m_impulseTranslation.cross(m_r1World);
// Compute the impulse P=J^T * lambda for the 3 rotation constraints for body 1 // Compute the impulse P=J^T * lambda for the 3 rotation constraints for body 1
angularImpulseBody1 += -mImpulseRotation; angularImpulseBody1 += -m_impulseRotation;
// Apply the impulse to the body 1 // Apply the impulse to the body 1
v1 += inverseMassBody1 * linearImpulseBody1; v1 += inverseMassBody1 * linearImpulseBody1;
w1 += mI1 * angularImpulseBody1; w1 += m_i1 * angularImpulseBody1;
// Compute the impulse P=J^T * lambda for the 3 translation constraints for body 2 // Compute the impulse P=J^T * lambda for the 3 translation constraints for body 2
Vector3 angularImpulseBody2 = -mImpulseTranslation.cross(mR2World); Vector3 angularImpulseBody2 = -m_impulseTranslation.cross(m_r2World);
// Compute the impulse P=J^T * lambda for the 3 rotation constraints for body 2 // Compute the impulse P=J^T * lambda for the 3 rotation constraints for body 2
angularImpulseBody2 += mImpulseRotation; angularImpulseBody2 += m_impulseRotation;
// Apply the impulse to the body 2 // Apply the impulse to the body 2
v2 += inverseMassBody2 * mImpulseTranslation; v2 += inverseMassBody2 * m_impulseTranslation;
w2 += mI2 * angularImpulseBody2; w2 += m_i2 * angularImpulseBody2;
} }
// Solve the velocity constraint // Solve the velocity constraint
@ -157,27 +157,27 @@ void FixedJoint::solveVelocityConstraint(const ConstraintSolverData& constraintS
// --------------- Translation Constraints --------------- // // --------------- Translation Constraints --------------- //
// Compute J*v for the 3 translation constraints // Compute J*v for the 3 translation constraints
const Vector3 JvTranslation = v2 + w2.cross(mR2World) - v1 - w1.cross(mR1World); const Vector3 JvTranslation = v2 + w2.cross(m_r2World) - v1 - w1.cross(m_r1World);
// Compute the Lagrange multiplier lambda // Compute the Lagrange multiplier lambda
const Vector3 deltaLambda = mInverseMassMatrixTranslation * const Vector3 deltaLambda = m_inverseMassMatrixTranslation *
(-JvTranslation - mBiasTranslation); (-JvTranslation - mBiasTranslation);
mImpulseTranslation += deltaLambda; m_impulseTranslation += deltaLambda;
// Compute the impulse P=J^T * lambda for body 1 // Compute the impulse P=J^T * lambda for body 1
const Vector3 linearImpulseBody1 = -deltaLambda; const Vector3 linearImpulseBody1 = -deltaLambda;
Vector3 angularImpulseBody1 = deltaLambda.cross(mR1World); Vector3 angularImpulseBody1 = deltaLambda.cross(m_r1World);
// Apply the impulse to the body 1 // Apply the impulse to the body 1
v1 += inverseMassBody1 * linearImpulseBody1; v1 += inverseMassBody1 * linearImpulseBody1;
w1 += mI1 * angularImpulseBody1; w1 += m_i1 * angularImpulseBody1;
// Compute the impulse P=J^T * lambda for body 2 // Compute the impulse P=J^T * lambda for body 2
const Vector3 angularImpulseBody2 = -deltaLambda.cross(mR2World); const Vector3 angularImpulseBody2 = -deltaLambda.cross(m_r2World);
// Apply the impulse to the body 2 // Apply the impulse to the body 2
v2 += inverseMassBody2 * deltaLambda; v2 += inverseMassBody2 * deltaLambda;
w2 += mI2 * angularImpulseBody2; w2 += m_i2 * angularImpulseBody2;
// --------------- Rotation Constraints --------------- // // --------------- Rotation Constraints --------------- //
@ -185,17 +185,17 @@ void FixedJoint::solveVelocityConstraint(const ConstraintSolverData& constraintS
const Vector3 JvRotation = w2 - w1; const Vector3 JvRotation = w2 - w1;
// Compute the Lagrange multiplier lambda for the 3 rotation constraints // Compute the Lagrange multiplier lambda for the 3 rotation constraints
Vector3 deltaLambda2 = mInverseMassMatrixRotation * (-JvRotation - mBiasRotation); Vector3 deltaLambda2 = m_inverseMassMatrixRotation * (-JvRotation - mBiasRotation);
mImpulseRotation += deltaLambda2; m_impulseRotation += deltaLambda2;
// Compute the impulse P=J^T * lambda for the 3 rotation constraints for body 1 // Compute the impulse P=J^T * lambda for the 3 rotation constraints for body 1
angularImpulseBody1 = -deltaLambda2; angularImpulseBody1 = -deltaLambda2;
// Apply the impulse to the body 1 // Apply the impulse to the body 1
w1 += mI1 * angularImpulseBody1; w1 += m_i1 * angularImpulseBody1;
// Apply the impulse to the body 2 // Apply the impulse to the body 2
w2 += mI2 * deltaLambda2; w2 += m_i2 * deltaLambda2;
} }
// Solve the position constraint (for position error correction) // Solve the position constraint (for position error correction)
@ -216,16 +216,16 @@ void FixedJoint::solvePositionConstraint(const ConstraintSolverData& constraintS
float inverseMassBody2 = mBody2->mMassInverse; float inverseMassBody2 = mBody2->mMassInverse;
// Recompute the inverse inertia tensors // Recompute the inverse inertia tensors
mI1 = mBody1->getInertiaTensorInverseWorld(); m_i1 = mBody1->getInertiaTensorInverseWorld();
mI2 = mBody2->getInertiaTensorInverseWorld(); m_i2 = mBody2->getInertiaTensorInverseWorld();
// Compute the vector from body center to the anchor point in world-space // Compute the vector from body center to the anchor point in world-space
mR1World = q1 * mLocalAnchorPointBody1; m_r1World = q1 * m_localAnchorPointBody1;
mR2World = q2 * mLocalAnchorPointBody2; m_r2World = q2 * m_localAnchorPointBody2;
// Compute the corresponding skew-symmetric matrices // Compute the corresponding skew-symmetric matrices
Matrix3x3 skewSymmetricMatrixU1= Matrix3x3::computeSkewSymmetricMatrixForCrossProduct(mR1World); Matrix3x3 skewSymmetricMatrixU1= Matrix3x3::computeSkewSymmetricMatrixForCrossProduct(m_r1World);
Matrix3x3 skewSymmetricMatrixU2= Matrix3x3::computeSkewSymmetricMatrixForCrossProduct(mR2World); Matrix3x3 skewSymmetricMatrixU2= Matrix3x3::computeSkewSymmetricMatrixForCrossProduct(m_r2World);
// --------------- Translation Constraints --------------- // // --------------- Translation Constraints --------------- //
@ -234,26 +234,26 @@ void FixedJoint::solvePositionConstraint(const ConstraintSolverData& constraintS
Matrix3x3 massMatrix = Matrix3x3(inverseMassBodies, 0, 0, Matrix3x3 massMatrix = Matrix3x3(inverseMassBodies, 0, 0,
0, inverseMassBodies, 0, 0, inverseMassBodies, 0,
0, 0, inverseMassBodies) + 0, 0, inverseMassBodies) +
skewSymmetricMatrixU1 * mI1 * skewSymmetricMatrixU1.getTranspose() + skewSymmetricMatrixU1 * m_i1 * skewSymmetricMatrixU1.getTranspose() +
skewSymmetricMatrixU2 * mI2 * skewSymmetricMatrixU2.getTranspose(); skewSymmetricMatrixU2 * m_i2 * skewSymmetricMatrixU2.getTranspose();
mInverseMassMatrixTranslation.setToZero(); m_inverseMassMatrixTranslation.setToZero();
if (mBody1->getType() == DYNAMIC || mBody2->getType() == DYNAMIC) { if (mBody1->getType() == DYNAMIC || mBody2->getType() == DYNAMIC) {
mInverseMassMatrixTranslation = massMatrix.getInverse(); m_inverseMassMatrixTranslation = massMatrix.getInverse();
} }
// Compute position error for the 3 translation constraints // Compute position error for the 3 translation constraints
const Vector3 errorTranslation = x2 + mR2World - x1 - mR1World; const Vector3 errorTranslation = x2 + m_r2World - x1 - m_r1World;
// Compute the Lagrange multiplier lambda // Compute the Lagrange multiplier lambda
const Vector3 lambdaTranslation = mInverseMassMatrixTranslation * (-errorTranslation); const Vector3 lambdaTranslation = m_inverseMassMatrixTranslation * (-errorTranslation);
// Compute the impulse of body 1 // Compute the impulse of body 1
Vector3 linearImpulseBody1 = -lambdaTranslation; Vector3 linearImpulseBody1 = -lambdaTranslation;
Vector3 angularImpulseBody1 = lambdaTranslation.cross(mR1World); Vector3 angularImpulseBody1 = lambdaTranslation.cross(m_r1World);
// Compute the pseudo velocity of body 1 // Compute the pseudo velocity of body 1
const Vector3 v1 = inverseMassBody1 * linearImpulseBody1; const Vector3 v1 = inverseMassBody1 * linearImpulseBody1;
Vector3 w1 = mI1 * angularImpulseBody1; Vector3 w1 = m_i1 * angularImpulseBody1;
// Update the body position/orientation of body 1 // Update the body position/orientation of body 1
x1 += v1; x1 += v1;
@ -261,11 +261,11 @@ void FixedJoint::solvePositionConstraint(const ConstraintSolverData& constraintS
q1.normalize(); q1.normalize();
// Compute the impulse of body 2 // Compute the impulse of body 2
Vector3 angularImpulseBody2 = -lambdaTranslation.cross(mR2World); Vector3 angularImpulseBody2 = -lambdaTranslation.cross(m_r2World);
// Compute the pseudo velocity of body 2 // Compute the pseudo velocity of body 2
const Vector3 v2 = inverseMassBody2 * lambdaTranslation; const Vector3 v2 = inverseMassBody2 * lambdaTranslation;
Vector3 w2 = mI2 * angularImpulseBody2; Vector3 w2 = m_i2 * angularImpulseBody2;
// Update the body position/orientation of body 2 // Update the body position/orientation of body 2
x2 += v2; x2 += v2;
@ -276,9 +276,9 @@ void FixedJoint::solvePositionConstraint(const ConstraintSolverData& constraintS
// 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
// contraints (3x3 matrix) // contraints (3x3 matrix)
mInverseMassMatrixRotation = mI1 + mI2; m_inverseMassMatrixRotation = m_i1 + m_i2;
if (mBody1->getType() == DYNAMIC || mBody2->getType() == DYNAMIC) { if (mBody1->getType() == DYNAMIC || mBody2->getType() == DYNAMIC) {
mInverseMassMatrixRotation = mInverseMassMatrixRotation.getInverse(); m_inverseMassMatrixRotation = m_inverseMassMatrixRotation.getInverse();
} }
// Compute the position error for the 3 rotation constraints // Compute the position error for the 3 rotation constraints
@ -288,20 +288,20 @@ void FixedJoint::solvePositionConstraint(const ConstraintSolverData& constraintS
const Vector3 errorRotation = float(2.0) * qError.getVectorV(); const Vector3 errorRotation = float(2.0) * qError.getVectorV();
// Compute the Lagrange multiplier lambda for the 3 rotation constraints // Compute the Lagrange multiplier lambda for the 3 rotation constraints
Vector3 lambdaRotation = mInverseMassMatrixRotation * (-errorRotation); Vector3 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 = -lambdaRotation; angularImpulseBody1 = -lambdaRotation;
// Compute the pseudo velocity of body 1 // Compute the pseudo velocity of body 1
w1 = mI1 * angularImpulseBody1; w1 = m_i1 * angularImpulseBody1;
// Update the body position/orientation of body 1 // Update the body position/orientation of body 1
q1 += Quaternion(0, w1) * q1 * float(0.5); q1 += Quaternion(0, w1) * q1 * float(0.5);
q1.normalize(); q1.normalize();
// Compute the pseudo velocity of body 2 // Compute the pseudo velocity of body 2
w2 = mI2 * lambdaRotation; w2 = m_i2 * lambdaRotation;
// Update the body position/orientation of body 2 // Update the body position/orientation of body 2
q2 += Quaternion(0, w2) * q2 * float(0.5); q2 += Quaternion(0, w2) * q2 * float(0.5);

24
ephysics/constraint/FixedJoint.h
View File

@ -23,7 +23,7 @@ struct FixedJointInfo : public JointInfo {
// -------------------- Attributes -------------------- // // -------------------- Attributes -------------------- //
/// Anchor point (in world-space coordinates) /// Anchor point (in world-space coordinates)
Vector3 anchorPointWorldSpace; Vector3 m_m_m_m_anchorPointWorldSpace;
/// Constructor /// Constructor
/** /**
@ -35,7 +35,7 @@ struct FixedJointInfo : public JointInfo {
FixedJointInfo(RigidBody* rigidBody1, RigidBody* rigidBody2, FixedJointInfo(RigidBody* rigidBody1, RigidBody* rigidBody2,
const Vector3& initAnchorPointWorldSpace) const Vector3& initAnchorPointWorldSpace)
: JointInfo(rigidBody1, rigidBody2, FIXEDJOINT), : JointInfo(rigidBody1, rigidBody2, FIXEDJOINT),
anchorPointWorldSpace(initAnchorPointWorldSpace){} m_m_m_m_anchorPointWorldSpace(initAnchorPointWorldSpace){}
}; };
// Class FixedJoint // Class FixedJoint
@ -55,34 +55,34 @@ class FixedJoint : public Joint {
// -------------------- Attributes -------------------- // // -------------------- Attributes -------------------- //
/// Anchor point of body 1 (in local-space coordinates of body 1) /// Anchor point of body 1 (in local-space coordinates of body 1)
Vector3 mLocalAnchorPointBody1; Vector3 m_localAnchorPointBody1;
/// Anchor point of body 2 (in local-space coordinates of body 2) /// Anchor point of body 2 (in local-space coordinates of body 2)
Vector3 mLocalAnchorPointBody2; Vector3 m_localAnchorPointBody2;
/// Vector from center of body 2 to anchor point in world-space /// Vector from center of body 2 to anchor point in world-space
Vector3 mR1World; Vector3 m_r1World;
/// Vector from center of body 2 to anchor point in world-space /// Vector from center of body 2 to anchor point in world-space
Vector3 mR2World; Vector3 m_r2World;
/// Inertia tensor of body 1 (in world-space coordinates) /// Inertia tensor of body 1 (in world-space coordinates)
Matrix3x3 mI1; Matrix3x3 m_i1;
/// Inertia tensor of body 2 (in world-space coordinates) /// Inertia tensor of body 2 (in world-space coordinates)
Matrix3x3 mI2; Matrix3x3 m_i2;
/// Accumulated impulse for the 3 translation constraints /// Accumulated impulse for the 3 translation constraints
Vector3 mImpulseTranslation; Vector3 m_impulseTranslation;
/// Accumulate impulse for the 3 rotation constraints /// Accumulate impulse for the 3 rotation constraints
Vector3 mImpulseRotation; Vector3 m_impulseRotation;
/// Inverse mass matrix K=JM^-1J^-t of the 3 translation constraints (3x3 matrix) /// Inverse mass matrix K=JM^-1J^-t of the 3 translation constraints (3x3 matrix)
Matrix3x3 mInverseMassMatrixTranslation; Matrix3x3 m_inverseMassMatrixTranslation;
/// Inverse mass matrix K=JM^-1J^-t of the 3 rotation constraints (3x3 matrix) /// Inverse mass matrix K=JM^-1J^-t of the 3 rotation constraints (3x3 matrix)
Matrix3x3 mInverseMassMatrixRotation; Matrix3x3 m_inverseMassMatrixRotation;
/// Bias vector for the 3 translation constraints /// Bias vector for the 3 translation constraints
Vector3 mBiasTranslation; Vector3 mBiasTranslation;

212
ephysics/constraint/HingeJoint.cpp
View File

@ -16,8 +16,8 @@ const float HingeJoint::BETA = float(0.2);
// Constructor // Constructor
HingeJoint::HingeJoint(const HingeJointInfo& jointInfo) HingeJoint::HingeJoint(const HingeJointInfo& jointInfo)
: Joint(jointInfo), mImpulseTranslation(0, 0, 0), mImpulseRotation(0, 0), : Joint(jointInfo), m_impulseTranslation(0, 0, 0), m_impulseRotation(0, 0),
mImpulseLowerLimit(0), mImpulseUpperLimit(0), mImpulseMotor(0), m_impulseLowerLimit(0), m_impulseUpperLimit(0), m_impulseMotor(0),
mIsLimitEnabled(jointInfo.isLimitEnabled), mIsMotorEnabled(jointInfo.isMotorEnabled), mIsLimitEnabled(jointInfo.isLimitEnabled), mIsMotorEnabled(jointInfo.isMotorEnabled),
mLowerLimit(jointInfo.minAngleLimit), mUpperLimit(jointInfo.maxAngleLimit), mLowerLimit(jointInfo.minAngleLimit), mUpperLimit(jointInfo.maxAngleLimit),
mIsLowerLimitViolated(false), mIsUpperLimitViolated(false), mIsLowerLimitViolated(false), mIsUpperLimitViolated(false),
@ -29,8 +29,8 @@ HingeJoint::HingeJoint(const HingeJointInfo& jointInfo)
// Compute the local-space anchor point for each body // Compute the local-space anchor point for each body
Transform transform1 = mBody1->getTransform(); Transform transform1 = mBody1->getTransform();
Transform transform2 = mBody2->getTransform(); Transform transform2 = mBody2->getTransform();
mLocalAnchorPointBody1 = transform1.getInverse() * jointInfo.anchorPointWorldSpace; m_localAnchorPointBody1 = transform1.getInverse() * jointInfo.m_m_m_m_anchorPointWorldSpace;
mLocalAnchorPointBody2 = transform2.getInverse() * jointInfo.anchorPointWorldSpace; m_localAnchorPointBody2 = transform2.getInverse() * jointInfo.m_m_m_m_anchorPointWorldSpace;
// Compute the local-space hinge axis // Compute the local-space hinge axis
mHingeLocalAxisBody1 = transform1.getOrientation().getInverse() * jointInfo.rotationAxisWorld; mHingeLocalAxisBody1 = transform1.getOrientation().getInverse() * jointInfo.rotationAxisWorld;
@ -64,12 +64,12 @@ void HingeJoint::initBeforeSolve(const ConstraintSolverData& constraintSolverDat
const Quaternion& orientationBody2 = mBody2->getTransform().getOrientation(); const Quaternion& orientationBody2 = mBody2->getTransform().getOrientation();
// Get the inertia tensor of bodies // Get the inertia tensor of bodies
mI1 = mBody1->getInertiaTensorInverseWorld(); m_i1 = mBody1->getInertiaTensorInverseWorld();
mI2 = mBody2->getInertiaTensorInverseWorld(); m_i2 = mBody2->getInertiaTensorInverseWorld();
// Compute the vector from body center to the anchor point in world-space // Compute the vector from body center to the anchor point in world-space
mR1World = orientationBody1 * mLocalAnchorPointBody1; m_r1World = orientationBody1 * m_localAnchorPointBody1;
mR2World = orientationBody2 * mLocalAnchorPointBody2; m_r2World = orientationBody2 * m_localAnchorPointBody2;
// Compute the current angle around the hinge axis // Compute the current angle around the hinge axis
float hingeAngle = computeCurrentHingeAngle(orientationBody1, orientationBody2); float hingeAngle = computeCurrentHingeAngle(orientationBody1, orientationBody2);
@ -80,12 +80,12 @@ void HingeJoint::initBeforeSolve(const ConstraintSolverData& constraintSolverDat
bool oldIsLowerLimitViolated = mIsLowerLimitViolated; bool oldIsLowerLimitViolated = mIsLowerLimitViolated;
mIsLowerLimitViolated = lowerLimitError <= 0; mIsLowerLimitViolated = lowerLimitError <= 0;
if (mIsLowerLimitViolated != oldIsLowerLimitViolated) { if (mIsLowerLimitViolated != oldIsLowerLimitViolated) {
mImpulseLowerLimit = 0.0; m_impulseLowerLimit = 0.0;
} }
bool oldIsUpperLimitViolated = mIsUpperLimitViolated; bool oldIsUpperLimitViolated = mIsUpperLimitViolated;
mIsUpperLimitViolated = upperLimitError <= 0; mIsUpperLimitViolated = upperLimitError <= 0;
if (mIsUpperLimitViolated != oldIsUpperLimitViolated) { if (mIsUpperLimitViolated != oldIsUpperLimitViolated) {
mImpulseUpperLimit = 0.0; m_impulseUpperLimit = 0.0;
} }
// Compute vectors needed in the Jacobian // Compute vectors needed in the Jacobian
@ -99,33 +99,33 @@ void HingeJoint::initBeforeSolve(const ConstraintSolverData& constraintSolverDat
mC2CrossA1 = c2.cross(mA1); mC2CrossA1 = c2.cross(mA1);
// Compute the corresponding skew-symmetric matrices // Compute the corresponding skew-symmetric matrices
Matrix3x3 skewSymmetricMatrixU1= Matrix3x3::computeSkewSymmetricMatrixForCrossProduct(mR1World); Matrix3x3 skewSymmetricMatrixU1= Matrix3x3::computeSkewSymmetricMatrixForCrossProduct(m_r1World);
Matrix3x3 skewSymmetricMatrixU2= Matrix3x3::computeSkewSymmetricMatrixForCrossProduct(mR2World); Matrix3x3 skewSymmetricMatrixU2= Matrix3x3::computeSkewSymmetricMatrixForCrossProduct(m_r2World);
// Compute the inverse mass matrix K=JM^-1J^t for the 3 translation constraints (3x3 matrix) // Compute the inverse mass matrix K=JM^-1J^t for the 3 translation constraints (3x3 matrix)
float inverseMassBodies = mBody1->mMassInverse + mBody2->mMassInverse; float inverseMassBodies = mBody1->mMassInverse + mBody2->mMassInverse;
Matrix3x3 massMatrix = Matrix3x3(inverseMassBodies, 0, 0, Matrix3x3 massMatrix = Matrix3x3(inverseMassBodies, 0, 0,
0, inverseMassBodies, 0, 0, inverseMassBodies, 0,
0, 0, inverseMassBodies) + 0, 0, inverseMassBodies) +
skewSymmetricMatrixU1 * mI1 * skewSymmetricMatrixU1.getTranspose() + skewSymmetricMatrixU1 * m_i1 * skewSymmetricMatrixU1.getTranspose() +
skewSymmetricMatrixU2 * mI2 * skewSymmetricMatrixU2.getTranspose(); skewSymmetricMatrixU2 * m_i2 * skewSymmetricMatrixU2.getTranspose();
mInverseMassMatrixTranslation.setToZero(); m_inverseMassMatrixTranslation.setToZero();
if (mBody1->getType() == DYNAMIC || mBody2->getType() == DYNAMIC) { if (mBody1->getType() == DYNAMIC || mBody2->getType() == DYNAMIC) {
mInverseMassMatrixTranslation = massMatrix.getInverse(); m_inverseMassMatrixTranslation = massMatrix.getInverse();
} }
// Compute the bias "b" of the translation constraints // Compute the bias "b" of the translation constraints
mBTranslation.setToZero(); mBTranslation.setToZero();
float biasFactor = (BETA / constraintSolverData.timeStep); float biasFactor = (BETA / constraintSolverData.timeStep);
if (mPositionCorrectionTechnique == BAUMGARTE_JOINTS) { if (mPositionCorrectionTechnique == BAUMGARTE_JOINTS) {
mBTranslation = biasFactor * (x2 + mR2World - x1 - mR1World); mBTranslation = 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)
Vector3 I1B2CrossA1 = mI1 * mB2CrossA1; Vector3 I1B2CrossA1 = m_i1 * mB2CrossA1;
Vector3 I1C2CrossA1 = mI1 * mC2CrossA1; Vector3 I1C2CrossA1 = m_i1 * mC2CrossA1;
Vector3 I2B2CrossA1 = mI2 * mB2CrossA1; Vector3 I2B2CrossA1 = m_i2 * mB2CrossA1;
Vector3 I2C2CrossA1 = mI2 * mC2CrossA1; Vector3 I2C2CrossA1 = m_i2 * mC2CrossA1;
const float el11 = mB2CrossA1.dot(I1B2CrossA1) + const float el11 = mB2CrossA1.dot(I1B2CrossA1) +
mB2CrossA1.dot(I2B2CrossA1); mB2CrossA1.dot(I2B2CrossA1);
const float el12 = mB2CrossA1.dot(I1C2CrossA1) + const float el12 = mB2CrossA1.dot(I1C2CrossA1) +
@ -135,9 +135,9 @@ void HingeJoint::initBeforeSolve(const ConstraintSolverData& constraintSolverDat
const float el22 = mC2CrossA1.dot(I1C2CrossA1) + const float el22 = mC2CrossA1.dot(I1C2CrossA1) +
mC2CrossA1.dot(I2C2CrossA1); mC2CrossA1.dot(I2C2CrossA1);
const Matrix2x2 matrixKRotation(el11, el12, el21, el22); const Matrix2x2 matrixKRotation(el11, el12, el21, el22);
mInverseMassMatrixRotation.setToZero(); m_inverseMassMatrixRotation.setToZero();
if (mBody1->getType() == DYNAMIC || mBody2->getType() == DYNAMIC) { if (mBody1->getType() == DYNAMIC || mBody2->getType() == DYNAMIC) {
mInverseMassMatrixRotation = matrixKRotation.getInverse(); m_inverseMassMatrixRotation = matrixKRotation.getInverse();
} }
// Compute the bias "b" of the rotation constraints // Compute the bias "b" of the rotation constraints
@ -150,20 +150,20 @@ void HingeJoint::initBeforeSolve(const ConstraintSolverData& constraintSolverDat
if (!constraintSolverData.isWarmStartingActive) { if (!constraintSolverData.isWarmStartingActive) {
// Reset all the accumulated impulses // Reset all the accumulated impulses
mImpulseTranslation.setToZero(); m_impulseTranslation.setToZero();
mImpulseRotation.setToZero(); m_impulseRotation.setToZero();
mImpulseLowerLimit = 0.0; m_impulseLowerLimit = 0.0;
mImpulseUpperLimit = 0.0; m_impulseUpperLimit = 0.0;
mImpulseMotor = 0.0; m_impulseMotor = 0.0;
} }
// If the motor or limits are enabled // If the motor or limits are enabled
if (mIsMotorEnabled || (mIsLimitEnabled && (mIsLowerLimitViolated || mIsUpperLimitViolated))) { if (mIsMotorEnabled || (mIsLimitEnabled && (mIsLowerLimitViolated || mIsUpperLimitViolated))) {
// 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)
mInverseMassMatrixLimitMotor = mA1.dot(mI1 * mA1) + mA1.dot(mI2 * mA1); m_inverseMassMatrixLimitMotor = mA1.dot(m_i1 * mA1) + mA1.dot(m_i2 * mA1);
mInverseMassMatrixLimitMotor = (mInverseMassMatrixLimitMotor > 0.0) ? m_inverseMassMatrixLimitMotor = (m_inverseMassMatrixLimitMotor > 0.0) ?
float(1.0) / mInverseMassMatrixLimitMotor : float(0.0); float(1.0) / m_inverseMassMatrixLimitMotor : float(0.0);
if (mIsLimitEnabled) { if (mIsLimitEnabled) {
@ -196,17 +196,17 @@ void HingeJoint::warmstart(const ConstraintSolverData& constraintSolverData) {
const float inverseMassBody2 = mBody2->mMassInverse; const float inverseMassBody2 = mBody2->mMassInverse;
// Compute the impulse P=J^T * lambda for the 2 rotation constraints // Compute the impulse P=J^T * lambda for the 2 rotation constraints
Vector3 rotationImpulse = -mB2CrossA1 * mImpulseRotation.x - mC2CrossA1 * mImpulseRotation.y; Vector3 rotationImpulse = -mB2CrossA1 * m_impulseRotation.x - mC2CrossA1 * 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 Vector3 limitsImpulse = (mImpulseUpperLimit - mImpulseLowerLimit) * mA1; const Vector3 limitsImpulse = (m_impulseUpperLimit - m_impulseLowerLimit) * mA1;
// Compute the impulse P=J^T * lambda for the motor constraint // Compute the impulse P=J^T * lambda for the motor constraint
const Vector3 motorImpulse = -mImpulseMotor * mA1; const Vector3 motorImpulse = -m_impulseMotor * mA1;
// Compute the impulse P=J^T * lambda for the 3 translation constraints of body 1 // Compute the impulse P=J^T * lambda for the 3 translation constraints of body 1
Vector3 linearImpulseBody1 = -mImpulseTranslation; Vector3 linearImpulseBody1 = -m_impulseTranslation;
Vector3 angularImpulseBody1 = mImpulseTranslation.cross(mR1World); Vector3 angularImpulseBody1 = m_impulseTranslation.cross(m_r1World);
// 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 += rotationImpulse; angularImpulseBody1 += rotationImpulse;
@ -219,10 +219,10 @@ void HingeJoint::warmstart(const ConstraintSolverData& constraintSolverData) {
// Apply the impulse to the body 1 // Apply the impulse to the body 1
v1 += inverseMassBody1 * linearImpulseBody1; v1 += inverseMassBody1 * linearImpulseBody1;
w1 += mI1 * angularImpulseBody1; w1 += m_i1 * angularImpulseBody1;
// Compute the impulse P=J^T * lambda for the 3 translation constraints of body 2 // Compute the impulse P=J^T * lambda for the 3 translation constraints of body 2
Vector3 angularImpulseBody2 = -mImpulseTranslation.cross(mR2World); Vector3 angularImpulseBody2 = -m_impulseTranslation.cross(m_r2World);
// 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 += -rotationImpulse; angularImpulseBody2 += -rotationImpulse;
@ -234,8 +234,8 @@ void HingeJoint::warmstart(const ConstraintSolverData& constraintSolverData) {
angularImpulseBody2 += -motorImpulse; angularImpulseBody2 += -motorImpulse;
// Apply the impulse to the body 2 // Apply the impulse to the body 2
v2 += inverseMassBody2 * mImpulseTranslation; v2 += inverseMassBody2 * m_impulseTranslation;
w2 += mI2 * angularImpulseBody2; w2 += m_i2 * angularImpulseBody2;
} }
// Solve the velocity constraint // Solve the velocity constraint
@ -254,27 +254,27 @@ void HingeJoint::solveVelocityConstraint(const ConstraintSolverData& constraintS
// --------------- Translation Constraints --------------- // // --------------- Translation Constraints --------------- //
// Compute J*v // Compute J*v
const Vector3 JvTranslation = v2 + w2.cross(mR2World) - v1 - w1.cross(mR1World); const Vector3 JvTranslation = v2 + w2.cross(m_r2World) - v1 - w1.cross(m_r1World);
// Compute the Lagrange multiplier lambda // Compute the Lagrange multiplier lambda
const Vector3 deltaLambdaTranslation = mInverseMassMatrixTranslation * const Vector3 deltaLambdaTranslation = m_inverseMassMatrixTranslation *
(-JvTranslation - mBTranslation); (-JvTranslation - mBTranslation);
mImpulseTranslation += deltaLambdaTranslation; m_impulseTranslation += deltaLambdaTranslation;
// Compute the impulse P=J^T * lambda of body 1 // Compute the impulse P=J^T * lambda of body 1
const Vector3 linearImpulseBody1 = -deltaLambdaTranslation; const Vector3 linearImpulseBody1 = -deltaLambdaTranslation;
Vector3 angularImpulseBody1 = deltaLambdaTranslation.cross(mR1World); Vector3 angularImpulseBody1 = deltaLambdaTranslation.cross(m_r1World);
// Apply the impulse to the body 1 // Apply the impulse to the body 1
v1 += inverseMassBody1 * linearImpulseBody1; v1 += inverseMassBody1 * linearImpulseBody1;
w1 += mI1 * angularImpulseBody1; w1 += m_i1 * angularImpulseBody1;
// Compute the impulse P=J^T * lambda of body 2 // Compute the impulse P=J^T * lambda of body 2
Vector3 angularImpulseBody2 = -deltaLambdaTranslation.cross(mR2World); Vector3 angularImpulseBody2 = -deltaLambdaTranslation.cross(m_r2World);
// Apply the impulse to the body 2 // Apply the impulse to the body 2
v2 += inverseMassBody2 * deltaLambdaTranslation; v2 += inverseMassBody2 * deltaLambdaTranslation;
w2 += mI2 * angularImpulseBody2; w2 += m_i2 * angularImpulseBody2;
// --------------- Rotation Constraints --------------- // // --------------- Rotation Constraints --------------- //
@ -283,22 +283,22 @@ void HingeJoint::solveVelocityConstraint(const ConstraintSolverData& constraintS
-mC2CrossA1.dot(w1) + mC2CrossA1.dot(w2)); -mC2CrossA1.dot(w1) + mC2CrossA1.dot(w2));
// Compute the Lagrange multiplier lambda for the 2 rotation constraints // Compute the Lagrange multiplier lambda for the 2 rotation constraints
Vector2 deltaLambdaRotation = mInverseMassMatrixRotation * (-JvRotation - mBRotation); Vector2 deltaLambdaRotation = m_inverseMassMatrixRotation * (-JvRotation - mBRotation);
mImpulseRotation += 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 = -mB2CrossA1 * deltaLambdaRotation.x -
mC2CrossA1 * deltaLambdaRotation.y; mC2CrossA1 * deltaLambdaRotation.y;
// Apply the impulse to the body 1 // Apply the impulse to the body 1
w1 += mI1 * 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 = mB2CrossA1 * deltaLambdaRotation.x +
mC2CrossA1 * deltaLambdaRotation.y; mC2CrossA1 * deltaLambdaRotation.y;
// Apply the impulse to the body 2 // Apply the impulse to the body 2
w2 += mI2 * angularImpulseBody2; w2 += m_i2 * angularImpulseBody2;
// --------------- Limits Constraints --------------- // // --------------- Limits Constraints --------------- //
@ -311,22 +311,22 @@ void HingeJoint::solveVelocityConstraint(const ConstraintSolverData& constraintS
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 = mInverseMassMatrixLimitMotor * (-JvLowerLimit -mBLowerLimit); float deltaLambdaLower = m_inverseMassMatrixLimitMotor * (-JvLowerLimit -mBLowerLimit);
float lambdaTemp = mImpulseLowerLimit; float lambdaTemp = m_impulseLowerLimit;
mImpulseLowerLimit = std::max(mImpulseLowerLimit + deltaLambdaLower, float(0.0)); m_impulseLowerLimit = std::max(m_impulseLowerLimit + deltaLambdaLower, float(0.0));
deltaLambdaLower = mImpulseLowerLimit - 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 Vector3 angularImpulseBody1 = -deltaLambdaLower * mA1; const Vector3 angularImpulseBody1 = -deltaLambdaLower * mA1;
// Apply the impulse to the body 1 // Apply the impulse to the body 1
w1 += mI1 * 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 Vector3 angularImpulseBody2 = deltaLambdaLower * mA1; const Vector3 angularImpulseBody2 = deltaLambdaLower * mA1;
// Apply the impulse to the body 2 // Apply the impulse to the body 2
w2 += mI2 * angularImpulseBody2; w2 += m_i2 * angularImpulseBody2;
} }
// If the upper limit is violated // If the upper limit is violated
@ -336,22 +336,22 @@ void HingeJoint::solveVelocityConstraint(const ConstraintSolverData& constraintS
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 = mInverseMassMatrixLimitMotor * (-JvUpperLimit -mBUpperLimit); float deltaLambdaUpper = m_inverseMassMatrixLimitMotor * (-JvUpperLimit -mBUpperLimit);
float lambdaTemp = mImpulseUpperLimit; float lambdaTemp = m_impulseUpperLimit;
mImpulseUpperLimit = std::max(mImpulseUpperLimit + deltaLambdaUpper, float(0.0)); m_impulseUpperLimit = std::max(m_impulseUpperLimit + deltaLambdaUpper, float(0.0));
deltaLambdaUpper = mImpulseUpperLimit - 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 Vector3 angularImpulseBody1 = deltaLambdaUpper * mA1; const Vector3 angularImpulseBody1 = deltaLambdaUpper * mA1;
// Apply the impulse to the body 1 // Apply the impulse to the body 1
w1 += mI1 * 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 Vector3 angularImpulseBody2 = -deltaLambdaUpper * mA1; const Vector3 angularImpulseBody2 = -deltaLambdaUpper * mA1;
// Apply the impulse to the body 2 // Apply the impulse to the body 2
w2 += mI2 * angularImpulseBody2; w2 += m_i2 * angularImpulseBody2;
} }
} }
@ -365,22 +365,22 @@ void HingeJoint::solveVelocityConstraint(const ConstraintSolverData& constraintS
// 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 = mMaxMotorTorque * constraintSolverData.timeStep;
float deltaLambdaMotor = mInverseMassMatrixLimitMotor * (-JvMotor - mMotorSpeed); float deltaLambdaMotor = m_inverseMassMatrixLimitMotor * (-JvMotor - mMotorSpeed);
float lambdaTemp = mImpulseMotor; float lambdaTemp = m_impulseMotor;
mImpulseMotor = clamp(mImpulseMotor + deltaLambdaMotor, -maxMotorImpulse, maxMotorImpulse); m_impulseMotor = clamp(m_impulseMotor + deltaLambdaMotor, -maxMotorImpulse, maxMotorImpulse);
deltaLambdaMotor = mImpulseMotor - 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 Vector3 angularImpulseBody1 = -deltaLambdaMotor * mA1; const Vector3 angularImpulseBody1 = -deltaLambdaMotor * mA1;
// Apply the impulse to the body 1 // Apply the impulse to the body 1
w1 += mI1 * angularImpulseBody1; w1 += m_i1 * angularImpulseBody1;
// 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 Vector3 angularImpulseBody2 = deltaLambdaMotor * mA1; const Vector3 angularImpulseBody2 = deltaLambdaMotor * mA1;
// Apply the impulse to the body 2 // Apply the impulse to the body 2
w2 += mI2 * angularImpulseBody2; w2 += m_i2 * angularImpulseBody2;
} }
} }
@ -402,12 +402,12 @@ void HingeJoint::solvePositionConstraint(const ConstraintSolverData& constraintS
float inverseMassBody2 = mBody2->mMassInverse; float inverseMassBody2 = mBody2->mMassInverse;
// Recompute the inverse inertia tensors // Recompute the inverse inertia tensors
mI1 = mBody1->getInertiaTensorInverseWorld(); m_i1 = mBody1->getInertiaTensorInverseWorld();
mI2 = mBody2->getInertiaTensorInverseWorld(); m_i2 = mBody2->getInertiaTensorInverseWorld();
// Compute the vector from body center to the anchor point in world-space // Compute the vector from body center to the anchor point in world-space
mR1World = q1 * mLocalAnchorPointBody1; m_r1World = q1 * m_localAnchorPointBody1;
mR2World = q2 * mLocalAnchorPointBody2; m_r2World = q2 * m_localAnchorPointBody2;
// Compute the current angle around the hinge axis // Compute the current angle around the hinge axis
float hingeAngle = computeCurrentHingeAngle(q1, q2); float hingeAngle = computeCurrentHingeAngle(q1, q2);
@ -429,8 +429,8 @@ void HingeJoint::solvePositionConstraint(const ConstraintSolverData& constraintS
mC2CrossA1 = c2.cross(mA1); mC2CrossA1 = c2.cross(mA1);
// Compute the corresponding skew-symmetric matrices // Compute the corresponding skew-symmetric matrices
Matrix3x3 skewSymmetricMatrixU1= Matrix3x3::computeSkewSymmetricMatrixForCrossProduct(mR1World); Matrix3x3 skewSymmetricMatrixU1= Matrix3x3::computeSkewSymmetricMatrixForCrossProduct(m_r1World);
Matrix3x3 skewSymmetricMatrixU2= Matrix3x3::computeSkewSymmetricMatrixForCrossProduct(mR2World); Matrix3x3 skewSymmetricMatrixU2= Matrix3x3::computeSkewSymmetricMatrixForCrossProduct(m_r2World);
// --------------- Translation Constraints --------------- // // --------------- Translation Constraints --------------- //
@ -439,26 +439,26 @@ void HingeJoint::solvePositionConstraint(const ConstraintSolverData& constraintS
Matrix3x3 massMatrix = Matrix3x3(inverseMassBodies, 0, 0, Matrix3x3 massMatrix = Matrix3x3(inverseMassBodies, 0, 0,
0, inverseMassBodies, 0, 0, inverseMassBodies, 0,
0, 0, inverseMassBodies) + 0, 0, inverseMassBodies) +
skewSymmetricMatrixU1 * mI1 * skewSymmetricMatrixU1.getTranspose() + skewSymmetricMatrixU1 * m_i1 * skewSymmetricMatrixU1.getTranspose() +
skewSymmetricMatrixU2 * mI2 * skewSymmetricMatrixU2.getTranspose(); skewSymmetricMatrixU2 * m_i2 * skewSymmetricMatrixU2.getTranspose();
mInverseMassMatrixTranslation.setToZero(); m_inverseMassMatrixTranslation.setToZero();
if (mBody1->getType() == DYNAMIC || mBody2->getType() == DYNAMIC) { if (mBody1->getType() == DYNAMIC || mBody2->getType() == DYNAMIC) {
mInverseMassMatrixTranslation = massMatrix.getInverse(); m_inverseMassMatrixTranslation = massMatrix.getInverse();
} }
// Compute position error for the 3 translation constraints // Compute position error for the 3 translation constraints
const Vector3 errorTranslation = x2 + mR2World - x1 - mR1World; const Vector3 errorTranslation = x2 + m_r2World - x1 - m_r1World;
// Compute the Lagrange multiplier lambda // Compute the Lagrange multiplier lambda
const Vector3 lambdaTranslation = mInverseMassMatrixTranslation * (-errorTranslation); const Vector3 lambdaTranslation = m_inverseMassMatrixTranslation * (-errorTranslation);
// Compute the impulse of body 1 // Compute the impulse of body 1
Vector3 linearImpulseBody1 = -lambdaTranslation; Vector3 linearImpulseBody1 = -lambdaTranslation;
Vector3 angularImpulseBody1 = lambdaTranslation.cross(mR1World); Vector3 angularImpulseBody1 = lambdaTranslation.cross(m_r1World);
// Compute the pseudo velocity of body 1 // Compute the pseudo velocity of body 1
const Vector3 v1 = inverseMassBody1 * linearImpulseBody1; const Vector3 v1 = inverseMassBody1 * linearImpulseBody1;
Vector3 w1 = mI1 * angularImpulseBody1; Vector3 w1 = m_i1 * angularImpulseBody1;
// Update the body position/orientation of body 1 // Update the body position/orientation of body 1
x1 += v1; x1 += v1;
@ -466,11 +466,11 @@ void HingeJoint::solvePositionConstraint(const ConstraintSolverData& constraintS
q1.normalize(); q1.normalize();
// Compute the impulse of body 2 // Compute the impulse of body 2
Vector3 angularImpulseBody2 = -lambdaTranslation.cross(mR2World); Vector3 angularImpulseBody2 = -lambdaTranslation.cross(m_r2World);
// Compute the pseudo velocity of body 2 // Compute the pseudo velocity of body 2
const Vector3 v2 = inverseMassBody2 * lambdaTranslation; const Vector3 v2 = inverseMassBody2 * lambdaTranslation;
Vector3 w2 = mI2 * angularImpulseBody2; Vector3 w2 = m_i2 * angularImpulseBody2;
// Update the body position/orientation of body 2 // Update the body position/orientation of body 2
x2 += v2; x2 += v2;
@ -480,10 +480,10 @@ 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)
Vector3 I1B2CrossA1 = mI1 * mB2CrossA1; Vector3 I1B2CrossA1 = m_i1 * mB2CrossA1;
Vector3 I1C2CrossA1 = mI1 * mC2CrossA1; Vector3 I1C2CrossA1 = m_i1 * mC2CrossA1;
Vector3 I2B2CrossA1 = mI2 * mB2CrossA1; Vector3 I2B2CrossA1 = m_i2 * mB2CrossA1;
Vector3 I2C2CrossA1 = mI2 * mC2CrossA1; Vector3 I2C2CrossA1 = m_i2 * mC2CrossA1;
const float el11 = mB2CrossA1.dot(I1B2CrossA1) + const float el11 = mB2CrossA1.dot(I1B2CrossA1) +
mB2CrossA1.dot(I2B2CrossA1); mB2CrossA1.dot(I2B2CrossA1);
const float el12 = mB2CrossA1.dot(I1C2CrossA1) + const float el12 = mB2CrossA1.dot(I1C2CrossA1) +
@ -493,22 +493,22 @@ void HingeJoint::solvePositionConstraint(const ConstraintSolverData& constraintS
const float el22 = mC2CrossA1.dot(I1C2CrossA1) + const float el22 = mC2CrossA1.dot(I1C2CrossA1) +
mC2CrossA1.dot(I2C2CrossA1); mC2CrossA1.dot(I2C2CrossA1);
const Matrix2x2 matrixKRotation(el11, el12, el21, el22); const Matrix2x2 matrixKRotation(el11, el12, el21, el22);
mInverseMassMatrixRotation.setToZero(); m_inverseMassMatrixRotation.setToZero();
if (mBody1->getType() == DYNAMIC || mBody2->getType() == DYNAMIC) { if (mBody1->getType() == DYNAMIC || mBody2->getType() == DYNAMIC) {
mInverseMassMatrixRotation = matrixKRotation.getInverse(); m_inverseMassMatrixRotation = matrixKRotation.getInverse();
} }
// Compute the position error for the 3 rotation constraints // Compute the position error for the 3 rotation constraints
const Vector2 errorRotation = Vector2(mA1.dot(b2), mA1.dot(c2)); const Vector2 errorRotation = Vector2(mA1.dot(b2), mA1.dot(c2));
// Compute the Lagrange multiplier lambda for the 3 rotation constraints // Compute the Lagrange multiplier lambda for the 3 rotation constraints
Vector2 lambdaRotation = mInverseMassMatrixRotation * (-errorRotation); Vector2 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 = -mB2CrossA1 * lambdaRotation.x - mC2CrossA1 * lambdaRotation.y;
// Compute the pseudo velocity of body 1 // Compute the pseudo velocity of body 1
w1 = mI1 * angularImpulseBody1; w1 = m_i1 * angularImpulseBody1;
// Update the body position/orientation of body 1 // Update the body position/orientation of body 1
q1 += Quaternion(0, w1) * q1 * float(0.5); q1 += Quaternion(0, w1) * q1 * float(0.5);
@ -518,7 +518,7 @@ void HingeJoint::solvePositionConstraint(const ConstraintSolverData& constraintS
angularImpulseBody2 = mB2CrossA1 * lambdaRotation.x + mC2CrossA1 * lambdaRotation.y; angularImpulseBody2 = mB2CrossA1 * lambdaRotation.x + mC2CrossA1 * lambdaRotation.y;
// Compute the pseudo velocity of body 2 // Compute the pseudo velocity of body 2
w2 = mI2 * angularImpulseBody2; w2 = m_i2 * angularImpulseBody2;
// Update the body position/orientation of body 2 // Update the body position/orientation of body 2
q2 += Quaternion(0, w2) * q2 * float(0.5); q2 += Quaternion(0, w2) * q2 * float(0.5);
@ -531,22 +531,22 @@ void HingeJoint::solvePositionConstraint(const ConstraintSolverData& constraintS
if (mIsLowerLimitViolated || mIsUpperLimitViolated) { if (mIsLowerLimitViolated || mIsUpperLimitViolated) {
// 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)
mInverseMassMatrixLimitMotor = mA1.dot(mI1 * mA1) + mA1.dot(mI2 * mA1); m_inverseMassMatrixLimitMotor = mA1.dot(m_i1 * mA1) + mA1.dot(m_i2 * mA1);
mInverseMassMatrixLimitMotor = (mInverseMassMatrixLimitMotor > 0.0) ? m_inverseMassMatrixLimitMotor = (m_inverseMassMatrixLimitMotor > 0.0) ?
float(1.0) / mInverseMassMatrixLimitMotor : float(0.0); float(1.0) / m_inverseMassMatrixLimitMotor : float(0.0);
} }
// If the lower limit is violated // If the lower limit is violated
if (mIsLowerLimitViolated) { if (mIsLowerLimitViolated) {
// Compute the Lagrange multiplier lambda for the lower limit constraint // Compute the Lagrange multiplier lambda for the lower limit constraint
float lambdaLowerLimit = mInverseMassMatrixLimitMotor * (-lowerLimitError ); float lambdaLowerLimit = m_inverseMassMatrixLimitMotor * (-lowerLimitError );
// Compute the impulse P=J^T * lambda of body 1 // Compute the impulse P=J^T * lambda of body 1
const Vector3 angularImpulseBody1 = -lambdaLowerLimit * mA1; const Vector3 angularImpulseBody1 = -lambdaLowerLimit * mA1;
// Compute the pseudo velocity of body 1 // Compute the pseudo velocity of body 1
const Vector3 w1 = mI1 * angularImpulseBody1; const Vector3 w1 = m_i1 * angularImpulseBody1;
// Update the body position/orientation of body 1 // Update the body position/orientation of body 1
q1 += Quaternion(0, w1) * q1 * float(0.5); q1 += Quaternion(0, w1) * q1 * float(0.5);
@ -556,7 +556,7 @@ void HingeJoint::solvePositionConstraint(const ConstraintSolverData& constraintS
const Vector3 angularImpulseBody2 = lambdaLowerLimit * mA1; const Vector3 angularImpulseBody2 = lambdaLowerLimit * mA1;
// Compute the pseudo velocity of body 2 // Compute the pseudo velocity of body 2
const Vector3 w2 = mI2 * angularImpulseBody2; const Vector3 w2 = m_i2 * angularImpulseBody2;
// Update the body position/orientation of body 2 // Update the body position/orientation of body 2
q2 += Quaternion(0, w2) * q2 * float(0.5); q2 += Quaternion(0, w2) * q2 * float(0.5);
@ -567,13 +567,13 @@ void HingeJoint::solvePositionConstraint(const ConstraintSolverData& constraintS
if (mIsUpperLimitViolated) { if (mIsUpperLimitViolated) {
// Compute the Lagrange multiplier lambda for the upper limit constraint // Compute the Lagrange multiplier lambda for the upper limit constraint
float lambdaUpperLimit = mInverseMassMatrixLimitMotor * (-upperLimitError); float lambdaUpperLimit = m_inverseMassMatrixLimitMotor * (-upperLimitError);
// Compute the impulse P=J^T * lambda of body 1 // Compute the impulse P=J^T * lambda of body 1
const Vector3 angularImpulseBody1 = lambdaUpperLimit * mA1; const Vector3 angularImpulseBody1 = lambdaUpperLimit * mA1;
// Compute the pseudo velocity of body 1 // Compute the pseudo velocity of body 1
const Vector3 w1 = mI1 * angularImpulseBody1; const Vector3 w1 = m_i1 * angularImpulseBody1;
// Update the body position/orientation of body 1 // Update the body position/orientation of body 1
q1 += Quaternion(0, w1) * q1 * float(0.5); q1 += Quaternion(0, w1) * q1 * float(0.5);
@ -583,7 +583,7 @@ void HingeJoint::solvePositionConstraint(const ConstraintSolverData& constraintS
const Vector3 angularImpulseBody2 = -lambdaUpperLimit * mA1; const Vector3 angularImpulseBody2 = -lambdaUpperLimit * mA1;
// Compute the pseudo velocity of body 2 // Compute the pseudo velocity of body 2
const Vector3 w2 = mI2 * angularImpulseBody2; const Vector3 w2 = m_i2 * angularImpulseBody2;
// Update the body position/orientation of body 2 // Update the body position/orientation of body 2
q2 += Quaternion(0, w2) * q2 * float(0.5); q2 += Quaternion(0, w2) * q2 * float(0.5);
@ -617,7 +617,7 @@ void HingeJoint::enableLimit(bool isLimitEnabled) {
void HingeJoint::enableMotor(bool isMotorEnabled) { void HingeJoint::enableMotor(bool isMotorEnabled) {
mIsMotorEnabled = isMotorEnabled; mIsMotorEnabled = isMotorEnabled;
mImpulseMotor = 0.0; m_impulseMotor = 0.0;
// Wake up the two bodies of the joint // Wake up the two bodies of the joint
mBody1->setIsSleeping(false); mBody1->setIsSleeping(false);
@ -662,8 +662,8 @@ void HingeJoint::setMaxAngleLimit(float upperLimit) {
void HingeJoint::resetLimits() { void HingeJoint::resetLimits() {
// Reset the accumulated impulses for the limits // Reset the accumulated impulses for the limits
mImpulseLowerLimit = 0.0; m_impulseLowerLimit = 0.0;
mImpulseUpperLimit = 0.0; m_impulseUpperLimit = 0.0;
// Wake up the two bodies of the joint // Wake up the two bodies of the joint
mBody1->setIsSleeping(false); mBody1->setIsSleeping(false);

40
ephysics/constraint/HingeJoint.h
View File

@ -23,7 +23,7 @@ struct HingeJointInfo : public JointInfo {
// -------------------- Attributes -------------------- // // -------------------- Attributes -------------------- //
/// Anchor point (in world-space coordinates) /// Anchor point (in world-space coordinates)
Vector3 anchorPointWorldSpace; Vector3 m_m_m_m_anchorPointWorldSpace;
/// Hinge rotation axis (in world-space coordinates) /// Hinge rotation axis (in world-space coordinates)
Vector3 rotationAxisWorld; Vector3 rotationAxisWorld;
@ -62,7 +62,7 @@ struct HingeJointInfo : public JointInfo {
const Vector3& initAnchorPointWorldSpace, const Vector3& initAnchorPointWorldSpace,
const Vector3& initRotationAxisWorld) const Vector3& initRotationAxisWorld)
: JointInfo(rigidBody1, rigidBody2, HINGEJOINT), : JointInfo(rigidBody1, rigidBody2, HINGEJOINT),
anchorPointWorldSpace(initAnchorPointWorldSpace), m_m_m_m_anchorPointWorldSpace(initAnchorPointWorldSpace),
rotationAxisWorld(initRotationAxisWorld), isLimitEnabled(false), rotationAxisWorld(initRotationAxisWorld), isLimitEnabled(false),
isMotorEnabled(false), minAngleLimit(-1), maxAngleLimit(1), isMotorEnabled(false), minAngleLimit(-1), maxAngleLimit(1),
motorSpeed(0), maxMotorTorque(0) {} motorSpeed(0), maxMotorTorque(0) {}
@ -81,7 +81,7 @@ struct HingeJointInfo : public JointInfo {
const Vector3& initRotationAxisWorld, const Vector3& initRotationAxisWorld,
float initMinAngleLimit, float initMaxAngleLimit) float initMinAngleLimit, float initMaxAngleLimit)
: JointInfo(rigidBody1, rigidBody2, HINGEJOINT), : JointInfo(rigidBody1, rigidBody2, HINGEJOINT),
anchorPointWorldSpace(initAnchorPointWorldSpace), m_m_m_m_anchorPointWorldSpace(initAnchorPointWorldSpace),
rotationAxisWorld(initRotationAxisWorld), isLimitEnabled(true), rotationAxisWorld(initRotationAxisWorld), isLimitEnabled(true),
isMotorEnabled(false), minAngleLimit(initMinAngleLimit), isMotorEnabled(false), minAngleLimit(initMinAngleLimit),
maxAngleLimit(initMaxAngleLimit), motorSpeed(0), maxAngleLimit(initMaxAngleLimit), motorSpeed(0),
@ -104,7 +104,7 @@ struct HingeJointInfo : public JointInfo {
float initMinAngleLimit, float initMaxAngleLimit, float initMinAngleLimit, float initMaxAngleLimit,
float initMotorSpeed, float initMaxMotorTorque) float initMotorSpeed, float initMaxMotorTorque)
: JointInfo(rigidBody1, rigidBody2, HINGEJOINT), : JointInfo(rigidBody1, rigidBody2, HINGEJOINT),
anchorPointWorldSpace(initAnchorPointWorldSpace), m_m_m_m_anchorPointWorldSpace(initAnchorPointWorldSpace),
rotationAxisWorld(initRotationAxisWorld), isLimitEnabled(true), rotationAxisWorld(initRotationAxisWorld), isLimitEnabled(true),
isMotorEnabled(false), minAngleLimit(initMinAngleLimit), isMotorEnabled(false), minAngleLimit(initMinAngleLimit),
maxAngleLimit(initMaxAngleLimit), motorSpeed(initMotorSpeed), maxAngleLimit(initMaxAngleLimit), motorSpeed(initMotorSpeed),
@ -129,10 +129,10 @@ class HingeJoint : public Joint {
// -------------------- Attributes -------------------- // // -------------------- Attributes -------------------- //
/// Anchor point of body 1 (in local-space coordinates of body 1) /// Anchor point of body 1 (in local-space coordinates of body 1)
Vector3 mLocalAnchorPointBody1; Vector3 m_localAnchorPointBody1;
/// Anchor point of body 2 (in local-space coordinates of body 2) /// Anchor point of body 2 (in local-space coordinates of body 2)
Vector3 mLocalAnchorPointBody2; Vector3 m_localAnchorPointBody2;
/// Hinge rotation axis (in local-space coordinates of body 1) /// Hinge rotation axis (in local-space coordinates of body 1)
Vector3 mHingeLocalAxisBody1; Vector3 mHingeLocalAxisBody1;
@ -141,19 +141,19 @@ class HingeJoint : public Joint {
Vector3 mHingeLocalAxisBody2; Vector3 mHingeLocalAxisBody2;
/// Inertia tensor of body 1 (in world-space coordinates) /// Inertia tensor of body 1 (in world-space coordinates)
Matrix3x3 mI1; Matrix3x3 m_i1;
/// Inertia tensor of body 2 (in world-space coordinates) /// Inertia tensor of body 2 (in world-space coordinates)
Matrix3x3 mI2; Matrix3x3 m_i2;
/// Hinge rotation axis (in world-space coordinates) computed from body 1 /// Hinge rotation axis (in world-space coordinates) computed from body 1
Vector3 mA1; Vector3 mA1;
/// Vector from center of body 2 to anchor point in world-space /// Vector from center of body 2 to anchor point in world-space
Vector3 mR1World; Vector3 m_r1World;
/// Vector from center of body 2 to anchor point in world-space /// Vector from center of body 2 to anchor point in world-space
Vector3 mR2World; Vector3 m_r2World;
/// Cross product of vector b2 and a1 /// Cross product of vector b2 and a1
Vector3 mB2CrossA1; Vector3 mB2CrossA1;
@ -162,31 +162,31 @@ class HingeJoint : public Joint {
Vector3 mC2CrossA1; Vector3 mC2CrossA1;
/// Impulse for the 3 translation constraints /// Impulse for the 3 translation constraints
Vector3 mImpulseTranslation; Vector3 m_impulseTranslation;
/// Impulse for the 2 rotation constraints /// Impulse for the 2 rotation constraints
Vector2 mImpulseRotation; Vector2 m_impulseRotation;
/// Accumulated impulse for the lower limit constraint /// Accumulated impulse for the lower limit constraint
float mImpulseLowerLimit; float m_impulseLowerLimit;
/// Accumulated impulse for the upper limit constraint /// Accumulated impulse for the upper limit constraint
float mImpulseUpperLimit; float m_impulseUpperLimit;
/// Accumulated impulse for the motor constraint; /// Accumulated impulse for the motor constraint;
float mImpulseMotor; float m_impulseMotor;
/// Inverse mass matrix K=JM^-1J^t for the 3 translation constraints /// Inverse mass matrix K=JM^-1J^t for the 3 translation constraints
Matrix3x3 mInverseMassMatrixTranslation; Matrix3x3 m_inverseMassMatrixTranslation;
/// Inverse mass matrix K=JM^-1J^t for the 2 rotation constraints /// Inverse mass matrix K=JM^-1J^t for the 2 rotation constraints
Matrix2x2 mInverseMassMatrixRotation; Matrix2x2 m_inverseMassMatrixRotation;
/// Inverse of mass matrix K=JM^-1J^t for the limits and motor constraints (1x1 matrix) /// Inverse of mass matrix K=JM^-1J^t for the limits and motor constraints (1x1 matrix)
float mInverseMassMatrixLimitMotor; float m_inverseMassMatrixLimitMotor;
/// Inverse of mass matrix K=JM^-1J^t for the motor /// Inverse of mass matrix K=JM^-1J^t for the motor
float mInverseMassMatrixMotor; float m_inverseMassMatrixMotor;
/// Bias vector for the error correction for the translation constraints /// Bias vector for the error correction for the translation constraints
Vector3 mBTranslation; Vector3 mBTranslation;
@ -371,7 +371,7 @@ inline float HingeJoint::getMaxMotorTorque() const {
* @return The int32_tensity of the current torque (in Newtons) of the joint motor * @return The int32_tensity of the current torque (in Newtons) of the joint motor
*/ */
inline float HingeJoint::getMotorTorque(float timeStep) const { inline float HingeJoint::getMotorTorque(float timeStep) const {
return mImpulseMotor / timeStep; return m_impulseMotor / timeStep;
} }
// Return the number of bytes used by the joint // Return the number of bytes used by the joint

2
ephysics/constraint/Joint.cpp
View File

@ -14,7 +14,7 @@ using namespace reactphysics3d;
Joint::Joint(const JointInfo& jointInfo) Joint::Joint(const JointInfo& jointInfo)
:mBody1(jointInfo.body1), mBody2(jointInfo.body2), mType(jointInfo.type), :mBody1(jointInfo.body1), mBody2(jointInfo.body2), mType(jointInfo.type),
mPositionCorrectionTechnique(jointInfo.positionCorrectionTechnique), mPositionCorrectionTechnique(jointInfo.positionCorrectionTechnique),
mIsCollisionEnabled(jointInfo.isCollisionEnabled), mIsAlreadyInIsland(false) { mIsCollisionEnabled(jointInfo.isCollisionEnabled), m_isAlreadyInIsland(false) {
assert(mBody1 != NULL); assert(mBody1 != NULL);
assert(mBody2 != NULL); assert(mBody2 != NULL);

4
ephysics/constraint/Joint.h
View File

@ -121,7 +121,7 @@ class Joint {
bool mIsCollisionEnabled; bool mIsCollisionEnabled;
/// True if the joint has already been added int32_to an island /// True if the joint has already been added int32_to an island
bool mIsAlreadyInIsland; bool m_isAlreadyInIsland;
// -------------------- Methods -------------------- // // -------------------- Methods -------------------- //
@ -224,7 +224,7 @@ inline bool Joint::isCollisionEnabled() const {
// Return true if the joint has already been added int32_to an island // Return true if the joint has already been added int32_to an island
inline bool Joint::isAlreadyInIsland() const { inline bool Joint::isAlreadyInIsland() const {
return mIsAlreadyInIsland; return m_isAlreadyInIsland;
} }
} }

202
ephysics/constraint/SliderJoint.cpp
View File

@ -14,8 +14,8 @@ const float SliderJoint::BETA = float(0.2);
// Constructor // Constructor
SliderJoint::SliderJoint(const SliderJointInfo& jointInfo) SliderJoint::SliderJoint(const SliderJointInfo& jointInfo)
: Joint(jointInfo), mImpulseTranslation(0, 0), mImpulseRotation(0, 0, 0), : Joint(jointInfo), m_impulseTranslation(0, 0), m_impulseRotation(0, 0, 0),
mImpulseLowerLimit(0), mImpulseUpperLimit(0), mImpulseMotor(0), m_impulseLowerLimit(0), m_impulseUpperLimit(0), m_impulseMotor(0),
mIsLimitEnabled(jointInfo.isLimitEnabled), mIsMotorEnabled(jointInfo.isMotorEnabled), mIsLimitEnabled(jointInfo.isLimitEnabled), mIsMotorEnabled(jointInfo.isMotorEnabled),
mLowerLimit(jointInfo.minTranslationLimit), mLowerLimit(jointInfo.minTranslationLimit),
mUpperLimit(jointInfo.maxTranslationLimit), mIsLowerLimitViolated(false), mUpperLimit(jointInfo.maxTranslationLimit), mIsLowerLimitViolated(false),
@ -29,8 +29,8 @@ SliderJoint::SliderJoint(const SliderJointInfo& jointInfo)
// Compute the local-space anchor point for each body // Compute the local-space anchor point for each body
const Transform& transform1 = mBody1->getTransform(); const Transform& transform1 = mBody1->getTransform();
const Transform& transform2 = mBody2->getTransform(); const Transform& transform2 = mBody2->getTransform();
mLocalAnchorPointBody1 = transform1.getInverse() * jointInfo.anchorPointWorldSpace; m_localAnchorPointBody1 = transform1.getInverse() * jointInfo.m_m_m_m_anchorPointWorldSpace;
mLocalAnchorPointBody2 = transform2.getInverse() * jointInfo.anchorPointWorldSpace; m_localAnchorPointBody2 = transform2.getInverse() * jointInfo.m_m_m_m_anchorPointWorldSpace;
// Compute the inverse of the initial orientation difference between the two bodies // Compute the inverse of the initial orientation difference between the two bodies
mInitOrientationDifferenceInv = transform2.getOrientation() * mInitOrientationDifferenceInv = transform2.getOrientation() *
@ -63,12 +63,12 @@ void SliderJoint::initBeforeSolve(const ConstraintSolverData& constraintSolverDa
const Quaternion& orientationBody2 = mBody2->getTransform().getOrientation(); const Quaternion& orientationBody2 = mBody2->getTransform().getOrientation();
// Get the inertia tensor of bodies // Get the inertia tensor of bodies
mI1 = mBody1->getInertiaTensorInverseWorld(); m_i1 = mBody1->getInertiaTensorInverseWorld();
mI2 = mBody2->getInertiaTensorInverseWorld(); m_i2 = mBody2->getInertiaTensorInverseWorld();
// Vector from body center to the anchor point // Vector from body center to the anchor point
mR1 = orientationBody1 * mLocalAnchorPointBody1; mR1 = orientationBody1 * m_localAnchorPointBody1;
mR2 = orientationBody2 * mLocalAnchorPointBody2; mR2 = orientationBody2 * m_localAnchorPointBody2;
// Compute the vector u (difference between anchor points) // Compute the vector u (difference between anchor points)
const Vector3 u = x2 + mR2 - x1 - mR1; const Vector3 u = x2 + mR2 - x1 - mR1;
@ -86,12 +86,12 @@ void SliderJoint::initBeforeSolve(const ConstraintSolverData& constraintSolverDa
bool oldIsLowerLimitViolated = mIsLowerLimitViolated; bool oldIsLowerLimitViolated = mIsLowerLimitViolated;
mIsLowerLimitViolated = lowerLimitError <= 0; mIsLowerLimitViolated = lowerLimitError <= 0;
if (mIsLowerLimitViolated != oldIsLowerLimitViolated) { if (mIsLowerLimitViolated != oldIsLowerLimitViolated) {
mImpulseLowerLimit = 0.0; m_impulseLowerLimit = 0.0;
} }
bool oldIsUpperLimitViolated = mIsUpperLimitViolated; bool oldIsUpperLimitViolated = mIsUpperLimitViolated;
mIsUpperLimitViolated = upperLimitError <= 0; mIsUpperLimitViolated = upperLimitError <= 0;
if (mIsUpperLimitViolated != oldIsUpperLimitViolated) { if (mIsUpperLimitViolated != oldIsUpperLimitViolated) {
mImpulseUpperLimit = 0.0; m_impulseUpperLimit = 0.0;
} }
// Compute the cross products used in the Jacobians // Compute the cross products used in the Jacobians
@ -106,10 +106,10 @@ void SliderJoint::initBeforeSolve(const ConstraintSolverData& constraintSolverDa
// 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 = mBody1->mMassInverse + mBody2->mMassInverse; float sumInverseMass = mBody1->mMassInverse + mBody2->mMassInverse;
Vector3 I1R1PlusUCrossN1 = mI1 * mR1PlusUCrossN1; Vector3 I1R1PlusUCrossN1 = m_i1 * mR1PlusUCrossN1;
Vector3 I1R1PlusUCrossN2 = mI1 * mR1PlusUCrossN2; Vector3 I1R1PlusUCrossN2 = m_i1 * mR1PlusUCrossN2;
Vector3 I2R2CrossN1 = mI2 * mR2CrossN1; Vector3 I2R2CrossN1 = m_i2 * mR2CrossN1;
Vector3 I2R2CrossN2 = mI2 * mR2CrossN2; Vector3 I2R2CrossN2 = m_i2 * mR2CrossN2;
const float el11 = sumInverseMass + mR1PlusUCrossN1.dot(I1R1PlusUCrossN1) + const float el11 = sumInverseMass + mR1PlusUCrossN1.dot(I1R1PlusUCrossN1) +
mR2CrossN1.dot(I2R2CrossN1); mR2CrossN1.dot(I2R2CrossN1);
const float el12 = mR1PlusUCrossN1.dot(I1R1PlusUCrossN2) + const float el12 = mR1PlusUCrossN1.dot(I1R1PlusUCrossN2) +
@ -119,9 +119,9 @@ void SliderJoint::initBeforeSolve(const ConstraintSolverData& constraintSolverDa
const float el22 = sumInverseMass + mR1PlusUCrossN2.dot(I1R1PlusUCrossN2) + const float el22 = sumInverseMass + mR1PlusUCrossN2.dot(I1R1PlusUCrossN2) +
mR2CrossN2.dot(I2R2CrossN2); mR2CrossN2.dot(I2R2CrossN2);
Matrix2x2 matrixKTranslation(el11, el12, el21, el22); Matrix2x2 matrixKTranslation(el11, el12, el21, el22);
mInverseMassMatrixTranslationConstraint.setToZero(); m_inverseMassMatrixTranslationConstraint.setToZero();
if (mBody1->getType() == DYNAMIC || mBody2->getType() == DYNAMIC) { if (mBody1->getType() == DYNAMIC || mBody2->getType() == DYNAMIC) {
mInverseMassMatrixTranslationConstraint = matrixKTranslation.getInverse(); m_inverseMassMatrixTranslationConstraint = matrixKTranslation.getInverse();
} }
// Compute the bias "b" of the translation constraint // Compute the bias "b" of the translation constraint
@ -135,9 +135,9 @@ void SliderJoint::initBeforeSolve(const ConstraintSolverData& constraintSolverDa
// 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
// contraints (3x3 matrix) // contraints (3x3 matrix)
mInverseMassMatrixRotationConstraint = mI1 + mI2; m_inverseMassMatrixRotationConstraint = m_i1 + m_i2;
if (mBody1->getType() == DYNAMIC || mBody2->getType() == DYNAMIC) { if (mBody1->getType() == DYNAMIC || mBody2->getType() == DYNAMIC) {
mInverseMassMatrixRotationConstraint = mInverseMassMatrixRotationConstraint.getInverse(); m_inverseMassMatrixRotationConstraint = m_inverseMassMatrixRotationConstraint.getInverse();
} }
// Compute the bias "b" of the rotation constraint // Compute the bias "b" of the rotation constraint
@ -153,11 +153,11 @@ void SliderJoint::initBeforeSolve(const ConstraintSolverData& constraintSolverDa
if (mIsLimitEnabled && (mIsLowerLimitViolated || mIsUpperLimitViolated)) { if (mIsLimitEnabled && (mIsLowerLimitViolated || mIsUpperLimitViolated)) {
// 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)
mInverseMassMatrixLimit = mBody1->mMassInverse + mBody2->mMassInverse + m_inverseMassMatrixLimit = mBody1->mMassInverse + mBody2->mMassInverse +
mR1PlusUCrossSliderAxis.dot(mI1 * mR1PlusUCrossSliderAxis) + mR1PlusUCrossSliderAxis.dot(m_i1 * mR1PlusUCrossSliderAxis) +
mR2CrossSliderAxis.dot(mI2 * mR2CrossSliderAxis); mR2CrossSliderAxis.dot(m_i2 * mR2CrossSliderAxis);
mInverseMassMatrixLimit = (mInverseMassMatrixLimit > 0.0) ? m_inverseMassMatrixLimit = (m_inverseMassMatrixLimit > 0.0) ?
float(1.0) / mInverseMassMatrixLimit : float(0.0); float(1.0) / m_inverseMassMatrixLimit : float(0.0);
// Compute the bias "b" of the lower limit constraint // Compute the bias "b" of the lower limit constraint
mBLowerLimit = 0.0; mBLowerLimit = 0.0;
@ -176,20 +176,20 @@ void SliderJoint::initBeforeSolve(const ConstraintSolverData& constraintSolverDa
if (mIsMotorEnabled) { if (mIsMotorEnabled) {
// 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)
mInverseMassMatrixMotor = mBody1->mMassInverse + mBody2->mMassInverse; m_inverseMassMatrixMotor = mBody1->mMassInverse + mBody2->mMassInverse;
mInverseMassMatrixMotor = (mInverseMassMatrixMotor > 0.0) ? m_inverseMassMatrixMotor = (m_inverseMassMatrixMotor > 0.0) ?
float(1.0) / mInverseMassMatrixMotor : float(0.0); float(1.0) / m_inverseMassMatrixMotor : float(0.0);
} }
// If warm-starting is not enabled // If warm-starting is not enabled
if (!constraintSolverData.isWarmStartingActive) { if (!constraintSolverData.isWarmStartingActive) {
// Reset all the accumulated impulses // Reset all the accumulated impulses
mImpulseTranslation.setToZero(); m_impulseTranslation.setToZero();
mImpulseRotation.setToZero(); m_impulseRotation.setToZero();
mImpulseLowerLimit = 0.0; m_impulseLowerLimit = 0.0;
mImpulseUpperLimit = 0.0; m_impulseUpperLimit = 0.0;
mImpulseMotor = 0.0; m_impulseMotor = 0.0;
} }
} }
@ -207,19 +207,19 @@ void SliderJoint::warmstart(const ConstraintSolverData& constraintSolverData) {
const float inverseMassBody2 = mBody2->mMassInverse; const float inverseMassBody2 = mBody2->mMassInverse;
// 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 = mImpulseUpperLimit - mImpulseLowerLimit; float impulseLimits = m_impulseUpperLimit - m_impulseLowerLimit;
Vector3 linearImpulseLimits = impulseLimits * mSliderAxisWorld; Vector3 linearImpulseLimits = impulseLimits * mSliderAxisWorld;
// 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
Vector3 impulseMotor = mImpulseMotor * mSliderAxisWorld; Vector3 impulseMotor = m_impulseMotor * mSliderAxisWorld;
// 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
Vector3 linearImpulseBody1 = -mN1 * mImpulseTranslation.x - mN2 * mImpulseTranslation.y; Vector3 linearImpulseBody1 = -mN1 * m_impulseTranslation.x - mN2 * m_impulseTranslation.y;
Vector3 angularImpulseBody1 = -mR1PlusUCrossN1 * mImpulseTranslation.x - Vector3 angularImpulseBody1 = -mR1PlusUCrossN1 * m_impulseTranslation.x -
mR1PlusUCrossN2 * mImpulseTranslation.y; mR1PlusUCrossN2 * 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 += -mImpulseRotation; 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;
@ -230,15 +230,15 @@ void SliderJoint::warmstart(const ConstraintSolverData& constraintSolverData) {
// Apply the impulse to the body 1 // Apply the impulse to the body 1
v1 += inverseMassBody1 * linearImpulseBody1; v1 += inverseMassBody1 * linearImpulseBody1;
w1 += mI1 * 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
Vector3 linearImpulseBody2 = mN1 * mImpulseTranslation.x + mN2 * mImpulseTranslation.y; Vector3 linearImpulseBody2 = mN1 * m_impulseTranslation.x + mN2 * m_impulseTranslation.y;
Vector3 angularImpulseBody2 = mR2CrossN1 * mImpulseTranslation.x + Vector3 angularImpulseBody2 = mR2CrossN1 * m_impulseTranslation.x +
mR2CrossN2 * mImpulseTranslation.y; mR2CrossN2 * 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 += mImpulseRotation; 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;
@ -249,7 +249,7 @@ void SliderJoint::warmstart(const ConstraintSolverData& constraintSolverData) {
// Apply the impulse to the body 2 // Apply the impulse to the body 2
v2 += inverseMassBody2 * linearImpulseBody2; v2 += inverseMassBody2 * linearImpulseBody2;
w2 += mI2 * angularImpulseBody2; w2 += m_i2 * angularImpulseBody2;
} }
// Solve the velocity constraint // Solve the velocity constraint
@ -275,8 +275,8 @@ void SliderJoint::solveVelocityConstraint(const ConstraintSolverData& constraint
const Vector2 JvTranslation(el1, el2); const Vector2 JvTranslation(el1, el2);
// Compute the Lagrange multiplier lambda for the 2 translation constraints // Compute the Lagrange multiplier lambda for the 2 translation constraints
Vector2 deltaLambda = mInverseMassMatrixTranslationConstraint * (-JvTranslation -mBTranslation); Vector2 deltaLambda = m_inverseMassMatrixTranslationConstraint * (-JvTranslation -mBTranslation);
mImpulseTranslation += 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 Vector3 linearImpulseBody1 = -mN1 * deltaLambda.x - mN2 * deltaLambda.y; const Vector3 linearImpulseBody1 = -mN1 * deltaLambda.x - mN2 * deltaLambda.y;
@ -285,7 +285,7 @@ void SliderJoint::solveVelocityConstraint(const ConstraintSolverData& constraint
// Apply the impulse to the body 1 // Apply the impulse to the body 1
v1 += inverseMassBody1 * linearImpulseBody1; v1 += inverseMassBody1 * linearImpulseBody1;
w1 += mI1 * 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 Vector3 linearImpulseBody2 = mN1 * deltaLambda.x + mN2 * deltaLambda.y; const Vector3 linearImpulseBody2 = mN1 * deltaLambda.x + mN2 * deltaLambda.y;
@ -293,7 +293,7 @@ void SliderJoint::solveVelocityConstraint(const ConstraintSolverData& constraint
// Apply the impulse to the body 2 // Apply the impulse to the body 2
v2 += inverseMassBody2 * linearImpulseBody2; v2 += inverseMassBody2 * linearImpulseBody2;
w2 += mI2 * angularImpulseBody2; w2 += m_i2 * angularImpulseBody2;
// --------------- Rotation Constraints --------------- // // --------------- Rotation Constraints --------------- //
@ -301,20 +301,20 @@ void SliderJoint::solveVelocityConstraint(const ConstraintSolverData& constraint
const Vector3 JvRotation = w2 - w1; const Vector3 JvRotation = w2 - w1;
// Compute the Lagrange multiplier lambda for the 3 rotation constraints // Compute the Lagrange multiplier lambda for the 3 rotation constraints
Vector3 deltaLambda2 = mInverseMassMatrixRotationConstraint * (-JvRotation - mBRotation); Vector3 deltaLambda2 = m_inverseMassMatrixRotationConstraint * (-JvRotation - mBRotation);
mImpulseRotation += 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
angularImpulseBody1 = -deltaLambda2; angularImpulseBody1 = -deltaLambda2;
// Apply the impulse to the body to body 1 // Apply the impulse to the body to body 1
w1 += mI1 * angularImpulseBody1; w1 += m_i1 * angularImpulseBody1;
// 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 = deltaLambda2; angularImpulseBody2 = deltaLambda2;
// Apply the impulse to the body 2 // Apply the impulse to the body 2
w2 += mI2 * angularImpulseBody2; w2 += m_i2 * angularImpulseBody2;
// --------------- Limits Constraints --------------- // // --------------- Limits Constraints --------------- //
@ -328,10 +328,10 @@ void SliderJoint::solveVelocityConstraint(const ConstraintSolverData& constraint
mSliderAxisWorld.dot(v1) - mR1PlusUCrossSliderAxis.dot(w1); mSliderAxisWorld.dot(v1) - mR1PlusUCrossSliderAxis.dot(w1);
// Compute the Lagrange multiplier lambda for the lower limit constraint // Compute the Lagrange multiplier lambda for the lower limit constraint
float deltaLambdaLower = mInverseMassMatrixLimit * (-JvLowerLimit -mBLowerLimit); float deltaLambdaLower = m_inverseMassMatrixLimit * (-JvLowerLimit -mBLowerLimit);
float lambdaTemp = mImpulseLowerLimit; float lambdaTemp = m_impulseLowerLimit;
mImpulseLowerLimit = std::max(mImpulseLowerLimit + deltaLambdaLower, float(0.0)); m_impulseLowerLimit = std::max(m_impulseLowerLimit + deltaLambdaLower, float(0.0));
deltaLambdaLower = mImpulseLowerLimit - 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 Vector3 linearImpulseBody1 = -deltaLambdaLower * mSliderAxisWorld; const Vector3 linearImpulseBody1 = -deltaLambdaLower * mSliderAxisWorld;
@ -339,7 +339,7 @@ void SliderJoint::solveVelocityConstraint(const ConstraintSolverData& constraint
// Apply the impulse to the body 1 // Apply the impulse to the body 1
v1 += inverseMassBody1 * linearImpulseBody1; v1 += inverseMassBody1 * linearImpulseBody1;
w1 += mI1 * 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 Vector3 linearImpulseBody2 = deltaLambdaLower * mSliderAxisWorld; const Vector3 linearImpulseBody2 = deltaLambdaLower * mSliderAxisWorld;
@ -347,7 +347,7 @@ void SliderJoint::solveVelocityConstraint(const ConstraintSolverData& constraint
// Apply the impulse to the body 2 // Apply the impulse to the body 2
v2 += inverseMassBody2 * linearImpulseBody2; v2 += inverseMassBody2 * linearImpulseBody2;
w2 += mI2 * angularImpulseBody2; w2 += m_i2 * angularImpulseBody2;
} }
// If the upper limit is violated // If the upper limit is violated
@ -358,10 +358,10 @@ void SliderJoint::solveVelocityConstraint(const ConstraintSolverData& constraint
- mSliderAxisWorld.dot(v2) - mR2CrossSliderAxis.dot(w2); - mSliderAxisWorld.dot(v2) - mR2CrossSliderAxis.dot(w2);
// Compute the Lagrange multiplier lambda for the upper limit constraint // Compute the Lagrange multiplier lambda for the upper limit constraint
float deltaLambdaUpper = mInverseMassMatrixLimit * (-JvUpperLimit -mBUpperLimit); float deltaLambdaUpper = m_inverseMassMatrixLimit * (-JvUpperLimit -mBUpperLimit);
float lambdaTemp = mImpulseUpperLimit; float lambdaTemp = m_impulseUpperLimit;
mImpulseUpperLimit = std::max(mImpulseUpperLimit + deltaLambdaUpper, float(0.0)); m_impulseUpperLimit = std::max(m_impulseUpperLimit + deltaLambdaUpper, float(0.0));
deltaLambdaUpper = mImpulseUpperLimit - 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 Vector3 linearImpulseBody1 = deltaLambdaUpper * mSliderAxisWorld; const Vector3 linearImpulseBody1 = deltaLambdaUpper * mSliderAxisWorld;
@ -369,7 +369,7 @@ void SliderJoint::solveVelocityConstraint(const ConstraintSolverData& constraint
// Apply the impulse to the body 1 // Apply the impulse to the body 1
v1 += inverseMassBody1 * linearImpulseBody1; v1 += inverseMassBody1 * linearImpulseBody1;
w1 += mI1 * 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 Vector3 linearImpulseBody2 = -deltaLambdaUpper * mSliderAxisWorld; const Vector3 linearImpulseBody2 = -deltaLambdaUpper * mSliderAxisWorld;
@ -377,7 +377,7 @@ void SliderJoint::solveVelocityConstraint(const ConstraintSolverData& constraint
// Apply the impulse to the body 2 // Apply the impulse to the body 2
v2 += inverseMassBody2 * linearImpulseBody2; v2 += inverseMassBody2 * linearImpulseBody2;
w2 += mI2 * angularImpulseBody2; w2 += m_i2 * angularImpulseBody2;
} }
} }
@ -390,10 +390,10 @@ void SliderJoint::solveVelocityConstraint(const ConstraintSolverData& constraint
// 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 = mMaxMotorForce * constraintSolverData.timeStep;
float deltaLambdaMotor = mInverseMassMatrixMotor * (-JvMotor - mMotorSpeed); float deltaLambdaMotor = m_inverseMassMatrixMotor * (-JvMotor - mMotorSpeed);
float lambdaTemp = mImpulseMotor; float lambdaTemp = m_impulseMotor;
mImpulseMotor = clamp(mImpulseMotor + deltaLambdaMotor, -maxMotorImpulse, maxMotorImpulse); m_impulseMotor = clamp(m_impulseMotor + deltaLambdaMotor, -maxMotorImpulse, maxMotorImpulse);
deltaLambdaMotor = mImpulseMotor - 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 Vector3 linearImpulseBody1 = deltaLambdaMotor * mSliderAxisWorld; const Vector3 linearImpulseBody1 = deltaLambdaMotor * mSliderAxisWorld;
@ -427,12 +427,12 @@ void SliderJoint::solvePositionConstraint(const ConstraintSolverData& constraint
float inverseMassBody2 = mBody2->mMassInverse; float inverseMassBody2 = mBody2->mMassInverse;
// Recompute the inertia tensor of bodies // Recompute the inertia tensor of bodies
mI1 = mBody1->getInertiaTensorInverseWorld(); m_i1 = mBody1->getInertiaTensorInverseWorld();
mI2 = mBody2->getInertiaTensorInverseWorld(); m_i2 = mBody2->getInertiaTensorInverseWorld();
// Vector from body center to the anchor point // Vector from body center to the anchor point
mR1 = q1 * mLocalAnchorPointBody1; mR1 = q1 * m_localAnchorPointBody1;
mR2 = q2 * mLocalAnchorPointBody2; mR2 = q2 * m_localAnchorPointBody2;
// Compute the vector u (difference between anchor points) // Compute the vector u (difference between anchor points)
const Vector3 u = x2 + mR2 - x1 - mR1; const Vector3 u = x2 + mR2 - x1 - mR1;
@ -464,10 +464,10 @@ void SliderJoint::solvePositionConstraint(const ConstraintSolverData& constraint
// 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 = mBody1->mMassInverse + mBody2->mMassInverse; float sumInverseMass = mBody1->mMassInverse + mBody2->mMassInverse;
Vector3 I1R1PlusUCrossN1 = mI1 * mR1PlusUCrossN1; Vector3 I1R1PlusUCrossN1 = m_i1 * mR1PlusUCrossN1;
Vector3 I1R1PlusUCrossN2 = mI1 * mR1PlusUCrossN2; Vector3 I1R1PlusUCrossN2 = m_i1 * mR1PlusUCrossN2;
Vector3 I2R2CrossN1 = mI2 * mR2CrossN1; Vector3 I2R2CrossN1 = m_i2 * mR2CrossN1;
Vector3 I2R2CrossN2 = mI2 * mR2CrossN2; Vector3 I2R2CrossN2 = m_i2 * mR2CrossN2;
const float el11 = sumInverseMass + mR1PlusUCrossN1.dot(I1R1PlusUCrossN1) + const float el11 = sumInverseMass + mR1PlusUCrossN1.dot(I1R1PlusUCrossN1) +
mR2CrossN1.dot(I2R2CrossN1); mR2CrossN1.dot(I2R2CrossN1);
const float el12 = mR1PlusUCrossN1.dot(I1R1PlusUCrossN2) + const float el12 = mR1PlusUCrossN1.dot(I1R1PlusUCrossN2) +
@ -477,16 +477,16 @@ void SliderJoint::solvePositionConstraint(const ConstraintSolverData& constraint
const float el22 = sumInverseMass + mR1PlusUCrossN2.dot(I1R1PlusUCrossN2) + const float el22 = sumInverseMass + mR1PlusUCrossN2.dot(I1R1PlusUCrossN2) +
mR2CrossN2.dot(I2R2CrossN2); mR2CrossN2.dot(I2R2CrossN2);
Matrix2x2 matrixKTranslation(el11, el12, el21, el22); Matrix2x2 matrixKTranslation(el11, el12, el21, el22);
mInverseMassMatrixTranslationConstraint.setToZero(); m_inverseMassMatrixTranslationConstraint.setToZero();
if (mBody1->getType() == DYNAMIC || mBody2->getType() == DYNAMIC) { if (mBody1->getType() == DYNAMIC || mBody2->getType() == DYNAMIC) {
mInverseMassMatrixTranslationConstraint = matrixKTranslation.getInverse(); m_inverseMassMatrixTranslationConstraint = matrixKTranslation.getInverse();
} }
// Compute the position error for the 2 translation constraints // Compute the position error for the 2 translation constraints
const Vector2 translationError(u.dot(mN1), u.dot(mN2)); const Vector2 translationError(u.dot(mN1), u.dot(mN2));
// Compute the Lagrange multiplier lambda for the 2 translation constraints // Compute the Lagrange multiplier lambda for the 2 translation constraints
Vector2 lambdaTranslation = mInverseMassMatrixTranslationConstraint * (-translationError); Vector2 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 Vector3 linearImpulseBody1 = -mN1 * lambdaTranslation.x - mN2 * lambdaTranslation.y; const Vector3 linearImpulseBody1 = -mN1 * lambdaTranslation.x - mN2 * lambdaTranslation.y;
@ -495,7 +495,7 @@ void SliderJoint::solvePositionConstraint(const ConstraintSolverData& constraint
// Apply the impulse to the body 1 // Apply the impulse to the body 1
const Vector3 v1 = inverseMassBody1 * linearImpulseBody1; const Vector3 v1 = inverseMassBody1 * linearImpulseBody1;
Vector3 w1 = mI1 * angularImpulseBody1; Vector3 w1 = m_i1 * angularImpulseBody1;
// Update the body position/orientation of body 1 // Update the body position/orientation of body 1
x1 += v1; x1 += v1;
@ -509,7 +509,7 @@ void SliderJoint::solvePositionConstraint(const ConstraintSolverData& constraint
// Apply the impulse to the body 2 // Apply the impulse to the body 2
const Vector3 v2 = inverseMassBody2 * linearImpulseBody2; const Vector3 v2 = inverseMassBody2 * linearImpulseBody2;
Vector3 w2 = mI2 * angularImpulseBody2; Vector3 w2 = m_i2 * angularImpulseBody2;
// Update the body position/orientation of body 2 // Update the body position/orientation of body 2
x2 += v2; x2 += v2;
@ -520,9 +520,9 @@ void SliderJoint::solvePositionConstraint(const ConstraintSolverData& constraint
// 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
// contraints (3x3 matrix) // contraints (3x3 matrix)
mInverseMassMatrixRotationConstraint = mI1 + mI2; m_inverseMassMatrixRotationConstraint = m_i1 + m_i2;
if (mBody1->getType() == DYNAMIC || mBody2->getType() == DYNAMIC) { if (mBody1->getType() == DYNAMIC || mBody2->getType() == DYNAMIC) {
mInverseMassMatrixRotationConstraint = mInverseMassMatrixRotationConstraint.getInverse(); m_inverseMassMatrixRotationConstraint = m_inverseMassMatrixRotationConstraint.getInverse();
} }
// Compute the position error for the 3 rotation constraints // Compute the position error for the 3 rotation constraints
@ -532,13 +532,13 @@ void SliderJoint::solvePositionConstraint(const ConstraintSolverData& constraint
const Vector3 errorRotation = float(2.0) * qError.getVectorV(); const Vector3 errorRotation = float(2.0) * qError.getVectorV();
// Compute the Lagrange multiplier lambda for the 3 rotation constraints // Compute the Lagrange multiplier lambda for the 3 rotation constraints
Vector3 lambdaRotation = mInverseMassMatrixRotationConstraint * (-errorRotation); Vector3 lambdaRotation = m_inverseMassMatrixRotationConstraint * (-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 = -lambdaRotation; angularImpulseBody1 = -lambdaRotation;
// Apply the impulse to the body 1 // Apply the impulse to the body 1
w1 = mI1 * angularImpulseBody1; w1 = m_i1 * angularImpulseBody1;
// Update the body position/orientation of body 1 // Update the body position/orientation of body 1
q1 += Quaternion(0, w1) * q1 * float(0.5); q1 += Quaternion(0, w1) * q1 * float(0.5);
@ -548,7 +548,7 @@ void SliderJoint::solvePositionConstraint(const ConstraintSolverData& constraint
angularImpulseBody2 = lambdaRotation; angularImpulseBody2 = lambdaRotation;
// Apply the impulse to the body 2 // Apply the impulse to the body 2
w2 = mI2 * angularImpulseBody2; w2 = m_i2 * angularImpulseBody2;
// Update the body position/orientation of body 2 // Update the body position/orientation of body 2
q2 += Quaternion(0, w2) * q2 * float(0.5); q2 += Quaternion(0, w2) * q2 * float(0.5);
@ -561,18 +561,18 @@ void SliderJoint::solvePositionConstraint(const ConstraintSolverData& constraint
if (mIsLowerLimitViolated || mIsUpperLimitViolated) { if (mIsLowerLimitViolated || mIsUpperLimitViolated) {
// 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)
mInverseMassMatrixLimit = mBody1->mMassInverse + mBody2->mMassInverse + m_inverseMassMatrixLimit = mBody1->mMassInverse + mBody2->mMassInverse +
mR1PlusUCrossSliderAxis.dot(mI1 * mR1PlusUCrossSliderAxis) + mR1PlusUCrossSliderAxis.dot(m_i1 * mR1PlusUCrossSliderAxis) +
mR2CrossSliderAxis.dot(mI2 * mR2CrossSliderAxis); mR2CrossSliderAxis.dot(m_i2 * mR2CrossSliderAxis);
mInverseMassMatrixLimit = (mInverseMassMatrixLimit > 0.0) ? m_inverseMassMatrixLimit = (m_inverseMassMatrixLimit > 0.0) ?
float(1.0) / mInverseMassMatrixLimit : float(0.0); float(1.0) / m_inverseMassMatrixLimit : float(0.0);
} }
// If the lower limit is violated // If the lower limit is violated
if (mIsLowerLimitViolated) { if (mIsLowerLimitViolated) {
// Compute the Lagrange multiplier lambda for the lower limit constraint // Compute the Lagrange multiplier lambda for the lower limit constraint
float lambdaLowerLimit = mInverseMassMatrixLimit * (-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 Vector3 linearImpulseBody1 = -lambdaLowerLimit * mSliderAxisWorld; const Vector3 linearImpulseBody1 = -lambdaLowerLimit * mSliderAxisWorld;
@ -580,7 +580,7 @@ void SliderJoint::solvePositionConstraint(const ConstraintSolverData& constraint
// Apply the impulse to the body 1 // Apply the impulse to the body 1
const Vector3 v1 = inverseMassBody1 * linearImpulseBody1; const Vector3 v1 = inverseMassBody1 * linearImpulseBody1;
const Vector3 w1 = mI1 * angularImpulseBody1; const Vector3 w1 = m_i1 * angularImpulseBody1;
// Update the body position/orientation of body 1 // Update the body position/orientation of body 1
x1 += v1; x1 += v1;
@ -593,7 +593,7 @@ void SliderJoint::solvePositionConstraint(const ConstraintSolverData& constraint
// Apply the impulse to the body 2 // Apply the impulse to the body 2
const Vector3 v2 = inverseMassBody2 * linearImpulseBody2; const Vector3 v2 = inverseMassBody2 * linearImpulseBody2;
const Vector3 w2 = mI2 * angularImpulseBody2; const Vector3 w2 = m_i2 * angularImpulseBody2;
// Update the body position/orientation of body 2 // Update the body position/orientation of body 2
x2 += v2; x2 += v2;
@ -605,7 +605,7 @@ void SliderJoint::solvePositionConstraint(const ConstraintSolverData& constraint
if (mIsUpperLimitViolated) { if (mIsUpperLimitViolated) {
// Compute the Lagrange multiplier lambda for the upper limit constraint // Compute the Lagrange multiplier lambda for the upper limit constraint
float lambdaUpperLimit = mInverseMassMatrixLimit * (-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 Vector3 linearImpulseBody1 = lambdaUpperLimit * mSliderAxisWorld; const Vector3 linearImpulseBody1 = lambdaUpperLimit * mSliderAxisWorld;
@ -613,7 +613,7 @@ void SliderJoint::solvePositionConstraint(const ConstraintSolverData& constraint
// Apply the impulse to the body 1 // Apply the impulse to the body 1
const Vector3 v1 = inverseMassBody1 * linearImpulseBody1; const Vector3 v1 = inverseMassBody1 * linearImpulseBody1;
const Vector3 w1 = mI1 * angularImpulseBody1; const Vector3 w1 = m_i1 * angularImpulseBody1;
// Update the body position/orientation of body 1 // Update the body position/orientation of body 1
x1 += v1; x1 += v1;
@ -626,7 +626,7 @@ void SliderJoint::solvePositionConstraint(const ConstraintSolverData& constraint
// Apply the impulse to the body 2 // Apply the impulse to the body 2
const Vector3 v2 = inverseMassBody2 * linearImpulseBody2; const Vector3 v2 = inverseMassBody2 * linearImpulseBody2;
const Vector3 w2 = mI2 * angularImpulseBody2; const Vector3 w2 = m_i2 * angularImpulseBody2;
// Update the body position/orientation of body 2 // Update the body position/orientation of body 2
x2 += v2; x2 += v2;
@ -660,7 +660,7 @@ void SliderJoint::enableLimit(bool isLimitEnabled) {
void SliderJoint::enableMotor(bool isMotorEnabled) { void SliderJoint::enableMotor(bool isMotorEnabled) {
mIsMotorEnabled = isMotorEnabled; mIsMotorEnabled = isMotorEnabled;
mImpulseMotor = 0.0; m_impulseMotor = 0.0;
// Wake up the two bodies of the joint // Wake up the two bodies of the joint
mBody1->setIsSleeping(false); mBody1->setIsSleeping(false);
@ -682,8 +682,8 @@ float SliderJoint::getTranslation() const {
const Quaternion& q2 = mBody2->getTransform().getOrientation(); const Quaternion& q2 = mBody2->getTransform().getOrientation();
// Compute the two anchor points in world-space coordinates // Compute the two anchor points in world-space coordinates
const Vector3 anchorBody1 = x1 + q1 * mLocalAnchorPointBody1; const Vector3 anchorBody1 = x1 + q1 * m_localAnchorPointBody1;
const Vector3 anchorBody2 = x2 + q2 * mLocalAnchorPointBody2; const Vector3 anchorBody2 = x2 + q2 * m_localAnchorPointBody2;
// Compute the vector u (difference between anchor points) // Compute the vector u (difference between anchor points)
const Vector3 u = anchorBody2 - anchorBody1; const Vector3 u = anchorBody2 - anchorBody1;
@ -734,8 +734,8 @@ void SliderJoint::setMaxTranslationLimit(float upperLimit) {
void SliderJoint::resetLimits() { void SliderJoint::resetLimits() {
// Reset the accumulated impulses for the limits // Reset the accumulated impulses for the limits
mImpulseLowerLimit = 0.0; m_impulseLowerLimit = 0.0;
mImpulseUpperLimit = 0.0; m_impulseUpperLimit = 0.0;
// Wake up the two bodies of the joint // Wake up the two bodies of the joint
mBody1->setIsSleeping(false); mBody1->setIsSleeping(false);

36
ephysics/constraint/SliderJoint.h
View File

@ -23,7 +23,7 @@ struct SliderJointInfo : public JointInfo {
// -------------------- Attributes -------------------- // // -------------------- Attributes -------------------- //
/// Anchor point (in world-space coordinates) /// Anchor point (in world-space coordinates)
Vector3 anchorPointWorldSpace; Vector3 m_m_m_m_anchorPointWorldSpace;
/// Slider axis (in world-space coordinates) /// Slider axis (in world-space coordinates)
Vector3 sliderAxisWorldSpace; Vector3 sliderAxisWorldSpace;
@ -57,7 +57,7 @@ struct SliderJointInfo : public JointInfo {
const Vector3& initAnchorPointWorldSpace, const Vector3& initAnchorPointWorldSpace,
const Vector3& initSliderAxisWorldSpace) const Vector3& initSliderAxisWorldSpace)
: JointInfo(rigidBody1, rigidBody2, SLIDERJOINT), : JointInfo(rigidBody1, rigidBody2, SLIDERJOINT),
anchorPointWorldSpace(initAnchorPointWorldSpace), m_m_m_m_anchorPointWorldSpace(initAnchorPointWorldSpace),
sliderAxisWorldSpace(initSliderAxisWorldSpace), sliderAxisWorldSpace(initSliderAxisWorldSpace),
isLimitEnabled(false), isMotorEnabled(false), minTranslationLimit(-1.0), isLimitEnabled(false), isMotorEnabled(false), minTranslationLimit(-1.0),
maxTranslationLimit(1.0), motorSpeed(0), maxMotorForce(0) {} maxTranslationLimit(1.0), motorSpeed(0), maxMotorForce(0) {}
@ -76,7 +76,7 @@ struct SliderJointInfo : public JointInfo {
const Vector3& initSliderAxisWorldSpace, const Vector3& initSliderAxisWorldSpace,
float initMinTranslationLimit, float initMaxTranslationLimit) float initMinTranslationLimit, float initMaxTranslationLimit)
: JointInfo(rigidBody1, rigidBody2, SLIDERJOINT), : JointInfo(rigidBody1, rigidBody2, SLIDERJOINT),
anchorPointWorldSpace(initAnchorPointWorldSpace), m_m_m_m_anchorPointWorldSpace(initAnchorPointWorldSpace),
sliderAxisWorldSpace(initSliderAxisWorldSpace), sliderAxisWorldSpace(initSliderAxisWorldSpace),
isLimitEnabled(true), isMotorEnabled(false), isLimitEnabled(true), isMotorEnabled(false),
minTranslationLimit(initMinTranslationLimit), minTranslationLimit(initMinTranslationLimit),
@ -100,7 +100,7 @@ struct SliderJointInfo : public JointInfo {
float initMinTranslationLimit, float initMaxTranslationLimit, float initMinTranslationLimit, float initMaxTranslationLimit,
float initMotorSpeed, float initMaxMotorForce) float initMotorSpeed, float initMaxMotorForce)
: JointInfo(rigidBody1, rigidBody2, SLIDERJOINT), : JointInfo(rigidBody1, rigidBody2, SLIDERJOINT),
anchorPointWorldSpace(initAnchorPointWorldSpace), m_m_m_m_anchorPointWorldSpace(initAnchorPointWorldSpace),
sliderAxisWorldSpace(initSliderAxisWorldSpace), sliderAxisWorldSpace(initSliderAxisWorldSpace),
isLimitEnabled(true), isMotorEnabled(true), isLimitEnabled(true), isMotorEnabled(true),
minTranslationLimit(initMinTranslationLimit), minTranslationLimit(initMinTranslationLimit),
@ -126,19 +126,19 @@ class SliderJoint : public Joint {
// -------------------- Attributes -------------------- // // -------------------- Attributes -------------------- //
/// Anchor point of body 1 (in local-space coordinates of body 1) /// Anchor point of body 1 (in local-space coordinates of body 1)
Vector3 mLocalAnchorPointBody1; Vector3 m_localAnchorPointBody1;
/// Anchor point of body 2 (in local-space coordinates of body 2) /// Anchor point of body 2 (in local-space coordinates of body 2)
Vector3 mLocalAnchorPointBody2; Vector3 m_localAnchorPointBody2;
/// Slider axis (in local-space coordinates of body 1) /// Slider axis (in local-space coordinates of body 1)
Vector3 mSliderAxisBody1; Vector3 mSliderAxisBody1;
/// Inertia tensor of body 1 (in world-space coordinates) /// Inertia tensor of body 1 (in world-space coordinates)
Matrix3x3 mI1; Matrix3x3 m_i1;
/// Inertia tensor of body 2 (in world-space coordinates) /// Inertia tensor of body 2 (in world-space coordinates)
Matrix3x3 mI2; Matrix3x3 m_i2;
/// Inverse of the initial orientation difference between the two bodies /// Inverse of the initial orientation difference between the two bodies
Quaternion mInitOrientationDifferenceInv; Quaternion mInitOrientationDifferenceInv;
@ -186,31 +186,31 @@ class SliderJoint : public Joint {
float mBUpperLimit; float mBUpperLimit;
/// Inverse of mass matrix K=JM^-1J^t for the translation constraint (2x2 matrix) /// Inverse of mass matrix K=JM^-1J^t for the translation constraint (2x2 matrix)
Matrix2x2 mInverseMassMatrixTranslationConstraint; Matrix2x2 m_inverseMassMatrixTranslationConstraint;
/// Inverse of mass matrix K=JM^-1J^t for the rotation constraint (3x3 matrix) /// Inverse of mass matrix K=JM^-1J^t for the rotation constraint (3x3 matrix)
Matrix3x3 mInverseMassMatrixRotationConstraint; Matrix3x3 m_inverseMassMatrixRotationConstraint;
/// Inverse of mass matrix K=JM^-1J^t for the upper and lower limit constraints (1x1 matrix) /// Inverse of mass matrix K=JM^-1J^t for the upper and lower limit constraints (1x1 matrix)
float mInverseMassMatrixLimit; float m_inverseMassMatrixLimit;
/// Inverse of mass matrix K=JM^-1J^t for the motor /// Inverse of mass matrix K=JM^-1J^t for the motor
float mInverseMassMatrixMotor; float m_inverseMassMatrixMotor;
/// Accumulated impulse for the 2 translation constraints /// Accumulated impulse for the 2 translation constraints
Vector2 mImpulseTranslation; Vector2 m_impulseTranslation;
/// Accumulated impulse for the 3 rotation constraints /// Accumulated impulse for the 3 rotation constraints
Vector3 mImpulseRotation; Vector3 m_impulseRotation;
/// Accumulated impulse for the lower limit constraint /// Accumulated impulse for the lower limit constraint
float mImpulseLowerLimit; float m_impulseLowerLimit;
/// Accumulated impulse for the upper limit constraint /// Accumulated impulse for the upper limit constraint
float mImpulseUpperLimit; float m_impulseUpperLimit;
/// Accumulated impulse for the motor /// Accumulated impulse for the motor
float mImpulseMotor; float m_impulseMotor;
/// True if the slider limits are enabled /// True if the slider limits are enabled
bool mIsLimitEnabled; bool mIsLimitEnabled;
@ -372,7 +372,7 @@ inline float SliderJoint::getMaxMotorForce() const {
* @return The current force of the joint motor (in Newton x meters) * @return The current force of the joint motor (in Newton x meters)
*/ */
inline float SliderJoint::getMotorForce(float timeStep) const { inline float SliderJoint::getMotorForce(float timeStep) const {
return mImpulseMotor / timeStep; return m_impulseMotor / timeStep;
} }
// Return the number of bytes used by the joint // Return the number of bytes used by the joint

12
ephysics/engine/CollisionWorld.cpp
View File

@ -14,7 +14,7 @@ using namespace std;
// Constructor // Constructor
CollisionWorld::CollisionWorld() CollisionWorld::CollisionWorld()
: mCollisionDetection(this, mMemoryAllocator), mCurrentBodyID(0), : m_collisionDetection(this, mMemoryAllocator), mCurrentBodyID(0),
mEventListener(NULL) { mEventListener(NULL) {
} }
@ -147,7 +147,7 @@ void CollisionWorld::testCollision(const ProxyShape* shape,
std::set<uint32_t> emptySet; std::set<uint32_t> emptySet;
// Perform the collision detection and report contacts // Perform the collision detection and report contacts
mCollisionDetection.testCollisionBetweenShapes(callback, shapes, emptySet); m_collisionDetection.testCollisionBetweenShapes(callback, shapes, emptySet);
} }
// Test and report collisions between two given shapes // Test and report collisions between two given shapes
@ -170,7 +170,7 @@ void CollisionWorld::testCollision(const ProxyShape* shape1,
shapes2.insert(shape2->mBroadPhaseID); shapes2.insert(shape2->mBroadPhaseID);
// Perform the collision detection and report contacts // Perform the collision detection and report contacts
mCollisionDetection.testCollisionBetweenShapes(callback, shapes1, shapes2); m_collisionDetection.testCollisionBetweenShapes(callback, shapes1, shapes2);
} }
// Test and report collisions between a body and all the others bodies of the // Test and report collisions between a body and all the others bodies of the
@ -197,7 +197,7 @@ void CollisionWorld::testCollision(const CollisionBody* body,
std::set<uint32_t> emptySet; std::set<uint32_t> emptySet;
// Perform the collision detection and report contacts // Perform the collision detection and report contacts
mCollisionDetection.testCollisionBetweenShapes(callback, shapes1, emptySet); m_collisionDetection.testCollisionBetweenShapes(callback, shapes1, emptySet);
} }
// Test and report collisions between two bodies // Test and report collisions between two bodies
@ -227,7 +227,7 @@ void CollisionWorld::testCollision(const CollisionBody* body1,
} }
// Perform the collision detection and report contacts // Perform the collision detection and report contacts
mCollisionDetection.testCollisionBetweenShapes(callback, shapes1, shapes2); m_collisionDetection.testCollisionBetweenShapes(callback, shapes1, shapes2);
} }
// Test and report collisions between all shapes of the world // Test and report collisions between all shapes of the world
@ -242,6 +242,6 @@ void CollisionWorld::testCollision(CollisionCallback* callback) {
std::set<uint32_t> emptySet; std::set<uint32_t> emptySet;
// Perform the collision detection and report contacts // Perform the collision detection and report contacts
mCollisionDetection.testCollisionBetweenShapes(callback, emptySet, emptySet); m_collisionDetection.testCollisionBetweenShapes(callback, emptySet, emptySet);
} }

8
ephysics/engine/CollisionWorld.h
View File

@ -40,7 +40,7 @@ class CollisionWorld {
// -------------------- Attributes -------------------- // // -------------------- Attributes -------------------- //
/// Reference to the collision detection /// Reference to the collision detection
CollisionDetection mCollisionDetection; CollisionDetection m_collisionDetection;
/// All the bodies (rigid and soft) of the world /// All the bodies (rigid and soft) of the world
std::set<CollisionBody*> mBodies; std::set<CollisionBody*> mBodies;
@ -163,7 +163,7 @@ inline std::set<CollisionBody*>::iterator CollisionWorld::getBodiesEndIterator()
* which collision detection algorithm to use for two given collision shapes * which collision detection algorithm to use for two given collision shapes
*/ */
inline void CollisionWorld::setCollisionDispatch(CollisionDispatch* collisionDispatch) { inline void CollisionWorld::setCollisionDispatch(CollisionDispatch* collisionDispatch) {
mCollisionDetection.setCollisionDispatch(collisionDispatch); m_collisionDetection.setCollisionDispatch(collisionDispatch);
} }
// Ray cast method // Ray cast method
@ -176,7 +176,7 @@ inline void CollisionWorld::setCollisionDispatch(CollisionDispatch* collisionDis
inline void CollisionWorld::raycast(const Ray& ray, inline void CollisionWorld::raycast(const Ray& ray,
RaycastCallback* raycastCallback, RaycastCallback* raycastCallback,
unsigned short raycastWithCategoryMaskBits) const { unsigned short raycastWithCategoryMaskBits) const {
mCollisionDetection.raycast(raycastCallback, ray, raycastWithCategoryMaskBits); m_collisionDetection.raycast(raycastCallback, ray, raycastWithCategoryMaskBits);
} }
// Test if the AABBs of two proxy shapes overlap // Test if the AABBs of two proxy shapes overlap
@ -188,7 +188,7 @@ inline void CollisionWorld::raycast(const Ray& ray,
inline bool CollisionWorld::testAABBOverlap(const ProxyShape* shape1, inline bool CollisionWorld::testAABBOverlap(const ProxyShape* shape1,
const ProxyShape* shape2) const { const ProxyShape* shape2) const {
return mCollisionDetection.testAABBOverlap(shape1, shape2); return m_collisionDetection.testAABBOverlap(shape1, shape2);
} }
// Class CollisionCallback // Class CollisionCallback

56
ephysics/engine/DynamicsWorld.cpp
View File

@ -25,7 +25,7 @@ DynamicsWorld::DynamicsWorld(const Vector3 &gravity)
mConstraintSolver(mMapBodyToConstrainedVelocityIndex), mConstraintSolver(mMapBodyToConstrainedVelocityIndex),
mNbVelocitySolverIterations(DEFAULT_VELOCITY_SOLVER_NB_ITERATIONS), mNbVelocitySolverIterations(DEFAULT_VELOCITY_SOLVER_NB_ITERATIONS),
mNbPositionSolverIterations(DEFAULT_POSITION_SOLVER_NB_ITERATIONS), mNbPositionSolverIterations(DEFAULT_POSITION_SOLVER_NB_ITERATIONS),
mIsSleepingEnabled(SPLEEPING_ENABLED), mGravity(gravity), m_isSleepingEnabled(SPLEEPING_ENABLED), mGravity(gravity),
mIsGravityEnabled(true), mConstrainedLinearVelocities(NULL), mIsGravityEnabled(true), mConstrainedLinearVelocities(NULL),
mConstrainedAngularVelocities(NULL), mSplitLinearVelocities(NULL), mConstrainedAngularVelocities(NULL), mSplitLinearVelocities(NULL),
mSplitAngularVelocities(NULL), mConstrainedPositions(NULL), mSplitAngularVelocities(NULL), mConstrainedPositions(NULL),
@ -122,7 +122,7 @@ void DynamicsWorld::update(float timeStep) {
} }
// Compute the collision detection // Compute the collision detection
mCollisionDetection.computeCollisionDetection(); m_collisionDetection.computeCollisionDetection();
// Compute the islands (separate groups of bodies with constraints between each others) // Compute the islands (separate groups of bodies with constraints between each others)
computeIslands(); computeIslands();
@ -142,7 +142,7 @@ void DynamicsWorld::update(float timeStep) {
// Update the state (positions and velocities) of the bodies // Update the state (positions and velocities) of the bodies
updateBodiesState(); updateBodiesState();
if (mIsSleepingEnabled) updateSleepingBodies(); if (m_isSleepingEnabled) updateSleepingBodies();
// Notify the event listener about the end of an int32_ternal tick // Notify the event listener about the end of an int32_ternal tick
if (mEventListener != NULL) mEventListener->endInternalTick(); if (mEventListener != NULL) mEventListener->endInternalTick();
@ -538,7 +538,7 @@ Joint* DynamicsWorld::createJoint(const JointInfo& jointInfo) {
if (!jointInfo.isCollisionEnabled) { if (!jointInfo.isCollisionEnabled) {
// Add the pair of bodies in the set of body pairs that cannot collide with each other // Add the pair of bodies in the set of body pairs that cannot collide with each other
mCollisionDetection.addNoCollisionPair(jointInfo.body1, jointInfo.body2); m_collisionDetection.addNoCollisionPair(jointInfo.body1, jointInfo.body2);
} }
// Add the joint int32_to the world // Add the joint int32_to the world
@ -563,7 +563,7 @@ void DynamicsWorld::destroyJoint(Joint* joint) {
if (!joint->isCollisionEnabled()) { if (!joint->isCollisionEnabled()) {
// Remove the pair of bodies from the set of body pairs that cannot collide with each other // Remove the pair of bodies from the set of body pairs that cannot collide with each other
mCollisionDetection.removeNoCollisionPair(joint->getBody1(), joint->getBody2()); m_collisionDetection.removeNoCollisionPair(joint->getBody1(), joint->getBody2());
} }
// Wake up the two bodies of the joint // Wake up the two bodies of the joint
@ -645,7 +645,7 @@ void DynamicsWorld::computeIslands() {
nbContactManifolds += nbBodyManifolds; nbContactManifolds += nbBodyManifolds;
} }
for (std::set<Joint*>::iterator it = mJoints.begin(); it != mJoints.end(); ++it) { for (std::set<Joint*>::iterator it = mJoints.begin(); it != mJoints.end(); ++it) {
(*it)->mIsAlreadyInIsland = false; (*it)->m_isAlreadyInIsland = false;
} }
// Create a stack (using an array) for the rigid bodies to visit during the Depth First Search // Create a stack (using an array) for the rigid bodies to visit during the Depth First Search
@ -658,7 +658,7 @@ void DynamicsWorld::computeIslands() {
RigidBody* body = *it; RigidBody* body = *it;
// If the body has already been added to an island, we go to the next body // If the body has already been added to an island, we go to the next body
if (body->mIsAlreadyInIsland) continue; if (body->m_isAlreadyInIsland) continue;
// If the body is static, we go to the next body // If the body is static, we go to the next body
if (body->getType() == STATIC) continue; if (body->getType() == STATIC) continue;
@ -670,7 +670,7 @@ void DynamicsWorld::computeIslands() {
uint32_t stackIndex = 0; uint32_t stackIndex = 0;
stackBodiesToVisit[stackIndex] = body; stackBodiesToVisit[stackIndex] = body;
stackIndex++; stackIndex++;
body->mIsAlreadyInIsland = true; body->m_isAlreadyInIsland = true;
// Create the new island // Create the new island
void* allocatedMemoryIsland = mMemoryAllocator.allocate(sizeof(Island)); void* allocatedMemoryIsland = mMemoryAllocator.allocate(sizeof(Island));
@ -710,7 +710,7 @@ void DynamicsWorld::computeIslands() {
// Add the contact manifold int32_to the island // Add the contact manifold int32_to the island
mIslands[mNbIslands]->addContactManifold(contactManifold); mIslands[mNbIslands]->addContactManifold(contactManifold);
contactManifold->mIsAlreadyInIsland = true; contactManifold->m_isAlreadyInIsland = true;
// Get the other body of the contact manifold // Get the other body of the contact manifold
RigidBody* body1 = static_cast<RigidBody*>(contactManifold->getBody1()); RigidBody* body1 = static_cast<RigidBody*>(contactManifold->getBody1());
@ -718,12 +718,12 @@ void DynamicsWorld::computeIslands() {
RigidBody* otherBody = (body1->getID() == bodyToVisit->getID()) ? body2 : body1; RigidBody* otherBody = (body1->getID() == bodyToVisit->getID()) ? body2 : body1;
// Check if the other body has already been added to the island // Check if the other body has already been added to the island
if (otherBody->mIsAlreadyInIsland) continue; if (otherBody->m_isAlreadyInIsland) continue;
// Insert the other body int32_to the stack of bodies to visit // Insert the other body int32_to the stack of bodies to visit
stackBodiesToVisit[stackIndex] = otherBody; stackBodiesToVisit[stackIndex] = otherBody;
stackIndex++; stackIndex++;
otherBody->mIsAlreadyInIsland = true; otherBody->m_isAlreadyInIsland = true;
} }
// For each joint in which the current body is involved // For each joint in which the current body is involved
@ -738,7 +738,7 @@ void DynamicsWorld::computeIslands() {
// Add the joint int32_to the island // Add the joint int32_to the island
mIslands[mNbIslands]->addJoint(joint); mIslands[mNbIslands]->addJoint(joint);
joint->mIsAlreadyInIsland = true; joint->m_isAlreadyInIsland = true;
// Get the other body of the contact manifold // Get the other body of the contact manifold
RigidBody* body1 = static_cast<RigidBody*>(joint->getBody1()); RigidBody* body1 = static_cast<RigidBody*>(joint->getBody1());
@ -746,12 +746,12 @@ void DynamicsWorld::computeIslands() {
RigidBody* otherBody = (body1->getID() == bodyToVisit->getID()) ? body2 : body1; RigidBody* otherBody = (body1->getID() == bodyToVisit->getID()) ? body2 : body1;
// Check if the other body has already been added to the island // Check if the other body has already been added to the island
if (otherBody->mIsAlreadyInIsland) continue; if (otherBody->m_isAlreadyInIsland) continue;
// Insert the other body int32_to the stack of bodies to visit // Insert the other body int32_to the stack of bodies to visit
stackBodiesToVisit[stackIndex] = otherBody; stackBodiesToVisit[stackIndex] = otherBody;
stackIndex++; stackIndex++;
otherBody->mIsAlreadyInIsland = true; otherBody->m_isAlreadyInIsland = true;
} }
} }
@ -760,7 +760,7 @@ void DynamicsWorld::computeIslands() {
for (uint32_t i=0; i < mIslands[mNbIslands]->mNbBodies; i++) { for (uint32_t i=0; i < mIslands[mNbIslands]->mNbBodies; i++) {
if (mIslands[mNbIslands]->mBodies[i]->getType() == STATIC) { if (mIslands[mNbIslands]->mBodies[i]->getType() == STATIC) {
mIslands[mNbIslands]->mBodies[i]->mIsAlreadyInIsland = false; mIslands[mNbIslands]->mBodies[i]->m_isAlreadyInIsland = false;
} }
} }
@ -799,15 +799,15 @@ void DynamicsWorld::updateSleepingBodies() {
!bodies[b]->isAllowedToSleep()) { !bodies[b]->isAllowedToSleep()) {
// Reset the sleep time of the body // Reset the sleep time of the body
bodies[b]->mSleepTime = float(0.0); bodies[b]->m_sleepTime = float(0.0);
minSleepTime = float(0.0); minSleepTime = float(0.0);
} }
else { // If the body velocity is bellow the sleeping velocity threshold else { // If the body velocity is bellow the sleeping velocity threshold
// Increase the sleep time // Increase the sleep time
bodies[b]->mSleepTime += mTimeStep; bodies[b]->m_sleepTime += mTimeStep;
if (bodies[b]->mSleepTime < minSleepTime) { if (bodies[b]->m_sleepTime < minSleepTime) {
minSleepTime = bodies[b]->mSleepTime; minSleepTime = bodies[b]->m_sleepTime;
} }
} }
} }
@ -833,9 +833,9 @@ void DynamicsWorld::updateSleepingBodies() {
* and false otherwise * and false otherwise
*/ */
void DynamicsWorld::enableSleeping(bool isSleepingEnabled) { void DynamicsWorld::enableSleeping(bool isSleepingEnabled) {
mIsSleepingEnabled = isSleepingEnabled; m_isSleepingEnabled = isSleepingEnabled;
if (!mIsSleepingEnabled) { if (!m_isSleepingEnabled) {
// For each body of the world // For each body of the world
std::set<RigidBody*>::iterator it; std::set<RigidBody*>::iterator it;
@ -864,7 +864,7 @@ void DynamicsWorld::testCollision(const ProxyShape* shape,
std::set<uint32_t> emptySet; std::set<uint32_t> emptySet;
// Perform the collision detection and report contacts // Perform the collision detection and report contacts
mCollisionDetection.reportCollisionBetweenShapes(callback, shapes, emptySet); m_collisionDetection.reportCollisionBetweenShapes(callback, shapes, emptySet);
} }
// Test and report collisions between two given shapes. // Test and report collisions between two given shapes.
@ -886,7 +886,7 @@ void DynamicsWorld::testCollision(const ProxyShape* shape1,
shapes2.insert(shape2->mBroadPhaseID); shapes2.insert(shape2->mBroadPhaseID);
// Perform the collision detection and report contacts // Perform the collision detection and report contacts
mCollisionDetection.reportCollisionBetweenShapes(callback, shapes1, shapes2); m_collisionDetection.reportCollisionBetweenShapes(callback, shapes1, shapes2);
} }
// Test and report collisions between a body and all the others bodies of the // Test and report collisions between a body and all the others bodies of the
@ -912,7 +912,7 @@ void DynamicsWorld::testCollision(const CollisionBody* body,
std::set<uint32_t> emptySet; std::set<uint32_t> emptySet;
// Perform the collision detection and report contacts // Perform the collision detection and report contacts
mCollisionDetection.reportCollisionBetweenShapes(callback, shapes1, emptySet); m_collisionDetection.reportCollisionBetweenShapes(callback, shapes1, emptySet);
} }
// Test and report collisions between two bodies. // Test and report collisions between two bodies.
@ -941,7 +941,7 @@ void DynamicsWorld::testCollision(const CollisionBody* body1,
} }
// Perform the collision detection and report contacts // Perform the collision detection and report contacts
mCollisionDetection.reportCollisionBetweenShapes(callback, shapes1, shapes2); m_collisionDetection.reportCollisionBetweenShapes(callback, shapes1, shapes2);
} }
// Test and report collisions between all shapes of the world. // Test and report collisions between all shapes of the world.
@ -955,7 +955,7 @@ void DynamicsWorld::testCollision(CollisionCallback* callback) {
std::set<uint32_t> emptySet; std::set<uint32_t> emptySet;
// Perform the collision detection and report contacts // Perform the collision detection and report contacts
mCollisionDetection.reportCollisionBetweenShapes(callback, emptySet, emptySet); m_collisionDetection.reportCollisionBetweenShapes(callback, emptySet, emptySet);
} }
/// Return the list of all contacts of the world /// Return the list of all contacts of the world
@ -965,8 +965,8 @@ std::vector<const ContactManifold*> DynamicsWorld::getContactsList() const {
// For each currently overlapping pair of bodies // For each currently overlapping pair of bodies
std::map<overlappingpairid, OverlappingPair*>::const_iterator it; std::map<overlappingpairid, OverlappingPair*>::const_iterator it;
for (it = mCollisionDetection.mOverlappingPairs.begin(); for (it = m_collisionDetection.mOverlappingPairs.begin();
it != mCollisionDetection.mOverlappingPairs.end(); ++it) { it != m_collisionDetection.mOverlappingPairs.end(); ++it) {
OverlappingPair* pair = it->second; OverlappingPair* pair = it->second;

4
ephysics/engine/DynamicsWorld.h
View File

@ -42,7 +42,7 @@ class DynamicsWorld : public CollisionWorld {
uint32_t mNbPositionSolverIterations; uint32_t mNbPositionSolverIterations;
/// True if the spleeping technique for inactive bodies is enabled /// True if the spleeping technique for inactive bodies is enabled
bool mIsSleepingEnabled; bool m_isSleepingEnabled;
/// All the rigid bodies of the physics world /// All the rigid bodies of the physics world
std::set<RigidBody*> mRigidBodies; std::set<RigidBody*> mRigidBodies;
@ -429,7 +429,7 @@ inline std::set<RigidBody*>::iterator DynamicsWorld::getRigidBodiesEndIterator()
* @return True if the sleeping technique is enabled and false otherwise * @return True if the sleeping technique is enabled and false otherwise
*/ */
inline bool DynamicsWorld::isSleepingEnabled() const { inline bool DynamicsWorld::isSleepingEnabled() const {
return mIsSleepingEnabled; return m_isSleepingEnabled;
} }
// Return the current sleep linear velocity // Return the current sleep linear velocity

4
tools/testbed/opengl-framework/src/Light.cpp
View File

@ -32,14 +32,14 @@ using namespace openglframework;
// Constructor // Constructor
Light::Light() Light::Light()
: mDiffuseColor(Color::white()), : mDiffuseColor(Color::white()),
mSpecularColor(Color::white()), mIsActive(false) { mSpecularColor(Color::white()), m_isActive(false) {
} }
// Constructor // Constructor
Light::Light(Color diffuseColor, Color specularColor) Light::Light(Color diffuseColor, Color specularColor)
: mDiffuseColor(diffuseColor), : mDiffuseColor(diffuseColor),
mSpecularColor(specularColor), mIsActive(false) { mSpecularColor(specularColor), m_isActive(false) {
} }

8
tools/testbed/opengl-framework/src/Light.h
View File

@ -50,7 +50,7 @@ class Light : public Object3D {
Color mSpecularColor; Color mSpecularColor;
// True if the light is active // True if the light is active
bool mIsActive; bool m_isActive;
public: public:
@ -112,13 +112,13 @@ inline void Light::setSpecularColor(const Color& color) {
// Return true if the light is active // Return true if the light is active
inline bool Light::getIsActive() const { inline bool Light::getIsActive() const {
return mIsActive; return m_isActive;
} }
// Enable the light // Enable the light
inline void Light::enable() { inline void Light::enable() {
mIsActive = true; m_isActive = true;
// Enable the light // Enable the light
//glEnable(GL_LIGHTING); //glEnable(GL_LIGHTING);
@ -128,7 +128,7 @@ inline void Light::enable() {
// Disable the light // Disable the light
inline void Light::disable() { inline void Light::disable() {
mIsActive = false; m_isActive = false;
// Disable the light // Disable the light
//glDisable(GL_LIGHT0 + mLightID); //glDisable(GL_LIGHT0 + mLightID);

22
tools/testbed/opengl-framework/src/Texture2D.cpp
View File

@ -33,13 +33,13 @@
using namespace openglframework; using namespace openglframework;
// Constructor // Constructor
Texture2D::Texture2D() : mID(0), mUnit(0), mWidth(0), mHeight(0) { Texture2D::Texture2D() : m_id(0), mUnit(0), mWidth(0), mHeight(0) {
} }
// Constructor // Constructor
Texture2D::Texture2D(uint32_t width, uint32_t height, uint32_t int32_ternalFormat, uint32_t format, uint32_t type) Texture2D::Texture2D(uint32_t width, uint32_t height, uint32_t int32_ternalFormat, uint32_t format, uint32_t type)
: mID(0), mUnit(0), mWidth(0), mHeight(0) { : m_id(0), mUnit(0), mWidth(0), mHeight(0) {
// Create the texture // Create the texture
create(width, height, int32_ternalFormat, format, type); create(width, height, int32_ternalFormat, format, type);
@ -61,9 +61,9 @@ void Texture2D::create(uint32_t width, uint32_t height, uint32_t int32_ternalFor
mHeight = height; mHeight = height;
// Create the OpenGL texture // Create the OpenGL texture
glGenTextures(1, &mID); glGenTextures(1, &m_id);
assert(mID != 0); assert(m_id != 0);
glBindTexture(GL_TEXTURE_2D, mID); glBindTexture(GL_TEXTURE_2D, m_id);
glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_WRAP_S, GL_CLAMP_TO_EDGE); glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_WRAP_S, GL_CLAMP_TO_EDGE);
glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_WRAP_T, GL_CLAMP_TO_EDGE); glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_WRAP_T, GL_CLAMP_TO_EDGE);
glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_LINEAR); glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_LINEAR);
@ -83,9 +83,9 @@ void Texture2D::create(uint32_t width, uint32_t height, uint32_t int32_ternalFor
mHeight = height; mHeight = height;
// Create the OpenGL texture // Create the OpenGL texture
glGenTextures(1, &mID); glGenTextures(1, &m_id);
assert(mID != 0); assert(m_id != 0);
glBindTexture(GL_TEXTURE_2D, mID); glBindTexture(GL_TEXTURE_2D, m_id);
glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_WRAP_S, wrapS); glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_WRAP_S, wrapS);
glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_WRAP_T, wrapT); glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_WRAP_T, wrapT);
glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, maxFilter); glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, maxFilter);
@ -96,9 +96,9 @@ void Texture2D::create(uint32_t width, uint32_t height, uint32_t int32_ternalFor
// Destroy the texture // Destroy the texture
void Texture2D::destroy() { void Texture2D::destroy() {
if (mID != 0) { if (m_id != 0) {
glDeleteTextures(1, &mID); glDeleteTextures(1, &m_id);
mID = 0; m_id = 0;
mUnit = 0; mUnit = 0;
mWidth = 0; mWidth = 0;
mHeight = 0; mHeight = 0;

10
tools/testbed/opengl-framework/src/Texture2D.h
View File

@ -43,7 +43,7 @@ class Texture2D {
// -------------------- Attributes -------------------- // // -------------------- Attributes -------------------- //
// OpenGL texture ID // OpenGL texture ID
GLuint32_t mID; GLuint32_t m_id;
// Current texture unit for this texture // Current texture unit for this texture
GLuint32_t mUnit; GLuint32_t mUnit;
@ -102,21 +102,21 @@ class Texture2D {
// Bind the texture // Bind the texture
inline void Texture2D::bind() const { inline void Texture2D::bind() const {
assert(mID != 0); assert(m_id != 0);
glActiveTexture(GL_TEXTURE0 + mUnit); glActiveTexture(GL_TEXTURE0 + mUnit);
glBindTexture(GL_TEXTURE_2D, mID); glBindTexture(GL_TEXTURE_2D, m_id);
} }
// Unbind the texture // Unbind the texture
inline void Texture2D::unbind() const { inline void Texture2D::unbind() const {
assert(mID != 0); assert(m_id != 0);
glActiveTexture(GL_TEXTURE0 + mUnit); glActiveTexture(GL_TEXTURE0 + mUnit);
glBindTexture(GL_TEXTURE_2D, 0); glBindTexture(GL_TEXTURE_2D, 0);
} }
// Get the OpenGL texture ID // Get the OpenGL texture ID
inline uint32_t Texture2D::getID() const { inline uint32_t Texture2D::getID() const {
return mID; return m_id;
} }
// Get the unit of the texture // Get the unit of the texture

20
tools/testbed/scenes/joints/JointsScene.cpp
View File

@ -296,8 +296,8 @@ void JointsScene::createBallAndSocketJoints() {
rp3d::RigidBody* body2 = mBallAndSocketJointChainBoxes[i+1]->getRigidBody(); rp3d::RigidBody* body2 = mBallAndSocketJointChainBoxes[i+1]->getRigidBody();
rp3d::Vector3 body1Position = body1->getTransform().getPosition(); rp3d::Vector3 body1Position = body1->getTransform().getPosition();
rp3d::Vector3 body2Position = body2->getTransform().getPosition(); rp3d::Vector3 body2Position = body2->getTransform().getPosition();
const rp3d::Vector3 anchorPointWorldSpace = 0.5 * (body1Position + body2Position); const rp3d::Vector3 m_m_m_m_anchorPointWorldSpace = 0.5 * (body1Position + body2Position);
rp3d::BallAndSocketJointInfo jointInfo(body1, body2, anchorPointWorldSpace); rp3d::BallAndSocketJointInfo jointInfo(body1, body2, m_m_m_m_anchorPointWorldSpace);
// Create the joint in the dynamics world // Create the joint in the dynamics world
mBallAndSocketJoints[i] = dynamic_cast<rp3d::BallAndSocketJoint*>( mBallAndSocketJoints[i] = dynamic_cast<rp3d::BallAndSocketJoint*>(
@ -352,9 +352,9 @@ void JointsScene::createSliderJoint() {
rp3d::RigidBody* body2 = mSliderJointTopBox->getRigidBody(); rp3d::RigidBody* body2 = mSliderJointTopBox->getRigidBody();
const rp3d::Vector3& body1Position = body1->getTransform().getPosition(); const rp3d::Vector3& body1Position = body1->getTransform().getPosition();
const rp3d::Vector3& body2Position = body2->getTransform().getPosition(); const rp3d::Vector3& body2Position = body2->getTransform().getPosition();
const rp3d::Vector3 anchorPointWorldSpace = rp3d::float(0.5) * (body2Position + body1Position); const rp3d::Vector3 m_m_m_m_anchorPointWorldSpace = rp3d::float(0.5) * (body2Position + body1Position);
const rp3d::Vector3 sliderAxisWorldSpace = (body2Position - body1Position); const rp3d::Vector3 sliderAxisWorldSpace = (body2Position - body1Position);
rp3d::SliderJointInfo jointInfo(body1, body2, anchorPointWorldSpace, sliderAxisWorldSpace, rp3d::SliderJointInfo jointInfo(body1, body2, m_m_m_m_anchorPointWorldSpace, sliderAxisWorldSpace,
rp3d::float(-1.7), rp3d::float(1.7)); rp3d::float(-1.7), rp3d::float(1.7));
jointInfo.isMotorEnabled = true; jointInfo.isMotorEnabled = true;
jointInfo.motorSpeed = 0.0; jointInfo.motorSpeed = 0.0;
@ -392,9 +392,9 @@ void JointsScene::createPropellerHingeJoint() {
rp3d::RigidBody* body2 = mSliderJointTopBox->getRigidBody(); rp3d::RigidBody* body2 = mSliderJointTopBox->getRigidBody();
const rp3d::Vector3& body1Position = body1->getTransform().getPosition(); const rp3d::Vector3& body1Position = body1->getTransform().getPosition();
const rp3d::Vector3& body2Position = body2->getTransform().getPosition(); const rp3d::Vector3& body2Position = body2->getTransform().getPosition();
const rp3d::Vector3 anchorPointWorldSpace = 0.5 * (body2Position + body1Position); const rp3d::Vector3 m_m_m_m_anchorPointWorldSpace = 0.5 * (body2Position + body1Position);
const rp3d::Vector3 hingeAxisWorldSpace(0, 1, 0); const rp3d::Vector3 hingeAxisWorldSpace(0, 1, 0);
rp3d::HingeJointInfo jointInfo(body1, body2, anchorPointWorldSpace, hingeAxisWorldSpace); rp3d::HingeJointInfo jointInfo(body1, body2, m_m_m_m_anchorPointWorldSpace, hingeAxisWorldSpace);
jointInfo.isMotorEnabled = true; jointInfo.isMotorEnabled = true;
jointInfo.motorSpeed = - rp3d::float(0.5) * PI; jointInfo.motorSpeed = - rp3d::float(0.5) * PI;
jointInfo.maxMotorTorque = rp3d::float(60.0); jointInfo.maxMotorTorque = rp3d::float(60.0);
@ -445,8 +445,8 @@ void JointsScene::createFixedJoints() {
// Create the joint info object // Create the joint info object
rp3d::RigidBody* body1 = mFixedJointBox1->getRigidBody(); rp3d::RigidBody* body1 = mFixedJointBox1->getRigidBody();
rp3d::RigidBody* propellerBody = mPropellerBox->getRigidBody(); rp3d::RigidBody* propellerBody = mPropellerBox->getRigidBody();
const rp3d::Vector3 anchorPointWorldSpace1(5, 7, 0); const rp3d::Vector3 m_m_m_m_anchorPointWorldSpace1(5, 7, 0);
rp3d::FixedJointInfo jointInfo1(body1, propellerBody, anchorPointWorldSpace1); rp3d::FixedJointInfo jointInfo1(body1, propellerBody, m_m_m_m_anchorPointWorldSpace1);
jointInfo1.isCollisionEnabled = false; jointInfo1.isCollisionEnabled = false;
// Create the joint in the dynamics world // Create the joint in the dynamics world
@ -456,8 +456,8 @@ void JointsScene::createFixedJoints() {
// Create the joint info object // Create the joint info object
rp3d::RigidBody* body2 = mFixedJointBox2->getRigidBody(); rp3d::RigidBody* body2 = mFixedJointBox2->getRigidBody();
const rp3d::Vector3 anchorPointWorldSpace2(-5, 7, 0); const rp3d::Vector3 m_m_m_m_anchorPointWorldSpace2(-5, 7, 0);
rp3d::FixedJointInfo jointInfo2(body2, propellerBody, anchorPointWorldSpace2); rp3d::FixedJointInfo jointInfo2(body2, propellerBody, m_m_m_m_anchorPointWorldSpace2);
jointInfo2.isCollisionEnabled = false; jointInfo2.isCollisionEnabled = false;
// Create the joint in the dynamics world // Create the joint in the dynamics world