Back to previous contact solver

src/engine/ContactSolver.h

@@ -31,7 +31,6 @@
 #include "configuration.h"
 #include "constraint/Joint.h"
 #include "collision/ContactManifold.h"
-#include "memory/SingleFrameAllocator.h"
 #include "Island.h"
 #include "Impulse.h"
 #include <map>
@@ -40,6 +39,7 @@
 /// ReactPhysics3D namespace
 namespace reactphysics3d {
 
+
 // Class Contact Solver
 /**
  * This class represents the contact solver that is used to solve rigid bodies contacts.
@@ -113,100 +113,30 @@ class ContactSolver {
 
     private:
 
-        struct PenetrationConstraint {
+        // Structure ContactPointSolver
-
+        /**
-            /// Index of body 1 in the constraint solver
+         * Contact solver internal data structure that to store all the
-            uint indexBody1;
+         * information relative to a contact point
-
+         */
-            /// Index of body 2 in the constraint solver
+        struct ContactPointSolver {
-            uint indexBody2;
-
-            /// Normal vector of the contact
-            Vector3 normal;
-
-            /// Vector from the body 1 center to the contact point
-            Vector3 r1;
-
-            /// Vector from the body 2 center to the contact point
-            Vector3 r2;
-
-            /// Cross product of r1 with the contact normal
-            Vector3 r1CrossN;
-
-            /// Cross product of r2 with the contact normal
-            Vector3 r2CrossN;
-
-            /// Penetration depth
-            decimal penetrationDepth;
-
-            /// Velocity restitution bias
-            decimal restitutionBias;
 
             /// Accumulated normal impulse
             decimal penetrationImpulse;
 
-            /// Accumulated split impulse for penetration correction
-            decimal penetrationSplitImpulse;
-
-            /// Inverse of the mass of body 1
-            decimal massInverseBody1;
-
-            /// Inverse of the mass of body 2
-            decimal massInverseBody2;
-
-            /// Inverse of the matrix K for the penenetration
-            decimal inversePenetrationMass;
-
-            /// Inverse inertia tensor of body 1
-            Matrix3x3 inverseInertiaTensorBody1;
-
-            /// Inverse inertia tensor of body 2
-            Matrix3x3 inverseInertiaTensorBody2;
-
-            /// Index of the corresponding friction constraint
-            uint indexFrictionConstraint;
-
-            /// Pointer to the corresponding contact point
-            ContactPoint* contactPoint;
-
-            /// True if this constraint is for a resting contact
-            bool isRestingContact;
-        };
-
-        struct FrictionConstraint {
-
-            /// Index of body 1 in the constraint solver
-            uint indexBody1;
-
-            /// Index of body 2 in the constraint solver
-            uint indexBody2;
-
-            /// R1 vector for the friction constraints
-            Vector3 r1Friction;
-
-            /// R2 vector for the friction constraints
-            Vector3 r2Friction;
-
-            /// Average normal vector of the contact manifold
-            Vector3 normal;
-
             /// Accumulated impulse in the 1st friction direction
             decimal friction1Impulse;
 
             /// Accumulated impulse in the 2nd friction direction
             decimal friction2Impulse;
 
-            /// Twist friction impulse at contact manifold center
+            /// Accumulated split impulse for penetration correction
-            decimal frictionTwistImpulse;
+            decimal penetrationSplitImpulse;
 
             /// Accumulated rolling resistance impulse
             Vector3 rollingResistanceImpulse;
 
-            /// Rolling resistance factor between the two bodies
+            /// Normal vector of the contact
-            decimal rollingResistanceFactor;
+            Vector3 normal;
-
-            /// Mix friction coefficient for the two bodies
-            decimal frictionCoefficient;
 
             /// First friction vector in the tangent plane
             Vector3 frictionVector1;
@@ -214,12 +144,18 @@ class ContactSolver {
             /// Second friction vector in the tangent plane
             Vector3 frictionVector2;
 
-            /// Old 1st friction direction at contact manifold center
+            /// Old first friction vector in the tangent plane
             Vector3 oldFrictionVector1;
 
-            /// Old 2nd friction direction at contact manifold center
+            /// Old second friction vector in the tangent plane
             Vector3 oldFrictionVector2;
 
+            /// Vector from the body 1 center to the contact point
+            Vector3 r1;
+
+            /// Vector from the body 2 center to the contact point
+            Vector3 r2;
+
             /// Cross product of r1 with 1st friction vector
             Vector3 r1CrossT1;
 
@@ -232,8 +168,20 @@ class ContactSolver {
             /// Cross product of r2 with 2nd friction vector
             Vector3 r2CrossT2;
 
-            /// Total of the all the corresponding penetration impulses
+            /// Cross product of r1 with the contact normal
-            decimal totalPenetrationImpulse;
+            Vector3 r1CrossN;
+
+            /// Cross product of r2 with the contact normal
+            Vector3 r2CrossN;
+
+            /// Penetration depth
+            decimal penetrationDepth;
+
+            /// Velocity restitution bias
+            decimal restitutionBias;
+
+            /// Inverse of the matrix K for the penenetration
+            decimal inversePenetrationMass;
 
             /// Inverse of the matrix K for the 1st friction
             decimal inverseFriction1Mass;
@@ -241,16 +189,30 @@ class ContactSolver {
             /// Inverse of the matrix K for the 2nd friction
             decimal inverseFriction2Mass;
 
-            /// Matrix K for the twist friction constraint
+            /// True if the contact was existing last time step
-            decimal inverseTwistFrictionMass;
+            bool isRestingContact;
 
-            /// Matrix K for the rolling resistance constraint
+            /// Pointer to the external contact
-            Matrix3x3 inverseRollingResistance;
+            ContactPoint* externalContact;
+        };
+
+        // Structure ContactManifoldSolver
+        /**
+         * Contact solver internal data structure to store all the
+         * information relative to a contact manifold.
+         */
+        struct ContactManifoldSolver {
+
+            /// Index of body 1 in the constraint solver
+            uint indexBody1;
+
+            /// Index of body 2 in the constraint solver
+            uint indexBody2;
 
             /// Inverse of the mass of body 1
             decimal massInverseBody1;
 
-            /// Inverse of the mass of body 2
+            // Inverse of the mass of body 2
             decimal massInverseBody2;
 
             /// Inverse inertia tensor of body 1
@@ -259,11 +221,94 @@ class ContactSolver {
             /// Inverse inertia tensor of body 2
             Matrix3x3 inverseInertiaTensorBody2;
 
-            /// Pointer to the corresponding contact manifold
+            /// Contact point constraints
-            ContactManifold* contactManifold;
+            ContactPointSolver contacts[MAX_CONTACT_POINTS_IN_MANIFOLD];
 
-            /// True if the original contact manifold has at least one resting contact
+            /// Number of contact points
-            bool hasAtLeastOneRestingContactPoint;
+            uint nbContacts;
+
+            /// True if the body 1 is of type dynamic
+            bool isBody1DynamicType;
+
+            /// True if the body 2 is of type dynamic
+            bool isBody2DynamicType;
+
+            /// Mix of the restitution factor for two bodies
+            decimal restitutionFactor;
+
+            /// Mix friction coefficient for the two bodies
+            decimal frictionCoefficient;
+
+            /// Rolling resistance factor between the two bodies
+            decimal rollingResistanceFactor;
+
+            /// Pointer to the external contact manifold
+            ContactManifold* externalContactManifold;
+
+            // - Variables used when friction constraints are apply at the center of the manifold-//
+
+            /// Average normal vector of the contact manifold
+            Vector3 normal;
+
+            /// Point on body 1 where to apply the friction constraints
+            Vector3 frictionPointBody1;
+
+            /// Point on body 2 where to apply the friction constraints
+            Vector3 frictionPointBody2;
+
+            /// R1 vector for the friction constraints
+            Vector3 r1Friction;
+
+            /// R2 vector for the friction constraints
+            Vector3 r2Friction;
+
+            /// Cross product of r1 with 1st friction vector
+            Vector3 r1CrossT1;
+
+            /// Cross product of r1 with 2nd friction vector
+            Vector3 r1CrossT2;
+
+            /// Cross product of r2 with 1st friction vector
+            Vector3 r2CrossT1;
+
+            /// Cross product of r2 with 2nd friction vector
+            Vector3 r2CrossT2;
+
+            /// Matrix K for the first friction constraint
+            decimal inverseFriction1Mass;
+
+            /// Matrix K for the second friction constraint
+            decimal inverseFriction2Mass;
+
+            /// Matrix K for the twist friction constraint
+            decimal inverseTwistFrictionMass;
+
+            /// Matrix K for the rolling resistance constraint
+            Matrix3x3 inverseRollingResistance;
+
+            /// First friction direction at contact manifold center
+            Vector3 frictionVector1;
+
+            /// Second friction direction at contact manifold center
+            Vector3 frictionVector2;
+
+            /// Old 1st friction direction at contact manifold center
+            Vector3 oldFrictionVector1;
+
+            /// Old 2nd friction direction at contact manifold center
+            Vector3 oldFrictionVector2;
+
+            /// First friction direction impulse at manifold center
+            decimal friction1Impulse;
+
+            /// Second friction direction impulse at manifold center
+            decimal friction2Impulse;
+
+            /// Twist friction impulse at contact manifold center
+            decimal frictionTwistImpulse;
+
+            /// Rolling resistance impulse
+            Vector3 rollingResistanceImpulse;
         };
 
         // -------------------- Constants --------------------- //
@@ -285,19 +330,17 @@ class ContactSolver {
         /// Split angular velocities for the position contact solver (split impulse)
         Vector3* mSplitAngularVelocities;
 
-        /// Reference to the single frame memory allocator
-        SingleFrameAllocator& mSingleFrameAllocator;
-
         /// Current time step
         decimal mTimeStep;
 
-        PenetrationConstraint* mPenetrationConstraints;
+        /// Contact constraints
+        ContactManifoldSolver* mContactConstraints;
 
-        FrictionConstraint* mFrictionConstraints;
+        /// Number of contact constraints
+        uint mNbContactManifolds;
 
-        uint mNbPenetrationConstraints;
+        /// Single frame memory allocator
-
+        SingleFrameAllocator& mSingleFrameAllocator;
-        uint mNbFrictionConstraints;
 
         /// Array of linear velocities
         Vector3* mLinearVelocities;
@@ -320,15 +363,15 @@ class ContactSolver {
 
         // -------------------- Methods -------------------- //
 
-        /// Initialize the constraint solver for a given island
+        /// Initialize the contact constraints before solving the system
-        void initializeForIsland(Island* island);
+        void initializeContactConstraints();
 
         /// Apply an impulse to the two bodies of a constraint
-        //void applyImpulse(const Impulse& impulse, const ContactManifoldSolver& manifold);
+        void applyImpulse(const Impulse& impulse, const ContactManifoldSolver& manifold);
 
         /// Apply an impulse to the two bodies of a constraint
-        //void applySplitImpulse(const Impulse& impulse,
+        void applySplitImpulse(const Impulse& impulse,
-        //                       const ContactManifoldSolver& manifold);
+                               const ContactManifoldSolver& manifold);
 
         /// Compute the collision restitution factor from the restitution factor of each body
         decimal computeMixedRestitutionFactor(RigidBody *body1,
@@ -341,30 +384,29 @@ class ContactSolver {
         /// Compute th mixed rolling resistance factor between two bodies
         decimal computeMixedRollingResistance(RigidBody* body1, RigidBody* body2) const;
 
-        // TODO : Delete this
         /// Compute the two unit orthogonal vectors "t1" and "t2" that span the tangential friction
         /// plane for a contact point. The two vectors have to be
         /// such that : t1 x t2 = contactNormal.
-//        void computeFrictionVectors(const Vector3& deltaVelocity,
+        void computeFrictionVectors(const Vector3& deltaVelocity,
-//                                    ContactPointSolver &contactPoint) const;
+                                    ContactPointSolver &contactPoint) 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
         /// such that : t1 x t2 = contactNormal.
         void computeFrictionVectors(const Vector3& deltaVelocity,
-                                    FrictionConstraint& frictionConstraint) const;
+                                    ContactManifoldSolver& contactPoint) const;
 
         /// Compute a penetration constraint impulse
-//        const Impulse computePenetrationImpulse(decimal deltaLambda,
+        const Impulse computePenetrationImpulse(decimal deltaLambda,
-//                                                const PenetrationConstraint& constraint) const;
+                                                const ContactPointSolver& contactPoint) const;
 
         /// Compute the first friction constraint impulse
         const Impulse computeFriction1Impulse(decimal deltaLambda,
-                                              const FrictionConstraint& contactPoint) const;
+                                              const ContactPointSolver& contactPoint) const;
 
         /// Compute the second friction constraint impulse
         const Impulse computeFriction2Impulse(decimal deltaLambda,
-                                              const FrictionConstraint& contactPoint) const;
+                                              const ContactPointSolver& contactPoint) const;
 
    public:
 
@@ -372,16 +414,13 @@ class ContactSolver {
 
         /// Constructor
         ContactSolver(const std::map<RigidBody*, uint>& mapBodyToVelocityIndex,
-                      SingleFrameAllocator& singleFrameAllocator);
+                      SingleFrameAllocator& allocator);
 
         /// Destructor
         ~ContactSolver() = default;
 
-        /// Initialize the contact constraints
+        /// Initialize the constraint solver for a given island
-        void init(Island** islands, uint nbIslands, decimal timeStep);
+        void initializeForIsland(decimal dt, Island* island);
-
-        /// Solve the contact constraints of one iteration of the solve
-        void solve();
 
         /// Set the split velocities arrays
         void setSplitVelocitiesArrays(Vector3* splitLinearVelocities,
@@ -399,16 +438,7 @@ class ContactSolver {
         void storeImpulses();
 
         /// Solve the contacts
-        //void solve();
+        void solve();
-
-        /// Reset the total penetration impulse of friction constraints
-        void resetTotalPenetrationImpulse();
-
-        /// Solve the penetration constraints
-        void solvePenetrationConstraints();
-
-        /// Solve the friction constraints
-        void solveFrictionConstraints();
 
         /// Return true if the split impulses position correction technique is used for contacts
         bool isSplitImpulseActive() const;
@@ -420,8 +450,8 @@ class ContactSolver {
         /// the contact manifold instead of solving them at each contact point
         void setIsSolveFrictionAtContactManifoldCenterActive(bool isActive);
 
-        /// Return true if warmstarting is active
+        /// Clean up the constraint solver
-        bool IsWarmStartingActive() const;
+        void cleanup();
 };
 
 // Set the split velocities arrays
@@ -476,7 +506,7 @@ inline decimal ContactSolver::computeMixedRestitutionFactor(RigidBody* body1,
 inline decimal ContactSolver::computeMixedFrictionCoefficient(RigidBody *body1,
                                                               RigidBody *body2) const {
     // Use the geometric mean to compute the mixed friction coefficient
-    return std::sqrt(body1->getMaterial().getFrictionCoefficient() *
+    return sqrt(body1->getMaterial().getFrictionCoefficient() *
                 body2->getMaterial().getFrictionCoefficient());
 }
 
@@ -487,16 +517,16 @@ inline decimal ContactSolver::computeMixedRollingResistance(RigidBody* body1,
 }
 
 // Compute a penetration constraint impulse
-//inline const Impulse ContactSolver::computePenetrationImpulse(decimal deltaLambda,
+inline const Impulse ContactSolver::computePenetrationImpulse(decimal deltaLambda,
-//                                                          const PenetrationConstraint& constraint)
+                                                          const ContactPointSolver& contactPoint)
-//                                                          const {
+                                                          const {
-//    return Impulse(-constraint.normal * deltaLambda, -constraint.r1CrossN * deltaLambda,
+    return Impulse(-contactPoint.normal * deltaLambda, -contactPoint.r1CrossN * deltaLambda,
-//                   constraint.normal * deltaLambda, constraint.r2CrossN * deltaLambda);
+                   contactPoint.normal * deltaLambda, contactPoint.r2CrossN * deltaLambda);
-//}
+}
 
 // Compute the first friction constraint impulse
 inline const Impulse ContactSolver::computeFriction1Impulse(decimal deltaLambda,
-                                                        const FrictionConstraint& contactPoint)
+                                                        const ContactPointSolver& contactPoint)
                                                         const {
     return Impulse(-contactPoint.frictionVector1 * deltaLambda,
                    -contactPoint.r1CrossT1 * deltaLambda,
@@ -506,7 +536,7 @@ inline const Impulse ContactSolver::computeFriction1Impulse(decimal deltaLambda,
 
 // Compute the second friction constraint impulse
 inline const Impulse ContactSolver::computeFriction2Impulse(decimal deltaLambda,
-                                                        const FrictionConstraint& contactPoint)
+                                                        const ContactPointSolver& contactPoint)
                                                         const {
     return Impulse(-contactPoint.frictionVector2 * deltaLambda,
                    -contactPoint.r1CrossT2 * deltaLambda,
@@ -514,11 +544,6 @@ inline const Impulse ContactSolver::computeFriction2Impulse(decimal deltaLambda,
                    contactPoint.r2CrossT2 * deltaLambda);
 }
 
-// Return true if warmstarting is active
-inline bool ContactSolver::IsWarmStartingActive() const {
-    return mIsWarmStartingActive;
-}
-
 }
 
 #endif

src/engine/DynamicsWorld.cpp

@@ -331,8 +331,6 @@ void DynamicsWorld::solveContactsAndConstraints() {
 
     PROFILE("DynamicsWorld::solveContactsAndConstraints()");
 
-    // TODO : Do not solve per island but solve every constraints at once
-
     // Set the velocities arrays
     mContactSolver.setSplitVelocitiesArrays(mSplitLinearVelocities, mSplitAngularVelocities);
     mContactSolver.setConstrainedVelocitiesArrays(mConstrainedLinearVelocities,
@@ -342,28 +340,25 @@ void DynamicsWorld::solveContactsAndConstraints() {
     mConstraintSolver.setConstrainedPositionsArrays(mConstrainedPositions,
                                                     mConstrainedOrientations);
 
-    // Initialize the contact solver
+    // ---------- Solve velocity constraints for joints and contacts ---------- //
-    mContactSolver.init(mIslands, mNbIslands, mTimeStep);
 
     // For each island of the world
     for (uint islandIndex = 0; islandIndex < mNbIslands; islandIndex++) {
 
         // Check if there are contacts and constraints to solve
         bool isConstraintsToSolve = mIslands[islandIndex]->getNbJoints() > 0;
-        //bool isContactsToSolve = mIslands[islandIndex]->getNbContactManifolds() > 0;
+        bool isContactsToSolve = mIslands[islandIndex]->getNbContactManifolds() > 0;
-        //if (!isConstraintsToSolve && !isContactsToSolve) continue;
+        if (!isConstraintsToSolve && !isContactsToSolve) continue;
 
         // If there are contacts in the current island
-//        if (isContactsToSolve) {
+        if (isContactsToSolve) {
 
-//            // Initialize the solver
+            // Initialize the solver
-//            mContactSolver.initializeForIsland(mTimeStep, mIslands[islandIndex]);
+            mContactSolver.initializeForIsland(mTimeStep, mIslands[islandIndex]);
 
-//            // Warm start the contact solver
+            // Warm start the contact solver
-//            if (mContactSolver.IsWarmStartingActive()) {
+            mContactSolver.warmStart();
-//                mContactSolver.warmStart();
+        }
-//            }
-//        }
 
         // If there are constraints
         if (isConstraintsToSolve) {
@@ -371,40 +366,26 @@ void DynamicsWorld::solveContactsAndConstraints() {
             // Initialize the constraint solver
             mConstraintSolver.initializeForIsland(mTimeStep, mIslands[islandIndex]);
         }
-    }
 
         // For each iteration of the velocity solver
         for (uint i=0; i<mNbVelocitySolverIterations; i++) {
 
-            for (uint islandIndex = 0; islandIndex < mNbIslands; islandIndex++) {
+            // Solve the constraints
-                // Solve the constraints
+            if (isConstraintsToSolve) {
-                bool isConstraintsToSolve = mIslands[islandIndex]->getNbJoints() > 0;
+                mConstraintSolver.solveVelocityConstraints(mIslands[islandIndex]);
-                if (isConstraintsToSolve) {
-                    mConstraintSolver.solveVelocityConstraints(mIslands[islandIndex]);
-                }
             }
 
-            mContactSolver.solve();
-
             // Solve the contacts
-//            if (isContactsToSolve) {
+            if (isContactsToSolve) mContactSolver.solve();
-
-//                mContactSolver.resetTotalPenetrationImpulse();
-
-//                mContactSolver.solvePenetrationConstraints();
-//                mContactSolver.solveFrictionConstraints();
-//            }
         }
 
         // Cache the lambda values in order to use them in the next
         // step and cleanup the contact solver
-//        if (isContactsToSolve) {
+        if (isContactsToSolve) {
-//            mContactSolver.storeImpulses();
+            mContactSolver.storeImpulses();
-//            mContactSolver.cleanup();
+            mContactSolver.cleanup();
-//        }
+        }
-    //}
+    }
-
-    mContactSolver.storeImpulses();
 }
 
 // Solve the position error correction of the constraints