diff --git a/src/engine/ConstraintSolver.cpp b/src/engine/ConstraintSolver.cpp index 5190d1f4..33347390 100644 --- a/src/engine/ConstraintSolver.cpp +++ b/src/engine/ConstraintSolver.cpp @@ -86,6 +86,7 @@ void ConstraintSolver::initialize() { constraint.massInverseBody1 = body1->getMassInverse(); constraint.massInverseBody2 = body2->getMassInverse(); constraint.nbContacts = contactManifold.nbContacts; + constraint.restitutionFactor = computeMixRestitutionFactor(body1, body2); // For each contact point of the contact manifold for (uint c=0; cgetFrictionVector1(); contactPointConstraint.frictionVector2 = contact->getFrictionVector2(); + contactPointConstraint.penetrationDepth = contact->getPenetrationDepth(); } mNbContactConstraints++; @@ -200,6 +202,24 @@ void ConstraintSolver::initializeContactConstraints(decimal dt) { contact.inverseFriction2Mass += constraint.massInverseBody2 + ((I2 * contact.r2CrossT2).cross(contact.r2)).dot(contact.frictionVector2); } + const Vector3& v1 = mLinearVelocities[constraint.indexBody1]; + const Vector3& w1 = mAngularVelocities[constraint.indexBody1]; + const Vector3& v2 = mLinearVelocities[constraint.indexBody2]; + const Vector3& w2 = mAngularVelocities[constraint.indexBody2]; + + // Compute the restitution velocity bias "b". We compute this here instead + // of inside the solve() method because we need to use the velocity difference + // at the beginning of the contact. Note that if it is a resting contact (normal velocity + // under a given threshold), we don't add a restitution velocity bias + contact.restitutionBias = 0.0; + Vector3 deltaV = v2 + w2.cross(contact.r2) - v1 - w1.cross(contact.r1); + decimal deltaVDotN = deltaV.dot(contact.normal); + // TODO : Use a constant here + decimal elasticLinearVelocityThreshold = 0.3; + if (deltaVDotN < -elasticLinearVelocityThreshold) { + contact.restitutionBias = constraint.restitutionFactor * deltaVDotN; + } + // Fill in the J_sp matrix realContact->computeJacobianPenetration(contact.J_spBody1Penetration, contact.J_spBody2Penetration); realContact->computeJacobianFriction1(contact.J_spBody1Friction1, contact.J_spBody2Friction1); @@ -382,14 +402,38 @@ void ConstraintSolver::solveLCP() { indexBody1Array = constraint.indexBody1; indexBody2Array = constraint.indexBody2; + const Vector3& v1 = mLinearVelocities[constraint.indexBody1]; + const Vector3& w1 = mAngularVelocities[constraint.indexBody1]; + const Vector3& v2 = mLinearVelocities[constraint.indexBody2]; + const Vector3& w2 = mAngularVelocities[constraint.indexBody2]; + // --------- Penetration --------- // + // Compute J*v + Vector3 deltaV = v2 + w2.cross(contact.r2) - v1 - w1.cross(contact.r1); + decimal deltaVDotN = deltaV.dot(contact.normal); + decimal Jv = deltaVDotN; + + // Compute the bias "b" of the constraint + decimal beta = 0.2; + // TODO : Use a constant for the slop + decimal slop = 0.005; + decimal biasPenetrationDepth = 0.0; + if (contact.penetrationDepth > slop) biasPenetrationDepth = -(beta/mTimeStep) * + max(0.0f, float(contact.penetrationDepth - slop)); + decimal b = biasPenetrationDepth + contact.restitutionBias; + + // Compute the Lagrange multiplier + deltaLambda = - (Jv + b); + + /* deltaLambda = 0.0; for (uint j=0; j<3; j++) { deltaLambda -= (contact.J_spBody1Penetration[j] * mLinearVelocities[indexBody1Array][j] + contact.J_spBody2Penetration[j] * mLinearVelocities[indexBody2Array][j]); deltaLambda -= (contact.J_spBody1Penetration[j + 3] * mAngularVelocities[indexBody1Array][j] + contact.J_spBody2Penetration[j + 3] * mAngularVelocities[indexBody2Array][j]); } - deltaLambda -= contact.b_Penetration; + */ + //deltaLambda -= contact.b_Penetration; deltaLambda /= contact.inversePenetrationMass; lambdaTemp = contact.penetrationImpulse; contact.penetrationImpulse = std::max(contact.lowerBoundPenetration, std::min(contact.penetrationImpulse + deltaLambda, contact.upperBoundPenetration)); @@ -537,3 +581,23 @@ void ConstraintSolver::solve(decimal dt) { // Cache the lambda values in order to use them in the next step cacheLambda(); } + +// Apply an impulse to the two bodies of a constraint +void ConstraintSolver::applyImpulse(const Impulse& impulse, const ContactConstraint& constraint) { + + // Update the velocities of the bodies by applying the impulse P + if (constraint.isBody1Moving) { + mLinearVelocities[constraint.indexBody1] += constraint.massInverseBody1 * + impulse.linearImpulseBody1; + mAngularVelocities[constraint.indexBody1] += constraint.inverseInertiaTensorBody1 * + impulse.angularImpulseBody1; + } + if (constraint.isBody2Moving) { + mLinearVelocities[constraint.indexBody2] += constraint.massInverseBody2 * + impulse.linearImpulseBody2; + mAngularVelocities[constraint.indexBody2] += constraint.inverseInertiaTensorBody2 * + impulse.angularImpulseBody2; + } +} + + diff --git a/src/engine/ConstraintSolver.h b/src/engine/ConstraintSolver.h index 3ac82eb6..7a703b0d 100644 --- a/src/engine/ConstraintSolver.h +++ b/src/engine/ConstraintSolver.h @@ -39,6 +39,24 @@ namespace reactphysics3d { // Declarations class DynamicsWorld; +// Structure Impulse +struct Impulse { + + public: + Vector3& linearImpulseBody1; + Vector3& linearImpulseBody2; + Vector3& angularImpulseBody1; + Vector3& angularImpulseBody2; + + // Constructor + Impulse(Vector3& linearImpulseBody1, Vector3& angularImpulseBody1, + Vector3& linearImpulseBody2, Vector3& angularImpulseBody2) + : linearImpulseBody1(linearImpulseBody1), angularImpulseBody1(angularImpulseBody1), + linearImpulseBody2(linearImpulseBody2), angularImpulseBody2(angularImpulseBody2) { + + } +}; + // Structure ContactPointConstraint // Internal structure for a contact point constraint struct ContactPointConstraint { @@ -137,9 +155,7 @@ class ConstraintSolver { std::vector activeConstraints; // Current active constraints in the physics world bool isErrorCorrectionActive; // True if error correction (with world order) is active uint mNbIterations; // Number of iterations of the LCP solver - uint nbIterationsLCPErrorCorrection; // Number of iterations of the LCP solver for error correction uint nbConstraints; // Total number of constraints (with the auxiliary constraints) - uint nbConstraintsError; // Number of constraints for error correction projection (only contact constraints) uint nbBodies; // Current number of bodies in the physics world RigidBody* bodyMapping[NB_MAX_CONSTRAINTS][2]; // 2-dimensional array that contains the mapping of body reference // in the J_sp and B_sp matrices. For instance the cell bodyMapping[i][j] contains @@ -151,21 +167,8 @@ class ConstraintSolver { decimal B_sp[2][6*NB_MAX_CONSTRAINTS]; // 2-dimensional array that correspond to a useful matrix in sparse representation // This array contains for each constraint two 6x1 matrices (one for each body of the constraint) // a 6x1 matrix - decimal J_spError[NB_MAX_CONSTRAINTS][2*6]; // Same as J_sp for error correction projection (only contact constraints) - decimal B_spError[2][6*NB_MAX_CONSTRAINTS]; // Same as B_sp for error correction projection (only contact constraints) decimal b[NB_MAX_CONSTRAINTS]; // Vector "b" of the LCP problem - decimal bError[NB_MAX_CONSTRAINTS]; // Vector "b" of the LCP problem for error correction projection decimal d[NB_MAX_CONSTRAINTS]; // Vector "d" - decimal aError[6*NB_MAX_BODIES]; // Vector "a" for error correction projection - decimal penetrationDepths[NB_MAX_CONSTRAINTS]; // Array of penetration depths for error correction projection - decimal lambda[NB_MAX_CONSTRAINTS]; // Lambda vector of the LCP problem - decimal lambdaError[NB_MAX_CONSTRAINTS]; // Lambda vector of the LCP problem for error correction projections - decimal lambdaInit[NB_MAX_CONSTRAINTS]; // Lambda init vector for the LCP solver - decimal errorValues[NB_MAX_CONSTRAINTS]; // Error vector of all constraints - decimal lowerBounds[NB_MAX_CONSTRAINTS]; // Vector that contains the low limits for the variables of the LCP problem - decimal upperBounds[NB_MAX_CONSTRAINTS]; // Vector that contains the high limits for the variables of the LCP problem - decimal lowerBoundsError[NB_MAX_CONSTRAINTS]; // Same as "lowerBounds" but for error correction projections - decimal upperBoundsError[NB_MAX_CONSTRAINTS]; // Same as "upperBounds" but for error correction projections Matrix3x3 Minv_sp_inertia[NB_MAX_BODIES]; // 3x3 world inertia tensor matrix I for each body (from the Minv_sp matrix) decimal Minv_sp_mass_diag[NB_MAX_BODIES]; // Array that contains for each body the inverse of its mass // This is an array of size nbBodies that contains in each cell a 6x6 matrix @@ -193,7 +196,13 @@ class ConstraintSolver { void cacheLambda(); // Cache the lambda values in order to reuse them in the next step to initialize the lambda vector void warmStart(); // Compute the vector a used in the solve() method void solveLCP(); // Solve a LCP problem using Projected-Gauss-Seidel algorithm - + + // Apply an impulse to the two bodies of a constraint + void applyImpulse(const Impulse& impulse, const ContactConstraint& constraint); + + // Compute the collision restitution factor from the restitution factor of each body + decimal computeMixRestitutionFactor(const RigidBody *body1, const RigidBody *body2) const; + public: ConstraintSolver(DynamicsWorld* world); // Constructor virtual ~ConstraintSolver(); // Destructor @@ -249,6 +258,16 @@ inline void ConstraintSolver::setNbLCPIterations(uint nbIterations) { mNbIterations = nbIterations; } +// Compute the collision restitution factor from the restitution factor of each body +inline decimal ConstraintSolver::computeMixRestitutionFactor(const RigidBody* body1, + const RigidBody* body2) const { + decimal restitution1 = body1->getRestitution(); + decimal restitution2 = body2->getRestitution(); + + // Return the largest restitution factor + return (restitution1 > restitution2) ? restitution1 : restitution2; +} + } // End of ReactPhysics3D namespace #endif