static void solve(struct PdSolver *solver, Eigen::VectorXf const &b) { float const epsilon = 1e-6f; float error = 1.0f; uint32_t iters = 0; Eigen::VectorXf x_prev = solver->positions_last; while (error > epsilon * epsilon || iters < 2) { #pragma omp parallel for for (uint32_t i = 0; i < solver->m; ++i) { Eigen::VectorXf y = b.block(i*solver->block_n, 0, solver->block_n, 1); SolverT &lu = solver->blocks[i]; for (uint32_t j = 0; j < solver->n; ++j) { if (i != j); else continue; y -= solver->a_mat.block(i*solver->block_m, j*solver->block_n, solver->block_m, solver->block_n) * x_prev.block(j * solver->block_n, 0, solver->block_n, 1); } solver->positions.block(i*solver->block_n, 0, solver->block_n, 1) = lu.solve(y); } x_prev = solver->positions; error = (b - solver->a_mat * x_prev).squaredNorm(); ++iters; } solver->block_jacobi_iters = iters; solver->block_jacobi_err = std::sqrt(error); }