Constraint Solving - nullstar/SecondBrain GitHub Wiki

Constraint Solving

Overview

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.

Fundamentals

Defining Constraints

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 $\vec{r}$ to be a certain distance $l$ from the origin could be defined with the constraint function $C$:

$$C_{(r)} = \vec{r}.\vec{r} - l*l = 0$$

Let us generalise this for generic coordinates to create a method for solving all constraint types. Our constraint function becomes:

$$C_{(q_1, q_2, ..., q_N)} = 0$$

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 $Q_{int}$ and $Q_{ext}$ are the general internal and external forces respectively, $v$ is the general velocities, $M$ is the mass matrix, and $J$ is the Jacobian matrix with $J'$ being its derivative. The first equation forms a system of linear equations which can be solved using an appropriate method such as the Gauss–Seidel method to find $\lambda_{force}$. This is then substituted into the second equation to find $Q_{int}$.

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.

Impulse Based Dynamics

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 $Imp$ required to resolve the constraint rather than the internal force:

$$Imp = J^T*\lambda_{impulse}$$

Fortunately converting $\lambda_{force}$ to $\lambda_{impulse}$ is easy by remembering that impulse is just the integral of force over time. In addition most physics engines will have already applied the results of external forces to their bodies before resolving constraints so we can eliminate the $Q_{ext}$ term. Therefore our $\lambda$ equation becomes:

$$\lambda_{impulse} = \frac{-Jv}{JM^{-1}*J^T}$$

Equation Parameters

Consider a constraint that joins two rigid bodies together which we identify by the subscripts 1 and 2. In this system $Imp$ is a vector that contain the linear impulse $\Delta P$ and angular impulse $\Delta L$ vectors that act on each body:

$$Imp = \left(\begin{array} & \vec{\Delta P_1}, & \vec{\Delta L_1}, & \vec{\Delta P_2}, & \vec{\Delta L_2} \end{array}\right)$$

$v$ is a vector that contains the linear and angular velocity vectors of both bodies:

$$v = \left(\begin{array} & \vec{v_1}, & \vec{\omega_1}, & \vec{v_2}, & \vec{\omega_2} \end{array}\right)$$

$M$ is a sparse matrix that contains the mass and inertia tensors of both bodies where $m_n$ is mass of body n, $I$ is the identity matrix, and $I_n$ is the inertia tensor of body n:

$$M = \left(\begin{array} & m_1I, & I_1, & m_2I, & I_2 \end{array}\right) = \left(\begin{array} & m_{1x} & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 \\ 0 & m_{1y} & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 \\ 0 & 0 & m_{1z} & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 \\ 0 & 0 & 0 & I_{1xx} & I_{1xy} & I_{1xz} & 0 & 0 & 0 & 0 & 0 & 0 \\ 0 & 0 & 0 & I_{1yx} & I_{1yy} & I_{1yz} & 0 & 0 & 0 & 0 & 0 & 0 \\ 0 & 0 & 0 & I_{1zx} & I_{1zy} & I_{1zz} & 0 & 0 & 0 & 0 & 0 & 0 \\ 0 & 0 & 0 & 0 & 0 & 0 & m_{2x} & 0 & 0 & 0 & 0 & 0 \\ 0 & 0 & 0 & 0 & 0 & 0 & 0 & m_{2y} & 0 & 0 & 0 & 0 \\ 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & m_{2z} & 0 & 0 & 0 \\ 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & I_{2xx} & I_{2xy} & I_{2xz} \\ 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & I_{2yx} & I_{2yy} & I_{2yz} \\ 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & I_{2zx} & I_{2zy} & I_{2zz} \\ \end{array}\right)$$

Jacobian Matrix Calculation

To calculate the Jacobian we need to refactor our constraint equation $C$. First we need to calculate the derivative of the constraint equation $C'$. Then we use the velocity constraint definition which relates the constraint equation to the Jacobian according to the following equation:

$$C' = J*v$$

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.

Core Constraints

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.

Position Constraint

The position constraint ensures that the distance between the bodies and the joint always remains constant. If $\vec{r_n}$ is the world space position of the joint according to body n calculated by transforming the bodies local space joint vector by the bodies world transform, then the constraint function for this constraint is:

$$C = (\vec{r_2} - \vec{r_1}).(\vec{r_2} - \vec{r_1}) = 0$$

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:

$$C' = 2*(\vec{r_2} - \vec{r_1}).\vec{r_2}' + 2*(\vec{r_1} - \vec{r_2}).\vec{r_1}'$$

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 $\vec{c_n}$ is the world space offset to the joint position from body n's center of mass:

$$\vec{r_n}' = \vec{v_n} + \vec{\omega_n} \times \vec{c_n}$$

Substituting those in, applying some vector identities to reorder the cross products, and rewriting in matrix form gives the following:

