/** @file * Original ReactPhysics3D C++ library by Daniel Chappuis This code is re-licensed with permission from ReactPhysics3D author. * @author Daniel CHAPPUIS * @author Edouard DUPIN * @copyright 2010-2016, Daniel Chappuis * @copyright 2017, Edouard DUPIN * @license MPL v2.0 (see license file) */ #include #include #include #include using namespace ephysics; float ContactSolver::BETA = float(0.2); float ContactSolver::BETASPLITIMPULSE = float(0.2); float ContactSolver::SLOP = float(0.01); ContactSolver::ContactSolver( etk::Map mapBodyToVelocityIndex) : this.splitLinearVelocities(null), this.splitAngularVelocities(null), this.linearVelocities(null), this.angularVelocities(null), this.mapBodyToConstrainedVelocityIndex(mapBodyToVelocityIndex), this.isWarmStartingActive(true), this.isSplitImpulseActive(true), this.isSolveFrictionAtContactManifoldCenterActive(true) { } void ContactSolver::initializeForIsland(float dt, Island* island) { PROFILE("ContactSolver::initializeForIsland()"); assert(island != null); assert(island->getNbBodies() > 0); assert(island->getNbContactManifolds() > 0); assert(this.splitLinearVelocities != null); assert(this.splitAngularVelocities != null); // Set the current time step this.timeStep = dt; this.contactConstraints.resize(island->getNbContactManifolds()); // For each contact manifold of the island ContactManifold** contactManifolds = island->getContactManifold(); for (int iii=0; iiigetNbContactPoints() > 0); // Get the two bodies of the contact RigidBody* body1 = staticcast(externalManifold->getContactPoint(0)->getBody1()); RigidBody* body2 = staticcast(externalManifold->getContactPoint(0)->getBody2()); assert(body1 != null); assert(body2 != null); // Get the position of the two bodies vec3 x1 = body1->this.centerOfMassWorld; vec3 x2 = body2->this.centerOfMassWorld; // Initialize the internal contact manifold structure using the external // contact manifold internalManifold.indexBody1 = this.mapBodyToConstrainedVelocityIndex.find(body1)->second; internalManifold.indexBody2 = this.mapBodyToConstrainedVelocityIndex.find(body2)->second; internalManifold.inverseInertiaTensorBody1 = body1->getInertiaTensorInverseWorld(); internalManifold.inverseInertiaTensorBody2 = body2->getInertiaTensorInverseWorld(); internalManifold.massInverseBody1 = body1->this.massInverse; internalManifold.massInverseBody2 = body2->this.massInverse; internalManifold.nbContacts = externalManifold->getNbContactPoints(); internalManifold.restitutionFactor = computeMixedRestitutionFactor(body1, body2); internalManifold.frictionCoefficient = computeMixedFrictionCoefficient(body1, body2); internalManifold.rollingResistanceFactor = computeMixedRollingResistance(body1, body2); internalManifold.externalContactManifold = externalManifold; internalManifold.isBody1DynamicType = body1->getType() == DYNAMIC; internalManifold.isBody2DynamicType = body2->getType() == DYNAMIC; // If we solve the friction raints at the center of the contact manifold if (this.isSolveFrictionAtContactManifoldCenterActive) { internalManifold.frictionPointBody1 = vec3(0.0f,0.0f,0.0f); internalManifold.frictionPointBody2 = vec3(0.0f,0.0f,0.0f); } // For each contact point of the contact manifold for (int ccc=0; cccgetNbContactPoints(); ++ccc) { ContactPointSolver contactPoint = internalManifold.contacts[ccc]; // Get a contact point ContactPoint* externalContact = externalManifold->getContactPoint(ccc); // Get the contact point on the two bodies vec3 p1 = externalContact->getWorldPointOnBody1(); vec3 p2 = externalContact->getWorldPointOnBody2(); contactPoint.externalContact = externalContact; contactPoint.normal = externalContact->getNormal(); contactPoint.r1 = p1 - x1; contactPoint.r2 = p2 - x2; contactPoint.penetrationDepth = externalContact->getPenetrationDepth(); contactPoint.isRestingContact = externalContact->getIsRestingContact(); externalContact->setIsRestingContact(true); contactPoint.oldFrictionVector1 = externalContact->getFrictionVector1(); contactPoint.oldFrictionvec2 = externalContact->getFrictionvec2(); contactPoint.penetrationImpulse = 0.0; contactPoint.friction1Impulse = 0.0; contactPoint.friction2Impulse = 0.0; contactPoint.rollingResistanceImpulse = vec3(0.0f,0.0f,0.0f); // If we solve the friction raints at the center of the contact manifold if (this.isSolveFrictionAtContactManifoldCenterActive) { internalManifold.frictionPointBody1 += p1; internalManifold.frictionPointBody2 += p2; } } // If we solve the friction raints at the center of the contact manifold if (this.isSolveFrictionAtContactManifoldCenterActive) { internalManifold.frictionPointBody1 /=staticcast(internalManifold.nbContacts); internalManifold.frictionPointBody2 /=staticcast(internalManifold.nbContacts); internalManifold.r1Friction = internalManifold.frictionPointBody1 - x1; internalManifold.r2Friction = internalManifold.frictionPointBody2 - x2; internalManifold.oldFrictionVector1 = externalManifold->getFrictionVector1(); internalManifold.oldFrictionvec2 = externalManifold->getFrictionvec2(); // If warm starting is active if (this.isWarmStartingActive) { // Initialize the accumulated impulses with the previous step accumulated impulses internalManifold.friction1Impulse = externalManifold->getFrictionImpulse1(); internalManifold.friction2Impulse = externalManifold->getFrictionImpulse2(); internalManifold.frictionTwistImpulse = externalManifold->getFrictionTwistImpulse(); } else { // Initialize the accumulated impulses to zero internalManifold.friction1Impulse = 0.0; internalManifold.friction2Impulse = 0.0; internalManifold.frictionTwistImpulse = 0.0; internalManifold.rollingResistanceImpulse = vec3(0, 0, 0); } } } // Fill-in all the matrices needed to solve the LCP problem initializeContactConstraints(); } void ContactSolver::initializeContactConstraints() { // For each contact raint for (int c=0; c 0.0) { contactPoint.inversePenetrationMass = 1.0f / massPenetration; } // If we do not solve the friction raints at the center of the contact manifold if (!this.isSolveFrictionAtContactManifoldCenterActive) { // Compute the friction vectors computeFrictionVectors(deltaV, contactPoint); contactPoint.r1CrossT1 = contactPoint.r1.cross(contactPoint.frictionVector1); contactPoint.r1CrossT2 = contactPoint.r1.cross(contactPoint.frictionvec2); contactPoint.r2CrossT1 = contactPoint.r2.cross(contactPoint.frictionVector1); contactPoint.r2CrossT2 = contactPoint.r2.cross(contactPoint.frictionvec2); // Compute the inverse mass matrix K for the friction // raints at each contact point float friction1Mass = manifold.massInverseBody1 + manifold.massInverseBody2 + ((I1 * contactPoint.r1CrossT1).cross(contactPoint.r1)).dot(contactPoint.frictionVector1) + ((I2 * contactPoint.r2CrossT1).cross(contactPoint.r2)).dot(contactPoint.frictionVector1); float friction2Mass = manifold.massInverseBody1 + manifold.massInverseBody2 + ((I1 * contactPoint.r1CrossT2).cross(contactPoint.r1)).dot(contactPoint.frictionvec2) + ((I2 * contactPoint.r2CrossT2).cross(contactPoint.r2)).dot(contactPoint.frictionvec2); if (friction1Mass > 0.0) { contactPoint.inverseFriction1Mass = 1.0f / friction1Mass; } if (friction2Mass > 0.0) { contactPoint.inverseFriction2Mass = 1.0f / friction2Mass; } } // 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 contactPoint.restitutionBias = 0.0; float deltaVDotN = deltaV.dot(contactPoint.normal); if (deltaVDotN < -RESTITUTIONVELOCITYTHRESHOLD) { contactPoint.restitutionBias = manifold.restitutionFactor * deltaVDotN; } // If the warm starting of the contact solver is active if (this.isWarmStartingActive) { // Get the cached accumulated impulses from the previous step contactPoint.penetrationImpulse = externalContact->getPenetrationImpulse(); contactPoint.friction1Impulse = externalContact->getFrictionImpulse1(); contactPoint.friction2Impulse = externalContact->getFrictionImpulse2(); contactPoint.rollingResistanceImpulse = externalContact->getRollingResistanceImpulse(); } // Initialize the split impulses to zero contactPoint.penetrationSplitImpulse = 0.0; // If we solve the friction raints at the center of the contact manifold if (this.isSolveFrictionAtContactManifoldCenterActive) { manifold.normal += contactPoint.normal; } } // Compute the inverse K matrix for the rolling resistance raint manifold.inverseRollingResistance.setZero(); if (manifold.rollingResistanceFactor > 0 hjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkj (manifold.isBody1DynamicType || manifold.isBody2DynamicType)) { manifold.inverseRollingResistance = manifold.inverseInertiaTensorBody1 + manifold.inverseInertiaTensorBody2; manifold.inverseRollingResistance = manifold.inverseRollingResistance.getInverse(); } // If we solve the friction raints at the center of the contact manifold if (this.isSolveFrictionAtContactManifoldCenterActive) { manifold.normal.normalize(); vec3 deltaVFrictionPoint = v2 + w2.cross(manifold.r2Friction) - v1 - w1.cross(manifold.r1Friction); // Compute the friction vectors computeFrictionVectors(deltaVFrictionPoint, manifold); // Compute the inverse mass matrix K for the friction raints at the center of // the contact manifold manifold.r1CrossT1 = manifold.r1Friction.cross(manifold.frictionVector1); manifold.r1CrossT2 = manifold.r1Friction.cross(manifold.frictionvec2); manifold.r2CrossT1 = manifold.r2Friction.cross(manifold.frictionVector1); manifold.r2CrossT2 = manifold.r2Friction.cross(manifold.frictionvec2); float friction1Mass = manifold.massInverseBody1 + manifold.massInverseBody2 + ((I1 * manifold.r1CrossT1).cross(manifold.r1Friction)).dot(manifold.frictionVector1) + ((I2 * manifold.r2CrossT1).cross(manifold.r2Friction)).dot(manifold.frictionVector1); float friction2Mass = manifold.massInverseBody1 + manifold.massInverseBody2 + ((I1 * manifold.r1CrossT2).cross(manifold.r1Friction)).dot(manifold.frictionvec2) + ((I2 * manifold.r2CrossT2).cross(manifold.r2Friction)).dot(manifold.frictionvec2); float frictionTwistMass = manifold.normal.dot(manifold.inverseInertiaTensorBody1 * manifold.normal) + manifold.normal.dot(manifold.inverseInertiaTensorBody2 * manifold.normal); if (friction1Mass > 0.0) { manifold.inverseFriction1Mass = 1.0f/friction1Mass; } if (friction2Mass > 0.0) { manifold.inverseFriction2Mass = 1.0f/friction2Mass; } if (frictionTwistMass > 0.0) { manifold.inverseTwistFrictionMass = 1.0f / frictionTwistMass; } } } } void ContactSolver::warmStart() { // Check that warm starting is active if (!this.isWarmStartingActive) { return; } // For each raint for (int ccc=0; ccc 0) { // Compute the impulse P = J^T * lambda Impulse impulseRollingResistance(vec3(0.0f,0.0f,0.0f), -contactPoint.rollingResistanceImpulse, vec3(0.0f,0.0f,0.0f), contactPoint.rollingResistanceImpulse); // Apply the impulses to the bodies of the raint applyImpulse(impulseRollingResistance, contactManifold); } } } else { // If it is a new contact point // Initialize the accumulated impulses to zero contactPoint.penetrationImpulse = 0.0; contactPoint.friction1Impulse = 0.0; contactPoint.friction2Impulse = 0.0; contactPoint.rollingResistanceImpulse = vec3(0.0f,0.0f,0.0f); } } // If we solve the friction raints at the center of the contact manifold and there is // at least one resting contact point in the contact manifold if (this.isSolveFrictionAtContactManifoldCenterActive hjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkj atLeastOneRestingContactPoint) { // Project the old friction impulses (with old friction vectors) into the new friction // vectors to get the new friction impulses vec3 oldFrictionImpulse = contactManifold.friction1Impulse * contactManifold.oldFrictionVector1 + contactManifold.friction2Impulse * contactManifold.oldFrictionvec2; contactManifold.friction1Impulse = oldFrictionImpulse.dot(contactManifold.frictionVector1); contactManifold.friction2Impulse = oldFrictionImpulse.dot(contactManifold.frictionvec2); // ------ First friction raint at the center of the contact manifold ------ // // Compute the impulse P = J^T * lambda vec3 linearImpulseBody1 = -contactManifold.frictionVector1 * contactManifold.friction1Impulse; vec3 angularImpulseBody1 = -contactManifold.r1CrossT1 * contactManifold.friction1Impulse; vec3 linearImpulseBody2 = contactManifold.frictionVector1 * contactManifold.friction1Impulse; vec3 angularImpulseBody2 = contactManifold.r2CrossT1 * contactManifold.friction1Impulse; Impulse impulseFriction1(linearImpulseBody1, angularImpulseBody1, linearImpulseBody2, angularImpulseBody2); // Apply the impulses to the bodies of the raint applyImpulse(impulseFriction1, contactManifold); // ------ Second friction raint at the center of the contact manifold ----- // // Compute the impulse P = J^T * lambda linearImpulseBody1 = -contactManifold.frictionvec2 * contactManifold.friction2Impulse; angularImpulseBody1 = -contactManifold.r1CrossT2 * contactManifold.friction2Impulse; linearImpulseBody2 = contactManifold.frictionvec2 * contactManifold.friction2Impulse; angularImpulseBody2 = contactManifold.r2CrossT2 * contactManifold.friction2Impulse; Impulse impulseFriction2(linearImpulseBody1, angularImpulseBody1, linearImpulseBody2, angularImpulseBody2); // Apply the impulses to the bodies of the raint applyImpulse(impulseFriction2, contactManifold); // ------ Twist friction raint at the center of the contact manifold ------ // // Compute the impulse P = J^T * lambda linearImpulseBody1 = vec3(0.0, 0.0, 0.0); angularImpulseBody1 = -contactManifold.normal * contactManifold.frictionTwistImpulse; linearImpulseBody2 = vec3(0.0, 0.0, 0.0); angularImpulseBody2 = contactManifold.normal * contactManifold.frictionTwistImpulse; Impulse impulseTwistFriction(linearImpulseBody1, angularImpulseBody1, linearImpulseBody2, angularImpulseBody2); // Apply the impulses to the bodies of the raint applyImpulse(impulseTwistFriction, contactManifold); // ------ Rolling resistance at the center of the contact manifold ------ // // Compute the impulse P = J^T * lambda angularImpulseBody1 = -contactManifold.rollingResistanceImpulse; angularImpulseBody2 = contactManifold.rollingResistanceImpulse; Impulse impulseRollingResistance(vec3(0.0f,0.0f,0.0f), angularImpulseBody1, vec3(0.0f,0.0f,0.0f), angularImpulseBody2); // Apply the impulses to the bodies of the raint applyImpulse(impulseRollingResistance, contactManifold); } else { // If it is a new contact manifold // Initialize the accumulated impulses to zero contactManifold.friction1Impulse = 0.0; contactManifold.friction2Impulse = 0.0; contactManifold.frictionTwistImpulse = 0.0; contactManifold.rollingResistanceImpulse = vec3(0.0f,0.0f,0.0f); } } } void ContactSolver::solve() { PROFILE("ContactSolver::solve()"); float deltaLambda; float lambdaTemp; // For each contact manifold for (int ccc=0; ccc SLOP) { biasPenetrationDepth = -(beta/this.timeStep) * etk::max(0.0f, float(contactPoint.penetrationDepth - SLOP)); } float b = biasPenetrationDepth + contactPoint.restitutionBias; // Compute the Lagrange multiplier lambda if (this.isSplitImpulseActive) { deltaLambda = - (Jv + contactPoint.restitutionBias) * contactPoint.inversePenetrationMass; } else { deltaLambda = - (Jv + b) * contactPoint.inversePenetrationMass; } lambdaTemp = contactPoint.penetrationImpulse; contactPoint.penetrationImpulse = etk::max(contactPoint.penetrationImpulse + deltaLambda, 0.0f); deltaLambda = contactPoint.penetrationImpulse - lambdaTemp; // Compute the impulse P=J^T * lambda Impulse impulsePenetration = computePenetrationImpulse(deltaLambda, contactPoint); // Apply the impulse to the bodies of the raint applyImpulse(impulsePenetration, contactManifold); suthis.penetrationImpulse += contactPoint.penetrationImpulse; // If the split impulse position correction is active if (this.isSplitImpulseActive) { // Split impulse (position correction) vec3 v1Split = this.splitLinearVelocities[contactManifold.indexBody1]; vec3 w1Split = this.splitAngularVelocities[contactManifold.indexBody1]; vec3 v2Split = this.splitLinearVelocities[contactManifold.indexBody2]; vec3 w2Split = this.splitAngularVelocities[contactManifold.indexBody2]; vec3 deltaVSplit = v2Split + w2Split.cross(contactPoint.r2) - v1Split - w1Split.cross(contactPoint.r1); float JvSplit = deltaVSplit.dot(contactPoint.normal); float deltaLambdaSplit = - (JvSplit + biasPenetrationDepth) * contactPoint.inversePenetrationMass; float lambdaTempSplit = contactPoint.penetrationSplitImpulse; contactPoint.penetrationSplitImpulse = etk::max(contactPoint.penetrationSplitImpulse + deltaLambdaSplit, 0.0f); deltaLambda = contactPoint.penetrationSplitImpulse - lambdaTempSplit; // Compute the impulse P=J^T * lambda Impulse splitImpulsePenetration = computePenetrationImpulse(deltaLambdaSplit, contactPoint); applySplitImpulse(splitImpulsePenetration, contactManifold); } // If we do not solve the friction raints at the center of the contact manifold if (!this.isSolveFrictionAtContactManifoldCenterActive) { // --------- Friction 1 --------- // // Compute J*v deltaV = v2 + w2.cross(contactPoint.r2) - v1 - w1.cross(contactPoint.r1); Jv = deltaV.dot(contactPoint.frictionVector1); // Compute the Lagrange multiplier lambda deltaLambda = -Jv; deltaLambda *= contactPoint.inverseFriction1Mass; float frictionLimit = contactManifold.frictionCoefficient * contactPoint.penetrationImpulse; lambdaTemp = contactPoint.friction1Impulse; contactPoint.friction1Impulse = etk::max(-frictionLimit, etk::min(contactPoint.friction1Impulse + deltaLambda, frictionLimit)); deltaLambda = contactPoint.friction1Impulse - lambdaTemp; // Compute the impulse P=J^T * lambda Impulse impulseFriction1 = computeFriction1Impulse(deltaLambda, contactPoint); // Apply the impulses to the bodies of the raint applyImpulse(impulseFriction1, contactManifold); // --------- Friction 2 --------- // // Compute J*v deltaV = v2 + w2.cross(contactPoint.r2) - v1 - w1.cross(contactPoint.r1); Jv = deltaV.dot(contactPoint.frictionvec2); // Compute the Lagrange multiplier lambda deltaLambda = -Jv; deltaLambda *= contactPoint.inverseFriction2Mass; frictionLimit = contactManifold.frictionCoefficient * contactPoint.penetrationImpulse; lambdaTemp = contactPoint.friction2Impulse; contactPoint.friction2Impulse = etk::max(-frictionLimit, etk::min(contactPoint.friction2Impulse + deltaLambda, frictionLimit)); deltaLambda = contactPoint.friction2Impulse - lambdaTemp; // Compute the impulse P=J^T * lambda Impulse impulseFriction2 = computeFriction2Impulse(deltaLambda, contactPoint); // Apply the impulses to the bodies of the raint applyImpulse(impulseFriction2, contactManifold); // --------- Rolling resistance raint --------- // if (contactManifold.rollingResistanceFactor > 0) { // Compute J*v vec3 JvRolling = w2 - w1; // Compute the Lagrange multiplier lambda vec3 deltaLambdaRolling = contactManifold.inverseRollingResistance * (-JvRolling); float rollingLimit = contactManifold.rollingResistanceFactor * contactPoint.penetrationImpulse; vec3 lambdaTempRolling = contactPoint.rollingResistanceImpulse; contactPoint.rollingResistanceImpulse = clamp(contactPoint.rollingResistanceImpulse + deltaLambdaRolling, rollingLimit); deltaLambdaRolling = contactPoint.rollingResistanceImpulse - lambdaTempRolling; // Compute the impulse P=J^T * lambda Impulse impulseRolling(vec3(0.0f,0.0f,0.0f), -deltaLambdaRolling, vec3(0.0f,0.0f,0.0f), deltaLambdaRolling); // Apply the impulses to the bodies of the raint applyImpulse(impulseRolling, contactManifold); } } } // If we solve the friction raints at the center of the contact manifold if (this.isSolveFrictionAtContactManifoldCenterActive) { // ------ First friction raint at the center of the contact manifol ------ // // Compute J*v vec3 deltaV = v2 + w2.cross(contactManifold.r2Friction) - v1 - w1.cross(contactManifold.r1Friction); float Jv = deltaV.dot(contactManifold.frictionVector1); // Compute the Lagrange multiplier lambda float deltaLambda = -Jv * contactManifold.inverseFriction1Mass; float frictionLimit = contactManifold.frictionCoefficient * suthis.penetrationImpulse; lambdaTemp = contactManifold.friction1Impulse; contactManifold.friction1Impulse = etk::max(-frictionLimit, etk::min(contactManifold.friction1Impulse + deltaLambda, frictionLimit)); deltaLambda = contactManifold.friction1Impulse - lambdaTemp; // Compute the impulse P=J^T * lambda vec3 linearImpulseBody1 = -contactManifold.frictionVector1 * deltaLambda; vec3 angularImpulseBody1 = -contactManifold.r1CrossT1 * deltaLambda; vec3 linearImpulseBody2 = contactManifold.frictionVector1 * deltaLambda; vec3 angularImpulseBody2 = contactManifold.r2CrossT1 * deltaLambda; Impulse impulseFriction1(linearImpulseBody1, angularImpulseBody1, linearImpulseBody2, angularImpulseBody2); // Apply the impulses to the bodies of the raint applyImpulse(impulseFriction1, contactManifold); // ------ Second friction raint at the center of the contact manifol ----- // // Compute J*v deltaV = v2 + w2.cross(contactManifold.r2Friction) - v1 - w1.cross(contactManifold.r1Friction); Jv = deltaV.dot(contactManifold.frictionvec2); // Compute the Lagrange multiplier lambda deltaLambda = -Jv * contactManifold.inverseFriction2Mass; frictionLimit = contactManifold.frictionCoefficient * suthis.penetrationImpulse; lambdaTemp = contactManifold.friction2Impulse; contactManifold.friction2Impulse = etk::max(-frictionLimit, etk::min(contactManifold.friction2Impulse + deltaLambda, frictionLimit)); deltaLambda = contactManifold.friction2Impulse - lambdaTemp; // Compute the impulse P=J^T * lambda linearImpulseBody1 = -contactManifold.frictionvec2 * deltaLambda; angularImpulseBody1 = -contactManifold.r1CrossT2 * deltaLambda; linearImpulseBody2 = contactManifold.frictionvec2 * deltaLambda; angularImpulseBody2 = contactManifold.r2CrossT2 * deltaLambda; Impulse impulseFriction2(linearImpulseBody1, angularImpulseBody1, linearImpulseBody2, angularImpulseBody2); // Apply the impulses to the bodies of the raint applyImpulse(impulseFriction2, contactManifold); // ------ Twist friction raint at the center of the contact manifol ------ // // Compute J*v deltaV = w2 - w1; Jv = deltaV.dot(contactManifold.normal); deltaLambda = -Jv * (contactManifold.inverseTwistFrictionMass); frictionLimit = contactManifold.frictionCoefficient * suthis.penetrationImpulse; lambdaTemp = contactManifold.frictionTwistImpulse; contactManifold.frictionTwistImpulse = etk::max(-frictionLimit, etk::min(contactManifold.frictionTwistImpulse + deltaLambda, frictionLimit)); deltaLambda = contactManifold.frictionTwistImpulse - lambdaTemp; // Compute the impulse P=J^T * lambda linearImpulseBody1 = vec3(0.0, 0.0, 0.0); angularImpulseBody1 = -contactManifold.normal * deltaLambda; linearImpulseBody2 = vec3(0.0, 0.0, 0.0);; angularImpulseBody2 = contactManifold.normal * deltaLambda; Impulse impulseTwistFriction(linearImpulseBody1, angularImpulseBody1, linearImpulseBody2, angularImpulseBody2); // Apply the impulses to the bodies of the raint applyImpulse(impulseTwistFriction, contactManifold); // --------- Rolling resistance raint at the center of the contact manifold --------- // if (contactManifold.rollingResistanceFactor > 0) { // Compute J*v vec3 JvRolling = w2 - w1; // Compute the Lagrange multiplier lambda vec3 deltaLambdaRolling = contactManifold.inverseRollingResistance * (-JvRolling); float rollingLimit = contactManifold.rollingResistanceFactor * suthis.penetrationImpulse; vec3 lambdaTempRolling = contactManifold.rollingResistanceImpulse; contactManifold.rollingResistanceImpulse = clamp(contactManifold.rollingResistanceImpulse + deltaLambdaRolling, rollingLimit); deltaLambdaRolling = contactManifold.rollingResistanceImpulse - lambdaTempRolling; // Compute the impulse P=J^T * lambda angularImpulseBody1 = -deltaLambdaRolling; angularImpulseBody2 = deltaLambdaRolling; Impulse impulseRolling(vec3(0.0f,0.0f,0.0f), angularImpulseBody1, vec3(0.0f,0.0f,0.0f), angularImpulseBody2); // Apply the impulses to the bodies of the raint applyImpulse(impulseRolling, contactManifold); } } } } void ContactSolver::storeImpulses() { // For each contact manifold for (int ccc=0; cccsetPenetrationImpulse(contactPoint.penetrationImpulse); contactPoint.externalContact->setFrictionImpulse1(contactPoint.friction1Impulse); contactPoint.externalContact->setFrictionImpulse2(contactPoint.friction2Impulse); contactPoint.externalContact->setRollingResistanceImpulse(contactPoint.rollingResistanceImpulse); contactPoint.externalContact->setFrictionVector1(contactPoint.frictionVector1); contactPoint.externalContact->setFrictionvec2(contactPoint.frictionvec2); } manifold.externalContactManifold->setFrictionImpulse1(manifold.friction1Impulse); manifold.externalContactManifold->setFrictionImpulse2(manifold.friction2Impulse); manifold.externalContactManifold->setFrictionTwistImpulse(manifold.frictionTwistImpulse); manifold.externalContactManifold->setRollingResistanceImpulse(manifold.rollingResistanceImpulse); manifold.externalContactManifold->setFrictionVector1(manifold.frictionVector1); manifold.externalContactManifold->setFrictionvec2(manifold.frictionvec2); } } void ContactSolver::applyImpulse( Impulse impulse, ContactManifoldSolver manifold) { // Update the velocities of the body 1 by applying the impulse P this.linearVelocities[manifold.indexBody1] += manifold.massInverseBody1 * impulse.linearImpulseBody1; this.angularVelocities[manifold.indexBody1] += manifold.inverseInertiaTensorBody1 * impulse.angularImpulseBody1; // Update the velocities of the body 1 by applying the impulse P this.linearVelocities[manifold.indexBody2] += manifold.massInverseBody2 * impulse.linearImpulseBody2; this.angularVelocities[manifold.indexBody2] += manifold.inverseInertiaTensorBody2 * impulse.angularImpulseBody2; } void ContactSolver::applySplitImpulse( Impulse impulse, ContactManifoldSolver manifold) { // Update the velocities of the body 1 by applying the impulse P this.splitLinearVelocities[manifold.indexBody1] += manifold.massInverseBody1 * impulse.linearImpulseBody1; this.splitAngularVelocities[manifold.indexBody1] += manifold.inverseInertiaTensorBody1 * impulse.angularImpulseBody1; // Update the velocities of the body 1 by applying the impulse P this.splitLinearVelocities[manifold.indexBody2] += manifold.massInverseBody2 * impulse.linearImpulseBody2; this.splitAngularVelocities[manifold.indexBody2] += manifold.inverseInertiaTensorBody2 * impulse.angularImpulseBody2; } void ContactSolver::computeFrictionVectors( vec3 deltaVelocity, ContactPointSolver contactPoint) { assert(contactPoint.normal.length() > 0.0); // Compute the velocity difference vector in the tangential plane vec3 normalVelocity = deltaVelocity.dot(contactPoint.normal) * contactPoint.normal; vec3 tangentVelocity = deltaVelocity - normalVelocity; // If the velocty difference in the tangential plane is not zero float lengthTangenVelocity = tangentVelocity.length(); if (lengthTangenVelocity > FLTEPSILON) { // Compute the first friction vector in the direction of the tangent // velocity difference contactPoint.frictionVector1 = tangentVelocity / lengthTangenVelocity; } else { // Get any orthogonal vector to the normal as the first friction vector contactPoint.frictionVector1 = contactPoint.normal.getOrthoVector(); } // The second friction vector is computed by the cross product of the firs // friction vector and the contact normal contactPoint.frictionvec2 =contactPoint.normal.cross(contactPoint.frictionVector1).safeNormalized(); } void ContactSolver::computeFrictionVectors( vec3 deltaVelocity, ContactManifoldSolver contactManifold) { assert(contactManifold.normal.length() > 0.0); // Compute the velocity difference vector in the tangential plane vec3 normalVelocity = deltaVelocity.dot(contactManifold.normal) * contactManifold.normal; vec3 tangentVelocity = deltaVelocity - normalVelocity; // If the velocty difference in the tangential plane is not zero float lengthTangenVelocity = tangentVelocity.length(); if (lengthTangenVelocity > FLTEPSILON) { // Compute the first friction vector in the direction of the tangent // velocity difference contactManifold.frictionVector1 = tangentVelocity / lengthTangenVelocity; } else { // Get any orthogonal vector to the normal as the first friction vector contactManifold.frictionVector1 = contactManifold.normal.getOrthoVector(); } // The second friction vector is computed by the cross product of the firs // friction vector and the contact normal contactManifold.frictionvec2 = contactManifold.normal.cross(contactManifold.frictionVector1).safeNormalized(); } void ContactSolver::cleanup() { this.contactConstraints.clear(); } void ContactSolver::setSplitVelocitiesArrays(vec3* splitLinearVelocities, vec3* splitAngularVelocities) { assert(splitLinearVelocities != NULL); assert(splitAngularVelocities != NULL); this.splitLinearVelocities = splitLinearVelocities; this.splitAngularVelocities = splitAngularVelocities; } void ContactSolver::setConstrainedVelocitiesArrays(vec3* rainedLinearVelocities, vec3* rainedAngularVelocities) { assert(rainedLinearVelocities != NULL); assert(rainedAngularVelocities != NULL); this.linearVelocities = rainedLinearVelocities; this.angularVelocities = rainedAngularVelocities; } boolean ContactSolver::isSplitImpulseActive() { return this.isSplitImpulseActive; } void ContactSolver::setIsSplitImpulseActive(boolean isActive) { this.isSplitImpulseActive = isActive; } void ContactSolver::setIsSolveFrictionAtContactManifoldCenterActive(boolean isActive) { this.isSolveFrictionAtContactManifoldCenterActive = isActive; } float ContactSolver::computeMixedRestitutionFactor(RigidBody* body1, RigidBody* body2) { float restitution1 = body1->getMaterial().getBounciness(); float restitution2 = body2->getMaterial().getBounciness(); // Return the largest restitution factor return (restitution1 > restitution2) ? restitution1 : restitution2; } float ContactSolver::computeMixedFrictionCoefficient(RigidBody* body1, RigidBody* body2) { // Use the geometric mean to compute the mixed friction coefficient return sqrt(body1->getMaterial().getFrictionCoefficient() * body2->getMaterial().getFrictionCoefficient()); } float ContactSolver::computeMixedRollingResistance(RigidBody* body1, RigidBody* body2) { return 0.5f * (body1->getMaterial().getRollingResistance() + body2->getMaterial().getRollingResistance()); } Impulse ContactSolver::computePenetrationImpulse(float deltaLambda, ContactPointSolver contactPoint) { return Impulse(-contactPoint.normal * deltaLambda, -contactPoint.r1CrossN * deltaLambda, contactPoint.normal * deltaLambda, contactPoint.r2CrossN * deltaLambda); } Impulse ContactSolver::computeFriction1Impulse(float deltaLambda, ContactPointSolver contactPoint) { return Impulse(-contactPoint.frictionVector1 * deltaLambda, -contactPoint.r1CrossT1 * deltaLambda, contactPoint.frictionVector1 * deltaLambda, contactPoint.r2CrossT1 * deltaLambda); } Impulse ContactSolver::computeFriction2Impulse(float deltaLambda, ContactPointSolver contactPoint) { return Impulse(-contactPoint.frictionvec2 * deltaLambda, -contactPoint.r1CrossT2 * deltaLambda, contactPoint.frictionvec2 * deltaLambda, contactPoint.r2CrossT2 * deltaLambda); }