From aa6b228e101d727f8b2c8f862475cc2fd77fec28 Mon Sep 17 00:00:00 2001 From: Daniel Chappuis Date: Tue, 28 Jul 2020 23:34:25 +0200 Subject: [PATCH] Optimizations --- .../collision/ContactManifold.h | 8 +- include/reactphysics3d/engine/Material.h | 45 ++-- .../reactphysics3d/mathematics/Matrix3x3.h | 3 + src/collision/ContactManifold.cpp | 4 +- src/engine/Material.cpp | 11 +- src/mathematics/Matrix3x3.cpp | 21 ++ src/systems/CollisionDetectionSystem.cpp | 66 ++--- src/systems/ContactSolverSystem.cpp | 248 +++++++++--------- 8 files changed, 215 insertions(+), 191 deletions(-) diff --git a/include/reactphysics3d/collision/ContactManifold.h b/include/reactphysics3d/collision/ContactManifold.h index 3dfeb0df..cdd95e58 100644 --- a/include/reactphysics3d/collision/ContactManifold.h +++ b/include/reactphysics3d/collision/ContactManifold.h @@ -101,13 +101,19 @@ class ContactManifold { /// True if the contact manifold has already been added into an island 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: // -------------------- Methods -------------------- // /// Constructor ContactManifold(Entity bodyEntity1, Entity bodyEntity2, Entity colliderEntity1, Entity colliderEntity2, - uint contactPointsIndex, uint8 nbContactPoints); + uint contactPointsIndex, uint8 nbContactPoints, uint32 rigidBody1Index, uint32 rigidBody2Index); // -------------------- Friendship -------------------- // diff --git a/include/reactphysics3d/engine/Material.h b/include/reactphysics3d/engine/Material.h index c4406b74..8a8bf2b4 100644 --- a/include/reactphysics3d/engine/Material.h +++ b/include/reactphysics3d/engine/Material.h @@ -28,6 +28,7 @@ // Libraries #include +#include #include namespace reactphysics3d { @@ -44,8 +45,8 @@ class Material { // -------------------- Attributes -------------------- // - /// Friction coefficient (positive value) - decimal mFrictionCoefficient; + /// Square root of the friction coefficient + decimal mFrictionCoefficientSqrt; /// Rolling resistance factor (positive value) decimal mRollingResistance; @@ -59,14 +60,7 @@ class Material { // -------------------- Methods -------------------- // /// Constructor - Material(decimal frictionCoefficient, decimal rollingResistance, decimal bounciness, - decimal massDensity = decimal(1.0)); - - /// Copy-constructor - Material(const Material& material); - - /// Destructor - ~Material() = default; + Material(decimal frictionCoefficient, decimal rollingResistance, decimal bounciness, decimal massDensity = decimal(1.0)); public : @@ -84,6 +78,9 @@ class Material { /// Set the friction coefficient. void setFrictionCoefficient(decimal frictionCoefficient); + /// Return the square root friction coefficient + decimal getFrictionCoefficientSqrt() const; + /// Return the rolling resistance factor decimal getRollingResistance() const; @@ -99,9 +96,6 @@ class Material { /// Return a string representation for the material std::string to_string() const; - /// Overloaded assignment operator - Material& operator=(const Material& material); - // ---------- Friendship ---------- // friend class Collider; @@ -131,7 +125,7 @@ RP3D_FORCE_INLINE void Material::setBounciness(decimal bounciness) { * @return Friction coefficient (positive value) */ RP3D_FORCE_INLINE decimal Material::getFrictionCoefficient() const { - return mFrictionCoefficient; + return mFrictionCoefficientSqrt * mFrictionCoefficientSqrt; } // Set the friction coefficient. @@ -142,7 +136,12 @@ RP3D_FORCE_INLINE decimal Material::getFrictionCoefficient() const { */ RP3D_FORCE_INLINE void Material::setFrictionCoefficient(decimal frictionCoefficient) { 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, @@ -184,27 +183,13 @@ RP3D_FORCE_INLINE std::string Material::to_string() const { std::stringstream ss; - ss << "frictionCoefficient=" << mFrictionCoefficient << std::endl; + ss << "frictionCoefficient=" << (mFrictionCoefficientSqrt * mFrictionCoefficientSqrt) << std::endl; ss << "rollingResistance=" << mRollingResistance << std::endl; ss << "bounciness=" << mBounciness << std::endl; 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 diff --git a/include/reactphysics3d/mathematics/Matrix3x3.h b/include/reactphysics3d/mathematics/Matrix3x3.h index 575adfc4..6d14d993 100644 --- a/include/reactphysics3d/mathematics/Matrix3x3.h +++ b/include/reactphysics3d/mathematics/Matrix3x3.h @@ -95,6 +95,9 @@ class Matrix3x3 { /// Return the inverse matrix Matrix3x3 getInverse() const; + /// Return the inverse matrix + Matrix3x3 getInverseWithDeterminant(decimal determinant) const; + /// Return the matrix with absolute values Matrix3x3 getAbsoluteMatrix() const; diff --git a/src/collision/ContactManifold.cpp b/src/collision/ContactManifold.cpp index 46f886de..cabdace2 100644 --- a/src/collision/ContactManifold.cpp +++ b/src/collision/ContactManifold.cpp @@ -32,9 +32,9 @@ using namespace reactphysics3d; // Constructor 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), colliderEntity1(colliderEntity1), colliderEntity2(colliderEntity2), nbContactPoints(nbContactPoints), frictionImpulse1(0), frictionImpulse2(0), - frictionTwistImpulse(0), isAlreadyInIsland(false) { + frictionTwistImpulse(0), isAlreadyInIsland(false), rigidBody1Index(rigidBody1Index), rigidBody2Index(rigidBody2Index) { } diff --git a/src/engine/Material.cpp b/src/engine/Material.cpp index bb2aaa35..3c73b11e 100644 --- a/src/engine/Material.cpp +++ b/src/engine/Material.cpp @@ -30,14 +30,7 @@ using namespace reactphysics3d; // Constructor Material::Material(decimal frictionCoefficient, decimal rollingResistance, decimal bounciness, decimal massDensity) - : mFrictionCoefficient(frictionCoefficient), mRollingResistance(rollingResistance), mBounciness(bounciness), - mMassDensity(massDensity) { - -} - -// Copy-constructor -Material::Material(const Material& material) - : mFrictionCoefficient(material.mFrictionCoefficient), mRollingResistance(material.mRollingResistance), - mBounciness(material.mBounciness) { + : mFrictionCoefficientSqrt(std::sqrt(frictionCoefficient)), + mRollingResistance(rollingResistance), mBounciness(bounciness), mMassDensity(massDensity) { } diff --git a/src/mathematics/Matrix3x3.cpp b/src/mathematics/Matrix3x3.cpp index 2cca4a2c..16502f0d 100644 --- a/src/mathematics/Matrix3x3.cpp +++ b/src/mathematics/Matrix3x3.cpp @@ -66,5 +66,26 @@ Matrix3x3 Matrix3x3::getInverse() const { 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); +} diff --git a/src/systems/CollisionDetectionSystem.cpp b/src/systems/CollisionDetectionSystem.cpp index 4d28ae89..e474dfcd 100644 --- a/src/systems/CollisionDetectionSystem.cpp +++ b/src/systems/CollisionDetectionSystem.cpp @@ -167,7 +167,8 @@ void CollisionDetectionSystem::updateOverlappingPairs(const List nodePair = overlappingNodes[i]; @@ -241,7 +242,8 @@ void CollisionDetectionSystem::computeMiddlePhase(NarrowPhaseInput& narrowPhaseI mOverlappingPairs.clearObsoleteLastFrameCollisionInfos(); // 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.mColliders2[i]) != -1); @@ -278,7 +280,8 @@ void CollisionDetectionSystem::computeMiddlePhase(NarrowPhaseInput& narrowPhaseI // For each possible convex vs concave pair of bodies 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.mColliders2[i]) != -1); @@ -307,7 +310,8 @@ void CollisionDetectionSystem::computeMiddlePhaseCollisionSnapshot(List& mOverlappingPairs.clearObsoleteLastFrameCollisionInfos(); // 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]; @@ -339,7 +343,8 @@ void CollisionDetectionSystem::computeMiddlePhaseCollisionSnapshot(List& } // 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 pairIndex = mOverlappingPairs.mMapPairIdToPairIndex[pairId]; @@ -407,7 +412,8 @@ void CollisionDetectionSystem::computeConvexVsConcaveMiddlePhase(uint64 pairInde const bool reportContacts = !isCollider1Trigger && !isCollider2Trigger; // 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 // destructor of the corresponding NarrowPhaseInfo. @@ -716,7 +722,8 @@ void CollisionDetectionSystem::createContacts() { mCurrentContactPoints->reserve(mCurrentContactManifolds->size()); // 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]; @@ -725,10 +732,12 @@ void CollisionDetectionSystem::createContacts() { contactPair.contactPointsIndex = mCurrentContactPoints->size(); // 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); } - if (mRigidBodyComponents.hasComponent(contactPair.body2Entity)) { + if (mRigidBodyComponents.hasComponentGetIndex(contactPair.body2Entity, rigidBody2Index)) { mRigidBodyComponents.addContacPair(contactPair.body2Entity, p); } @@ -742,12 +751,9 @@ void CollisionDetectionSystem::createContacts() { const int8 nbContactPoints = static_cast(potentialManifold.nbPotentialContactPoints); contactPair.nbToTalContactPoints += nbContactPoints; - // We create a new contact manifold - ContactManifold contactManifold(contactPair.body1Entity, contactPair.body2Entity, contactPair.collider1Entity, - contactPair.collider2Entity, contactPointsIndex, nbContactPoints); - - // Add the contact manifold - mCurrentContactManifolds->add(contactManifold); + // Create and add the contact manifold + mCurrentContactManifolds->emplace(contactPair.body1Entity, contactPair.body2Entity, contactPair.collider1Entity, + contactPair.collider2Entity, contactPointsIndex, nbContactPoints, rigidBody1Index, rigidBody2Index); assert(potentialManifold.nbPotentialContactPoints > 0); @@ -756,11 +762,8 @@ void CollisionDetectionSystem::createContacts() { ContactPointInfo& potentialContactPoint = mPotentialContactPoints[potentialManifold.potentialContactPointsIndices[c]]; - // Create a new contact point - ContactPoint contactPoint(potentialContactPoint, mWorld->mConfig.persistentContactDistanceThreshold); - - // Add the contact point - mCurrentContactPoints->add(contactPoint); + // Create and add the contact point + mCurrentContactPoints->emplace(potentialContactPoint, mWorld->mConfig.persistentContactDistanceThreshold); } } } @@ -812,7 +815,8 @@ void CollisionDetectionSystem::createSnapshotContacts(List& contact contactPoints.reserve(contactManifolds.size()); // 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]; assert(contactPair.nbPotentialContactManifolds > 0); @@ -831,12 +835,9 @@ void CollisionDetectionSystem::createSnapshotContacts(List& contact const uint8 nbContactPoints = potentialManifold.nbPotentialContactPoints; contactPair.nbToTalContactPoints += nbContactPoints; - // We create a new contact manifold - ContactManifold contactManifold(contactPair.body1Entity, contactPair.body2Entity, contactPair.collider1Entity, - contactPair.collider2Entity, contactPointsIndex, nbContactPoints); - - // Add the contact manifold - contactManifolds.add(contactManifold); + // Create and add the contact manifold + contactManifolds.emplace(contactPair.body1Entity, contactPair.body2Entity, contactPair.collider1Entity, + contactPair.collider2Entity, contactPointsIndex, nbContactPoints, 0, 0); assert(potentialManifold.nbPotentialContactPoints > 0); @@ -992,10 +993,12 @@ void CollisionDetectionSystem::processPotentialContacts(NarrowPhaseInfoBatch& na RP3D_PROFILE("CollisionDetectionSystem::processPotentialContacts()", mProfiler); + const uint nbObjects = narrowPhaseInfoBatch.getNbObjects(); + if (updateLastFrameInfo) { // 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; @@ -1005,7 +1008,7 @@ void CollisionDetectionSystem::processPotentialContacts(NarrowPhaseInfoBatch& na } // 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 pairIndex = mOverlappingPairs.mMapPairIdToPairIndex[pairId]; @@ -1175,7 +1178,8 @@ void CollisionDetectionSystem::reducePotentialContactManifolds(List RP3D_PROFILE("CollisionDetectionSystem::reducePotentialContactManifolds()", mProfiler); // 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]; @@ -1205,7 +1209,7 @@ void CollisionDetectionSystem::reducePotentialContactManifolds(List } // 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]; diff --git a/src/systems/ContactSolverSystem.cpp b/src/systems/ContactSolverSystem.cpp index c5616648..27f74cf6 100644 --- a/src/systems/ContactSolverSystem.cpp +++ b/src/systems/ContactSolverSystem.cpp @@ -90,7 +90,8 @@ void ContactSolverSystem::init(List* contactManifolds, List 0) { initializeForIsland(i); @@ -117,25 +118,24 @@ void ContactSolverSystem::initializeForIsland(uint islandIndex) { assert(mIslands.nbContactManifolds[islandIndex] > 0); // For each contact manifold of the island - uint contactManifoldsIndex = mIslands.contactManifoldsIndices[islandIndex]; - uint nbContactManifolds = mIslands.nbContactManifolds[islandIndex]; + const uint contactManifoldsIndex = mIslands.contactManifoldsIndices[islandIndex]; + const uint nbContactManifolds = mIslands.nbContactManifolds[islandIndex]; for (uint m=contactManifoldsIndex; m < contactManifoldsIndex + nbContactManifolds; m++) { ContactManifold& externalManifold = (*mAllContactManifolds)[m]; assert(externalManifold.nbContactPoints > 0); - const uint rigidBodyIndex1 = mRigidBodyComponents.getEntityIndex(externalManifold.bodyEntity1); - const uint rigidBodyIndex2 = mRigidBodyComponents.getEntityIndex(externalManifold.bodyEntity2); + const uint rigidBodyIndex1 = externalManifold.rigidBody1Index; + const uint rigidBodyIndex2 = externalManifold.rigidBody2Index; // Get the two bodies of the contact - RigidBody* body1 = mRigidBodyComponents.mRigidBodies[rigidBodyIndex1]; - RigidBody* body2 = mRigidBodyComponents.mRigidBodies[rigidBodyIndex2]; assert(body1 != nullptr); assert(body2 != nullptr); assert(!mBodyComponents.getIsEntityDisabled(externalManifold.bodyEntity1)); 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* collider2 = mColliderComponents.getCollider(externalManifold.colliderEntity2); @@ -152,7 +152,9 @@ void ContactSolverSystem::initializeForIsland(uint islandIndex) { mContactConstraints[mNbContactManifolds].massInverseBody1 = mRigidBodyComponents.mInverseMasses[rigidBodyIndex1]; mContactConstraints[mNbContactManifolds].massInverseBody2 = mRigidBodyComponents.mInverseMasses[rigidBodyIndex2]; mContactConstraints[mNbContactManifolds].nbContacts = externalManifold.nbContactPoints; + // TODO OPTI : Do not compute this using colliders pointers mContactConstraints[mNbContactManifolds].frictionCoefficient = computeMixedFrictionCoefficient(collider1, collider2); + // TODO OPTI : Do not compute this using colliders pointers mContactConstraints[mNbContactManifolds].rollingResistanceFactor = computeMixedRollingResistance(collider1, collider2); mContactConstraints[mNbContactManifolds].externalContactManifold = &externalManifold; mContactConstraints[mNbContactManifolds].normal.setToZero(); @@ -165,17 +167,21 @@ void ContactSolverSystem::initializeForIsland(uint islandIndex) { const Vector3& v2 = mRigidBodyComponents.mLinearVelocities[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 assert(externalManifold.nbContactPoints > 0); - uint contactPointsStartIndex = externalManifold.contactPointsIndex; - uint nbContactPoints = static_cast(externalManifold.nbContactPoints); + const uint contactPointsStartIndex = externalManifold.contactPointsIndex; + const uint nbContactPoints = static_cast(externalManifold.nbContactPoints); for (uint c=contactPointsStartIndex; c < contactPointsStartIndex + nbContactPoints; c++) { ContactPoint& externalContact = (*mAllContactPoints)[c]; // Get the contact point on the two bodies - Vector3 p1 = mColliderComponents.getLocalToWorldTransform(externalManifold.colliderEntity1) * externalContact.getLocalPointOnShape1(); - Vector3 p2 = mColliderComponents.getLocalToWorldTransform(externalManifold.colliderEntity2) * externalContact.getLocalPointOnShape2(); + const Vector3 p1 = collider1LocalToWorldTransform * externalContact.getLocalPointOnShape1(); + const Vector3 p2 = collider2LocalToWorldTransform * externalContact.getLocalPointOnShape2(); new (mContactPoints + mNbContactPoints) ContactPointSolver(); mContactPoints[mNbContactPoints].externalContact = &externalContact; @@ -270,20 +276,20 @@ void ContactSolverSystem::initializeForIsland(uint islandIndex) { mContactConstraints[mNbContactManifolds].frictionTwistImpulse = externalManifold.frictionTwistImpulse; // Compute the inverse K matrix for the rolling resistance constraint - bool isBody1DynamicType = body1->getType() == BodyType::DYNAMIC; - bool isBody2DynamicType = body2->getType() == BodyType::DYNAMIC; + const bool isBody1DynamicType = mRigidBodyComponents.mBodyTypes[rigidBodyIndex1] == BodyType::DYNAMIC; + const bool isBody2DynamicType = mRigidBodyComponents.mBodyTypes[rigidBodyIndex2] == BodyType::DYNAMIC; mContactConstraints[mNbContactManifolds].inverseRollingResistance.setToZero(); if (mContactConstraints[mNbContactManifolds].rollingResistanceFactor > 0 && (isBody1DynamicType || isBody2DynamicType)) { 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 (approxEqual(det, decimal(0.0))) { mContactConstraints[mNbContactManifolds].inverseRollingResistance.setToZero(); } 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; igetMaterial().getFrictionCoefficient() * - collider2->getMaterial().getFrictionCoefficient()); + return collider1->getMaterial().getFrictionCoefficientSqrt() * collider2->getMaterial().getFrictionCoefficientSqrt(); } // 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()); } @@ -851,8 +864,7 @@ void ContactSolverSystem::storeImpulses() { // 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. -void ContactSolverSystem::computeFrictionVectors(const Vector3& deltaVelocity, - ContactManifoldSolver& contact) const { +void ContactSolverSystem::computeFrictionVectors(const Vector3& deltaVelocity, ContactManifoldSolver& contact) const { RP3D_PROFILE("ContactSolver::computeFrictionVectors()", mProfiler);