$$C' = \left(\begin{array}&2*(\vec{r_1} - \vec{r_2}), &2*\vec{c_1} \times (\vec{r_1} - \vec{r_2}), &2*(\vec{r_2} - \vec{r_1}), &2*\vec{c_2} \times (\vec{r_2} - \vec{r_1})\end{array}\right) \left(\begin{array}&\vec{v_1}\\vec{\omega_1}\\vec{v_2}\\vec{\omega_2}\end{array}\right)$$

By inspection with the velocity constraint function ($C' = J*v$) we can see that:

$$J = \left(\begin{array}&2*(\vec{r_1} - \vec{r_2}), &2*\vec{c_1} \times (\vec{r_1} - \vec{r_2}), &2*(\vec{r_2} - \vec{r_1}), &2*\vec{c_2} \times (\vec{r_2} - \vec{r_1})\end{array}\right)$$

The Jacobian ends up as a 1 by 12 matrix.

Rotation Constraint

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 $q = (q_x, q_y, q_z, q_w)$ we will define the following two functions:

$$L(q) = \left(\begin{array}& q_x & -q_y & -q_z & -q_w \\ q_y & q_x & -q_w & q_z \\ q_z & q_w & q_x & -q_y \\ q_w & -q_z & -q_y & q_x \end{array}\right)$$

$$R(q) = \left(\begin{array}& q_x & -q_y & -q_z & -q_w \\ q_y & q_x & q_w & -q_z \\ q_z & -q_w & q_x & q_y \\ q_w & q_z & -q_y & q_x \end{array}\right)$$

If $\vec{p_n}$ is the local space rotation of the joint from body n's frame, and $\vec{q_n}$ is the world space rotation of the joint according to body n calculated by transforming $\vec{p_n}$ by the bodies world space transform, then the constraint function for this constraint is:

$$C = P.(q_1.q_2^{-1}) = 0$$

where P is a "projection" matrix:

$$P = \left(\begin{array}& 1 & 0 & 0 & 0 \\ 0 & 1 & 0 & 0 \\ 0 & 0 & 1 & 0 \\ \end{array}\right)$$

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 $L(q)$ and $R(q)$ functions to convert to matrix form, we can see by inspection that the Jacobian is:

$$J = \left(\begin{array}& 0, & \frac{1}{2}P.L(q_1).R(q_2^{-1}).P^T, & 0, & -\frac{1}{2}P.L(q_1).R(q_2^{-1}).P^T \end{array}\right)$$

The Jacobian ends up as a 3 by 12 matrix.

Fixed Constraint

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:

$$J = \left(\begin{array}& 2*(\vec{r_1} - \vec{r_2}), &2*\vec{c_1} \times (\vec{r_1} - \vec{r_2}), &2*(\vec{r_2} - \vec{r_1}), &2*\vec{c_2} \times (\vec{r_2} - \vec{r_1}) \\ 0, & \frac{1}{2}P.L(q_1).R(q_2^{-1}).P^T, & 0, & -\frac{1}{2}P.L(q_1).R(q_2^{-1}).P^T \end{array}\right)$$

Axis Constraints

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.

Position Axis Constraint

To constrain position along a given axis $\vec{n}$ we only need to change the position constraint so that the difference between world space anchor point according to each body when projected onto the axis is zero. Therefore the constraint equation becomes:

$$C = \vec{n}.(\vec{r_2} - \vec{r_1}) = 0$$

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:

$$J = \left(\begin{array}& -\vec{n}, & -\vec{c_1} \times \vec{n}, & \vec{n}, & \vec{c_2} \times \vec{n} \end{array}\right)$$

These constraints can be combined to constrain on multiple axes simultaneously. For example consider that $\vec{m}$ is an axis tangental to $\vec{n}$ that we also wish to constrain on. The Jacobian then becomes the following 2 by 12 matrix:

$$J = \left(\begin{array}& -\vec{n}, & -\vec{c_1} \times \vec{n}, & \vec{n}, & \vec{c_2} \times \vec{n} \\ -\vec{m}, & -\vec{c_1} \times \vec{m}, & \vec{m}, & \vec{c_2} \times \vec{m} \end{array}\right)$$

Rotation Axis Constraint

Constraining rotation around an axis $\vec{n}$ follows the same principle and we only need to modify the rotation constraint function so that it's projection along the axis in zero. There the constraint function becomes:

$$C = P.(q_1.q_2^{-1}).\vec{n} = 0$$

Applying the same mathematical process as before we get the following 1 by 12 Jacobian matrix:

$$J = \left(\begin{array}& 0, & \frac{1}{2}P.L(q_1).R(q_2^{-1}).P^T.\vec{n}, & 0, & -\frac{1}{2}P.L(q_1).R(q_2^{-1}).P^T.\vec{n} \end{array}\right)$$

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 $\vec{n}$ and $\vec{m}$ that are tangential to the allowed rotation axis to get a 2 by 12 Jacobian:

$$J = \left(\begin{array}& 0, & \frac{1}{2}P.L(q_1).R(q_2^{-1}).P^T.\vec{n}, & 0, & -\frac{1}{2}P.L(q_1).R(q_2^{-1}).P^T.\vec{n} \\ 0, & \frac{1}{2}P.L(q_1).R(q_2^{-1}).P^T.\vec{m}, & 0, & -\frac{1}{2}P.L(q_1).R(q_2^{-1}).P^T.\vec{m} \end{array}\right)$$

Conditional Constraints

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:

Penetration Constraint

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:

$$C = \vec{n}.(\vec{r_2} - \vec{r_1}) >= 0$$

The Jacobian is the same as for the position axis constraint:

$$J = \left(\begin{array}& -\vec{n}, & -\vec{c_1} \times \vec{n}, & \vec{n}, & \vec{c_2} \times \vec{n} \end{array}\right)$$

When solving this constraint we first evaluate the contraint function $C$ to determine the penetration distance $d$.

$$d = \vec{n}.(\vec{r_2} - \vec{r_1})$$

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.

Angle Limit Constraint

Say we are constraining rotation on an axis $\vec{n}$ but we want to allow a limited angle of movement and prevent rotation beyond this angle. This could be used to build a limited hinge for example. To do this we would implement a rotation axis constraint but before solving we would first calculate the angle of rotation and only apply the constraint if this angle was beyond the limit. If we assume that original orientation of the constrained bodies with respect to the joint is a rotation angle of zero, then we can calculate the current rotation angle $\theta$ using:

$$q_{rr} = q_1.q_2^{-1}$$ $$\theta = 2 * arcsin(\vec{q_{rr}}.\vec{n})$$

where $\vec{q_{rr}}$ is a vector made from the $x$, $y$, and $z$ components of the quaternion $q_{rr}$ ignoring the $w$ component. If the calculated angle $\theta$ is inside of our rotation limit then we ignore the constraint, otherwise we solve the constraint and apply the resulting impulses to the bodies.

Constraint Chaining

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 $B_1$, $B_2$, and $B_3$ where bodies $B_1$ and $B_2$ are connected by constraint $C_1$ and bodies $B_2$ and $B_3$ are connected by constraint $C_2$. If we first solve $C_1$ and then solve $C_2$, the outcome of solving $C_2$ will result in $C_1$ becoming invalid.

There are multiple way we can tackle this problem:

Iterative Solving

In the example given above, imagine after solving $C_2$ we then went back and solved $C_1$ again. This would make $C_1$ correct but would invalidate $C_2$ again. In most cases however $C_2$ will end up closer to a valid result than it started. We could therefore continue to keep solving for $C_1$ and $C_2$ back and forth and they should slowly converge to the correct solution where they are both valid.

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:

Warm Starting

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 $\lambda_{impulse}$ into a cache for that constraint.

$$\lambda_{cached} += \lambda_{impulse}$$

On the next frame before we start iterating we first apply impulses to the bodies attached to each constraint using the cached lambdas:

$$Imp = J^T*\lambda_{cached}$$

After that continue to iteratively solve the constraints as normal.

Simultaneous Solving

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}$$

