mrq/reactphysics3d
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Optimizations

Browse Source
This commit is contained in:
Daniel Chappuis 2020-07-28 23:34:25 +02:00
parent fd09fcf660
commit aa6b228e10
8 changed files with 215 additions and 191 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
include/reactphysics3d/collision/ContactManifold.h
View File

@ -101,13 +101,19 @@ class ContactManifold {
/// True if the contact manifold has already been added into an island /// True if the contact manifold has already been added into an island
bool isAlreadyInIsland; bool isAlreadyInIsland;
/// Index of the first body of the manifold in the mRigidBodyComponents array (only if body 1 is a RigidBody)
uint32 rigidBody1Index;
/// Index of the second body of the manifold in the mRigidBodyComponents array (only if body 2 is a RigidBody)
uint32 rigidBody2Index;
public: public:
// -------------------- Methods -------------------- // // -------------------- Methods -------------------- //
/// Constructor /// Constructor
ContactManifold(Entity bodyEntity1, Entity bodyEntity2, Entity colliderEntity1, Entity colliderEntity2, ContactManifold(Entity bodyEntity1, Entity bodyEntity2, Entity colliderEntity1, Entity colliderEntity2,
uint contactPointsIndex, uint8 nbContactPoints); uint contactPointsIndex, uint8 nbContactPoints, uint32 rigidBody1Index, uint32 rigidBody2Index);
// -------------------- Friendship -------------------- // // -------------------- Friendship -------------------- //

45
include/reactphysics3d/engine/Material.h
View File

@ -28,6 +28,7 @@
// Libraries // Libraries
#include <cassert> #include <cassert>
#include <cmath>
#include <reactphysics3d/configuration.h> #include <reactphysics3d/configuration.h>
namespace reactphysics3d { namespace reactphysics3d {
@ -44,8 +45,8 @@ class Material {
// -------------------- Attributes -------------------- // // -------------------- Attributes -------------------- //
/// Friction coefficient (positive value) /// Square root of the friction coefficient
decimal mFrictionCoefficient; decimal mFrictionCoefficientSqrt;
/// Rolling resistance factor (positive value) /// Rolling resistance factor (positive value)
decimal mRollingResistance; decimal mRollingResistance;
@ -59,14 +60,7 @@ class Material {
// -------------------- Methods -------------------- // // -------------------- Methods -------------------- //
/// Constructor /// Constructor
Material(decimal frictionCoefficient, decimal rollingResistance, decimal bounciness, Material(decimal frictionCoefficient, decimal rollingResistance, decimal bounciness, decimal massDensity = decimal(1.0));
decimal massDensity = decimal(1.0));
/// Copy-constructor
Material(const Material& material);
/// Destructor
~Material() = default;
public : public :
@ -84,6 +78,9 @@ class Material {
/// Set the friction coefficient. /// Set the friction coefficient.
void setFrictionCoefficient(decimal frictionCoefficient); void setFrictionCoefficient(decimal frictionCoefficient);
/// Return the square root friction coefficient
decimal getFrictionCoefficientSqrt() const;
/// Return the rolling resistance factor /// Return the rolling resistance factor
decimal getRollingResistance() const; decimal getRollingResistance() const;
@ -99,9 +96,6 @@ class Material {
/// Return a string representation for the material /// Return a string representation for the material
std::string to_string() const; std::string to_string() const;
/// Overloaded assignment operator
Material& operator=(const Material& material);
// ---------- Friendship ---------- // // ---------- Friendship ---------- //
friend class Collider; friend class Collider;
@ -131,7 +125,7 @@ RP3D_FORCE_INLINE void Material::setBounciness(decimal bounciness) {
* @return Friction coefficient (positive value) * @return Friction coefficient (positive value)
*/ */
RP3D_FORCE_INLINE decimal Material::getFrictionCoefficient() const { RP3D_FORCE_INLINE decimal Material::getFrictionCoefficient() const {
return mFrictionCoefficient; return mFrictionCoefficientSqrt * mFrictionCoefficientSqrt;
} }
// Set the friction coefficient. // Set the friction coefficient.
@ -142,7 +136,12 @@ RP3D_FORCE_INLINE decimal Material::getFrictionCoefficient() const {
*/ */
RP3D_FORCE_INLINE void Material::setFrictionCoefficient(decimal frictionCoefficient) { RP3D_FORCE_INLINE void Material::setFrictionCoefficient(decimal frictionCoefficient) {
assert(frictionCoefficient >= decimal(0.0)); assert(frictionCoefficient >= decimal(0.0));
mFrictionCoefficient = frictionCoefficient; mFrictionCoefficientSqrt = std::sqrt(frictionCoefficient);
}
// Return the square root friction coefficient
RP3D_FORCE_INLINE decimal Material::getFrictionCoefficientSqrt() const {
return mFrictionCoefficientSqrt;
} }
// Return the rolling resistance factor. If this value is larger than zero, // Return the rolling resistance factor. If this value is larger than zero,
@ -184,27 +183,13 @@ RP3D_FORCE_INLINE std::string Material::to_string() const {
std::stringstream ss; std::stringstream ss;
ss << "frictionCoefficient=" << mFrictionCoefficient << std::endl; ss << "frictionCoefficient=" << (mFrictionCoefficientSqrt * mFrictionCoefficientSqrt) << std::endl;
ss << "rollingResistance=" << mRollingResistance << std::endl; ss << "rollingResistance=" << mRollingResistance << std::endl;
ss << "bounciness=" << mBounciness << std::endl; ss << "bounciness=" << mBounciness << std::endl;
return ss.str(); return ss.str();
} }
// Overloaded assignment operator
RP3D_FORCE_INLINE Material& Material::operator=(const Material& material) {
// Check for self-assignment
if (this != &material) {
mFrictionCoefficient = material.mFrictionCoefficient;
mBounciness = material.mBounciness;
mRollingResistance = material.mRollingResistance;
}
// Return this material
return *this;
}
} }
#endif #endif

3
include/reactphysics3d/mathematics/Matrix3x3.h
View File

@ -95,6 +95,9 @@ class Matrix3x3 {
/// Return the inverse matrix /// Return the inverse matrix
Matrix3x3 getInverse() const; Matrix3x3 getInverse() const;
/// Return the inverse matrix
Matrix3x3 getInverseWithDeterminant(decimal determinant) const;
/// Return the matrix with absolute values /// Return the matrix with absolute values
Matrix3x3 getAbsoluteMatrix() const; Matrix3x3 getAbsoluteMatrix() const;

4
src/collision/ContactManifold.cpp
View File

@ -32,9 +32,9 @@ using namespace reactphysics3d;
// Constructor // Constructor
ContactManifold::ContactManifold(Entity bodyEntity1, Entity bodyEntity2, Entity colliderEntity1, Entity colliderEntity2, ContactManifold::ContactManifold(Entity bodyEntity1, Entity bodyEntity2, Entity colliderEntity1, Entity colliderEntity2,
uint contactPointsIndex, uint8 nbContactPoints) uint contactPointsIndex, uint8 nbContactPoints, uint32 rigidBody1Index, uint32 rigidBody2Index)
:contactPointsIndex(contactPointsIndex), bodyEntity1(bodyEntity1), bodyEntity2(bodyEntity2), :contactPointsIndex(contactPointsIndex), bodyEntity1(bodyEntity1), bodyEntity2(bodyEntity2),
colliderEntity1(colliderEntity1), colliderEntity2(colliderEntity2), nbContactPoints(nbContactPoints), frictionImpulse1(0), frictionImpulse2(0), colliderEntity1(colliderEntity1), colliderEntity2(colliderEntity2), nbContactPoints(nbContactPoints), frictionImpulse1(0), frictionImpulse2(0),
frictionTwistImpulse(0), isAlreadyInIsland(false) { frictionTwistImpulse(0), isAlreadyInIsland(false), rigidBody1Index(rigidBody1Index), rigidBody2Index(rigidBody2Index) {
} }

11
src/engine/Material.cpp
View File

@ -30,14 +30,7 @@ using namespace reactphysics3d;
// Constructor // Constructor
Material::Material(decimal frictionCoefficient, decimal rollingResistance, decimal bounciness, decimal massDensity) Material::Material(decimal frictionCoefficient, decimal rollingResistance, decimal bounciness, decimal massDensity)
: mFrictionCoefficient(frictionCoefficient), mRollingResistance(rollingResistance), mBounciness(bounciness), : mFrictionCoefficientSqrt(std::sqrt(frictionCoefficient)),
mMassDensity(massDensity) { mRollingResistance(rollingResistance), mBounciness(bounciness), mMassDensity(massDensity) {
}
// Copy-constructor
Material::Material(const Material& material)
: mFrictionCoefficient(material.mFrictionCoefficient), mRollingResistance(material.mRollingResistance),
mBounciness(material.mBounciness) {
} }

21
src/mathematics/Matrix3x3.cpp
View File

@ -66,5 +66,26 @@ Matrix3x3 Matrix3x3::getInverse() const {
return (invDeterminant * tempMatrix); return (invDeterminant * tempMatrix);
} }
// Return the inverse matrix
Matrix3x3 Matrix3x3::getInverseWithDeterminant(decimal determinant) const {
// Check if the determinant is equal to zero
assert(determinant != decimal(0.0));
decimal invDeterminant = decimal(1.0) / determinant;
Matrix3x3 tempMatrix((mRows[1][1]*mRows[2][2]-mRows[2][1]*mRows[1][2]),
-(mRows[0][1]*mRows[2][2]-mRows[2][1]*mRows[0][2]),
(mRows[0][1]*mRows[1][2]-mRows[0][2]*mRows[1][1]),
-(mRows[1][0]*mRows[2][2]-mRows[2][0]*mRows[1][2]),
(mRows[0][0]*mRows[2][2]-mRows[2][0]*mRows[0][2]),
-(mRows[0][0]*mRows[1][2]-mRows[1][0]*mRows[0][2]),
(mRows[1][0]*mRows[2][1]-mRows[2][0]*mRows[1][1]),
-(mRows[0][0]*mRows[2][1]-mRows[2][0]*mRows[0][1]),
(mRows[0][0]*mRows[1][1]-mRows[0][1]*mRows[1][0]));
// Return the inverse matrix
return (invDeterminant * tempMatrix);
}

66
src/systems/CollisionDetectionSystem.cpp
View File

@ -167,7 +167,8 @@ void CollisionDetectionSystem::updateOverlappingPairs(const List<Pair<int32, int
RP3D_PROFILE("CollisionDetectionSystem::updateOverlappingPairs()", mProfiler); RP3D_PROFILE("CollisionDetectionSystem::updateOverlappingPairs()", mProfiler);
// For each overlapping pair of nodes // For each overlapping pair of nodes
for (uint i=0; i < overlappingNodes.size(); i++) { const uint nbOverlappingNodes = overlappingNodes.size();
for (uint i=0; i < nbOverlappingNodes; i++) {
Pair<int32, int32> nodePair = overlappingNodes[i]; Pair<int32, int32> nodePair = overlappingNodes[i];
@ -241,7 +242,8 @@ void CollisionDetectionSystem::computeMiddlePhase(NarrowPhaseInput& narrowPhaseI
mOverlappingPairs.clearObsoleteLastFrameCollisionInfos(); mOverlappingPairs.clearObsoleteLastFrameCollisionInfos();
// For each possible convex vs convex pair of bodies // For each possible convex vs convex pair of bodies
for (uint64 i=0; i < mOverlappingPairs.getNbConvexVsConvexPairs(); i++) { const uint64 nbConvexVsConvexPairs = mOverlappingPairs.getNbConvexVsConvexPairs();
for (uint64 i=0; i < nbConvexVsConvexPairs; i++) {
assert(mCollidersComponents.getBroadPhaseId(mOverlappingPairs.mColliders1[i]) != -1); assert(mCollidersComponents.getBroadPhaseId(mOverlappingPairs.mColliders1[i]) != -1);
assert(mCollidersComponents.getBroadPhaseId(mOverlappingPairs.mColliders2[i]) != -1); assert(mCollidersComponents.getBroadPhaseId(mOverlappingPairs.mColliders2[i]) != -1);
@ -278,7 +280,8 @@ void CollisionDetectionSystem::computeMiddlePhase(NarrowPhaseInput& narrowPhaseI
// For each possible convex vs concave pair of bodies // For each possible convex vs concave pair of bodies
const uint64 convexVsConcaveStartIndex = mOverlappingPairs.getConvexVsConcavePairsStartIndex(); const uint64 convexVsConcaveStartIndex = mOverlappingPairs.getConvexVsConcavePairsStartIndex();
for (uint64 i=convexVsConcaveStartIndex; i < convexVsConcaveStartIndex + mOverlappingPairs.getNbConvexVsConcavePairs(); i++) { const uint64 nbConvexVsConcavePairs = mOverlappingPairs.getNbConvexVsConcavePairs();
for (uint64 i=convexVsConcaveStartIndex; i < convexVsConcaveStartIndex + nbConvexVsConcavePairs; i++) {
assert(mCollidersComponents.getBroadPhaseId(mOverlappingPairs.mColliders1[i]) != -1); assert(mCollidersComponents.getBroadPhaseId(mOverlappingPairs.mColliders1[i]) != -1);
assert(mCollidersComponents.getBroadPhaseId(mOverlappingPairs.mColliders2[i]) != -1); assert(mCollidersComponents.getBroadPhaseId(mOverlappingPairs.mColliders2[i]) != -1);
@ -307,7 +310,8 @@ void CollisionDetectionSystem::computeMiddlePhaseCollisionSnapshot(List<uint64>&
mOverlappingPairs.clearObsoleteLastFrameCollisionInfos(); mOverlappingPairs.clearObsoleteLastFrameCollisionInfos();
// For each possible convex vs convex pair of bodies // For each possible convex vs convex pair of bodies
for (uint64 p=0; p < convexPairs.size(); p++) { const uint64 nbConvexPairs = convexPairs.size();
for (uint64 p=0; p < nbConvexPairs; p++) {
const uint64 pairId = convexPairs[p]; const uint64 pairId = convexPairs[p];
@ -339,7 +343,8 @@ void CollisionDetectionSystem::computeMiddlePhaseCollisionSnapshot(List<uint64>&
} }
// For each possible convex vs concave pair of bodies // For each possible convex vs concave pair of bodies
for (uint p=0; p < concavePairs.size(); p++) { const uint nbConcavePairs = concavePairs.size();
for (uint p=0; p < nbConcavePairs; p++) {
const uint64 pairId = concavePairs[p]; const uint64 pairId = concavePairs[p];
const uint64 pairIndex = mOverlappingPairs.mMapPairIdToPairIndex[pairId]; const uint64 pairIndex = mOverlappingPairs.mMapPairIdToPairIndex[pairId];
@ -407,7 +412,8 @@ void CollisionDetectionSystem::computeConvexVsConcaveMiddlePhase(uint64 pairInde
const bool reportContacts = !isCollider1Trigger && !isCollider2Trigger; const bool reportContacts = !isCollider1Trigger && !isCollider2Trigger;
// For each overlapping triangle // For each overlapping triangle
for (uint i=0; i < shapeIds.size(); i++) const uint nbShapeIds = shapeIds.size();
for (uint i=0; i < nbShapeIds; i++)
{ {
// Create a triangle collision shape (the allocated memory for the TriangleShape will be released in the // Create a triangle collision shape (the allocated memory for the TriangleShape will be released in the
// destructor of the corresponding NarrowPhaseInfo. // destructor of the corresponding NarrowPhaseInfo.
@ -716,7 +722,8 @@ void CollisionDetectionSystem::createContacts() {
mCurrentContactPoints->reserve(mCurrentContactManifolds->size()); mCurrentContactPoints->reserve(mCurrentContactManifolds->size());
// For each contact pair // For each contact pair
for (uint p=0; p < (*mCurrentContactPairs).size(); p++) { const uint nbCurrentContactPairs = (*mCurrentContactPairs).size();
for (uint p=0; p < nbCurrentContactPairs; p++) {
ContactPair& contactPair = (*mCurrentContactPairs)[p]; ContactPair& contactPair = (*mCurrentContactPairs)[p];
@ -725,10 +732,12 @@ void CollisionDetectionSystem::createContacts() {
contactPair.contactPointsIndex = mCurrentContactPoints->size(); contactPair.contactPointsIndex = mCurrentContactPoints->size();
// Add the associated contact pair to both bodies of the pair (used to create islands later) // Add the associated contact pair to both bodies of the pair (used to create islands later)
if (mRigidBodyComponents.hasComponent(contactPair.body1Entity)) { uint32 rigidBody1Index = 0;
uint32 rigidBody2Index = 0;
if (mRigidBodyComponents.hasComponentGetIndex(contactPair.body1Entity, rigidBody1Index)) {
mRigidBodyComponents.addContacPair(contactPair.body1Entity, p); mRigidBodyComponents.addContacPair(contactPair.body1Entity, p);
} }
if (mRigidBodyComponents.hasComponent(contactPair.body2Entity)) { if (mRigidBodyComponents.hasComponentGetIndex(contactPair.body2Entity, rigidBody2Index)) {
mRigidBodyComponents.addContacPair(contactPair.body2Entity, p); mRigidBodyComponents.addContacPair(contactPair.body2Entity, p);
} }
@ -742,12 +751,9 @@ void CollisionDetectionSystem::createContacts() {
const int8 nbContactPoints = static_cast<int8>(potentialManifold.nbPotentialContactPoints); const int8 nbContactPoints = static_cast<int8>(potentialManifold.nbPotentialContactPoints);
contactPair.nbToTalContactPoints += nbContactPoints; contactPair.nbToTalContactPoints += nbContactPoints;
// We create a new contact manifold // Create and add the contact manifold
ContactManifold contactManifold(contactPair.body1Entity, contactPair.body2Entity, contactPair.collider1Entity, mCurrentContactManifolds->emplace(contactPair.body1Entity, contactPair.body2Entity, contactPair.collider1Entity,
contactPair.collider2Entity, contactPointsIndex, nbContactPoints); contactPair.collider2Entity, contactPointsIndex, nbContactPoints, rigidBody1Index, rigidBody2Index);
// Add the contact manifold
mCurrentContactManifolds->add(contactManifold);
assert(potentialManifold.nbPotentialContactPoints > 0); assert(potentialManifold.nbPotentialContactPoints > 0);
@ -756,11 +762,8 @@ void CollisionDetectionSystem::createContacts() {
ContactPointInfo& potentialContactPoint = mPotentialContactPoints[potentialManifold.potentialContactPointsIndices[c]]; ContactPointInfo& potentialContactPoint = mPotentialContactPoints[potentialManifold.potentialContactPointsIndices[c]];
// Create a new contact point // Create and add the contact point
ContactPoint contactPoint(potentialContactPoint, mWorld->mConfig.persistentContactDistanceThreshold); mCurrentContactPoints->emplace(potentialContactPoint, mWorld->mConfig.persistentContactDistanceThreshold);
// Add the contact point
mCurrentContactPoints->add(contactPoint);
} }
} }
} }
@ -812,7 +815,8 @@ void CollisionDetectionSystem::createSnapshotContacts(List<ContactPair>& contact
contactPoints.reserve(contactManifolds.size()); contactPoints.reserve(contactManifolds.size());
// For each contact pair // For each contact pair
for (uint p=0; p < contactPairs.size(); p++) { const uint nbContactPairs = contactPairs.size();
for (uint p=0; p < nbContactPairs; p++) {
ContactPair& contactPair = contactPairs[p]; ContactPair& contactPair = contactPairs[p];
assert(contactPair.nbPotentialContactManifolds > 0); assert(contactPair.nbPotentialContactManifolds > 0);
@ -831,12 +835,9 @@ void CollisionDetectionSystem::createSnapshotContacts(List<ContactPair>& contact
const uint8 nbContactPoints = potentialManifold.nbPotentialContactPoints; const uint8 nbContactPoints = potentialManifold.nbPotentialContactPoints;
contactPair.nbToTalContactPoints += nbContactPoints; contactPair.nbToTalContactPoints += nbContactPoints;
// We create a new contact manifold // Create and add the contact manifold
ContactManifold contactManifold(contactPair.body1Entity, contactPair.body2Entity, contactPair.collider1Entity, contactManifolds.emplace(contactPair.body1Entity, contactPair.body2Entity, contactPair.collider1Entity,
contactPair.collider2Entity, contactPointsIndex, nbContactPoints); contactPair.collider2Entity, contactPointsIndex, nbContactPoints, 0, 0);
// Add the contact manifold
contactManifolds.add(contactManifold);
assert(potentialManifold.nbPotentialContactPoints > 0); assert(potentialManifold.nbPotentialContactPoints > 0);
@ -992,10 +993,12 @@ void CollisionDetectionSystem::processPotentialContacts(NarrowPhaseInfoBatch& na
RP3D_PROFILE("CollisionDetectionSystem::processPotentialContacts()", mProfiler); RP3D_PROFILE("CollisionDetectionSystem::processPotentialContacts()", mProfiler);
const uint nbObjects = narrowPhaseInfoBatch.getNbObjects();
if (updateLastFrameInfo) { if (updateLastFrameInfo) {
// For each narrow phase info object // For each narrow phase info object
for(uint i=0; i < narrowPhaseInfoBatch.getNbObjects(); i++) { for(uint i=0; i < nbObjects; i++) {
narrowPhaseInfoBatch.narrowPhaseInfos[i].lastFrameCollisionInfo->wasColliding = narrowPhaseInfoBatch.narrowPhaseInfos[i].isColliding; narrowPhaseInfoBatch.narrowPhaseInfos[i].lastFrameCollisionInfo->wasColliding = narrowPhaseInfoBatch.narrowPhaseInfos[i].isColliding;
@ -1005,7 +1008,7 @@ void CollisionDetectionSystem::processPotentialContacts(NarrowPhaseInfoBatch& na
} }
// For each narrow phase info object // For each narrow phase info object
for(uint i=0; i < narrowPhaseInfoBatch.getNbObjects(); i++) { for(uint i=0; i < nbObjects; i++) {
const uint64 pairId = narrowPhaseInfoBatch.narrowPhaseInfos[i].overlappingPairId; const uint64 pairId = narrowPhaseInfoBatch.narrowPhaseInfos[i].overlappingPairId;
const uint64 pairIndex = mOverlappingPairs.mMapPairIdToPairIndex[pairId]; const uint64 pairIndex = mOverlappingPairs.mMapPairIdToPairIndex[pairId];
@ -1175,7 +1178,8 @@ void CollisionDetectionSystem::reducePotentialContactManifolds(List<ContactPair>
RP3D_PROFILE("CollisionDetectionSystem::reducePotentialContactManifolds()", mProfiler); RP3D_PROFILE("CollisionDetectionSystem::reducePotentialContactManifolds()", mProfiler);
// Reduce the number of potential contact manifolds in a contact pair // Reduce the number of potential contact manifolds in a contact pair
for (uint i=0; i < contactPairs->size(); i++) { const uint nbContactPairs = contactPairs->size();
for (uint i=0; i < nbContactPairs; i++) {
ContactPair& contactPair = (*contactPairs)[i]; ContactPair& contactPair = (*contactPairs)[i];
@ -1205,7 +1209,7 @@ void CollisionDetectionSystem::reducePotentialContactManifolds(List<ContactPair>
} }
// Reduce the number of potential contact points in the manifolds // Reduce the number of potential contact points in the manifolds
for (uint i=0; i < contactPairs->size(); i++) { for (uint i=0; i < nbContactPairs; i++) {
const ContactPair& pairContact = (*contactPairs)[i]; const ContactPair& pairContact = (*contactPairs)[i];

248
src/systems/ContactSolverSystem.cpp
View File

@ -90,7 +90,8 @@ void ContactSolverSystem::init(List<ContactManifold>* contactManifolds, List<Con
assert(mContactConstraints != nullptr); assert(mContactConstraints != nullptr);
// For each island of the world // For each island of the world
for (uint i = 0; i < mIslands.getNbIslands(); i++) { const uint nbIslands = mIslands.getNbIslands();
for (uint i = 0; i < nbIslands; i++) {
if (mIslands.nbContactManifolds[i] > 0) { if (mIslands.nbContactManifolds[i] > 0) {
initializeForIsland(i); initializeForIsland(i);
@ -117,25 +118,24 @@ void ContactSolverSystem::initializeForIsland(uint islandIndex) {
assert(mIslands.nbContactManifolds[islandIndex] > 0); assert(mIslands.nbContactManifolds[islandIndex] > 0);
// For each contact manifold of the island // For each contact manifold of the island
uint contactManifoldsIndex = mIslands.contactManifoldsIndices[islandIndex]; const uint contactManifoldsIndex = mIslands.contactManifoldsIndices[islandIndex];
uint nbContactManifolds = mIslands.nbContactManifolds[islandIndex]; const uint nbContactManifolds = mIslands.nbContactManifolds[islandIndex];
for (uint m=contactManifoldsIndex; m < contactManifoldsIndex + nbContactManifolds; m++) { for (uint m=contactManifoldsIndex; m < contactManifoldsIndex + nbContactManifolds; m++) {
ContactManifold& externalManifold = (*mAllContactManifolds)[m]; ContactManifold& externalManifold = (*mAllContactManifolds)[m];
assert(externalManifold.nbContactPoints > 0); assert(externalManifold.nbContactPoints > 0);
const uint rigidBodyIndex1 = mRigidBodyComponents.getEntityIndex(externalManifold.bodyEntity1); const uint rigidBodyIndex1 = externalManifold.rigidBody1Index;
const uint rigidBodyIndex2 = mRigidBodyComponents.getEntityIndex(externalManifold.bodyEntity2); const uint rigidBodyIndex2 = externalManifold.rigidBody2Index;
// Get the two bodies of the contact // Get the two bodies of the contact
RigidBody* body1 = mRigidBodyComponents.mRigidBodies[rigidBodyIndex1];
RigidBody* body2 = mRigidBodyComponents.mRigidBodies[rigidBodyIndex2];
assert(body1 != nullptr); assert(body1 != nullptr);
assert(body2 != nullptr); assert(body2 != nullptr);
assert(!mBodyComponents.getIsEntityDisabled(externalManifold.bodyEntity1)); assert(!mBodyComponents.getIsEntityDisabled(externalManifold.bodyEntity1));
assert(!mBodyComponents.getIsEntityDisabled(externalManifold.bodyEntity2)); assert(!mBodyComponents.getIsEntityDisabled(externalManifold.bodyEntity2));
// TODO OPTI : Do not use pointers to colliders here, maybe we call totally remove those pointers
Collider* collider1 = mColliderComponents.getCollider(externalManifold.colliderEntity1); Collider* collider1 = mColliderComponents.getCollider(externalManifold.colliderEntity1);
Collider* collider2 = mColliderComponents.getCollider(externalManifold.colliderEntity2); Collider* collider2 = mColliderComponents.getCollider(externalManifold.colliderEntity2);
@ -152,7 +152,9 @@ void ContactSolverSystem::initializeForIsland(uint islandIndex) {
mContactConstraints[mNbContactManifolds].massInverseBody1 = mRigidBodyComponents.mInverseMasses[rigidBodyIndex1]; mContactConstraints[mNbContactManifolds].massInverseBody1 = mRigidBodyComponents.mInverseMasses[rigidBodyIndex1];
mContactConstraints[mNbContactManifolds].massInverseBody2 = mRigidBodyComponents.mInverseMasses[rigidBodyIndex2]; mContactConstraints[mNbContactManifolds].massInverseBody2 = mRigidBodyComponents.mInverseMasses[rigidBodyIndex2];
mContactConstraints[mNbContactManifolds].nbContacts = externalManifold.nbContactPoints; mContactConstraints[mNbContactManifolds].nbContacts = externalManifold.nbContactPoints;
// TODO OPTI : Do not compute this using colliders pointers
mContactConstraints[mNbContactManifolds].frictionCoefficient = computeMixedFrictionCoefficient(collider1, collider2); mContactConstraints[mNbContactManifolds].frictionCoefficient = computeMixedFrictionCoefficient(collider1, collider2);
// TODO OPTI : Do not compute this using colliders pointers
mContactConstraints[mNbContactManifolds].rollingResistanceFactor = computeMixedRollingResistance(collider1, collider2); mContactConstraints[mNbContactManifolds].rollingResistanceFactor = computeMixedRollingResistance(collider1, collider2);
mContactConstraints[mNbContactManifolds].externalContactManifold = &externalManifold; mContactConstraints[mNbContactManifolds].externalContactManifold = &externalManifold;
mContactConstraints[mNbContactManifolds].normal.setToZero(); mContactConstraints[mNbContactManifolds].normal.setToZero();
@ -165,17 +167,21 @@ void ContactSolverSystem::initializeForIsland(uint islandIndex) {
const Vector3& v2 = mRigidBodyComponents.mLinearVelocities[rigidBodyIndex2]; const Vector3& v2 = mRigidBodyComponents.mLinearVelocities[rigidBodyIndex2];
const Vector3& w2 = mRigidBodyComponents.mAngularVelocities[rigidBodyIndex2]; const Vector3& w2 = mRigidBodyComponents.mAngularVelocities[rigidBodyIndex2];
// TODO OPTI : Maybe use collider index here
const Transform& collider1LocalToWorldTransform = mColliderComponents.getLocalToWorldTransform(externalManifold.colliderEntity1);
const Transform& collider2LocalToWorldTransform = mColliderComponents.getLocalToWorldTransform(externalManifold.colliderEntity2);
// For each contact point of the contact manifold // For each contact point of the contact manifold
assert(externalManifold.nbContactPoints > 0); assert(externalManifold.nbContactPoints > 0);
uint contactPointsStartIndex = externalManifold.contactPointsIndex; const uint contactPointsStartIndex = externalManifold.contactPointsIndex;
uint nbContactPoints = static_cast<uint>(externalManifold.nbContactPoints); const uint nbContactPoints = static_cast<uint>(externalManifold.nbContactPoints);
for (uint c=contactPointsStartIndex; c < contactPointsStartIndex + nbContactPoints; c++) { for (uint c=contactPointsStartIndex; c < contactPointsStartIndex + nbContactPoints; c++) {
ContactPoint& externalContact = (*mAllContactPoints)[c]; ContactPoint& externalContact = (*mAllContactPoints)[c];
// Get the contact point on the two bodies // Get the contact point on the two bodies
Vector3 p1 = mColliderComponents.getLocalToWorldTransform(externalManifold.colliderEntity1) * externalContact.getLocalPointOnShape1(); const Vector3 p1 = collider1LocalToWorldTransform * externalContact.getLocalPointOnShape1();
Vector3 p2 = mColliderComponents.getLocalToWorldTransform(externalManifold.colliderEntity2) * externalContact.getLocalPointOnShape2(); const Vector3 p2 = collider2LocalToWorldTransform * externalContact.getLocalPointOnShape2();
new (mContactPoints + mNbContactPoints) ContactPointSolver(); new (mContactPoints + mNbContactPoints) ContactPointSolver();
mContactPoints[mNbContactPoints].externalContact = &externalContact; mContactPoints[mNbContactPoints].externalContact = &externalContact;
@ -270,20 +276,20 @@ void ContactSolverSystem::initializeForIsland(uint islandIndex) {
mContactConstraints[mNbContactManifolds].frictionTwistImpulse = externalManifold.frictionTwistImpulse; mContactConstraints[mNbContactManifolds].frictionTwistImpulse = externalManifold.frictionTwistImpulse;
// Compute the inverse K matrix for the rolling resistance constraint // Compute the inverse K matrix for the rolling resistance constraint
bool isBody1DynamicType = body1->getType() == BodyType::DYNAMIC; const bool isBody1DynamicType = mRigidBodyComponents.mBodyTypes[rigidBodyIndex1] == BodyType::DYNAMIC;
bool isBody2DynamicType = body2->getType() == BodyType::DYNAMIC; const bool isBody2DynamicType = mRigidBodyComponents.mBodyTypes[rigidBodyIndex2] == BodyType::DYNAMIC;
mContactConstraints[mNbContactManifolds].inverseRollingResistance.setToZero(); mContactConstraints[mNbContactManifolds].inverseRollingResistance.setToZero();
if (mContactConstraints[mNbContactManifolds].rollingResistanceFactor > 0 && (isBody1DynamicType || isBody2DynamicType)) { if (mContactConstraints[mNbContactManifolds].rollingResistanceFactor > 0 && (isBody1DynamicType || isBody2DynamicType)) {
mContactConstraints[mNbContactManifolds].inverseRollingResistance = mContactConstraints[mNbContactManifolds].inverseInertiaTensorBody1 + mContactConstraints[mNbContactManifolds].inverseInertiaTensorBody2; mContactConstraints[mNbContactManifolds].inverseRollingResistance = mContactConstraints[mNbContactManifolds].inverseInertiaTensorBody1 + mContactConstraints[mNbContactManifolds].inverseInertiaTensorBody2;
decimal det = mContactConstraints[mNbContactManifolds].inverseRollingResistance.getDeterminant(); const decimal det = mContactConstraints[mNbContactManifolds].inverseRollingResistance.getDeterminant();
// If the matrix is not inversible // If the matrix is not inversible
if (approxEqual(det, decimal(0.0))) { if (approxEqual(det, decimal(0.0))) {
mContactConstraints[mNbContactManifolds].inverseRollingResistance.setToZero(); mContactConstraints[mNbContactManifolds].inverseRollingResistance.setToZero();
} }
else { else {
mContactConstraints[mNbContactManifolds].inverseRollingResistance = mContactConstraints[mNbContactManifolds].inverseRollingResistance.getInverse(); mContactConstraints[mNbContactManifolds].inverseRollingResistance = mContactConstraints[mNbContactManifolds].inverseRollingResistance.getInverseWithDeterminant(det);
} }
} }
@ -352,10 +358,12 @@ void ContactSolverSystem::warmStart() {
for (short int i=0; i<mContactConstraints[c].nbContacts; i++) { for (short int i=0; i<mContactConstraints[c].nbContacts; i++) {
// If it is not a new contact (this contact was already existing at last time step) // If it is not a new contact (this contact was already existing at last time step)
if (mContactPoints[contactPointIndex].isRestingContact) { if (mContactPoints[contactPointIndex].isRestingContact) {
const uint32 rigidBody1Index = mContactConstraints[c].rigidBodyComponentIndexBody1;
const uint32 rigidBody2Index = mContactConstraints[c].rigidBodyComponentIndexBody2;
atLeastOneRestingContactPoint = true; atLeastOneRestingContactPoint = true;
// --------- Penetration --------- // // --------- Penetration --------- //
@ -364,22 +372,22 @@ void ContactSolverSystem::warmStart() {
Vector3 impulsePenetration(mContactPoints[contactPointIndex].normal.x * mContactPoints[contactPointIndex].penetrationImpulse, Vector3 impulsePenetration(mContactPoints[contactPointIndex].normal.x * mContactPoints[contactPointIndex].penetrationImpulse,
mContactPoints[contactPointIndex].normal.y * mContactPoints[contactPointIndex].penetrationImpulse, mContactPoints[contactPointIndex].normal.y * mContactPoints[contactPointIndex].penetrationImpulse,
mContactPoints[contactPointIndex].normal.z * mContactPoints[contactPointIndex].penetrationImpulse); mContactPoints[contactPointIndex].normal.z * mContactPoints[contactPointIndex].penetrationImpulse);
mRigidBodyComponents.mConstrainedLinearVelocities[mContactConstraints[c].rigidBodyComponentIndexBody1].x -= mContactConstraints[c].massInverseBody1 * impulsePenetration.x; mRigidBodyComponents.mConstrainedLinearVelocities[rigidBody1Index].x -= mContactConstraints[c].massInverseBody1 * impulsePenetration.x;
mRigidBodyComponents.mConstrainedLinearVelocities[mContactConstraints[c].rigidBodyComponentIndexBody1].y -= mContactConstraints[c].massInverseBody1 * impulsePenetration.y; mRigidBodyComponents.mConstrainedLinearVelocities[rigidBody1Index].y -= mContactConstraints[c].massInverseBody1 * impulsePenetration.y;
mRigidBodyComponents.mConstrainedLinearVelocities[mContactConstraints[c].rigidBodyComponentIndexBody1].z -= mContactConstraints[c].massInverseBody1 * impulsePenetration.z; mRigidBodyComponents.mConstrainedLinearVelocities[rigidBody1Index].z -= mContactConstraints[c].massInverseBody1 * impulsePenetration.z;
mRigidBodyComponents.mConstrainedAngularVelocities[mContactConstraints[c].rigidBodyComponentIndexBody1].x -= mContactPoints[contactPointIndex].i1TimesR1CrossN.x * mContactPoints[contactPointIndex].penetrationImpulse; mRigidBodyComponents.mConstrainedAngularVelocities[rigidBody1Index].x -= mContactPoints[contactPointIndex].i1TimesR1CrossN.x * mContactPoints[contactPointIndex].penetrationImpulse;
mRigidBodyComponents.mConstrainedAngularVelocities[mContactConstraints[c].rigidBodyComponentIndexBody1].y -= mContactPoints[contactPointIndex].i1TimesR1CrossN.y * mContactPoints[contactPointIndex].penetrationImpulse; mRigidBodyComponents.mConstrainedAngularVelocities[rigidBody1Index].y -= mContactPoints[contactPointIndex].i1TimesR1CrossN.y * mContactPoints[contactPointIndex].penetrationImpulse;
mRigidBodyComponents.mConstrainedAngularVelocities[mContactConstraints[c].rigidBodyComponentIndexBody1].z -= mContactPoints[contactPointIndex].i1TimesR1CrossN.z * mContactPoints[contactPointIndex].penetrationImpulse; mRigidBodyComponents.mConstrainedAngularVelocities[rigidBody1Index].z -= mContactPoints[contactPointIndex].i1TimesR1CrossN.z * mContactPoints[contactPointIndex].penetrationImpulse;
// Update the velocities of the body 2 by applying the impulse P // Update the velocities of the body 2 by applying the impulse P
mRigidBodyComponents.mConstrainedLinearVelocities[mContactConstraints[c].rigidBodyComponentIndexBody2].x += mContactConstraints[c].massInverseBody2 * impulsePenetration.x; mRigidBodyComponents.mConstrainedLinearVelocities[rigidBody2Index].x += mContactConstraints[c].massInverseBody2 * impulsePenetration.x;
mRigidBodyComponents.mConstrainedLinearVelocities[mContactConstraints[c].rigidBodyComponentIndexBody2].y += mContactConstraints[c].massInverseBody2 * impulsePenetration.y; mRigidBodyComponents.mConstrainedLinearVelocities[rigidBody2Index].y += mContactConstraints[c].massInverseBody2 * impulsePenetration.y;
mRigidBodyComponents.mConstrainedLinearVelocities[mContactConstraints[c].rigidBodyComponentIndexBody2].z += mContactConstraints[c].massInverseBody2 * impulsePenetration.z; mRigidBodyComponents.mConstrainedLinearVelocities[rigidBody2Index].z += mContactConstraints[c].massInverseBody2 * impulsePenetration.z;
mRigidBodyComponents.mConstrainedAngularVelocities[mContactConstraints[c].rigidBodyComponentIndexBody2].x += mContactPoints[contactPointIndex].i2TimesR2CrossN.x * mContactPoints[contactPointIndex].penetrationImpulse; mRigidBodyComponents.mConstrainedAngularVelocities[rigidBody2Index].x += mContactPoints[contactPointIndex].i2TimesR2CrossN.x * mContactPoints[contactPointIndex].penetrationImpulse;
mRigidBodyComponents.mConstrainedAngularVelocities[mContactConstraints[c].rigidBodyComponentIndexBody2].y += mContactPoints[contactPointIndex].i2TimesR2CrossN.y * mContactPoints[contactPointIndex].penetrationImpulse; mRigidBodyComponents.mConstrainedAngularVelocities[rigidBody2Index].y += mContactPoints[contactPointIndex].i2TimesR2CrossN.y * mContactPoints[contactPointIndex].penetrationImpulse;
mRigidBodyComponents.mConstrainedAngularVelocities[mContactConstraints[c].rigidBodyComponentIndexBody2].z += mContactPoints[contactPointIndex].i2TimesR2CrossN.z * mContactPoints[contactPointIndex].penetrationImpulse; mRigidBodyComponents.mConstrainedAngularVelocities[rigidBody2Index].z += mContactPoints[contactPointIndex].i2TimesR2CrossN.z * mContactPoints[contactPointIndex].penetrationImpulse;
} }
else { // If it is a new contact point else { // If it is a new contact point
@ -418,13 +426,16 @@ void ContactSolverSystem::warmStart() {
mContactConstraints[c].r2CrossT1.y * mContactConstraints[c].friction1Impulse, mContactConstraints[c].r2CrossT1.y * mContactConstraints[c].friction1Impulse,
mContactConstraints[c].r2CrossT1.z * mContactConstraints[c].friction1Impulse); mContactConstraints[c].r2CrossT1.z * mContactConstraints[c].friction1Impulse);
// Update the velocities of the body 1 by applying the impulse P const uint32 rigidBody1Index = mContactConstraints[c].rigidBodyComponentIndexBody1;
mRigidBodyComponents.mConstrainedLinearVelocities[mContactConstraints[c].rigidBodyComponentIndexBody1] -= mContactConstraints[c].massInverseBody1 * linearImpulseBody2; const uint32 rigidBody2Index = mContactConstraints[c].rigidBodyComponentIndexBody2;
mRigidBodyComponents.mConstrainedAngularVelocities[mContactConstraints[c].rigidBodyComponentIndexBody1] += mContactConstraints[c].inverseInertiaTensorBody1 * angularImpulseBody1;
// Update the velocities of the body 1 by applying the impulse P // Update the velocities of the body 1 by applying the impulse P
mRigidBodyComponents.mConstrainedLinearVelocities[mContactConstraints[c].rigidBodyComponentIndexBody2] += mContactConstraints[c].massInverseBody2 * linearImpulseBody2; mRigidBodyComponents.mConstrainedLinearVelocities[rigidBody1Index] -= mContactConstraints[c].massInverseBody1 * linearImpulseBody2;
mRigidBodyComponents.mConstrainedAngularVelocities[mContactConstraints[c].rigidBodyComponentIndexBody2] += mContactConstraints[c].inverseInertiaTensorBody2 * angularImpulseBody2; mRigidBodyComponents.mConstrainedAngularVelocities[rigidBody1Index] += mContactConstraints[c].inverseInertiaTensorBody1 * angularImpulseBody1;
// Update the velocities of the body 1 by applying the impulse P
mRigidBodyComponents.mConstrainedLinearVelocities[rigidBody2Index] += mContactConstraints[c].massInverseBody2 * linearImpulseBody2;
mRigidBodyComponents.mConstrainedAngularVelocities[rigidBody2Index] += mContactConstraints[c].inverseInertiaTensorBody2 * angularImpulseBody2;
// ------ Second friction constraint at the center of the contact manifold ----- // // ------ Second friction constraint at the center of the contact manifold ----- //
@ -440,18 +451,18 @@ void ContactSolverSystem::warmStart() {
angularImpulseBody2.z = mContactConstraints[c].r2CrossT2.z * mContactConstraints[c].friction2Impulse; angularImpulseBody2.z = mContactConstraints[c].r2CrossT2.z * mContactConstraints[c].friction2Impulse;
// Update the velocities of the body 1 by applying the impulse P // Update the velocities of the body 1 by applying the impulse P
mRigidBodyComponents.mConstrainedLinearVelocities[mContactConstraints[c].rigidBodyComponentIndexBody1].x -= mContactConstraints[c].massInverseBody1 * linearImpulseBody2.x; mRigidBodyComponents.mConstrainedLinearVelocities[rigidBody1Index].x -= mContactConstraints[c].massInverseBody1 * linearImpulseBody2.x;
mRigidBodyComponents.mConstrainedLinearVelocities[mContactConstraints[c].rigidBodyComponentIndexBody1].y -= mContactConstraints[c].massInverseBody1 * linearImpulseBody2.y; mRigidBodyComponents.mConstrainedLinearVelocities[rigidBody1Index].y -= mContactConstraints[c].massInverseBody1 * linearImpulseBody2.y;
mRigidBodyComponents.mConstrainedLinearVelocities[mContactConstraints[c].rigidBodyComponentIndexBody1].z -= mContactConstraints[c].massInverseBody1 * linearImpulseBody2.z; mRigidBodyComponents.mConstrainedLinearVelocities[rigidBody1Index].z -= mContactConstraints[c].massInverseBody1 * linearImpulseBody2.z;
mRigidBodyComponents.mConstrainedAngularVelocities[mContactConstraints[c].rigidBodyComponentIndexBody1] += mContactConstraints[c].inverseInertiaTensorBody1 * angularImpulseBody1; mRigidBodyComponents.mConstrainedAngularVelocities[rigidBody1Index] += mContactConstraints[c].inverseInertiaTensorBody1 * angularImpulseBody1;
// Update the velocities of the body 2 by applying the impulse P // Update the velocities of the body 2 by applying the impulse P
mRigidBodyComponents.mConstrainedLinearVelocities[mContactConstraints[c].rigidBodyComponentIndexBody2].x += mContactConstraints[c].massInverseBody2 * linearImpulseBody2.x; mRigidBodyComponents.mConstrainedLinearVelocities[rigidBody2Index].x += mContactConstraints[c].massInverseBody2 * linearImpulseBody2.x;
mRigidBodyComponents.mConstrainedLinearVelocities[mContactConstraints[c].rigidBodyComponentIndexBody2].y += mContactConstraints[c].massInverseBody2 * linearImpulseBody2.y; mRigidBodyComponents.mConstrainedLinearVelocities[rigidBody2Index].y += mContactConstraints[c].massInverseBody2 * linearImpulseBody2.y;
mRigidBodyComponents.mConstrainedLinearVelocities[mContactConstraints[c].rigidBodyComponentIndexBody2].z += mContactConstraints[c].massInverseBody2 * linearImpulseBody2.z; mRigidBodyComponents.mConstrainedLinearVelocities[rigidBody2Index].z += mContactConstraints[c].massInverseBody2 * linearImpulseBody2.z;
mRigidBodyComponents.mConstrainedAngularVelocities[mContactConstraints[c].rigidBodyComponentIndexBody2] += mContactConstraints[c].inverseInertiaTensorBody2 * angularImpulseBody2; mRigidBodyComponents.mConstrainedAngularVelocities[rigidBody2Index] += mContactConstraints[c].inverseInertiaTensorBody2 * angularImpulseBody2;
// ------ Twist friction constraint at the center of the contact manifold ------ // // ------ Twist friction constraint at the center of the contact manifold ------ //
@ -465,10 +476,10 @@ void ContactSolverSystem::warmStart() {
angularImpulseBody2.z = mContactConstraints[c].normal.z * mContactConstraints[c].frictionTwistImpulse; angularImpulseBody2.z = mContactConstraints[c].normal.z * mContactConstraints[c].frictionTwistImpulse;
// Update the velocities of the body 1 by applying the impulse P // Update the velocities of the body 1 by applying the impulse P
mRigidBodyComponents.mConstrainedAngularVelocities[mContactConstraints[c].rigidBodyComponentIndexBody1] += mContactConstraints[c].inverseInertiaTensorBody1 * angularImpulseBody1; mRigidBodyComponents.mConstrainedAngularVelocities[rigidBody1Index] += mContactConstraints[c].inverseInertiaTensorBody1 * angularImpulseBody1;
// Update the velocities of the body 2 by applying the impulse P // Update the velocities of the body 2 by applying the impulse P
mRigidBodyComponents.mConstrainedAngularVelocities[mContactConstraints[c].rigidBodyComponentIndexBody2] += mContactConstraints[c].inverseInertiaTensorBody2 * angularImpulseBody2; mRigidBodyComponents.mConstrainedAngularVelocities[rigidBody2Index] += mContactConstraints[c].inverseInertiaTensorBody2 * angularImpulseBody2;
// ------ Rolling resistance at the center of the contact manifold ------ // // ------ Rolling resistance at the center of the contact manifold ------ //
@ -476,10 +487,10 @@ void ContactSolverSystem::warmStart() {
angularImpulseBody2 = mContactConstraints[c].rollingResistanceImpulse; angularImpulseBody2 = mContactConstraints[c].rollingResistanceImpulse;
// Update the velocities of the body 1 by applying the impulse P // Update the velocities of the body 1 by applying the impulse P
mRigidBodyComponents.mConstrainedAngularVelocities[mContactConstraints[c].rigidBodyComponentIndexBody1] -= mContactConstraints[c].inverseInertiaTensorBody1 * angularImpulseBody2; mRigidBodyComponents.mConstrainedAngularVelocities[rigidBody1Index] -= mContactConstraints[c].inverseInertiaTensorBody1 * angularImpulseBody2;
// Update the velocities of the body 1 by applying the impulse P // Update the velocities of the body 1 by applying the impulse P
mRigidBodyComponents.mConstrainedAngularVelocities[mContactConstraints[c].rigidBodyComponentIndexBody2] += mContactConstraints[c].inverseInertiaTensorBody2 * angularImpulseBody2; mRigidBodyComponents.mConstrainedAngularVelocities[rigidBody2Index] += mContactConstraints[c].inverseInertiaTensorBody2 * angularImpulseBody2;
} }
else { // If it is a new contact manifold else { // If it is a new contact manifold
@ -508,11 +519,14 @@ void ContactSolverSystem::solve() {
decimal sumPenetrationImpulse = 0.0; decimal sumPenetrationImpulse = 0.0;
const uint32 rigidBody1Index = mContactConstraints[c].rigidBodyComponentIndexBody1;
const uint32 rigidBody2Index = mContactConstraints[c].rigidBodyComponentIndexBody2;
// Get the constrained velocities // Get the constrained velocities
const Vector3& v1 = mRigidBodyComponents.mConstrainedLinearVelocities[mContactConstraints[c].rigidBodyComponentIndexBody1]; const Vector3& v1 = mRigidBodyComponents.mConstrainedLinearVelocities[rigidBody1Index];
const Vector3& w1 = mRigidBodyComponents.mConstrainedAngularVelocities[mContactConstraints[c].rigidBodyComponentIndexBody1]; const Vector3& w1 = mRigidBodyComponents.mConstrainedAngularVelocities[rigidBody1Index];
const Vector3& v2 = mRigidBodyComponents.mConstrainedLinearVelocities[mContactConstraints[c].rigidBodyComponentIndexBody2]; const Vector3& v2 = mRigidBodyComponents.mConstrainedLinearVelocities[rigidBody2Index];
const Vector3& w2 = mRigidBodyComponents.mConstrainedAngularVelocities[mContactConstraints[c].rigidBodyComponentIndexBody2]; const Vector3& w2 = mRigidBodyComponents.mConstrainedAngularVelocities[rigidBody2Index];
for (short int i=0; i<mContactConstraints[c].nbContacts; i++) { for (short int i=0; i<mContactConstraints[c].nbContacts; i++) {
@ -554,22 +568,22 @@ void ContactSolverSystem::solve() {
mContactPoints[contactPointIndex].normal.z * deltaLambda); mContactPoints[contactPointIndex].normal.z * deltaLambda);
// Update the velocities of the body 1 by applying the impulse P // Update the velocities of the body 1 by applying the impulse P
mRigidBodyComponents.mConstrainedLinearVelocities[mContactConstraints[c].rigidBodyComponentIndexBody1].x -= mContactConstraints[c].massInverseBody1 * linearImpulse.x; mRigidBodyComponents.mConstrainedLinearVelocities[rigidBody1Index].x -= mContactConstraints[c].massInverseBody1 * linearImpulse.x;
mRigidBodyComponents.mConstrainedLinearVelocities[mContactConstraints[c].rigidBodyComponentIndexBody1].y -= mContactConstraints[c].massInverseBody1 * linearImpulse.y; mRigidBodyComponents.mConstrainedLinearVelocities[rigidBody1Index].y -= mContactConstraints[c].massInverseBody1 * linearImpulse.y;
mRigidBodyComponents.mConstrainedLinearVelocities[mContactConstraints[c].rigidBodyComponentIndexBody1].z -= mContactConstraints[c].massInverseBody1 * linearImpulse.z; mRigidBodyComponents.mConstrainedLinearVelocities[rigidBody1Index].z -= mContactConstraints[c].massInverseBody1 * linearImpulse.z;
mRigidBodyComponents.mConstrainedAngularVelocities[mContactConstraints[c].rigidBodyComponentIndexBody1].x -= mContactPoints[contactPointIndex].i1TimesR1CrossN.x * deltaLambda; mRigidBodyComponents.mConstrainedAngularVelocities[rigidBody1Index].x -= mContactPoints[contactPointIndex].i1TimesR1CrossN.x * deltaLambda;
mRigidBodyComponents.mConstrainedAngularVelocities[mContactConstraints[c].rigidBodyComponentIndexBody1].y -= mContactPoints[contactPointIndex].i1TimesR1CrossN.y * deltaLambda; mRigidBodyComponents.mConstrainedAngularVelocities[rigidBody1Index].y -= mContactPoints[contactPointIndex].i1TimesR1CrossN.y * deltaLambda;
mRigidBodyComponents.mConstrainedAngularVelocities[mContactConstraints[c].rigidBodyComponentIndexBody1].z -= mContactPoints[contactPointIndex].i1TimesR1CrossN.z * deltaLambda; mRigidBodyComponents.mConstrainedAngularVelocities[rigidBody1Index].z -= mContactPoints[contactPointIndex].i1TimesR1CrossN.z * deltaLambda;
// Update the velocities of the body 2 by applying the impulse P // Update the velocities of the body 2 by applying the impulse P
mRigidBodyComponents.mConstrainedLinearVelocities[mContactConstraints[c].rigidBodyComponentIndexBody2].x += mContactConstraints[c].massInverseBody2 * linearImpulse.x; mRigidBodyComponents.mConstrainedLinearVelocities[rigidBody2Index].x += mContactConstraints[c].massInverseBody2 * linearImpulse.x;
mRigidBodyComponents.mConstrainedLinearVelocities[mContactConstraints[c].rigidBodyComponentIndexBody2].y += mContactConstraints[c].massInverseBody2 * linearImpulse.y; mRigidBodyComponents.mConstrainedLinearVelocities[rigidBody2Index].y += mContactConstraints[c].massInverseBody2 * linearImpulse.y;
mRigidBodyComponents.mConstrainedLinearVelocities[mContactConstraints[c].rigidBodyComponentIndexBody2].z += mContactConstraints[c].massInverseBody2 * linearImpulse.z; mRigidBodyComponents.mConstrainedLinearVelocities[rigidBody2Index].z += mContactConstraints[c].massInverseBody2 * linearImpulse.z;
mRigidBodyComponents.mConstrainedAngularVelocities[mContactConstraints[c].rigidBodyComponentIndexBody2].x += mContactPoints[contactPointIndex].i2TimesR2CrossN.x * deltaLambda; mRigidBodyComponents.mConstrainedAngularVelocities[rigidBody2Index].x += mContactPoints[contactPointIndex].i2TimesR2CrossN.x * deltaLambda;
mRigidBodyComponents.mConstrainedAngularVelocities[mContactConstraints[c].rigidBodyComponentIndexBody2].y += mContactPoints[contactPointIndex].i2TimesR2CrossN.y * deltaLambda; mRigidBodyComponents.mConstrainedAngularVelocities[rigidBody2Index].y += mContactPoints[contactPointIndex].i2TimesR2CrossN.y * deltaLambda;
mRigidBodyComponents.mConstrainedAngularVelocities[mContactConstraints[c].rigidBodyComponentIndexBody2].z += mContactPoints[contactPointIndex].i2TimesR2CrossN.z * deltaLambda; mRigidBodyComponents.mConstrainedAngularVelocities[rigidBody2Index].z += mContactPoints[contactPointIndex].i2TimesR2CrossN.z * deltaLambda;
sumPenetrationImpulse += mContactPoints[contactPointIndex].penetrationImpulse; sumPenetrationImpulse += mContactPoints[contactPointIndex].penetrationImpulse;
@ -577,10 +591,10 @@ void ContactSolverSystem::solve() {
if (mIsSplitImpulseActive) { if (mIsSplitImpulseActive) {
// Split impulse (position correction) // Split impulse (position correction)
const Vector3& v1Split = mRigidBodyComponents.mSplitLinearVelocities[mContactConstraints[c].rigidBodyComponentIndexBody1]; const Vector3& v1Split = mRigidBodyComponents.mSplitLinearVelocities[rigidBody1Index];
const Vector3& w1Split = mRigidBodyComponents.mSplitAngularVelocities[mContactConstraints[c].rigidBodyComponentIndexBody1]; const Vector3& w1Split = mRigidBodyComponents.mSplitAngularVelocities[rigidBody1Index];
const Vector3& v2Split = mRigidBodyComponents.mSplitLinearVelocities[mContactConstraints[c].rigidBodyComponentIndexBody2]; const Vector3& v2Split = mRigidBodyComponents.mSplitLinearVelocities[rigidBody2Index];
const Vector3& w2Split = mRigidBodyComponents.mSplitAngularVelocities[mContactConstraints[c].rigidBodyComponentIndexBody2]; const Vector3& w2Split = mRigidBodyComponents.mSplitAngularVelocities[rigidBody2Index];
//Vector3 deltaVSplit = v2Split + w2Split.cross(mContactPoints[contactPointIndex].r2) - v1Split - w1Split.cross(mContactPoints[contactPointIndex].r1); //Vector3 deltaVSplit = v2Split + w2Split.cross(mContactPoints[contactPointIndex].r2) - v1Split - w1Split.cross(mContactPoints[contactPointIndex].r1);
Vector3 deltaVSplit(v2Split.x + w2Split.y * mContactPoints[contactPointIndex].r2.z - w2Split.z * mContactPoints[contactPointIndex].r2.y - v1Split.x - Vector3 deltaVSplit(v2Split.x + w2Split.y * mContactPoints[contactPointIndex].r2.z - w2Split.z * mContactPoints[contactPointIndex].r2.y - v1Split.x -
@ -605,22 +619,22 @@ void ContactSolverSystem::solve() {
mContactPoints[contactPointIndex].normal.z * deltaLambdaSplit); mContactPoints[contactPointIndex].normal.z * deltaLambdaSplit);
// Update the velocities of the body 1 by applying the impulse P // Update the velocities of the body 1 by applying the impulse P
mRigidBodyComponents.mSplitLinearVelocities[mContactConstraints[c].rigidBodyComponentIndexBody1].x -= mContactConstraints[c].massInverseBody1 * linearImpulse.x; mRigidBodyComponents.mSplitLinearVelocities[rigidBody1Index].x -= mContactConstraints[c].massInverseBody1 * linearImpulse.x;
mRigidBodyComponents.mSplitLinearVelocities[mContactConstraints[c].rigidBodyComponentIndexBody1].y -= mContactConstraints[c].massInverseBody1 * linearImpulse.y; mRigidBodyComponents.mSplitLinearVelocities[rigidBody1Index].y -= mContactConstraints[c].massInverseBody1 * linearImpulse.y;
mRigidBodyComponents.mSplitLinearVelocities[mContactConstraints[c].rigidBodyComponentIndexBody1].z -= mContactConstraints[c].massInverseBody1 * linearImpulse.z; mRigidBodyComponents.mSplitLinearVelocities[rigidBody1Index].z -= mContactConstraints[c].massInverseBody1 * linearImpulse.z;
mRigidBodyComponents.mSplitAngularVelocities[mContactConstraints[c].rigidBodyComponentIndexBody1].x -= mContactPoints[contactPointIndex].i1TimesR1CrossN.x * deltaLambdaSplit; mRigidBodyComponents.mSplitAngularVelocities[rigidBody1Index].x -= mContactPoints[contactPointIndex].i1TimesR1CrossN.x * deltaLambdaSplit;
mRigidBodyComponents.mSplitAngularVelocities[mContactConstraints[c].rigidBodyComponentIndexBody1].y -= mContactPoints[contactPointIndex].i1TimesR1CrossN.y * deltaLambdaSplit; mRigidBodyComponents.mSplitAngularVelocities[rigidBody1Index].y -= mContactPoints[contactPointIndex].i1TimesR1CrossN.y * deltaLambdaSplit;
mRigidBodyComponents.mSplitAngularVelocities[mContactConstraints[c].rigidBodyComponentIndexBody1].z -= mContactPoints[contactPointIndex].i1TimesR1CrossN.z * deltaLambdaSplit; mRigidBodyComponents.mSplitAngularVelocities[rigidBody1Index].z -= mContactPoints[contactPointIndex].i1TimesR1CrossN.z * deltaLambdaSplit;
// Update the velocities of the body 1 by applying the impulse P // Update the velocities of the body 1 by applying the impulse P
mRigidBodyComponents.mSplitLinearVelocities[mContactConstraints[c].rigidBodyComponentIndexBody2].x += mContactConstraints[c].massInverseBody2 * linearImpulse.x; mRigidBodyComponents.mSplitLinearVelocities[rigidBody2Index].x += mContactConstraints[c].massInverseBody2 * linearImpulse.x;
mRigidBodyComponents.mSplitLinearVelocities[mContactConstraints[c].rigidBodyComponentIndexBody2].y += mContactConstraints[c].massInverseBody2 * linearImpulse.y; mRigidBodyComponents.mSplitLinearVelocities[rigidBody2Index].y += mContactConstraints[c].massInverseBody2 * linearImpulse.y;
mRigidBodyComponents.mSplitLinearVelocities[mContactConstraints[c].rigidBodyComponentIndexBody2].z += mContactConstraints[c].massInverseBody2 * linearImpulse.z; mRigidBodyComponents.mSplitLinearVelocities[rigidBody2Index].z += mContactConstraints[c].massInverseBody2 * linearImpulse.z;
mRigidBodyComponents.mSplitAngularVelocities[mContactConstraints[c].rigidBodyComponentIndexBody2].x += mContactPoints[contactPointIndex].i2TimesR2CrossN.x * deltaLambdaSplit; mRigidBodyComponents.mSplitAngularVelocities[rigidBody2Index].x += mContactPoints[contactPointIndex].i2TimesR2CrossN.x * deltaLambdaSplit;
mRigidBodyComponents.mSplitAngularVelocities[mContactConstraints[c].rigidBodyComponentIndexBody2].y += mContactPoints[contactPointIndex].i2TimesR2CrossN.y * deltaLambdaSplit; mRigidBodyComponents.mSplitAngularVelocities[rigidBody2Index].y += mContactPoints[contactPointIndex].i2TimesR2CrossN.y * deltaLambdaSplit;
mRigidBodyComponents.mSplitAngularVelocities[mContactConstraints[c].rigidBodyComponentIndexBody2].z += mContactPoints[contactPointIndex].i2TimesR2CrossN.z * deltaLambdaSplit; mRigidBodyComponents.mSplitAngularVelocities[rigidBody2Index].z += mContactPoints[contactPointIndex].i2TimesR2CrossN.z * deltaLambdaSplit;
} }
contactPointIndex++; contactPointIndex++;
@ -664,24 +678,24 @@ void ContactSolverSystem::solve() {
// Update the velocities of the body 1 by applying the impulse P // Update the velocities of the body 1 by applying the impulse P
mRigidBodyComponents.mConstrainedLinearVelocities[mContactConstraints[c].rigidBodyComponentIndexBody1].x -= mContactConstraints[c].massInverseBody1 * linearImpulseBody2.x; mRigidBodyComponents.mConstrainedLinearVelocities[rigidBody1Index].x -= mContactConstraints[c].massInverseBody1 * linearImpulseBody2.x;
mRigidBodyComponents.mConstrainedLinearVelocities[mContactConstraints[c].rigidBodyComponentIndexBody1].y -= mContactConstraints[c].massInverseBody1 * linearImpulseBody2.y; mRigidBodyComponents.mConstrainedLinearVelocities[rigidBody1Index].y -= mContactConstraints[c].massInverseBody1 * linearImpulseBody2.y;
mRigidBodyComponents.mConstrainedLinearVelocities[mContactConstraints[c].rigidBodyComponentIndexBody1].z -= mContactConstraints[c].massInverseBody1 * linearImpulseBody2.z; mRigidBodyComponents.mConstrainedLinearVelocities[rigidBody1Index].z -= mContactConstraints[c].massInverseBody1 * linearImpulseBody2.z;
Vector3 angularVelocity1 = mContactConstraints[c].inverseInertiaTensorBody1 * angularImpulseBody1; Vector3 angularVelocity1 = mContactConstraints[c].inverseInertiaTensorBody1 * angularImpulseBody1;
mRigidBodyComponents.mConstrainedAngularVelocities[mContactConstraints[c].rigidBodyComponentIndexBody1].x += angularVelocity1.x; mRigidBodyComponents.mConstrainedAngularVelocities[rigidBody1Index].x += angularVelocity1.x;
mRigidBodyComponents.mConstrainedAngularVelocities[mContactConstraints[c].rigidBodyComponentIndexBody1].y += angularVelocity1.y; mRigidBodyComponents.mConstrainedAngularVelocities[rigidBody1Index].y += angularVelocity1.y;
mRigidBodyComponents.mConstrainedAngularVelocities[mContactConstraints[c].rigidBodyComponentIndexBody1].z += angularVelocity1.z; mRigidBodyComponents.mConstrainedAngularVelocities[rigidBody1Index].z += angularVelocity1.z;
// Update the velocities of the body 2 by applying the impulse P // Update the velocities of the body 2 by applying the impulse P
mRigidBodyComponents.mConstrainedLinearVelocities[mContactConstraints[c].rigidBodyComponentIndexBody2].x += mContactConstraints[c].massInverseBody2 * linearImpulseBody2.x; mRigidBodyComponents.mConstrainedLinearVelocities[rigidBody2Index].x += mContactConstraints[c].massInverseBody2 * linearImpulseBody2.x;
mRigidBodyComponents.mConstrainedLinearVelocities[mContactConstraints[c].rigidBodyComponentIndexBody2].y += mContactConstraints[c].massInverseBody2 * linearImpulseBody2.y; mRigidBodyComponents.mConstrainedLinearVelocities[rigidBody2Index].y += mContactConstraints[c].massInverseBody2 * linearImpulseBody2.y;
mRigidBodyComponents.mConstrainedLinearVelocities[mContactConstraints[c].rigidBodyComponentIndexBody2].z += mContactConstraints[c].massInverseBody2 * linearImpulseBody2.z; mRigidBodyComponents.mConstrainedLinearVelocities[rigidBody2Index].z += mContactConstraints[c].massInverseBody2 * linearImpulseBody2.z;
Vector3 angularVelocity2 = mContactConstraints[c].inverseInertiaTensorBody2 * angularImpulseBody2; Vector3 angularVelocity2 = mContactConstraints[c].inverseInertiaTensorBody2 * angularImpulseBody2;
mRigidBodyComponents.mConstrainedAngularVelocities[mContactConstraints[c].rigidBodyComponentIndexBody2].x += angularVelocity2.x; mRigidBodyComponents.mConstrainedAngularVelocities[rigidBody2Index].x += angularVelocity2.x;
mRigidBodyComponents.mConstrainedAngularVelocities[mContactConstraints[c].rigidBodyComponentIndexBody2].y += angularVelocity2.y; mRigidBodyComponents.mConstrainedAngularVelocities[rigidBody2Index].y += angularVelocity2.y;
mRigidBodyComponents.mConstrainedAngularVelocities[mContactConstraints[c].rigidBodyComponentIndexBody2].z += angularVelocity2.z; mRigidBodyComponents.mConstrainedAngularVelocities[rigidBody2Index].z += angularVelocity2.z;
// ------ Second friction constraint at the center of the contact manifold ----- // // ------ Second friction constraint at the center of the contact manifold ----- //
@ -719,24 +733,24 @@ void ContactSolverSystem::solve() {
angularImpulseBody2.z = mContactConstraints[c].r2CrossT2.z * deltaLambda; angularImpulseBody2.z = mContactConstraints[c].r2CrossT2.z * deltaLambda;
// Update the velocities of the body 1 by applying the impulse P // Update the velocities of the body 1 by applying the impulse P
mRigidBodyComponents.mConstrainedLinearVelocities[mContactConstraints[c].rigidBodyComponentIndexBody1].x -= mContactConstraints[c].massInverseBody1 * linearImpulseBody2.x; mRigidBodyComponents.mConstrainedLinearVelocities[rigidBody1Index].x -= mContactConstraints[c].massInverseBody1 * linearImpulseBody2.x;
mRigidBodyComponents.mConstrainedLinearVelocities[mContactConstraints[c].rigidBodyComponentIndexBody1].y -= mContactConstraints[c].massInverseBody1 * linearImpulseBody2.y; mRigidBodyComponents.mConstrainedLinearVelocities[rigidBody1Index].y -= mContactConstraints[c].massInverseBody1 * linearImpulseBody2.y;
mRigidBodyComponents.mConstrainedLinearVelocities[mContactConstraints[c].rigidBodyComponentIndexBody1].z -= mContactConstraints[c].massInverseBody1 * linearImpulseBody2.z; mRigidBodyComponents.mConstrainedLinearVelocities[rigidBody1Index].z -= mContactConstraints[c].massInverseBody1 * linearImpulseBody2.z;
angularVelocity1 = mContactConstraints[c].inverseInertiaTensorBody1 * angularImpulseBody1; angularVelocity1 = mContactConstraints[c].inverseInertiaTensorBody1 * angularImpulseBody1;
mRigidBodyComponents.mConstrainedAngularVelocities[mContactConstraints[c].rigidBodyComponentIndexBody1].x += angularVelocity1.x; mRigidBodyComponents.mConstrainedAngularVelocities[rigidBody1Index].x += angularVelocity1.x;
mRigidBodyComponents.mConstrainedAngularVelocities[mContactConstraints[c].rigidBodyComponentIndexBody1].y += angularVelocity1.y; mRigidBodyComponents.mConstrainedAngularVelocities[rigidBody1Index].y += angularVelocity1.y;
mRigidBodyComponents.mConstrainedAngularVelocities[mContactConstraints[c].rigidBodyComponentIndexBody1].z += angularVelocity1.z; mRigidBodyComponents.mConstrainedAngularVelocities[rigidBody1Index].z += angularVelocity1.z;
// Update the velocities of the body 2 by applying the impulse P // Update the velocities of the body 2 by applying the impulse P
mRigidBodyComponents.mConstrainedLinearVelocities[mContactConstraints[c].rigidBodyComponentIndexBody2].x += mContactConstraints[c].massInverseBody2 * linearImpulseBody2.x; mRigidBodyComponents.mConstrainedLinearVelocities[rigidBody2Index].x += mContactConstraints[c].massInverseBody2 * linearImpulseBody2.x;
mRigidBodyComponents.mConstrainedLinearVelocities[mContactConstraints[c].rigidBodyComponentIndexBody2].y += mContactConstraints[c].massInverseBody2 * linearImpulseBody2.y; mRigidBodyComponents.mConstrainedLinearVelocities[rigidBody2Index].y += mContactConstraints[c].massInverseBody2 * linearImpulseBody2.y;
mRigidBodyComponents.mConstrainedLinearVelocities[mContactConstraints[c].rigidBodyComponentIndexBody2].z += mContactConstraints[c].massInverseBody2 * linearImpulseBody2.z; mRigidBodyComponents.mConstrainedLinearVelocities[rigidBody2Index].z += mContactConstraints[c].massInverseBody2 * linearImpulseBody2.z;
angularVelocity2 = mContactConstraints[c].inverseInertiaTensorBody2 * angularImpulseBody2; angularVelocity2 = mContactConstraints[c].inverseInertiaTensorBody2 * angularImpulseBody2;
mRigidBodyComponents.mConstrainedAngularVelocities[mContactConstraints[c].rigidBodyComponentIndexBody2].x += angularVelocity2.x; mRigidBodyComponents.mConstrainedAngularVelocities[rigidBody2Index].x += angularVelocity2.x;
mRigidBodyComponents.mConstrainedAngularVelocities[mContactConstraints[c].rigidBodyComponentIndexBody2].y += angularVelocity2.y; mRigidBodyComponents.mConstrainedAngularVelocities[rigidBody2Index].y += angularVelocity2.y;
mRigidBodyComponents.mConstrainedAngularVelocities[mContactConstraints[c].rigidBodyComponentIndexBody2].z += angularVelocity2.z; mRigidBodyComponents.mConstrainedAngularVelocities[rigidBody2Index].z += angularVelocity2.z;
// ------ Twist friction constraint at the center of the contact manifol ------ // // ------ Twist friction constraint at the center of the contact manifol ------ //
@ -760,15 +774,15 @@ void ContactSolverSystem::solve() {
// Update the velocities of the body 1 by applying the impulse P // Update the velocities of the body 1 by applying the impulse P
angularVelocity1 = mContactConstraints[c].inverseInertiaTensorBody1 * angularImpulseBody2; angularVelocity1 = mContactConstraints[c].inverseInertiaTensorBody1 * angularImpulseBody2;
mRigidBodyComponents.mConstrainedAngularVelocities[mContactConstraints[c].rigidBodyComponentIndexBody1].x -= angularVelocity1.x; mRigidBodyComponents.mConstrainedAngularVelocities[rigidBody1Index].x -= angularVelocity1.x;
mRigidBodyComponents.mConstrainedAngularVelocities[mContactConstraints[c].rigidBodyComponentIndexBody1].y -= angularVelocity1.y; mRigidBodyComponents.mConstrainedAngularVelocities[rigidBody1Index].y -= angularVelocity1.y;
mRigidBodyComponents.mConstrainedAngularVelocities[mContactConstraints[c].rigidBodyComponentIndexBody1].z -= angularVelocity1.z; mRigidBodyComponents.mConstrainedAngularVelocities[rigidBody1Index].z -= angularVelocity1.z;
// Update the velocities of the body 1 by applying the impulse P // Update the velocities of the body 1 by applying the impulse P
angularVelocity2 = mContactConstraints[c].inverseInertiaTensorBody2 * angularImpulseBody2; angularVelocity2 = mContactConstraints[c].inverseInertiaTensorBody2 * angularImpulseBody2;
mRigidBodyComponents.mConstrainedAngularVelocities[mContactConstraints[c].rigidBodyComponentIndexBody2].x += angularVelocity2.x; mRigidBodyComponents.mConstrainedAngularVelocities[rigidBody2Index].x += angularVelocity2.x;
mRigidBodyComponents.mConstrainedAngularVelocities[mContactConstraints[c].rigidBodyComponentIndexBody2].y += angularVelocity2.y; mRigidBodyComponents.mConstrainedAngularVelocities[rigidBody2Index].y += angularVelocity2.y;
mRigidBodyComponents.mConstrainedAngularVelocities[mContactConstraints[c].rigidBodyComponentIndexBody2].z += angularVelocity2.z; mRigidBodyComponents.mConstrainedAngularVelocities[rigidBody2Index].z += angularVelocity2.z;
// --------- Rolling resistance constraint at the center of the contact manifold --------- // // --------- Rolling resistance constraint at the center of the contact manifold --------- //
@ -787,15 +801,15 @@ void ContactSolverSystem::solve() {
// Update the velocities of the body 1 by applying the impulse P // Update the velocities of the body 1 by applying the impulse P
angularVelocity1 = mContactConstraints[c].inverseInertiaTensorBody1 * deltaLambdaRolling; angularVelocity1 = mContactConstraints[c].inverseInertiaTensorBody1 * deltaLambdaRolling;
mRigidBodyComponents.mConstrainedAngularVelocities[mContactConstraints[c].rigidBodyComponentIndexBody1].x -= angularVelocity1.x; mRigidBodyComponents.mConstrainedAngularVelocities[rigidBody1Index].x -= angularVelocity1.x;
mRigidBodyComponents.mConstrainedAngularVelocities[mContactConstraints[c].rigidBodyComponentIndexBody1].y -= angularVelocity1.y; mRigidBodyComponents.mConstrainedAngularVelocities[rigidBody1Index].y -= angularVelocity1.y;
mRigidBodyComponents.mConstrainedAngularVelocities[mContactConstraints[c].rigidBodyComponentIndexBody1].z -= angularVelocity1.z; mRigidBodyComponents.mConstrainedAngularVelocities[rigidBody1Index].z -= angularVelocity1.z;
// Update the velocities of the body 2 by applying the impulse P // Update the velocities of the body 2 by applying the impulse P
angularVelocity2 = mContactConstraints[c].inverseInertiaTensorBody2 * deltaLambdaRolling; angularVelocity2 = mContactConstraints[c].inverseInertiaTensorBody2 * deltaLambdaRolling;
mRigidBodyComponents.mConstrainedAngularVelocities[mContactConstraints[c].rigidBodyComponentIndexBody2].x += angularVelocity2.x; mRigidBodyComponents.mConstrainedAngularVelocities[rigidBody2Index].x += angularVelocity2.x;
mRigidBodyComponents.mConstrainedAngularVelocities[mContactConstraints[c].rigidBodyComponentIndexBody2].y += angularVelocity2.y; mRigidBodyComponents.mConstrainedAngularVelocities[rigidBody2Index].y += angularVelocity2.y;
mRigidBodyComponents.mConstrainedAngularVelocities[mContactConstraints[c].rigidBodyComponentIndexBody2].z += angularVelocity2.z; mRigidBodyComponents.mConstrainedAngularVelocities[rigidBody2Index].z += angularVelocity2.z;
} }
} }
} }
@ -813,12 +827,11 @@ decimal ContactSolverSystem::computeMixedRestitutionFactor(Collider* collider1,
decimal ContactSolverSystem::computeMixedFrictionCoefficient(Collider* collider1, Collider* collider2) const { decimal ContactSolverSystem::computeMixedFrictionCoefficient(Collider* collider1, Collider* collider2) const {
// Use the geometric mean to compute the mixed friction coefficient // Use the geometric mean to compute the mixed friction coefficient
return std::sqrt(collider1->getMaterial().getFrictionCoefficient() * return collider1->getMaterial().getFrictionCoefficientSqrt() * collider2->getMaterial().getFrictionCoefficientSqrt();
collider2->getMaterial().getFrictionCoefficient());
} }
// Compute th mixed rolling resistance factor between two colliders // Compute th mixed rolling resistance factor between two colliders
RP3D_FORCE_INLINE decimal ContactSolverSystem::computeMixedRollingResistance(Collider* collider1, Collider* collider2) const { decimal ContactSolverSystem::computeMixedRollingResistance(Collider* collider1, Collider* collider2) const {
return decimal(0.5f) * (collider1->getMaterial().getRollingResistance() + collider2->getMaterial().getRollingResistance()); return decimal(0.5f) * (collider1->getMaterial().getRollingResistance() + collider2->getMaterial().getRollingResistance());
} }
@ -851,8 +864,7 @@ void ContactSolverSystem::storeImpulses() {
// Compute the two unit orthogonal vectors "t1" and "t2" that span the tangential friction plane // Compute the two unit orthogonal vectors "t1" and "t2" that span the tangential friction plane
// for a contact manifold. The two vectors have to be such that : t1 x t2 = contactNormal. // for a contact manifold. The two vectors have to be such that : t1 x t2 = contactNormal.
void ContactSolverSystem::computeFrictionVectors(const Vector3& deltaVelocity, void ContactSolverSystem::computeFrictionVectors(const Vector3& deltaVelocity, ContactManifoldSolver& contact) const {
ContactManifoldSolver& contact) const {
RP3D_PROFILE("ContactSolver::computeFrictionVectors()", mProfiler); RP3D_PROFILE("ContactSolver::computeFrictionVectors()", mProfiler);