/** @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) */ #pragma once #include #include #include #include #include #include #include namespace ephysics { /** * @brief This class represents the contact solver that is used to solve rigid bodies contacts. * The raint solver is based on the "Sequential Impulse" technique described by * Erin Catto in his GDC slides (http://code.google.com/p/box2d/downloads/list). * * A raint between two bodies is represented by a function C(x) which is equal to zero * when the raint is satisfied. The condition C(x)=0 describes a valid position and the * condition dC(x)/dt=0 describes a valid velocity. We have dC(x)/dt = Jv + b = 0 where J is * the Jacobian matrix of the raint, v is a vector that contains the velocity of both * bodies and b is the raint bias. We are looking for a force Fc that will act on the * bodies to keep the raint satisfied. Note that from the virtual work principle, we have * Fc = J^t * lambda where J^t is the transpose of the Jacobian matrix and lambda is a * Lagrange multiplier. Therefore, finding the force Fc is equivalent to finding the Lagrange * multiplier lambda. * * An impulse P = F * dt where F is a force and dt is the timestep. We can apply impulses a * body to change its velocity. The idea of the Sequential Impulse technique is to apply * impulses to bodies of each raints in order to keep the raint satisfied. * * --- Step 1 --- * * First, we integrate the applied force Fa acting of each rigid body (like gravity, ...) and * we obtain some new velocities v2' that tends to violate the raints. * * v2' = v1 + dt * M^-1 * Fa * * where M is a matrix that contains mass and inertia tensor information. * * --- Step 2 --- * * During the second step, we iterate over all the raints for a certain number of * iterations and for each raint we compute the impulse to apply to the bodies needed * so that the new velocity of the bodies satisfy Jv + b = 0. From the Newton law, we know that * M * deltaV = Pc where M is the mass of the body, deltaV is the difference of velocity and * Pc is the raint impulse to apply to the body. Therefore, we have * v2 = v2' + M^-1 * Pc. For each raint, we can compute the Lagrange multiplier lambda * using : lambda = -this.c (Jv2' + b) where this.c = 1 / (J * M^-1 * J^t). Now that we have the * Lagrange multiplier lambda, we can compute the impulse Pc = J^t * lambda * dt to apply to * the bodies to satisfy the raint. * * --- Step 3 --- * * In the third step, we integrate the new position x2 of the bodies using the new velocities * v2 computed in the second step with : x2 = x1 + dt * v2. * * Note that in the following code (as it is also explained in the slides from Erin Catto), * the value lambda is not only the lagrange multiplier but is the multiplication of the * Lagrange multiplier with the timestep dt. Therefore, in the following code, when we use * lambda, we mean (lambda * dt). * * We are using the accumulated impulse technique that is also described in the slides from * Erin Catto. * * We are also using warm starting. The idea is to warm start the solver at the beginning of * each step by applying the last impulstes for the raints that we already existing at the * previous step. This allows the iterative solver to converge faster towards the solution. * * For contact raints, we are also using split impulses so that the position correction * that uses Baumgarte stabilization does not change the momentum of the bodies. * * There are two ways to apply the friction raints. Either the friction raints are * applied at each contact point or they are applied only at the center of the contact manifold * between two bodies. If we solve the friction raints at each contact point, we need * two raints (two tangential friction directions) and if we solve the friction * raints at the center of the contact manifold, we need two raints for tangential * friction but also another twist friction raint to prevent spin of the body around the * contact manifold center. */ class ContactSolver { private: /** * Contact solver internal data structure that to store all the * information relative to a contact point */ struct ContactPointSolver { float penetrationImpulse; //!< Accumulated normal impulse float friction1Impulse; //!< Accumulated impulse in the 1st friction direction float friction2Impulse; //!< Accumulated impulse in the 2nd friction direction float penetrationSplitImpulse; //!< Accumulated split impulse for penetration correction vec3 rollingResistanceImpulse; //!< Accumulated rolling resistance impulse vec3 normal; //!< Normal vector of the contact vec3 frictionVector1; //!< First friction vector in the tangent plane vec3 frictionvec2; //!< Second friction vector in the tangent plane vec3 oldFrictionVector1; //!< Old first friction vector in the tangent plane vec3 oldFrictionvec2; //!< Old second friction vector in the tangent plane vec3 r1; //!< Vector from the body 1 center to the contact point vec3 r2; //!< Vector from the body 2 center to the contact point vec3 r1CrossT1; //!< Cross product of r1 with 1st friction vector vec3 r1CrossT2; //!< Cross product of r1 with 2nd friction vector vec3 r2CrossT1; //!< Cross product of r2 with 1st friction vector vec3 r2CrossT2; //!< Cross product of r2 with 2nd friction vector vec3 r1CrossN; //!< Cross product of r1 with the contact normal vec3 r2CrossN; //!< Cross product of r2 with the contact normal float penetrationDepth; //!< Penetration depth float restitutionBias; //!< Velocity restitution bias float inversePenetrationMass; //!< Inverse of the matrix K for the penenetration float inverseFriction1Mass; //!< Inverse of the matrix K for the 1st friction float inverseFriction2Mass; //!< Inverse of the matrix K for the 2nd friction boolean isRestingContact; //!< True if the contact was existing last time step ContactPoint* externalContact; //!< Pointer to the external contact }; /** * @brief Contact solver internal data structure to store all the information relative to a contact manifold. */ struct ContactManifoldSolver { int indexBody1; //!< Index of body 1 in the raint solver int indexBody2; //!< Index of body 2 in the raint solver float massInverseBody1; //!< Inverse of the mass of body 1 float massInverseBody2; //!< Inverse of the mass of body 2 etk::Matrix3x3 inverseInertiaTensorBody1; //!< Inverse inertia tensor of body 1 etk::Matrix3x3 inverseInertiaTensorBody2; //!< Inverse inertia tensor of body 2 ContactPointSolver contacts[MAXCONTACTPOINTSINMANIFOLD]; //!< Contact point raints int nbContacts; //!< Number of contact points boolean isBody1DynamicType; //!< True if the body 1 is of type dynamic boolean isBody2DynamicType; //!< True if the body 2 is of type dynamic float restitutionFactor; //!< Mix of the restitution factor for two bodies float frictionCoefficient; //!< Mix friction coefficient for the two bodies float rollingResistanceFactor; //!< Rolling resistance factor between the two bodies ContactManifold* externalContactManifold; //!< Pointer to the external contact manifold // - Variables used when friction raints are apply at the center of the manifold-// vec3 normal; //!< Average normal vector of the contact manifold vec3 frictionPointBody1; //!< Point on body 1 where to apply the friction raints vec3 frictionPointBody2; //!< Point on body 2 where to apply the friction raints vec3 r1Friction; //!< R1 vector for the friction raints vec3 r2Friction; //!< R2 vector for the friction raints vec3 r1CrossT1; //!< Cross product of r1 with 1st friction vector vec3 r1CrossT2; //!< Cross product of r1 with 2nd friction vector vec3 r2CrossT1; //!< Cross product of r2 with 1st friction vector vec3 r2CrossT2; //!< Cross product of r2 with 2nd friction vector float inverseFriction1Mass; //!< Matrix K for the first friction raint float inverseFriction2Mass; //!< Matrix K for the second friction raint float inverseTwistFrictionMass; //!< Matrix K for the twist friction raint etk::Matrix3x3 inverseRollingResistance; //!< Matrix K for the rolling resistance raint vec3 frictionVector1; //!< First friction direction at contact manifold center vec3 frictionvec2; //!< Second friction direction at contact manifold center vec3 oldFrictionVector1; //!< Old 1st friction direction at contact manifold center vec3 oldFrictionvec2; //!< Old 2nd friction direction at contact manifold center float friction1Impulse; //!< First friction direction impulse at manifold center float friction2Impulse; //!< Second friction direction impulse at manifold center float frictionTwistImpulse; //!< Twist friction impulse at contact manifold center vec3 rollingResistanceImpulse; //!< Rolling resistance impulse }; static float BETA; //!< Beta value for the penetration depth position correction without split impulses static float BETASPLITIMPULSE; //!< Beta value for the penetration depth position correction with split impulses static float SLOP; //!< Slop distance (allowed penetration distance between bodies) vec3* this.splitLinearVelocities; //!< Split linear velocities for the position contact solver (split impulse) vec3* this.splitAngularVelocities; //!< Split angular velocities for the position contact solver (split impulse) float this.timeStep; //!< Current time step etk::Vector this.contactConstraints; //!< Contact raints vec3* this.linearVelocities; //!< Array of linear velocities vec3* this.angularVelocities; //!< Array of angular velocities etk::Map this.mapBodyToConstrainedVelocityIndex; //!< Reference to the map of rigid body to their index in the rained velocities array boolean this.isWarmStartingActive; //!< True if the warm starting of the solver is active boolean this.isSplitImpulseActive; //!< True if the split impulse position correction is active boolean this.isSolveFrictionAtContactManifoldCenterActive; //!< True if we solve 3 friction raints at the contact manifold center only instead of 2 friction raints at each contact point /** * @brief Initialize the contact raints before solving the system */ void initializeContactConstraints(); /** * @brief Apply an impulse to the two bodies of a raint * @param[in] impulse Impulse to apply * @param[in] manifold Constraint to apply the impulse */ void applyImpulse( Impulse impulse, ContactManifoldSolver manifold); /** * @brief Apply an impulse to the two bodies of a raint * @param[in] impulse Impulse to apply * @param[in] manifold Constraint to apply the impulse */ void applySplitImpulse( Impulse impulse, ContactManifoldSolver manifold); /** * @brief Compute the collision restitution factor from the restitution factor of each body * @param[in] body1 First body to compute * @param[in] body2 Second body to compute * @return Collision restitution factor */ float computeMixedRestitutionFactor(RigidBody* body1, RigidBody* body2) ; /** * @brief Compute the mixed friction coefficient from the friction coefficient of each body * @param[in] body1 First body to compute * @param[in] body2 Second body to compute * @return Mixed friction coefficient */ float computeMixedFrictionCoefficient(RigidBody* body1, RigidBody* body2) ; /** * @brief Compute the mixed rolling resistance factor between two bodies * @param[in] body1 First body to compute * @param[in] body2 Second body to compute * @return Mixed rolling resistance */ float computeMixedRollingResistance(RigidBody* body1, RigidBody* body2) ; /** * @brief 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. * @param[in] deltaVelocity Velocity ratio (with the delta time step) * @param[in,out] contactPoint Contact point property */ void computeFrictionVectors( vec3 deltaVelocity, ContactPointSolver contactPoint) ; /** * @brief 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. * @param[in] deltaVelocity Velocity ratio (with the delta time step) * @param[in,out] contactPoint Contact point property */ void computeFrictionVectors( vec3 deltaVelocity, ContactManifoldSolver contactPoint) ; /** * @brief Compute a penetration raint impulse * @param[in] deltaLambda Ratio to apply at the calculation. * @param[in,out] contactPoint Contact point property * @return Impulse of the penetration result */ Impulse computePenetrationImpulse(float deltaLambda, ContactPointSolver contactPoint) ; /** * @brief Compute the first friction raint impulse * @param[in] deltaLambda Ratio to apply at the calculation. * @param[in] contactPoint Contact point property * @return Impulse of the friction result */ Impulse computeFriction1Impulse(float deltaLambda, ContactPointSolver contactPoint) ; /** * @brief Compute the second friction raint impulse * @param[in] deltaLambda Ratio to apply at the calculation. * @param[in] contactPoint Contact point property * @return Impulse of the friction result */ Impulse computeFriction2Impulse(float deltaLambda, ContactPointSolver contactPoint) ; public: /** * @brief Constructor * @param[in] mapBodyToVelocityIndex */ ContactSolver( etk::Map mapBodyToVelocityIndex); /** * @brief Virtualize the destructor */ virtual ~ContactSolver() = default; /** * @brief Initialize the raint solver for a given island * @param[in] dt Delta step time * @param[in] island Island list property */ void initializeForIsland(float dt, Island* island); /** * @brief Set the split velocities arrays * @param[in] splitLinearVelocities Split linear velocities Table pointer (not free) * @param[in] splitAngularVelocities Split angular velocities Table pointer (not free) */ void setSplitVelocitiesArrays(vec3* splitLinearVelocities, vec3* splitAngularVelocities); /** * @brief Set the rained velocities arrays * @param[in] rainedLinearVelocities Constrained Linear velocities Table pointer (not free) * @param[in] rainedAngularVelocities Constrained angular velocities Table pointer (not free) */ void setConstrainedVelocitiesArrays(vec3* rainedLinearVelocities, vec3* rainedAngularVelocities); /** * @brief Warm start the solver. * For each raint, 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 warmStart(); /** * @brief Store the computed impulses to use them to warm start the solver at the next iteration */ void storeImpulses(); /** * @brief Solve the contacts */ void solve(); /** * @brief Get the split impulses position correction technique is used for contacts * @return true the split status is Enable * @return true the split status is Disable */ boolean isSplitImpulseActive() ; /** * @brief Activate or Deactivate the split impulses for contacts * @param[in] isActive status to set. */ void setIsSplitImpulseActive(boolean isActive); /** * @brief Activate or deactivate the solving of friction raints at the center of /// the contact manifold instead of solving them at each contact point * @param[in] isActive Enable or not the center inertie */ void setIsSolveFrictionAtContactManifoldCenterActive(boolean isActive); /** * @brief Clean up the raint solver */ void cleanup(); }; }