Merge master into optimization and fix conflicts

documentation/UserManual/ReactPhysics3D-UserManual.tex

@@ -1201,7 +1201,7 @@ indices[16]=2; indices[17]=3; indices[18]=7; indices[19]=6;
 indices[20]=0; indices[21]=4; indices[22]=7; indices[23]=3;
 
 // Description of the six faces of the convex mesh
-polygonFaces = new PolygonVertexArray::PolygonFace[6];
+PolygonVertexArray::PolygonFace* polygonFaces = new PolygonVertexArray::PolygonFace[6];
 PolygonVertexArray::PolygonFace* face = polygonFaces;
 for (int f = 0; f < 6; f++) {
 
@@ -1215,7 +1215,7 @@ for (int f = 0; f < 6; f++) {
 }
 
 // Create the polygon vertex array
-PolygonaVertexArray* polygonVertexArray = new PolygonVertexArray(8, vertices, 3 x sizeof(float),
+PolygonVertexArray* polygonVertexArray = new PolygonVertexArray(8, vertices, 3 x sizeof(float),
 indices, sizeof(int), 6, polygonFaces,
 PolygonVertexArray::VertexDataType::VERTEX_FLOAT_TYPE,
 PolygonVertexArray::IndexDataType::INDEX_INTEGER_TYPE);

include/reactphysics3d/engine/PhysicsWorld.h

@@ -230,6 +230,11 @@ class PhysicsWorld {
         /// All the islands of bodies of the current frame
         Islands mIslands;
 
+        /// Order in which to process the ContactPairs for contact creation such that
+        /// all the contact manifolds and contact points of a given island are packed together
+        /// This array contains the indices of the ContactPairs.
+        List<uint32> mProcessContactPairsOrderIslands;
+
         /// Contact solver system
         ContactSolverSystem mContactSolverSystem;
 
@@ -388,7 +393,7 @@ class PhysicsWorld {
         Vector3 getGravity() const;
 
         /// Set the gravity vector of the world
-        void setGravity(Vector3& gravity);
+        void setGravity(const Vector3& gravity);
 
         /// Return if the gravity is on
         bool isGravityEnabled() const;

include/reactphysics3d/systems/CollisionDetectionSystem.h

@@ -230,6 +230,9 @@ class CollisionDetectionSystem {
         /// Create the actual contact manifolds and contacts points (from potential contacts) for a given contact pair
         void createContacts();
 
+        /// Add the contact pairs to the corresponding bodies
+        void addContactPairsToBodies();
+
         /// Compute the map from contact pairs ids to contact pair for the next frame
         void computeMapPreviousContactPairs();
 

src/engine/PhysicsWorld.cpp

@@ -59,7 +59,7 @@ PhysicsWorld::PhysicsWorld(MemoryManager& memoryManager, const WorldSettings& wo
                 mSliderJointsComponents(mMemoryManager.getHeapAllocator()), mCollisionDetection(this, mCollidersComponents, mTransformComponents, mCollisionBodyComponents, mRigidBodyComponents,
                                         mMemoryManager),
                 mCollisionBodies(mMemoryManager.getHeapAllocator()), mEventListener(nullptr),
-                mName(worldSettings.worldName),  mIslands(mMemoryManager.getSingleFrameAllocator()),
+                mName(worldSettings.worldName),  mIslands(mMemoryManager.getSingleFrameAllocator()), mProcessContactPairsOrderIslands(mMemoryManager.getSingleFrameAllocator()),
                 mContactSolverSystem(mMemoryManager, *this, mIslands, mCollisionBodyComponents, mRigidBodyComponents,
                                mCollidersComponents, mConfig.restitutionVelocityThreshold),
                 mConstraintSolverSystem(*this, mIslands, mRigidBodyComponents, mTransformComponents, mJointsComponents,
@@ -338,6 +338,9 @@ void PhysicsWorld::update(decimal timeStep) {
     // Create the islands
     createIslands();
 
+    // Create the actual narrow-phase contacts
+    mCollisionDetection.createContacts();
+
     // Report the contacts to the user
     mCollisionDetection.reportContactsAndTriggers();
 
@@ -373,6 +376,8 @@ void PhysicsWorld::update(decimal timeStep) {
     // Reset the islands
     mIslands.clear();
 
+    mProcessContactPairsOrderIslands.clear(true);
+
     // Generate debug rendering primitives (if enabled)
     if (mIsDebugRenderingEnabled) {
         mDebugRenderer.computeDebugRenderingPrimitives(*this);
@@ -755,6 +760,8 @@ void PhysicsWorld::createIslands() {
 
     RP3D_PROFILE("PhysicsWorld::createIslands()", mProfiler);
 
+    assert(mProcessContactPairsOrderIslands.size() == 0);
+
     // Reset all the isAlreadyInIsland variables of bodies and joints
     for (uint b=0; b < mRigidBodyComponents.getNbComponents(); b++) {
 
@@ -824,7 +831,8 @@ void PhysicsWorld::createIslands() {
             // For each contact pair in which the current body is involded
             for (uint32 p=0; p < mRigidBodyComponents.mContactPairs[bodyToVisitIndex].size(); p++) {
 
-                ContactPair& pair = (*mCollisionDetection.mCurrentContactPairs)[mRigidBodyComponents.mContactPairs[bodyToVisitIndex][p]];
+                const uint32 contactPairIndex = mRigidBodyComponents.mContactPairs[bodyToVisitIndex][p];
+                ContactPair& pair = (*mCollisionDetection.mCurrentContactPairs)[contactPairIndex];
 
                 // Check if the current contact pair has already been added into an island
                 if (pair.isAlreadyInIsland) continue;
@@ -836,6 +844,8 @@ void PhysicsWorld::createIslands() {
                 if (mRigidBodyComponents.hasComponentGetIndex(otherBodyEntity, otherBodyIndex)
                     && !mCollidersComponents.getIsTrigger(pair.collider1Entity) && !mCollidersComponents.getIsTrigger(pair.collider2Entity)) {
 
+                    mProcessContactPairsOrderIslands.add(contactPairIndex);
+
                     assert(pair.nbPotentialContactManifolds > 0);
                     nbTotalManifolds += pair.nbPotentialContactManifolds;
 
@@ -999,7 +1009,7 @@ void PhysicsWorld::setNbIterationsPositionSolver(uint nbIterations) {
 /**
  * @param gravity The gravity vector (in meter per seconds squared)
  */
-void PhysicsWorld::setGravity(Vector3& gravity) {
+void PhysicsWorld::setGravity(const Vector3& gravity) {
 
     mConfig.gravity = gravity;
 

src/mathematics/Quaternion.cpp

@@ -106,7 +106,7 @@ Quaternion::Quaternion(const Matrix3x3& matrix) {
             // Compute the quaternion
             x = decimal(0.5) * r;
             y = (matrix[0][1] + matrix[1][0]) * s;
-            z = (matrix[2][0] - matrix[0][2]) * s;
+            z = (matrix[2][0] + matrix[0][2]) * s;
             w = (matrix[2][1] - matrix[1][2]) * s;
         }
     }

src/systems/CollisionDetectionSystem.cpp

@@ -608,19 +608,32 @@ void CollisionDetectionSystem::computeNarrowPhase() {
     // Reduce the number of contact points in the manifolds
     reducePotentialContactManifolds(mCurrentContactPairs, mPotentialContactManifolds, mPotentialContactPoints);
 
+    // Add the contact pairs to the bodies
+    addContactPairsToBodies();
+
     assert(mCurrentContactManifolds->size() == 0);
     assert(mCurrentContactPoints->size() == 0);
-
-    // Create the actual narrow-phase contacts
-    createContacts();
-
-    // Compute the map from contact pairs ids to contact pair for the next frame
-    computeMapPreviousContactPairs();
-
-    mNarrowPhaseInput.clear();
 }
 
-/// Compute the map from contact pairs ids to contact pair for the next frame
+// Add the contact pairs to the corresponding bodies
+void CollisionDetectionSystem::addContactPairsToBodies() {
+
+    const uint32 nbContactPairs = mCurrentContactPairs->size();
+    for (uint32 p=0 ; p < nbContactPairs; p++) {
+
+        ContactPair& contactPair = (*mCurrentContactPairs)[p];
+
+        // Add the associated contact pair to both bodies of the pair (used to create islands later)
+        if (mRigidBodyComponents.hasComponent(contactPair.body1Entity)) {
+           mRigidBodyComponents.addContacPair(contactPair.body1Entity, p);
+        }
+        if (mRigidBodyComponents.hasComponent(contactPair.body2Entity)) {
+           mRigidBodyComponents.addContacPair(contactPair.body2Entity, p);
+        }
+    }
+}
+
+// Compute the map from contact pairs ids to contact pair for the next frame
 void CollisionDetectionSystem::computeMapPreviousContactPairs() {
 
     mPreviousMapPairIdToContactPairIndex.clear();
@@ -798,24 +811,34 @@ void CollisionDetectionSystem::createContacts() {
     mCurrentContactManifolds->reserve(mCurrentContactPairs->size());
     mCurrentContactPoints->reserve(mCurrentContactManifolds->size());
 
-    // For each contact pair
+    // We go through all the contact pairs and add the pairs with a least a CollisionBody at the end of the
-    const uint nbCurrentContactPairs = (*mCurrentContactPairs).size();
+    // mProcessContactPairsOrderIslands array because those pairs have not been added during the islands
-    for (uint p=0; p < nbCurrentContactPairs; p++) {
+    // creation (only the pairs with two RigidBodies are added during island creation)
+    Set<uint32> processedContactPairsIndices(mMemoryManager.getSingleFrameAllocator(), mCurrentContactPairs->size());
+    for (uint32 i=0; i < mWorld->mProcessContactPairsOrderIslands.size(); i++) {
+        processedContactPairsIndices.add(mWorld->mProcessContactPairsOrderIslands[i]);
+    }
+    for (uint32 i=0; i < mCurrentContactPairs->size(); i++) {
+        if (!processedContactPairsIndices.contains(i)) {
+            mWorld->mProcessContactPairsOrderIslands.add(i);
+        }
+    }
 
-        ContactPair& contactPair = (*mCurrentContactPairs)[p];
+    assert(mWorld->mProcessContactPairsOrderIslands.size() == (*mCurrentContactPairs).size());
+
+    // Process the contact pairs in the order defined by the islands such that the contact manifolds and
+    // contact points of a given island are packed together in the array of manifolds and contact points
+    uint32 nbContactPairsToProcess = mWorld->mProcessContactPairsOrderIslands.size();
+    for (uint p=0; p < nbContactPairsToProcess; p++) {
+
+        uint32 contactPairIndex = mWorld->mProcessContactPairsOrderIslands[p];
+
+        ContactPair& contactPair = (*mCurrentContactPairs)[contactPairIndex];
 
         contactPair.contactManifoldsIndex = mCurrentContactManifolds->size();
         contactPair.nbContactManifolds = contactPair.nbPotentialContactManifolds;
         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)) {
-           mRigidBodyComponents.addContacPair(contactPair.body1Entity, p);
-        }
-        if (mRigidBodyComponents.hasComponent(contactPair.body2Entity)) {
-           mRigidBodyComponents.addContacPair(contactPair.body2Entity, p);
-        }
-
         // For each potential contact manifold of the pair
         for (uint m=0; m < contactPair.nbPotentialContactManifolds; m++) {
 
@@ -856,6 +879,11 @@ void CollisionDetectionSystem::createContacts() {
     // Reset the potential contacts
     mPotentialContactPoints.clear(true);
     mPotentialContactManifolds.clear(true);
+
+    // Compute the map from contact pairs ids to contact pair for the next frame
+    computeMapPreviousContactPairs();
+
+    mNarrowPhaseInput.clear();
 }
 
 // Compute the lost contact pairs (contact pairs in contact in the previous frame but not in the current one)

src/utils/DebugRenderer.cpp

@@ -463,7 +463,7 @@ void DebugRenderer::onContact(const CollisionCallback::CallbackData& callbackDat
                     if (getIsDebugItemDisplayed(DebugItem::CONTACT_POINT)) {
 
                         // Contact point
-                        drawSphere(point, DEFAULT_CONTACT_POINT_SPHERE_RADIUS, mMapDebugItemWithColor[DebugItem::CONTACT_POINT]);
+                        drawSphere(point, mContactPointSphereRadius, mMapDebugItemWithColor[DebugItem::CONTACT_POINT]);
                     }
 
                     if (getIsDebugItemDisplayed(DebugItem::CONTACT_NORMAL)) {

test/tests/mathematics/TestQuaternion.h

@@ -90,6 +90,21 @@ class TestQuaternion : public Test {
             rp3d_test(approxEqual(quaternion4.z, mQuaternion1.z));
             rp3d_test(approxEqual(quaternion4.w, mQuaternion1.w));
 
+            Matrix3x3 original(0.001743,-0.968608,0.248589,-0.614229,-0.197205,-0.764090,0.789126,-0.151359,-0.595290);
+            Matrix3x3 converted = Quaternion(original).getMatrix();
+            rp3d_test(approxEqual(original[0][0], converted[0][0], 0.0001));
+            rp3d_test(approxEqual(original[0][1], converted[0][1], 0.0001));
+            rp3d_test(approxEqual(original[0][2], converted[0][2], 0.0001));
+            rp3d_test(approxEqual(original[1][0], converted[1][0], 0.0001));
+            rp3d_test(approxEqual(original[1][1], converted[1][1], 0.0001));
+            rp3d_test(approxEqual(original[1][2], converted[1][2], 0.0001));
+            rp3d_test(approxEqual(original[2][0], converted[2][0], 0.0001));
+            rp3d_test(approxEqual(original[2][1], converted[2][1], 0.0001));
+            rp3d_test(approxEqual(original[2][2], converted[2][2], 0.0001));
+
+            std::cout << original.to_string() << std::endl;
+            std::cout << converted.to_string() << std::endl;
+
             // Test conversion from Euler angles to quaternion
 
             const decimal PI_OVER_2 = PI_RP3D * decimal(0.5);