More materials into the ColliderComponents and more optimizations

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

include/reactphysics3d/components/ColliderComponents.h

@@ -32,6 +32,7 @@
 #include <reactphysics3d/containers/Map.h>
 #include <reactphysics3d/collision/shapes/AABB.h>
 #include <reactphysics3d/components/Components.h>
+#include <reactphysics3d/engine/Material.h>
 
 // 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

include/reactphysics3d/engine/Material.h

@@ -99,6 +99,8 @@ class Material {
         // ---------- Friendship ---------- //
 
         friend class Collider;
+        friend class CollisionBody;
+        friend class RigidBody;
 };
 
 // Return the bounciness

include/reactphysics3d/systems/ContactSolverSystem.h

@@ -30,6 +30,7 @@
 #include <reactphysics3d/configuration.h>
 #include <reactphysics3d/mathematics/Vector3.h>
 #include <reactphysics3d/mathematics/Matrix3x3.h>
+#include <reactphysics3d/engine/Material.h>
 
 /// 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,
+        decimal computeMixedRestitutionFactor(const Material& material1, const Material& material2) const;
-                                              Collider* collider2) 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

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,
+    Material material(mWorld.mConfig.defaultFrictionCoefficient, mWorld.mConfig.defaultRollingRestistance, mWorld.mConfig.defaultBounciness);
-                                                                  AABB(localBoundsMin, localBoundsMax),
+    ColliderComponents::ColliderComponent colliderComponent(mEntity, collider, AABB(localBoundsMin, localBoundsMax),
-                                                                  transform, collisionShape, 0x0001, 0xFFFF, localToWorldTransform);
+                                                                  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

src/body/RigidBody.cpp

@@ -331,15 +331,15 @@ Vector3 RigidBody::computeCenterOfMass() const {
     const List<Entity>& 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 colliderVolume = mWorld.mCollidersComponents.mCollisionShapes[colliderIndex]->getVolume();
-        const decimal colliderMassDensity = collider->getMaterial().getMassDensity();
+        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<Entity>& 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 colliderVolume = mWorld.mCollidersComponents.mCollisionShapes[colliderIndex]->getVolume();
-        const decimal colliderMassDensity = collider->getMaterial().getMassDensity();
+        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<Entity>& 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 uint colliderIndex = mWorld.mCollidersComponents.getEntityIndex(colliderEntities[i]);
-        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;
 
@@ -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);
 

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,
+            mUserData(nullptr) {
-                      body->mWorld.mConfig.defaultBounciness), 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
 
 

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<uint64>) + 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<uint64>* newOverlappingPairs = reinterpret_cast<List<uint64>*>(newLocalToWorldTransforms + nbComponentsToAllocate);
     bool* hasCollisionShapeChangedSize = reinterpret_cast<bool*>(newOverlappingPairs + nbComponentsToAllocate);
     bool* isTrigger = reinterpret_cast<bool*>(hasCollisionShapeChangedSize + nbComponentsToAllocate);
+    Material* materials = reinterpret_cast<Material*>(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<uint64>));
         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<uint64>(mMemoryAllocator);
     mHasCollisionShapeChangedSize[index] = false;
     mIsTrigger[index] = false;
+    mMaterials[index] = component.material;
 
     // Map the entity with the new component lookup index
     mMapEntityToComponentIndex.add(Pair<Entity, uint32>(colliderEntity, index));
@@ -156,6 +160,7 @@ void ColliderComponents::moveComponentToIndex(uint32 srcIndex, uint32 destIndex)
     new (mOverlappingPairs + destIndex) List<uint64>(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<uint64> 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<uint64>(overlappingPairs);
     mHasCollisionShapeChangedSize[index2] = hasCollisionShapeChangedSize;
     mIsTrigger[index2] = isTrigger;
+    mMaterials[index2] = material;
 
     // Update the entity to component index mapping
     mMapEntityToComponentIndex.add(Pair<Entity, uint32>(colliderEntity1, index2));
@@ -228,4 +235,5 @@ void ColliderComponents::destroyComponent(uint32 index) {
     mCollisionShapes[index] = nullptr;
     mLocalToWorldTransforms[index].~Transform();
     mOverlappingPairs[index].~List<uint64>();
+    mMaterials[index].~Material();
 }

src/systems/ContactSolverSystem.cpp

@@ -34,6 +34,7 @@
 #include <reactphysics3d/components/CollisionBodyComponents.h>
 #include <reactphysics3d/components/ColliderComponents.h>
 #include <reactphysics3d/collision/ContactManifold.h>
+#include <algorithm>
 
 using namespace reactphysics3d;
 using namespace std;
@@ -70,8 +71,8 @@ void ContactSolverSystem::init(List<ContactManifold>* contactManifolds, List<Con
 
     mTimeStep = timeStep;
 
-    uint nbContactManifolds = mAllContactManifolds->size();
+    const uint nbContactManifolds = mAllContactManifolds->size();
-    uint nbContactPoints = mAllContactPoints->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
+        const uint collider1Index = mColliderComponents.getEntityIndex(externalManifold.colliderEntity1);
-        Collider* collider1 = mColliderComponents.getCollider(externalManifold.colliderEntity1);
+        const uint collider2Index = mColliderComponents.getEntityIndex(externalManifold.colliderEntity2);
-        Collider* collider2 = mColliderComponents.getCollider(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(mColliderComponents.mMaterials[collider1Index], mColliderComponents.mMaterials[collider2Index]);
-        mContactConstraints[mNbContactManifolds].frictionCoefficient = computeMixedFrictionCoefficient(collider1, collider2);
+        mContactConstraints[mNbContactManifolds].rollingResistanceFactor = computeMixedRollingResistance(mColliderComponents.mMaterials[collider1Index], mColliderComponents.mMaterials[collider2Index]);
-        // TODO OPTI : Do not compute this using colliders pointers
-        mContactConstraints[mNbContactManifolds].rollingResistanceFactor = computeMixedRollingResistance(collider1, collider2);
         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.mLocalToWorldTransforms[collider1Index];
-        const Transform& collider1LocalToWorldTransform = mColliderComponents.getLocalToWorldTransform(externalManifold.colliderEntity1);
+        const Transform& collider2LocalToWorldTransform = mColliderComponents.mLocalToWorldTransforms[collider2Index];
-        const Transform& collider2LocalToWorldTransform = mColliderComponents.getLocalToWorldTransform(externalManifold.colliderEntity2);
 
         // 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<decimal>(mContactConstraints[mNbContactManifolds].nbContacts);
+        mContactConstraints[mNbContactManifolds].frictionPointBody1 /= static_cast<decimal>(mContactConstraints[mNbContactManifolds].nbContacts);
-        mContactConstraints[mNbContactManifolds].frictionPointBody2 /=static_cast<decimal>(mContactConstraints[mNbContactManifolds].nbContacts);
+        mContactConstraints[mNbContactManifolds].frictionPointBody2 /= static_cast<decimal>(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);
 }