diff --git a/examples/Freefem/MMS/3D/coro_script.py b/examples/Freefem/MMS/3D/coro_script.py new file mode 100644 index 00000000..ce6f08e1 --- /dev/null +++ b/examples/Freefem/MMS/3D/coro_script.py @@ -0,0 +1,129 @@ +import numpy as np + +from manufactured_solution import MMSCase3D, lame +from solidCoro import (case_scene, run_reference_scene, + element_hex, hex_q1_rule) + +SINUS_AMPLITUDE = 1.0 +THETA = np.radians(45.0) + + +def _Rz(theta): + """Rotation matrix about z-axis.""" + c, s = np.cos(theta), np.sin(theta) + return np.array([[ c, -s, 0], + [ s, c, 0], + [ 0, 0, 1]]) + + +class SinusCorotational(MMSCase3D): + name = "sinus_corotational" + source_quadrature_hex = staticmethod(hex_q1_rule(2)) + + _Rtheta = _Rz(THETA) + + def _u_d(self, x, y, z, L): + """Deformational part only (small sinusoidal field).""" + k = np.pi / L + A = SINUS_AMPLITUDE + return np.array([A * np.sin(k*x) * np.sin(k*y), + A * np.sin(k*y) * np.sin(k*z), + A * np.sin(k*z) * np.sin(k*x)]) + + def _grad_u_d(self, x, y, z, L): + """∇u_d (3x3).""" + k = np.pi / L + A = SINUS_AMPLITUDE + zero = 0.0 if np.isscalar(x) else np.zeros_like(np.asarray(x, float)) + return np.array([ + [A*k*np.cos(k*x)*np.sin(k*y), A*k*np.sin(k*x)*np.cos(k*y), zero], + [zero, A*k*np.cos(k*y)*np.sin(k*z), A*k*np.sin(k*y)*np.cos(k*z)], + [A*k*np.cos(k*x)*np.sin(k*z), zero, A*k*np.cos(k*z)*np.sin(k*x)], + ]) + + def u_ex(self, x, y, z, L): + R = self._Rtheta + x_vec = np.array([x, y, z]) + rbm = (R - np.eye(3)) @ x_vec # composante rigide + ud = self._u_d(x, y, z, L) + total = rbm + ud + return (total[0], total[1], total[2]) + + def grad_u_ex(self, x, y, z, L): + R = self._Rtheta + grad_rbm = R - np.eye(3) + grad_ud = self._grad_u_d(x, y, z, L) + return grad_rbm + grad_ud + + def source(self, x, y, z, E, nu, L): + lam, mu = lame(E, nu) + R = self._Rtheta + + A = SINUS_AMPLITUDE + p = np.pi / L + sx, sy, sz = np.sin(p*x), np.sin(p*y), np.sin(p*z) + cx, cy, cz = np.cos(p*x), np.cos(p*y), np.cos(p*z) + + lap_ux = -2 * p**2 * sx * sy + lap_uy = -2 * p**2 * sy * sz + lap_uz = -2 * p**2 * sz * sx + + d_divu_dx = p**2 * (-sx*sy + cz*cx) + d_divu_dy = p**2 * ( cx*cy - sy*sz) + d_divu_dz = p**2 * ( cy*cz - sz*sx) + + div_sigma_local = A * np.array([ + (lam + mu) * d_divu_dx + mu * lap_ux, + (lam + mu) * d_divu_dy + mu * lap_uy, + (lam + mu) * d_divu_dz + mu * lap_uz, + ]) + + f = R @ (-div_sigma_local) + return (f[0], f[1], f[2]) + + def traction(self, x, y, z, nx, ny, nz, E, nu, L): + """t = R_theta * sigma_bar . n sur ∂Ω_N.""" + lam, mu = lame(E, nu) + R = self._Rtheta + grad_ud = self._grad_u_d(x, y, z, L) + eps_bar = 0.5 * (grad_ud + grad_ud.T) + tr_eps = eps_bar[0,0] + eps_bar[1,1] + eps_bar[2,2] + sigma_bar = lam * tr_eps * np.eye(3) + 2 * mu * eps_bar + n = np.array([nx, ny, nz]) + t = R @ (sigma_bar @ n) + return (t[0], t[1], t[2]) + + + def apply_bcs(self, Solid, nodes_3d, L): + eps = 1e-10 + xyz = nodes_3d[:, :3] + dofs = Solid.dofs + + # ── Shift trick sur TOUS les nœuds ──────────────────────────────── + # → Newton part d'un état cohérent, proche de la solution + with dofs.position.writeable() as pos: + for i, (x, y, z) in enumerate(xyz): + ux, uy, uz = self.u_ex(x, y, z, L) + pos[i] = [x + ux, y + uy, z + uz] + + + boundary_indices = [ + i for i, (x, y, z) in enumerate(xyz) + if x < eps or x > L - eps + or y < eps or y > L - eps + or z < eps or z > L - eps + ] + Solid.addObject("FixedProjectiveConstraint", + name="fix_boundary", + indices=boundary_indices) + + + + +mms = SinusCorotational() + +createScene = case_scene(mms, element_hex) + + +if __name__ == "__main__": + run_reference_scene(element_hex, mms) \ No newline at end of file diff --git a/examples/Freefem/MMS/3D/params.json b/examples/Freefem/MMS/3D/params.json index 6a902e26..fc4f2d8b 100644 --- a/examples/Freefem/MMS/3D/params.json +++ b/examples/Freefem/MMS/3D/params.json @@ -2,13 +2,14 @@ "length": 1.0, "youngModulus": 1e6, "reference": { - "nx": 6, + "nx": 20, "nu": 0.3 }, "convergence": { "nu_values": [0.0, 0.3, 0.49], "nx_values": { - "sinus_neumann": [4, 6, 8, 12, 16] + "sinus_neumann": [4, 6, 8, 12, 16], + "sinus_corotational": [4, 6, 8, 12, 20] } } } diff --git a/examples/Freefem/MMS/3D/solidCoro.py b/examples/Freefem/MMS/3D/solidCoro.py new file mode 100644 index 00000000..60433650 --- /dev/null +++ b/examples/Freefem/MMS/3D/solidCoro.py @@ -0,0 +1,310 @@ +import json +import numpy as np +import matplotlib.pyplot as plt +import os +import sys + +import Sofa +import Sofa.Core +import Sofa.Simulation +import SofaRuntime + +sys.path.insert(0, os.path.dirname(os.path.dirname(os.path.abspath(__file__)))) +from fem import hex_q1_rule +from elements import element_hex +from solid_solution import SolidSolution3D +from output import write_solution_table +from scene import NodalForceAssembler + +RESULTS_DIR = os.path.join(os.path.dirname(os.path.abspath(__file__)), "results") + + +# --------------------------------------------------------------------------- +# Parameters +# --------------------------------------------------------------------------- + +def load_params(path=None): + if path is None: + path = os.path.join(os.path.dirname(os.path.abspath(__file__)), + "params.json") + with open(path) as f: + return json.load(f) + + +# ============== Mesh helpers ============================ + +def get_nodes_3d(L, nx, ny, nz): + dx = L / (nx - 1) + dy = L / (ny - 1) + dz = L / (nz - 1) + pts = [[i*dx, j*dy, k*dz] + for k in range(nz) for j in range(ny) for i in range(nx)] + return np.array(pts, dtype=float) + + +# --------------------------------------------------------------------------- +# SOFA scene +# --------------------------------------------------------------------------- + +def _solid_force_compute(element, mms, L, E, nu, nx, ny, nz): + """Return a `(nodes, topology) -> (N, 3) force array` for NodalForceAssembler.""" + def compute(nodes, topology): + conn = element.read_connectivity(topology) + return element.compute_nodal_forces( + nodes, conn, mms, L, E, nu, nx, ny, nz) + return compute + + +def build_solid_scene(rootNode, mms, element, L=1.0, E=1e6, nu=0.3, + nx=6, ny=6, nz=6, with_visual=True): + """Build a SOFA scene for `mms` on the 3D `element` strategy using + HexahedronCorotationalFEMForceField. + + Returns (dofs, topology). + """ + rootNode.addObject("RequiredPlugin", pluginName=[ + "Sofa.Component.Constraint.Projective", + "Sofa.Component.Engine.Select", + "Sofa.Component.LinearSolver.Direct", + "Sofa.Component.MechanicalLoad", + "Sofa.Component.ODESolver.Backward", + "Sofa.Component.SolidMechanics.FEM.Elastic", # HexahedronCorotationalFEMForceField + "Sofa.Component.StateContainer", + "Sofa.Component.Topology.Container.Grid", + "Sofa.Component.Topology.Container.Dynamic", + "Sofa.Component.Topology.Mapping", + "Sofa.Component.Visual", + ]) + rootNode.addObject("DefaultAnimationLoop") + if with_visual: + rootNode.addObject("VisualStyle", + displayFlags="showBehaviorModels showForceFields") + + nodes_3d = get_nodes_3d(L, nx, ny, nz) + + Grid = rootNode.addChild("Grid") + Grid.addObject("RegularGridTopology", name="grid", + nx=nx, ny=ny, nz=nz, + min=[0.0, 0.0, 0.0], max=[L, L, L]) + + Solid = rootNode.addChild("Solid") + Solid.addObject("StaticSolver", name="staticSolver", printLog=False) + Solid.addObject("NewtonRaphsonSolver", name="newtonSolver", + maxNbIterationsNewton=5, + absoluteResidualStoppingThreshold=1e-10, + printLog=False) + Solid.addObject("SparseLDLSolver", name="linearSolver", + template="CompressedRowSparseMatrixd") + + dofs = Solid.addObject("MechanicalObject", name="dofs", template="Vec3d", + position=nodes_3d.tolist(), + showObject=with_visual, showObjectScale=0.005 * L) + + topology = element.add_topology(Solid) + + # ------------------------------------------------------------------ + # Corotational hexahedral FEM + # ------------------------------------------------------------------ + # method: "large" (default) uses the polar-decomposition corotational + # frame; "small" falls back to linear kinematics; "polar2" is an + # alternative polar variant. Use "large" for a genuine CR formulation. + Solid.addObject("HexahedronCorotationalFEMForceField", + name="FEM", + template="Vec3d", + youngModulus=E, + poissonRatio=nu, + rotationMethod="large") + # ------------------------------------------------------------------ + + mms.apply_bcs(Solid, nodes_3d, L) + + n_nodes = len(nodes_3d) + force_field = Solid.addObject("ConstantForceField", name="MMS_forces", + template="Vec3d", + indices=list(range(n_nodes)), + forces=[[0.0, 0.0, 0.0]] * n_nodes) + + Solid.addObject(NodalForceAssembler( + dofs=dofs, topology=topology, force_field=force_field, + compute_forces=_solid_force_compute(element, mms, L, E, nu, nx, ny, nz), + name="nodalForceAssembler")) + + return dofs, topology + + +# ───────────────────────────────────────────────────────────────────────────── +# Simulation runner +# ───────────────────────────────────────────────────────────────────────────── + +def solve_solid(elem, mms, L, E, nu, nx, ny, nz): + """Build, init, and run one static step. Returns a SolidSolution3D snapshot.""" + root = Sofa.Core.Node("root") + dofs, topology = build_solid_scene( + root, mms, elem, L=L, E=E, nu=nu, + nx=nx, ny=ny, nz=nz, with_visual=False + ) + + # ── AJOUT 1 : sauvegarder X avant tout ──────────────────────────── + nodes_3d_original = get_nodes_3d(L, nx, ny, nz) + + Sofa.Simulation.init(root) + + # ── AJOUT 2 : rest_position = position shiftée après init ───────── + # apply_bcs a placé dofs.position à X + u_ex(X) + # On synchronise rest_position pour que SOFA parte de là + with dofs.rest_position.writeable() as rpos: + rpos[:] = dofs.position.array().copy() + + conn = elem.read_connectivity(topology) + Sofa.Simulation.animate(root, root.dt.value) + pos1 = dofs.position.array().copy() + Sofa.Simulation.unload(root) + + # ── AJOUT 3 : déplacement = pos_finale - X_original ─────────────── + # (pas pos_finale - pos0_shiftée comme avant) + ux = pos1[:, 0] - nodes_3d_original[:, 0] + uy = pos1[:, 1] - nodes_3d_original[:, 1] + uz = pos1[:, 2] - nodes_3d_original[:, 2] + + return SolidSolution3D( + nodes=nodes_3d_original, conn=conn, ux=ux, uy=uy, uz=uz + ) + + +# --------------------------------------------------------------------------- +# Output helpers +# --------------------------------------------------------------------------- + +def plot_solution_profile(stem, sol, mms, L, nx, ny, nz, label, nu, l2, h1): + """Save 1-D centerline profiles (ux(x), uy(y), uz(z)) to results/.png.""" + os.makedirs(RESULTS_DIR, exist_ok=True) + xyz = sol.nodes[:, :3] + + mid_i, mid_j, mid_k = nx // 2, ny // 2, nz // 2 + + def nidx(i, j, k): + return i + j * nx + k * nx * ny + + x_fine = np.linspace(0, L, 200) + + line_x = [nidx(i, mid_j, mid_k) for i in range(nx)] + line_y = [nidx(mid_i, j, mid_k) for j in range(ny)] + line_z = [nidx(mid_i, mid_j, k) for k in range(nz)] + + yc, zc = xyz[line_x[0], 1], xyz[line_x[0], 2] + xc2, zc2 = xyz[line_y[0], 0], xyz[line_y[0], 2] + xc3, yc3 = xyz[line_z[0], 0], xyz[line_z[0], 1] + + ux_ex, _, _ = mms.u_ex(x_fine, + np.full_like(x_fine, yc), + np.full_like(x_fine, zc), L) + _, uy_ex, _ = mms.u_ex(np.full_like(x_fine, xc2), + x_fine, + np.full_like(x_fine, zc2), L) + _, _, uz_ex = mms.u_ex(np.full_like(x_fine, xc3), + np.full_like(x_fine, yc3), + x_fine, L) + + fig, axes = plt.subplots(1, 3, figsize=(15, 4)) + for ax, coord, sofa, exact, ylabel, axname in [ + (axes[0], xyz[line_x, 0], sol.ux[line_x], ux_ex, r"$u_x$", "x"), + (axes[1], xyz[line_y, 1], sol.uy[line_y], uy_ex, r"$u_y$", "y"), + (axes[2], xyz[line_z, 2], sol.uz[line_z], uz_ex, r"$u_z$", "z"), + ]: + ax.plot(coord, sofa, "o-", color="tab:green", + label=f"SOFA {label}", ms=5) + ax.plot(x_fine, exact, "--", color="tab:blue", label="MMS exact") + ax.set_xlabel(axname); ax.set_ylabel(ylabel) + ax.legend(); ax.grid(True, alpha=0.3) + fig.suptitle(f"{mms.name} — {label} " + f"nu={nu} nx={nx} |L2={l2:.2e} H1={h1:.2e}") + fig.tight_layout() + fig.savefig(os.path.join(RESULTS_DIR, f"{stem}.png"), dpi=150) + plt.close(fig) + + +def plot_solution_slices(stem, sol, mms, L, nx, ny, nz, label, nu): + """Save midplane heatmaps to results/.png.""" + os.makedirs(RESULTS_DIR, exist_ok=True) + xyz = sol.nodes[:, :3] + mid_i, mid_j, mid_k = nx // 2, ny // 2, nz // 2 + + def nidx(i, j, k): + return i + j * nx + k * nx * ny + + sl_z = np.array([[nidx(i, j, mid_k) for i in range(nx)] for j in range(ny)]) + sl_y = np.array([[nidx(i, mid_j, k) for i in range(nx)] for k in range(nz)]) + sl_x = np.array([[nidx(mid_i, j, k) for j in range(ny)] for k in range(nz)]) + + fig, axes = plt.subplots(2, 3, figsize=(16, 10)) + + X, Y = xyz[sl_z, 0], xyz[sl_z, 1] + for row, u, title in [(0, sol.ux, r"$u_x$ z=mid"), + (1, sol.uy, r"$u_y$ z=mid")]: + im = axes[row, 0].pcolormesh(X, Y, u[sl_z], cmap="RdBu_r", shading="auto") + axes[row, 0].set(xlabel="x", ylabel="y", title=title) + plt.colorbar(im, ax=axes[row, 0]) + + X, Z = xyz[sl_y, 0], xyz[sl_y, 2] + for row, u, title in [(0, sol.ux, r"$u_x$ y=mid"), + (1, sol.uz, r"$u_z$ y=mid")]: + im = axes[row, 1].pcolormesh(X, Z, u[sl_y], cmap="RdBu_r", shading="auto") + axes[row, 1].set(xlabel="x", ylabel="z", title=title) + plt.colorbar(im, ax=axes[row, 1]) + + Y, Z = xyz[sl_x, 1], xyz[sl_x, 2] + for row, u, title in [(0, sol.uy, r"$u_y$ x=mid"), + (1, sol.uz, r"$u_z$ x=mid")]: + im = axes[row, 2].pcolormesh(Y, Z, u[sl_x], cmap="RdBu_r", shading="auto") + axes[row, 2].set(xlabel="y", ylabel="z", title=title) + plt.colorbar(im, ax=axes[row, 2]) + + fig.suptitle(f"Fields 3D — {label} {mms.name} nu={nu} nx={nx}") + fig.tight_layout() + fig.savefig(os.path.join(RESULTS_DIR, f"{stem}.png"), dpi=150) + plt.close(fig) + + +# ───────────────────────────────────────────────────────────────────────────── +# Single-case driver +# ───────────────────────────────────────────────────────────────────────────── + +def run_reference_scene(elem, mms): + """Solve one MMS case at the reference mesh, write the solution table and plots.""" + cfg = load_params() + ref = cfg["reference"] + L, E = cfg["length"], cfg["youngModulus"] + nu = ref["nu"] + nx = ny = nz = ref["nx"] + + sol = solve_solid(elem, mms, L, E, nu, nx, ny, nz) + l2 = elem.compute_l2(sol, mms, L) + h1 = elem.compute_h1(sol, mms, L) + + label = elem.LABEL + tag = label.replace(" ", "_") + stem = f"{mms.name}_{tag}_nu{nu}_nx{nx}" + + xyz = sol.nodes[:, :3] + write_solution_table(f"solution_{stem}", xyz, + np.column_stack([sol.ux, sol.uy, sol.uz]), + lambda xi, yi, zi: mms.u_ex(xi, yi, zi, L), + RESULTS_DIR, {"L2": l2, "H1_semi": h1}) + plot_solution_profile(f"solution_{stem}", sol, mms, L, nx, ny, nz, + label, nu, l2, h1) + plot_solution_slices (f"fields3D_{stem}", sol, mms, L, nx, ny, nz, + label, nu) + + +def case_scene(mms, element): + + def createScene(rootNode): + cfg = load_params() + ref = cfg["reference"] + build_solid_scene(rootNode, mms, element, + L=cfg["length"], E=cfg["youngModulus"], + nu=ref["nu"], + nx=ref["nx"], ny=ref["nx"], nz=ref["nx"], + with_visual=True) + return rootNode + return createScene \ No newline at end of file