313 lines
16 KiB
C++
313 lines
16 KiB
C++
/** @file
|
|
* Original ReactPhysics3D C++ library by Daniel Chappuis <http://www.reactphysics3d.com/> 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 <ephysics/raint/ContactPoint.hpp>
|
|
#include <ephysics/configuration.hpp>
|
|
#include <ephysics/raint/Joint.hpp>
|
|
#include <ephysics/collision/ContactManifold.hpp>
|
|
#include <ephysics/engine/Island.hpp>
|
|
#include <ephysics/engine/Impulse.hpp>
|
|
#include <etk/Map.hpp>
|
|
|
|
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<ContactManifoldSolver> this.contactConstraints; //!< Contact raints
|
|
vec3* this.linearVelocities; //!< Array of linear velocities
|
|
vec3* this.angularVelocities; //!< Array of angular velocities
|
|
etk::Map<RigidBody*, int> 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<RigidBody*, int> 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();
|
|
};
|
|
|
|
}
|