Completed solve_density.c

The algorithm works properly