mrq/reactphysics3d
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Back to previous contact solver

Browse Source
This commit is contained in:
Daniel Chappuis 2016-10-08 01:18:56 +02:00
parent 54be20c5d3
commit 25fddd6fb2
3 changed files with 878 additions and 659 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1161
src/engine/ContactSolver.cpp
View File

File diff suppressed because it is too large Load Diff

319
src/engine/ContactSolver.h
View File

@ -31,7 +31,6 @@
#include "configuration.h"
#include "constraint/Joint.h"
#include "collision/ContactManifold.h"
#include "memory/SingleFrameAllocator.h"
#include "Island.h"
#include "Impulse.h"
#include <map>
@ -40,6 +39,7 @@
/// ReactPhysics3D namespace
namespace reactphysics3d {
// Class Contact Solver
/**
* This class represents the contact solver that is used to solve rigid bodies contacts.
@ -113,100 +113,30 @@ class ContactSolver {
private:
struct PenetrationConstraint {
/// Index of body 1 in the constraint solver
uint indexBody1;
/// Index of body 2 in the constraint solver
uint indexBody2;
/// Normal vector of the contact
Vector3 normal;
/// Vector from the body 1 center to the contact point
Vector3 r1;
/// Vector from the body 2 center to the contact point
Vector3 r2;
/// Cross product of r1 with the contact normal
Vector3 r1CrossN;
/// Cross product of r2 with the contact normal
Vector3 r2CrossN;
/// Penetration depth
decimal penetrationDepth;
/// Velocity restitution bias
decimal restitutionBias;
// Structure ContactPointSolver
/**
* Contact solver internal data structure that to store all the
* information relative to a contact point
*/
struct ContactPointSolver {
/// Accumulated normal impulse
decimal penetrationImpulse;
/// Accumulated split impulse for penetration correction
decimal penetrationSplitImpulse;
/// Inverse of the mass of body 1
decimal massInverseBody1;
/// Inverse of the mass of body 2
decimal massInverseBody2;
/// Inverse of the matrix K for the penenetration
decimal inversePenetrationMass;
/// Inverse inertia tensor of body 1
Matrix3x3 inverseInertiaTensorBody1;
/// Inverse inertia tensor of body 2
Matrix3x3 inverseInertiaTensorBody2;
/// Index of the corresponding friction constraint
uint indexFrictionConstraint;
/// Pointer to the corresponding contact point
ContactPoint* contactPoint;
/// True if this constraint is for a resting contact
bool isRestingContact;
};
struct FrictionConstraint {
/// Index of body 1 in the constraint solver
uint indexBody1;
/// Index of body 2 in the constraint solver
uint indexBody2;
/// R1 vector for the friction constraints
Vector3 r1Friction;
/// R2 vector for the friction constraints
Vector3 r2Friction;
/// Average normal vector of the contact manifold
Vector3 normal;
/// Accumulated impulse in the 1st friction direction
decimal friction1Impulse;
/// Accumulated impulse in the 2nd friction direction
decimal friction2Impulse;
/// Twist friction impulse at contact manifold center
decimal frictionTwistImpulse;
/// Accumulated split impulse for penetration correction
decimal penetrationSplitImpulse;
/// Accumulated rolling resistance impulse
Vector3 rollingResistanceImpulse;
/// Rolling resistance factor between the two bodies
decimal rollingResistanceFactor;
/// Mix friction coefficient for the two bodies
decimal frictionCoefficient;
/// Normal vector of the contact
Vector3 normal;
/// First friction vector in the tangent plane
Vector3 frictionVector1;
@ -214,12 +144,18 @@ class ContactSolver {
/// Second friction vector in the tangent plane
Vector3 frictionVector2;
/// Old 1st friction direction at contact manifold center
/// Old first friction vector in the tangent plane
Vector3 oldFrictionVector1;
/// Old 2nd friction direction at contact manifold center
/// Old second friction vector in the tangent plane
Vector3 oldFrictionVector2;
/// Vector from the body 1 center to the contact point
Vector3 r1;
/// Vector from the body 2 center to the contact point
Vector3 r2;
/// Cross product of r1 with 1st friction vector
Vector3 r1CrossT1;
@ -232,8 +168,20 @@ class ContactSolver {
/// Cross product of r2 with 2nd friction vector
Vector3 r2CrossT2;
/// Total of the all the corresponding penetration impulses
decimal totalPenetrationImpulse;
/// Cross product of r1 with the contact normal
Vector3 r1CrossN;
/// Cross product of r2 with the contact normal
Vector3 r2CrossN;
/// Penetration depth
decimal penetrationDepth;
/// Velocity restitution bias
decimal restitutionBias;
/// Inverse of the matrix K for the penenetration
decimal inversePenetrationMass;
/// Inverse of the matrix K for the 1st friction
decimal inverseFriction1Mass;
@ -241,16 +189,30 @@ class ContactSolver {
/// Inverse of the matrix K for the 2nd friction
decimal inverseFriction2Mass;
/// Matrix K for the twist friction constraint
decimal inverseTwistFrictionMass;
/// True if the contact was existing last time step
bool isRestingContact;
/// Matrix K for the rolling resistance constraint
Matrix3x3 inverseRollingResistance;
/// Pointer to the external contact
ContactPoint* externalContact;
};
// Structure ContactManifoldSolver
/**
* Contact solver internal data structure to store all the
* information relative to a contact manifold.
*/
struct ContactManifoldSolver {
/// Index of body 1 in the constraint solver
uint indexBody1;
/// Index of body 2 in the constraint solver
uint indexBody2;
/// Inverse of the mass of body 1
decimal massInverseBody1;
/// Inverse of the mass of body 2
// Inverse of the mass of body 2
decimal massInverseBody2;
/// Inverse inertia tensor of body 1
@ -259,11 +221,94 @@ class ContactSolver {
/// Inverse inertia tensor of body 2
Matrix3x3 inverseInertiaTensorBody2;
/// Pointer to the corresponding contact manifold
ContactManifold* contactManifold;
/// Contact point constraints
ContactPointSolver contacts[MAX_CONTACT_POINTS_IN_MANIFOLD];
/// True if the original contact manifold has at least one resting contact
bool hasAtLeastOneRestingContactPoint;
/// Number of contact points
uint nbContacts;
/// True if the body 1 is of type dynamic
bool isBody1DynamicType;
/// True if the body 2 is of type dynamic
bool isBody2DynamicType;
/// Mix of the restitution factor for two bodies
decimal restitutionFactor;
/// Mix friction coefficient for the two bodies
decimal frictionCoefficient;
/// Rolling resistance factor between the two bodies
decimal rollingResistanceFactor;
/// Pointer to the external contact manifold
ContactManifold* externalContactManifold;
// - Variables used when friction constraints are apply at the center of the manifold-//
/// Average normal vector of the contact manifold
Vector3 normal;
/// Point on body 1 where to apply the friction constraints
Vector3 frictionPointBody1;
/// Point on body 2 where to apply the friction constraints
Vector3 frictionPointBody2;
/// R1 vector for the friction constraints
Vector3 r1Friction;
/// R2 vector for the friction constraints
Vector3 r2Friction;
/// Cross product of r1 with 1st friction vector
Vector3 r1CrossT1;
/// Cross product of r1 with 2nd friction vector
Vector3 r1CrossT2;
/// Cross product of r2 with 1st friction vector
Vector3 r2CrossT1;
/// Cross product of r2 with 2nd friction vector
Vector3 r2CrossT2;
/// Matrix K for the first friction constraint
decimal inverseFriction1Mass;
/// Matrix K for the second friction constraint
decimal inverseFriction2Mass;
/// Matrix K for the twist friction constraint
decimal inverseTwistFrictionMass;
/// Matrix K for the rolling resistance constraint
Matrix3x3 inverseRollingResistance;
/// First friction direction at contact manifold center
Vector3 frictionVector1;
/// Second friction direction at contact manifold center
Vector3 frictionVector2;
/// Old 1st friction direction at contact manifold center
Vector3 oldFrictionVector1;
/// Old 2nd friction direction at contact manifold center
Vector3 oldFrictionVector2;
/// First friction direction impulse at manifold center
decimal friction1Impulse;
/// Second friction direction impulse at manifold center
decimal friction2Impulse;
/// Twist friction impulse at contact manifold center
decimal frictionTwistImpulse;
/// Rolling resistance impulse
Vector3 rollingResistanceImpulse;
};
// -------------------- Constants --------------------- //
@ -285,19 +330,17 @@ class ContactSolver {
/// Split angular velocities for the position contact solver (split impulse)
Vector3* mSplitAngularVelocities;
/// Reference to the single frame memory allocator
SingleFrameAllocator& mSingleFrameAllocator;
/// Current time step
decimal mTimeStep;
PenetrationConstraint* mPenetrationConstraints;
/// Contact constraints
ContactManifoldSolver* mContactConstraints;
FrictionConstraint* mFrictionConstraints;
/// Number of contact constraints
uint mNbContactManifolds;
uint mNbPenetrationConstraints;
uint mNbFrictionConstraints;
/// Single frame memory allocator
SingleFrameAllocator& mSingleFrameAllocator;
/// Array of linear velocities
Vector3* mLinearVelocities;
@ -320,15 +363,15 @@ class ContactSolver {
// -------------------- Methods -------------------- //
/// Initialize the constraint solver for a given island
void initializeForIsland(Island* island);
/// Initialize the contact constraints before solving the system
void initializeContactConstraints();
/// Apply an impulse to the two bodies of a constraint
//void applyImpulse(const Impulse& impulse, const ContactManifoldSolver& manifold);
void applyImpulse(const Impulse& impulse, const ContactManifoldSolver& manifold);
/// Apply an impulse to the two bodies of a constraint
//void applySplitImpulse(const Impulse& impulse,
// const ContactManifoldSolver& manifold);
void applySplitImpulse(const Impulse& impulse,
const ContactManifoldSolver& manifold);
/// Compute the collision restitution factor from the restitution factor of each body
decimal computeMixedRestitutionFactor(RigidBody *body1,
@ -341,30 +384,29 @@ class ContactSolver {
/// Compute th mixed rolling resistance factor between two bodies
decimal computeMixedRollingResistance(RigidBody* body1, RigidBody* body2) const;
// TODO : Delete this
/// 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.
// void computeFrictionVectors(const Vector3& deltaVelocity,
// ContactPointSolver &contactPoint) const;
void computeFrictionVectors(const Vector3& deltaVelocity,
ContactPointSolver &contactPoint) const;
/// 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 computeFrictionVectors(const Vector3& deltaVelocity,
FrictionConstraint& frictionConstraint) const;
ContactManifoldSolver& contactPoint) const;
/// Compute a penetration constraint impulse
// const Impulse computePenetrationImpulse(decimal deltaLambda,
// const PenetrationConstraint& constraint) const;
const Impulse computePenetrationImpulse(decimal deltaLambda,
const ContactPointSolver& contactPoint) const;
/// Compute the first friction constraint impulse
const Impulse computeFriction1Impulse(decimal deltaLambda,
const FrictionConstraint& contactPoint) const;
const ContactPointSolver& contactPoint) const;
/// Compute the second friction constraint impulse
const Impulse computeFriction2Impulse(decimal deltaLambda,
const FrictionConstraint& contactPoint) const;
const ContactPointSolver& contactPoint) const;
public:
@ -372,16 +414,13 @@ class ContactSolver {
/// Constructor
ContactSolver(const std::map<RigidBody*, uint>& mapBodyToVelocityIndex,
SingleFrameAllocator& singleFrameAllocator);
SingleFrameAllocator& allocator);
/// Destructor
~ContactSolver() = default;
/// Initialize the contact constraints
void init(Island** islands, uint nbIslands, decimal timeStep);
/// Solve the contact constraints of one iteration of the solve
void solve();
/// Initialize the constraint solver for a given island
void initializeForIsland(decimal dt, Island* island);
/// Set the split velocities arrays
void setSplitVelocitiesArrays(Vector3* splitLinearVelocities,
@ -399,16 +438,7 @@ class ContactSolver {
void storeImpulses();
/// Solve the contacts
//void solve();
/// Reset the total penetration impulse of friction constraints
void resetTotalPenetrationImpulse();
/// Solve the penetration constraints
void solvePenetrationConstraints();
/// Solve the friction constraints
void solveFrictionConstraints();
void solve();
/// Return true if the split impulses position correction technique is used for contacts
bool isSplitImpulseActive() const;
@ -420,8 +450,8 @@ class ContactSolver {
/// the contact manifold instead of solving them at each contact point
void setIsSolveFrictionAtContactManifoldCenterActive(bool isActive);
/// Return true if warmstarting is active
bool IsWarmStartingActive() const;
/// Clean up the constraint solver
void cleanup();
};
// Set the split velocities arrays
@ -476,7 +506,7 @@ inline decimal ContactSolver::computeMixedRestitutionFactor(RigidBody* body1,
inline decimal ContactSolver::computeMixedFrictionCoefficient(RigidBody *body1,
RigidBody *body2) const {
// Use the geometric mean to compute the mixed friction coefficient
return std::sqrt(body1->getMaterial().getFrictionCoefficient() *
return sqrt(body1->getMaterial().getFrictionCoefficient() *
body2->getMaterial().getFrictionCoefficient());
}
@ -487,16 +517,16 @@ inline decimal ContactSolver::computeMixedRollingResistance(RigidBody* body1,
}
// Compute a penetration constraint impulse
//inline const Impulse ContactSolver::computePenetrationImpulse(decimal deltaLambda,
// const PenetrationConstraint& constraint)
// const {
// return Impulse(-constraint.normal * deltaLambda, -constraint.r1CrossN * deltaLambda,
// constraint.normal * deltaLambda, constraint.r2CrossN * deltaLambda);
//}
inline const Impulse ContactSolver::computePenetrationImpulse(decimal deltaLambda,
const ContactPointSolver& contactPoint)
const {
return Impulse(-contactPoint.normal * deltaLambda, -contactPoint.r1CrossN * deltaLambda,
contactPoint.normal * deltaLambda, contactPoint.r2CrossN * deltaLambda);
}
// Compute the first friction constraint impulse
inline const Impulse ContactSolver::computeFriction1Impulse(decimal deltaLambda,
const FrictionConstraint& contactPoint)
const ContactPointSolver& contactPoint)
const {
return Impulse(-contactPoint.frictionVector1 * deltaLambda,
-contactPoint.r1CrossT1 * deltaLambda,
@ -506,7 +536,7 @@ inline const Impulse ContactSolver::computeFriction1Impulse(decimal deltaLambda,
// Compute the second friction constraint impulse
inline const Impulse ContactSolver::computeFriction2Impulse(decimal deltaLambda,
const FrictionConstraint& contactPoint)
const ContactPointSolver& contactPoint)
const {
return Impulse(-contactPoint.frictionVector2 * deltaLambda,
-contactPoint.r1CrossT2 * deltaLambda,
@ -514,11 +544,6 @@ inline const Impulse ContactSolver::computeFriction2Impulse(decimal deltaLambda,
contactPoint.r2CrossT2 * deltaLambda);
}
// Return true if warmstarting is active
inline bool ContactSolver::IsWarmStartingActive() const {
return mIsWarmStartingActive;
}
}
#endif

57
src/engine/DynamicsWorld.cpp
View File

@ -331,8 +331,6 @@ void DynamicsWorld::solveContactsAndConstraints() {
PROFILE("DynamicsWorld::solveContactsAndConstraints()");
// TODO : Do not solve per island but solve every constraints at once
// Set the velocities arrays
mContactSolver.setSplitVelocitiesArrays(mSplitLinearVelocities, mSplitAngularVelocities);
mContactSolver.setConstrainedVelocitiesArrays(mConstrainedLinearVelocities,
@ -342,28 +340,25 @@ void DynamicsWorld::solveContactsAndConstraints() {
mConstraintSolver.setConstrainedPositionsArrays(mConstrainedPositions,
mConstrainedOrientations);
// Initialize the contact solver
mContactSolver.init(mIslands, mNbIslands, mTimeStep);
// ---------- Solve velocity constraints for joints and contacts ---------- //
// For each island of the world
for (uint islandIndex = 0; islandIndex < mNbIslands; islandIndex++) {
// Check if there are contacts and constraints to solve
bool isConstraintsToSolve = mIslands[islandIndex]->getNbJoints() > 0;
//bool isContactsToSolve = mIslands[islandIndex]->getNbContactManifolds() > 0;
//if (!isConstraintsToSolve && !isContactsToSolve) continue;
bool isContactsToSolve = mIslands[islandIndex]->getNbContactManifolds() > 0;
if (!isConstraintsToSolve && !isContactsToSolve) continue;
// If there are contacts in the current island
// if (isContactsToSolve) {
if (isContactsToSolve) {
// // Initialize the solver
// mContactSolver.initializeForIsland(mTimeStep, mIslands[islandIndex]);
// Initialize the solver
mContactSolver.initializeForIsland(mTimeStep, mIslands[islandIndex]);
// // Warm start the contact solver
// if (mContactSolver.IsWarmStartingActive()) {
// mContactSolver.warmStart();
// }
// }
// Warm start the contact solver
mContactSolver.warmStart();
}
// If there are constraints
if (isConstraintsToSolve) {
@ -371,40 +366,26 @@ void DynamicsWorld::solveContactsAndConstraints() {
// Initialize the constraint solver
mConstraintSolver.initializeForIsland(mTimeStep, mIslands[islandIndex]);
}
}
// For each iteration of the velocity solver
for (uint i=0; i<mNbVelocitySolverIterations; i++) {
for (uint islandIndex = 0; islandIndex < mNbIslands; islandIndex++) {
// Solve the constraints
bool isConstraintsToSolve = mIslands[islandIndex]->getNbJoints() > 0;
if (isConstraintsToSolve) {
mConstraintSolver.solveVelocityConstraints(mIslands[islandIndex]);
}
// Solve the constraints
if (isConstraintsToSolve) {
mConstraintSolver.solveVelocityConstraints(mIslands[islandIndex]);
}
mContactSolver.solve();
// Solve the contacts
// if (isContactsToSolve) {
// mContactSolver.resetTotalPenetrationImpulse();
// mContactSolver.solvePenetrationConstraints();
// mContactSolver.solveFrictionConstraints();
// }
}
if (isContactsToSolve) mContactSolver.solve();
}
// Cache the lambda values in order to use them in the next
// step and cleanup the contact solver
// if (isContactsToSolve) {
// mContactSolver.storeImpulses();
// mContactSolver.cleanup();
// }
//}
mContactSolver.storeImpulses();
if (isContactsToSolve) {
mContactSolver.storeImpulses();
mContactSolver.cleanup();
}
}
}
// Solve the position error correction of the constraints