Dynamic State
Given $n$ 3d bodies, the dynamic state of the $i^{\text{th}}$ body at time $t$ is its position, orientation, linear velocity, and angular velocity: $\left[\begin{array}{cccc}x_{it}&R_{it}&v_{it}&\omega_{it}\end{array}\right]$. The body also has mass $m_i$ and inertia tensor $\mathcal{I}_i$. The body also has geometry for the purposes of collision, but we won't worry about that here. We will assume that all of these values are framed in the global coordinate system (this means that $\mathcal{I}$ is actually a function of time since it's affected by $R$, but we'll assume that's taken care of).Integration Step
The dynamics equations start-out straightforward: $f = ma$. A single 3d rigid body has six degrees of freedom; so we get $a = M^{-1}\left[\begin{array}{c} f\\ \tau \end{array}\right]$ where $M=\left[\begin{array}{cc}mI&0\\0&\mathcal{I}\end{array}\right]$ and $f$ and $\tau$ are force and torque. After a time step of $h$ using semi-implicit Euler integration (and lumping gyroscopic effects into $\tau$), the future velocity is the current velocity plus the acceleration, scaled by time: $\left[\begin{array}{c}v_{t+h}\\ \omega_{t+h}\end{array}\right] = \left[\begin{array}{c}v_{t} \\ \omega_{t}\end{array}\right] + ha$. Then the future position is the current position plus the future velocity scaled by time: $x_{t+h} = x_t + hv_{t+h}$ and $R_{t+h} = \mathcal{R}(h\omega_{t+h})R_t$ where $\mathcal{R}()$ is a function that makes a rotation matrix out of time-scaled angular velocity (when using ODE's FiniteRotationMode, otherwise, the orientation, in quaternion form, is updated the same way as position and then re-normalized).Constraint Equation
So far, so good. When a rigid body is constrained in some way, we need an extra step to figure out the forces resulting from constraints. If our $n$ bodies are all part of a single interacting system and we constrain $k$ degrees of freedom, then we need to create a matrix $J$ of size $\left\{k \times 6n\right\}$. Each row of $J$ describes a linear relationship between all of these bodies.The ODE document on creating new joints does a good job describing how to compute individual rows of $J$. Russell Smith (original author of ODE) expanded on this in "Constraints in rigid body dynamics" (Game Programming Gems, 2004). Erin Catto also put together a nice paper that covers forming and resolving constraints. This paper also describes the iterative Projected Gauss-Seidel algorithm for solving the constraint equations. The ODE function, dWorldQuickStep(), uses this method. For my work, I prefer the dWorldStep() solver which uses a slower, but more accurate algorithm sometimes called Lemke's algorithm and sometimes called the Dantzig-Cottle simplex algorithm. David Baraff seems to get credit for introducing this algorithm as a solver for physical simulation with contacts and friction.
In ODE, constraints start out being applied to velocities, but then are actually solved as accelerations. A single "constraint row" applies to a single degree of freedom. We frame a constraint stating that a body can't move along the $x$ axis as $[\begin{array}{cccccc}1&0&0&0&0&0\end{array}]\left[\begin{array}{c}v_{t} \\ \omega_{t}\end{array}\right] = 0$. Solving this constraint is easy. We just need a force that cancels out whatever forces and momentum exist along that axis. This leads to zero acceleration (or acceleration canceling our current velocity) for a final velocity of zero.
If we have many such constraints, it still boils down to solving a linear system. When solved as a purely linear system, however, if we aren't careful and accidentally over-constrain the system, things break. This is why ODE applies constraint-force mixing (CFM) component which, as discussed in previous posts, allows constraints to slip proportional to the force required to satisfy them. If we turn to the ODE manual we see the $\text{CFM}$ augmented constraint framed as, $J\left[\begin{array}{c}v_{t+h} \\ \omega_{t+h}\end{array}\right] = c + \text{CFM}\lambda$. The right hand side should actually read, $c - \text{CFM}\lambda$, because that is how it's used in the code.
The $j^{\text{th}}$ row of $J$ looks like this:
$\left[\begin{array}{ccccc}p_{j1}&q_{j1}&\cdots&p_{jn}&q_{jn}\end{array}\right]$
where $p_{ji}$ is applied to $v_i$ and $q_{ji}$ is applied to $\omega_i$. Good examples exist in the sources cited above. Each row of $J$ also has associated elements that define limits on the force ($\text{lo}_j$ and $\text{hi}_j$), springiness of each degree of freedom ($\text{cfm}_j$), the desired relative velocity along the constraint ($c_j$), and a reference to another row ($\text{findex}_j$) that modulates $\text{lo}_j$ and $\text{hi}_j$ for friction constraints (to make friction proportional to normal force).
So we have our desired constraint relationships: $J\left[\begin{array}{ccccc}v_1&\omega_1&\cdots&v_n&\omega_n \end{array}\right]^\intercal = c - \text{CFM}\lambda$. In ODE, the rows of $J$ are restricted to dealing with two or fewer bodies, with all other columns set to zero. That simplifies the code, but there are interesting constraints that involve three or more bodies and you can't easily create those.
Final Equation in Terms of Acceleration
For the solver, we re-frame the system as constraints on acceleration. When we project the velocity into acceleration, we divide both sides by time and then replace the left hand-side (acceleration) with inverse-mass times force: $JM^{-1}J^\intercal\lambda$ (here $M$ represents the diagonal mass and the rotated inertia tensor together). Grouping terms involving $\lambda$, we get $\left(JM^{-1}J^\intercal + \frac{1}{h}\text{CFM}\right)\lambda = \frac{1}{h}c$. Since $c$ is the desired velocity, converting it into an acceleration this way assumes that the starting velocity is zero. To correct for that, we subtract out the current velocity, projected onto the constraint space. We also need to subtract out the effects of external forces, $F$ (gravity, gyroscopic torques, and any forces the user may have applied) since they affect the bodies' velocities. This gives us the final equation:$\left(JM^{-1}J^\intercal + \frac{1}{h}\text{CFM}\right)\lambda = \frac{1}{h}c -J\left(M^{-1}F+\frac{1}{h}v_t\right)$.
where $\text{CFM}$ is a diagonal matrix and $F$ is the vector of externally applied forces and torques.
For convenience, we group the pieces of this equation into three variables: $A\lambda = b$. In spirit, this equation is still $m^{-1}f = a$, and we need to solve for $f$. The solver finds $\lambda$ that satisfies the above system subject to constraints $\text{lo}\leq\lambda\leq\text{hi}$. The solver algorithms are quite interesting, but are a topic for another day. Once we have solved for $\lambda$, we project out of constraint space back into force space and add it to the other forces, $F_{\text{new}} = F_{\text{old}} + J^\intercal\lambda$, and we integrate to find our new velocities and positions.
