Algorithm

The dynamics of an object can be described by $n$ vertices and $M$ constraints on vertex position. The mass, position and velocity of the $i$-th vertex are denoted as $m_i,\mathbf{x}_i,\mathbf{v}_i$ respectively. The $j$-th constraint may be an equality or inequality, written as

$$C_{j}(\mathbf{x}_{1}, \cdots, \mathbf{x}_{N})=0\quad \text{或}\quad C_{j}(\mathbf{x}_{1}, \cdots, \mathbf{x}_{N})\ge0.$$

We use the symbol $\succ$ to denote $=$ or $\ge$ and let $\mathbf{x}=(\mathbf{x}_{1}, \cdots, \mathbf{x}_{N})$. Then the $j$-th constraint is denoted as $C_{j}(\mathbf{x})\succ 0$.

Position based dynamics can be abbreviated as PBD. The algorithm first calculates the initial predicted position of each vertex $\mathbf{p}_i(i=1,2,\cdots,N)$, and then try to move $\mathbf{p}_i(i=1,2,\cdots,N)$ to satisfy all the constraints

$$C_j\succ 0(j=1,2,\cdots,M).$$

The specific algorithm of PBD can be represented by the following pseudo code.

Figure 1 - Pseudo code for PBD

Solver

In the PBD pseudocode, lines 8 to 10 are the iterative solver. Here, the function

$$\mathrm{projectConstraints}(C_{1}, \cdots, C_{M + M_{\mathrm{Coll}}}, \mathbf{p}_{1}, \ldots, \mathbf{p}_{N})$$

is a nonlinear Gauss-Seidel solver for the system of equations/inequalities

