Constraint Solving - nullstar/SecondBrain GitHub Wiki
Constraints in rigid dynamics are joints that connect dynamic bodies restricting their motion in specific ways. Constraints can be used to create different types of joints such as hinges, ball-socket, and prismatic. The aim of constraint solving is to find the internal forces and torques to apply to each connected body such that the constraint is satisfied accounting for any given dynamic state of the bodies and any external forces and torques being applied.
Constraints can be defined as functions where the function expression determines the type of joint. For example a distance constraint which limits a particle defined by its vector position
Let us generalise this for generic coordinates to create a method for solving all constraint types. Our constraint function becomes:
Taking the derivatives of this function, substituting equations of motion, and applying D'Alembert's principle we arrive at a set of general equations that allows us to solve the internal forces required to satisfy a constraint:
$$\lambda_{force} = \frac{-J'v - JM^{-1}Q_{ext}}{JM^{-1}J^T}$$ $$Q_{int} = J^T\lambda_{force}$$
where
The external forces from the above equations can be gathered from the simulation, and the velocities and mass matrix from the constrained bodies. The Jacobian matrix will depend on the constraint type. The calculation of these parameters is discussed further below.
Most physics engines are impulse based, that is they resolve collisions and constraints using impulses rather than forces. Therefore we would like to calculate the impulse
Fortunately converting
$$\lambda_{impulse} = \frac{-Jv}{JM^{-1}*J^T}$$
Consider a constraint that joins two rigid bodies together which we identify by the subscripts 1 and 2. In this system
To calculate the Jacobian we need to refactor our constraint equation
Substute in your constraint derivative calculated previously into the above equation and factor out the velocities from all other terms of this equation so we are left over with a matrix multiplied by a velocity vector. By inspection with the above equation you should see that the left matrix term is equivalent to the Jacobian.
In the next sections we'll cover calculating these Jacobians for different constraint types.
There are two core constraint types from which other more complex constraints can be constructed with modifications to the formulas and/or by combinig the core types. These are:
- Position Constraint : This constraint ensures that the relative anchor points of two bodies maintain the same world space position as each other.
- Rotation Constraint : This constraint ensures that the relative orientation between two bodies remains fixed.
The position constraint ensures that the distance between the bodies and the joint always remains constant. If
In other words, the constraint ensures that both bodies agree on the world space position of the joint. As discussed above, to calculate the Jacobian we need to calculate the differential, substitute that into the velocity constraint function, and then factor out the velocities. The differential of this constraint function is:
The derivate of the world space joint position according to body n can be calculated by considering the body's linear and angular velocities with the following equation where
Substituting those in, applying some vector identities to reorder the cross products, and rewriting in matrix form gives the following:
By inspection with the velocity constraint function (
The Jacobian ends up as a 1 by 12 matrix.
The rotation constraint ensures that the rotation between the bodies and the joint always remains fixed.
Before we start it's useful to define a couple helper functions for converting quaternions into 4x4 matrices. For a quaternion
If
where P is a "projection" matrix:
In other words, the constraint ensures that the rotation difference between the joint rotation according to each body is identity, i.e. both bodies agree on the world space rotation of the joint. Calculating the derivative of the constraint function, using some identity substitutions, and using our
The Jacobian ends up as a 3 by 12 matrix.
To make the fixed constraint we just combine the two core constraints into a single 4 by 12 Jacobian matrix. Putting the position contraint first and the rotation contraint second, the jacobian ends up being:
Axis constraints constrain the position or rotation of the two bodies only on the specified axis or axes. This is useful for creating contraints such as prismatic joints or hinge joints.
To constrain position along a given axis
Following the same process as before where we calculate the derivative of the constraint function, subtitute in the velocity functions, and re-arrage to matrix form, we get the following 1 by 12 Jacobian matrix:
These constraints can be combined to constrain on multiple axes simultaneously. For example consider that
Constraining rotation around an axis
Applying the same mathematical process as before we get the following 1 by 12 Jacobian matrix:
As with the position axis constraint, multiple constraints can be combined. Therefore to create a joint that only allows rotation around one axis similar to a hinge we would combine rotation constraints on the two axes
We can add conditions to our constraints such as distance or angle limits by only applying the constraint when the condition isn't met. The following examples demonstrate this:
The penetration constraint is used by physics engines to enforce non penetration when objects collide. This is essentialy the same as a position axis constraint with one axis as discussed above where the anchor point is set to the contact point between the bodies and the constraint axis is set to the contact normal. Where this constraint differs is that we only wish to conditionaly enforce it when the distance is less than zero. Therefore the constraint equation is:
The Jacobian is the same as for the position axis constraint:
When solving this constraint we first evaluate the contraint function
Only if this distance is below zero do we then solve the constraint and apply impulses to the constrained bodies, otherwise we ignore the constraint.
Say we are constraining rotation on an axis
where
So far we have only considered a single constraint that connects two bodies. Now imagine we have many constraints that are attaching many different bodies together in a chain. If we solve each constraint individually each frame then the chain will suffer from instability since solving one constraint might violate another constraint resulting in an incorrect solution each frame.
For example imagine a system with 3 bodies
There are multiple way we can tackle this problem:
In the example given above, imagine after solving
This downside to this approach is that often a large number of iterations is required for the solution to converge which can be prohibitive for realtime performance. This can be partially improved by caching constraint calculations that remain constant for the frame and only re-calculating what is needed each iteration. In addition for realtime applications we can leverage warm starting as discussed below:
The reason the iterative approach can take so long to converge is because the starting result of the first iteration can be so far away from the actual solution. In a realtime application however we don't expect the bodies to move very far each frame and thus the correct solution for this frame also shouldn't be too far from the previous frames solution. Therefore by caching and applying the previous frames solution to our current frame before we start iterating we can nudge the system much closer to the correct solution initially. This then lets us use a much lower iteration count to solve the system.
To do this, when we solve a constraint we accumulate the calculated
On the next frame before we start iterating we first apply impulses to the bodies attached to each constraint using the cached lambdas:
After that continue to iteratively solve the constraints as normal.
Another approach is to combine the entire system into a single solver rather than processing each constraint seperately. Therefore given the above example, our system of equations to solve would become:
$$\lambda_{impulse} = \frac{-Jv}{JM^{-1}*J^T}$$
where
The elements of the Jacobian matrix can still be calculated the same way as discussed previously but the Jacobian will grow to be size n by 18 in this instance and care must be take to position the elements correctly so they effect the appropriate bodies.
For example imagine that constraints
Solving in this way is more accurate and requires no iteration so theoretically is faster, but it does have limitations:
- For a large number of bodies and constraints the system of equations can become excessively large. To solve this system performantly requires the implementation of sparse matricies and sparse solvers. Without this added optimisation the system of equations would be prohibitively expensive to solve and an iterative solution would be better.
- In implementation, solving constraints simultaneously is a lot less flexible. Using an iterative approach all constraint systems follow the same format allowing constraints to be easily added and removed on the fly by the user or application whilst resuing the same simple solver code over and over. For a simultaneous approach, the formulation of the system of equations changes depending on the number of bodies and constraints resulting in less flexibility without significant code complications.
For general constraint solutions or very large system I would therefore recommend the iterative approach. If you have a reasonable sized system of bodies and constraints that is defined before hand and will remain fixed, then a simultanous solving approach may be better.
Note that solving constraints will only result in the impulses necessary to enforce tangential movement to the constraint at that exact instance in time. Realtime applications split up their simulation into discrete time steps and iterating rigid bodies over these time steps results in them drifting relative to their constraints. Since the constraint impulses only enforce tangential movement, these impulses by themselves wont correct for this error and thus bodies will drift from their constraints further and further over time.
To correct for this drift you can evaluate the constraint function to determine the constraint error, the same as for testing against limits for conditional constraints. You can then use this error to calculate where each body should be positioned to rectify the constraint. If your simulation allows it, the bodies can simply be moved directly to the correct transforms after solving. Teleportation is usualy bad practice in most physics engine however since it can cause collision penetration and tunelling. In this case you should calculate restorative velocities or forces to apply to each body to move them towards the correct transform. One method for doing this is known as Baumgarte stabilisation.
The restorative force is applied by calcuating a Baumgarte term which is defined as a fraction
The
Below I will lay out psuedo code for a configurable 6 degree of freedom constraint structure that uses all the above principles. I also layout psuedo code for a simulation loop so you can see how this would fit into a physics solver:
struct SixDofConstraint
{
enum class EConstraintAxisType : int
{
Free,
Limit,
Fixed
};
struct ConstraintAxis
{
EConstraintAxisType Type = EConstraintAxisType::Fixed;
float MinLimit = 0.0f;
float MaxLimit = 0.0f;
};
void PreSolve(float deltaTime);
void Solve();
void PostSolve();
MatMN InverseMassMatrix() const;
VecN Velocities() const;
void ApplyImpulses(const VecN& impulses);
void Rectify();
MatMN LeftMatrix(const Quat& q) const;
MatMN RightMatrix(const Quat& q) const;
RigidBody* BodyA = nullptr;
RigidBody* BodyB = nullptr;
Transform JointALs = Transform::Identity; // the joint transform in bodyA's local space
Transform JointBLs = Transform::Identity; // the joint transform in bodyB's local space
ConstraintAxis PositionAxes[3];
ConstraintAxis RotationAxes[3];
float PositionAxisDistance[3] = {};
float RotationAxisAngle[3] = {};
MatMN Jacobian = MatMN(6, 12);
VecN LambdaCache = VecN(6);
};
void SixDofConstraint::PreSolve(float deltaTime)
{
// calculate world space properties
Transform jointAWs = JointALs;
Vec3 comAWs = Vec3::Zero;
if (BodyA)
{
jointAWs = BodyA->TransformWs.Transform(JointALs);
comAWs = BodyA->CenterOfMassWs();
}
Transform jointBWs = JointBLs;
Vec3 comBWs = Vec3::Zero;
if (BodyB)
{
jointBWs = BodyB->TransformWs.Transform(JointBLs);
comBWs = BodyB->CenterOfMassWs();
}
// calculate axis distances and angles
for (unsigned int axisIndex = 0; axisIndex < 3; ++axisIndex)
{
const Vec3 jointAxisWs = jointAWs.GetAxis(axisIndex);
PositionAxisDistance[axisIndex] = (jointBWs.Position - jointAWs.Position) | jointAxisWs;
RotationAxisAngle[axisIndex] = 2.0f * Math::Asin((jointAWs.Rotation.Inverse() * jointBWs.Rotation).ToVec3() | jointAxisWs);
}
// calculate properties for jacobian
const Vec3 comOffsetAWs = jointAWs.Position - comAWs;
const Vec3 comOffsetBWs = jointBWs.Position - comBWs;
MatMN proj(3, 4);
proj.Zero();
proj(0, 0) = 1.0f;
proj(1, 1) = 1.0f;
proj(2, 2) = 1.0f;
MatMN projTrans = proj.Transpose();
MatMN mat = proj * LeftMatrix(jointAWs.Rotation) * RightMatrix(jointBWs.Rotation.Inverse()) * projTrans * 0.5f;
Jacobian.Zero();
// add position axis constraints to jacobian
for (unsigned int posAxisIndex = 0; posAxisIndex < 3; ++posAxisIndex)
{
if (PositionAxes[posAxisIndex].Type == EConstraintAxisType::Free)
continue;
if (PositionAxes[posAxisIndex].Type == EConstraintAxisType::Limit
&& PositionAxisDistance[posAxisIndex] > PositionAxes[posAxisIndex].MinLimit
&& PositionAxisDistance[posAxisIndex] < PositionAxes[posAxisIndex].MaxLimit)
continue;
const Vec3 jointAxisWs = jointAWs.GetAxis(posAxisIndex);
Jacobian(posAxisIndex, 0) = -jointAxisWs.X;
Jacobian(posAxisIndex, 1) = -jointAxisWs.Y;
Jacobian(posAxisIndex, 2) = -jointAxisWs.Z;
const Vec3 jA = -comOffsetAWs ^ jointAxisWs;
Jacobian(posAxisIndex, 3) = jA.X;
Jacobian(posAxisIndex, 4) = jA.Y;
Jacobian(posAxisIndex, 5) = jA.Z;
Jacobian(posAxisIndex, 6) = jointAxisWs.X;
Jacobian(posAxisIndex, 7) = jointAxisWs.Y;
Jacobian(posAxisIndex, 8) = jointAxisWs.Z;
const Vec3 jB = comOffsetBWs ^ jointAxisWs;
Jacobian(posAxisIndex, 9) = jB.X;
Jacobian(posAxisIndex, 10) = jB.Y;
Jacobian(posAxisIndex, 11) = jB.Z;
}
// add rotation axis constraint to jacobian
for (unsigned int rotAxisIndex = 0; rotAxisIndex < 3; ++rotAxisIndex)
{
if (RotationAxes[rotAxisIndex].Type == EConstraintAxisType::Free)
continue;
if (RotationAxes[rotAxisIndex].Type == EConstraintAxisType::Limit
&& RotationAxisAngle[rotAxisIndex] > RotationAxes[rotAxisIndex].MinLimit
&& RotationAxisAngle[rotAxisIndex] < RotationAxes[rotAxisIndex].MaxLimit)
continue;
const Vec3 jointAxisWs = jointAWs.GetAxis(rotAxisIndex);
const Vec3 j = mat * jointAxisWs;
Jacobian(rotAxisIndex + 3, 3) = j.X;
Jacobian(rotAxisIndex + 3, 4) = j.Y;
Jacobian(rotAxisIndex + 3, 5) = j.Z;
Jacobian(rotAxisIndex + 3, 9) = -j.X;
Jacobian(rotAxisIndex + 3, 10) = -j.Y;
Jacobian(rotAxisIndex + 3, 11) = -j.Z;
}
// apply warm starting
const VecN impulses = Jacobian.Transpose() * LambdaCache;
ApplyImpulses(impulses);
}
void SixDofConstraint::Solve()
{
// build system of equations
const MatMN jacobianTranspose = Jacobian.Transpose();
const VecN velocities = Velocities();
const MatMN invMassMatrix = InverseMassMatrix();
const MatMN A = Jacobian * invMassMatrix * jacobianTranspose;
VecN b = Jacobian * velocities * -1.0f;
// solve for the lagrange multipliers
const VecN lambda = Math::GaussSeidel(A, b, b.Size);
// apply impulses
const VecN impulses = jacobianTranspose * lambda;
ApplyImpulses(impulses);
// accumulate lambda for warm starting
LambdaCache += lambda;
}
void SixDofConstraint::PostSolve()
{
// limit warming to reasonable limits
constexpr float warmingLimit = 1.0e5f;
for (unsigned int i = 0; i < LambdaCache.Size; ++i)
LambdaCache[i] = Math::Clamp(LambdaCache[i], -warmingLimit, warmingLimit);
// rectify drift
Rectify();
}
MatMN SixDofConstraint::InverseMassMatrix() const
{
MatMN invMassMatrix(12, 12);
invMassMatrix.Zero();
if (BodyA)
{
invMassMatrix(0, 0) = BodyA->InvMass;
invMassMatrix(1, 1) = BodyA->InvMass;
invMassMatrix(2, 2) = BodyA->InvMass;
const RMat33 invInertiaWsA = BodyA->InvInertiaTensorWs();
for (unsigned int i = 0; i < 3; ++i)
{
invMassMatrix(3 + i, 3) = invInertiaWsA(i, 0);
invMassMatrix(3 + i, 4) = invInertiaWsA(i, 1);
invMassMatrix(3 + i, 5) = invInertiaWsA(i, 2);
}
}
if (BodyB)
{
invMassMatrix(6, 6) = BodyB->InvMass;
invMassMatrix(7, 7) = BodyB->InvMass;
invMassMatrix(8, 8) = BodyB->InvMass;
const RMat33 invInertiaWsB = BodyB->InvInertiaTensorWs();
for (unsigned int i = 0; i < 3; ++i)
{
invMassMatrix(9 + i, 9) = invInertiaWsB(i, 0);
invMassMatrix(9 + i, 10) = invInertiaWsB(i, 1);
invMassMatrix(9 + i, 11) = invInertiaWsB(i, 2);
}
}
return invMassMatrix;
}
VecN SixDofConstraint::Velocities() const
{
VecN velocities(12);
velocities.Zero();
if (BodyA)
{
velocities[0] = BodyA->LinearVelocityWs.X;
velocities[1] = BodyA->LinearVelocityWs.Y;
velocities[2] = BodyA->LinearVelocityWs.Z;
velocities[3] = BodyA->AngularVelocityWs.X;
velocities[4] = BodyA->AngularVelocityWs.Y;
velocities[5] = BodyA->AngularVelocityWs.Z;
}
if (BodyB)
{
velocities[6] = BodyB->LinearVelocityWs.X;
velocities[7] = BodyB->LinearVelocityWs.Y;
velocities[8] = BodyB->LinearVelocityWs.Z;
velocities[9] = BodyB->AngularVelocityWs.X;
velocities[10] = BodyB->AngularVelocityWs.Y;
velocities[11] = BodyB->AngularVelocityWs.Z;
}
return velocities;
}
void SixDofConstraint::ApplyImpulses(const VecN& impulses)
{
if (BodyA)
{
BodyA->ApplyLinearImpulseWs(Vec3(&impulses.Data[0]));
BodyA->ApplyAngularImpulseWs(Vec3(&impulses.Data[3]));
}
if (BodyB)
{
BodyB->ApplyLinearImpulseWs(Vec3(&impulses.Data[6]));
BodyB->ApplyAngularImpulseWs(Vec3(&impulses.Data[9]));
}
}
void SixDofConstraint::Rectify()
{
// calcualte world space properties
const Transform jointAWs = BodyA ? BodyA->TransformWs.Transform(JointALs) : JointALs;
const Transform jointBWs = BodyB ? BodyB->TransformWs.Transform(JointBLs) : JointBLs;
// apply position corrections
for (unsigned int axisIndex = 0; axisIndex < 3; ++axisIndex)
{
float positionError = 0.0f;
switch (PositionAxes[axisIndex].Type)
{
case EConstraintAxisType::Free:
continue;
break;
case EConstraintAxisType::Limit:
if (PositionAxisDistance[axisIndex] < PositionAxes[axisIndex].MinLimit)
positionError = PositionAxisDistance[axisIndex] - PositionAxes[axisIndex].MinLimit;
else if (PositionAxisDistance[axisIndex] > PositionAxes[axisIndex].MaxLimit)
positionError = PositionAxisDistance[axisIndex] - PositionAxes[axisIndex].MaxLimit;
else
continue;
break;
case EConstraintAxisType::Fixed:
positionError = PositionAxisDistance[axisIndex];
break;
}
const Vec3 jointAxisWs = jointAWs.GetAxis(axisIndex);
const Vec3 positionCorrectionWs = 0.5f * positionError * jointAxisWs;
if (BodyA)
BodyA->TransformWs.Position += positionCorrectionWs;
if (BodyB)
BodyB->TransformWs.Position -= positionCorrectionWs;
}
// apply rotation corrections
for (unsigned int axisIndex = 0; axisIndex < 3; ++axisIndex)
{
float rotationError = 0.0f;
switch (RotationAxes[axisIndex].Type)
{
case EConstraintAxisType::Free:
continue;
break;
case EConstraintAxisType::Limit:
if (RotationAxisAngle[axisIndex] < RotationAxes[axisIndex].MinLimit)
rotationError = RotationAxisAngle[axisIndex] - RotationAxes[axisIndex].MinLimit;
else if (RotationAxisAngle[axisIndex] > RotationAxes[axisIndex].MaxLimit)
rotationError = RotationAxisAngle[axisIndex] - RotationAxes[axisIndex].MaxLimit;
else
continue;
break;
case EConstraintAxisType::Fixed:
rotationError = RotationAxisAngle[axisIndex];
break;
}
const Vec3 jointAxisWs = jointAWs.GetAxis(axisIndex);
const RQuat rotationCorrectionWs(jointAxisWs, 0.5f * rotationError);
if (BodyA)
BodyA->TransformWs.Rotation *= rotationCorrectionWs;
if (BodyB)
BodyB->TransformWs.Rotation *= rotationCorrectionWs.Inverse();
}
}
MatMN SixDofConstraint::LeftMatrix(const RQuat& q)
{
MatMN mat(4, 4);
mat(0, 0) = q.X;
mat(0, 1) = -q.Y;
mat(0, 2) = -q.Z;
mat(0, 3) = -q.W;
mat(1, 0) = q.Y;
mat(1, 1) = q.X;
mat(1, 2) = -q.W;
mat(1, 3) = q.Z;
mat(2, 0) = q.Z;
mat(2, 1) = q.W;
mat(2, 2) = q.X;
mat(2, 3) = -q.Y;
mat(3, 0) = q.W;
mat(3, 1) = -q.Z;
mat(3, 2) = -q.Y;
mat(3, 3) = q.X;
return mat;
}
MatMN SixDofConstraint::RightMatrix(const RQuat& q)
{
MatMN mat(4, 4);
mat(0, 0) = q.X;
mat(0, 1) = -q.Y;
mat(0, 2) = -q.Z;
mat(0, 3) = -q.W;
mat(1, 0) = q.Y;
mat(1, 1) = q.X;
mat(1, 2) = q.W;
mat(1, 3) = -q.Z;
mat(2, 0) = q.Z;
mat(2, 1) = -q.W;
mat(2, 2) = q.X;
mat(2, 3) = q.Y;
mat(3, 0) = q.W;
mat(3, 1) = q.Z;
mat(3, 2) = -q.Y;
mat(3, 3) = q.X;
return mat;
}
void Solver::Update(float deltaTime)
{
// update rigid bodies
for (RigidBody* pBody : RigidBodyArray)
pBody->Update(deltaTime);
// apply constraints
for (Constraint* pConstraint : ConstraintArray)
pConstraint->PreSolve(deltaTime);
constexpr unsigned int numConstrainSolverSteps = 5;
for (unsigned int i = 0; i < numConstrainSolverSteps; ++i)
{
for (Constraint* pConstraint : ConstraintArray)
pConstraint->Solve();
}
for (Constraint* pConstraint : ConstraintArray)
pConstraint->PostSolve();
}