/******************************************************************************** * ReactPhysics3D physics library, http://www.reactphysics3d.com * * Copyright (c) 2010-2016 Daniel Chappuis * ********************************************************************************* * * * This software is provided 'as-is', without any express or implied warranty. * * In no event will the authors be held liable for any damages arising from the * * use of this software. * * * * Permission is granted to anyone to use this software for any purpose, * * including commercial applications, and to alter it and redistribute it * * freely, subject to the following restrictions: * * * * 1. The origin of this software must not be misrepresented; you must not claim * * that you wrote the original software. If you use this software in a * * product, an acknowledgment in the product documentation would be * * appreciated but is not required. * * * * 2. Altered source versions must be plainly marked as such, and must not be * * misrepresented as being the original software. * * * * 3. This notice may not be removed or altered from any source distribution. * * * ********************************************************************************/ // Libraries #include "ContactSolver.h" #include "DynamicsWorld.h" #include "body/RigidBody.h" #include "Profiler.h" #include using namespace reactphysics3d; using namespace std; // Constants initialization const decimal ContactSolver::BETA = decimal(0.2); const decimal ContactSolver::BETA_SPLIT_IMPULSE = decimal(0.2); const decimal ContactSolver::SLOP= decimal(0.01); // Constructor ContactSolver::ContactSolver(const std::map& mapBodyToVelocityIndex, SingleFrameAllocator& singleFrameAllocator) :mSplitLinearVelocities(nullptr), mSplitAngularVelocities(nullptr), mSingleFrameAllocator(singleFrameAllocator), mPenetrationConstraints(nullptr), mFrictionConstraints(nullptr), mLinearVelocities(nullptr), mAngularVelocities(nullptr), mMapBodyToConstrainedVelocityIndex(mapBodyToVelocityIndex), mIsWarmStartingActive(true), mIsSplitImpulseActive(true), mIsSolveFrictionAtContactManifoldCenterActive(true) { } // Initialize the contact constraints void ContactSolver::init(Island** islands, uint nbIslands, decimal timeStep) { mTimeStep = timeStep; // TODO : Try not to count manifolds here // Count the contact manifolds uint nbContactManifolds = 0; for (uint i = 0; i < nbIslands; i++) { nbContactManifolds += islands[i]->getNbContactManifolds(); } mNbFrictionConstraints = 0; mNbPenetrationConstraints = 0; mPenetrationConstraints = nullptr; mFrictionConstraints = nullptr; if (nbContactManifolds == 0) return; // TODO : Count exactly the number of constraints to allocate here uint nbPenetrationConstraints = nbContactManifolds * MAX_CONTACT_POINTS_IN_MANIFOLD; mPenetrationConstraints = static_cast(mSingleFrameAllocator.allocate(sizeof(PenetrationConstraint) * nbPenetrationConstraints)); //mPenetrationConstraints = new PenetrationConstraint[nbPenetrationConstraints]; assert(mPenetrationConstraints != nullptr); //mPenetrationConstraints = new PenetrationConstraint[mNbContactManifolds * 4]; mFrictionConstraints = static_cast(mSingleFrameAllocator.allocate(sizeof(FrictionConstraint) * nbContactManifolds)); //mFrictionConstraints = new FrictionConstraint[nbContactManifolds]; assert(mFrictionConstraints != nullptr); //mFrictionConstraints = new FrictionConstraint[mNbContactManifolds]; // For each island of the world for (uint islandIndex = 0; islandIndex < nbIslands; islandIndex++) { initializeForIsland(islands[islandIndex]); } // Warmstarting warmStart(); } // Initialize the constraint solver for a given island void ContactSolver::initializeForIsland(Island* island) { PROFILE("ContactSolver::initializeForIsland()"); assert(island != nullptr); assert(island->getNbBodies() > 0); assert(mSplitLinearVelocities != nullptr); assert(mSplitAngularVelocities != nullptr); // For each contact manifold of the island ContactManifold** contactManifolds = island->getContactManifolds(); for (uint i=0; igetNbContactManifolds(); i++) { ContactManifold* externalManifold = contactManifolds[i]; assert(externalManifold->getNbContactPoints() > 0); // Get the two bodies of the contact RigidBody* body1 = static_cast(externalManifold->getContactPoint(0)->getBody1()); RigidBody* body2 = static_cast(externalManifold->getContactPoint(0)->getBody2()); assert(body1 != nullptr); assert(body2 != nullptr); // TODO : Check if we have a better way to find the body index uint indexBody1 = mMapBodyToConstrainedVelocityIndex.find(body1)->second; uint indexBody2 = mMapBodyToConstrainedVelocityIndex.find(body2)->second; new (mFrictionConstraints + mNbFrictionConstraints) FrictionConstraint(); mFrictionConstraints[mNbFrictionConstraints].indexBody1 = indexBody1; mFrictionConstraints[mNbFrictionConstraints].indexBody2 = indexBody2; mFrictionConstraints[mNbFrictionConstraints].contactManifold = externalManifold; // Get the position of the two bodies const Vector3& x1 = body1->mCenterOfMassWorld; const Vector3& x2 = body2->mCenterOfMassWorld; // Get the velocities of the bodies const Vector3& v1 = mLinearVelocities[indexBody1]; const Vector3& w1 = mAngularVelocities[indexBody1]; const Vector3& v2 = mLinearVelocities[indexBody2]; const Vector3& w2 = mAngularVelocities[indexBody2]; // Get the inertia tensors of both bodies Matrix3x3 I1 = body1->getInertiaTensorInverseWorld(); Matrix3x3 I2 = body2->getInertiaTensorInverseWorld(); mFrictionConstraints[mNbFrictionConstraints].inverseInertiaTensorBody1 = I1; mFrictionConstraints[mNbFrictionConstraints].inverseInertiaTensorBody2 = I2; // Initialize the internal contact manifold structure using the external // contact manifold mFrictionConstraints[mNbFrictionConstraints].massInverseBody1 = body1->mMassInverse; mFrictionConstraints[mNbFrictionConstraints].massInverseBody2 = body2->mMassInverse; //internalManifold.nbContacts = externalManifold->getNbContactPoints(); decimal restitutionFactor = computeMixedRestitutionFactor(body1, body2); mFrictionConstraints[mNbFrictionConstraints].frictionCoefficient = computeMixedFrictionCoefficient(body1, body2); mFrictionConstraints[mNbFrictionConstraints].rollingResistanceFactor = computeMixedRollingResistance(body1, body2); mFrictionConstraints[mNbFrictionConstraints].hasAtLeastOneRestingContactPoint = false; //internalManifold.isBody1DynamicType = body1->getType() == BodyType::DYNAMIC; //internalManifold.isBody2DynamicType = body2->getType() == BodyType::DYNAMIC; bool isBody1DynamicType = body1->getType() == BodyType::DYNAMIC; bool isBody2DynamicType = body2->getType() == BodyType::DYNAMIC; // If we solve the friction constraints at the center of the contact manifold //if (mIsSolveFrictionAtContactManifoldCenterActive) { mFrictionConstraints[mNbFrictionConstraints].frictionPointBody1 = Vector3::zero(); mFrictionConstraints[mNbFrictionConstraints].frictionPointBody2 = Vector3::zero(); mFrictionConstraints[mNbFrictionConstraints].normal = Vector3::zero(); //} // Compute the inverse K matrix for the rolling resistance constraint mFrictionConstraints[mNbFrictionConstraints].inverseRollingResistance.setToZero(); if (mFrictionConstraints[mNbFrictionConstraints].rollingResistanceFactor > 0 && (isBody1DynamicType || isBody2DynamicType)) { mFrictionConstraints[mNbFrictionConstraints].inverseRollingResistance = I1 + I2; mFrictionConstraints[mNbFrictionConstraints].inverseRollingResistance = mFrictionConstraints[mNbFrictionConstraints].inverseRollingResistance.getInverse(); } int nbContacts = 0; // For each contact point of the contact manifold for (uint c=0; cgetNbContactPoints(); c++) { // Get a contact point ContactPoint* externalContact = externalManifold->getContactPoint(c); new (mPenetrationConstraints + mNbPenetrationConstraints) PenetrationConstraint(); mPenetrationConstraints[mNbPenetrationConstraints].indexBody1 = indexBody1; mPenetrationConstraints[mNbPenetrationConstraints].indexBody2 = indexBody2; mPenetrationConstraints[mNbPenetrationConstraints].inverseInertiaTensorBody1 = I1; mPenetrationConstraints[mNbPenetrationConstraints].inverseInertiaTensorBody2 = I2; mPenetrationConstraints[mNbPenetrationConstraints].massInverseBody1 = body1->mMassInverse; mPenetrationConstraints[mNbPenetrationConstraints].massInverseBody2 = body2->mMassInverse; mPenetrationConstraints[mNbPenetrationConstraints].restitutionFactor = restitutionFactor; mPenetrationConstraints[mNbPenetrationConstraints].indexFrictionConstraint = mNbFrictionConstraints; mPenetrationConstraints[mNbPenetrationConstraints].contactPoint = externalContact; // Get the contact point on the two bodies Vector3 p1 = externalContact->getWorldPointOnBody1(); Vector3 p2 = externalContact->getWorldPointOnBody2(); mPenetrationConstraints[mNbPenetrationConstraints].r1 = p1 - x1; mPenetrationConstraints[mNbPenetrationConstraints].r2 = p2 - x2; //mPenetrationConstraints[penConstIndex].externalContact = externalContact; mPenetrationConstraints[mNbPenetrationConstraints].normal = externalContact->getNormal(); mPenetrationConstraints[mNbPenetrationConstraints].penetrationDepth = externalContact->getPenetrationDepth(); mPenetrationConstraints[mNbPenetrationConstraints].isRestingContact = externalContact->getIsRestingContact(); mFrictionConstraints[mNbFrictionConstraints].hasAtLeastOneRestingContactPoint |= mPenetrationConstraints[mNbPenetrationConstraints].isRestingContact; externalContact->setIsRestingContact(true); //mPenetrationConstraints[penConstIndex].oldFrictionVector1 = externalContact->getFrictionVector1(); //mPenetrationConstraints[penConstIndex].oldFrictionVector2 = externalContact->getFrictionVector2(); mPenetrationConstraints[mNbPenetrationConstraints].penetrationImpulse = 0.0; //mPenetrationConstraints[penConstIndex].friction1Impulse = 0.0; //mPenetrationConstraints[penConstIndex].friction2Impulse = 0.0; //mPenetrationConstraints[penConstIndex].rollingResistanceImpulse = Vector3::zero(); // If we solve the friction constraints at the center of the contact manifold //if (mIsSolveFrictionAtContactManifoldCenterActive) { mFrictionConstraints[mNbFrictionConstraints].frictionPointBody1 += p1; mFrictionConstraints[mNbFrictionConstraints].frictionPointBody2 += p2; //} // Compute the velocity difference Vector3 deltaV = v2 + w2.cross(mPenetrationConstraints[mNbPenetrationConstraints].r2) - v1 - w1.cross(mPenetrationConstraints[mNbPenetrationConstraints].r1); mPenetrationConstraints[mNbPenetrationConstraints].r1CrossN = mPenetrationConstraints[mNbPenetrationConstraints].r1.cross(mPenetrationConstraints[mNbPenetrationConstraints].normal); mPenetrationConstraints[mNbPenetrationConstraints].r2CrossN = mPenetrationConstraints[mNbPenetrationConstraints].r2.cross(mPenetrationConstraints[mNbPenetrationConstraints].normal); // Compute the inverse mass matrix K for the penetration constraint decimal massPenetration = mPenetrationConstraints[mNbPenetrationConstraints].massInverseBody1 + mPenetrationConstraints[mNbPenetrationConstraints].massInverseBody2 + ((mPenetrationConstraints[mNbPenetrationConstraints].inverseInertiaTensorBody1 * mPenetrationConstraints[mNbPenetrationConstraints].r1CrossN ).cross(mPenetrationConstraints[mNbPenetrationConstraints].r1)).dot(mPenetrationConstraints[mNbPenetrationConstraints].normal) + ((mPenetrationConstraints[mNbPenetrationConstraints].inverseInertiaTensorBody2 * mPenetrationConstraints[mNbPenetrationConstraints].r2CrossN ).cross(mPenetrationConstraints[mNbPenetrationConstraints].r2)).dot(mPenetrationConstraints[mNbPenetrationConstraints].normal); massPenetration > decimal(0.0) ? mPenetrationConstraints[mNbPenetrationConstraints].inversePenetrationMass = decimal(1.0) / massPenetration : decimal(0.0); // 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 bellow a given threshold), we do not add a restitution velocity bias mPenetrationConstraints[mNbPenetrationConstraints].restitutionBias = 0.0; decimal deltaVDotN = deltaV.dot(mPenetrationConstraints[mNbPenetrationConstraints].normal); if (deltaVDotN < -RESTITUTION_VELOCITY_THRESHOLD) { mPenetrationConstraints[mNbPenetrationConstraints].restitutionBias = mPenetrationConstraints[mNbPenetrationConstraints].restitutionFactor * deltaVDotN; } // If the warm starting of the contact solver is active if (mIsWarmStartingActive) { // Get the cached accumulated impulses from the previous step mPenetrationConstraints[mNbPenetrationConstraints].penetrationImpulse = externalContact->getPenetrationImpulse(); } // Initialize the split impulses to zero mPenetrationConstraints[mNbPenetrationConstraints].penetrationSplitImpulse = 0.0; // If we solve the friction constraints at the center of the contact manifold //if (mIsSolveFrictionAtContactManifoldCenterActive) { mFrictionConstraints[mNbFrictionConstraints].normal += mPenetrationConstraints[mNbPenetrationConstraints].normal; //} mNbPenetrationConstraints++; nbContacts++; } // If we solve the friction constraints at the center of the contact manifold //if (mIsSolveFrictionAtContactManifoldCenterActive) { //mFrictionConstraints[mNbFrictionConstraints].normal = Vector3::zero(); mFrictionConstraints[mNbFrictionConstraints].frictionPointBody1 /= nbContacts; mFrictionConstraints[mNbFrictionConstraints].frictionPointBody2 /= nbContacts; mFrictionConstraints[mNbFrictionConstraints].r1Friction = mFrictionConstraints[mNbFrictionConstraints].frictionPointBody1 - x1; mFrictionConstraints[mNbFrictionConstraints].r2Friction = mFrictionConstraints[mNbFrictionConstraints].frictionPointBody2 - x2; mFrictionConstraints[mNbFrictionConstraints].oldFrictionVector1 = externalManifold->getFrictionVector1(); mFrictionConstraints[mNbFrictionConstraints].oldFrictionVector2 = externalManifold->getFrictionVector2(); // If warm starting is active if (mIsWarmStartingActive) { // Initialize the accumulated impulses with the previous step accumulated impulses mFrictionConstraints[mNbFrictionConstraints].friction1Impulse = externalManifold->getFrictionImpulse1(); mFrictionConstraints[mNbFrictionConstraints].friction2Impulse = externalManifold->getFrictionImpulse2(); mFrictionConstraints[mNbFrictionConstraints].frictionTwistImpulse = externalManifold->getFrictionTwistImpulse(); } else { // Initialize the accumulated impulses to zero mFrictionConstraints[mNbFrictionConstraints].friction1Impulse = 0.0; mFrictionConstraints[mNbFrictionConstraints].friction2Impulse = 0.0; mFrictionConstraints[mNbFrictionConstraints].frictionTwistImpulse = 0.0; mFrictionConstraints[mNbFrictionConstraints].rollingResistanceImpulse = Vector3(0, 0, 0); } mFrictionConstraints[mNbFrictionConstraints].normal.normalize(); Vector3 deltaVFrictionPoint = v2 + w2.cross(mFrictionConstraints[mNbFrictionConstraints].r2Friction) - v1 - w1.cross(mFrictionConstraints[mNbFrictionConstraints].r1Friction); // Compute the friction vectors computeFrictionVectors(deltaVFrictionPoint, mFrictionConstraints[mNbFrictionConstraints]); // Compute the inverse mass matrix K for the friction constraints at the center of the contact manifold mFrictionConstraints[mNbFrictionConstraints].r1CrossT1 = mFrictionConstraints[mNbFrictionConstraints].r1Friction.cross(mFrictionConstraints[mNbFrictionConstraints].frictionVector1); mFrictionConstraints[mNbFrictionConstraints].r1CrossT2 = mFrictionConstraints[mNbFrictionConstraints].r1Friction.cross(mFrictionConstraints[mNbFrictionConstraints].frictionVector2); mFrictionConstraints[mNbFrictionConstraints].r2CrossT1 = mFrictionConstraints[mNbFrictionConstraints].r2Friction.cross(mFrictionConstraints[mNbFrictionConstraints].frictionVector1); mFrictionConstraints[mNbFrictionConstraints].r2CrossT2 = mFrictionConstraints[mNbFrictionConstraints].r2Friction.cross(mFrictionConstraints[mNbFrictionConstraints].frictionVector2); decimal friction1Mass = mFrictionConstraints[mNbFrictionConstraints].massInverseBody1 + mFrictionConstraints[mNbFrictionConstraints].massInverseBody2 + ((I1 * mFrictionConstraints[mNbFrictionConstraints].r1CrossT1).cross(mFrictionConstraints[mNbFrictionConstraints].r1Friction)).dot( mFrictionConstraints[mNbFrictionConstraints].frictionVector1) + ((I2 * mFrictionConstraints[mNbFrictionConstraints].r2CrossT1).cross(mFrictionConstraints[mNbFrictionConstraints].r2Friction)).dot( mFrictionConstraints[mNbFrictionConstraints].frictionVector1); decimal friction2Mass = mFrictionConstraints[mNbFrictionConstraints].massInverseBody1 + mFrictionConstraints[mNbFrictionConstraints].massInverseBody2 + ((I1 * mFrictionConstraints[mNbFrictionConstraints].r1CrossT2).cross(mFrictionConstraints[mNbFrictionConstraints].r1Friction)).dot( mFrictionConstraints[mNbFrictionConstraints].frictionVector2) + ((I2 * mFrictionConstraints[mNbFrictionConstraints].r2CrossT2).cross(mFrictionConstraints[mNbFrictionConstraints].r2Friction)).dot( mFrictionConstraints[mNbFrictionConstraints].frictionVector2); decimal frictionTwistMass = mFrictionConstraints[mNbFrictionConstraints].normal.dot(mFrictionConstraints[mNbFrictionConstraints].inverseInertiaTensorBody1 * mFrictionConstraints[mNbFrictionConstraints].normal) + mFrictionConstraints[mNbFrictionConstraints].normal.dot(mFrictionConstraints[mNbFrictionConstraints].inverseInertiaTensorBody2 * mFrictionConstraints[mNbFrictionConstraints].normal); friction1Mass > decimal(0.0) ? mFrictionConstraints[mNbFrictionConstraints].inverseFriction1Mass = decimal(1.0)/friction1Mass : decimal(0.0); friction2Mass > decimal(0.0) ? mFrictionConstraints[mNbFrictionConstraints].inverseFriction2Mass = decimal(1.0)/friction2Mass : decimal(0.0); frictionTwistMass > decimal(0.0) ? mFrictionConstraints[mNbFrictionConstraints].inverseTwistFrictionMass = decimal(1.0) / frictionTwistMass : decimal(0.0); mNbFrictionConstraints++; } } // Solve the contact constraints of one iteration of the solve void ContactSolver::solve() { assert(mTimeStep > decimal(0.0)); resetTotalPenetrationImpulse(); solvePenetrationConstraints(); solveFrictionConstraints(); } // Warm start the solver. /// For each constraint, we apply the previous impulse (from the previous step) /// at the beginning. With this technique, we will converge faster towards /// the solution of the linear system void ContactSolver::warmStart() { PROFILE("ContactSolver::warmStart()"); // Penetration constraints for (uint i=0; i SLOP) biasPenetrationDepth = -(beta/mTimeStep) * max(0.0f, float(mPenetrationConstraints[i].penetrationDepth - SLOP)); decimal b = biasPenetrationDepth + mPenetrationConstraints[i].restitutionBias; // Compute the Lagrange multiplier lambda if (mIsSplitImpulseActive) { deltaLambda = - (Jv + mPenetrationConstraints[i].restitutionBias) * mPenetrationConstraints[i].inversePenetrationMass; } else { deltaLambda = - (Jv + b) * mPenetrationConstraints[i].inversePenetrationMass; } lambdaTemp = mPenetrationConstraints[i].penetrationImpulse; mPenetrationConstraints[i].penetrationImpulse = std::max(mPenetrationConstraints[i].penetrationImpulse + deltaLambda, decimal(0.0)); deltaLambda = mPenetrationConstraints[i].penetrationImpulse - lambdaTemp; // Add the penetration impulse to the total impulse of the corresponding friction constraint mFrictionConstraints[mPenetrationConstraints[i].indexFrictionConstraint].totalPenetrationImpulse += mPenetrationConstraints[i].penetrationImpulse; // Update the velocities of the body 1 by applying the impulse P=J^T * lambda Vector3 linearImpulse = mPenetrationConstraints[i].normal * deltaLambda; v1 += mPenetrationConstraints[i].massInverseBody1 * (-linearImpulse); w1 += mPenetrationConstraints[i].inverseInertiaTensorBody1 * (-mPenetrationConstraints[i].r1CrossN * deltaLambda); // Update the velocities of the body 1 by applying the impulse P=J^T * lambda v2 += mPenetrationConstraints[i].massInverseBody2 * linearImpulse; w2 += mPenetrationConstraints[i].inverseInertiaTensorBody2 * (mPenetrationConstraints[i].r2CrossN * deltaLambda); // If the split impulse position correction is active if (mIsSplitImpulseActive) { // Split impulse (position correction) const Vector3& v1Split = mSplitLinearVelocities[mPenetrationConstraints[i].indexBody1]; const Vector3& w1Split = mSplitAngularVelocities[mPenetrationConstraints[i].indexBody1]; const Vector3& v2Split = mSplitLinearVelocities[mPenetrationConstraints[i].indexBody2]; const Vector3& w2Split = mSplitAngularVelocities[mPenetrationConstraints[i].indexBody2]; Vector3 deltaVSplit = v2Split + w2Split.cross(mPenetrationConstraints[i].r2) - v1Split - w1Split.cross(mPenetrationConstraints[i].r1); decimal JvSplit = deltaVSplit.dot(mPenetrationConstraints[i].normal); decimal deltaLambdaSplit = - (JvSplit + biasPenetrationDepth) * mPenetrationConstraints[i].inversePenetrationMass; decimal lambdaTempSplit = mPenetrationConstraints[i].penetrationSplitImpulse; mPenetrationConstraints[i].penetrationSplitImpulse = std::max( mPenetrationConstraints[i].penetrationSplitImpulse + deltaLambdaSplit, decimal(0.0)); deltaLambdaSplit = mPenetrationConstraints[i].penetrationSplitImpulse - lambdaTempSplit; // Update the velocities of the body 1 by applying the impulse P=J^T * lambda Vector3 linearImpulse = mPenetrationConstraints[i].normal * deltaLambdaSplit; mSplitLinearVelocities[mPenetrationConstraints[i].indexBody1] += mPenetrationConstraints[i].massInverseBody1 * (-linearImpulse); mSplitAngularVelocities[mPenetrationConstraints[i].indexBody1] += mPenetrationConstraints[i].inverseInertiaTensorBody1 * (-mPenetrationConstraints[i].r1CrossN * deltaLambdaSplit); // Update the velocities of the body 1 by applying the impulse P=J^T * lambda mSplitLinearVelocities[mPenetrationConstraints[i].indexBody2] += mPenetrationConstraints[i].massInverseBody2 * linearImpulse; mSplitAngularVelocities[mPenetrationConstraints[i].indexBody2] += mPenetrationConstraints[i].inverseInertiaTensorBody2 * (mPenetrationConstraints[i].r2CrossN * deltaLambdaSplit); } } } // Solve the friction constraints void ContactSolver::solveFrictionConstraints() { // TODO : Check that the FrictionConstraint struct only contains variables that are // used in this method, nothing more PROFILE("ContactSolver::solveFrictionConstraints()"); for (uint i=0; i 0) { // Compute J*v const Vector3 JvRolling = w2 - w1; // Compute the Lagrange multiplier lambda Vector3 deltaLambdaRolling = mFrictionConstraints[i].inverseRollingResistance * (-JvRolling); decimal rollingLimit = mFrictionConstraints[i].rollingResistanceFactor * mFrictionConstraints[i].totalPenetrationImpulse; Vector3 lambdaTempRolling = mFrictionConstraints[i].rollingResistanceImpulse; mFrictionConstraints[i].rollingResistanceImpulse = clamp(mFrictionConstraints[i].rollingResistanceImpulse + deltaLambdaRolling, rollingLimit); deltaLambdaRolling = mFrictionConstraints[i].rollingResistanceImpulse - lambdaTempRolling; // Compute the impulse P=J^T * lambda angularImpulseBody1 = -deltaLambdaRolling; angularImpulseBody2 = deltaLambdaRolling; // Update the velocities of the body 1 by applying the impulse P w1 += mFrictionConstraints[i].inverseInertiaTensorBody1 * angularImpulseBody1; // Update the velocities of the body 1 by applying the impulse P w2 += mFrictionConstraints[i].inverseInertiaTensorBody2 * angularImpulseBody2; } } } // Store the computed impulses to use them to // warm start the solver at the next iteration void ContactSolver::storeImpulses() { // Penetration constraints for (uint i=0; isetPenetrationImpulse(mPenetrationConstraints[i].penetrationImpulse); } // Friction constraints for (uint i=0; isetFrictionImpulse1(mFrictionConstraints[i].friction1Impulse); mFrictionConstraints[i].contactManifold->setFrictionImpulse2(mFrictionConstraints[i].friction2Impulse); mFrictionConstraints[i].contactManifold->setFrictionTwistImpulse(mFrictionConstraints[i].frictionTwistImpulse); mFrictionConstraints[i].contactManifold->setRollingResistanceImpulse(mFrictionConstraints[i].rollingResistanceImpulse); mFrictionConstraints[i].contactManifold->setFrictionVector1(mFrictionConstraints[i].frictionVector1); mFrictionConstraints[i].contactManifold->setFrictionVector2(mFrictionConstraints[i].frictionVector2); } /* if (mPenetrationConstraints != nullptr) { // TODO : Delete this delete[] mPenetrationConstraints; } if (mFrictionConstraints != nullptr) { delete[] mFrictionConstraints; } */ } // 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 ContactSolver::computeFrictionVectors(const Vector3& deltaVelocity, FrictionConstraint& frictionConstraint) const { assert(frictionConstraint.normal.length() > MACHINE_EPSILON); // Compute the velocity difference vector in the tangential plane Vector3 normalVelocity = deltaVelocity.dot(frictionConstraint.normal) * frictionConstraint.normal; Vector3 tangentVelocity = deltaVelocity - normalVelocity; // If the velocty difference in the tangential plane is not zero decimal lengthTangenVelocity = tangentVelocity.length(); if (lengthTangenVelocity > MACHINE_EPSILON) { // Compute the first friction vector in the direction of the tangent // velocity difference frictionConstraint.frictionVector1 = tangentVelocity / lengthTangenVelocity; } else { // Get any orthogonal vector to the normal as the first friction vector frictionConstraint.frictionVector1 = frictionConstraint.normal.getOneUnitOrthogonalVector(); } // The second friction vector is computed by the cross product of the firs // friction vector and the contact normal frictionConstraint.frictionVector2 = frictionConstraint.normal.cross(frictionConstraint.frictionVector1).getUnit(); }