# Nimble source code reading: DantzigBoxedLcpSolver

The version of the Nimblephysics is still 0.7.7

This part is about using Dantzig method to solve the LCP problem stated as follow:

find $f,v_{t+1}$

such that $f \geq 0, v_{v+1} \geq 0, f^Tv_{t+1} = 0$

$v_{t+1} = Af + b$

Main Code address: dart/external/odelcpsolver/lcp.cpp

Start from the line 780

# The parameter

1 | bool dSolveLCP (int n, dReal *A, dReal *x, dReal *b, |

$n$ is the size of $A$, which means $A$ is a $n \times n$ matrix.

$A$ is the above $A$, it should be symmetry and positive definite.

$x$ is the solved $f$

$b$ is the above $b$

$nub$ is the number of unbounded variables

$lo$ and $hi$ is the lower bounds and higher bounds of the $x$

$findex$ is the friction index

$earlyTermination$ is whether not to use PGS algorithm

# The body

## Unbounded Variables

Only when $nub \geq n$ can this process be activated.

1 | if (nub >= n) { |

The process is mainly about:

$$ \begin{align} A &= LDL^T\\\ x &= A^{-1}b = L^{-T}D^{-1}L^{-1}b \end{align} $$The $b$ in the above parameter might be $-b$.

## Lo-Hi LCP

Start the main part.

### PrePare Containers

Unfortunately, usually the $nub$ is $0$. So we have to

Things we need:

1 | dLCP lcp(n,nskip,nub,A,x,b,w,lo,hi,L,d,Dell,ell,delta_w,state,findex,p,C,Arows); |

These new parameters are all constructed by `new`

.

Let’s dive into its constructor and see what happened:

1 | dLCP::dLCP (int _n, int _nskip, int _nub, dReal *_Adata, dReal *_x, dReal *_b, dReal *_w, dReal *_lo, dReal *_hi, dReal *_L, dReal *_d, dReal *_Dell, dReal *_ell, dReal *_tmp, bool *_state, int *_findex, int *_p, int *_C, dReal **Arows) |

The `dLCP`

is actually a struct, and the family variables are:

1 | const int m_n; |

Basically, it just puts these parameters in corresponding family variables. Besides, all unbounded rows and columns shall be swapped to the first $nub$ rows and columns. And `p`

will record the new permutation.

Understand this, we could know what is the `adj_nub`

:

1 | int adj_nub = lcp.getNub();// sdj_nub = m_nub |

From the above code, `m_code`

is `nub`

, which is the number of unbounded variables. Actually, if `nub`

greater than $0$, depending on the constructor:

1 | const int nub = m_nub; |

Keep in mind that the current $x$ is all normal forces!!!

### Start Solving

Now here is where the dream started:

1 | for (int i=adj_nub; i<n; ++i) |

This loop will finish the computation of these bounded questions.

The author’s words:

Loop over all indexes adj_nub..n-1. For index $i$, if $x(i),w(i)$ satisfy the LCP conditions then $i$ is added to the appropriate index set. Otherwise, $x(i),w(i)$ is driven either $+ve$ or $-ve$ to force it to the valid region. As we drive $x(i), x(C)$ is also adjusted to keep $w(C)$ at zero. While driving $x(i)$ we maintain the LCP conditions on the other variables $0..i-1$. We do this by watching out for other $x(i),w(i)$ values going outside the valid region, and then switching them between index sets when that happens.

The index i is the driving index and indexes i+1..n-1 are “don’t care”, i.e. When we make changes to the system those x’s will be zero and we don’t care what happens to those w’s. In other words, we only need to consider an $(i+1)

(i+1)$ sub-problem of $Ax=b+w$.

Remember, the algorithm goes from $A[0][0], A[i][i]$, to $A[n][n]$.

Why $A*x=b+w$???

1 | bool hit_first_friction_index = false;//this line is in the outside of the loop!!! |

This bool variable is used to control to initialize the $lo$ and $hi$ constraints:

1 | for (int j=0; j<n; ++j) delta_w[p[j]] = x[j]; //unpermuted the answer x to make the findex valid |

The author’s words:

If we’ve hit the first friction index, we have to compute the $lo$ and $hi$ values based on the values of $x$ already computed. we have been permuting the indexes, so the values stored in the $findex$ vector are no longer valid. thus we have to temporarily unpermute the $x$ vector. for the purposes of this computation, $0*\infty = 0$ … so if the contact constraint’s normal force is $0$, there should be no tangential force applied.

Because of the existence of `hit_first_friction_index`

, the above process could only be executed once.

Now we initialize the $Lo$ and $Hi$, giving bounds to the tangential forces.

1 | bool s_error = false; |

This bool variable is aimed at killing the loop, when the $s$ is negtive, which is illegal.

1 | w[i] = lcp.AiC_times_qC (i,x) + lcp.AiN_times_qN (i,x) - b[i]; |

The author’s words:

Thus far we have not even been computing the $w$ values for indexes greater than $i$, so compute $w[i]$ now.

This line of code is:

$$ \begin{align} w_i &= A_{i(0C)}*x_{0C} + A_{i(C\ C+N)}*x_{C\ (C+N)} - b[i] \\ &= A_{i(0(C+N))}*x_{0(C+N)} - b[i] \end{align} $$So :

$$ w = A_{n\ C+N}*x - b $$1 | // for i in N, state[i] is 0 if x(i)==lo(i) or 1 if x(i)==hi(i) |

Definition of the $state$

1 | if (lo[i]==0 && w[i] >= 0) { |

Weird… Why $w[i] \geq 0\ and\ Lo[i]$ means that $x[i] = 0$? Same question to the $Hi[n]$.

$x[i] = 0$, so $i$ in $N$.

The author’s words:

If $Lo=Hi=0$ (which can happen for tangential friction when normals are $0$. Then the index will be assigned to set $N$ with some state. However, set $C$’s line has zero sizes, so the index will always remain in set $N$. With the “normal” switching logic, if $w$ changed sign then the index would have to switch to set $C$ and then back to set $N$ with an inverted state. This is pointless, and also computationally expensive. To prevent this from happening, we use the rule that indexes with $Lo=Hi=0$ will never be checked for set changes. This means that the state for these indexes may be incorrect, but that doesn’t matter.

1 | else if (w[i]==0) { |

In this state, $Lo[i] < 0 = w[i] = 0 < Hi[i]$. $w[i] = (Af(or\ x)-b)[i] = 0$, $i$ in $C$.

Now we only have two instances:

$$ \begin{align} w[i] > 0\ &and\ Lo[i] < 0 \\ w[i] < 0\ &and\ Hi[i] > 0 \end{align} $$Both situations $w*x \neq 0$, so we need to do our job.

### Eternal Loop

The code used a permanent loop to adjust the answer until the sets $C$ and $N$ are not changing.

1 | int dir; |

The author’s words:

Find direction to push on $x(i)$.

As you can see, when $w[i] < 0$, $x[i]$ must increase to let $w[i] = 0$, so the direction should be $1$. The opposite is the same reason.

1 | lcp.solve1 (delta_x,i,dir); |

The author’s word:

compute: delta_x(C) = -dir*A(C,C)\A(C,i).

I believe it should correspond to the pseudo-code *dfirection* in the paper Fast Contact Force Computation for Nonpenetrating Rigid Bodies, and:

1 | lcp.pN_equals_ANC_times_qC (delta_w,delta_x);//delta_w[N] = A[N]*delta_x |

The author’s words:

Compute: delta_w = A*delta_x … note we only care about delta_w(N) and delta_w(i), the rest is ignored

Still, this part corresponds to $\Delta a = A \Delta f$.

Now we step into the process to culculate the largest step $s$.

1 | int cmd = 1; // index switching command |

Used to deal with different situations.

1 | dReal s = -w[i]/delta_w[i]; |

$s$ is used to record the maximum step.

1 | if (dir > 0) { |

Really simple code…

1 | { |

Need to judge whether the step is broken:

1 | if (s <= REAL(0.0)) { |

Plus these step and direction to $x$ and $w$.

1 | // apply x = x + s * delta_x |

The equations are:

$$ \begin{align} x[C]\ &+=\ s*delta\_x[C] \\ x[i]\ &+=\ s*dir \\ w[N]\ &+=\ s*delta\_w[N] \\ w[i]\ &+=\ s*dir \end{align} $$Since we get the maximum step length $s$, it is time to move these indexes.

1 | case 1: // done |

1 | case 2: // done |

1 | case 3: // done |

1 | case 4: // keep going |

1 | case 5: // keep going |

1 | case 6: // keep going |

If the $cmd \leq3$, this means the set $N$ and $C$ are all stable, we could be saved from the while(true) loop.

### Finish Inner Loop

Note that if $s_error = true$, we need break from the main loop right now:

1 | if (s_error) { |

## TaDa! Finish it!

When everything is done, we need to upermute the $x$ and $w$:

1 | lcp.unpermute(); |

and delete everything, return true:

1 | if (!outer_w) |

# Conclusion

It takes me two weeks to realize what is going on in this function. Really nice experience about reading a giant program. It is my first time reading these things ^_^.