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);
}
