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
*/
Body::Body(bodyindex id)
: mID(id), mIsAlreadyInIsland(false), mIsAllowedToSleep(true), mIsActive(true),
mIsSleeping(false), mSleepTime(0), mUserData(NULL) {
: m_id(id), m_isAlreadyInIsland(false), m_isAllowedToSleep(true), m_isActive(true),
m_isSleeping(false), m_sleepTime(0), m_userData(NULL) {
}

48
ephysics/body/Body.h
View File

@ -27,13 +27,13 @@ class Body {
// -------------------- Attributes -------------------- //
/// ID of the body
bodyindex mID;
bodyindex m_id;
/// 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
bool mIsAllowedToSleep;
bool m_isAllowedToSleep;
/// True if the body is active.
/// 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",
/// all the proxy shapes will be added to the broad-phase. A joint
/// connected to an inactive body will also be inactive.
bool mIsActive;
bool m_isActive;
/// 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
float mSleepTime;
float m_sleepTime;
/// Pointer that can be used to attach user data to the body
void* mUserData;
void* m_userData;
// -------------------- Methods -------------------- //
@ -121,7 +121,7 @@ class Body {
* @return The ID of the body
*/
inline bodyindex Body::getID() const {
return mID;
return m_id;
}
// 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
*/
inline bool Body::isAllowedToSleep() const {
return mIsAllowedToSleep;
return m_isAllowedToSleep;
}
// 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
*/
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
@ -147,7 +147,7 @@ inline void Body::setIsAllowedToSleep(bool isAllowedToSleep) {
* @return True if the body is currently sleeping and false otherwise
*/
inline bool Body::isSleeping() const {
return mIsSleeping;
return m_isSleeping;
}
// 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
*/
inline bool Body::isActive() const {
return mIsActive;
return m_isActive;
}
// 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
*/
inline void Body::setIsActive(bool isActive) {
mIsActive = isActive;
m_isActive = isActive;
}
// Set the variable to know whether or not the body is sleeping
inline void Body::setIsSleeping(bool isSleeping) {
if (isSleeping) {
mSleepTime = float(0.0);
m_sleepTime = float(0.0);
}
else {
if (mIsSleeping) {
mSleepTime = float(0.0);
if (m_isSleeping) {
m_sleepTime = float(0.0);
}
}
mIsSleeping = isSleeping;
m_isSleeping = isSleeping;
}
// 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
*/
inline void* Body::getUserData() const {
return mUserData;
return m_userData;
}
// 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
*/
inline void Body::setUserData(void* userData) {
mUserData = userData;
m_userData = userData;
}
// Smaller than operator
inline bool Body::operator<(const Body& body2) const {
return (mID < body2.mID);
return (m_id < body2.m_id);
}
// Larger than operator
inline bool Body::operator>(const Body& body2) const {
return (mID > body2.mID);
return (m_id > body2.m_id);
}
// Equal operator
inline bool Body::operator==(const Body& body2) const {
return (mID == body2.mID);
return (m_id == body2.m_id);
}
// Not equal operator
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);
// Notify the collision detection about this new collision shape
mWorld.mCollisionDetection.addProxyCollisionShape(proxyShape, aabb);
mWorld.m_collisionDetection.addProxyCollisionShape(proxyShape, aabb);
mNbCollisionShapes++;
@ -92,8 +92,8 @@ void CollisionBody::removeCollisionShape(const ProxyShape* proxyShape) {
if (current == proxyShape) {
mProxyCollisionShapes = current->mNext;
if (mIsActive) {
mWorld.mCollisionDetection.removeProxyCollisionShape(current);
if (m_isActive) {
mWorld.m_collisionDetection.removeProxyCollisionShape(current);
}
current->~ProxyShape();
@ -112,8 +112,8 @@ void CollisionBody::removeCollisionShape(const ProxyShape* proxyShape) {
ProxyShape* elementToRemove = current->mNext;
current->mNext = elementToRemove->mNext;
if (mIsActive) {
mWorld.mCollisionDetection.removeProxyCollisionShape(elementToRemove);
if (m_isActive) {
mWorld.m_collisionDetection.removeProxyCollisionShape(elementToRemove);
}
elementToRemove->~ProxyShape();
@ -138,8 +138,8 @@ void CollisionBody::removeAllCollisionShapes() {
// Remove the proxy collision shape
ProxyShape* nextElement = current->mNext;
if (mIsActive) {
mWorld.mCollisionDetection.removeProxyCollisionShape(current);
if (m_isActive) {
mWorld.m_collisionDetection.removeProxyCollisionShape(current);
}
current->~ProxyShape();
@ -188,7 +188,7 @@ void CollisionBody::updateProxyShapeInBroadPhase(ProxyShape* proxyShape, bool fo
proxyShape->getCollisionShape()->computeAABB(aabb, mTransform * proxyShape->getLocalToBodyTransform());
// 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
@ -198,7 +198,7 @@ void CollisionBody::updateProxyShapeInBroadPhase(ProxyShape* proxyShape, bool fo
void CollisionBody::setIsActive(bool isActive) {
// If the state does not change
if (mIsActive == isActive) return;
if (m_isActive == isActive) return;
Body::setIsActive(isActive);
@ -213,7 +213,7 @@ void CollisionBody::setIsActive(bool isActive) {
shape->getCollisionShape()->computeAABB(aabb, mTransform * shape->mLocalToBodyTransform);
// 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
@ -222,7 +222,7 @@ void CollisionBody::setIsActive(bool isActive) {
for (ProxyShape* shape = mProxyCollisionShapes; shape != NULL; shape = shape->mNext) {
// 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
@ -237,23 +237,23 @@ void CollisionBody::askForBroadPhaseCollisionCheck() const {
// For all the proxy collision shapes of the body
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.
int32_t CollisionBody::resetIsAlreadyInIslandAndCountManifolds() {
mIsAlreadyInIsland = false;
m_isAlreadyInIsland = false;
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
ContactManifoldListElement* currentElement = mContactManifoldsList;
while (currentElement != NULL) {
currentElement->contactManifold->mIsAlreadyInIsland = false;
currentElement->contactManifold->m_isAlreadyInIsland = false;
currentElement = currentElement->next;
nbManifolds++;
}
@ -290,7 +290,7 @@ bool CollisionBody::testPointInside(const Vector3& worldPoint) const {
bool CollisionBody::raycast(const Ray& ray, RaycastInfo& raycastInfo) {
// If the body is not active, it cannot be hit by rays
if (!mIsActive) return false;
if (!m_isActive) return false;
bool isHit = false;
Ray rayTemp(ray);

2
ephysics/body/CollisionBody.h
View File

@ -88,7 +88,7 @@ class CollisionBody : public Body {
/// (as if the body has moved).
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();
public :

4
ephysics/body/RigidBody.cpp
View File

@ -215,7 +215,7 @@ ProxyShape* RigidBody::addCollisionShape(CollisionShape* collisionShape,
collisionShape->computeAABB(aabb, mTransform * transform);
// Notify the collision detection about this new collision shape
mWorld.mCollisionDetection.addProxyCollisionShape(proxyShape, aabb);
mWorld.m_collisionDetection.addProxyCollisionShape(proxyShape, aabb);
mNbCollisionShapes++;
@ -389,7 +389,7 @@ void RigidBody::updateBroadPhaseState() const {
shape->getCollisionShape()->computeAABB(aabb, mTransform *shape->getLocalToBodyTransform());
// 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;
// Awake the body if it was sleeping
if (mIsSleeping) {
if (m_isSleeping) {
setIsSleeping(false);
}
@ -414,7 +414,7 @@ inline void RigidBody::applyForce(const Vector3& force, const Vector3& point) {
if (mType != DYNAMIC) return;
// Awake the body if it was sleeping
if (mIsSleeping) {
if (m_isSleeping) {
setIsSleeping(false);
}
@ -437,7 +437,7 @@ inline void RigidBody::applyTorque(const Vector3& torque) {
if (mType != DYNAMIC) return;
// Awake the body if it was sleeping
if (mIsSleeping) {
if (m_isSleeping) {
setIsSleeping(false);
}

2
ephysics/collision/ContactManifold.cpp
View File

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

4
ephysics/collision/ContactManifold.h
View File

@ -102,7 +102,7 @@ class ContactManifold {
Vector3 mRollingResistanceImpulse;
/// True if the contact manifold has already been added int32_to an island
bool mIsAlreadyInIsland;
bool m_isAlreadyInIsland;
/// Reference to the memory allocator
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
inline bool ContactManifold::isAlreadyInIsland() const {
return mIsAlreadyInIsland;
return m_isAlreadyInIsland;
}
// 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)
: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) {
}

6
ephysics/collision/ProxyShape.h
View File

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

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

@ -14,27 +14,27 @@ using namespace reactphysics3d;
// Constructor
BroadPhaseAlgorithm::BroadPhaseAlgorithm(CollisionDetection& collisionDetection)
:m_dynamicAABBTree(DYNAMIC_TREE_AABB_GAP), mNbMovedShapes(0), mNbAllocatedMovedShapes(8),
mNbNonUsedMovedShapes(0), mNbPotentialPairs(0), mNbAllocatedPotentialPairs(8),
mCollisionDetection(collisionDetection) {
:m_dynamicAABBTree(DYNAMIC_TREE_AABB_GAP), m_numberMovedShapes(0), m_numberAllocatedMovedShapes(8),
m_numberNonUsedMovedShapes(0), m_numberPotentialPairs(0), m_numberAllocatedPotentialPairs(8),
m_collisionDetection(collisionDetection) {
// Allocate memory for the array of non-static proxy shapes IDs
mMovedShapes = (int32_t*) malloc(mNbAllocatedMovedShapes * sizeof(int32_t));
assert(mMovedShapes != NULL);
m_movedShapes = (int32_t*) malloc(m_numberAllocatedMovedShapes * sizeof(int32_t));
assert(m_movedShapes != NULL);
// Allocate memory for the array of potential overlapping pairs
mPotentialPairs = (BroadPhasePair*) malloc(mNbAllocatedPotentialPairs * sizeof(BroadPhasePair));
assert(mPotentialPairs != NULL);
m_potentialPairs = (BroadPhasePair*) malloc(m_numberAllocatedPotentialPairs * sizeof(BroadPhasePair));
assert(m_potentialPairs != NULL);
}
// Destructor
BroadPhaseAlgorithm::~BroadPhaseAlgorithm() {
// 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
free(mPotentialPairs);
free(m_potentialPairs);
}
// 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) {
// Allocate more elements in the array of shapes that have moved if necessary
if (mNbAllocatedMovedShapes == mNbMovedShapes) {
mNbAllocatedMovedShapes *= 2;
int32_t* oldArray = mMovedShapes;
mMovedShapes = (int32_t*) malloc(mNbAllocatedMovedShapes * sizeof(int32_t));
assert(mMovedShapes != NULL);
memcpy(mMovedShapes, oldArray, mNbMovedShapes * sizeof(int32_t));
if (m_numberAllocatedMovedShapes == m_numberMovedShapes) {
m_numberAllocatedMovedShapes *= 2;
int32_t* oldArray = m_movedShapes;
m_movedShapes = (int32_t*) malloc(m_numberAllocatedMovedShapes * sizeof(int32_t));
assert(m_movedShapes != NULL);
memcpy(m_movedShapes, oldArray, m_numberMovedShapes * sizeof(int32_t));
free(oldArray);
}
// Store the broad-phase ID int32_to the array of shapes that have moved
assert(mNbMovedShapes < mNbAllocatedMovedShapes);
assert(mMovedShapes != NULL);
mMovedShapes[mNbMovedShapes] = broadPhaseID;
mNbMovedShapes++;
assert(m_numberMovedShapes < m_numberAllocatedMovedShapes);
assert(m_movedShapes != NULL);
m_movedShapes[m_numberMovedShapes] = broadPhaseID;
m_numberMovedShapes++;
}
// 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.
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
// are used, we release some allocated memory
if ((mNbMovedShapes - mNbNonUsedMovedShapes) < mNbAllocatedMovedShapes / 4 &&
mNbAllocatedMovedShapes > 8) {
if ((m_numberMovedShapes - m_numberNonUsedMovedShapes) < m_numberAllocatedMovedShapes / 4 &&
m_numberAllocatedMovedShapes > 8) {
mNbAllocatedMovedShapes /= 2;
int32_t* oldArray = mMovedShapes;
mMovedShapes = (int32_t*) malloc(mNbAllocatedMovedShapes * sizeof(int32_t));
assert(mMovedShapes != NULL);
m_numberAllocatedMovedShapes /= 2;
int32_t* oldArray = m_movedShapes;
m_movedShapes = (int32_t*) malloc(m_numberAllocatedMovedShapes * sizeof(int32_t));
assert(m_movedShapes != NULL);
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) {
mMovedShapes[nbElements] = oldArray[i];
m_movedShapes[nbElements] = oldArray[i];
nbElements++;
}
}
mNbMovedShapes = nbElements;
mNbNonUsedMovedShapes = 0;
m_numberMovedShapes = nbElements;
m_numberNonUsedMovedShapes = 0;
free(oldArray);
}
// Remove the broad-phase ID from the array
for (uint32_t i=0; i<mNbMovedShapes; i++) {
if (mMovedShapes[i] == broadPhaseID) {
mMovedShapes[i] = -1;
mNbNonUsedMovedShapes++;
for (uint32_t i=0; i<m_numberMovedShapes; i++) {
if (m_movedShapes[i] == broadPhaseID) {
m_movedShapes[i] = -1;
m_numberNonUsedMovedShapes++;
break;
}
}
@ -147,12 +147,12 @@ void BroadPhaseAlgorithm::updateProxyCollisionShape(ProxyShape* proxyShape, cons
void BroadPhaseAlgorithm::computeOverlappingPairs() {
// Reset the potential overlapping pairs
mNbPotentialPairs = 0;
m_numberPotentialPairs = 0;
// For all collision shapes that have moved (or have been created) during the
// last simulation step
for (uint32_t i=0; i<mNbMovedShapes; i++) {
int32_t shapeID = mMovedShapes[i];
for (uint32_t i=0; i<m_numberMovedShapes; i++) {
int32_t shapeID = m_movedShapes[i];
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
// last simulation step
mNbMovedShapes = 0;
m_numberMovedShapes = 0;
// 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
// overlapping pairs
uint32_t i=0;
while (i < mNbPotentialPairs) {
while (i < m_numberPotentialPairs) {
// Get a potential overlapping pair
BroadPhasePair* pair = mPotentialPairs + i;
BroadPhasePair* pair = m_potentialPairs + i;
i++;
assert(pair->collisionShape1ID != pair->collisionShape2ID);
@ -190,13 +190,13 @@ void BroadPhaseAlgorithm::computeOverlappingPairs() {
ProxyShape* shape2 = static_cast<ProxyShape*>(m_dynamicAABBTree.getNodeDataPointer(pair->collisionShape2ID));
// Notify the collision detection about the overlapping pair
mCollisionDetection.broadPhaseNotifyOverlappingPair(shape1, shape2);
m_collisionDetection.broadPhaseNotifyOverlappingPair(shape1, shape2);
// Skip the duplicate overlapping pairs
while (i < mNbPotentialPairs) {
while (i < m_numberPotentialPairs) {
// 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 (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
// 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
BroadPhasePair* oldPairs = mPotentialPairs;
mNbAllocatedPotentialPairs /= 2;
mPotentialPairs = (BroadPhasePair*) malloc(mNbAllocatedPotentialPairs * sizeof(BroadPhasePair));
assert(mPotentialPairs);
memcpy(mPotentialPairs, oldPairs, mNbPotentialPairs * sizeof(BroadPhasePair));
BroadPhasePair* oldPairs = m_potentialPairs;
m_numberAllocatedPotentialPairs /= 2;
m_potentialPairs = (BroadPhasePair*) malloc(m_numberAllocatedPotentialPairs * sizeof(BroadPhasePair));
assert(m_potentialPairs);
memcpy(m_potentialPairs, oldPairs, m_numberPotentialPairs * sizeof(BroadPhasePair));
free(oldPairs);
}
}
@ -228,28 +228,28 @@ void BroadPhaseAlgorithm::notifyOverlappingNodes(int32_t node1ID, int32_t node2I
if (node1ID == node2ID) return;
// 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
BroadPhasePair* oldPairs = mPotentialPairs;
mNbAllocatedPotentialPairs *= 2;
mPotentialPairs = (BroadPhasePair*) malloc(mNbAllocatedPotentialPairs * sizeof(BroadPhasePair));
assert(mPotentialPairs);
memcpy(mPotentialPairs, oldPairs, mNbPotentialPairs * sizeof(BroadPhasePair));
BroadPhasePair* oldPairs = m_potentialPairs;
m_numberAllocatedPotentialPairs *= 2;
m_potentialPairs = (BroadPhasePair*) malloc(m_numberAllocatedPotentialPairs * sizeof(BroadPhasePair));
assert(m_potentialPairs);
memcpy(m_potentialPairs, oldPairs, m_numberPotentialPairs * sizeof(BroadPhasePair));
free(oldPairs);
}
// Add the new potential pair int32_to the array of potential overlapping pairs
mPotentialPairs[mNbPotentialPairs].collisionShape1ID = std::min(node1ID, node2ID);
mPotentialPairs[mNbPotentialPairs].collisionShape2ID = std::max(node1ID, node2ID);
mNbPotentialPairs++;
m_potentialPairs[m_numberPotentialPairs].collisionShape1ID = std::min(node1ID, node2ID);
m_potentialPairs[m_numberPotentialPairs].collisionShape2ID = std::max(node1ID, node2ID);
m_numberPotentialPairs++;
}
// Called when a overlapping node has been found during the call to
// DynamicAABBTree:reportAllShapesOverlappingWithAABB()
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

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

@ -47,13 +47,13 @@ class AABBOverlapCallback : public DynamicAABBTreeOverlapCallback {
BroadPhaseAlgorithm& mBroadPhaseAlgorithm;
int32_t mReferenceNodeId;
int32_t m_referenceNodeId;
public:
// Constructor
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
/// created) during the last simulation step. Those are the shapes that need to be tested
/// 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
/// simulation step.
uint32_t mNbMovedShapes;
uint32_t m_numberMovedShapes;
/// Number of allocated elements for the array of shapes that have moved during the last
/// 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
/// simulation step.
uint32_t mNbNonUsedMovedShapes;
uint32_t m_numberNonUsedMovedShapes;
/// Temporary array of potential overlapping pairs (with potential duplicates)
BroadPhasePair* mPotentialPairs;
BroadPhasePair* m_potentialPairs;
/// Number of potential overlapping pairs
uint32_t mNbPotentialPairs;
uint32_t m_numberPotentialPairs;
/// Number of allocated elements for the array of potential overlapping pairs
uint32_t mNbAllocatedPotentialPairs;
uint32_t m_numberAllocatedPotentialPairs;
/// Reference to the collision detection object
CollisionDetection& mCollisionDetection;
CollisionDetection& m_collisionDetection;
// -------------------- 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
mNodes[nodeID].aabb = newAABB;
const Vector3 gap(mExtraAABBGap, mExtraAABBGap, mExtraAABBGap);
mNodes[nodeID].aabb.mMinCoordinates -= gap;
mNodes[nodeID].aabb.mMaxCoordinates += gap;
mNodes[nodeID].aabb.m_minCoordinates -= gap;
mNodes[nodeID].aabb.m_maxCoordinates += gap;
// Inflate the fat AABB in direction of the linear motion of the AABB
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 {
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)) {
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 {
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)) {
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 {
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));

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
ConvexVsTriangleCallback convexVsTriangleCallback;
convexVsTriangleCallback.setCollisionDetection(mCollisionDetection);
convexVsTriangleCallback.setCollisionDetection(m_collisionDetection);
convexVsTriangleCallback.setConvexShape(convexShape);
convexVsTriangleCallback.setConcaveShape(concaveShape);
convexVsTriangleCallback.setProxyShapes(convexProxyShape, concaveProxyShape);
@ -92,7 +92,7 @@ void ConvexVsTriangleCallback::testTriangle(const Vector3* trianglePoints) {
TriangleShape triangleShape(trianglePoints[0], trianglePoints[1], trianglePoints[2], margin);
// 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());
// 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:
/// Pointer to the collision detection object
CollisionDetection* mCollisionDetection;
CollisionDetection* m_collisionDetection;
/// Narrow-phase collision callback
NarrowPhaseCallback* mNarrowPhaseCallback;
@ -53,7 +53,7 @@ class ConvexVsTriangleCallback : public TriangleCallback {
/// Set the collision detection pointer
void setCollisionDetection(CollisionDetection* collisionDetection) {
mCollisionDetection = collisionDetection;
m_collisionDetection = collisionDetection;
}
/// Set the narrow-phase collision callback

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

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

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

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

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

@ -20,13 +20,13 @@ AABB::AABB() {
// Constructor
AABB::AABB(const Vector3& minCoordinates, const Vector3& maxCoordinates)
:mMinCoordinates(minCoordinates), mMaxCoordinates(maxCoordinates) {
:m_minCoordinates(minCoordinates), m_maxCoordinates(maxCoordinates) {
}
// Copy-constructor
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
void AABB::mergeWithAABB(const AABB& aabb) {
mMinCoordinates.x = std::min(mMinCoordinates.x, aabb.mMinCoordinates.x);
mMinCoordinates.y = std::min(mMinCoordinates.y, aabb.mMinCoordinates.y);
mMinCoordinates.z = std::min(mMinCoordinates.z, aabb.mMinCoordinates.z);
m_minCoordinates.x = std::min(m_minCoordinates.x, aabb.m_minCoordinates.x);
m_minCoordinates.y = std::min(m_minCoordinates.y, aabb.m_minCoordinates.y);
m_minCoordinates.z = std::min(m_minCoordinates.z, aabb.m_minCoordinates.z);
mMaxCoordinates.x = std::max(mMaxCoordinates.x, aabb.mMaxCoordinates.x);
mMaxCoordinates.y = std::max(mMaxCoordinates.y, aabb.mMaxCoordinates.y);
mMaxCoordinates.z = std::max(mMaxCoordinates.z, aabb.mMaxCoordinates.z);
m_maxCoordinates.x = std::max(m_maxCoordinates.x, aabb.m_maxCoordinates.x);
m_maxCoordinates.y = std::max(m_maxCoordinates.y, aabb.m_maxCoordinates.y);
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
void AABB::mergeTwoAABBs(const AABB& aabb1, const AABB& aabb2) {
mMinCoordinates.x = std::min(aabb1.mMinCoordinates.x, aabb2.mMinCoordinates.x);
mMinCoordinates.y = std::min(aabb1.mMinCoordinates.y, aabb2.mMinCoordinates.y);
mMinCoordinates.z = std::min(aabb1.mMinCoordinates.z, aabb2.mMinCoordinates.z);
m_minCoordinates.x = std::min(aabb1.m_minCoordinates.x, aabb2.m_minCoordinates.x);
m_minCoordinates.y = std::min(aabb1.m_minCoordinates.y, aabb2.m_minCoordinates.y);
m_minCoordinates.z = std::min(aabb1.m_minCoordinates.z, aabb2.m_minCoordinates.z);
mMaxCoordinates.x = std::max(aabb1.mMaxCoordinates.x, aabb2.mMaxCoordinates.x);
mMaxCoordinates.y = std::max(aabb1.mMaxCoordinates.y, aabb2.mMaxCoordinates.y);
mMaxCoordinates.z = std::max(aabb1.mMaxCoordinates.z, aabb2.mMaxCoordinates.z);
m_maxCoordinates.x = std::max(aabb1.m_maxCoordinates.x, aabb2.m_maxCoordinates.x);
m_maxCoordinates.y = std::max(aabb1.m_maxCoordinates.y, aabb2.m_maxCoordinates.y);
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
bool AABB::contains(const AABB& aabb) const {
bool isInside = true;
isInside = isInside && mMinCoordinates.x <= aabb.mMinCoordinates.x;
isInside = isInside && mMinCoordinates.y <= aabb.mMinCoordinates.y;
isInside = isInside && mMinCoordinates.z <= aabb.mMinCoordinates.z;
isInside = isInside && m_minCoordinates.x <= aabb.m_minCoordinates.x;
isInside = isInside && m_minCoordinates.y <= aabb.m_minCoordinates.y;
isInside = isInside && m_minCoordinates.z <= aabb.m_minCoordinates.z;
isInside = isInside && mMaxCoordinates.x >= aabb.mMaxCoordinates.x;
isInside = isInside && mMaxCoordinates.y >= aabb.mMaxCoordinates.y;
isInside = isInside && mMaxCoordinates.z >= aabb.mMaxCoordinates.z;
isInside = isInside && m_maxCoordinates.x >= aabb.m_maxCoordinates.x;
isInside = isInside && m_maxCoordinates.y >= aabb.m_maxCoordinates.y;
isInside = isInside && m_maxCoordinates.z >= aabb.m_maxCoordinates.z;
return isInside;
}
@ -102,9 +102,9 @@ AABB AABB::createAABBForTriangle(const Vector3* trianglePoints) {
bool AABB::testRayIntersect(const Ray& ray) const {
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 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
float adx = std::abs(d.x);

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

@ -25,10 +25,10 @@ class AABB {
// -------------------- Attributes -------------------- //
/// 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
Vector3 mMaxCoordinates;
Vector3 m_maxCoordinates;
public :
@ -104,68 +104,68 @@ class AABB {
// Return the center point of the AABB in world coordinates
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
inline const Vector3& AABB::getMin() const {
return mMinCoordinates;
return m_minCoordinates;
}
// Set the minimum coordinates of the AABB
inline void AABB::setMin(const Vector3& min) {
mMinCoordinates = min;
m_minCoordinates = min;
}
// Return the maximum coordinates of the AABB
inline const Vector3& AABB::getMax() const {
return mMaxCoordinates;
return m_maxCoordinates;
}
// Set the maximum coordinates of the AABB
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
inline Vector3 AABB::getExtent() const {
return mMaxCoordinates - mMinCoordinates;
return m_maxCoordinates - m_minCoordinates;
}
// Inflate each side of the AABB by a given size
inline void AABB::inflate(float dx, float dy, float dz) {
mMaxCoordinates += Vector3(dx, dy, dz);
mMinCoordinates -= Vector3(dx, dy, dz);
m_maxCoordinates += Vector3(dx, dy, dz);
m_minCoordinates -= Vector3(dx, dy, dz);
}
// 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
inline bool AABB::testCollision(const AABB& aabb) const {
if (mMaxCoordinates.x < aabb.mMinCoordinates.x ||
aabb.mMaxCoordinates.x < mMinCoordinates.x) return false;
if (mMaxCoordinates.y < aabb.mMinCoordinates.y ||
aabb.mMaxCoordinates.y < mMinCoordinates.y) return false;
if (mMaxCoordinates.z < aabb.mMinCoordinates.z||
aabb.mMaxCoordinates.z < mMinCoordinates.z) return false;
if (m_maxCoordinates.x < aabb.m_minCoordinates.x ||
aabb.m_maxCoordinates.x < m_minCoordinates.x) return false;
if (m_maxCoordinates.y < aabb.m_minCoordinates.y ||
aabb.m_maxCoordinates.y < m_minCoordinates.y) return false;
if (m_maxCoordinates.z < aabb.m_minCoordinates.z||
aabb.m_maxCoordinates.z < m_minCoordinates.z) return false;
return true;
}
// Return the volume of the AABB
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 true if the AABB of a triangle int32_tersects the AABB
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].y, trianglePoints[1].y, trianglePoints[2].y) > mMaxCoordinates.y) return false;
if (min3(trianglePoints[0].z, trianglePoints[1].z, trianglePoints[2].z) > mMaxCoordinates.z) 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) > m_maxCoordinates.y) 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].y, trianglePoints[1].y, trianglePoints[2].y) < mMinCoordinates.y) return false;
if (max3(trianglePoints[0].z, trianglePoints[1].z, trianglePoints[2].z) < mMinCoordinates.z) 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) < m_minCoordinates.y) return false;
if (max3(trianglePoints[0].z, trianglePoints[1].z, trianglePoints[2].z) < m_minCoordinates.z) return false;
return true;
}
@ -173,16 +173,16 @@ inline bool AABB::testCollisionTriangleAABB(const Vector3* trianglePoints) const
// Return true if a point is inside the AABB
inline bool AABB::contains(const Vector3& point) const {
return (point.x >= mMinCoordinates.x - MACHINE_EPSILON && point.x <= mMaxCoordinates.x + MACHINE_EPSILON &&
point.y >= mMinCoordinates.y - MACHINE_EPSILON && point.y <= mMaxCoordinates.y + MACHINE_EPSILON &&
point.z >= mMinCoordinates.z - MACHINE_EPSILON && point.z <= mMaxCoordinates.z + MACHINE_EPSILON);
return (point.x >= m_minCoordinates.x - MACHINE_EPSILON && point.x <= m_maxCoordinates.x + MACHINE_EPSILON &&
point.y >= m_minCoordinates.y - MACHINE_EPSILON && point.y <= m_maxCoordinates.y + MACHINE_EPSILON &&
point.z >= m_minCoordinates.z - MACHINE_EPSILON && point.z <= m_maxCoordinates.z + MACHINE_EPSILON);
}
// Assignment operator
inline AABB& AABB::operator=(const AABB& aabb) {
if (this != &aabb) {
mMinCoordinates = aabb.mMinCoordinates;
mMaxCoordinates = aabb.mMaxCoordinates;
m_minCoordinates = aabb.m_minCoordinates;
m_maxCoordinates = aabb.m_maxCoordinates;
}
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
*/
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.y > float(0.0) && extent.y > margin);
assert(extent.z > float(0.0) && extent.z > margin);
@ -38,7 +38,7 @@ BoxShape::~BoxShape() {
*/
void BoxShape::computeLocalInertiaTensor(Matrix3x3& tensor, float mass) const {
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 ySquare = realExtent.y * realExtent.y;
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 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 {
// Compute the int32_tersection of the ray with the near and far plane of the slab
float oneOverD = float(1.0) / rayDirection[i];
float t1 = (-mExtent[i] - ray.point1[i]) * oneOverD;
float t2 = (mExtent[i] - ray.point1[i]) * oneOverD;
currentNormal[0] = (i == 0) ? -mExtent[i] : float(0.0);
currentNormal[1] = (i == 1) ? -mExtent[i] : float(0.0);
currentNormal[2] = (i == 2) ? -mExtent[i] : float(0.0);
float t1 = (-m_extent[i] - ray.point1[i]) * oneOverD;
float t2 = (m_extent[i] - ray.point1[i]) * oneOverD;
currentNormal[0] = (i == 0) ? -m_extent[i] : float(0.0);
currentNormal[1] = (i == 1) ? -m_extent[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
// t2 with far plane

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

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

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

@ -244,6 +244,6 @@ void ConvexMeshShape::recalculateBounds() {
// Raycast method with feedback information
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);
}

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

@ -235,7 +235,7 @@ inline bool ConvexMeshShape::testPointInside(const Vector3& localPoint,
ProxyShape* proxyShape) const {
// 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);
}

90
ephysics/constraint/BallAndSocketJoint.cpp
View File

@ -15,11 +15,11 @@ const float BallAndSocketJoint::BETA = float(0.2);
// Constructor
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
mLocalAnchorPointBody1 = mBody1->getTransform().getInverse() * jointInfo.anchorPointWorldSpace;
mLocalAnchorPointBody2 = mBody2->getTransform().getInverse() * jointInfo.anchorPointWorldSpace;
m_localAnchorPointBody1 = mBody1->getTransform().getInverse() * jointInfo.m_m_m_m_anchorPointWorldSpace;
m_localAnchorPointBody2 = mBody2->getTransform().getInverse() * jointInfo.m_m_m_m_anchorPointWorldSpace;
}
// Destructor
@ -41,43 +41,43 @@ void BallAndSocketJoint::initBeforeSolve(const ConstraintSolverData& constraintS
const Quaternion& orientationBody2 = mBody2->getTransform().getOrientation();
// Get the inertia tensor of bodies
mI1 = mBody1->getInertiaTensorInverseWorld();
mI2 = mBody2->getInertiaTensorInverseWorld();
m_i1 = mBody1->getInertiaTensorInverseWorld();
m_i2 = mBody2->getInertiaTensorInverseWorld();
// Compute the vector from body center to the anchor point in world-space
mR1World = orientationBody1 * mLocalAnchorPointBody1;
mR2World = orientationBody2 * mLocalAnchorPointBody2;
m_r1World = orientationBody1 * m_localAnchorPointBody1;
m_r2World = orientationBody2 * m_localAnchorPointBody2;
// Compute the corresponding skew-symmetric matrices
Matrix3x3 skewSymmetricMatrixU1= Matrix3x3::computeSkewSymmetricMatrixForCrossProduct(mR1World);
Matrix3x3 skewSymmetricMatrixU2= Matrix3x3::computeSkewSymmetricMatrixForCrossProduct(mR2World);
Matrix3x3 skewSymmetricMatrixU1= Matrix3x3::computeSkewSymmetricMatrixForCrossProduct(m_r1World);
Matrix3x3 skewSymmetricMatrixU2= Matrix3x3::computeSkewSymmetricMatrixForCrossProduct(m_r2World);
// Compute the matrix K=JM^-1J^t (3x3 matrix)
float inverseMassBodies = mBody1->mMassInverse + mBody2->mMassInverse;
Matrix3x3 massMatrix = Matrix3x3(inverseMassBodies, 0, 0,
0, inverseMassBodies, 0,
0, 0, inverseMassBodies) +
skewSymmetricMatrixU1 * mI1 * skewSymmetricMatrixU1.getTranspose() +
skewSymmetricMatrixU2 * mI2 * skewSymmetricMatrixU2.getTranspose();
skewSymmetricMatrixU1 * m_i1 * skewSymmetricMatrixU1.getTranspose() +
skewSymmetricMatrixU2 * m_i2 * skewSymmetricMatrixU2.getTranspose();
// Compute the inverse mass matrix K^-1
mInverseMassMatrix.setToZero();
m_inverseMassMatrix.setToZero();
if (mBody1->getType() == DYNAMIC || mBody2->getType() == DYNAMIC) {
mInverseMassMatrix = massMatrix.getInverse();
m_inverseMassMatrix = massMatrix.getInverse();
}
// Compute the bias "b" of the constraint
mBiasVector.setToZero();
m_biasVector.setToZero();
if (mPositionCorrectionTechnique == BAUMGARTE_JOINTS) {
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 (!constraintSolverData.isWarmStartingActive) {
// Reset the accumulated impulse
mImpulse.setToZero();
m_impulse.setToZero();
}
}
@ -91,19 +91,19 @@ void BallAndSocketJoint::warmstart(const ConstraintSolverData& constraintSolverD
Vector3& w2 = constraintSolverData.angularVelocities[mIndexBody2];
// Compute the impulse P=J^T * lambda for the body 1
const Vector3 linearImpulseBody1 = -mImpulse;
const Vector3 angularImpulseBody1 = mImpulse.cross(mR1World);
const Vector3 linearImpulseBody1 = -m_impulse;
const Vector3 angularImpulseBody1 = m_impulse.cross(m_r1World);
// Apply the impulse to the body 1
v1 += mBody1->mMassInverse * linearImpulseBody1;
w1 += mI1 * angularImpulseBody1;
w1 += m_i1 * angularImpulseBody1;
// 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
v2 += mBody2->mMassInverse * mImpulse;
w2 += mI2 * angularImpulseBody2;
v2 += mBody2->mMassInverse * m_impulse;
w2 += m_i2 * angularImpulseBody2;
}
// Solve the velocity constraint
@ -116,26 +116,26 @@ void BallAndSocketJoint::solveVelocityConstraint(const ConstraintSolverData& con
Vector3& w2 = constraintSolverData.angularVelocities[mIndexBody2];
// 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
const Vector3 deltaLambda = mInverseMassMatrix * (-Jv - mBiasVector);
mImpulse += deltaLambda;
const Vector3 deltaLambda = m_inverseMassMatrix * (-Jv - m_biasVector);
m_impulse += deltaLambda;
// Compute the impulse P=J^T * lambda for the body 1
const Vector3 linearImpulseBody1 = -deltaLambda;
const Vector3 angularImpulseBody1 = deltaLambda.cross(mR1World);
const Vector3 angularImpulseBody1 = deltaLambda.cross(m_r1World);
// Apply the impulse to the body 1
v1 += mBody1->mMassInverse * linearImpulseBody1;
w1 += mI1 * angularImpulseBody1;
w1 += m_i1 * angularImpulseBody1;
// 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
v2 += mBody2->mMassInverse * deltaLambda;
w2 += mI2 * angularImpulseBody2;
w2 += m_i2 * angularImpulseBody2;
}
// Solve the position constraint (for position error correction)
@ -156,44 +156,44 @@ void BallAndSocketJoint::solvePositionConstraint(const ConstraintSolverData& con
float inverseMassBody2 = mBody2->mMassInverse;
// Recompute the inverse inertia tensors
mI1 = mBody1->getInertiaTensorInverseWorld();
mI2 = mBody2->getInertiaTensorInverseWorld();
m_i1 = mBody1->getInertiaTensorInverseWorld();
m_i2 = mBody2->getInertiaTensorInverseWorld();
// Compute the vector from body center to the anchor point in world-space
mR1World = q1 * mLocalAnchorPointBody1;
mR2World = q2 * mLocalAnchorPointBody2;
m_r1World = q1 * m_localAnchorPointBody1;
m_r2World = q2 * m_localAnchorPointBody2;
// Compute the corresponding skew-symmetric matrices
Matrix3x3 skewSymmetricMatrixU1= Matrix3x3::computeSkewSymmetricMatrixForCrossProduct(mR1World);
Matrix3x3 skewSymmetricMatrixU2= Matrix3x3::computeSkewSymmetricMatrixForCrossProduct(mR2World);
Matrix3x3 skewSymmetricMatrixU1= Matrix3x3::computeSkewSymmetricMatrixForCrossProduct(m_r1World);
Matrix3x3 skewSymmetricMatrixU2= Matrix3x3::computeSkewSymmetricMatrixForCrossProduct(m_r2World);
// Recompute the inverse mass matrix K=J^TM^-1J of of the 3 translation constraints
float inverseMassBodies = inverseMassBody1 + inverseMassBody2;
Matrix3x3 massMatrix = Matrix3x3(inverseMassBodies, 0, 0,
0, inverseMassBodies, 0,
0, 0, inverseMassBodies) +
skewSymmetricMatrixU1 * mI1 * skewSymmetricMatrixU1.getTranspose() +
skewSymmetricMatrixU2 * mI2 * skewSymmetricMatrixU2.getTranspose();
mInverseMassMatrix.setToZero();
skewSymmetricMatrixU1 * m_i1 * skewSymmetricMatrixU1.getTranspose() +
skewSymmetricMatrixU2 * m_i2 * skewSymmetricMatrixU2.getTranspose();
m_inverseMassMatrix.setToZero();
if (mBody1->getType() == DYNAMIC || mBody2->getType() == DYNAMIC) {
mInverseMassMatrix = massMatrix.getInverse();
m_inverseMassMatrix = massMatrix.getInverse();
}
// 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
// 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.
const Vector3 lambda = mInverseMassMatrix * (-constraintError);
const Vector3 lambda = m_inverseMassMatrix * (-constraintError);
// Compute the impulse of body 1
const Vector3 linearImpulseBody1 = -lambda;
const Vector3 angularImpulseBody1 = lambda.cross(mR1World);
const Vector3 angularImpulseBody1 = lambda.cross(m_r1World);
// Compute the pseudo velocity of body 1
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
x1 += v1;
@ -201,11 +201,11 @@ void BallAndSocketJoint::solvePositionConstraint(const ConstraintSolverData& con
q1.normalize();
// 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
const Vector3 v2 = inverseMassBody2 * lambda;
const Vector3 w2 = mI2 * angularImpulseBody2;
const Vector3 w2 = m_i2 * angularImpulseBody2;
// Update the body position/orientation of body 2
x2 += v2;

22
ephysics/constraint/BallAndSocketJoint.h
View File

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

116
ephysics/constraint/FixedJoint.cpp
View File

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

24
ephysics/constraint/FixedJoint.h
View File

@ -23,7 +23,7 @@ struct FixedJointInfo : public JointInfo {
// -------------------- Attributes -------------------- //
/// Anchor point (in world-space coordinates)
Vector3 anchorPointWorldSpace;
Vector3 m_m_m_m_anchorPointWorldSpace;
/// Constructor
/**
@ -35,7 +35,7 @@ struct FixedJointInfo : public JointInfo {
FixedJointInfo(RigidBody* rigidBody1, RigidBody* rigidBody2,
const Vector3& initAnchorPointWorldSpace)
: JointInfo(rigidBody1, rigidBody2, FIXEDJOINT),
anchorPointWorldSpace(initAnchorPointWorldSpace){}
m_m_m_m_anchorPointWorldSpace(initAnchorPointWorldSpace){}
};
// Class FixedJoint
@ -55,34 +55,34 @@ class FixedJoint : public Joint {
// -------------------- Attributes -------------------- //
/// 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)
Vector3 mLocalAnchorPointBody2;
Vector3 m_localAnchorPointBody2;
/// 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
Vector3 mR2World;
Vector3 m_r2World;
/// Inertia tensor of body 1 (in world-space coordinates)
Matrix3x3 mI1;
Matrix3x3 m_i1;
/// Inertia tensor of body 2 (in world-space coordinates)
Matrix3x3 mI2;
Matrix3x3 m_i2;
/// Accumulated impulse for the 3 translation constraints
Vector3 mImpulseTranslation;
Vector3 m_impulseTranslation;
/// 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)
Matrix3x3 mInverseMassMatrixTranslation;
Matrix3x3 m_inverseMassMatrixTranslation;
/// 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
Vector3 mBiasTranslation;

212
ephysics/constraint/HingeJoint.cpp
View File

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

40
ephysics/constraint/HingeJoint.h
View File

@ -23,7 +23,7 @@ struct HingeJointInfo : public JointInfo {
// -------------------- Attributes -------------------- //
/// Anchor point (in world-space coordinates)
Vector3 anchorPointWorldSpace;
Vector3 m_m_m_m_anchorPointWorldSpace;
/// Hinge rotation axis (in world-space coordinates)
Vector3 rotationAxisWorld;
@ -62,7 +62,7 @@ struct HingeJointInfo : public JointInfo {
const Vector3& initAnchorPointWorldSpace,
const Vector3& initRotationAxisWorld)
: JointInfo(rigidBody1, rigidBody2, HINGEJOINT),
anchorPointWorldSpace(initAnchorPointWorldSpace),
m_m_m_m_anchorPointWorldSpace(initAnchorPointWorldSpace),
rotationAxisWorld(initRotationAxisWorld), isLimitEnabled(false),
isMotorEnabled(false), minAngleLimit(-1), maxAngleLimit(1),
motorSpeed(0), maxMotorTorque(0) {}
@ -81,7 +81,7 @@ struct HingeJointInfo : public JointInfo {
const Vector3& initRotationAxisWorld,
float initMinAngleLimit, float initMaxAngleLimit)
: JointInfo(rigidBody1, rigidBody2, HINGEJOINT),
anchorPointWorldSpace(initAnchorPointWorldSpace),
m_m_m_m_anchorPointWorldSpace(initAnchorPointWorldSpace),
rotationAxisWorld(initRotationAxisWorld), isLimitEnabled(true),
isMotorEnabled(false), minAngleLimit(initMinAngleLimit),
maxAngleLimit(initMaxAngleLimit), motorSpeed(0),
@ -104,7 +104,7 @@ struct HingeJointInfo : public JointInfo {
float initMinAngleLimit, float initMaxAngleLimit,
float initMotorSpeed, float initMaxMotorTorque)
: JointInfo(rigidBody1, rigidBody2, HINGEJOINT),
anchorPointWorldSpace(initAnchorPointWorldSpace),
m_m_m_m_anchorPointWorldSpace(initAnchorPointWorldSpace),
rotationAxisWorld(initRotationAxisWorld), isLimitEnabled(true),
isMotorEnabled(false), minAngleLimit(initMinAngleLimit),
maxAngleLimit(initMaxAngleLimit), motorSpeed(initMotorSpeed),
@ -129,10 +129,10 @@ class HingeJoint : public Joint {
// -------------------- Attributes -------------------- //
/// 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)
Vector3 mLocalAnchorPointBody2;
Vector3 m_localAnchorPointBody2;
/// Hinge rotation axis (in local-space coordinates of body 1)
Vector3 mHingeLocalAxisBody1;
@ -141,19 +141,19 @@ class HingeJoint : public Joint {
Vector3 mHingeLocalAxisBody2;
/// Inertia tensor of body 1 (in world-space coordinates)
Matrix3x3 mI1;
Matrix3x3 m_i1;
/// 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
Vector3 mA1;
/// 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
Vector3 mR2World;
Vector3 m_r2World;
/// Cross product of vector b2 and a1
Vector3 mB2CrossA1;
@ -162,31 +162,31 @@ class HingeJoint : public Joint {
Vector3 mC2CrossA1;
/// Impulse for the 3 translation constraints
Vector3 mImpulseTranslation;
Vector3 m_impulseTranslation;
/// Impulse for the 2 rotation constraints
Vector2 mImpulseRotation;
Vector2 m_impulseRotation;
/// Accumulated impulse for the lower limit constraint
float mImpulseLowerLimit;
float m_impulseLowerLimit;
/// Accumulated impulse for the upper limit constraint
float mImpulseUpperLimit;
float m_impulseUpperLimit;
/// Accumulated impulse for the motor constraint;
float mImpulseMotor;
float m_impulseMotor;
/// 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
Matrix2x2 mInverseMassMatrixRotation;
Matrix2x2 m_inverseMassMatrixRotation;
/// 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
float mInverseMassMatrixMotor;
float m_inverseMassMatrixMotor;
/// Bias vector for the error correction for the translation constraints
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
*/
inline float HingeJoint::getMotorTorque(float timeStep) const {
return mImpulseMotor / timeStep;
return m_impulseMotor / timeStep;
}
// 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)
:mBody1(jointInfo.body1), mBody2(jointInfo.body2), mType(jointInfo.type),
mPositionCorrectionTechnique(jointInfo.positionCorrectionTechnique),
mIsCollisionEnabled(jointInfo.isCollisionEnabled), mIsAlreadyInIsland(false) {
mIsCollisionEnabled(jointInfo.isCollisionEnabled), m_isAlreadyInIsland(false) {
assert(mBody1 != NULL);
assert(mBody2 != NULL);

4
ephysics/constraint/Joint.h
View File

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

36
ephysics/constraint/SliderJoint.h
View File

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

12
ephysics/engine/CollisionWorld.cpp
View File

@ -14,7 +14,7 @@ using namespace std;
// Constructor
CollisionWorld::CollisionWorld()
: mCollisionDetection(this, mMemoryAllocator), mCurrentBodyID(0),
: m_collisionDetection(this, mMemoryAllocator), mCurrentBodyID(0),
mEventListener(NULL) {
}
@ -147,7 +147,7 @@ void CollisionWorld::testCollision(const ProxyShape* shape,
std::set<uint32_t> emptySet;
// 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
@ -170,7 +170,7 @@ void CollisionWorld::testCollision(const ProxyShape* shape1,
shapes2.insert(shape2->mBroadPhaseID);
// 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
@ -197,7 +197,7 @@ void CollisionWorld::testCollision(const CollisionBody* body,
std::set<uint32_t> emptySet;
// 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
@ -227,7 +227,7 @@ void CollisionWorld::testCollision(const CollisionBody* body1,
}
// 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
@ -242,6 +242,6 @@ void CollisionWorld::testCollision(CollisionCallback* callback) {
std::set<uint32_t> emptySet;
// 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 -------------------- //
/// Reference to the collision detection
CollisionDetection mCollisionDetection;
CollisionDetection m_collisionDetection;
/// All the bodies (rigid and soft) of the world
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
*/
inline void CollisionWorld::setCollisionDispatch(CollisionDispatch* collisionDispatch) {
mCollisionDetection.setCollisionDispatch(collisionDispatch);
m_collisionDetection.setCollisionDispatch(collisionDispatch);
}
// Ray cast method
@ -176,7 +176,7 @@ inline void CollisionWorld::setCollisionDispatch(CollisionDispatch* collisionDis
inline void CollisionWorld::raycast(const Ray& ray,
RaycastCallback* raycastCallback,
unsigned short raycastWithCategoryMaskBits) const {
mCollisionDetection.raycast(raycastCallback, ray, raycastWithCategoryMaskBits);
m_collisionDetection.raycast(raycastCallback, ray, raycastWithCategoryMaskBits);
}
// 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,
const ProxyShape* shape2) const {
return mCollisionDetection.testAABBOverlap(shape1, shape2);
return m_collisionDetection.testAABBOverlap(shape1, shape2);
}
// Class CollisionCallback

56
ephysics/engine/DynamicsWorld.cpp
View File

@ -25,7 +25,7 @@ DynamicsWorld::DynamicsWorld(const Vector3 &gravity)
mConstraintSolver(mMapBodyToConstrainedVelocityIndex),
mNbVelocitySolverIterations(DEFAULT_VELOCITY_SOLVER_NB_ITERATIONS),
mNbPositionSolverIterations(DEFAULT_POSITION_SOLVER_NB_ITERATIONS),
mIsSleepingEnabled(SPLEEPING_ENABLED), mGravity(gravity),
m_isSleepingEnabled(SPLEEPING_ENABLED), mGravity(gravity),
mIsGravityEnabled(true), mConstrainedLinearVelocities(NULL),
mConstrainedAngularVelocities(NULL), mSplitLinearVelocities(NULL),
mSplitAngularVelocities(NULL), mConstrainedPositions(NULL),
@ -122,7 +122,7 @@ void DynamicsWorld::update(float timeStep) {
}
// Compute the collision detection
mCollisionDetection.computeCollisionDetection();
m_collisionDetection.computeCollisionDetection();
// Compute the islands (separate groups of bodies with constraints between each others)
computeIslands();
@ -142,7 +142,7 @@ void DynamicsWorld::update(float timeStep) {
// Update the state (positions and velocities) of the bodies
updateBodiesState();
if (mIsSleepingEnabled) updateSleepingBodies();
if (m_isSleepingEnabled) updateSleepingBodies();
// Notify the event listener about the end of an int32_ternal tick
if (mEventListener != NULL) mEventListener->endInternalTick();
@ -538,7 +538,7 @@ Joint* DynamicsWorld::createJoint(const JointInfo& jointInfo) {
if (!jointInfo.isCollisionEnabled) {
// 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
@ -563,7 +563,7 @@ void DynamicsWorld::destroyJoint(Joint* joint) {
if (!joint->isCollisionEnabled()) {
// 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
@ -645,7 +645,7 @@ void DynamicsWorld::computeIslands() {
nbContactManifolds += nbBodyManifolds;
}
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
@ -658,7 +658,7 @@ void DynamicsWorld::computeIslands() {
RigidBody* body = *it;
// 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 (body->getType() == STATIC) continue;
@ -670,7 +670,7 @@ void DynamicsWorld::computeIslands() {
uint32_t stackIndex = 0;
stackBodiesToVisit[stackIndex] = body;
stackIndex++;
body->mIsAlreadyInIsland = true;
body->m_isAlreadyInIsland = true;
// Create the new island
void* allocatedMemoryIsland = mMemoryAllocator.allocate(sizeof(Island));
@ -710,7 +710,7 @@ void DynamicsWorld::computeIslands() {
// Add the contact manifold int32_to the island
mIslands[mNbIslands]->addContactManifold(contactManifold);
contactManifold->mIsAlreadyInIsland = true;
contactManifold->m_isAlreadyInIsland = true;
// Get the other body of the contact manifold
RigidBody* body1 = static_cast<RigidBody*>(contactManifold->getBody1());
@ -718,12 +718,12 @@ void DynamicsWorld::computeIslands() {
RigidBody* otherBody = (body1->getID() == bodyToVisit->getID()) ? body2 : body1;
// 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
stackBodiesToVisit[stackIndex] = otherBody;
stackIndex++;
otherBody->mIsAlreadyInIsland = true;
otherBody->m_isAlreadyInIsland = true;
}
// For each joint in which the current body is involved
@ -738,7 +738,7 @@ void DynamicsWorld::computeIslands() {
// Add the joint int32_to the island
mIslands[mNbIslands]->addJoint(joint);
joint->mIsAlreadyInIsland = true;
joint->m_isAlreadyInIsland = true;
// Get the other body of the contact manifold
RigidBody* body1 = static_cast<RigidBody*>(joint->getBody1());
@ -746,12 +746,12 @@ void DynamicsWorld::computeIslands() {
RigidBody* otherBody = (body1->getID() == bodyToVisit->getID()) ? body2 : body1;
// 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
stackBodiesToVisit[stackIndex] = otherBody;
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++) {
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()) {
// Reset the sleep time of the body
bodies[b]->mSleepTime = float(0.0);
bodies[b]->m_sleepTime = float(0.0);
minSleepTime = float(0.0);
}
else { // If the body velocity is bellow the sleeping velocity threshold
// Increase the sleep time
bodies[b]->mSleepTime += mTimeStep;
if (bodies[b]->mSleepTime < minSleepTime) {
minSleepTime = bodies[b]->mSleepTime;
bodies[b]->m_sleepTime += mTimeStep;
if (bodies[b]->m_sleepTime < minSleepTime) {
minSleepTime = bodies[b]->m_sleepTime;
}
}
}
@ -833,9 +833,9 @@ void DynamicsWorld::updateSleepingBodies() {
* and false otherwise
*/
void DynamicsWorld::enableSleeping(bool isSleepingEnabled) {
mIsSleepingEnabled = isSleepingEnabled;
m_isSleepingEnabled = isSleepingEnabled;
if (!mIsSleepingEnabled) {
if (!m_isSleepingEnabled) {
// For each body of the world
std::set<RigidBody*>::iterator it;
@ -864,7 +864,7 @@ void DynamicsWorld::testCollision(const ProxyShape* shape,
std::set<uint32_t> emptySet;
// 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.
@ -886,7 +886,7 @@ void DynamicsWorld::testCollision(const ProxyShape* shape1,
shapes2.insert(shape2->mBroadPhaseID);
// 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
@ -912,7 +912,7 @@ void DynamicsWorld::testCollision(const CollisionBody* body,
std::set<uint32_t> emptySet;
// 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.
@ -941,7 +941,7 @@ void DynamicsWorld::testCollision(const CollisionBody* body1,
}
// 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.
@ -955,7 +955,7 @@ void DynamicsWorld::testCollision(CollisionCallback* callback) {
std::set<uint32_t> emptySet;
// 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
@ -965,8 +965,8 @@ std::vector<const ContactManifold*> DynamicsWorld::getContactsList() const {
// For each currently overlapping pair of bodies
std::map<overlappingpairid, OverlappingPair*>::const_iterator it;
for (it = mCollisionDetection.mOverlappingPairs.begin();
it != mCollisionDetection.mOverlappingPairs.end(); ++it) {
for (it = m_collisionDetection.mOverlappingPairs.begin();
it != m_collisionDetection.mOverlappingPairs.end(); ++it) {
OverlappingPair* pair = it->second;

4
ephysics/engine/DynamicsWorld.h
View File

@ -42,7 +42,7 @@ class DynamicsWorld : public CollisionWorld {
uint32_t mNbPositionSolverIterations;
/// True if the spleeping technique for inactive bodies is enabled
bool mIsSleepingEnabled;
bool m_isSleepingEnabled;
/// All the rigid bodies of the physics world
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
*/
inline bool DynamicsWorld::isSleepingEnabled() const {
return mIsSleepingEnabled;
return m_isSleepingEnabled;
}
// Return the current sleep linear velocity

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

@ -32,14 +32,14 @@ using namespace openglframework;
// Constructor
Light::Light()
: mDiffuseColor(Color::white()),
mSpecularColor(Color::white()), mIsActive(false) {
mSpecularColor(Color::white()), m_isActive(false) {
}
// Constructor
Light::Light(Color diffuseColor, Color specularColor)
: 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;
// True if the light is active
bool mIsActive;
bool m_isActive;
public:
@ -112,13 +112,13 @@ inline void Light::setSpecularColor(const Color& color) {
// Return true if the light is active
inline bool Light::getIsActive() const {
return mIsActive;
return m_isActive;
}
// Enable the light
inline void Light::enable() {
mIsActive = true;
m_isActive = true;
// Enable the light
//glEnable(GL_LIGHTING);
@ -128,7 +128,7 @@ inline void Light::enable() {
// Disable the light
inline void Light::disable() {
mIsActive = false;
m_isActive = false;
// Disable the light
//glDisable(GL_LIGHT0 + mLightID);

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

@ -33,13 +33,13 @@
using namespace openglframework;
// Constructor
Texture2D::Texture2D() : mID(0), mUnit(0), mWidth(0), mHeight(0) {
Texture2D::Texture2D() : m_id(0), mUnit(0), mWidth(0), mHeight(0) {
}
// Constructor
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(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;
// Create the OpenGL texture
glGenTextures(1, &mID);
assert(mID != 0);
glBindTexture(GL_TEXTURE_2D, mID);
glGenTextures(1, &m_id);
assert(m_id != 0);
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_T, GL_CLAMP_TO_EDGE);
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;
// Create the OpenGL texture
glGenTextures(1, &mID);
assert(mID != 0);
glBindTexture(GL_TEXTURE_2D, mID);
glGenTextures(1, &m_id);
assert(m_id != 0);
glBindTexture(GL_TEXTURE_2D, m_id);
glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_WRAP_S, wrapS);
glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_WRAP_T, wrapT);
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
void Texture2D::destroy() {
if (mID != 0) {
glDeleteTextures(1, &mID);
mID = 0;
if (m_id != 0) {
glDeleteTextures(1, &m_id);
m_id = 0;
mUnit = 0;
mWidth = 0;
mHeight = 0;

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

@ -43,7 +43,7 @@ class Texture2D {
// -------------------- Attributes -------------------- //
// OpenGL texture ID
GLuint32_t mID;
GLuint32_t m_id;
// Current texture unit for this texture
GLuint32_t mUnit;
@ -102,21 +102,21 @@ class Texture2D {
// Bind the texture
inline void Texture2D::bind() const {
assert(mID != 0);
assert(m_id != 0);
glActiveTexture(GL_TEXTURE0 + mUnit);
glBindTexture(GL_TEXTURE_2D, mID);
glBindTexture(GL_TEXTURE_2D, m_id);
}
// Unbind the texture
inline void Texture2D::unbind() const {
assert(mID != 0);
assert(m_id != 0);
glActiveTexture(GL_TEXTURE0 + mUnit);
glBindTexture(GL_TEXTURE_2D, 0);
}
// Get the OpenGL texture ID
inline uint32_t Texture2D::getID() const {
return mID;
return m_id;
}
// 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::Vector3 body1Position = body1->getTransform().getPosition();
rp3d::Vector3 body2Position = body2->getTransform().getPosition();
const rp3d::Vector3 anchorPointWorldSpace = 0.5 * (body1Position + body2Position);
rp3d::BallAndSocketJointInfo jointInfo(body1, body2, anchorPointWorldSpace);
const rp3d::Vector3 m_m_m_m_anchorPointWorldSpace = 0.5 * (body1Position + body2Position);
rp3d::BallAndSocketJointInfo jointInfo(body1, body2, m_m_m_m_anchorPointWorldSpace);
// Create the joint in the dynamics world
mBallAndSocketJoints[i] = dynamic_cast<rp3d::BallAndSocketJoint*>(
@ -352,9 +352,9 @@ void JointsScene::createSliderJoint() {
rp3d::RigidBody* body2 = mSliderJointTopBox->getRigidBody();
const rp3d::Vector3& body1Position = body1->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);
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));
jointInfo.isMotorEnabled = true;
jointInfo.motorSpeed = 0.0;
@ -392,9 +392,9 @@ void JointsScene::createPropellerHingeJoint() {
rp3d::RigidBody* body2 = mSliderJointTopBox->getRigidBody();
const rp3d::Vector3& body1Position = body1->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);
rp3d::HingeJointInfo jointInfo(body1, body2, anchorPointWorldSpace, hingeAxisWorldSpace);
rp3d::HingeJointInfo jointInfo(body1, body2, m_m_m_m_anchorPointWorldSpace, hingeAxisWorldSpace);
jointInfo.isMotorEnabled = true;
jointInfo.motorSpeed = - rp3d::float(0.5) * PI;
jointInfo.maxMotorTorque = rp3d::float(60.0);
@ -445,8 +445,8 @@ void JointsScene::createFixedJoints() {
// Create the joint info object
rp3d::RigidBody* body1 = mFixedJointBox1->getRigidBody();
rp3d::RigidBody* propellerBody = mPropellerBox->getRigidBody();
const rp3d::Vector3 anchorPointWorldSpace1(5, 7, 0);
rp3d::FixedJointInfo jointInfo1(body1, propellerBody, anchorPointWorldSpace1);
const rp3d::Vector3 m_m_m_m_anchorPointWorldSpace1(5, 7, 0);
rp3d::FixedJointInfo jointInfo1(body1, propellerBody, m_m_m_m_anchorPointWorldSpace1);
jointInfo1.isCollisionEnabled = false;
// Create the joint in the dynamics world
@ -456,8 +456,8 @@ void JointsScene::createFixedJoints() {
// Create the joint info object
rp3d::RigidBody* body2 = mFixedJointBox2->getRigidBody();
const rp3d::Vector3 anchorPointWorldSpace2(-5, 7, 0);
rp3d::FixedJointInfo jointInfo2(body2, propellerBody, anchorPointWorldSpace2);
const rp3d::Vector3 m_m_m_m_anchorPointWorldSpace2(-5, 7, 0);
rp3d::FixedJointInfo jointInfo2(body2, propellerBody, m_m_m_m_anchorPointWorldSpace2);
jointInfo2.isCollisionEnabled = false;
// Create the joint in the dynamics world