diff --git a/include/reactphysics3d/collision/Collider.h b/include/reactphysics3d/collision/Collider.h index ae590d9f..20b83405 100644 --- a/include/reactphysics3d/collision/Collider.h +++ b/include/reactphysics3d/collision/Collider.h @@ -59,9 +59,6 @@ class Collider { /// Pointer to the parent body CollisionBody* mBody; - /// Material properties of the rigid body - Material mMaterial; - /// Pointer to user data void* mUserData; @@ -225,14 +222,6 @@ RP3D_FORCE_INLINE bool Collider::testAABBOverlap(const AABB& worldAABB) const { return worldAABB.testCollision(getWorldAABB()); } -// Return a reference to the material properties of the collider -/** - * @return A reference to the material of the body - */ -RP3D_FORCE_INLINE Material& Collider::getMaterial() { - return mMaterial; -} - } #endif diff --git a/include/reactphysics3d/components/ColliderComponents.h b/include/reactphysics3d/components/ColliderComponents.h index 8dfa5bf3..8207309c 100644 --- a/include/reactphysics3d/components/ColliderComponents.h +++ b/include/reactphysics3d/components/ColliderComponents.h @@ -32,6 +32,7 @@ #include #include #include +#include // ReactPhysics3D namespace namespace reactphysics3d { @@ -99,6 +100,9 @@ class ColliderComponents : public Components { /// True if the collider is a trigger bool* mIsTrigger; + /// Array with the material of each collider + Material* mMaterials; + // -------------------- Methods -------------------- // @@ -127,14 +131,15 @@ class ColliderComponents : public Components { unsigned short collisionCategoryBits; unsigned short collideWithMaskBits; const Transform& localToWorldTransform; + const Material& material; /// Constructor ColliderComponent(Entity bodyEntity, Collider* collider, AABB localBounds, const Transform& localToBodyTransform, CollisionShape* collisionShape, unsigned short collisionCategoryBits, - unsigned short collideWithMaskBits, const Transform& localToWorldTransform) + unsigned short collideWithMaskBits, const Transform& localToWorldTransform, const Material& material) :bodyEntity(bodyEntity), collider(collider), localBounds(localBounds), localToBodyTransform(localToBodyTransform), collisionShape(collisionShape), collisionCategoryBits(collisionCategoryBits), collideWithMaskBits(collideWithMaskBits), - localToWorldTransform(localToWorldTransform) { + localToWorldTransform(localToWorldTransform), material(material) { } }; @@ -204,12 +209,20 @@ class ColliderComponents : public Components { /// Set whether a collider is a trigger void setIsTrigger(Entity colliderEntity, bool isTrigger); + /// Return a reference to the material of a collider + Material& getMaterial(Entity colliderEntity); + + /// Set the material of a collider + void setMaterial(Entity colliderEntity, const Material& material); + // -------------------- Friendship -------------------- // friend class BroadPhaseSystem; friend class CollisionDetectionSystem; + friend class ContactSolverSystem; friend class DynamicsSystem; friend class OverlappingPairs; + friend class RigidBody; }; // Return the body entity of a given collider @@ -357,6 +370,22 @@ RP3D_FORCE_INLINE void ColliderComponents::setIsTrigger(Entity colliderEntity, b mIsTrigger[mMapEntityToComponentIndex[colliderEntity]] = isTrigger; } +// Return a reference to the material of a collider +RP3D_FORCE_INLINE Material& ColliderComponents::getMaterial(Entity colliderEntity) { + + assert(mMapEntityToComponentIndex.containsKey(colliderEntity)); + + return mMaterials[mMapEntityToComponentIndex[colliderEntity]]; +} + +// Set the material of a collider +RP3D_FORCE_INLINE void ColliderComponents::setMaterial(Entity colliderEntity, const Material& material) { + + assert(mMapEntityToComponentIndex.containsKey(colliderEntity)); + + mMaterials[mMapEntityToComponentIndex[colliderEntity]] = material; +} + } #endif diff --git a/include/reactphysics3d/engine/Material.h b/include/reactphysics3d/engine/Material.h index 8a8bf2b4..a8462c20 100644 --- a/include/reactphysics3d/engine/Material.h +++ b/include/reactphysics3d/engine/Material.h @@ -99,6 +99,8 @@ class Material { // ---------- Friendship ---------- // friend class Collider; + friend class CollisionBody; + friend class RigidBody; }; // Return the bounciness diff --git a/include/reactphysics3d/systems/ContactSolverSystem.h b/include/reactphysics3d/systems/ContactSolverSystem.h index ba30a0ac..11960b68 100644 --- a/include/reactphysics3d/systems/ContactSolverSystem.h +++ b/include/reactphysics3d/systems/ContactSolverSystem.h @@ -30,6 +30,7 @@ #include #include #include +#include /// ReactPhysics3D namespace namespace reactphysics3d { @@ -338,14 +339,13 @@ class ContactSolverSystem { // -------------------- Methods -------------------- // /// Compute the collision restitution factor from the restitution factor of each collider - decimal computeMixedRestitutionFactor(Collider* collider1, - Collider* collider2) const; + decimal computeMixedRestitutionFactor(const Material& material1, const Material& material2) const; /// Compute the mixed friction coefficient from the friction coefficient of each collider - decimal computeMixedFrictionCoefficient(Collider* collider1, Collider* collider2) const; + decimal computeMixedFrictionCoefficient(const Material &material1, const Material &material2) const; /// Compute th mixed rolling resistance factor between two colliders - decimal computeMixedRollingResistance(Collider* collider1, Collider* collider2) const; + decimal computeMixedRollingResistance(const Material& material1, const Material& material2) const; /// 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 @@ -407,6 +407,28 @@ RP3D_FORCE_INLINE void ContactSolverSystem::setIsSplitImpulseActive(bool isActiv mIsSplitImpulseActive = isActive; } +// Compute the collision restitution factor from the restitution factor of each collider +RP3D_FORCE_INLINE decimal ContactSolverSystem::computeMixedRestitutionFactor(const Material& material1, const Material& material2) const { + + const decimal restitution1 = material1.getBounciness(); + const decimal restitution2 = material2.getBounciness(); + + // Return the largest restitution factor + return (restitution1 > restitution2) ? restitution1 : restitution2; +} + +// Compute the mixed friction coefficient from the friction coefficient of each collider +RP3D_FORCE_INLINE decimal ContactSolverSystem::computeMixedFrictionCoefficient(const Material& material1, const Material& material2) const { + + // Use the geometric mean to compute the mixed friction coefficient + return material1.getFrictionCoefficientSqrt() * material2.getFrictionCoefficientSqrt(); +} + +// Compute th mixed rolling resistance factor between two colliders +RP3D_FORCE_INLINE decimal ContactSolverSystem::computeMixedRollingResistance(const Material& material1, const Material& material2) const { + return decimal(0.5f) * (material1.getRollingResistance() + material2.getRollingResistance()); +} + #ifdef IS_RP3D_PROFILING_ENABLED // Set the profiler diff --git a/src/body/CollisionBody.cpp b/src/body/CollisionBody.cpp index 16279cf7..5c9c9610 100644 --- a/src/body/CollisionBody.cpp +++ b/src/body/CollisionBody.cpp @@ -89,12 +89,13 @@ Collider* CollisionBody::addCollider(CollisionShape* collisionShape, const Trans // TODO : Maybe this method can directly returns an AABB collisionShape->getLocalBounds(localBoundsMin, localBoundsMax); const Transform localToWorldTransform = mWorld.mTransformComponents.getTransform(mEntity) * transform; - ColliderComponents::ColliderComponent colliderComponent(mEntity, collider, - AABB(localBoundsMin, localBoundsMax), - transform, collisionShape, 0x0001, 0xFFFF, localToWorldTransform); + Material material(mWorld.mConfig.defaultFrictionCoefficient, mWorld.mConfig.defaultRollingRestistance, mWorld.mConfig.defaultBounciness); + ColliderComponents::ColliderComponent colliderComponent(mEntity, collider, AABB(localBoundsMin, localBoundsMax), + transform, collisionShape, 0x0001, 0xFFFF, localToWorldTransform, material); bool isActive = mWorld.mCollisionBodyComponents.getIsActive(mEntity); mWorld.mCollidersComponents.addComponent(colliderEntity, !isActive, colliderComponent); + mWorld.mCollisionBodyComponents.addColliderToBody(mEntity, colliderEntity); // Assign the collider with the collision shape diff --git a/src/body/RigidBody.cpp b/src/body/RigidBody.cpp index 76db1b04..4e884efc 100644 --- a/src/body/RigidBody.cpp +++ b/src/body/RigidBody.cpp @@ -331,15 +331,15 @@ Vector3 RigidBody::computeCenterOfMass() const { const List& colliderEntities = mWorld.mCollisionBodyComponents.getColliders(mEntity); for (uint i=0; i < colliderEntities.size(); i++) { - Collider* collider = mWorld.mCollidersComponents.getCollider(colliderEntities[i]); + const uint colliderIndex = mWorld.mCollidersComponents.getEntityIndex(colliderEntities[i]); - const decimal colliderVolume = mWorld.mCollidersComponents.getCollisionShape(colliderEntities[i])->getVolume(); - const decimal colliderMassDensity = collider->getMaterial().getMassDensity(); + const decimal colliderVolume = mWorld.mCollidersComponents.mCollisionShapes[colliderIndex]->getVolume(); + const decimal colliderMassDensity = mWorld.mCollidersComponents.mMaterials[colliderIndex].getMassDensity(); const decimal colliderMass = colliderVolume * colliderMassDensity; totalMass += colliderMass; - centerOfMassLocal += colliderMass * mWorld.mCollidersComponents.getLocalToBodyTransform(colliderEntities[i]).getPosition(); + centerOfMassLocal += colliderMass * mWorld.mCollidersComponents.mLocalToBodyTransforms[colliderIndex].getPosition(); } if (totalMass > decimal(0.0)) { @@ -363,19 +363,19 @@ void RigidBody::computeMassAndInertiaTensorLocal(Vector3& inertiaTensorLocal, de const List& colliderEntities = mWorld.mCollisionBodyComponents.getColliders(mEntity); for (uint i=0; i < colliderEntities.size(); i++) { - Collider* collider = mWorld.mCollidersComponents.getCollider(colliderEntities[i]); + const uint colliderIndex = mWorld.mCollidersComponents.getEntityIndex(colliderEntities[i]); - const decimal colliderVolume = mWorld.mCollidersComponents.getCollisionShape(colliderEntities[i])->getVolume(); - const decimal colliderMassDensity = collider->getMaterial().getMassDensity(); + const decimal colliderVolume = mWorld.mCollidersComponents.mCollisionShapes[colliderIndex]->getVolume(); + const decimal colliderMassDensity = mWorld.mCollidersComponents.mMaterials[colliderIndex].getMassDensity(); const decimal colliderMass = colliderVolume * colliderMassDensity; totalMass += colliderMass; // Get the inertia tensor of the collider in its local-space - Vector3 shapeLocalInertiaTensor = collider->getCollisionShape()->getLocalInertiaTensor(colliderMass); + Vector3 shapeLocalInertiaTensor = mWorld.mCollidersComponents.mCollisionShapes[colliderIndex]->getLocalInertiaTensor(colliderMass); // Convert the collider inertia tensor into the local-space of the body - const Transform& shapeTransform = collider->getLocalToBodyTransform(); + const Transform& shapeTransform = mWorld.mCollidersComponents.mLocalToBodyTransforms[colliderIndex]; Matrix3x3 rotationMatrix = shapeTransform.getOrientation().getMatrix(); Matrix3x3 rotationMatrixTranspose = rotationMatrix.getTranspose(); rotationMatrixTranspose[0] *= shapeLocalInertiaTensor.x; @@ -437,10 +437,11 @@ void RigidBody::updateMassFromColliders() { // Compute the total mass of the body const List& colliderEntities = mWorld.mCollisionBodyComponents.getColliders(mEntity); for (uint i=0; i < colliderEntities.size(); i++) { - Collider* collider = mWorld.mCollidersComponents.getCollider(colliderEntities[i]); - const decimal colliderVolume = mWorld.mCollidersComponents.getCollisionShape(colliderEntities[i])->getVolume(); - const decimal colliderMassDensity = collider->getMaterial().getMassDensity(); + const uint colliderIndex = mWorld.mCollidersComponents.getEntityIndex(colliderEntities[i]); + + const decimal colliderVolume = mWorld.mCollidersComponents.mCollisionShapes[colliderIndex]->getVolume(); + const decimal colliderMassDensity = mWorld.mCollidersComponents.mMaterials[colliderIndex].getMassDensity(); const decimal colliderMass = colliderVolume * colliderMassDensity; @@ -589,8 +590,9 @@ Collider* RigidBody::addCollider(CollisionShape* collisionShape, const Transform // TODO : Maybe this method can directly returns an AABB collisionShape->getLocalBounds(localBoundsMin, localBoundsMax); const Transform localToWorldTransform = mWorld.mTransformComponents.getTransform(mEntity) * transform; + Material material(mWorld.mConfig.defaultFrictionCoefficient, mWorld.mConfig.defaultRollingRestistance, mWorld.mConfig.defaultBounciness); ColliderComponents::ColliderComponent colliderComponent(mEntity, collider, AABB(localBoundsMin, localBoundsMax), - transform, collisionShape, 0x0001, 0xFFFF, localToWorldTransform); + transform, collisionShape, 0x0001, 0xFFFF, localToWorldTransform, material); bool isSleeping = mWorld.mRigidBodyComponents.getIsSleeping(mEntity); mWorld.mCollidersComponents.addComponent(colliderEntity, isSleeping, colliderComponent); diff --git a/src/collision/Collider.cpp b/src/collision/Collider.cpp index 1ae22bdb..a5948ce1 100644 --- a/src/collision/Collider.cpp +++ b/src/collision/Collider.cpp @@ -42,8 +42,7 @@ using namespace reactphysics3d; */ Collider::Collider(Entity entity, CollisionBody* body, MemoryManager& memoryManager) :mMemoryManager(memoryManager), mEntity(entity), mBody(body), - mMaterial(body->mWorld.mConfig.defaultFrictionCoefficient, body->mWorld.mConfig.defaultRollingRestistance, - body->mWorld.mConfig.defaultBounciness), mUserData(nullptr) { + mUserData(nullptr) { } @@ -223,10 +222,10 @@ void Collider::setHasCollisionShapeChangedSize(bool hasCollisionShapeChangedSize */ void Collider::setMaterial(const Material& material) { - mMaterial = material; + mBody->mWorld.mCollidersComponents.setMaterial(mEntity, material); RP3D_LOG(mBody->mWorld.mConfig.worldName, Logger::Level::Information, Logger::Category::Collider, - "Collider " + std::to_string(mEntity.id) + ": Set Material" + mMaterial.to_string(), __FILE__, __LINE__); + "Collider " + std::to_string(mEntity.id) + ": Set Material" + material.to_string(), __FILE__, __LINE__); } // Return the local to world transform @@ -254,6 +253,14 @@ void Collider::setIsTrigger(bool isTrigger) const { mBody->mWorld.mCollidersComponents.setIsTrigger(mEntity, isTrigger); } +// Return a reference to the material properties of the collider +/** + * @return A reference to the material of the body + */ +Material& Collider::getMaterial() { + return mBody->mWorld.mCollidersComponents.getMaterial(mEntity); +} + #ifdef IS_RP3D_PROFILING_ENABLED diff --git a/src/components/ColliderComponents.cpp b/src/components/ColliderComponents.cpp index 3c296e58..550acd2f 100644 --- a/src/components/ColliderComponents.cpp +++ b/src/components/ColliderComponents.cpp @@ -38,7 +38,7 @@ ColliderComponents::ColliderComponents(MemoryAllocator& allocator) :Components(allocator, sizeof(Entity) + sizeof(Entity) + sizeof(Collider*) + sizeof(int32) + sizeof(Transform) + sizeof(CollisionShape*) + sizeof(unsigned short) + sizeof(unsigned short) + sizeof(Transform) + sizeof(List) + sizeof(bool) + - sizeof(bool)) { + sizeof(bool) + sizeof(Material)) { // Allocate memory for the components data allocate(INIT_NB_ALLOCATED_COMPONENTS); @@ -69,6 +69,7 @@ void ColliderComponents::allocate(uint32 nbComponentsToAllocate) { List* newOverlappingPairs = reinterpret_cast*>(newLocalToWorldTransforms + nbComponentsToAllocate); bool* hasCollisionShapeChangedSize = reinterpret_cast(newOverlappingPairs + nbComponentsToAllocate); bool* isTrigger = reinterpret_cast(hasCollisionShapeChangedSize + nbComponentsToAllocate); + Material* materials = reinterpret_cast(isTrigger + nbComponentsToAllocate); // If there was already components before if (mNbComponents > 0) { @@ -86,6 +87,7 @@ void ColliderComponents::allocate(uint32 nbComponentsToAllocate) { memcpy(newOverlappingPairs, mOverlappingPairs, mNbComponents * sizeof(List)); memcpy(hasCollisionShapeChangedSize, mHasCollisionShapeChangedSize, mNbComponents * sizeof(bool)); memcpy(isTrigger, mIsTrigger, mNbComponents * sizeof(bool)); + memcpy(materials, mMaterials, mNbComponents * sizeof(Material)); // Deallocate previous memory mMemoryAllocator.release(mBuffer, mNbAllocatedComponents * mComponentDataSize); @@ -105,6 +107,7 @@ void ColliderComponents::allocate(uint32 nbComponentsToAllocate) { mOverlappingPairs = newOverlappingPairs; mHasCollisionShapeChangedSize = hasCollisionShapeChangedSize; mIsTrigger = isTrigger; + mMaterials = materials; mNbAllocatedComponents = nbComponentsToAllocate; } @@ -128,6 +131,7 @@ void ColliderComponents::addComponent(Entity colliderEntity, bool isSleeping, co new (mOverlappingPairs + index) List(mMemoryAllocator); mHasCollisionShapeChangedSize[index] = false; mIsTrigger[index] = false; + mMaterials[index] = component.material; // Map the entity with the new component lookup index mMapEntityToComponentIndex.add(Pair(colliderEntity, index)); @@ -156,6 +160,7 @@ void ColliderComponents::moveComponentToIndex(uint32 srcIndex, uint32 destIndex) new (mOverlappingPairs + destIndex) List(mOverlappingPairs[srcIndex]); mHasCollisionShapeChangedSize[destIndex] = mHasCollisionShapeChangedSize[srcIndex]; mIsTrigger[destIndex] = mIsTrigger[srcIndex]; + mMaterials[destIndex] = mMaterials[srcIndex]; // Destroy the source component destroyComponent(srcIndex); @@ -184,6 +189,7 @@ void ColliderComponents::swapComponents(uint32 index1, uint32 index2) { List overlappingPairs = mOverlappingPairs[index1]; bool hasCollisionShapeChangedSize = mHasCollisionShapeChangedSize[index1]; bool isTrigger = mIsTrigger[index1]; + Material material = mMaterials[index1]; // Destroy component 1 destroyComponent(index1); @@ -203,6 +209,7 @@ void ColliderComponents::swapComponents(uint32 index1, uint32 index2) { new (mOverlappingPairs + index2) List(overlappingPairs); mHasCollisionShapeChangedSize[index2] = hasCollisionShapeChangedSize; mIsTrigger[index2] = isTrigger; + mMaterials[index2] = material; // Update the entity to component index mapping mMapEntityToComponentIndex.add(Pair(colliderEntity1, index2)); @@ -228,4 +235,5 @@ void ColliderComponents::destroyComponent(uint32 index) { mCollisionShapes[index] = nullptr; mLocalToWorldTransforms[index].~Transform(); mOverlappingPairs[index].~List(); + mMaterials[index].~Material(); } diff --git a/src/systems/ContactSolverSystem.cpp b/src/systems/ContactSolverSystem.cpp index 27f74cf6..cfa69713 100644 --- a/src/systems/ContactSolverSystem.cpp +++ b/src/systems/ContactSolverSystem.cpp @@ -34,6 +34,7 @@ #include #include #include +#include using namespace reactphysics3d; using namespace std; @@ -70,8 +71,8 @@ void ContactSolverSystem::init(List* contactManifolds, Listsize(); - uint nbContactPoints = mAllContactPoints->size(); + const uint nbContactManifolds = mAllContactManifolds->size(); + const uint nbContactPoints = mAllContactPoints->size(); mNbContactManifolds = 0; mNbContactPoints = 0; @@ -135,9 +136,8 @@ void ContactSolverSystem::initializeForIsland(uint islandIndex) { 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); + const uint collider1Index = mColliderComponents.getEntityIndex(externalManifold.colliderEntity1); + const uint collider2Index = mColliderComponents.getEntityIndex(externalManifold.colliderEntity2); // Get the position of the two bodies const Vector3& x1 = mRigidBodyComponents.mCentersOfMassWorld[rigidBodyIndex1]; @@ -152,10 +152,8 @@ 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].frictionCoefficient = computeMixedFrictionCoefficient(mColliderComponents.mMaterials[collider1Index], mColliderComponents.mMaterials[collider2Index]); + mContactConstraints[mNbContactManifolds].rollingResistanceFactor = computeMixedRollingResistance(mColliderComponents.mMaterials[collider1Index], mColliderComponents.mMaterials[collider2Index]); mContactConstraints[mNbContactManifolds].externalContactManifold = &externalManifold; mContactConstraints[mNbContactManifolds].normal.setToZero(); mContactConstraints[mNbContactManifolds].frictionPointBody1.setToZero(); @@ -167,9 +165,8 @@ 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); + const Transform& collider1LocalToWorldTransform = mColliderComponents.mLocalToWorldTransforms[collider1Index]; + const Transform& collider2LocalToWorldTransform = mColliderComponents.mLocalToWorldTransforms[collider2Index]; // For each contact point of the contact manifold assert(externalManifold.nbContactPoints > 0); @@ -179,13 +176,14 @@ void ContactSolverSystem::initializeForIsland(uint islandIndex) { ContactPoint& externalContact = (*mAllContactPoints)[c]; + new (mContactPoints + mNbContactPoints) ContactPointSolver(); + mContactPoints[mNbContactPoints].externalContact = &externalContact; + mContactPoints[mNbContactPoints].normal = externalContact.getNormal(); + // Get the contact point on the two bodies const Vector3 p1 = collider1LocalToWorldTransform * externalContact.getLocalPointOnShape1(); const Vector3 p2 = collider2LocalToWorldTransform * externalContact.getLocalPointOnShape2(); - new (mContactPoints + mNbContactPoints) ContactPointSolver(); - mContactPoints[mNbContactPoints].externalContact = &externalContact; - mContactPoints[mNbContactPoints].normal = externalContact.getNormal(); mContactPoints[mNbContactPoints].r1.x = p1.x - x1.x; mContactPoints[mNbContactPoints].r1.y = p1.y - x1.y; mContactPoints[mNbContactPoints].r1.z = p1.z - x1.z; @@ -247,7 +245,7 @@ void ContactSolverSystem::initializeForIsland(uint islandIndex) { decimal deltaVDotN = deltaV.x * mContactPoints[mNbContactPoints].normal.x + deltaV.y * mContactPoints[mNbContactPoints].normal.y + deltaV.z * mContactPoints[mNbContactPoints].normal.z; - const decimal restitutionFactor = computeMixedRestitutionFactor(collider1, collider2); + const decimal restitutionFactor = computeMixedRestitutionFactor(mColliderComponents.mMaterials[collider1Index], mColliderComponents.mMaterials[collider2Index]); if (deltaVDotN < -mRestitutionVelocityThreshold) { mContactPoints[mNbContactPoints].restitutionBias = restitutionFactor * deltaVDotN; } @@ -259,8 +257,8 @@ void ContactSolverSystem::initializeForIsland(uint islandIndex) { mNbContactPoints++; } - mContactConstraints[mNbContactManifolds].frictionPointBody1 /=static_cast(mContactConstraints[mNbContactManifolds].nbContacts); - mContactConstraints[mNbContactManifolds].frictionPointBody2 /=static_cast(mContactConstraints[mNbContactManifolds].nbContacts); + mContactConstraints[mNbContactManifolds].frictionPointBody1 /= static_cast(mContactConstraints[mNbContactManifolds].nbContacts); + mContactConstraints[mNbContactManifolds].frictionPointBody2 /= static_cast(mContactConstraints[mNbContactManifolds].nbContacts); mContactConstraints[mNbContactManifolds].r1Friction.x = mContactConstraints[mNbContactManifolds].frictionPointBody1.x - x1.x; mContactConstraints[mNbContactManifolds].r1Friction.y = mContactConstraints[mNbContactManifolds].frictionPointBody1.y - x1.y; mContactConstraints[mNbContactManifolds].r1Friction.z = mContactConstraints[mNbContactManifolds].frictionPointBody1.z - x1.z; @@ -547,7 +545,7 @@ void ContactSolverSystem::solve() { // Compute the bias "b" of the constraint decimal biasPenetrationDepth = 0.0; if (mContactPoints[contactPointIndex].penetrationDepth > SLOP) biasPenetrationDepth = -(beta/mTimeStep) * - max(0.0f, float(mContactPoints[contactPointIndex].penetrationDepth - SLOP)); + std::max(0.0f, float(mContactPoints[contactPointIndex].penetrationDepth - SLOP)); decimal b = biasPenetrationDepth + mContactPoints[contactPointIndex].restitutionBias; // Compute the Lagrange multiplier lambda @@ -814,27 +812,6 @@ void ContactSolverSystem::solve() { } } -// Compute the collision restitution factor from the restitution factor of each collider -decimal ContactSolverSystem::computeMixedRestitutionFactor(Collider* collider1, Collider* collider2) const { - decimal restitution1 = collider1->getMaterial().getBounciness(); - decimal restitution2 = collider2->getMaterial().getBounciness(); - - // Return the largest restitution factor - return (restitution1 > restitution2) ? restitution1 : restitution2; -} - -// Compute the mixed friction coefficient from the friction coefficient of each collider -decimal ContactSolverSystem::computeMixedFrictionCoefficient(Collider* collider1, Collider* collider2) const { - - // Use the geometric mean to compute the mixed friction coefficient - return collider1->getMaterial().getFrictionCoefficientSqrt() * collider2->getMaterial().getFrictionCoefficientSqrt(); -} - -// Compute th mixed rolling resistance factor between two colliders -decimal ContactSolverSystem::computeMixedRollingResistance(Collider* collider1, Collider* collider2) const { - return decimal(0.5f) * (collider1->getMaterial().getRollingResistance() + collider2->getMaterial().getRollingResistance()); -} - // Store the computed impulses to use them to // warm start the solver at the next iteration void ContactSolverSystem::storeImpulses() { @@ -871,14 +848,14 @@ void ContactSolverSystem::computeFrictionVectors(const Vector3& deltaVelocity, C assert(contact.normal.length() > decimal(0.0)); // Compute the velocity difference vector in the tangential plane - Vector3 normalVelocity(deltaVelocity.x * contact.normal.x * contact.normal.x, + const Vector3 normalVelocity(deltaVelocity.x * contact.normal.x * contact.normal.x, deltaVelocity.y * contact.normal.y * contact.normal.y, deltaVelocity.z * contact.normal.z * contact.normal.z); - Vector3 tangentVelocity(deltaVelocity.x - normalVelocity.x, deltaVelocity.y - normalVelocity.y, + const Vector3 tangentVelocity(deltaVelocity.x - normalVelocity.x, deltaVelocity.y - normalVelocity.y, deltaVelocity.z - normalVelocity.z); // If the velocty difference in the tangential plane is not zero - decimal lengthTangentVelocity = tangentVelocity.length(); + const decimal lengthTangentVelocity = tangentVelocity.length(); if (lengthTangentVelocity > MACHINE_EPSILON) { // Compute the first friction vector in the direction of the tangent @@ -893,5 +870,5 @@ void ContactSolverSystem::computeFrictionVectors(const Vector3& deltaVelocity, C // The second friction vector is computed by the cross product of the first // friction vector and the contact normal - contact.frictionVector2 = contact.normal.cross(contact.frictionVector1).getUnit(); + contact.frictionVector2 = contact.normal.cross(contact.frictionVector1); }