diff --git a/src/engine/ConstraintSolver.cpp b/src/engine/ConstraintSolver.cpp index 3d8d0507..6bd8a941 100644 --- a/src/engine/ConstraintSolver.cpp +++ b/src/engine/ConstraintSolver.cpp @@ -31,11 +31,11 @@ using namespace reactphysics3d; using namespace std; - // Constructor ConstraintSolver::ConstraintSolver(DynamicsWorld* world) :world(world), nbConstraints(0), mNbIterations(10), mContactConstraints(0), - mLinearVelocities(0), mAngularVelocities(0), mIsWarmStartingActive(false) { + mLinearVelocities(0), mAngularVelocities(0), mIsWarmStartingActive(true), + mIsSplitImpulseActive(true) { } @@ -118,6 +118,8 @@ void ConstraintSolver::initialize() { mLinearVelocities = new Vector3[nbBodies]; mAngularVelocities = new Vector3[nbBodies]; + mSplitLinearVelocities = new Vector3[nbBodies]; + mSplitAngularVelocities = new Vector3[nbBodies]; assert(mMapBodyToIndex.size() == nbBodies); } @@ -136,6 +138,8 @@ void ConstraintSolver::initializeBodies() { mLinearVelocities[bodyNumber] = rigidBody->getLinearVelocity() + mTimeStep * rigidBody->getMassInverse() * rigidBody->getExternalForce(); mAngularVelocities[bodyNumber] = rigidBody->getAngularVelocity() + mTimeStep * rigidBody->getInertiaTensorInverseWorld() * rigidBody->getExternalTorque(); + mSplitLinearVelocities[bodyNumber] = Vector3(0, 0, 0); + mSplitAngularVelocities[bodyNumber] = Vector3(0, 0, 0); } } @@ -204,7 +208,7 @@ void ConstraintSolver::initializeContactConstraints() { contact.restitutionBias = 0.0; decimal deltaVDotN = deltaV.dot(contact.normal); // TODO : Use a constant here - if (!contact.isRestingContact) { + if (deltaVDotN < 1.0f) { contact.restitutionBias = constraint.restitutionFactor * deltaVDotN; } @@ -212,6 +216,9 @@ void ConstraintSolver::initializeContactConstraints() { contact.penetrationImpulse = realContact->getCachedLambda(0); contact.friction1Impulse = realContact->getCachedLambda(1); contact.friction2Impulse = realContact->getCachedLambda(2); + + // Initialize the split impulses to zero + contact.penetrationSplitImpulse = 0.0; } } } @@ -323,17 +330,19 @@ void ConstraintSolver::solveContactConstraints() { // Compute the bias "b" of the constraint decimal beta = 0.2; // TODO : Use a constant for the slop - decimal slop = 0.005; + decimal slop = 0.01; 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 -= contact.b_Penetration; - deltaLambda *= contact.inversePenetrationMass; + if (mIsSplitImpulseActive) { + deltaLambda = - (Jv + contact.restitutionBias) * contact.inversePenetrationMass; + } + else { + deltaLambda = - (Jv + b) * contact.inversePenetrationMass; + } lambdaTemp = contact.penetrationImpulse; contact.penetrationImpulse = std::max(contact.penetrationImpulse + deltaLambda, 0.0f); deltaLambda = contact.penetrationImpulse - lambdaTemp; @@ -349,6 +358,32 @@ void ConstraintSolver::solveContactConstraints() { // Apply the impulse to the bodies of the constraint applyImpulse(impulsePenetration, constraint); + // If the split impulse position correction is active + if (mIsSplitImpulseActive) { + + // Split impulse (position correction) + const Vector3& v1Split = mSplitLinearVelocities[constraint.indexBody1]; + const Vector3& w1Split = mSplitAngularVelocities[constraint.indexBody1]; + const Vector3& v2Split = mSplitLinearVelocities[constraint.indexBody2]; + const Vector3& w2Split = mSplitAngularVelocities[constraint.indexBody2]; + Vector3 deltaVSplit = v2Split + w2Split.cross(contact.r2) - v1Split - w1Split.cross(contact.r1); + decimal JvSplit = deltaVSplit.dot(contact.normal); + decimal deltaLambdaSplit = - (JvSplit + biasPenetrationDepth) * contact.inversePenetrationMass; + decimal lambdaTempSplit = contact.penetrationSplitImpulse; + contact.penetrationSplitImpulse = std::max(contact.penetrationSplitImpulse + deltaLambdaSplit, 0.0f); + deltaLambda = contact.penetrationSplitImpulse - lambdaTempSplit; + + // Compute the impulse P=J^T * lambda + linearImpulseBody1 = -contact.normal * deltaLambdaSplit; + angularImpulseBody1 = -contact.r1CrossN * deltaLambdaSplit; + linearImpulseBody2 = contact.normal * deltaLambdaSplit; + angularImpulseBody2 = contact.r2CrossN * deltaLambdaSplit; + const Impulse splitImpulsePenetration(linearImpulseBody1, angularImpulseBody1, + linearImpulseBody2, angularImpulseBody2); + + applySplitImpulse(splitImpulsePenetration, constraint); + } + // --------- Friction 1 --------- // // Compute J*v @@ -467,6 +502,24 @@ void ConstraintSolver::applyImpulse(const Impulse& impulse, const ContactConstra } } +// Apply an impulse to the two bodies of a constraint +void ConstraintSolver::applySplitImpulse(const Impulse& impulse, const ContactConstraint& constraint) { + + // Update the velocities of the bodies by applying the impulse P + if (constraint.isBody1Moving) { + mSplitLinearVelocities[constraint.indexBody1] += constraint.massInverseBody1 * + impulse.linearImpulseBody1; + mSplitAngularVelocities[constraint.indexBody1] += constraint.inverseInertiaTensorBody1 * + impulse.angularImpulseBody1; + } + if (constraint.isBody2Moving) { + mSplitLinearVelocities[constraint.indexBody2] += constraint.massInverseBody2 * + impulse.linearImpulseBody2; + mSplitAngularVelocities[constraint.indexBody2] += constraint.inverseInertiaTensorBody2 * + impulse.angularImpulseBody2; + } +} + // Compute the two unit orthogonal vectors "t1" and "t2" that span the tangential friction plane // The two vectors have to be such that : t1 x t2 = contactNormal void ConstraintSolver::computeFrictionVectors(const Vector3& deltaVelocity, diff --git a/src/engine/ConstraintSolver.h b/src/engine/ConstraintSolver.h index e59657e5..9675a63e 100644 --- a/src/engine/ConstraintSolver.h +++ b/src/engine/ConstraintSolver.h @@ -64,6 +64,7 @@ struct ContactPointConstraint { decimal penetrationImpulse; // Accumulated normal impulse decimal friction1Impulse; // Accumulated impulse in the 1st friction direction decimal friction2Impulse; // Accumulated impulse in the 2nd friction direction + decimal penetrationSplitImpulse; // Accumulated split impulse for penetration correction Vector3 normal; // Normal vector of the contact Vector3 frictionVector1; // First friction vector in the tangent plane Vector3 frictionVector2; // Second friction vector in the tangent plane @@ -146,6 +147,13 @@ class ConstraintSolver { // in the J_sp and B_sp matrices. For instance the cell bodyMapping[i][j] contains Vector3* mLinearVelocities; // Array of constrained linear velocities Vector3* mAngularVelocities; // Array of constrained angular velocities + + // Split linear velocities for the position contact solver (split impulse) + Vector3* mSplitLinearVelocities; + + // Split angular velocities for the position contact solver (split impulse) + Vector3* mSplitAngularVelocities; + decimal mTimeStep; // Current time step // Contact constraints @@ -163,6 +171,9 @@ class ConstraintSolver { // True if the warm starting of the solver is active bool mIsWarmStartingActive; + // True if the split impulse position correction is active + bool mIsSplitImpulseActive; + // -------------------- Methods -------------------- // // Initialize the constraint solver @@ -187,6 +198,9 @@ class ConstraintSolver { // Apply an impulse to the two bodies of a constraint void applyImpulse(const Impulse& impulse, const ContactConstraint& constraint); + // Apply an impulse to the two bodies of a constraint + void applySplitImpulse(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; @@ -214,9 +228,15 @@ class ConstraintSolver { // Return the constrained linear velocity of a body after solving the constraints Vector3 getConstrainedLinearVelocityOfBody(RigidBody *body); + // Return the split linear velocity + Vector3 getSplitLinearVelocityOfBody(RigidBody* body); + // Return the constrained angular velocity of a body after solving the constraints Vector3 getConstrainedAngularVelocityOfBody(RigidBody* body); + // Return the split angular velocity + Vector3 getSplitAngularVelocityOfBody(RigidBody* body); + // Clean up the constraint solver void cleanup(); @@ -232,15 +252,29 @@ inline bool ConstraintSolver::isConstrainedBody(RigidBody* body) const { // Return the constrained linear velocity of a body after solving the constraints inline Vector3 ConstraintSolver::getConstrainedLinearVelocityOfBody(RigidBody* body) { assert(isConstrainedBody(body)); - uint indexBodyArray = mMapBodyToIndex[body]; - return mLinearVelocities[indexBodyArray]; + uint indexBody = mMapBodyToIndex[body]; + return mLinearVelocities[indexBody]; +} + +// Return the split linear velocity +inline Vector3 ConstraintSolver::getSplitLinearVelocityOfBody(RigidBody* body) { + assert(isConstrainedBody(body)); + uint indexBody = mMapBodyToIndex[body]; + return mSplitLinearVelocities[indexBody]; } // Return the constrained angular velocity of a body after solving the constraints inline Vector3 ConstraintSolver::getConstrainedAngularVelocityOfBody(RigidBody *body) { assert(isConstrainedBody(body)); - uint indexBodyArray = mMapBodyToIndex[body]; - return mAngularVelocities[indexBodyArray]; + uint indexBody = mMapBodyToIndex[body]; + return mAngularVelocities[indexBody]; +} + +// Return the split angular velocity +inline Vector3 ConstraintSolver::getSplitAngularVelocityOfBody(RigidBody* body) { + assert(isConstrainedBody(body)); + uint indexBody = mMapBodyToIndex[body]; + return mSplitAngularVelocities[indexBody]; } // Clean up the constraint solver diff --git a/src/engine/DynamicsWorld.cpp b/src/engine/DynamicsWorld.cpp index bd4b3c6d..f03a48c1 100644 --- a/src/engine/DynamicsWorld.cpp +++ b/src/engine/DynamicsWorld.cpp @@ -120,8 +120,8 @@ void DynamicsWorld::updateAllBodiesMotion() { // If it's a constrained body if (mConstraintSolver.isConstrainedBody(*it)) { // Get the constrained linear and angular velocities from the constraint solver - newLinearVelocity = mConstraintSolver.getConstrainedLinearVelocityOfBody(*it); - newAngularVelocity = mConstraintSolver.getConstrainedAngularVelocityOfBody(*it); + newLinearVelocity = mConstraintSolver.getConstrainedLinearVelocityOfBody(rigidBody); + newAngularVelocity = mConstraintSolver.getConstrainedAngularVelocityOfBody(rigidBody); } else { // Compute V_forces = dt * (M^-1 * F_ext) which is the velocity of the body due to the @@ -147,8 +147,8 @@ void DynamicsWorld::updateAllBodiesMotion() { // Use the Semi-Implicit Euler (Sympletic Euler) method to compute the new position and the new // orientation of the body void DynamicsWorld::updatePositionAndOrientationOfBody(RigidBody* rigidBody, - const Vector3& newLinVelocity, - const Vector3& newAngVelocity) { + Vector3 newLinVelocity, + Vector3 newAngVelocity) { decimal dt = mTimer.getTimeStep(); assert(rigidBody); @@ -159,6 +159,12 @@ void DynamicsWorld::updatePositionAndOrientationOfBody(RigidBody* rigidBody, // Update the linear and angular velocity of the body rigidBody->setLinearVelocity(newLinVelocity); rigidBody->setAngularVelocity(newAngVelocity); + + // Split velocity (only used to update the position) + if (mConstraintSolver.isConstrainedBody(rigidBody)) { + newLinVelocity += mConstraintSolver.getSplitLinearVelocityOfBody(rigidBody); + newAngVelocity += mConstraintSolver.getSplitAngularVelocityOfBody(rigidBody); + } // Get current position and orientation of the body const Vector3& currentPosition = rigidBody->getTransform().getPosition(); diff --git a/src/engine/DynamicsWorld.h b/src/engine/DynamicsWorld.h index 73737ff9..2d970784 100644 --- a/src/engine/DynamicsWorld.h +++ b/src/engine/DynamicsWorld.h @@ -96,8 +96,8 @@ class DynamicsWorld : public CollisionWorld { void updateAllBodiesMotion(); // Update the position and orientation of a body - void updatePositionAndOrientationOfBody(RigidBody* body, const Vector3& newLinVelocity, - const Vector3& newAngVelocity); + void updatePositionAndOrientationOfBody(RigidBody* body, Vector3 newLinVelocity, + Vector3 newAngVelocity); // Compute and set the interpolation factor to all bodies void setInterpolationFactorToAllBodies();