20 Projection Algorithm for index 1,2 and 3

Author

Affiliations

Enrico Bertolazzi

University of Trento, Department of Industrial Engineering

20.1 Index-1 DAE Projection Algorithm

Consider an index-1 Differential-Algebraic Equation (DAE) system of the form:

\left\{ \begin{aligned} \bm{x}' &= \bm{f}(\bm{x}, \bm{y}, t) \\[0.25em] \bm{0} &= \bm{g}(\bm{x}, \bm{y}, t) \end{aligned} \right. \tag{20.1}

where:

\bm{x}: differential variables
\bm{y}: algebraic variables
\bm{f}(\bm{x}, \bm{y}, t): differential dynamics
\bm{g}(\bm{x}, \bm{y}, t) = \bm{0}: algebraic constraints

Reformulate with Dummy Variables by introducing a dummy derivative \bm{y}_{\text{dot}}(t):

\left\{ \begin{aligned} \bm{x}'(t) &= \bm{f}(\bm{x}(t), \bm{y}(t), t) \\[0.25em] \bm{y}'(t) &= \bm{y}_{\text{dot}}(t) \\[0.25em] {\color{blue}\bm{0}} &= {\color{blue}\bm{h}(\bm{x}, \bm{y}, \bm{y}_{\text{dot}}, t)} \\[0.25em] {\color{red}\bm{0}} &= {\color{red}\bm{g}(\bm{x}, \bm{y}, t)} \end{aligned} \right.

Define:

\begin{aligned} \bm{h}(\bm{x}, \bm{y}, {\color{red}\bm{y}_{\text{dot}}}, t) &= \frac{d}{dt} \bm{g}(\bm{x}, \bm{y}, t) \\[0.5em] &= \frac{\partial \bm{g}}{\partial \bm{x}} \bm{x}' + \frac{\partial \bm{g}}{\partial \bm{y}} \bm{y}' + \frac{\partial \bm{g}}{\partial t} \\[0.5em] &= \bm{g}_{\bm{x}}(\bm{x}, \bm{y}, t)\, \bm{f}(\bm{x}, \bm{y}, t) + \bm{g}_{\bm{y}}(\bm{x}, \bm{y}, t)\, {\color{red}\bm{y}_{\text{dot}}} + \bm{g}_t(\bm{x}, \bm{y}, t) \end{aligned}

Using:

\begin{aligned} \bm{g}_{\bm{x}} &= \frac{\partial \bm{g}}{\partial \bm{x}}, \quad \bm{g}_{\bm{y}} = \frac{\partial \bm{g}}{\partial \bm{y}}, \quad \bm{g}_{t} = \frac{\partial \bm{g}}{\partial t} \end{aligned}

we solve for \bm{y}_{\text{dot}}:

{\color{red}\bm{y}_{\text{dot}}} = -\bm{g}_{\bm{y}}(\bm{x},\bm{y},t)^{-1} \left[ \bm{g}_{\bm{x}}(\bm{x},\bm{y},t)(\bm{x},\bm{y},t)\, \bm{f} + \bm{g}_t(\bm{x},\bm{y},t) \right]

Define:

\bm{e}(\bm{x}, \bm{y}, t) = -\bm{g}_{\bm{y}}(\bm{x}, \bm{y}, t)^{-1} \left[ \bm{g}_{\bm{x}}(\bm{x}, \bm{y}, t)\, \bm{f}(\bm{x}, \bm{y}, t) + \bm{g}_t(\bm{x}, \bm{y}, t) \right]

and obtain the ODE + constraint system:

\left\{ \begin{aligned} \bm{x}' &= \bm{f}(\bm{x}, \bm{y}, t) \\[0.25em] \bm{y}' &= \bm{e}(\bm{x}, \bm{y}, t) \\[0.25em] {\color{red}\bm{0}} &= {\color{red}\bm{g}(\bm{x}, \bm{y}, t)} \end{aligned} \right.

The projection method advances the ODE using any numerical integration scheme (Euler, RK, etc.) and then projects onto the constraints (red part).

Projection Phase

The system is advanced with a time integrator (e.g., Euler, Runge–Kutta), then projected onto the constraint manifold.

E.g. using Explicit Euler step:

\left\{ \begin{aligned} \widetilde{\bm{x}}_{k+1} &= \bm{x}_k + \Delta t\, \bm{f}(\bm{x}_k, \bm{y}_k, t_k) \\[0.5em] \widetilde{\bm{y}}_{k+1} &= \bm{y}_k + \Delta t\, \bm{e}(\bm{x}_k, \bm{y}_k, t_k) \end{aligned} \right.

The true solution will be

\begin{aligned} \bm{x}_{k+1} &= \widetilde{\bm{x}}_{k+1} + \delta\bm{x} \\ \bm{y}_{k+1} &= \widetilde{\bm{y}}_{k+1} + \delta\bm{y} \end{aligned}

Index-1 Projection as Minimization

Project the point (\widetilde{\bm{x}}_{k+1}, \widetilde{\bm{y}}_{k+1}) onto the constraint manifold:

\begin{aligned} \min_{\bm{x}, \bm{y}} \quad & \frac{1}{2} \left\| {\color{red}\bm{x}} - \widetilde{\bm{x}}_{k+1} \right\|^2 + \frac{1}{2} \left\| {\color{red}\bm{y}} - \widetilde{\bm{y}}_{k+1} \right\|^2 \\[1.5em] \text{subject to} \quad &\bm{g}({\color{red}\bm{x}}, {\color{red}\bm{y}}, t_{k+1}) = \bm{0} \end{aligned}

Using Lagrange multipliers, the candidate solutions are the stationary points of the Lagrangian:

\mathcal{L}(\bm{x},\bm{y},\bm{\lambda}) = \frac{1}{2}\|{\color{red}\bm{x}}-\widetilde{\bm{x}}_{k+1}\|^2+ \frac{1}{2}\|{\color{red}\bm{y}}-\widetilde{\bm{y}}_{k+1}\|^2- \bm{\lambda} \cdot \bm{g}({\color{red}\bm{x}},{\color{red}\bm{y}},t_{k+1})

which leads to the stationery conditions:

\left\{\begin{aligned} {\color{red}\bm{x}}-\widetilde{\bm{x}}_{k+1} - \bm{g}_{\bm{x}}({\color{red}\bm{x}},{\color{red}\bm{y}},t_{k+1})^T{\color{blue}\bm{\lambda}} &= \bm{0}\\[0.5em] {\color{red}\bm{y}}-\widetilde{\bm{y}}_{k+1} - \bm{g}_{\bm{y}}({\color{red}\bm{x}},{\color{red}\bm{y}},t_{k+1})^T{\color{blue}\bm{\lambda}} &= \bm{0}\\[0.5em] \bm{g}({\color{red}\bm{x}},{\color{red}\bm{y}},t_{k+1}) &= \bm{0} \end{aligned}\right.

Newton’s Method for Projection

Use Newton (like) iterations to solve the nonlinear system. Set

\bm{x}_{k+1} = \widetilde{\bm{x}}_{k+1}+ \delta\bm{x} \quad\text{and}\quad \bm{y}_{k+1} = \widetilde{\bm{y}}_{k+1}+ \delta\bm{y}

after linearization neglecting high order term the Newton like iterative method become:

Initialization \ell\leftarrow 0

\begin{aligned} \bm{x}^{(\ell)} & = \widetilde{\bm{x}}_{k+1} \\ \bm{y}^{(\ell)} & = \widetilde{\bm{y}}_{k+1} \end{aligned}
Solve:

\begin{aligned} \begin{pmatrix} \bm{I} & \bm{0} & -\bm{g}_{\bm{x}}^T \\[0.25em] \bm{0} & \bm{I} & -\bm{g}_{\bm{y}}^T \\[0.25em] -\bm{g}_{\bm{x}} & -\bm{g}_{\bm{y}} & \bm {0} \end{pmatrix} \begin{pmatrix} \delta\bm{x} \\[0.25em] \delta\bm{y} \\[0.25em] \bm{\lambda} \end{pmatrix} = \begin{pmatrix} \widetilde{\bm{x}}_{k+1} - \bm{x}^{(\ell)} \\[0.5em] \widetilde{\bm{y}}_{k+1} - \bm{y}^{(\ell)} \\[0.5em] \bm{g}(\bm{x}^{(\ell)}, \bm{y}^{(\ell)}, t_ {k+1}) \end{pmatrix} \end{aligned}
Update:

\begin{aligned} \bm{x}^{(\ell+1)} &= \bm{x}^{(\ell)} + \delta\bm{x}, \\ \bm{y}^{(\ell+1)} &= \bm{y}^{(\ell)} + \delta\bm{y} \end{aligned}
\ell\leftarrow\ell+1 and repeat step 2 and 3 until convergence.

Addendum

For Equation 20.1 it is not convenient to use projection, instead solve the ODE

\bm{x}' = \bm{F}(\bm{x},t)

where

\bm{F}(\bm{x},t) = \bm{f}(\bm{x},{\color{red}\bm{y}(x,t)},t)

and \color{red}\bm{y}(x,t) is the solution of

\bm{g}(\bm{x},{\color{red}\bm{y}},t) = 0\qquad\qquad (\star)

how to solve (\star)?

For example using explicit Euler

\bm{x}_{k+1} = \bm{x}_{k} + \Delta t \bm{f}(\bm{x}_k,\bm{y}_k,t_k)

\bm{y}_{k+1} is the solution of \bm{c}(\bm{z}) =\bm{0} where \bm{c}(\bm{z}) = \bm{g}(\bm{x}_{k+1},\bm{z},t_{k+1}) that can be solved using Newton-Raphson method

\bm{z}^{(0)} = \bm{y}_{k},\qquad \bm{z}^{(\ell+1)} = \bm{z}^{(\ell)} - \left(\dfrac{\partial\bm{c}(\bm{z}^{(\ell)})}{\partial\bm{z}}\right)^{-1} \bm{c}(\bm{z}^{(\ell)})

or better, start with {\color{blue}\bm{y}_{k+1}^{(0)}} = \bm{y}_{k} and then

{\color{blue}\bm{y}_{k+1}^{(\ell+1)}} = {\color{blue}\bm{y}_{k+1}^{(\ell)}} - \bm{g}_{\bm{y}}(\bm{x}_{k+1},{\color{blue}\bm{y}_{k+1}^{(\ell)}},t_{k+1})^{-1} \bm{g}(\bm{x}_{k+1},{\color{blue}\bm{y}_{k+1}^{(\ell)}},t_{k+1})

set \bm{y}_{k+1}={\color{blue}\bm{y}_{k+1}^{(L)}} with L accepted value.

20.2 Index 2 DAE projection algorithm

An index-2 differential-algebraic equation (DAE) can be written as:

\left\{\begin{aligned} \bm{x}'& = \bm{f}(\bm{x},\bm{y},t) \\[0.5em] \bm{0} & = \bm{g}(\bm{x},t) \end{aligned}\right.

where:

\bm{x} and \bm{y} are the differential and algebraic variables;
\bm{f}(\bm{x}, \bm{y}, t) is the right-hand side of the differential equations;
\bm{g}(\bm{x}, t) encodes the algebraic constraints.

Constraint Differentiation

To regularize the system, differentiate the constraint and introduce a dummy derivative for \bm{y}:

\left\{\begin{aligned} \bm{x}'(t)& = \bm{f}(\bm{x}(t),\bm{y}(t),t) \\[0.5em] \bm{y}'(t) & = \bm{y}_{\text{dot}}(t) \\[0.5em] \color{DarkGreen}\bm{0} & = \color{DarkGreen}\bm{a}(\bm{x},\bm{y},t) + \bm{g}_{\bm{x}}(\bm{x},t)\bm{f}_{\bm{y}}(\bm{x},\bm{y},t)\bm{y}_{\text{dot}} \\[0.5em] \color{blue}\bm{0} & = \color{blue}\bm{h}(\bm{x},\bm{y},t) \\[0.5em] \color{red}\bm{0} & = \color{red}\bm{g}(\bm{x},t) \end{aligned}\right.

where

First hidden constraint \begin{aligned} \bm{0}&= \dfrac{\mathrm{d}}{\mathrm{d}t}\bm{g}(\bm{x},t) \\[0.5em] & = \bm{g}_{\bm{x}}(\bm{x},t)\bm{f}(\bm{x},\bm{y},t)+ \bm{g}_t(\bm{x},t) \\[0.5em] &= \bm{h}(\bm{x},\bm{y},t) \end{aligned}
Second hidden constraint \begin{aligned} \bm{0} &=\dfrac{\mathrm{d}}{\mathrm{d}t}\bm{h}(\bm{x},\bm{y},t)\\[0.5em] & = \underbrace{\bm{h}_{\bm{x}}(\bm{x},\bm{y},t)\bm{f}(\bm{x},\bm{y},t)+ \bm{h}_t(\bm{x},\bm{y},t)}_{ \bm{a}(\bm{x},\bm{y},t) }+ \underbrace{\bm{h}_{\bm{y}}(\bm{x},\bm{y},t)}_{ \bm{g}_{\bm{x}}(\bm{x},t)\bm{f}_{\bm{y}}(\bm{x},\bm{y},t) } \bm{y}_{\text{dot}} \end{aligned}

Solving for \bm{y}_{\text{dot}} gives the regularized system:

\left\{\begin{aligned} \bm{x}'(t) & = \bm{f}(\bm{x},\bm{y},t) \\[0.5em] \bm{y}'(t) & = -\Big[\bm{g}_{\bm{x}}(\bm{x},t)\bm{f}_{\bm{y}}(\bm{x},\bm{y},t)\Big]^{-1} \bm{a}_{\bm{x}}(\bm{x},\bm{y},t) \\[0.5em] \color{blue}\bm{0} & = \color{blue}\bm{h}(\bm{x},\bm{y},t) \\[0.5em] \color{red}\bm{0} & = \color{red}\bm{g}(\bm{x},t) \end{aligned}\right.

an ODE with hidden constraints.

Numerical Solution

The system is solved using a numerical method (e.g., Euler or Runge-Kutta) followed by a projection step to enforce the constraints.

Explicit Euler Step Example

Advance solution

\left\{\begin{aligned} \bm{x}_{k+1} & = \bm{x}_{k}+\Delta t\,\bm{f}(\bm{x}_k,\bm{y}_k,t_k) \\[0.5em] \bm{y}_{k+1} & = \bm{y}_{k}-\Delta t\, \Big[\bm{g}_{\bm{x}}(\bm{x}_k,t_k)\bm{f}_{\bm{y}}(\bm{x}_k,\bm{y}_k,t_k)\Big]^{-1} \bm{a}_{\bm{x}}(\bm{x}_k,\bm{y}_k,t_k) \end{aligned}\right.
Project onto Constraints:
Solve the constrained minimization problem:

\begin{aligned} \textrm{minimize}\qquad & \frac{1}{2} \left\| {\color{red}\bm{x}}-\widetilde{\bm{x}}_{k+1} \right\|^2 + \frac{1}{2} \left\| {\color{red}\bm{y}}-\widetilde{\bm{y}}_{k+1} \right\|^2 \\[2em] \textrm{subject to}\qquad & \left\{\begin{array}{l} \bm{g}({\color{red}\bm{x}},t_{k+1}) = \bm{0}\\[0.5em] \bm{h}({\color{red}\bm{x}},{\color{red}\bm{y}},t_{k+1}) = \bm{0} \end{array}\right. \end{aligned}

This is solved using Lagrange multipliers and linearization.

Projection algorithm

Using Lagrange multiplier:

\begin{aligned} \mathcal{L}({\color{red}\bm{x}},{\color{red}\bm{y}},{\color{blue}\bm{\lambda}},{\color{blue}\bm{\mu}}) & = \frac{1}{2}\|{\color{red}\bm{x}}-\widetilde{\bm{x}}_{k+1}\|^2+ \frac{1}{2}\|{\color{red}\bm{y}}-\widetilde{\bm{y}}_{k+1}\|^2 \\[0.5em] & -{\color{blue}\bm{\lambda}}\cdot\bm{g}({\color{red}\bm{x}},t_{k+1}) -{\color{blue}\bm{\mu}}\cdot\bm{h}({\color{red}\bm{x}},{\color{red}\bm{y}},t_{k+1}) \end{aligned}

or the solution of the non linear system:

\dfrac{\partial\mathcal{L}}{\partial\bm{x}} = \bm{0},\quad \dfrac{\partial\mathcal{L}}{\partial\bm{y}} = \bm{0},\quad \dfrac{\partial\mathcal{L}}{\partial\bm{\lambda}} =\bm{0},\quad \dfrac{\partial\mathcal{L}}{\partial\bm{\mu}} = \bm{0}.

that written explicitly is \left\{\begin{aligned} {\color{red}\bm{x}}-\widetilde{\bm{x}}_{k+1} - \bm{g}_{\bm{x}}({\color{red}\bm{x}},t_{k+1})^T{\color{blue}\bm{\lambda}} - \bm{h}_{\bm{x}}({\color{red}\bm{x}},{\color{red}\bm{y}},t_{k+1})^T{\color{blue}\bm{\mu}} &= \bm{0} \\[0.5em] {\color{red}\bm{y}}-\widetilde{\bm{y}}_{k+1} - \bm{h}_{\bm{y}}({\color{red}\bm{x}},{\color{red}\bm{y}},t_{k+1})^T{\color{blue}\bm{\mu}} &= \bm{0} \\[0.5em] \bm{g}({\color{red}\bm{x}},t_{k+1}) &= \bm{0} \\[0.5em] \bm{h}({\color{red}\bm{x}},{\color{red}\bm{y}},t_{k+1}) &= \bm{0} \end{aligned}\right.

after linearization do until convergence (\ell = 0,1,\ldots) the following iterations

Initialization \ell\leftarrow 0 \begin{aligned} \bm{x}^{(\ell)} & = \widetilde{\bm{x}}_{k+1},\\ \bm{y}^{(\ell)} & = \widetilde{\bm{y}}_{k+1} \end{aligned}
Solve \begin{pmatrix} \bm{I} & \bm{0} & -\bm{g}_{\bm{x}}^T & -\bm{h}_{\bm{x}}^T \\[0.5em] \bm{0} & \bm{I} & \bm{0} & -\bm{h}_{\bm{y}}^T \\[0.5em] -\bm{g}_{\bm{x}} & \bm{0} & \bm{0} & \bm{0}\\[0.5em] -\bm{h}_{\bm{x}} & -\bm{h}_{\bm{y}} & \bm{0} & \bm{0} \end{pmatrix} %%% \begin{pmatrix} \delta\bm{x}\\[0.5em] \delta\bm{y}\\[0.5em] \color{blue}\bm{\lambda} \\[0.5em] \color{blue}\bm{\mu} \end{pmatrix} = \begin{pmatrix} \widetilde{\bm{x}}_{k+1}-\bm{x}^{(\ell)}\\[0.5em] \widetilde{\bm{y}}_{k+1}-\bm{y}^{(\ell)}\\[0.5em] \bm{g}(\bm{x}^{(\ell)},t_{k+1})\\[0.5em] \bm{h}(\bm{x}^{(\ell)},\bm{y}^{(\ell)},t_{k+1}) \end{pmatrix}
Advance:

\begin{aligned} \bm{x}^{(\ell+1)} & = \bm{x}^{(\ell)} + \delta\bm{x}, \\ \bm{y}^{(\ell+1)} & = \bm{y}^{(\ell)} + \delta\bm{y}, \end{aligned}
\ell\leftarrow\ell+1 and repeat step 2 and 3 until convergence.

Approximate Projection algorithm

To speed up computation we can project separately position and velocity solving two consecutive project:

x projection:

\begin{aligned} \textrm{minimize}\qquad & \frac{1}{2} \left\| {\color{red}\bm{x}}-\widetilde{\bm{x}}_{k+1} \right\|^2 \\[1em] \textrm{subject to}\qquad & \bm{g}({\color{red}\bm{x}},t_{k+1}) = \bm{0} \end{aligned}
y projection:

\begin{aligned} \textrm{minimize}\qquad & \frac{1}{2} \left\| {\color{red}\bm{y}}-\widetilde{\bm{y}}_{k+1} \right\|^2 \\[1em] \textrm{subject to}\qquad & \bm{h}(\bm{x}_{k+1},{\color{red}\bm{y}},t_{k+1}) = \bm{0} \end{aligned}

Using Lagrange multiplier we obtain the two nonlinear system to be solved

x problem: \left\{\begin{aligned} {\color{red}\bm{x}}-\widetilde{\bm{x}}_{k+1} - \bm{g}_{\bm{x}}({\color{red}\bm{x}},t_{k+1})^T{\color{blue}\bm{\lambda}} &= \bm{0} \\[0.5em] \bm{g}({\color{red}\bm{x}},t_{k+1}) &= \bm{0} \end{aligned}\right.
1. Initialization \ell\leftarrow 0
  
  \bm{x}^{(\ell)} = \widetilde{\bm{x}}_{k+1}
2. Solve \begin{pmatrix} \bm{I} & -\bm{g}_{\bm{x}}^T \\[0.5em] -\bm{g}_{\bm{x}} & \bm{0} \end{pmatrix} %%% \begin{pmatrix} \delta\bm{x}\\[0.5em] \color{blue}\bm{\lambda} \end{pmatrix} = \begin{pmatrix} \widetilde{\bm{x}}_{k+1}-\bm{x}^{(\ell)}\\[0.5em] \bm{g}(\bm{x}^{(\ell)},t_{k+1}) \end{pmatrix}
3. Advance:
\bm{x}^{(\ell+1)} = \bm{x}^{(\ell)} + \delta\bm{x}
1. \ell\leftarrow\ell+1 and repeat step 2 and 3 until convergence.
y problem: \left\{\begin{aligned} {\color{red}\bm{y}}-\widetilde{\bm{y}}_{k+1} - \bm{h}_{\bm{y}}({\color{red}\bm{x}},{\color{red}\bm{y}},t_{k+1})^T{\color{blue}\bm{\mu}} &= \bm{0} \\[0.5em] \bm{h}({\color{red}\bm{x}},{\color{red}\bm{y}},t_{k+1}) &= \bm{0} \end{aligned}\right.
1. Initialization \ell\leftarrow 0 \bm{y}^{(\ell)} = \widetilde{\bm{y}}_{k+1}
2. Solve \begin{pmatrix} \bm{I} & -\bm{h}_{\bm{y}}^T \\[0.5em] -\bm{h}_{\bm{y}} & \bm{0} \end{pmatrix} %%% \begin{pmatrix} \delta\bm{y}\\[0.5em] \color{blue}\bm{\mu} \end{pmatrix} = \begin{pmatrix} \widetilde{\bm{y}}_{k+1}-\bm{y}^{(\ell)}\\[0.5em] \bm{h}(\bm{x}^{(\ell)},\bm{y}^{(\ell)},t_{k+1}) \end{pmatrix}
3. Advance:
  
  \bm{y}^{(\ell+1)} = \bm{y}^{(\ell)} + \delta\bm{y},
4. \ell\leftarrow\ell+1 and repeat step 2 and 3 until convergence.

20.3 Index 3 DAE projection algorithm

An Index-3 DAE typically arises in mechanical systems with constraints, such as: \left\{\begin{aligned} \bm{p}'& = \bm{v} \\[0.25em] \bm{M}(\bm{p},\bm{v},t)\bm{v}'+\bm{\Phi}_{\bm{p}}(\bm{p},t)^T\bm{\lambda} & = \bm{f}(\bm{p},\bm{v},t) \\[0.25em] \bm{\Phi}(\bm{p},t) & = \bm{0} \end{aligned}\right.

where:

\bm{p} and \bm{v} are the position and velocity variables,
\bm{M} is the mass matrix,
\bm{\Phi} represents the constraints,
\bm{\lambda} are the Lagrange multipliers.

To handle the higher-index constraints, we introduce dummy variables and differentiate the constraints

\left\{\begin{aligned} \bm{p}'(t) &= \bm{v}(t), \\[0.25em] \bm{v}'(t) &= \bm{v}_{\text{dot}} \\[1em] & \text{Index one part (solve for $\bm{v}_{\text{dot}}$ and $\bm{\lambda}$)} \\ \bm{0} &= \bm{M}(\bm{p},\bm{v},t)\bm{v}_{\text{dot}}+\bm{\Phi}_{\bm{p}}(\bm{p},t)^T\bm{\lambda} - \bm{f}(\bm{p},\bm{v},t) \\[0.25em] \color{DarkGreen}\bm{0}&= \color{DarkGreen}\bm{\Phi}_{\bm{p}}(\bm{p},t)\bm{v}_{\text{dot}}+ \bm{a}(\bm{p},\bm{v},t) \\[1em] & \text{Hidden constraints (projection for $\bm{p}$ and $\bm{v}$)} \\ \color{blue}\bm{0} &= \color{blue}\bm{h}(\bm{p},\bm{v},t), \\ \color{red}\bm{0} &= \color{red}\bm{\Phi}(\bm{p},t) \end{aligned}\right.

where

\begin{aligned} \bm{h}(\bm{p},\bm{v},t) & = \frac{\mathrm{d}}{\mathrm{d}t}\bm{\Phi}(\bm{p},t) \\ & = \bm{\Phi}_{\bm{p}}(\bm{x},t)\bm{v}+\bm{\Phi}_t(\bm{p},t) \\[1em] \bm{a}(\bm{p},\bm{v},t) & = \bm{h}_{\bm{p}}(\bm{p},\bm{v},t)\bm{v}+ \bm{h}_t(\bm{p},\bm{v},t) \end{aligned}

By solving for \bm{v}_{\text{dot}} and \bm{\lambda} we obtain the reduced ODE system:

\left\{\begin{aligned} \bm{p}' & = \bm{v} \\[0.5em] \bm{v}' & = \bm{v}_{\text{dot}}(\bm{p},\bm{v},t) \end{aligned}\right.

where \bm{v}_{\text{dot}} is obtained from \bm{p} and \bm{v} by solving the linear system:

\begin{pmatrix} \bm{M}(\bm{p},\bm{v},t) & \bm{\Phi}_{\bm{p}}(\bm{p},t)^T \\[1em] \bm{\Phi}_{\bm{p}}(\bm{p},t) & \bm{0} \end{pmatrix} \begin{pmatrix} \bm{v}_{\text{dot}} \\[1em] \bm{\lambda} \end{pmatrix} = \begin{pmatrix} \bm{f}(\bm{p},\bm{v},t) \\[1em] -\bm{a}(\bm{p},\bm{v},t) \end{pmatrix}

The invariants

\left\{\begin{aligned} \color{blue}\bm{0} & = \color{blue}\bm{h}(\bm{p},\bm{v},t) \\[0.5em] \color{red}\bm{0} & = \color{red}\bm{\Phi}(\bm{p},t) \end{aligned}\right.

are used in the projection step.

Index-3 Advancing phase

Projection method advance the ODE using any numerical method (Euler, RK, …) and next project onto the constraints (red and blue part).

E.g. using Explicit Euler step:

\left\{\begin{aligned} \bm{p}_{k+1} & = \bm{p}_{k} + \Delta t \,\bm{v}_k \\[0.5em] \bm{v}_{k+1} & = \bm{v}_{k} + \Delta t \,(\bm{v}_{\text{dot}})_k \end{aligned}\right.

where \bm{v}_{\text{dot}} is obtained from \bm{p}_k and \bm{v}_k by solving the linear system:

\begin{pmatrix} \bm{M}(\bm{p}_k,\bm{v}_k,t_k) & \bm{\Phi}_{\bm{p}}(\bm{p}_k,t_k)^T \\[1em] \bm{\Phi}_{\bm{p}}(\bm{p}_k,t_k) & \bm{0} \end{pmatrix} \begin{pmatrix} \bm{v}_{\text{dot}} \\[1em] \bm{\lambda} \end{pmatrix} = \begin{pmatrix} \bm{f}(\bm{p}_k,\bm{v}_k,t_k) \\[1em] -\bm{a}(\bm{p}_k,\bm{v}_k,t_k) \end{pmatrix}

Index-3 Projection phase

The projection phase solve the problem \begin{aligned} \textrm{minimize}\qquad & \frac{1}{2} \left\| {\color{red}\bm{p}}-\widetilde{\bm{p}}_{k+1} \right\|^2 + \frac{1}{2} \left\| {\color{red}\bm{v}}-\widetilde{\bm{v}}_{k+1} \right\|^2 \\[1em] \textrm{subject to}\qquad & \left\{ \begin{aligned} \bm{\Phi}({\color{red}\bm{p}},t_{k+1}) = \bm{0}\\[0.25em] \bm{h}({\color{red}\bm{p}},{\color{red}\bm{v}},t_{k+1}) = \bm{0} \end{aligned} \right. \end{aligned}

using Lagrange multiplier:

\begin{aligned} \mathcal{L}({\color{red}\bm{p}},{\color{red}\bm{v}},{\color{blue}\bm{\lambda}},{\color{blue}\bm{\mu}}) &= \frac{1}{2}\|{\color{red}\bm{p}}-\widetilde{\bm{p}}_{k+1}\|^2+ \frac{1}{2}\|{\color{red}\bm{v}}-\widetilde{\bm{v}}_{k+1}\|^2 \\[1em] & -{\color{blue}\bm{\lambda}}\cdot\bm{\Phi}({\color{red}\bm{x}},t_{k+1}) -{\color{blue}\bm{\mu}}\cdot\bm{h}({\color{red}\bm{x}},{\color{red}\bm{y}},t_{k+1}) \end{aligned}

or the solution of the non linear system:

\dfrac{\partial\mathcal{L}}{\partial\bm{p}} = \dfrac{\partial\mathcal{L}}{\partial\bm{v}} = \dfrac{\partial\mathcal{L}}{\partial\bm{\lambda}} = \dfrac{\partial\mathcal{L}}{\partial\bm{\mu}} = \bm{0}.

Use Newton (like) iterations to solve the nonlinear system. Set

\bm{p}_{k+1} = \widetilde{\bm{p}}_{k+1}+ \delta\bm{p} \quad\text{and}\quad \bm{v}_{k+1} = \widetilde{\bm{v}}_{k+1}+ \delta\bm{v}

after linearization neglecting high order term the Newton like iterative method become:

Initialization \ell\leftarrow 1

\bm{p}^{(\ell)} = \widetilde{\bm{p}}_{k+1} \quad \text{and} \quad \bm{v}^{(\ell)} = \widetilde{\bm{v}}_{k+1}
Solve:

\begin{pmatrix} \bm{I} & \bm{0} & -\bm{\Phi}_{\bm{p}}^T & -\bm{h}_{\bm{p}}^T \\[0.25em] \bm{0} & \bm{I} & \bm{0} & -\bm{h}_{\bm{v}}^T \\[0.35em] -\bm{\Phi}_{\bm{p}} & \bm{0} & \bm{0} & \bm{0}\\[0.25em] -\bm{h}_{\bm{p}} & -\bm{h}_{\bm{v}} & \bm{0} & \bm{0} \end{pmatrix} %%% \begin{pmatrix} \delta\bm{p}\\[0.25em] \delta\bm{v}\\[0.25em] \color{blue}\bm{\lambda} \\[0.25em] \color{blue}\bm{\mu} \end{pmatrix} = \begin{pmatrix} \widetilde{\bm{p}}_{k+1}-\bm{p}^{(\ell)}\\[0.25em] \widetilde{\bm{v}}_{k+1}-\bm{v}^{(\ell)}\\[0.25em] \bm{\Phi}(\bm{p}^{(\ell)},t_{k+1})\\[0.25em] \bm{h}(\bm{p}^{(\ell)},\bm{v}^{(\ell)},t_{k+1}) \end{pmatrix}
Update:

\begin{aligned} \bm{p}^{(\ell+1)} &= \bm{p}^{(\ell)} + \delta\bm{p}, \\ \bm{v}^{(\ell+1)} &= \bm{v}^{(\ell)} + \delta\bm{v} \end{aligned}
Repeat step 2 and 3 until convergence.

Index 3 DAE approximate projection algorithm

Projection phase: position

\begin{aligned} \textrm{minimize}\qquad & \frac{1}{2} \left|\left|{\color{red}\bm{p}}-\widetilde{\bm{p}}_{k+1}\right|\right|^2 \\ \textrm{subject to}\qquad & \bm{\Phi}({\color{red}\bm{p}},t_{k+1}) = \bm{0} \end{aligned}

using Lagrange multiplier:

\mathcal{L}({\color{red}\bm{p}},{\color{blue}\bm{\lambda}}) = \frac{1}{2}||{\color{red}\bm{p}}-\widetilde{\bm{p}}_{k+1}||^2 -{\color{blue}\bm{\lambda}}\cdot\bm{\Phi}({\color{red}\bm{p}},t_{k+1})

or find the solution of the non linear system:

\dfrac{\partial\mathcal{L}}{\partial\bm{p}} = \bm{0},\quad \dfrac{\partial\mathcal{L}}{\partial\bm{\lambda}} = \bm{0}.

Projection phase: velocity

\begin{aligned} \textrm{minimize}\qquad & \frac{1}{2} \left\|{\color{red}\bm{v}}-\widetilde{\bm{v}}_{k+1}\right\|^2 \\ \textrm{subject to}\qquad & \bm{h}(\bm{p}_{k+1},{\color{red}\bm{v}},t_{k+1}) = \bm{0} \end{aligned}

using Lagrange multiplier:

\mathcal{L}({\color{red}\bm{v}},{\color{blue}\bm{\mu}}) = \frac{1}{2}|{\color{red}\bm{v}}-\widetilde{\bm{v}}_{k+1}||^2 -{\color{blue}\bm{\mu}}\cdot\bm{h}(\bm{p}_{k+1},{\color{red}\bm{v}},t_{k+1})

or the solution of the non linear system:

\dfrac{\partial\mathcal{L}}{\partial\bm{v}} = \bm{0},\qquad \dfrac{\partial\mathcal{L}}{\partial\bm{\mu}} = \bm{0}.

\bm{p}^{(0)} = \widetilde{\bm{p}}_{k+1},\qquad \bm{p}^{(\ell+1)} = \bm{p}^{(\ell)} + \delta\bm{p},

\bm{v}^{(0)} = \widetilde{\bm{v}}_{k+1},\qquad \bm{v}^{(\ell+1)} = \bm{v}^{(\ell)} + \delta\bm{v},

\begin{pmatrix} \bm{I} & -\bm{\Phi}_{\bm{p}}^T \\[1em] -\bm{\Phi}_{\bm{p}} & \bm{0} \end{pmatrix} %%% \begin{pmatrix} \delta\bm{p}\\[1em] \color{blue}\bm{\lambda} \end{pmatrix} = \begin{pmatrix} \widetilde{\bm{p}}_{k+1}-\bm{p}^{(\ell)}\\[1em] \bm{\Phi}(\bm{p}^{(\ell)},t_{k+1}) \end{pmatrix}

\begin{pmatrix} \bm{I} & -\bm{h}_{\bm{v}}^T \\[1em] -\bm{h}_{\bm{v}} & \bm{0} \end{pmatrix} %%% \begin{pmatrix} \delta\bm{v}\\[1em] \color{blue}\bm{\mu} \end{pmatrix} = \begin{pmatrix} \widetilde{\bm{v}}_{k+1}-\bm{v}^{(\ell)}\\[1em] \bm{h}(\bm{p}_{k+1},\bm{v}^{(\ell)},t_{k+1}) \end{pmatrix}

where \bm{h}_{\bm{v}}=\bm{h}_{\bm{v}}(\bm{p}_{k+1},\bm{v}^{(\ell)},t_{k+1}) and \bm{\Phi}_{\bm{p}}=\bm{\Phi}(\bm{p}^{(\ell)},t_{k+1}).