void ApplyPreconditioner(Array2Df x, Array2Df y, Array2Df m)
{
//Maybe Incomplete Cholesky??
int i, j;
float d;
m.Reset();
// solve L*m=x
for (j = 1; j < x.ny - 1; ++j)
{
for (i = 1; i < x.nx - 1; ++i)
{
if (marker[i, j] == FLUID)
{
d = x[i, j] - poisson[i - 1, j, 1] * preconditioner[i - 1, j] * m[i - 1, j]
- poisson[i, j - 1, 2] * preconditioner[i, j - 1] * m[i, j - 1];
m[i, j] = preconditioner[i, j] * d;
}
}
}
// solve L'*y=m
y.Reset();
for (j = x.ny - 2; j > 0; --j)
{
for (i = x.nx - 2; i > 0; --i)
{
if (marker[i, j] == FLUID)
{
d = m[i, j] - poisson[i, j, 1] * preconditioner[i, j] * y[i + 1, j]
- poisson[i, j, 2] * preconditioner[i, j] * y[i, j + 1];
y[i, j] = preconditioner[i, j] * d;
}
}
}
}
public void FindDivergence()
{
r.Reset();
for (int j = 0; j < r.ny; ++j)
{
for (int i = 0; i < r.nx; ++i)
{
if (marker[i, j] == FLUID)
{
r[i, j] = u[i + 1, j] - u[i, j] + v[i, j + 1] - v[i, j];
}
}
}
}
protected void BuildB()
{
rhsB.Reset();
var density = 1;
var cellSpace = 1;
var dt = 1 / 30f;
var scale = density * cellSpace / dt;
for (int j = 0; j < rhsB.ny; ++j)
{
for (int i = 0; i < rhsB.nx; ++i)
{
if (marker[i, j] == FLUID)
{
var im1 = Mathf.Max(0, i - 1);
var jm1 = Mathf.Max(0, j - 1);
var ip1 = Mathf.Min(rhsB.nx - 1, i + 1);
var jp1 = Mathf.Min(rhsB.ny - 1, j + 1);
var div = u[ip1, j] - u[i, j] + v[i, jp1] - v[i, j];
var baseRhs = scale * -div;
//left
if (marker[im1, j] == SOLID)
{
baseRhs += -scale * (u[i, j] - u[im1, j]);
}
//right
if (marker[ip1, j] == SOLID)
{
baseRhs += scale * (u[i, j] - u[ip1, j]);
}
if (marker[i, jm1] == SOLID)
{
baseRhs += -scale * (v[i, j] - v[im1, j]);
}
if (marker[i, jp1] == SOLID)
{
baseRhs += scale * (v[i, j] - v[ip1, j]);
}
rhsB[i, j] = baseRhs;
}
}
}
}
public void ApplyPoisson(Array2Df x, Array2Df y)
{
y.Reset();
for (int j = 1; j < poisson.ny - 1; ++j)
{
for (int i = 1; i < poisson.nx - 1; ++i)
{
if (marker[i, j] == FLUID)
{
y[i, j] = poisson[i, j, 0] * x[i, j]
+ poisson[i - 1, j, 1] * x[i - 1, j]
+ poisson[i, j, 1] * x[i + 1, j]
+ poisson[i, j - 1, 2] * x[i, j - 1]
+ poisson[i, j, 2] * x[i, j + 1];
}
}
}
}
void FormPreconditioner()
{
const float mic_parameter = 0.99f;
float d;
preconditioner.Reset();
for (int j = 1; j < preconditioner.ny - 1; ++j)
{
for (int i = 1; i < preconditioner.nx - 1; ++i)
{
if (marker[i, j] == FLUID)
{
d = poisson[i, j, 0] - sqr(poisson[i - 1, j, 1] * preconditioner[i - 1, j])
- sqr(poisson[i, j - 1, 2] * preconditioner[i, j - 1])
- mic_parameter * (poisson[i - 1, j, 1] * poisson[i - 1, j, 2] * sqr(preconditioner[i - 1, j])
+ poisson[i, j - 1, 2] * poisson[i, j - 1, 1] * sqr(preconditioner[i, j - 1]));
preconditioner[i, j] = 1 / Mathf.Sqrt(d + 1e-6f);
}
}
}
}
public void SolvePressure(int maxits, double tolerance)
{
int its;
double tol = tolerance * r.InfNorm();
pressure.Reset();
if (r.InfNorm() == 0)
{
return;
}
ApplyPreconditioner(r, z, m);
z.CopyTo(s);
float rho = z.Dot(r);
if (rho == 0)
{
return;
}
for (its = 0; its < maxits; ++its)
{
ApplyPoisson(s, z);
float alpha = rho / s.Dot(z);
pressure.Increment(alpha, s);
r.Increment(-alpha, z);
if (r.InfNorm() <= tol)
{
Debug.LogFormat("pressure converged to {0} in {1} iterations\n", r.InfNorm(), its);
return;
}
ApplyPreconditioner(r, z, m);
float rhonew = z.Dot(r);
float beta = rhonew / rho;
s.ScaleAndIncrement(beta, z);
rho = rhonew;
}
Debug.LogFormat("Didn't converge in pressure solve (its={0}, tol={1}, |r|={2})\n", its, tol, r.InfNorm());
}
public void TransferToGrid()
{
int p = 0, i = 0, ui = 0, j = 0, vj = 0;
float fx = 0, ufx = 0, fy = 0, vfy = 0;
grid.u.Reset();
sum.Reset();
for (p = 0; p < np; ++p)
{
grid.bary_x(x[p][0], ref ui, ref ufx);
grid.bary_y_centre(x[p][1], ref j, ref fy);
accumulate(grid.u, u[p][0], ui, j, ufx, fy);
/* TODO: call affineFix to incorporate c_px^n into the grid.u update */
if (type == PICMain.SimType.APIC)
{
affineFix(grid.u, cx[p], ui, j, ufx, fy);
}
}
for (j = 0; j < grid.u.ny; ++j)
{
for (i = 0; i < grid.u.nx; ++i)
{
if (sum[i, j] != 0)
{
grid.u[i, j] /= sum[i, j];
}
}
}
grid.v.Reset();
sum.Reset();
for (p = 0; p < np; ++p)
{
grid.bary_x_centre(x[p][0], ref i, ref fx);
grid.bary_y(x[p][1], ref vj, ref vfy);
accumulate(grid.v, u[p][1], i, vj, fx, vfy);
/* TODO: call affineFix to incorporate c_py^n into the grid.v update */
if (type == PICMain.SimType.APIC)
{
affineFix(grid.v, cy[p], i, vj, fx, vfy);
}
}
for (j = 0; j < grid.v.ny; ++j)
{
for (i = 0; i < grid.v.nx; ++i)
{
if (sum[i, j] != 0)
{
grid.v[i, j] /= sum[i, j];
}
}
}
// identify where particles are in grid
grid.marker.Reset();
for (p = 0; p < np; ++p)
{
grid.bary_x(x[p][0], ref i, ref fx);
grid.bary_y(x[p][1], ref j, ref fy);
grid.marker[i, j] = PICGrid.FLUID;
}
}