$$\left\{ \begin{array}{l} C_{1}(\mathbf{p}_{1}, \cdots, \mathbf{p}_{N}) \succ 0, \\ C_{2}(\mathbf{p}_{1}, \cdots, \mathbf{p}_{N}) \succ 0, \\ \quad \ldots \\ C_{M + M_{\mathrm{Coll}}}(\mathbf{p}_{1}, \cdots, \mathbf{p}_{N}) \succ 0, \\ \end{array} \right.$$

The solver $\mathrm{projectConstraints}$ will linearize and solve each constraint $C_{1}, C_{2}, \cdots, C_{M + M_{\mathrm{Coll}}}$ in turn. Each time a constraint $C_{j}$ is solved, the solver will immediately update the values of $\mathbf{p}_{1}, \mathbf{p}_{2}, \cdots, \mathbf{p}_{N}$ to affect the solution of $C_{j+1}$. Below we will specifically introduce the solution method for $C_{j}$.

If the constraint is an equation, we want to find a $\Delta \mathbf{p}$ such that $C_{j}(\mathbf{p} + \Delta \mathbf{p}) = 0$, where $\mathbf{p} = (\mathbf{p}_{1}, \ldots, \mathbf{p}_{N})$. Linearizing it to the first order, we get

$$C_{j}(\mathbf{p} + \Delta \mathbf{p}) \approx C_{j}(\mathbf{p}) + \nabla C_{j}(\mathbf{p}) \cdot \Delta \mathbf{p} = 0.$$

The above equation generally has infinitely many solutions. If we restrict the solution $\Delta \mathbf{p} = (\Delta \mathbf{p}_{1}, \Delta \mathbf{p}_{2}, \cdots, \Delta \mathbf{p}_{N})$ to be in the direction of

$$(w_1 \nabla_{\mathbf{p}_1} C_{j}(\mathbf{p}), w_2 \nabla_{\mathbf{p}_2} C_{j}(\mathbf{p}), \cdots, w_N \nabla_{\mathbf{p}_N} C_{j}(\mathbf{p})),$$

that is, let

$$\Delta \mathbf{p}_i = \lambda w_i \nabla_{\mathbf{p}_i} C_{j}(\mathbf{p}), \quad i = 1, 2, \cdots, N,$$

we can obtain a particular solution

$$\Delta \mathbf{p}_i = -\frac{w_i C_{j}(\mathbf{p})}{\sum_{k=1}^N w_{k} \left|\nabla_{\mathbf{p}_k} C_{j}(\mathbf{p})\right|^{2}} \nabla_{\mathbf{p}_i} C_{j}(\mathbf{p}), \quad i = 1, 2, \cdots, N.$$

The solution method for $C_{j}$ is to update the original $\mathbf{p}$ with $\mathbf{p} + \Delta \mathbf{p}$. Therefore, the $\mathrm{projectConstraints}$ function can be represented in pseudocode as follows

Figure 2 - Pseudocode for the $\mathrm{projectConstraints}$ Solver

Specific constraints

Stretch

The stretch constraint is

$$C(\mathbf{p}_1,\mathbf{p}_2) = \left\Vert \mathbf{p}_1-\mathbf{p}_2\right\Vert - d,$$

where $d$ represents the initial length. The gradient is calculated as

$$\begin{aligned} \nabla_{\mathbf{p}_1} C(\mathbf{p}_1,\mathbf{p}_2)&=\nabla_{\mathbf{p}_1} \left\Vert \mathbf{p}_1-\mathbf{p}_2\right\Vert\\ &=\nabla_{\mathbf{p}_1} \left(\left(\mathbf{p}_{1x}-\mathbf{p}_{2x}\right)^2+\left(\mathbf{p}_{1y}-\mathbf{p}_{2y}\right)^2+\left(\mathbf{p}_{1z}-\mathbf{p}_{2z}\right)^2\right)^{\frac{1}{2}}\\ &=\frac{1}{2\left\Vert \mathbf{p}_1-\mathbf{p}_2\right\Vert}2(\mathbf{p}_1-\mathbf{p}_2)\\ &=\frac{\mathbf{p}_1-\mathbf{p}_2}{\left\Vert \mathbf{p}_1-\mathbf{p}_2\right\Vert}, \end{aligned}$$ $$\begin{aligned} \nabla_{\mathbf{p}_2} C(\mathbf{p}_1,\mathbf{p}_2)&=\nabla_{\mathbf{p}_2} \left\Vert \mathbf{p}_1-\mathbf{p}_2\right\Vert\\ &=\nabla_{\mathbf{p}_2} \left(\left(\mathbf{p}_{1x}-\mathbf{p}_{2x}\right)^2+\left(\mathbf{p}_{1y}-\mathbf{p}_{2y}\right)^2+\left(\mathbf{p}_{1z}-\mathbf{p}_{2z}\right)^2\right)^{\frac{1}{2}}\\ &=\frac{1}{2\left\Vert \mathbf{p}_1-\mathbf{p}_2\right\Vert}2(\mathbf{p}_2-\mathbf{p}_1)\\ &=\frac{\mathbf{p}_2-\mathbf{p}_1}{\left\Vert \mathbf{p}_1-\mathbf{p}_2\right\Vert}. \end{aligned}$$

Thus

$$\begin{aligned} \lambda&=\frac{\left\Vert \mathbf{p}_1-\mathbf{p}_2\right\Vert - d}{w_1+w_2},\\ \Delta \mathbf{p}_1&=-\frac{\left\Vert \mathbf{p}_1-\mathbf{p}_2\right\Vert - d}{w_1+w_2}w_1\frac{\mathbf{p}_1-\mathbf{p}_2}{\left\Vert \mathbf{p}_1-\mathbf{p}_2\right\Vert}=\frac{w_1}{w_1+w_2}\left(1-\frac{d}{\left\Vert \mathbf{p}_1-\mathbf{p}_2\right\Vert}\right)(\mathbf{p}_2-\mathbf{p}_1),\\ \Delta \mathbf{p}_2&=-\frac{\left\Vert \mathbf{p}_1-\mathbf{p}_2\right\Vert - d}{w_1+w_2}w_2\frac{\mathbf{p}_2-\mathbf{p}_1}{\left\Vert \mathbf{p}_1-\mathbf{p}_2\right\Vert}=\frac{w_2}{w_1+w_2}\left(1-\frac{d}{\left\Vert \mathbf{p}_1-\mathbf{p}_2\right\Vert}\right)(\mathbf{p}_1-\mathbf{p}_2). \end{aligned}$$

SDF Collision

The SDF collision constraint is

$$C(\mathbf{p}_i) = \mathrm{SDF}(\mathbf{p}_i).$$

The correction term is

$$\Delta\mathbf{p}_i = - \frac{\mathrm{SDF}(\mathbf{p}_i)}{w_i\left\Vert\nabla_{\mathbf{p}_i}\mathrm{SDF}(\mathbf{p}_i)\right\Vert^2}w_i\nabla_{\mathbf{p}_i}\mathrm{SDF}(\mathbf{p}_i)= -\frac{\mathrm{SDF}(\mathbf{p}_i)}{\left\Vert\nabla_{\mathbf{p}_i}\mathrm{SDF}(\mathbf{p}_i)\right\Vert^2}\nabla_{\mathbf{p}_i}\mathrm{SDF}(\mathbf{p}_i).$$

Example：SDF collision for sphere

Given a sphere with radius $r$, its SDF collision constraint is

$$C(\mathbf{p}_i) = \left\Vert\mathbf{p}_i\right\Vert-r.$$

The correction term is

$$\Delta\mathbf{p}_i = -\frac{\left\Vert\mathbf{p}_i\right\Vert-r}{\left\Vert\mathbf{p}_i\right\Vert^2/\left\Vert\mathbf{p}_i\right\Vert^2}\frac{\mathbf{p}_i}{\left\Vert\mathbf{p}_i\right\Vert}=\left(\frac{r}{\left\Vert\mathbf{p}_i\right\Vert}-1\right)\mathbf{p}_i.$$

The calculation of numerical integral

We can use the tetrahedron technique to optimize the calculation of numerical differentiation of SDF. Let

$$\begin{aligned} \mathbf{k}_0&=(1,-1,-1)\\ \mathbf{k}_1&=(-1,-1,1)\\ \mathbf{k}_2&=(-1,1,-1)\\ \mathbf{k}_3&=(1,1,1)\\. \end{aligned}$$

Consider the sum

$$\begin{aligned} m &=\sum_{i=0}^3 \mathbf{k}_{i}\frac{f\left(\mathbf{p}+h \mathbf{k}_{i}\right)}{\sqrt{3}h} \\ &=\sum_{i=0}^3 \mathbf{k}_{i} \frac{f\left(\mathbf{p}+h \mathbf{k}_{i}\right)-f(\mathbf{p})}{\sqrt{3}h}\\ &\approx\sum_{i=0}^3\mathbf{k}_{i} \nabla_{\widehat{\mathbf{k}}_{i}}f(\mathbf{p})\\ &=\sum_{i=0}^3 \begin{pmatrix} k_{ix}\\ k_{iy}\\ k_{iz}\\ \end{pmatrix} \left(\widehat{k}_{ix},\widehat{k}_{iy},\widehat{k}_{iz}\right) \begin{pmatrix} \nabla_{x}f(\mathbf{p})\\ \nabla_{y}f(\mathbf{p})\\ \nabla_{z}f(\mathbf{p}) \end{pmatrix}\\ &=\frac{1}{\sqrt{3}}\sum_{i=0}^3 \begin{pmatrix} k_{ix}^2&k_{ix}k_{iy}&k_{ix}k_{iz}\\ k_{ix}k_{iy}&k_{iy}^2&k_{iy}k_{iz}\\ k_{ix}k_{iz}&k_{iy}k_{iz}&k_{iz} \end{pmatrix} \begin{pmatrix} \nabla_{x}f(\mathbf{p})\\ \nabla_{y}f(\mathbf{p})\\ \nabla_{z}f(\mathbf{p})\\ \end{pmatrix}\\ &=\frac{4}{\sqrt{3}}\nabla f(\mathbf{p}) \end{aligned}.$$

From this we can get

$$\nabla f(\mathbf{p})\approx\sum_{i=0}^3 \mathbf{k}_{i}\frac{f\left(\mathbf{p}+h \mathbf{k}_{i}\right)}{4h}.$$