$$Imp = J^T*\lambda_{impulse}$$

where

$$Imp = \left(\begin{array} & \vec{\Delta P_1}, & \vec{\Delta L_1}, & \vec{\Delta P_2}, & \vec{\Delta L_2}, & \vec{\Delta P_3}, & \vec{\Delta L_3} \end{array}\right)$$

$$v = \left(\begin{array} & \vec{v_1}, & \vec{\omega_1}, & \vec{v_2}, & \vec{\omega_2}, & \vec{v_3}, & \vec{\omega_3} \end{array}\right)$$

$$M = \left(\begin{array} & m_1I, & I_1, & m_2I, & I_2, & m_3I, & I_3 \end{array}\right)$$

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 $C_1$ and $C_2$ are both penetration constraints with axes $n_1$ and $n_2$ respectively. The world space vectors from the body center of mass to the joint position are defined as $c_{ij}$ where $i$ denotes the body index and $j$ denotes the constraint index. The Jacobian for this system would be:

$$J = \left(\begin{array}& -\vec{n_1}, & -\vec{c_{11}} \times \vec{n_1}, & \vec{n_1}, & \vec{c_{21}} \times \vec{n_1}, & 0, & 0 \\ 0, & 0, & -\vec{n_2}, & -\vec{c_{22}} \times \vec{n_2}, & \vec{n_2}, & \vec{c_{32}} \times \vec{n_2} \end{array}\right)$$

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.

Constraint Correction

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.

Baumgarte Stabilisation

The restorative force is applied by calcuating a Baumgarte term which is defined as a fraction $\beta$ of the constraint error over the time step:

$$baumgarte = -\frac{\beta.C}{\Delta t}$$

The $\beta$ term is a factor between 0 and 1 that controls the rate at which bodies conform to the correct transforms and should be hand tuned. A reasonable starting point is around 0.05. For positional constraints the Baumgarte term should then be subtracted from the right hand side vector term passed to the simultaneous equation solver when calculating $\lambda$.

Putting It All Together

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();
}
⚠️ **GitHub.com Fallback** ⚠️