void simulate_implicit_euler(lfVector *Xnew, lfVector *Vnew, lfVector *X, lfVector *V, lfVector *F, lfVector *olddV, fmatrix3x3 *dFdV, fmatrix3x3 *dFdX, float dt) { lfVector *dFdXmV = create_lfvector(dFdV[0].vcount); lfVector *dV = create_lfvector(dFdV[0].vcount); // cp_bfmatrix(A, big_I); initdiag_bfmatrix(A, I); subadd_bfmatrixS_bfmatrixS(A, dFdV, dt, dFdX, (dt*dt)); mul_bfmatrix_lfvector(dFdXmV, dFdX, V); add_lfvectorS_lfvectorS(B, F, dt, dFdXmV, (dt*dt)); cg_filtered(dV, A, B); /* conjugate gradient algorithm to solve Ax=b */ // advance positions add_lfvector_lfvector(Vnew, V, dV); add_lfvector_lfvectorS(Xnew, X, V, dt); del_lfvector(dFdXmV); del_lfvector(dV); }