import taichi as ti
from taichi.linalg import MatrixFreeCG, LinearOperator
ti.init(arch=ti.cpu,debug=False)
q = 0.5
n_particles = int(8192 * q**2)
n_grid = int(128 * q)
dx, inv_dx = 1 / n_grid, float(n_grid)
dt = 1e-4

p_rho = 1
p_vol = (dx * 0.5) ** 2
p_mass = p_vol * p_rho
gravity = 98
bound = 3

E, nu = 3e3, 0.0  # Young's modulus and Poisson's ratio
mu, la = E / (2 * (1 + nu)), E * nu / ((1 + nu) * (1 - 2 * nu))
kappa = .666666 * mu + la
x = ti.Vector.field(2, float, n_particles)
psi_max = ti.field(float, n_particles)
color = ti.Vector.field(3, float, n_particles)
v = ti.Vector.field(2, float, n_particles)
C = ti.Matrix.field(2, 2, float, n_particles)
F = ti.Matrix.field(2, 2, dtype=float, shape=n_particles)  # deformation gradient

grid_v = ti.Vector.field(2, float, (n_grid, n_grid))
grid_m = ti.field(float, (n_grid, n_grid))
@ti.func
def compute_stress(Fp,g=1.):
    B = Fp@Fp.transpose()
    devB = B - ti.Matrix.identity(float, 2)  * 1. / 2.*B.trace()
    J = Fp.determinant()
    tau_dev = mu * 1./J * devB
    prime = kappa/2. * (J-1./J)
    tau_vol = J*prime*ti.Matrix.identity(float, 2)
    out = g * (tau_dev + tau_vol) if J >= 1. else g * tau_dev + tau_vol
    return out
@ti.func
def get_psi_pos(Fp):
    J = Fp.determinant()
    JaF = 1/ti.sqrt(J)*Fp
    psi_dev = mu/2*((JaF.transpose()@JaF).trace()-2)
    psi_vol = kappa/2 * ((J**2-1)/2 - ti.log(J))
    out = psi_dev + psi_vol if J >= 1. else  psi_dev
    return out
@ti.kernel
def reset_grid():
    for i, j in grid_m:
        grid_v[i, j] = [0, 0]
        grid_m[i, j] = 0
@ti.kernel
def p2g():
    for p in x:
        Xp = x[p] / dx
        base = int(Xp - 0.5)
        fx = Xp - base
        w = [0.5 * (1.5 - fx) ** 2, 0.75 - (fx - 1) ** 2, 0.5 * (fx - 0.5) ** 2]
        F[p] = (ti.Matrix.identity(float, 2) + dt * C[p]) @ F[p]
        U, sig, V = ti.svd(F[p])
        J = F[p].determinant()
        stress = compute_stress(F[p])
        stress = (-dt * p_vol * 4 * inv_dx * inv_dx) * stress
        affine = stress + p_mass * C[p]
        for i, j in ti.static(ti.ndrange(3, 3)):
            offset = ti.Vector([i, j])
            dpos = (offset - fx) * dx
            grid_idx = base + offset
            weight = w[i].x * w[j].y
            grid_v[grid_idx] += weight * (p_mass * v[p] + affine @ dpos)
            grid_m[grid_idx] += weight * p_mass

@ti.kernel
def grid_op():
    for i, j in grid_m:
        if grid_m[i, j] > 0:
            grid_v[i, j] /= grid_m[i, j]
        grid_v[i, j].y -= dt * gravity
        if i < bound and grid_v[i, j].x < 0:
            grid_v[i, j].x = 0
        if i > n_grid - bound and grid_v[i, j].x > 0:
            grid_v[i, j].x = 0
        if j < bound and grid_v[i, j].y < 0:
            grid_v[i, j].y = 0
        if j > n_grid - bound and grid_v[i, j].y > 0:
            grid_v[i, j].y = 0

@ti.kernel
def g2p():
    for p in x:
        Xp = x[p] / dx
        base = int(Xp - 0.5)
        fx = Xp - base
        w = [0.5 * (1.5 - fx) ** 2, 0.75 - (fx - 1) ** 2, 0.5 * (fx - 0.5) ** 2]
        new_v = ti.Vector.zero(float, 2)
        new_C = ti.Matrix.zero(float, 2, 2)
        for i, j in ti.static(ti.ndrange(3, 3)):
            offset = ti.Vector([i, j])
            dpos = (offset - fx) * dx
            grid_idx = base + offset
            grid_idx = ti.Vector([ti.math.clamp(grid_idx[0], 0, grid_v.shape[0] - 1),ti.math.clamp(grid_idx[1], 0, grid_v.shape[1] - 1)])
            weight = w[i].x * w[j].y
            g_v = grid_v[grid_idx]
            new_v += weight * g_v
            new_C += 4 * weight * g_v.outer_product(dpos) / dx**2
        v[p] = new_v
        x[p] += dt * v[p]
        C[p] = new_C

@ti.kernel
def init():
    for i in range(n_particles):
        x[i] = [ti.random() * 0.5 + 0.25, ti.random() * 0.5 + 0.50]
        v[i] = [0, -1]
        F[i] = ti.Matrix([[1, 0], [0, 1]])

# Initialize the particles
init()

# Main GUI loop
gui = ti.ui.Window("MPM88", (1024, 1024))
canvas = gui.get_canvas()
while gui.running:
    canvas.set_background_color((1, 1, 1))
    for s in range(2):
        reset_grid()
        p2g()
        grid_op()
        g2p()
    canvas.circles(x, radius=0.005, per_vertex_color=color)
    gui.show()
Add phase field using a cg Solver, imlement it fully