LCPとdv
See also core/doc/LCP_springdamper.pdf
非対角部分が要らなくなる理由
From eq11, $ w[t+1] = A \lambda + b, b=J(v[t]+M^{-1} f h)= w[t] + JM^{-1} f h .
For $ b' \equiv b-w[t] = JM^{-1}fh and $ dv^{\infty} = w[t+1] - w[t]
$ w[t+1] = A \lambda + w[t] + b'
$ dw^{k} = A\lambda^{k} + b' = (U+L+D)\lambda^{k} + b'
($ dw corresponds to joint->dv in code )
$ dw_i^{k} = \sum_{j=0}^{i-1}L_{ij}\lambda^{k+1}_j + \sum_{j=i+1}^{n-1}U_{ij}\lambda^{k}_j + D_{ii}\lambda^{k+1}_i + b'_i
$ \sum_{j=0}^{i-1}L_{ij}\lambda^{k+1}_j + \sum_{j=i+1}^{n-1}U_{ij}\lambda^{k}_j = dw_i^k - D_{ii}\lambda^{k}_i - b'_i
From eq27, $ \lambda^{k+1}_i = -(\sum_{j=0}^{i-1}L_{ij}\lambda^{k+1}_j + \sum_{j=i+1}^{n-1}U_{ij}\lambda^{k}_j + b_i)/D_{ii} for $ b \equiv w[t]+ JM^{-1} fh
$ \lambda^{k+1}_i = -(dw_i^k - D_{ii}\lambda^{k} - b'_i + b_i)/D_{ii}=\lambda^{k} - (dw_i^{k} - b'_i + b_i)/D_{ii}= \lambda^{k} - (dw_i^{k} + w_i[t])/D_{ii}
$ = \lambda^k - w_i^{k}/D_{ii} (now $ L, U comes to not needed.)
$ dw_i[t]= [Jdv]_i ,~~~~~ dv_i=[ M^{-1} J^t \lambda] _i
Spring and damper
$ w_s[t+1] =-(D+Kh)^{-1}\lambda_s - (D+Kh)^{-1} Kq_s[t]
$ w_{target}[t+1] - w[t+1] =-(D+Kh)^{-1}\lambda_s - (D+Kh)^{-1} Kq_s[t]
$ w[t+1] = (D+Kh)^{-1}\lambda_s + (D+Kh)^{-1} Kq_s[t] + w_{target}[t+1]
$ w[t+1] = A \lambda_s + b for $ A \equiv (D+Kh)^{-1}, ~~~ b \equiv (D+Kh)^{-1} Kq_s[t] + w_{target}[t+1] .
For $ b' \equiv b-w[t] = (D+Kh)^{-1} Kq_s[t] + w_{target}[t+1] -w[t] and $ dw^{\infty} = w[t+1] - w[t]
$ \lambda^{k+1}_i = -(dw_i^k - D_{ii}\lambda^{k} - b'_i + b_i)/D_{ii}=\lambda^{k} - (dw_i^{k} - b'_i + b_i)/D_{ii}= \lambda^{k} - (dw_i^{k} + w_i[t])/D_{ii}
https://i.gyazo.com/c1d369b0d5adedebbf6124e8a57347ce.jpg