diff --git a/examples/rigid_body_bounce_ipc.py b/examples/rigid_body_bounce_ipc.py new file mode 100644 index 00000000..d787c095 --- /dev/null +++ b/examples/rigid_body_bounce_ipc.py @@ -0,0 +1,131 @@ +"""Sphere bouncing on a plane — NonLinear + IPC contact + Rayleigh damping. + +Uses RigidBody with Newmark integration built into the assembly, +solved by Fedoo's standard NonLinear solver. +""" + +import os +import time +import numpy as np +import pyvista as pv + +import fedoo as fd + +g = 9.81 +mass = 1.0 +radius = 0.1 +z0 = 0.5 +dt = 5e-4 +t_end = 2.0 + +print("=" * 60) +print("SPHERE BOUNCE — NonLinear + IPC + Rayleigh") +print(f" m={mass}kg, z0={z0}m, r={radius}m, dt={dt}s") +print("=" * 60) + +space = fd.ModelingSpace("3D") +space.new_variable("DispX") +space.new_variable("DispY") +space.new_variable("DispZ") +space.new_vector("Disp", ("DispX", "DispY", "DispZ")) + +ball_mesh = fd.Mesh.from_pyvista( + pv.Sphere(radius=radius, center=(0, 0, z0), theta_resolution=10, phi_resolution=10) +) +plane_mesh = fd.Mesh.from_pyvista( + pv.Plane( + center=(0, 0, 0), + direction=(0, 0, 1), + i_size=1.5, + j_size=1.5, + i_resolution=6, + j_resolution=6, + ).triangulate(), + name="Floor", +) + +body = fd.constraint.RigidBody( + ball_mesh, + mass=mass, + inertia_tensor=(2 / 5) * mass * radius**2 * np.eye(3), + center_of_mass=np.array([0, 0, z0]), +) +body.set_force([0, 0, -mass * g]) +body.set_rayleigh_damping(1.0) +body.set_static_obstacle(plane_mesh, dhat=0.01, kappa=1e8) + +# Solve with NonLinear and save the trajectory through Fedoo's output system. +pb = fd.problem.NonLinear(body.assembly) +pb.set_time_integrator(fd.time.SECOND_ORDER, fd.time.Newmark()) + +results = pb.add_output( + "rigid_body_bounce", + ["Disp", "RigidDisp", "RigidRot"], + include_static_obstacles=True, # include the plane_mesh in the results field +) + +t0 = time.time() +pb.nlsolve( + dt=dt, + dt_max=dt, + tmax=t_end, + update_dt=True, + print_info=1, + interval_output=dt * 80, +) + +# Results can be visualized with ``fd.viewer(results)``. + +elapsed = time.time() - t0 +# The initial state is prepended because output frames start after the first +# requested output interval. +history = results.get_history( + ["RigidDisp", "Time"], + component=["Z", 0], +) +t_hist = np.r_[0.0, history["Time"]] +z_hist = z0 + np.r_[0.0, np.asarray(history["RigidDisp"]).reshape(-1)] +print(f"\n {results.n_iter} saved frames in {elapsed:.1f}s") +print(f" z_min={z_hist.min():.4f}m, z_max={z_hist.max():.4f}m") + +# Animation +_here = os.path.dirname(__file__) if "__file__" in globals() else os.getcwd() +gif_path = os.path.join(_here, "rigid_body_bounce_ipc.gif") +fps = 25 +frame_indices = np.arange(len(t_hist)) + +sphere = pv.Sphere( + radius=radius, center=(0, 0, z0), theta_resolution=20, phi_resolution=20 +) +pts_ref = sphere.points.copy() +vis_plane = pv.Plane( + center=(0, 0, 0), + direction=(0, 0, 1), + i_size=1.5, + j_size=1.5, + i_resolution=10, + j_resolution=10, +) + +pl = pv.Plotter(window_size=[800, 600], off_screen=True) +pl.set_background("white") +pl.add_mesh(vis_plane, color="lightgrey", opacity=0.8, show_edges=True) +pl.add_mesh(sphere, color="steelblue", smooth_shading=True) +pl.camera_position = [(1.2, -1.2, 0.8), (0, 0, 0.25), (0, 0, 1)] +pl.open_gif(gif_path, fps=fps) + +for i in frame_indices: + sphere.points[:] = pts_ref + np.array([[0, 0, z_hist[i] - z0]]) + sphere.GetPoints().Modified() + pl.add_text( + f"t={t_hist[i]:.2f}s z={z_hist[i]:.3f}m NonLinear+IPC", + position="upper_edge", + font_size=11, + color="black", + name="title", + ) + pl.render() + pl.write_frame() + +pl.close() +print(f" Saved: {gif_path}") diff --git a/examples/rigid_body_bunny_bounce.py b/examples/rigid_body_bunny_bounce.py new file mode 100644 index 00000000..0841c328 --- /dev/null +++ b/examples/rigid_body_bunny_bounce.py @@ -0,0 +1,148 @@ +"""Bunny bouncing on a plane — NonLinear + IPC contact. + +Convex hull of Stanford bunny (watertight). Demonstrates tumbling +on impact with asymmetric geometry. +""" + +import os +import time +import numpy as np +import pyvista as pv + +import fedoo as fd + +from simcoon import Rotation # required dependency (fedoo imports it at load) + +g = 9.81 +dt = 5e-4 +t_end = 2.0 + +print("=" * 60) +print("BUNNY BOUNCE — NonLinear + IPC contact") +print("=" * 60) + +space = fd.ModelingSpace("3D") +space.new_variable("DispX") +space.new_variable("DispY") +space.new_variable("DispZ") +space.new_vector("Disp", ("DispX", "DispY", "DispZ")) + +# Watertight bunny from convex hull +pv_raw = pv.examples.download_bunny().decimate(0.97) +pv_bunny = pv_raw.delaunay_3d().extract_surface().triangulate().clean() +s = max( + pv_bunny.bounds[1] - pv_bunny.bounds[0], + pv_bunny.bounds[3] - pv_bunny.bounds[2], + pv_bunny.bounds[5] - pv_bunny.bounds[4], +) +pv_bunny = pv_bunny.scale(0.25 / s, inplace=False) +pv_bunny = pv_bunny.translate( + [-pv_bunny.center[0], -pv_bunny.center[1], -pv_bunny.bounds[4] + 0.4], inplace=False +) +pv_bunny = pv_bunny.compute_normals(consistent_normals=True, auto_orient_normals=True) + +bunny_mesh = fd.Mesh.from_pyvista(pv_bunny) +plane_mesh = fd.Mesh.from_pyvista( + pv.Plane( + center=(0, 0, 0), + direction=(0, 0, 1), + i_size=1.5, + j_size=1.5, + i_resolution=6, + j_resolution=6, + ).triangulate() +) + +mass = 0.5 +bb = pv_bunny.bounds +lx, ly, lz = bb[1] - bb[0], bb[3] - bb[2], bb[5] - bb[4] +I = (mass / 12) * np.diag([ly**2 + lz**2, lx**2 + lz**2, lx**2 + ly**2]) + +body = fd.constraint.RigidBody( + bunny_mesh, + mass=mass, + inertia_tensor=I, + center_of_mass=np.array(pv_bunny.center), + name="Bunny", +) +body.set_force([0, 0, -mass * g]) +body.set_rayleigh_damping(1.0) +body.set_static_obstacle(plane_mesh, dhat=0.008, kappa=1e8) + +print(f" Bunny: {bunny_mesh.n_nodes} nodes, mass={mass}kg") + +# Solve with manual loop for trajectory +pb = fd.problem.NonLinear(body.assembly) +pb.set_time_integrator(fd.time.SECOND_ORDER, fd.time.Newmark()) +pb.initialize() + +idx = body.assembly._dof_indices +q_hist = [np.zeros(6)] +t_hist = [0.0] + +t0 = time.time() +n_steps = int(round(t_end / dt)) +for step in range(n_steps): + pb.dtime = dt + pb.solve_time_increment() + pb.set_start() + dof = pb.get_dof_solution() + q_hist.append(dof[idx].copy()) + t_hist.append((step + 1) * dt) + +elapsed = time.time() - t0 +t_hist = np.array(t_hist) +q_hist = np.array(q_hist) +z_hist = body.center_of_mass[2] + q_hist[:, 2] +print( + f" {len(t_hist)} steps in {elapsed:.1f}s ({elapsed / len(t_hist) * 1000:.1f}ms/step)" +) +print(f" z_min={z_hist.min():.4f}m") + +# Animation (low res for file size) +_here = os.path.dirname(__file__) if "__file__" in globals() else os.getcwd() +out = os.path.join(_here, "rigid_body_bunny_bounce.gif") +fps = 15 +frame_skip = max(1, int(1.0 / (fps * dt))) +frame_indices = np.arange(0, len(t_hist), frame_skip) + +vis = pv_bunny.copy() +pts_ref = vis.points.copy() +center = body.center_of_mass + +pl = pv.Plotter(window_size=[600, 400], off_screen=True) +pl.set_background("white") +pl.add_mesh( + pv.Plane( + center=(0, 0, 0), + direction=(0, 0, 1), + i_size=1.5, + j_size=1.5, + i_resolution=8, + j_resolution=8, + ), + color="lightgrey", + opacity=0.8, + show_edges=True, +) +pl.add_mesh(vis, color="sandybrown", smooth_shading=True) +pl.camera_position = [(1.0, -1.0, 0.7), (0, 0, 0.2), (0, 0, 1)] +pl.open_gif(out, fps=fps) + +for i in frame_indices: + qi = q_hist[i] + R = Rotation.from_rotvec(qi[3:]).as_matrix() + vis.points[:] = (pts_ref - center) @ R.T + center + qi[:3] + vis.GetPoints().Modified() + pl.add_text( + f"t={t_hist[i]:.2f}s z={z_hist[i]:.3f}m", + position="upper_edge", + font_size=10, + color="black", + name="t", + ) + pl.render() + pl.write_frame() + +pl.close() +print(f" Saved: {out}") diff --git a/examples/rigid_body_cow_bounce.py b/examples/rigid_body_cow_bounce.py new file mode 100644 index 00000000..afd27107 --- /dev/null +++ b/examples/rigid_body_cow_bounce.py @@ -0,0 +1,153 @@ +"""Cow bouncing on a plane — NonLinear + IPC contact. + +The cow mesh from PyVista is watertight (2903 nodes), decimated for speed. +Full rotation via quaternion, rotation vectors (no gimbal lock). +""" + +import os +import time +import numpy as np +import pyvista as pv + +import fedoo as fd + +from simcoon import Rotation # required dependency (fedoo imports it at load) + +g = 9.81 +dt = 5e-4 +t_end = 2.0 + +print("=" * 60) +print("COW BOUNCE — NonLinear + IPC contact") +print("=" * 60) + +space = fd.ModelingSpace("3D") +space.new_variable("DispX") +space.new_variable("DispY") +space.new_variable("DispZ") +space.new_vector("Disp", ("DispX", "DispY", "DispZ")) + +# Cow mesh +pv_cow = pv.examples.download_cow().triangulate().clean() +size = max( + pv_cow.bounds[1] - pv_cow.bounds[0], + pv_cow.bounds[3] - pv_cow.bounds[2], + pv_cow.bounds[5] - pv_cow.bounds[4], +) +pv_cow = pv_cow.scale(0.3 / size, inplace=False) +pv_cow = pv_cow.translate( + [-pv_cow.center[0], -pv_cow.center[1], -pv_cow.bounds[4] + 0.5], inplace=False +) +pv_cow = pv_cow.decimate(0.9).triangulate().clean() +cow_mesh = fd.Mesh.from_pyvista(pv_cow) + +# Plane +pv_plane = pv.Plane( + center=(0, 0, 0), + direction=(0, 0, 1), + i_size=2.0, + j_size=2.0, + i_resolution=8, + j_resolution=8, +) +plane_mesh = fd.Mesh.from_pyvista(pv_plane.triangulate()) + +# Rigid body +mass = 1.0 +bb = pv_cow.bounds +lx, ly, lz = bb[1] - bb[0], bb[3] - bb[2], bb[5] - bb[4] +I = (mass / 12) * np.diag([ly**2 + lz**2, lx**2 + lz**2, lx**2 + ly**2]) + +body = fd.constraint.RigidBody( + cow_mesh, + mass=mass, + inertia_tensor=I, + center_of_mass=np.array(pv_cow.center), + name="Cow", +) +body.set_force([0, 0, -mass * g]) +body.set_rayleigh_damping(1.5) +body.set_static_obstacle(plane_mesh, dhat=0.01) + +print(f" Cow: {cow_mesh.n_nodes} nodes, mass={mass}kg") + +# Solve with NonLinear (manual stepping for trajectory) +pb = fd.problem.NonLinear(body.assembly) +pb.set_time_integrator(fd.time.SECOND_ORDER, fd.time.Newmark()) +pb.initialize() + +idx = body.assembly._dof_indices +q_hist = [np.zeros(6)] +t_hist = [0.0] +n_steps = int(round(t_end / dt)) + +t0 = time.time() +for step in range(n_steps): + pb.dtime = dt + pb.solve_time_increment() + pb.set_start() + dof = pb.get_dof_solution() + q_hist.append(dof[idx].copy()) + t_hist.append((step + 1) * dt) + if step % 500 == 0: + n_c = ( + len(body.assembly._ipc_collisions) + if body.assembly._ipc_collisions is not None + else 0 + ) + print( + f" t={(step+1)*dt:.2f}s z={body.center_of_mass[2]+dof[idx[2]]:.3f}m contacts={n_c}" + ) + +elapsed = time.time() - t0 +t_hist = np.array(t_hist) +q_hist = np.array(q_hist) +z_hist = body.center_of_mass[2] + q_hist[:, 2] +print( + f"\n {len(t_hist)} steps in {elapsed:.1f}s ({elapsed / len(t_hist) * 1000:.1f}ms/step)" +) +print(f" z_min={z_hist.min():.4f}m") + +# Animation +_here = os.path.dirname(__file__) if "__file__" in globals() else os.getcwd() +gif_path = os.path.join(_here, "rigid_body_cow_bounce.gif") +fps = 25 +frame_skip = max(1, int(1.0 / (fps * dt))) +frame_indices = np.arange(0, len(t_hist), frame_skip) + +vis_cow = pv_cow.copy() +pts_ref = vis_cow.points.copy() +center = body.center_of_mass +vis_plane = pv.Plane( + center=(0, 0, 0), + direction=(0, 0, 1), + i_size=2.0, + j_size=2.0, + i_resolution=10, + j_resolution=10, +) + +pl = pv.Plotter(window_size=[900, 600], off_screen=True) +pl.set_background("white") +pl.add_mesh(vis_plane, color="lightgrey", opacity=0.8, show_edges=True) +pl.add_mesh(vis_cow, color="sandybrown", smooth_shading=True) +pl.camera_position = [(1.5, -1.5, 1.0), (0, 0, 0.25), (0, 0, 1)] +pl.open_gif(gif_path, fps=fps) + +for i in frame_indices: + qi = q_hist[i] + R = Rotation.from_rotvec(qi[3:]).as_matrix() + vis_cow.points[:] = (pts_ref - center) @ R.T + center + qi[:3] + vis_cow.GetPoints().Modified() + pl.add_text( + f"t={t_hist[i]:.2f}s z={z_hist[i]:.3f}m", + position="upper_edge", + font_size=11, + color="black", + name="title", + ) + pl.render() + pl.write_frame() + +pl.close() +print(f" Saved: {gif_path}") diff --git a/examples/rigid_body_freefall.py b/examples/rigid_body_freefall.py new file mode 100644 index 00000000..4d5bdc96 --- /dev/null +++ b/examples/rigid_body_freefall.py @@ -0,0 +1,53 @@ +"""Rigid body free fall — validation with NonLinear solver. + +A rigid sphere falls under gravity. Compared with z(t) = z0 - 0.5*g*t^2. +Uses RigidBody + NonLinear (Fedoo's standard solver). +""" + +import time +import numpy as np +import pyvista as pv + +import fedoo as fd + +g = 9.81 +mass = 1.0 +radius = 0.1 +z0 = 1.0 +dt = 1e-3 +t_end = 0.6 + +print("=" * 60) +print("RIGID BODY FREE FALL — Fedoo validation") +print(f" m={mass}kg, z0={z0}m, dt={dt}s") +print("=" * 60) + +space = fd.ModelingSpace("3D") +space.new_variable("DispX") +space.new_variable("DispY") +space.new_variable("DispZ") +space.new_vector("Disp", ("DispX", "DispY", "DispZ")) + +mesh = fd.Mesh.from_pyvista( + pv.Sphere(radius=radius, center=(0, 0, z0), theta_resolution=8, phi_resolution=8) +) +body = fd.constraint.RigidBody( + mesh, + mass=mass, + inertia_tensor=0.004 * np.eye(3), + center_of_mass=np.array([0, 0, z0]), +) +body.set_force([0, 0, -mass * g]) + +pb = body.solve(dt=dt, tmax=t_end, print_info=0) + +# Read trajectory from final state +idx = body.assembly._dof_indices +dof = pb.get_dof_solution() +dz_final = dof[idx[2]] +z_final = z0 + dz_final +z_analytical = z0 - 0.5 * g * t_end**2 + +print(f" z_final = {z_final:.4f}m (analytical: {z_analytical:.4f}m)") +print(f" error = {abs(z_final - z_analytical):.2e}m") +print(f" {'PASS' if abs(z_final - z_analytical) < 0.01 else 'FAIL'}") diff --git a/fedoo/.DS_Store b/fedoo/.DS_Store deleted file mode 100644 index 5008ddfc..00000000 Binary files a/fedoo/.DS_Store and /dev/null differ diff --git a/fedoo/constitutivelaw/elastic_anisotropic.py b/fedoo/constitutivelaw/elastic_anisotropic.py index 496afdfb..578c6004 100644 --- a/fedoo/constitutivelaw/elastic_anisotropic.py +++ b/fedoo/constitutivelaw/elastic_anisotropic.py @@ -55,6 +55,15 @@ def update(self, assembly, pb): else: total_strain = assembly.sv["Strain"] + # Handle uninitialized strain (e.g. first Newmark step with zero + # displacement). The tangent ``H`` has already been stored in + # ``assembly.sv["TangentMatrix"]`` above, so the early return only + # skips the stress evaluation (correctly zero for zero strain) — no + # stale tangent is left behind. + if np.isscalar(total_strain) and total_strain == 0: + assembly.sv["Stress"] = 0 + return + assembly.sv["Stress"] = StressTensorList( [ sum( diff --git a/fedoo/constraint/__init__.py b/fedoo/constraint/__init__.py index 8416ace1..6138e09c 100644 --- a/fedoo/constraint/__init__.py +++ b/fedoo/constraint/__init__.py @@ -1,4 +1,5 @@ from .rigid_tie import RigidTie, RigidTie2D +from .rigid_body import RigidBody, RigidBodyAssembly from .periodic_bc import PeriodicBC from .contact import Contact, SelfContact from .distributed_load import ( diff --git a/fedoo/constraint/ipc_contact.py b/fedoo/constraint/ipc_contact.py index e34e7d79..caabe07a 100644 --- a/fedoo/constraint/ipc_contact.py +++ b/fedoo/constraint/ipc_contact.py @@ -10,8 +10,6 @@ def _import_ipctk(): """Lazy import of ipctk with a clear error message.""" try: import ipctk - - return ipctk except ImportError: raise ImportError( "The 'ipctk' package is required for IPC contact. " @@ -19,6 +17,48 @@ def _import_ipctk(): "Or install fedoo with IPC support: pip install fedoo[ipc]" ) + version = getattr(ipctk, "__version__", "0") + try: + major_minor = tuple(int(part) for part in version.split(".")[:2]) + except (AttributeError, ValueError): + major_minor = (0, 0) + if major_minor < (1, 6): + raise ImportError( + f"Fedoo IPC requires ipctk >= 1.6, but the loaded runtime is " + f"ipctk {version}. Restart Python after upgrading ipctk; mixing " + "Fedoo's 1.6 API calls with an already-loaded ipctk 1.5 changes " + "BarrierPotential argument semantics and produces invalid contact " + "forces." + ) + return ipctk + + +def _barrier_hessian_psd(barrier, collisions, collision_mesh, vertices): + """ipctk barrier hessian with PSD projection and a degenerate fallback. + + CLAMP/ABS PSD projection can raise on degenerate (zero-distance) contact + pairs; retry without projection so assembly still proceeds. Shared by + :class:`IPCContact` and ``RigidBodyAssembly`` so both use the same policy. + """ + # ipctk is already validated/imported by the callers; a plain cached import + # avoids re-running the version check on the contact hot path. + import ipctk + + try: + return barrier.hessian( + collisions, + collision_mesh, + vertices, + project_hessian_to_psd=ipctk.PSDProjectionMethod.CLAMP, + ) + except RuntimeError: + return barrier.hessian( + collisions, + collision_mesh, + vertices, + project_hessian_to_psd=ipctk.PSDProjectionMethod.NONE, + ) + class IPCContact(AssemblyBase): r"""Contact Assembly based on the IPC (Incremental Potential Contact) method. @@ -261,6 +301,77 @@ def __init__( self.sv = {} self.sv_start = {} + # Rigid body support: list of (node_indices, rigid_body) tuples + self._rigid_bodies = [] + self._rigid_node_set = None # set of rigid node indices (built in initialize) + + def add_rigid_body(self, rigid_body, surface_node_indices=None): + """Register a :class:`RigidBody` with this IPCContact. + + This optional convenience enables direct reduced-coordinate projection + for rigid-vs-deformable (or rigid-vs-rigid) contact where a single + ``IPCContact`` carries the full collision mesh. The body's surface + nodes are routed onto the 6 rigid-body global DOFs by the scatter + matrix via the exact rigid Jacobian + ``J = [I_3 | dR/d(omega) @ r_ref]``, instead of placing forces at + the per-node mesh DOFs. + + Registration is not required for correctness when the rigid surface + nodes already belong to the contact mesh and a :class:`RigidTie` is + attached to the problem. In that generic path, IPC assembles nodal + contact contributions and the problem's MPC condensation projects + them onto the rigid DOFs. Registration additionally associates the + body with the shared mesh and lets automatic barrier-stiffness + estimation account for generalized rigid-body forces directly. + + For rigid-vs-static contact (rigid body bouncing on a fixed floor), + use :meth:`RigidBody.set_static_obstacle` instead — that path uses + a private 6x6 fast route with no shared mesh. + + Parameters + ---------- + rigid_body : RigidBody + The rigid body. When the body's nodes live inside this + IPCContact's mesh (stacked rigid-plus-deformable case), + construct it from a :meth:`Mesh.extract_elements` of the + shared mesh so the carried ``parent_node_indices`` wires + RigidTie to the correct parent DOFs. + ``rigid_body.constraint.list_nodes`` is read as the default + surface index set. + surface_node_indices : array_like, optional + Override for ``rigid_body.constraint.list_nodes``. Rarely + needed; useful only when the rigid body's surface is a strict + subset of its tie node set. + """ + if rigid_body.assembly._ipc_collision_mesh is not None: + raise RuntimeError( + f"{rigid_body.name}: cannot use set_static_obstacle() and " + "IPCContact.add_rigid_body() on the same body — choose " + "either the private rigid-vs-static path or the shared " + "IPCContact." + ) + if surface_node_indices is None: + surface_node_indices = np.asarray( + rigid_body.constraint.list_nodes, dtype=int + ) + else: + surface_node_indices = np.asarray(surface_node_indices, dtype=int) + # Point the body's RigidBodyAssembly at the shared problem mesh so + # ``Assembly.sum`` accepts it (mass / inertia / Newmark stay 6x6 and + # are scattered at the 6 global DOFs regardless of mesh size). + # This rebinds ``rigid_body.assembly.mesh`` as a side effect; guard + # against silently re-pointing a body already registered on a + # *different* IPCContact mesh (re-registering on the same mesh is OK). + prev_mesh = getattr(rigid_body, "_ipc_registered_mesh", None) + if prev_mesh is not None and prev_mesh is not self.mesh: + raise RuntimeError( + f"{rigid_body.name}: already registered on another IPCContact " + "mesh; create a fresh RigidBody per shared-IPC problem." + ) + rigid_body.assembly.mesh = self.mesh + rigid_body._ipc_registered_mesh = self.mesh + self._rigid_bodies.append((surface_node_indices, rigid_body)) + def _create_broad_phase(self): """Create an ipctk BroadPhase instance from the string name.""" ipctk = _import_ipctk() @@ -278,13 +389,20 @@ def _create_broad_phase(self): ) return cls() - def _build_scatter_matrix(self, surface_node_indices, n_nodes, ndim): + def _build_scatter_matrix(self, surface_node_indices, n_nodes, ndim, pb=None): """Build sparse matrix mapping ipctk surface DOFs to fedoo full DOFs. ipctk uses interleaved layout for surface nodes: [x0, y0, z0, x1, y1, z1, ...] fedoo uses blocked layout for all nodes: - [ux_0..ux_N, uy_0..uy_N, uz_0..uz_N] + [ux_0..ux_N, uy_0..uy_N, uz_0..uz_N, global_DOFs...] + + For deformable nodes, the mapping is a permutation (identity values). + For rigid body nodes, the mapping uses the exact contact Jacobian + ``J = [I_3 | dR/d(omega) @ r_ref]`` derived from the RigidTie + rotation derivatives, projecting surface DOFs directly onto the 6 + rigid body global DOFs. Only ``r_ref`` (reference positions) + enters the Jacobian — current positions are not needed. Parameters ---------- @@ -294,31 +412,103 @@ def _build_scatter_matrix(self, surface_node_indices, n_nodes, ndim): Total number of nodes in the full mesh. ndim : int Number of spatial dimensions (2 or 3). + pb : Problem, optional + Problem instance (needed for rigid body global DOF indices). Returns ------- P : sparse csc_matrix - Shape (nvar * n_nodes, n_surf * ndim). + Shape (n_dof, n_surf * ndim) where n_dof includes global DOFs. """ disp_ranks = np.array(self.space.get_rank_vector("Disp")) n_surf = len(surface_node_indices) nvar = self.space.nvar + n_global_dof = pb.n_global_dof if pb is not None else self._n_global_dof + n_dof = nvar * n_nodes + n_global_dof - rows = np.empty(n_surf * ndim, dtype=int) - cols = np.empty(n_surf * ndim, dtype=int) + # Collect sparse entries + all_rows = [] + all_cols = [] + all_data = [] + + # Identify which surface nodes are rigid + if self._rigid_node_set is not None and len(self._rigid_node_set) > 0: + is_rigid = np.isin(surface_node_indices, list(self._rigid_node_set)) + else: + is_rigid = np.zeros(n_surf, dtype=bool) + deformable_mask = ~is_rigid + + # --- Deformable nodes: identity mapping --- + deform_local = np.where(deformable_mask)[0] + if len(deform_local) > 0: + deform_global = surface_node_indices[deform_local] + for d in range(ndim): + all_rows.append(disp_ranks[d] * n_nodes + deform_global) + all_cols.append(deform_local * ndim + d) + all_data.append(np.ones(len(deform_local))) + + # --- Rigid body nodes: contact Jacobian --- + for node_set, rb in self._rigid_bodies: + rt = rb.constraint + dof_indices = rb.assembly._dof_indices # (6,) global DOF positions + + # Find which surface indices belong to this rigid body + mask = np.isin(surface_node_indices, node_set) + local_indices = np.where(mask)[0] + if len(local_indices) == 0: + continue + + n_rb = len(local_indices) + + # Exact rotation derivatives from RigidTie. The Jacobian + # J = [I_3 | dR/d(omega) @ r_ref] uses only r_ref (reference + # positions) — current vertex positions and translation are + # not needed. + if pb is not None: + dof_ref, _ = rt._get_dof_ref(pb) + angles = dof_ref[3:] + else: + angles = np.zeros(3) + + # Translation part: J_i[:3, :3] = I_3 + for d in range(ndim): + all_rows.append(np.full(n_rb, dof_indices[d])) + all_cols.append(local_indices * ndim + d) + all_data.append(np.ones(n_rb)) + + # Rotation part: J_i[d, 3+k] = (dR_drk @ r_ref_i)[d], + # r_ref = vertex_ref - center. + _, du_drx, du_dry, du_drz = rt.rotation_jacobian( + angles, self._rest_positions[local_indices] + ) + + for d in range(ndim): + # RotX contribution + nonzero = np.abs(du_drx[:, d]) > 1e-15 + if np.any(nonzero): + all_rows.append(np.full(nonzero.sum(), dof_indices[3])) + all_cols.append(local_indices[nonzero] * ndim + d) + all_data.append(du_drx[nonzero, d]) + # RotY contribution + nonzero = np.abs(du_dry[:, d]) > 1e-15 + if np.any(nonzero): + all_rows.append(np.full(nonzero.sum(), dof_indices[4])) + all_cols.append(local_indices[nonzero] * ndim + d) + all_data.append(du_dry[nonzero, d]) + # RotZ contribution + nonzero = np.abs(du_drz[:, d]) > 1e-15 + if np.any(nonzero): + all_rows.append(np.full(nonzero.sum(), dof_indices[5])) + all_cols.append(local_indices[nonzero] * ndim + d) + all_data.append(du_drz[nonzero, d]) + + rows = np.concatenate(all_rows) if all_rows else np.array([], dtype=int) + cols = np.concatenate(all_cols) if all_cols else np.array([], dtype=int) + data = np.concatenate(all_data) if all_data else np.array([]) - for d in range(ndim): - start = d * n_surf - end = (d + 1) * n_surf - # fedoo blocked row: disp_ranks[d] * n_nodes + global_node - rows[start:end] = disp_ranks[d] * n_nodes + surface_node_indices - # ipctk interleaved col: local_i * ndim + d - cols[start:end] = np.arange(n_surf) * ndim + d - - data = np.ones(n_surf * ndim) P = sparse.csc_matrix( (data, (rows, cols)), - shape=(nvar * n_nodes, n_surf * ndim), + shape=(n_dof, n_surf * ndim), ) return P @@ -356,7 +546,13 @@ def _get_elastic_gradient_on_surface(self): if not hasattr(assembly, "_list_assembly"): return None - # Sum global vectors from all assemblies except this one + # Sum global vectors from all assemblies except this IPC one. + # RigidBodyAssembly safely returns zeros at init time (its + # assemble_global_mat guards against dt=0 internally), so we + # include it like any other assembly — its contribution at the + # 6 rigid DOFs is exactly the external/contact force balance, + # which is what kappa auto-tune wants to match against the + # barrier gradient. grad_energy_full = None for a in assembly._list_assembly: if a is self: @@ -371,9 +567,27 @@ def _get_elastic_gradient_on_surface(self): if grad_energy_full is None: return None - # Truncate to mesh DOFs (remove global DOFs like PeriodicBC) - n_mesh_dof = self.space.nvar * self.mesh.n_nodes - grad_energy_full = grad_energy_full[:n_mesh_dof] + # When rigid bodies are present, P includes global DOF rows, + # so keep the full vector. Otherwise truncate to mesh DOFs. + if not self._rigid_bodies: + n_mesh_dof = self.space.nvar * self.mesh.n_nodes + grad_energy_full = grad_energy_full[:n_mesh_dof] + + # Assembly vectors that don't span the extra global DOFs (PeriodicBC, + # RigidTie, RigidBody) are shorter than P's row count; zero-padding the + # global-DOF tail is correct since they contribute nothing there. A + # vector LONGER than P's rows means the DOF bookkeeping is inconsistent + # — fail loudly rather than silently truncate. + n_P_rows = self._scatter_matrix.shape[0] + if len(grad_energy_full) > n_P_rows: + raise ValueError( + f"Global vector length {len(grad_energy_full)} exceeds scatter " + f"matrix rows {n_P_rows}; contact DOF bookkeeping is inconsistent." + ) + if len(grad_energy_full) < n_P_rows: + grad_energy_full = np.pad( + grad_energy_full, (0, n_P_rows - len(grad_energy_full)) + ) # Project to surface DOFs: P^T @ full_grad -> surface_grad return self._scatter_matrix.T @ grad_energy_full @@ -467,22 +681,12 @@ def _compute_ipc_contributions(self, vertices, compute="all"): self.global_vector = -self._kappa * (P @ grad_surf) if compute != "vector": - try: - hess_surf = self._barrier_potential.hessian( - self._collisions, - self._collision_mesh, - vertices, - project_hessian_to_psd=ipctk.PSDProjectionMethod.CLAMP, - ) - except RuntimeError: - # CLAMP/ABS projection can fail on degenerate contact - # pairs (zero-distance); skip projection entirely. - hess_surf = self._barrier_potential.hessian( - self._collisions, - self._collision_mesh, - vertices, - project_hessian_to_psd=ipctk.PSDProjectionMethod.NONE, - ) + hess_surf = _barrier_hessian_psd( + self._barrier_potential, + self._collisions, + self._collision_mesh, + vertices, + ) if axi_D is not None: # Axisymmetric IPC uses a single 2*pi*r weight on the residual. # The physically closer single-weight tangent would be: @@ -526,17 +730,10 @@ def _compute_ipc_contributions(self, vertices, compute="all"): fric_hess_surf = axi_D @ fric_hess_surf @ axi_D self.global_matrix += P @ fric_hess_surf @ P.T - # Pad with zeros to account for extra global DOFs (e.g. PeriodicBC) - if self._n_global_dof > 0: - if compute != "matrix": - self.global_vector = np.pad(self.global_vector, (0, self._n_global_dof)) - if compute != "vector": - self.global_matrix = sparse.block_diag( - [ - self.global_matrix, - sparse.csr_array((self._n_global_dof, self._n_global_dof)), - ], - ) + # P already has n_dof rows (mesh DOFs + global DOFs) — see + # _build_scatter_matrix — so P @ hess @ P.T and P @ grad are already + # the right size. No padding is needed for PeriodicBC, RigidTie, or + # registered RigidBody global DOFs. def _ccd_line_search(self, pb, dX): """Compute step size using CCD + energy-based backtracking. @@ -570,10 +767,16 @@ def _ccd_line_search(self, pb, dX): # Current vertices vertices_current = self._get_current_vertices(pb) - # Compute displacement of surface nodes from dX (fedoo blocked layout) + # Surface displacement from dX (fedoo blocked layout). For + # rigid-tied slave nodes, ``_MatCB`` has already written the + # kinematic motion back into the mesh-DOF rows (see + # ``problem.Problem.solve``: ``X = MatCB @ X_free + Xbc``), so we + # can read the surface displacement directly regardless of + # whether a node is deformable or rigid-slaved. disp_ranks = np.array(self.space.get_rank_vector("Disp")) n_nodes = self.mesh.n_nodes - surf_disp = np.zeros((len(self._surface_node_indices), ndim)) + n_surf = len(self._surface_node_indices) + surf_disp = np.zeros((n_surf, ndim)) for d in range(ndim): surf_disp[:, d] = dX[disp_ranks[d] * n_nodes + self._surface_node_indices] @@ -758,14 +961,37 @@ def _ogc_step_filter_callback(self, pb, dX, is_elastic_prediction=False): else: self._ogc_filter_step(pb, dX) + def _refresh_rigid_scatter(self, pb): + """Rebuild the rotation-dependent scatter matrix for rigid bodies. + + The rigid columns of the scatter matrix depend on the current rigid + rotation, so the matrix is refreshed on every NR iteration (from both + :meth:`update` and :meth:`assemble_global_mat`) whenever rigid bodies + are registered. No-op otherwise. + """ + if self._rigid_bodies and pb is not None: + self._scatter_matrix = self._build_scatter_matrix( + self._surface_node_indices, + self.mesh.n_nodes, + self.space.ndim, + pb, + ) + def assemble_global_mat(self, compute="all"): """Recompute barrier contributions from cached vertex positions. Called by the solver when the tangent matrix needs reassembly without a full ``update()`` cycle. Uses vertex positions cached by the most recent ``update()`` or ``set_start()`` call. + + When rigid bodies are registered the scatter matrix depends on the + current rigid rotation; the MPC coefficients have just been + refreshed by ``update_boundary_conditions`` for this NR step, so + refresh the scatter here too to keep the contact tangent + consistent with the kinematic constraint. """ if self._last_vertices is not None: + self._refresh_rigid_scatter(self._pb) self._compute_ipc_contributions(self._last_vertices, compute) def initialize(self, pb): @@ -825,9 +1051,22 @@ def initialize(self, pb): # Rest positions of surface vertices self._rest_positions = self.mesh.nodes[self._surface_node_indices] + # Build rigid body node lookup set + if self._rigid_bodies: + self._rigid_node_set = set() + for node_set, rb in self._rigid_bodies: + self._rigid_node_set.update(node_set.tolist()) + + # OGC is incompatible with rigid bodies: per-vertex trust + # region cannot be reconciled with the 6-DOF rigid Jacobian + # reduction. CCD is supported — _ccd_line_search reconstructs + # rigid surface motion from dX[rigid_global_dofs] via J. + if self._use_ogc: + raise ValueError("use_ogc=True is not supported with rigid bodies.") + # Build scatter matrix for DOF mapping (ipctk surface -> fedoo full) self._scatter_matrix = self._build_scatter_matrix( - self._surface_node_indices, self.mesh.n_nodes, ndim + self._surface_node_indices, self.mesh.n_nodes, ndim, pb ) # Create broad phase instance @@ -1039,6 +1278,10 @@ def update(self, pb, compute="all"): vertices = self._get_current_vertices(pb) self._last_vertices = vertices + # The rigid Jacobian depends on the current rotation, so refresh the + # scatter matrix after every update. + self._refresh_rigid_scatter(pb) + # Rebuild collision set self._collisions.build( self._collision_mesh, diff --git a/fedoo/constraint/rigid_body.py b/fedoo/constraint/rigid_body.py new file mode 100644 index 00000000..2d0520e9 --- /dev/null +++ b/fedoo/constraint/rigid_body.py @@ -0,0 +1,764 @@ +"""Rigid body support for large-displacement dynamic simulation. + +Provides: +- ``RigidBodyAssembly``: AssemblyBase that contributes a 6x6 mass matrix and + a generalized force vector on the rigid body's global DOFs. +- ``RigidBody``: Facade that coordinates a RigidTie constraint (kinematics) + with a RigidBodyAssembly (dynamics) into a single user-facing object. + Optionally handles IPC contact via direct Jacobian projection (J^T @ F). + +Large rotations +--------------- +By default, each step uses an incremental rotation vector composed +multiplicatively with the last converged quaternion orientation. ``RigidTie`` +evaluates the exact rotation matrix and its derivatives, so the kinematics do +not rely on a small-angle approximation. + +The contact Jacobian ``J = [I_3 | dR/d(angle) @ r_ref]`` uses the exact +rotation derivatives from ``RigidTie.rotation_jacobian()``, not the +infinitesimal skew-symmetric approximation. +""" + +import numpy as np + +from fedoo.core.base import AssemblyBase +from fedoo.core.mesh import Mesh +from fedoo.core._sparsematrix import scatter_dense_block +from fedoo.core.time_evolution import SECOND_ORDER +from fedoo.constraint.rigid_tie import RigidTie +from fedoo.time.common import RayleighDamping + +# Rigid-body dynamics is formulated in 3D: 3 translational + 3 rotational DOFs. +_N_RIGID_DOF = 6 + + +class RigidBodyAssembly(AssemblyBase): + """Assembly contributing rigid body mass and generalized forces. + + Injects a 6x6 mass matrix and a 6-component force vector at the global + DOF positions created by a :class:`RigidTie` constraint. + + The mass matrix has the structure:: + + M = [[m*I_3, 0 ], + [ 0, J_global]] + + where ``J_global = R @ J_body @ R.T`` is the inertia tensor rotated + to the current trial configuration. + + Parameters + ---------- + mass : float + Total mass of the rigid body. + inertia_tensor : array_like, shape (3, 3) + Inertia tensor about the center of mass in the body-fixed frame. + rigid_tie : RigidTie + The associated rigid tie constraint. + mesh : fedoo.Mesh, optional + Mesh (needed by the Fedoo Problem constructor). + space : ModelingSpace, optional + Modeling space. + name : str, optional + Name of the assembly. + """ + + def __init__( + self, + mass, + inertia_tensor, + rigid_tie, + mesh=None, + space=None, + dynamic=True, + name="RigidBodyAssembly", + ): + if space is None: + from fedoo.core.modelingspace import ModelingSpace + + space = ModelingSpace.get_active() + AssemblyBase.__init__(self, name, space) + if self.space.ndim != 3: + raise NotImplementedError( + "RigidBodyAssembly supports 3D modeling spaces only " + f"(got ndim={self.space.ndim}). RigidTie2D provides 2D rigid " + "kinematics, but 2D rigid-body dynamics is not yet implemented." + ) + self.mass = float(mass) + self.inertia_body = np.asarray(inertia_tensor, dtype=float) + self.rigid_tie = rigid_tie + self.force = np.zeros(_N_RIGID_DOF) + self.mesh = mesh + self.time_evolution = SECOND_ORDER if dynamic else None + self.storage = self if dynamic else None + self.dissipation = None + self._time_integrator = None + self._fedoo_time_integrated = False + self.dynamic = bool(dynamic) + # A compiled time integrator supplies a mass tangent. Without one, the + # assembly is static and this tiny diagonal keeps the free rigid DOFs + # well posed before contact. + self.static_regularisation = 1e-9 + + self._dof_indices = None + self._pb_ref = None + + self.sv = {} + self.sv_start = {} + + self._ipc_collision_mesh = None + self._ipc_collisions = None + self._ipc_barrier = None + self._ipc_kappa = None + self._ipc_dhat = None + self._ipc_broad_phase = None + self._ipc_rest_positions = None + self._ipc_n_body = 0 + self._ipc_obstacle_nodes = None + self._ipc_obstacle_mesh = None + self._ipc_obstacle_source_id = None + self._contact_force = np.zeros(_N_RIGID_DOF) + self._contact_stiffness = np.zeros((_N_RIGID_DOF, _N_RIGID_DOF)) + + def _register_global_dofs(self, pb): + """Register the rigid kinematic constraint and its global DOFs.""" + if self.rigid_tie not in pb.bc: + pb.bc.add(self.rigid_tie) + + def initialize(self, pb): + """Extract global DOF indices from the problem.""" + rt = self.rigid_tie + # Same global-DOF layout the RigidTie constraint resolves; reuse it + # so the two never drift apart. + self._dof_indices = np.array(rt._get_dof_ref(pb)[1]) + self._pb_ref = pb + if self.mesh is None: + self.mesh = pb.mesh + + if SECOND_ORDER not in getattr(pb, "time_integrators", {}): + self._time_integrator = None + self._fedoo_time_integrated = False + elif self._time_integrator is not None: + self._time_integrator.initialize(self, pb) + + @property + def dof_indices(self): + """Global DOF indices [Fx,Fy,Fz,Mx,My,Mz] in the problem.""" + return self._dof_indices + + def _build_ipc_jacobian(self, rt, angles): + """Build contact Jacobian J mapping 6 rigid DOFs to all vertex DOFs. + + Uses exact rotation derivatives from RigidTie for consistency. + + Returns + ------- + J : ndarray, shape (n_all*3, 6) + Body vertices use exact derivatives; obstacle rows are zero. + """ + _, du_drx, du_dry, du_drz = rt.rotation_jacobian( + angles, self._ipc_rest_positions + ) + + n_body = self._ipc_n_body + n_all = n_body + len(self._ipc_obstacle_nodes) + J = np.zeros((n_all * 3, _N_RIGID_DOF)) + for d in range(3): + J[d : n_body * 3 : 3, d] = 1.0 + J[d : n_body * 3 : 3, 3] = du_drx[:, d] + J[d : n_body * 3 : 3, 4] = du_dry[:, d] + J[d : n_body * 3 : 3, 5] = du_drz[:, d] + return J + + def _ipc_vertices(self, q, rt): + """Compute all vertex positions (body + obstacle) from q.""" + R, _, _, _ = rt._compute_rotation(q[3:]) + r_ref = self._ipc_rest_positions - rt.center + body_verts = r_ref @ R.T + rt.center + q[:3] + return np.vstack([body_verts, self._ipc_obstacle_nodes]) + + def compute_contact(self, q, rt, compute="all"): + """Compute IPC contact force and/or stiffness on 6 rigid DOFs. + + Parameters + ---------- + q : ndarray (6,) + Rigid body DOFs [dx, dy, dz, rx, ry, rz]. + rt : RigidTie + Associated rigid tie constraint. + compute : str + ``"all"`` (force + stiffness), ``"force"`` (skip hessian), + or ``"stiffness"``. + + Returns + ------- + force : ndarray (6,) + stiffness : ndarray (6, 6) + """ + if self._ipc_collision_mesh is None: + self._contact_force[:] = 0 + self._contact_stiffness[:] = 0 + return self._contact_force, self._contact_stiffness + + vertices = self._ipc_vertices(q, rt) + self._ipc_collisions.build( + self._ipc_collision_mesh, + vertices, + self._ipc_dhat, + broad_phase=self._ipc_broad_phase, + ) + + if len(self._ipc_collisions) == 0: + self._contact_force[:] = 0 + self._contact_stiffness[:] = 0 + return self._contact_force, self._contact_stiffness + + from fedoo.constraint.ipc_contact import _barrier_hessian_psd + + J = self._build_ipc_jacobian(rt, q[3:]) + + grad = self._ipc_barrier.gradient( + self._ipc_collisions, self._ipc_collision_mesh, vertices + ) + self._contact_force = -self._ipc_kappa * (J.T @ grad) + + if compute in ("all", "stiffness"): + hess = _barrier_hessian_psd( + self._ipc_barrier, + self._ipc_collisions, + self._ipc_collision_mesh, + vertices, + ) + self._contact_stiffness = self._ipc_kappa * (J.T @ (hess @ J)) + else: + self._contact_stiffness[:] = 0 + + return self._contact_force, self._contact_stiffness + + @property + def time_dof_indices(self): + return self._dof_indices + + def _get_rotation_inertia(self, pb=None): + """Return trial orientation, world inertia, and its DOF derivatives.""" + pb = pb or self._pb_ref + rotvec = np.zeros(3) + if pb is not None and self._dof_indices is not None: + dof_ref, _ = self.rigid_tie._get_dof_ref(pb) + rotvec = np.asarray(dof_ref[3:], dtype=float) + + R, *dR = self.rigid_tie._compute_rotation(rotvec) + inertia = R @ self.inertia_body @ R.T + derivatives = tuple( + derivative @ self.inertia_body @ R.T + R @ self.inertia_body @ derivative.T + for derivative in dR + ) + return R, inertia, derivatives + + def _get_mass_matrix(self, pb=None): + """Return the rigid-body mass matrix in the current global frame.""" + M = np.zeros((_N_RIGID_DOF, _N_RIGID_DOF)) + M[:3, :3] = self.mass * np.eye(3) + _, M[3:, 3:], _ = self._get_rotation_inertia(pb) + return M + + def get_time_inertia_force(self, pb, acceleration, velocity): + """Return translational inertia and Euler's rotational inertia force.""" + acceleration = np.asarray(acceleration, dtype=float) + velocity = np.asarray(velocity, dtype=float) + force = np.zeros(_N_RIGID_DOF) + force[:3] = self.mass * acceleration[:3] + + _, inertia, _ = self._get_rotation_inertia(pb) + angular_acceleration = acceleration[3:] + angular_velocity = velocity[3:] + force[3:] = inertia @ angular_acceleration + if self.rigid_tie.use_quaternion: + force[3:] += np.cross(angular_velocity, inertia @ angular_velocity) + return force + + def get_time_inertia_tangent( + self, + pb, + acceleration, + velocity, + acceleration_factor, + velocity_factor, + ): + """Return the effective tangent of the rigid-body inertia force.""" + acceleration = np.asarray(acceleration, dtype=float) + velocity = np.asarray(velocity, dtype=float) + tangent = np.zeros((_N_RIGID_DOF, _N_RIGID_DOF)) + tangent[:3, :3] = acceleration_factor * self.mass * np.eye(3) + + _, inertia, inertia_derivatives = self._get_rotation_inertia(pb) + if not self.rigid_tie.use_quaternion: + tangent[3:, 3:] = acceleration_factor * inertia + return tangent + + angular_acceleration = acceleration[3:] + angular_velocity = velocity[3:] + angular_momentum = inertia @ angular_velocity + basis = np.eye(3) + for axis, inertia_derivative in enumerate(inertia_derivatives): + d_acceleration = acceleration_factor * basis[axis] + d_velocity = velocity_factor * basis[axis] + tangent[3:, 3 + axis] = ( + inertia_derivative @ angular_acceleration + + inertia @ d_acceleration + + np.cross(d_velocity, angular_momentum) + + np.cross( + angular_velocity, + inertia_derivative @ angular_velocity + inertia @ d_velocity, + ) + ) + return tangent + + def get_storage_matrix(self, pb=None): + """Assembly-level storage provider used by fedoo.time.""" + return self._get_mass_matrix(pb) + + def get_time_initial_force(self, pb=None): + return self.force + self._contact_force + + def get_time_stiffness_matrix(self, pb=None): + """Return the static/contact tangent before time integration.""" + return self._contact_stiffness + + def _get_n_dof(self): + if self._pb_ref is not None: + return self._pb_ref.n_dof + return max( + self.mesh.n_nodes * self.space.nvar, + int(self._dof_indices.max()) + 1, + ) + + def assemble_global_mat(self, compute="all"): + """Assemble static/contact terms, then apply the attached integrator.""" + if self._dof_indices is None: + return + + n = self._get_n_dof() + idx = self._dof_indices + regularisation = ( + self.static_regularisation if self._time_integrator is None else 0.0 + ) + + if compute in ("all", "matrix"): + stiffness = self._contact_stiffness + regularisation * np.eye(_N_RIGID_DOF) + self.global_matrix = scatter_dense_block(stiffness, idx, (n, n)) + if compute in ("all", "vector"): + self.global_vector = np.zeros(n) + self.global_vector[idx] = self.force + self._contact_force + + if self._time_integrator is not None: + self._time_integrator.integrate(self, self._pb_ref, compute) + + def update(self, pb, compute="all"): + self._pb_ref = pb + if self._time_integrator is not None: + self._time_integrator.update(self, pb) + + dof_solution = pb.get_dof_solution() + if self._ipc_collision_mesh is not None: + q = ( + np.asarray(dof_solution)[self._dof_indices] + if not np.isscalar(dof_solution) + else np.zeros(_N_RIGID_DOF) + ) + self.compute_contact(q, self.rigid_tie, compute="all") + + self.assemble_global_mat(compute) + + def set_start(self, pb): + self._pb_ref = pb + if self._time_integrator is not None: + self._time_integrator.set_start(self, pb) + + def to_start(self, pb): + if self._time_integrator is not None: + self._time_integrator.to_start(self, pb) + + +class RigidBody: + """Rigid body for large-displacement dynamic simulation in Fedoo. + + Coordinates a :class:`RigidTie` constraint (kinematics: MPC coupling + of surface nodes to 6 rigid DOFs) with a :class:`RigidBodyAssembly` + (dynamics: 6x6 mass matrix and generalized force vector). + + Supports large translations and multiplicative incremental rotations. + IPC barrier contact is handled via direct Jacobian + projection ``J^T @ F`` in the 6-DOF space. + + Solve via ``body.solve(dt, tmax)`` (wraps ``NonLinear``) or include + ``body.assembly`` in an ``Assembly.sum`` — the kinematic tie is then + registered automatically when the problem is constructed. Attach a + second-order integrator to manually constructed dynamic problems with + ``pb.set_time_integrator(fd.time.SECOND_ORDER, fd.time.Newmark())``. + + Parameters + ---------- + mesh : fedoo.Mesh + Mesh of the rigid body. For a standalone body, this is its own + mesh (tri3 surface for IPC contact, or volume for inertia + integration). For a body that is one part of a larger stacked + problem mesh (rigid-vs-deformable IPC), pass the output of + :meth:`Mesh.extract_elements` — the carried ``parent_node_indices`` + attribute tells RigidTie which DOFs to slave in the parent. + mass : float, optional + Total mass. Required if ``density`` is not given. + density : float, optional + Material density. Computes mass and inertia from mesh. + inertia_tensor : array_like, shape (3, 3), optional + Inertia tensor about center of mass in body frame. + center_of_mass : array_like, shape (3,), optional + Center of mass. Default: mean of mesh nodes. + use_quaternion : bool, optional + If ``True`` (default), compose incremental rotation vectors into a + quaternion orientation and include Euler's gyroscopic inertia term. + If ``False``, use one total rotation vector; this mode is primarily + retained for kinematic comparisons. + name : str, optional + Name of the rigid body. + + Example + ------- + .. code-block:: python + + import fedoo as fd + import numpy as np + + fd.ModelingSpace("3D") + sphere_mesh = fd.Mesh.from_pyvista(pv.Sphere(radius=0.1)) + + body = fd.constraint.RigidBody(sphere_mesh, mass=1.0, + inertia_tensor=0.004*np.eye(3)) + body.set_force([0, 0, -9.81]) + body.set_rayleigh_damping(1.0) + body.set_static_obstacle(plane_mesh, dhat=0.01, kappa=1e8) + + # solve() returns the underlying NonLinear problem; read the rigid + # DOFs from it via the assembly's _dof_indices. + pb = body.solve(dt=5e-4, tmax=2.0) + q = pb.get_dof_solution()[body.assembly._dof_indices] + """ + + def __init__( + self, + mesh, + mass=None, + density=None, + inertia_tensor=None, + center_of_mass=None, + use_quaternion=True, + dynamic=True, + name="RigidBody", + ): + self.mesh = mesh + self.name = name + self.dynamic = bool(dynamic) + + if center_of_mass is None: + # Use only nodes actually referenced by elements — protects + # the default against ``extract_elements`` submeshes that + # carry the parent's full node array. + active = np.unique(mesh.elements.ravel()) + center_of_mass = mesh.nodes[active].mean(axis=0) + center_of_mass = np.asarray(center_of_mass, dtype=float) + + if self.dynamic: + # Mass and inertia are consumed by the problem's second-order + # time integrator. Static mode allows callers to omit them. + if density is not None: + vol = mesh.get_volume() + if mass is None: + mass = density * vol + if inertia_tensor is None: + inertia_tensor = self._compute_inertia( + mesh, density, center_of_mass + ) + else: + if mass is None: + raise ValueError("Either mass or density must be provided.") + if inertia_tensor is None: + raise ValueError( + "inertia_tensor required when density is not given." + ) + else: + mass = 0.0 if mass is None else mass + inertia_tensor = ( + np.zeros((3, 3)) if inertia_tensor is None else inertia_tensor + ) + + self.mass = float(mass) + self.center_of_mass = center_of_mass + self.inertia_tensor = np.asarray(inertia_tensor, dtype=float) + if self.inertia_tensor.shape != (3, 3): + raise ValueError("inertia_tensor must have shape (3, 3).") + if not np.allclose(self.inertia_tensor, self.inertia_tensor.T): + raise ValueError("inertia_tensor must be symmetric.") + self.use_quaternion = bool(use_quaternion) + + # When ``mesh`` came from ``Mesh.extract_elements`` it carries + # the parent-mesh active-node list used to slave the right DOFs + # in a stacked rigid-plus-deformable problem. A standalone + # body's mesh has no such attribute — every node is tied. + tie_node_indices = getattr(mesh, "parent_node_indices", None) + if tie_node_indices is None: + tie_node_indices = np.arange(mesh.n_nodes) + + self.constraint = RigidTie( + tie_node_indices, + center=center_of_mass, + use_quaternion=self.use_quaternion, + name=f"{name}_tie", + ) + self.assembly = RigidBodyAssembly( + self.mass, + self.inertia_tensor, + self.constraint, + mesh=mesh, + dynamic=self.dynamic, + name=f"{name}_asm", + ) + + def set_force(self, force): + """Set external force [Fx, Fy, Fz] on translational DOFs.""" + self.assembly.force[:3] = force + + def set_torque(self, torque): + """Set external torque [Mx, My, Mz] on rotational DOFs.""" + self.assembly.force[3:] = torque + + def set_generalized_force(self, f): + """Set full generalized force [Fx, Fy, Fz, Mx, My, Mz].""" + self.assembly.force[:] = f + + def set_rayleigh_damping(self, alpha=0.0, beta=0.0): + """Set Rayleigh damping: ``C = alpha*M + beta*K``. + + A rigid body has no elastic stiffness. With the private contact made + by :meth:`set_static_obstacle`, ``K`` is that contact tangent. With a + shared :class:`IPCContact`, the contact tangent belongs to the IPC + assembly instead and the rigid-body ``beta`` contribution is zero. + """ + self.assembly.dissipation = RayleighDamping( + alpha=float(alpha), beta=float(beta) + ) + + def set_static_obstacle(self, obstacle_mesh, dhat=0.01, kappa=None): + """Enable IPC barrier contact with a STATIC obstacle surface. + + Builds a private collision mesh and barrier on this rigid body's + :class:`RigidBodyAssembly`. The obstacle is snapshotted once at + setup and treated as frozen geometry — use this for rigid-vs-static + scenarios (e.g., a rigid body falling onto a fixed floor). + + For rigid-vs-deformable contact (e.g., a punch crushing an elastic + disc), build a shared :class:`IPCContact` over the union of all + surfaces instead. Calling ``ipc.add_rigid_body(body)`` optionally + projects contact directly onto the rigid DOFs; otherwise the same + projection is performed by ``RigidTie`` MPC condensation. In both + cases the deformable obstacle's vertex positions are tracked at every + NR iteration and its reaction is assembled on the mesh DOFs. + + Parameters + ---------- + obstacle_mesh : fedoo.Mesh + Surface mesh of the obstacle (tri3), treated as static. + dhat : float + Barrier activation distance (meters). + kappa : float or None + Barrier stiffness. If None (default), automatically tuned at + first contact to balance external forces and barrier gradient. + """ + from fedoo.constraint.ipc_contact import _import_ipctk + + ipctk = _import_ipctk() + + body_nodes = self.mesh.nodes + obst_nodes = obstacle_mesh.nodes + n_body = len(body_nodes) + + if self.mesh.elements.shape[1] != 3: + raise ValueError("Body mesh must be tri3 for IPC contact.") + if obstacle_mesh.elements.shape[1] != 3: + raise ValueError("Obstacle mesh must be tri3 for IPC contact.") + + all_nodes = np.vstack([body_nodes, obst_nodes]) + all_elms = np.vstack([self.mesh.elements, obstacle_mesh.elements + n_body]) + + edges = ipctk.edges(all_elms) + cm = ipctk.CollisionMesh(all_nodes, edges, all_elms) + + # Only body-obstacle collisions (no self-contact within either mesh): + # the filter blocks same-patch pairs, so body (0) and obstacle (1) only. + patches = np.zeros(len(all_nodes), dtype=np.int32) + patches[n_body:] = 1 + cm.can_collide = ipctk.make_vertex_patches_filter(patches) + + asm = self.assembly + asm._ipc_collision_mesh = cm + asm._ipc_collisions = ipctk.NormalCollisions() + # Fedoo applies the contact stiffness separately through _ipc_kappa. + asm._ipc_barrier = ipctk.BarrierPotential(dhat, 1.0) + if kappa is None: + kappa = 1e9 + asm._ipc_kappa = kappa + asm._ipc_dhat = dhat + asm._ipc_broad_phase = ipctk.LBVH() + asm._ipc_rest_positions = body_nodes.copy() + asm._ipc_n_body = n_body + asm._ipc_obstacle_nodes = obst_nodes.copy() + # Keep the same frozen geometry for optional result export. The source + # identity lets shared obstacles be deduplicated across rigid bodies. + asm._ipc_obstacle_mesh = Mesh( + obst_nodes.copy(), + obstacle_mesh.elements.copy(), + obstacle_mesh.elm_type, + node_sets=obstacle_mesh.node_sets, + element_sets=obstacle_mesh.element_sets, + ndim=obstacle_mesh.ndim, + name=obstacle_mesh.name, + register_name=False, + ) + asm._ipc_obstacle_source_id = id(obstacle_mesh) + + def add_to_problem(self, pb): + """Register the rigid body's kinematic tie with a Fedoo problem. + + Usually unnecessary — :class:`NonLinear` auto-registers ties from + any :class:`RigidBodyAssembly` it discovers in its assembly sum. + Kept as an explicit hook for advanced flows (custom problem + classes, late attachment) and made idempotent so calling it + after auto-registration is a no-op. + """ + if self.constraint not in pb.bc: + pb.bc.add(self.constraint) + + def solve(self, dt, tmax, t0=0, print_info=1, solver=None): + """Solve rigid body dynamics using Fedoo's NonLinear solver. + + Creates an internal ``NonLinear`` problem, attaches + :class:`fedoo.time.Newmark` for a dynamic body, and calls ``nlsolve``. + IPC contact and Rayleigh damping remain assembly-level providers and + are integrated by the problem's time integrator. + + Parameters + ---------- + dt : float + Time step. + tmax : float + End time. + t0 : float + Start time. + print_info : int + Verbosity (0=silent, 1=iterations). + solver : str or callable, optional + Linear solver forwarded to :meth:`Problem.set_solver`. + + Returns + ------- + pb : NonLinear + The solved problem (access DOFs via ``pb.get_dof_solution()``). + """ + from fedoo.problem.non_linear import NonLinear + + pb = NonLinear(self.assembly) + if self.dynamic: + from fedoo.time import Newmark + + pb.set_time_integrator(SECOND_ORDER, Newmark()) + if solver is not None: + pb.set_solver(solver) + pb.nlsolve(dt=dt, tmax=tmax, t0=t0, print_info=print_info, update_dt=False) + return pb + + @staticmethod + def _compute_inertia(mesh, density, center_of_mass): + """Compute inertia tensor from a closed triangulated surface mesh. + + Uses the divergence theorem (Mirtich 1996). + """ + nodes = mesh.nodes - center_of_mass + elements = mesh.elements + + if elements.shape[1] != 3: + raise ValueError( + "Automatic inertia computation requires tri3 elements. " + f"Got elements with {elements.shape[1]} nodes." + ) + + v0 = nodes[elements[:, 0]] + v1 = nodes[elements[:, 1]] + v2 = nodes[elements[:, 2]] + + normals = np.cross(v1 - v0, v2 - v0) + + x2 = ( + v0[:, 0] ** 2 + + v1[:, 0] ** 2 + + v2[:, 0] ** 2 + + v0[:, 0] * v1[:, 0] + + v0[:, 0] * v2[:, 0] + + v1[:, 0] * v2[:, 0] + ) + y2 = ( + v0[:, 1] ** 2 + + v1[:, 1] ** 2 + + v2[:, 1] ** 2 + + v0[:, 1] * v1[:, 1] + + v0[:, 1] * v2[:, 1] + + v1[:, 1] * v2[:, 1] + ) + z2 = ( + v0[:, 2] ** 2 + + v1[:, 2] ** 2 + + v2[:, 2] ** 2 + + v0[:, 2] * v1[:, 2] + + v0[:, 2] * v2[:, 2] + + v1[:, 2] * v2[:, 2] + ) + + xy = ( + 2 * (v0[:, 0] * v0[:, 1] + v1[:, 0] * v1[:, 1] + v2[:, 0] * v2[:, 1]) + + v0[:, 0] * v1[:, 1] + + v0[:, 1] * v1[:, 0] + + v0[:, 0] * v2[:, 1] + + v0[:, 1] * v2[:, 0] + + v1[:, 0] * v2[:, 1] + + v1[:, 1] * v2[:, 0] + ) + xz = ( + 2 * (v0[:, 0] * v0[:, 2] + v1[:, 0] * v1[:, 2] + v2[:, 0] * v2[:, 2]) + + v0[:, 0] * v1[:, 2] + + v0[:, 2] * v1[:, 0] + + v0[:, 0] * v2[:, 2] + + v0[:, 2] * v2[:, 0] + + v1[:, 0] * v2[:, 2] + + v1[:, 2] * v2[:, 0] + ) + yz = ( + 2 * (v0[:, 1] * v0[:, 2] + v1[:, 1] * v1[:, 2] + v2[:, 1] * v2[:, 2]) + + v0[:, 1] * v1[:, 2] + + v0[:, 2] * v1[:, 1] + + v0[:, 1] * v2[:, 2] + + v0[:, 2] * v2[:, 1] + + v1[:, 1] * v2[:, 2] + + v1[:, 2] * v2[:, 1] + ) + + Ixx = np.sum(normals[:, 0] * x2) * density / 60.0 + Iyy = np.sum(normals[:, 1] * y2) * density / 60.0 + Izz = np.sum(normals[:, 2] * z2) * density / 60.0 + Ixy = np.sum(normals[:, 0] * xy) * density / 120.0 + Ixz = np.sum(normals[:, 0] * xz) * density / 120.0 + Iyz = np.sum(normals[:, 1] * yz) * density / 120.0 + + return np.array( + [ + [Iyy + Izz, -Ixy, -Ixz], + [-Ixy, Ixx + Izz, -Iyz], + [-Ixz, -Iyz, Ixx + Iyy], + ] + ) diff --git a/fedoo/constraint/rigid_tie.py b/fedoo/constraint/rigid_tie.py index 7d3cea70..2683ba97 100644 --- a/fedoo/constraint/rigid_tie.py +++ b/fedoo/constraint/rigid_tie.py @@ -8,6 +8,45 @@ from simcoon import dR_drotvec +def _resolve_rigid_dofs(problem, var_cd, node_cd, n_dof): + """Return the ``n_dof`` rigid DOF values and their global indices. + + Shared by :class:`RigidTie` (6 DOF) and :class:`RigidTie2D` (3 DOF). The + values fall back to zeros before the problem holds a solution. + """ + dof_cd = [ + problem.n_node_dof + problem._global_dof.indice_start(var_cd[i]) + node_cd[i] + for i in range(n_dof) + ] + dof_sol = problem.get_dof_solution() + xbc = problem._Xbc + + if np.isscalar(dof_sol) and dof_sol == 0: + if np.isscalar(xbc) and xbc == 0: + return np.zeros(n_dof), dof_cd + return np.asarray(xbc)[dof_cd], dof_cd + if np.isscalar(xbc) and xbc == 0: + return np.asarray(dof_sol)[dof_cd], dof_cd + return np.asarray(dof_sol)[dof_cd] + np.asarray(xbc)[dof_cd], dof_cd + + +def _apply_slave_disp(problem, list_nodes, center, disp_ref, rotation, disp_indices): + """Write rigid slave-node displacements into ``problem._dU``. + + Shared by :class:`RigidTie` (3D rotation matrix) and :class:`RigidTie2D` + (2D rotation matrix); ``rotation`` and ``center`` carry the dimension. + Returns the computed slave displacement. + """ + nodes = problem.mesh.nodes[list_nodes] + new_disp = (nodes - center) @ rotation.T + center + disp_ref - nodes + if not np.array_equal(problem._dU, 0): + if np.array_equal(problem._U, 0): + problem._dU[disp_indices] = new_disp + else: + problem._dU[disp_indices] = new_disp - problem._U[disp_indices] + return new_disp + + class RigidTie(BCBase): """Constraint that eliminate dof assuming a rigid body tie between nodes. @@ -39,6 +78,12 @@ class RigidTie(BCBase): of the corresponding node in the mesh. If center is an array (or list, ...) it contains the initial coordinates of the rotation center. By default, the center is set at the midle of the rigid sets. + use_quaternion : bool, optional + If ``True`` (default), the rotational DOFs describe an incremental + rotation vector relative to the last converged orientation. The + increment is composed multiplicatively and the converged orientation + is stored as a quaternion. If ``False``, the DOFs are interpreted as + one total rotation vector. name : str, optional Name of the created boundary condition. The default is "Rigid Tie". @@ -47,7 +92,10 @@ class RigidTie(BCBase): ----------------------- ``RigidRotX``, ``RigidRotY`` and ``RigidRotZ`` use a rotation-vector convention. The vector direction defines the rotation axis and its norm is - the rotation angle. + the rotation angle. With ``use_quaternion=True``, this rotation vector is + the current increment and is composed with the last converged orientation; + the quaternion is an orientation storage format, not a different DOF + convention. Notes @@ -76,9 +124,10 @@ class RigidTie(BCBase): rigid_tie = fd.constraint.RigidTie(right_face) """ - def __init__(self, list_nodes, center=None, name="Rigid Tie"): + def __init__(self, list_nodes, center=None, use_quaternion=True, name="Rigid Tie"): self.list_nodes = list_nodes self.center = center + self.use_quaternion = bool(use_quaternion) self.bc_type = "RigidTie" BCBase.__init__(self, name) self._keep_at_end = True @@ -117,16 +166,6 @@ def _register_global_dofs(self, problem): ] def initialize(self, problem): - if problem.space.is_axisymmetric: - warnings.warn( - "RigidTie under the '2Daxi' ModelingSpace: the constraint " - "is applied as pure-kinematics MPCs and will function, but " - "only rigid motions consistent with axisymmetry (axial " - "translation along the symmetry axis) preserve the " - "axisymmetric assumption. Radial translation and any " - "rotation around the in-plane axes break it.", - stacklevel=2, - ) if self.center is None: # initialize the rotation center at center of rigid nodes bounding box nodes_crd = problem.mesh.nodes[self.list_nodes] @@ -146,53 +185,105 @@ def initialize(self, problem): + self.list_nodes[:, None] ) + if self.use_quaternion: + self._Q_base = Rotation.identity() + self._angles_at_base = np.zeros(3) + + def _get_dof_ref(self, problem): + """Return the current six rigid-body DOFs and their global indices.""" + return _resolve_rigid_dofs(problem, self.var_cd, self.node_cd, 6) + + def _compute_rotation(self, rotvec): + """Return the orientation and exact derivatives for the active mode.""" + rotvec = np.asarray(rotvec, dtype=float) + if self.use_quaternion and hasattr(self, "_Q_base"): + increment = rotvec - self._angles_at_base + if not np.all(np.isfinite(increment)): + increment = np.zeros(3) + R_base = self._Q_base.as_matrix() + else: + increment = rotvec + R_base = np.eye(3) + + R_increment = Rotation.from_rotvec(increment).as_matrix() + derivatives = dR_drotvec(increment) + R = R_increment @ R_base + return ( + R, + derivatives[:, :, 0] @ R_base, + derivatives[:, :, 1] @ R_base, + derivatives[:, :, 2] @ R_base, + ) + + def _compute_slave_disp(self, problem, disp_ref, R): + """Update slave-node displacements for a given rigid motion.""" + return _apply_slave_disp( + problem, self.list_nodes, self.center, disp_ref, R, self._disp_indices + ) + + def rotation_jacobian(self, rotvec, points): + """Rotation matrix and per-point displacement/rotation derivatives. + + For each point, ``du_dr{k} = (point - center) @ (dR/dr{k}).T`` is the + sensitivity of the slaved displacement to rigid rotation DOF ``k``. + Shared by the MPC linearization (:meth:`generate`) and the IPC contact + Jacobians (``RigidBodyAssembly`` and ``IPCContact``) so the three stay + consistent. + + Returns ``(R, du_drx, du_dry, du_drz)`` where each derivative array has + the same shape as ``points``. + """ + R, dR_drx, dR_dry, dR_drz = self._compute_rotation(rotvec) + rel = np.asarray(points, dtype=float) - self.center + return R, rel @ dR_drx.T, rel @ dR_dry.T, rel @ dR_drz.T + + def pre_update(self, problem): + """Refresh rigid slave positions before contact assemblies update.""" + dof_ref, _ = self._get_dof_ref(problem) + R, _, _, _ = self._compute_rotation(dof_ref[3:]) + self._compute_slave_disp(problem, dof_ref[:3], R) + + def set_start(self, problem): + """Commit a converged incremental rotation to the quaternion state.""" + if not self.use_quaternion or not hasattr(self, "_Q_base"): + return + dof_ref, _ = self._get_dof_ref(problem) + angles = np.asarray(dof_ref[3:], dtype=float) + if not np.all(np.isfinite(angles)): + return + increment = angles - self._angles_at_base + if not np.allclose(increment, 0.0, atol=1e-15): + self._Q_base = Rotation.from_rotvec(increment) * self._Q_base + self._angles_at_base = angles.copy() + + def to_start(self, problem): + """Leave the last converged quaternion unchanged after a failed step.""" + return + + @property + def Q_total(self): + """Last converged quaternion orientation, or ``None`` in total mode.""" + if self.use_quaternion and hasattr(self, "_Q_base"): + return self._Q_base + return None + def generate(self, problem, t_fact=1, t_fact_old=None): mesh = problem.mesh var_cd = self.var_cd node_cd = self.node_cd list_nodes = self.list_nodes - dof_cd = [ - problem.n_node_dof - + problem._global_dof.indice_start(var_cd[i]) - + node_cd[i] - for i in range(len(var_cd)) - ] - - if np.isscalar(problem.get_dof_solution()) and problem.get_dof_solution() == 0: - dof_ref = np.array([problem._Xbc[dof] for dof in dof_cd]) - else: - dof_ref = np.array( - [problem.get_dof_solution()[dof] + problem._Xbc[dof] for dof in dof_cd] - ) + dof_ref, dof_cd = self._get_dof_ref(problem) disp_ref = dof_ref[:3] # reference displacement rotvec = dof_ref[3:] - R = Rotation.from_rotvec(rotvec).as_matrix() - - # Correct displacement of slave nodes to be consistent with the master nodes - new_disp = ( - (mesh.nodes[list_nodes] - self.center) @ R.T - + self.center - + disp_ref - - mesh.nodes[list_nodes] + # approche incrémentale + R, du_drx, du_dry, du_drz = self.rotation_jacobian( + rotvec, mesh.nodes[list_nodes] ) - if not (np.array_equal(problem._dU, 0)): - if np.array_equal(problem._U, 0): - problem._dU[self._disp_indices] = new_disp - else: - problem._dU[self._disp_indices] = ( - new_disp - problem._U[self._disp_indices] - ) - - # approche incrémentale: - dR = dR_drotvec(rotvec) - crd = mesh.nodes[list_nodes] - self.center - du_drx = crd @ dR[:, :, 0].T - du_dry = crd @ dR[:, :, 1].T - du_drz = crd @ dR[:, :, 2].T + self._compute_slave_disp(problem, disp_ref, R) #### MPC #### @@ -313,6 +404,8 @@ def initialize(self, problem): elif np.isscalar(self.center): # initialize the center at a position of a node self.center = problem.mesh.nodes[self.center] + else: + self.center = np.asarray(self.center) # extract indices array that gives the disp from the full dof solution n_nodes = problem.mesh.n_nodes rank = problem.space.variable_rank("DispX") @@ -321,58 +414,53 @@ def initialize(self, problem): np.c_[rank * n_nodes, (rank + 1) * n_nodes] + self.list_nodes[:, None] ) + def _get_dof_ref(self, problem): + """Return [dx, dy, rotZ] and their global DOF indices.""" + return _resolve_rigid_dofs(problem, self.var_cd, self.node_cd, 3) + + @staticmethod + def _compute_rotation(angle): + """Return the 2D rotation matrix and its angle derivative.""" + sin = np.sin(angle) + cos = np.cos(angle) + rotation = np.array([[cos, -sin], [sin, cos]]) + derivative = np.array([[-sin, -cos], [cos, -sin]]) + return rotation, derivative + + def _compute_slave_disp(self, problem, disp_ref, rotation): + """Update 2D slave-node displacements for a rigid motion.""" + return _apply_slave_disp( + problem, + self.list_nodes, + self.center, + disp_ref, + rotation, + self._disp_indices, + ) + + def pre_update(self, problem): + """Refresh rigid slave positions before contact assemblies update.""" + dof_ref, _ = self._get_dof_ref(problem) + rotation, _ = self._compute_rotation(dof_ref[2]) + self._compute_slave_disp(problem, dof_ref[:2], rotation) + def generate(self, problem, t_fact=1, t_fact_old=None): - mesh = problem.mesh var_cd = self.var_cd node_cd = self.node_cd list_nodes = self.list_nodes - dof_cd = [ - problem.n_node_dof - + problem._global_dof.indice_start(var_cd[i]) - + node_cd[i] - for i in range(len(var_cd)) - ] - - if np.isscalar(problem.get_dof_solution()) and problem.get_dof_solution() == 0: - dof_ref = np.array([problem._Xbc[dof] for dof in dof_cd]) - else: - dof_ref = np.array( - [problem.get_dof_solution()[dof] + problem._Xbc[dof] for dof in dof_cd] - ) + dof_ref, _ = self._get_dof_ref(problem) disp_ref = dof_ref[:2] # reference displacement - angles = dof_ref[2] # rotation Z angle - - sin = np.sin(angles) - cos = np.cos(angles) - - # Correct displacement of slave nodes to be consistent with the master nodes - R = np.array([[cos, -sin], [sin, cos]]) - - new_disp = ( - (mesh.nodes[list_nodes] - self.center) @ R.T - + self.center - + disp_ref - - mesh.nodes[list_nodes] - ) - - if not (np.array_equal(problem._dU, 0)): - if np.array_equal(problem._U, 0): - problem._dU[self._disp_indices] = new_disp - else: - problem._dU[self._disp_indices] = ( - new_disp - problem._U[self._disp_indices] - ) - + rotation, dR_drz = self._compute_rotation(dof_ref[2]) + # Correct displacement of slave nodes to be consistent with the masters + self._compute_slave_disp(problem, disp_ref, rotation) # approche incrémentale: - dR_drz = np.array([[-sin, -cos], [cos, -sin]]) - crd = mesh.nodes[list_nodes, :2] - self.center + # MPC linearization + crd = problem.mesh.nodes[list_nodes, :2] - self.center - du_drz = ( - crd @ dR_drz.T - ) # shape = (nnodes, nvar) with nvar = 3 in 3d (ux, uy, uz) + du_drz = crd @ dR_drz.T # shape = (nnodes, 2) #### MPC #### diff --git a/fedoo/core/_sparsematrix.py b/fedoo/core/_sparsematrix.py index ea4a4b30..86cc0138 100644 --- a/fedoo/core/_sparsematrix.py +++ b/fedoo/core/_sparsematrix.py @@ -541,3 +541,20 @@ def RowBlocMatrix(listBloc, nb_bloc, position, coef, n_cols=None): # return sum([bloc_matrix(listBloc[ii], (1,nb_bloc), (0,position[ii])) if coef[ii] == 1 \ # else coef[ii]*bloc_matrix(listBloc[ii], (1,nb_bloc), (0,position[ii])) for ii in range(len(listBloc))]) # + + +def scatter_dense_block(block, dof_indices, shape): + """Scatter a dense ``(k, k)`` block onto a global sparse matrix. + + ``dof_indices`` (length ``k``) gives the global row/column position of each + local DOF: entry ``block[i, j]`` lands at + ``(dof_indices[i], dof_indices[j])``. Returns a + :class:`scipy.sparse.csr_matrix` of the requested ``shape``. + """ + idx = np.asarray(dof_indices, dtype=int) + k = len(idx) + rows = np.repeat(idx, k) + cols = np.tile(idx, k) + return sparse.csr_matrix( + (np.asarray(block, dtype=float).ravel(), (rows, cols)), shape=shape + ) diff --git a/fedoo/core/dataset.py b/fedoo/core/dataset.py index 921f424c..4c9b5f3e 100644 --- a/fedoo/core/dataset.py +++ b/fedoo/core/dataset.py @@ -549,6 +549,11 @@ def plot( Only used with MultiMesh objects. Restrict the plot to the selected submesh ids, submesh names or element types. + missing_field_color : color, optional + Only used with MultiMesh objects. Render submeshes on which the + selected element or Gauss-point field is absent with this solid + color. By default, submeshes missing the field are not plotted. + clip_args : dict, optional Dictionary of arguments to pass to the pyvista clip filter in order to clip the current plot. @@ -999,6 +1004,29 @@ def _submesh_dataset(self, submesh_id: int) -> "DataSet": } return dataset + def _submesh_has_field(self, field, submesh_id: int) -> bool: + """Return whether a field has data on one MultiMesh submesh.""" + if field in self.node_data: + used_nodes = np.unique(self.mesh[submesh_id].elements) + values = np.asarray(self.node_data[field])[..., used_nodes] + try: + return not np.all(np.isnan(values)) + except TypeError: + return True + for field_dict in (self.element_data, self.gausspoint_data): + if field in field_dict: + submesh = self.mesh[submesh_id] + return ( + self._multimesh_block_for_submesh( + field_dict[field], + submesh_id, + submesh.n_elements, + fill_missing=False, + ) + is not None + ) + return False + def _submesh_element_set( self, element_set, @@ -1066,6 +1094,8 @@ def _plot_multimesh( ): """Plot a MultiMesh by reusing the single-mesh plotting path.""" global_element_set = kargs.pop("global_element_set", False) + missing_field_color = kargs.pop("missing_field_color", None) + default_solid_color = kargs.pop("color", "lightgray") submesh_indices = self._resolve_submesh_indices(selected_submeshes) if ( element_set is not None @@ -1092,6 +1122,7 @@ def _plot_multimesh( base_name = kargs.pop("name", None) scalar_bar_added = False plotted = False + scalar_plotted = False for submesh_id in submesh_indices: local_element_set, should_plot = self._submesh_element_set( @@ -1105,15 +1136,28 @@ def _plot_multimesh( continue subdataset = self._submesh_dataset(submesh_id) - if field is not None: + field_available = field is None or self._submesh_has_field( + field, submesh_id + ) + if field is not None and not field_available: + if missing_field_color is None: + continue + submesh_field = None + submesh_color = missing_field_color + else: + submesh_field = field + submesh_color = default_solid_color + if submesh_field is not None: try: - subdataset.get_data(field, component, data_type) + subdataset.get_data(submesh_field, component, data_type) except NameError: continue sub_kargs = dict(kargs) + if submesh_field is None: + sub_kargs["color"] = submesh_color if base_name is not None: sub_kargs["name"] = f"{base_name}_{submesh_id}" - if field is not None and scalar_bar_added: + if submesh_field is not None and scalar_bar_added: sub_kargs["show_scalar_bar"] = False sub_kargs["_extra_cell_data"] = { "_fedoo_global_cell_ids": ( @@ -1132,7 +1176,7 @@ def _plot_multimesh( sub_clip_args = None pl = subdataset.plot( - field=field, + field=submesh_field, component=component, data_type=data_type, scale=scale, @@ -1159,14 +1203,15 @@ def _plot_multimesh( **sub_kargs, ) plotted = True - if field is not None: + if submesh_field is not None: scalar_bar_added = True + scalar_plotted = True if pl is None: pl = pv.Plotter(window_size=window_size) backgroundplotter = False - if field is not None and not plotted: + if field is not None and not scalar_plotted: raise NameError(f"Field data {field!r} not found on any selected submesh.") if title is None: diff --git a/fedoo/core/mesh.py b/fedoo/core/mesh.py index 04f60247..4782a7c2 100644 --- a/fedoo/core/mesh.py +++ b/fedoo/core/mesh.py @@ -485,6 +485,10 @@ def find_coincident_nodes(self, tol: float = 1e-8) -> np.ndarray[int]: meshObject.merge_nodes(meshObject.find_coincident_nodes()) where meshObject is the Mesh object containing merged coincidentNodes. + + Detection here uses a round-to-tolerance lexsort. For a distance-based + (KDTree) detector that also collapses clusters of 3+ coincident nodes + in one call, see :py:meth:`merge_coincident_nodes`. """ decimal_round = int(-np.log10(tol) - 1) crd = self.nodes.round(decimal_round) # round coordinates to match tolerance @@ -498,6 +502,68 @@ def find_coincident_nodes(self, tol: float = 1e-8) -> np.ndarray[int]: )[0] # indices of the first coincident nodes return np.array([ind_sorted[ind_coincident], ind_sorted[ind_coincident + 1]]).T + def merge_coincident_nodes(self, tol: float) -> int: + """Find and merge node pairs whose Euclidean distance is below ``tol``. + + Useful when stacking volumes from separate sources (e.g. several + STEP files meshed without a boolean fuse): the interface nodes are + geometrically close but distinct, and elements on either side + don't share connectivity. After merging, an interface node from + one phase and its counterpart from another become a single node, + and any constraint or RigidTie applied via the surviving index + propagates kinematically across the interface. + + The pairing is greedy by distance (closest pair first, each node + merged at most once per pass), and the whole pass is repeated until no + coincident pair remains. A single greedy pass merges only one pair of + any cluster, so a point shared by three or more parts (e.g. an edge + where 3+ volumes meet) would be left partially merged; repeating the + pass collapses the whole cluster to a single survivor. Within a pass + the each-node-once rule still prevents two corners of one element + collapsing to the same survivor (only degenerate sub-``tol`` elements + could). + + Parameters + ---------- + tol : float + Geometric distance threshold (same units as ``self.nodes``). + + Returns + ------- + int + Total number of node pairs merged across all passes. + + See Also + -------- + find_coincident_nodes : lighter round-to-tolerance detector returning + pairs for a single-pass ``merge_nodes`` call. + """ + from scipy.spatial import cKDTree + + total_merged = 0 + while True: + tree = cKDTree(self.nodes) + pairs = tree.query_pairs(r=tol, output_type="ndarray") + if len(pairs) == 0: + break + dists = np.linalg.norm( + self.nodes[pairs[:, 0]] - self.nodes[pairs[:, 1]], axis=1 + ) + order = np.argsort(dists) + used = np.zeros(self.n_nodes, dtype=bool) + accepted = [] + for k in order: + i, j = int(pairs[k, 0]), int(pairs[k, 1]) + if used[i] or used[j]: + continue + used[i] = used[j] = True + accepted.append((i, j)) + if not accepted: + break + self.merge_nodes(np.asarray(accepted, dtype=int)) + total_merged += len(accepted) + return total_merged + def merge_nodes(self, node_couples: np.ndarray[int]) -> None: """ Merge some nodes @@ -624,9 +690,14 @@ def extract_elements(self, element_set: str | list, name: str = "") -> "Mesh": * The new mesh keep the former element_sets dict with only the extrated elements. * The element indices of the new mesh are not the same as the former one. - * The new mesh keep the initial nodes and node_sets. To also removed the - nodes, a simple solution is to use the method "remove_isolated_nodes" - with the new mesh. + * The new mesh keep the initial ``nodes`` array (shared with the parent), so + an assembly built on it stays in the parent DOF space and can be summed + with full-mesh assemblies. To also remove the nodes, a simple solution is + to use the method "remove_isolated_nodes" with the new mesh. + * The result carries a ``parent_node_indices`` attribute listing the rows of + ``self.nodes`` actually referenced by the extracted elements. Consumers + that need the active subset (e.g. ``RigidBody`` slaving the right DOFs) + read it directly without recomputing ``np.unique(elements.ravel())``. """ if isinstance(element_set, str): element_set = self.element_sets[element_set] @@ -644,6 +715,11 @@ def extract_elements(self, element_set: str | list, name: str = "") -> "Mesh": self.elm_type, name=name, ) + # ``parent_node_indices`` flags the nodes actually referenced by + # the extracted elements. Consumers that need the active subset + # (e.g. ``RigidBody`` slaving the right DOFs) can read it + # directly without recomputing ``np.unique(elements.ravel())``. + sub_mesh.parent_node_indices = np.unique(sub_mesh.elements.ravel()) return sub_mesh def nearest_node(self, X: np.ndarray[float]) -> int: diff --git a/fedoo/core/output.py b/fedoo/core/output.py index c28d94e3..b38de28e 100644 --- a/fedoo/core/output.py +++ b/fedoo/core/output.py @@ -137,6 +137,99 @@ def _output_mesh_for_assembly(assemb, element_set=None): return assemb.mesh.extract_elements(element_set) +def _mesh_with_static_obstacles(assemb, element_set=None): + """Build an output MultiMesh containing results and static geometry.""" + result_mesh = _output_mesh_for_assembly(assemb, element_set) + + def leaf_assemblies(assembly): + children = getattr(assembly, "list_assembly", None) + if children is None: + yield assembly + else: + for child in children: + yield from leaf_assemblies(child) + + obstacles = [] + seen = set() + for sub_assemb in leaf_assemblies(assemb): + obstacle = getattr(sub_assemb, "_ipc_obstacle_mesh", None) + if obstacle is None: + continue + obstacle_id = getattr(sub_assemb, "_ipc_obstacle_source_id", None) + if obstacle_id is None: + obstacle_id = id(obstacle) + if obstacle_id not in seen: + seen.add(obstacle_id) + obstacles.append(obstacle) + + if not obstacles: + return result_mesh + ndim = max(mesh.ndim for mesh in (result_mesh, *obstacles)) + + node_blocks = [] + submesh_entries = {} + node_offset = 0 + + def append_mesh(mesh, default_name): + nonlocal node_offset + mesh = mesh.as_ndim(ndim) + node_blocks.append(mesh.nodes) + submeshes = mesh.submeshes if isinstance(mesh, MultiMesh) else (mesh,) + for submesh in submeshes: + base_name = submesh.name or default_name + name = base_name + suffix = 2 + while name in submesh_entries: + name = f"{base_name}_{suffix}" + suffix += 1 + submesh_entries[name] = ( + submesh.elm_type, + submesh.elements + node_offset, + submesh.element_sets, + ) + node_offset += mesh.n_nodes + + append_mesh(result_mesh, "Results") + for i, obstacle in enumerate(obstacles, start=1): + append_mesh(obstacle, f"Static obstacle {i}") + + return MultiMesh( + np.vstack(node_blocks), + submesh_entries, + ndim=ndim, + name=getattr(result_mesh, "name", ""), + register_name=False, + ) + + +def _add_static_geometry_to_results(result, result_mesh, output_mesh): + """Extend result fields to an output mesh containing static nodes.""" + if output_mesh is result_mesh or output_mesh.n_nodes == result_mesh.n_nodes: + return result + + n_extra = output_mesh.n_nodes - result_mesh.n_nodes + for field, value in tuple(result.node_data.items()): + value = np.asarray(value) + if value.ndim == 0 or value.shape[-1] != result_mesh.n_nodes: + continue + dtype = np.result_type(value.dtype, float) + value = value.astype(dtype, copy=False) + fill_value = 0.0 if field == "Disp" else np.nan + padding = np.full(value.shape[:-1] + (n_extra,), fill_value, dtype=dtype) + result.node_data[field] = np.concatenate((value, padding), axis=-1) + + # A regular result mesh becomes submesh 0 of the augmented MultiMesh. + # Element and Gauss-point data therefore remain absent on obstacle blocks. + if not isinstance(result_mesh, MultiMesh): + result.element_data = { + field: {0: value} for field, value in result.element_data.items() + } + result.gausspoint_data = { + field: {0: value} for field, value in result.gausspoint_data.items() + } + return result + + def _find_constitutivelaw_with_method(weakform, method_name): """Return a constitutive law from a possibly nested weakform.""" law = getattr(weakform, "constitutivelaw", None) @@ -549,6 +642,7 @@ def add_output( position=1, element_set=None, save_mesh=True, + include_static_obstacles=False, ): dirname = os.path.dirname(filename) # filename = os.path.basename(filename) @@ -566,6 +660,11 @@ def add_output( 0 ] # remove extension for the base name + if include_static_obstacles and file_format != "fdh5": + raise ValueError( + "include_static_obstacles=True requires the 'fdh5' file format." + ) + if file_format not in _available_format: print( "WARNING: '", @@ -590,7 +689,10 @@ def add_output( if isinstance(assemb, str): assemb = AssemblyBase.get_all()[assemb] - mesh = _output_mesh_for_assembly(assemb, element_set) + if include_static_obstacles: + mesh = _mesh_with_static_obstacles(assemb, element_set) + else: + mesh = _output_mesh_for_assembly(assemb, element_set) if not (os.path.isdir(dirname)) and dirname != "": os.mkdir(dirname) @@ -604,6 +706,7 @@ def add_output( "position": position, "element_set": element_set, "compressed": compressed, + "include_static_obstacles": include_static_obstacles, } self.__list_output.append(new_output) @@ -642,9 +745,15 @@ def save_results(self, pb, comp_output=None): position = output["position"] element_set = output["element_set"] compressed = output["compressed"] + include_static_obstacles = output["include_static_obstacles"] assemb = output["assembly"] # material = assemb.weakform.GetConstitutiveLaw() + result_mesh = _output_mesh_for_assembly(assemb, element_set) + if include_static_obstacles: + output_mesh = _mesh_with_static_obstacles(assemb, element_set) + else: + output_mesh = result_mesh if file_format in _available_format: # if not ignored if (comp_output is None) or (file_format in ["fdz", "fdh5"]): @@ -663,7 +772,7 @@ def save_results(self, pb, comp_output=None): list_file_format.append(file_format) list_compressed.append(compressed) - out = DataSet(_output_mesh_for_assembly(assemb, element_set)) + out = DataSet(output_mesh) list_data.append(out) else: # else, the same file is used @@ -679,6 +788,8 @@ def save_results(self, pb, comp_output=None): element_set, False, ) + if include_static_obstacles: + _add_static_geometry_to_results(res, result_mesh, output_mesh) out.add_data(res) for i, out in enumerate(list_data): diff --git a/fedoo/core/problem.py b/fedoo/core/problem.py index eee4d2b3..abe3ad17 100644 --- a/fedoo/core/problem.py +++ b/fedoo/core/problem.py @@ -135,6 +135,7 @@ def add_output( position=1, element_set=None, save_mesh=True, + include_static_obstacles=False, ): """Add output requirement for automatic saving during nlsolve. @@ -179,6 +180,13 @@ def add_output( save_mesh : bool, default = True If True the mesh is saved. + + include_static_obstacles : bool, default = False + If True, static meshes registered by + :meth:`fedoo.constraint.RigidBody.set_static_obstacle` are added + to the output mesh. The same obstacle mesh shared by several + rigid bodies is included only once. This option requires the + ``fdh5`` file format, which preserves the MultiMesh structure. """ if output_list is None and hasattr(self, "assembly"): output_list = assembly @@ -194,6 +202,7 @@ def add_output( position, element_set, save_mesh, + include_static_obstacles, ) def save_results(self, iterOutput=None): diff --git a/fedoo/problem/non_linear.py b/fedoo/problem/non_linear.py index ff1fb26d..8dfbe998 100644 --- a/fedoo/problem/non_linear.py +++ b/fedoo/problem/non_linear.py @@ -79,6 +79,12 @@ def __init__(self, assembly, nlgeom=False, name="MainProblem"): self.exec_callback_at_each_iter = False self.err_num = 1e-8 # numerical error + def _run_constraint_hook(self, name): + for bc in self.bc: + hook = getattr(bc, name, None) + if hook is not None: + hook(self) + @property def n_iter(self): """Return the number of iterations made to solve the problem.""" @@ -183,6 +189,8 @@ def set_time_integrator(self, evolution, integrator): return integrator def _has_time_integrated_weakform(self, assembly): + if getattr(assembly, "_fedoo_time_integrated", False): + return True if isinstance(assembly, AssemblySum): return any( self._has_time_integrated_weakform(child) @@ -215,12 +223,23 @@ def _iter_leaf_weakforms(self, assembly): for wf in getattr(weakform, "list_weakform", [weakform]): yield wf + def _iter_assembly_time_providers(self, assembly): + if isinstance(assembly, AssemblySum): + for child in assembly.list_assembly: + yield from self._iter_assembly_time_providers(child) + return + if getattr(assembly, "weakform", None) is None and ( + getattr(assembly, "storage", None) is not None + or getattr(assembly, "dissipation", None) is not None + ): + yield assembly + def _warn_ignored_storage(self): """Warn when declared storage/dissipation terms are silently ignored. - Weakforms may declare transient metadata (e.g. HeatEquation always - declares its heat capacity storage); without a matching problem-level - integrator the analysis is steady/static and these terms are dropped. + Weakforms and assembly-level providers may declare transient metadata; + without a matching problem-level integrator the analysis is steady/static + and these terms are dropped. This is legitimate for a deliberately steady analysis, but silent for a user migrating from the former transient-by-default weakforms, so a warning is emitted once at compile time. @@ -249,6 +268,21 @@ def _warn_ignored_storage(self): stacklevel=3, ) + for assembly in self._iter_assembly_time_providers(self.__assembly): + evolution = getattr(assembly, "time_evolution", None) + if evolution is None or evolution in self.time_integrators: + continue + warnings.warn( + f"Assembly provider '{assembly.name}' declares storage or " + f"dissipation terms for the '{evolution.kind}' time evolution, " + "but no matching time integrator is attached to the problem: " + "these terms are ignored and the analysis is treated as " + "steady/static. Attach a matching integrator with " + "pb.set_time_integrator.", + UserWarning, + stacklevel=3, + ) + def initialize(self): self._compile_time_integrators() self.__assembly.initialize(self) @@ -262,6 +296,7 @@ def set_start(self, save_results=False, callback=None): self._U += self._dU self._dU = 0 self.__assembly.set_start(self) + self._run_constraint_hook("set_start") # Save results if save_results: @@ -273,6 +308,7 @@ def set_start(self, save_results=False, callback=None): callback(self) else: self.__assembly.set_start(self) + self._run_constraint_hook("set_start") def to_start(self): self._dU = 0 @@ -280,6 +316,7 @@ def to_start(self): self._t_fact_inc = None self._err0 = self.nr_parameters["err0"] # initial error for NR error estimation self.__assembly.to_start(self) + self._run_constraint_hook("to_start") def update(self, compute="all", updateWeakForm=True): """ @@ -290,6 +327,12 @@ def update(self, compute="all", updateWeakForm=True): - Change in constitutive law (internal variable) Don't Update the problem with the new assembled global matrix and global vector -> use UpdateA and UpdateD method for this purpose """ + if self.bc._update_during_inc: + for bc in self.bc: + pre_update = getattr(bc, "pre_update", None) + if pre_update is not None: + pre_update(self) + if updateWeakForm == True: self.__assembly.update(self, compute) else: @@ -1005,6 +1048,7 @@ def nlsolve( interval_output: int | float | None = None, callback: Callable[[Problem, ...], None] | None = None, exec_callback_at_each_iter: bool | None = None, + dt_max: float | None = None, ) -> None: """Solve the non linear problem using the newton-raphson algorithm. @@ -1027,6 +1071,10 @@ def nlsolve( else, the attribute t0 is modified. dt_min: float, default = 1e-6 Minimal time increment + dt_max: float, optional + Maximum time increment. The initial ``dt`` and subsequent + adaptive increases are capped to this value. If omitted, the time + increment has no upper bound other than output and end times. max_subiter: int, optional Maximal number of newton raphson iteration allowed for each time increment, after the initial linear guess. @@ -1080,6 +1128,12 @@ def nlsolve( self.tmax = tmax if t0 is not None: self.t0 = t0 # time at the start of the time step + if dt_max is not None: + if dt_max <= 0: + raise ValueError("dt_max should be strictly positive.") + if dt_max < dt_min: + raise ValueError("dt_max should be greater than or equal to dt_min.") + dt = min(dt, dt_max) if max_subiter is None: max_subiter = self.nr_parameters["max_subiter"] if dt_increase_niter is None: @@ -1181,6 +1235,8 @@ def nlsolve( # Check if dt can be increased if update_dt and nb_nr_iter <= dt_increase_niter and dt == self.dtime: dt *= 1.25 + if dt_max is not None: + dt = min(dt, dt_max) else: if self.print_info > 0: diff --git a/fedoo/time/base.py b/fedoo/time/base.py index 5cd1c56b..34602f2e 100644 --- a/fedoo/time/base.py +++ b/fedoo/time/base.py @@ -67,8 +67,13 @@ def _integrate_leaf(self, weakform): raise NotImplementedError def _compile_assembly_level_provider(self, assembly): + if getattr(assembly, "time_evolution", None) != self.evolution: + return assembly + if getattr(assembly, "_fedoo_time_integrated", False): + return assembly if any(hasattr(assembly, attr) for attr in ("storage", "dissipation")): raise NotImplementedError( f"Assembly-level time providers are part of the architecture but " f"do not have a concrete {type(self).__name__} adapter yet." ) + return assembly diff --git a/fedoo/time/generalized_alpha.py b/fedoo/time/generalized_alpha.py index e97a5c49..3a2a1d47 100644 --- a/fedoo/time/generalized_alpha.py +++ b/fedoo/time/generalized_alpha.py @@ -1,7 +1,9 @@ import copy import numpy as np +from scipy import sparse +from fedoo.core._sparsematrix import scatter_dense_block from fedoo.core.time_evolution import SECOND_ORDER from fedoo.core.weakform import WeakFormBase, WeakFormSum from fedoo.time.base import TimeIntegratorBase @@ -25,6 +27,194 @@ def _newmark_state(term, assembly, dt): ) +class GeneralizedAlphaAssemblyAdapter: + """Apply generalized-alpha to an assembly-level matrix provider. + + Assembly providers keep ownership of their static tangent and force. The + adapter owns the temporal state and augments those contributions on the + provider's declared time DOF indices. + """ + + def __init__(self, integrator): + self.alpha_m = integrator.alpha_m + self.alpha_f = integrator.alpha_f + self.beta = integrator.beta + self.gamma = integrator.gamma + self.force_start = None + self.current_force = None + + @staticmethod + def _copy_state(state): + return { + key: value.copy() if hasattr(value, "copy") else value + for key, value in state.items() + } + + def initialize(self, assembly, pb): + n_dof = len(assembly.time_dof_indices) + assembly.sv["Velocity"] = np.zeros(n_dof) + assembly.sv["Acceleration"] = np.zeros(n_dof) + assembly.sv["_DeltaDisp"] = np.zeros(n_dof) + + initial_force = getattr(assembly, "get_time_initial_force", None) + if initial_force is not None: + force = np.asarray(initial_force(pb), dtype=float) + self.force_start = force.copy() + mass = np.asarray(assembly.get_storage_matrix(pb), dtype=float) + try: + assembly.sv["Acceleration"] = np.linalg.solve(mass, force) + except np.linalg.LinAlgError: + pass + else: + self.force_start = np.zeros(n_dof) + + assembly.sv_start = self._copy_state(assembly.sv) + + def update(self, assembly, pb): + if np.isscalar(pb._dU) and pb._dU == 0: + delta_disp = np.zeros(len(assembly.time_dof_indices)) + else: + delta_disp = np.asarray(pb._dU)[assembly.time_dof_indices] + assembly.sv["_DeltaDisp"] = delta_disp.copy() + + def _local_static_matrix(self, assembly): + provider = getattr(assembly, "get_time_stiffness_matrix", None) + if provider is not None: + return np.asarray(provider(), dtype=float) + + idx = assembly.time_dof_indices + matrix = assembly.global_matrix + if sparse.issparse(matrix): + return matrix[idx][:, idx].toarray() + return np.asarray(matrix)[np.ix_(idx, idx)] + + def _damping_matrix(self, assembly, mass, stiffness): + damping = getattr(assembly, "dissipation", None) + if damping is None: + return np.zeros_like(mass) + if not isinstance(damping, RayleighDamping): + raise NotImplementedError( + "Assembly-level generalized-alpha currently supports only " + "RayleighDamping." + ) + return damping.alpha * mass + damping.beta * stiffness + + @staticmethod + def _inertia_force(assembly, pb, mass, acceleration, velocity): + provider = getattr(assembly, "get_time_inertia_force", None) + if provider is not None: + return np.asarray(provider(pb, acceleration, velocity), dtype=float) + return mass @ acceleration + + @staticmethod + def _inertia_tangent( + assembly, + pb, + mass, + acceleration, + velocity, + acceleration_factor, + velocity_factor, + ): + provider = getattr(assembly, "get_time_inertia_tangent", None) + if provider is not None: + return np.asarray( + provider( + pb, + acceleration, + velocity, + acceleration_factor, + velocity_factor, + ), + dtype=float, + ) + return acceleration_factor * mass + + def integrate(self, assembly, pb, compute="all"): + dt = pb.dtime + if dt == 0: + return + + idx = np.asarray(assembly.time_dof_indices, dtype=int) + mass = np.asarray(assembly.get_storage_matrix(pb), dtype=float) + stiffness = self._local_static_matrix(assembly) + damping = self._damping_matrix(assembly, mass, stiffness) + acc_np1, vel_np1 = newmark_acceleration_velocity( + self.beta, + self.gamma, + dt, + assembly.sv["_DeltaDisp"], + assembly.sv["Velocity"], + assembly.sv["Acceleration"], + ) + acc_alpha = (1.0 - self.alpha_m) * acc_np1 + self.alpha_m * assembly.sv[ + "Acceleration" + ] + vel_alpha = (1.0 - self.alpha_f) * vel_np1 + self.alpha_f * assembly.sv[ + "Velocity" + ] + + a0 = 1.0 / (self.beta * dt**2) + c0 = self.gamma / (self.beta * dt) + + if compute != "vector": + if self.alpha_f != 0.0: + assembly.global_matrix = (1.0 - self.alpha_f) * assembly.global_matrix + tangent = self._inertia_tangent( + assembly, + pb, + mass, + acc_alpha, + vel_alpha, + (1.0 - self.alpha_m) * a0, + (1.0 - self.alpha_f) * c0, + ) + tangent += (1.0 - self.alpha_f) * c0 * damping + dynamic_matrix = scatter_dense_block( + tangent, idx, assembly.global_matrix.shape + ) + assembly.global_matrix = assembly.global_matrix + dynamic_matrix + + if compute != "matrix": + static_force = np.asarray(assembly.global_vector)[idx].copy() + self.current_force = static_force + force_start = ( + self.force_start if self.force_start is not None else static_force + ) + force_alpha = ( + 1.0 - self.alpha_f + ) * static_force + self.alpha_f * force_start + inertia_force = self._inertia_force( + assembly, pb, mass, acc_alpha, vel_alpha + ) + assembly.global_vector[idx] = ( + force_alpha - inertia_force - damping @ vel_alpha + ) + + def set_start(self, assembly, pb): + dof_solution = pb.get_dof_solution() + if not (np.isscalar(dof_solution) and dof_solution == 0): + dt = getattr(pb, "_dtime_prev", None) or pb.dtime + if dt: + acc, vel = newmark_acceleration_velocity( + self.beta, + self.gamma, + dt, + assembly.sv["_DeltaDisp"], + assembly.sv["Velocity"], + assembly.sv["Acceleration"], + ) + assembly.sv["Acceleration"] = acc + assembly.sv["Velocity"] = vel + assembly.sv["_DeltaDisp"] = np.zeros_like(assembly.sv["_DeltaDisp"]) + if self.current_force is not None: + self.force_start = self.current_force.copy() + assembly.sv_start = self._copy_state(assembly.sv) + + def to_start(self, assembly, pb): + assembly.sv = self._copy_state(assembly.sv_start) + + class GeneralizedAlpha(TimeIntegratorBase): """Generalized-alpha time integrator for second-order evolutions. @@ -64,6 +254,20 @@ def __init__(self, alpha_m=0.0, alpha_f=0.0, beta=None, gamma=None): if self.gamma <= 0.0: raise ValueError("gamma must be strictly positive.") + def _compile_assembly_level_provider(self, assembly): + if getattr(assembly, "time_evolution", None) != self.evolution: + return assembly + if not hasattr(assembly, "get_storage_matrix") or not hasattr( + assembly, "time_dof_indices" + ): + raise NotImplementedError( + "Second-order assembly providers must expose " + "get_storage_matrix(pb) and time_dof_indices." + ) + assembly._time_integrator = GeneralizedAlphaAssemblyAdapter(self) + assembly._fedoo_time_integrated = True + return assembly + def _integrate_leaf(self, weakform): storage = self._resolve_storage(weakform) if storage is None: diff --git a/fedoo/util/viewer.py b/fedoo/util/viewer.py index 40ee5b2d..b4b1d54d 100644 --- a/fedoo/util/viewer.py +++ b/fedoo/util/viewer.py @@ -31,6 +31,7 @@ from fedoo.core.mesh import MultiMesh USE_PYVISTA_QT = True +SOLID_COLOR_FIELD = "Solid color" def _is_multimesh(mesh): @@ -236,6 +237,8 @@ def __init__(self, data, title, parent=None, opts=None, filename=None): ##################################################################### if opts: self.opts = opts + self.opts.setdefault("color", "lightgray") + self.opts.setdefault("color_missing_fields", True) else: self.opts = { # Scalar bar options @@ -259,6 +262,8 @@ def __init__(self, data, title, parent=None, opts=None, filename=None): "metallic": 1.0, "roughness": 0.5, "diffuse": 1.0, + "color": "lightgray", + "color_missing_fields": True, # Clip plane parameters "clip_args": None, "clip_origin": None, @@ -281,7 +286,7 @@ def __init__(self, data, title, parent=None, opts=None, filename=None): self.current_field = "Stress" self.current_comp = "vm" elif "Disp" in field_names: - self.current_field = "disp" + self.current_field = "Disp" self.current_comp = "X" else: try: @@ -402,10 +407,17 @@ def update_plot(self, val=None, iteration=None, lock_view=True, plotter=None): if is_multimesh: multimesh_kargs["global_element_set"] = True multimesh_kargs["name"] = "data" + if self.current_field is not None and self.opts.get( + "color_missing_fields", True + ): + multimesh_kargs["missing_field_color"] = self.opts["color"] with _defer_rendering(plotter, is_multimesh): # plotter.clear() # not compatible with pbr ??? plotter.renderer.clear_actors() + plot_kargs = {} + if self.current_field is None: + plot_kargs["color"] = self.opts["color"] self.data.plot( field=self.current_field, component=self.current_comp, @@ -431,6 +443,7 @@ def update_plot(self, val=None, iteration=None, lock_view=True, plotter=None): # element_set_invert = self.opts["element_set_invert"], cmap=self.opts["cmap"], **multimesh_kargs, + **plot_kargs, ) if self.parent()._plane_widget_enabled: @@ -1218,14 +1231,18 @@ def _set_active(self, dock): # old_state = self.field_combo.blockSignals(True) # update_field_combo + dock = self.active_dock + current_field = dock.current_field + field_blocker = QSignalBlocker(self.field_combo) self.field_combo.clear() + self.field_combo.addItem(SOLID_COLOR_FIELD) if self.data is not None: self.field_combo.addItems(self.data.field_names()) - dock = self.active_dock if dock.current_field is not None: self.field_combo.setCurrentText(dock.current_field) else: - self.field_combo.setCurrentIndex(0) + self.field_combo.setCurrentText(SOLID_COLOR_FIELD) + del field_blocker # self.field_combo.blockSignals(old_state) # update data_type value @@ -1233,7 +1250,7 @@ def _set_active(self, dock): self.avg_combo.setCurrentText(dock.current_data_type) # update component combo - self.update_components(dock.current_field) + self.update_components(current_field) # self.avg_combo.blockSignals(old_state) if self._plot_dialog: # if plot dialog exist @@ -1372,6 +1389,7 @@ def iteration(self) -> int: return self.iter_slider.value() def on_field_changed(self, field): + field = None if field == SOLID_COLOR_FIELD else field self.update_components(field) if self.active_dock: self.update_plot_with_clim(lock_view=True) @@ -1380,8 +1398,10 @@ def get_components(self, field): return self.active_dock.get_components(field) def update_components(self, field): - if not field or not self.active_dock: + if not field or field == SOLID_COLOR_FIELD or not self.active_dock: comps = [""] + if self.active_dock: + self.active_dock.current_field = None else: comps = self.get_components(field) self.active_dock.current_field = field @@ -1397,7 +1417,7 @@ def update_components(self, field): @property def current_field(self): field = self.field_combo.currentText() - if field == "": + if field in ("", SOLID_COLOR_FIELD): return None else: return self.field_combo.currentText() @@ -1672,6 +1692,10 @@ def _apply_renderer_options(self): list_docks = [self.active_dock] for dock in list_docks: dock.opts["opacity"] = float(self._renderer_dialog.opacity_spin.value()) + dock.opts["color"] = self._renderer_dialog.color + dock.opts["color_missing_fields"] = ( + self._renderer_dialog.missing_fields_cb.isChecked() + ) dock.opts["pbr"] = self._renderer_dialog.pbr_cb.isChecked() dock.opts["metallic"] = float(self._renderer_dialog.metallic_spin.value()) dock.opts["roughness"] = float(self._renderer_dialog.roughness_spin.value()) @@ -1684,7 +1708,9 @@ def update_plot_with_clim( if dock is None: if self._sync_field: for d in self.all_docks: - if self.current_field in d.data.field_names(): + if self.current_field is None: + d.current_field = None + elif self.current_field in d.data.field_names(): d.current_field = self.current_field # Only set component if field exists in this dock comps = d.get_components(self.current_field) @@ -1701,7 +1727,7 @@ def update_plot_with_clim( dock.current_data_type = self.current_data_type - if dock.opts["clim_mode"] == "all": + if dock.current_field is not None and dock.opts["clim_mode"] == "all": if hasattr(dock.data, "get_all_frame_lim"): dock.opts["clim"] = dock.data.get_all_frame_lim( field=dock.current_field, @@ -2712,9 +2738,9 @@ def _filter_cmaps(self, text): self.cmap_preview.update_preview(filtered[0], self.cmap_reverse.isChecked()) def _update_clim_values(self): - if self.rb_manual.isChecked(): - return parent = self.parent() + if self.rb_manual.isChecked() or parent.current_field is None: + return if self.rb_current.isChecked(): data = parent.get_current_data() data = _global_data_array(data) @@ -2828,6 +2854,15 @@ def __init__(self, parent=None): "Overall opacity (0.0 = transparent, 1.0 = opaque)." ) + self.color_button = QtWidgets.QPushButton() + self.color_button.setToolTip("Color used for geometry without field data.") + self.color_button.clicked.connect(self._choose_color) + self.color = "lightgray" + + self.missing_fields_cb = QtWidgets.QCheckBox( + "Apply to objects missing selected field" + ) + self.pbr_cb = QtWidgets.QCheckBox("Activate Physically Based Rendering (PBR)") self.pbr_cb.setToolTip("Enable VTK's PBR shading model for the actor/material.") @@ -2862,6 +2897,8 @@ def __init__(self, parent=None): form = QtWidgets.QFormLayout() form.addRow("Opacity:", self.opacity_spin) + form.addRow("Solid color:", self.color_button) + form.addRow(self.missing_fields_cb) main.addLayout(form) # PBR layout @@ -2895,11 +2932,25 @@ def __init__(self, parent=None): def update_values(self): opts = self.parent().opts self.opacity_spin.setValue(float(opts["opacity"])) + self._set_color(opts.get("color", "lightgray")) + self.missing_fields_cb.setChecked(opts.get("color_missing_fields", True)) self.pbr_cb.setChecked(opts["pbr"]) self.metallic_spin.setValue(float(opts["metallic"])) self.roughness_spin.setValue(float(opts["roughness"])) self.diffuse_spin.setValue(float(opts["diffuse"])) + def _set_color(self, color): + qcolor = QtGui.QColor(color) + if not qcolor.isValid(): + qcolor = QtGui.QColor("lightgray") + self.color = qcolor.name() + self.color_button.setStyleSheet(f"background-color: {self.color};") + + def _choose_color(self): + color = QtWidgets.QColorDialog.getColor(QtGui.QColor(self.color), self) + if color.isValid(): + self._set_color(color) + def _toggle_pbr_group(self, checked: bool): self.pbr_group.setEnabled(checked) diff --git a/tests/test_implicit_dynamic_damping.py b/tests/test_implicit_dynamic_damping.py new file mode 100644 index 00000000..1139307f --- /dev/null +++ b/tests/test_implicit_dynamic_damping.py @@ -0,0 +1,89 @@ +"""Rayleigh damping validation for the ImplicitDynamic weak form. + +``ImplicitDynamic`` (a Newmark weak form summed onto a standard ``NonLinear`` +problem) is fedoo's current implicit-dynamics path — the same +dynamics-as-assembly architecture as this PR's ``RigidBodyAssembly`` (and +unlike the legacy ``NonLinearNewmark`` problem class). The existing +``test_2DDynamicPlasticBending_v2`` sets ``rayleigh_damping`` but asserts +nothing about it; these tests pin the actual behaviour. +""" + +import numpy as np +import pytest + +import fedoo as fd + + +def _build_axial_bar(rayleigh=None): + """Axial bar (nu=0), fixed at x=0, constant tip load in X, ImplicitDynamic.""" + fd.Assembly.delete_memory() + fd.ModelingSpace("2Dplane") + E, nu, rho = 1.0, 0.0, 1.0 + fd.mesh.rectangle_mesh( + nx=9, + ny=3, + x_min=0, + x_max=1.0, + y_min=0, + y_max=0.2, + elm_type="quad4", + name="Domain", + ) + mesh = fd.Mesh["Domain"] + fd.constitutivelaw.ElasticIsotrop(E, nu, name="law") + wf = fd.weakform.ImplicitDynamic("law", rho, 0.25, 0.5) + if rayleigh is not None: + wf.rayleigh_damping = rayleigh # [alpha (mass), beta (stiffness)] + fd.Assembly.create(wf, "Domain", "quad4", name="asm") + pb = fd.problem.NonLinear("asm") + pb.bc.add("Dirichlet", mesh.find_nodes("X", 0.0), "Disp", 0) + pb.bc.add("Neumann", mesh.find_nodes("X", 1.0), "DispX", 0.05) + return pb, mesh + + +def test_rayleigh_damping_property_roundtrip(): + """``rayleigh_damping = [a, b]`` must read back as ``[a, b]`` (mass, stiff).""" + fd.Assembly.delete_memory() + fd.ModelingSpace("2Dplane") + fd.constitutivelaw.ElasticIsotrop(1.0, 0.0, name="law") + wf = fd.weakform.ImplicitDynamic("law", 1.0, 0.25, 0.5) + assert wf.rayleigh_damping is None + wf.rayleigh_damping = [0.7, 0.2] + assert wf.rayleigh_damping == [0.7, 0.2] + + +def _max_speed_over_run(rayleigh): + pb, mesh = _build_axial_bar(rayleigh=rayleigh) + speeds = [] + + def cb(p): + v = p.get_results("asm", ["Velocity"], "Node").node_data["Velocity"] + speeds.append(float(np.abs(v).max())) + + pb.nlsolve( + dt=0.05, + tmax=8.0, + update_dt=False, + print_info=0, + callback=cb, + exec_callback_at_each_iter=True, + ) + return max(speeds) if speeds else 0.0 + + +def test_implicit_dynamic_rayleigh_damping_reduces_peak_velocity(): + """Heavy mass-proportional Rayleigh damping must remove kinetic energy. + + A wrong-sign damping term would amplify the response, so this also pins + the sign of the damping contribution. + """ + v_undamped = _max_speed_over_run(None) + v_damped = _max_speed_over_run([20.0, 0.0]) + assert v_undamped > 0.0 + assert ( + v_damped < 0.5 * v_undamped + ), f"damped peak speed {v_damped:.3e} not < 0.5 * undamped {v_undamped:.3e}" + + +if __name__ == "__main__": + pytest.main([__file__, "-v"]) diff --git a/tests/test_ipc_rigid_tie_shape.py b/tests/test_ipc_rigid_tie_shape.py new file mode 100644 index 00000000..30300d45 --- /dev/null +++ b/tests/test_ipc_rigid_tie_shape.py @@ -0,0 +1,184 @@ +"""Regression test: IPCContact must not double-count global DOFs. + +Before this fix, ``_compute_ipc_contributions`` padded ``global_matrix`` +and ``global_vector`` by ``self._n_global_dof`` whenever there were extra +global DOFs and no rigid bodies registered via ``add_rigid_body``. The +scatter matrix already maps to the full ``n_dof = nvar*n_nodes + +n_global_dof``, so that padding inflated the result to ``n_dof + +n_global_dof`` rows, mismatching the FEM ``Assembly`` sibling and +raising ``ValueError: inconsistent shapes`` inside ``assembly_sum``. + +The tests below exercise: + * RigidTie (12 global DOFs) + IPCContact across a deformable bottom + + two rigid-tied blocks. + * PeriodicBC + IPCContact on a single periodic box. + +For each, we run ``nlsolve`` for one increment and assert no +``ValueError`` and that the IPC and elastic blocks have the same +matrix/vector shape. +""" + +import os + +os.environ.setdefault("KMP_DUPLICATE_LIB_OK", "TRUE") + +import numpy as np +import pytest + +import fedoo as fd + +ipctk = pytest.importorskip("ipctk") + + +def _reset_space(): + """Ensure each test starts with a fresh ModelingSpace.""" + fd.Assembly.delete_memory() + + +@pytest.fixture +def fresh_3d_space(): + _reset_space() + fd.ModelingSpace("3D") + yield + _reset_space() + + +def test_rigid_tie_plus_ipc_assembles_with_consistent_shape(fresh_3d_space): + """Two RigidTie + IPCContact: matrix shapes must match across all + assemblies in the sum.""" + # Deformable bottom plate + mesh_bot = fd.mesh.box_mesh( + nx=4, + ny=4, + nz=3, + x_min=0, + x_max=1, + y_min=0, + y_max=1, + z_min=0.0, + z_max=0.5, + ) + # Two rigid blocks above with an initial gap + mesh_A = fd.mesh.box_mesh( + nx=3, + ny=3, + nz=2, + x_min=0.05, + x_max=0.40, + y_min=0.30, + y_max=0.70, + z_min=0.70, + z_max=0.85, + ) + mesh_B = fd.mesh.box_mesh( + nx=3, + ny=3, + nz=2, + x_min=0.60, + x_max=0.95, + y_min=0.30, + y_max=0.70, + z_min=0.70, + z_max=0.85, + ) + + mesh = fd.Mesh.stack(fd.Mesh.stack(mesh_bot, mesh_A), mesh_B) + n_bot = mesh_bot.n_nodes + nodes_A = np.arange(n_bot, n_bot + mesh_A.n_nodes) + nodes_B = np.arange(n_bot + mesh_A.n_nodes, mesh.n_nodes) + + mat = fd.constitutivelaw.ElasticIsotrop(1e4, 0.3) + wf = fd.weakform.StressEquilibrium(mat, nlgeom=True) + solid = fd.Assembly.create(wf, mesh) + + surf = fd.mesh.extract_surface(mesh, quad2tri=True) + ipc = fd.constraint.IPCContact( + mesh, + surface_mesh=surf, + dhat=0.03, + dhat_is_relative=False, + use_ccd=False, + ) + assembly = fd.Assembly.sum(solid, ipc) + + pb = fd.problem.NonLinear(assembly) + # Two RigidTie constraints contribute 12 global DOFs total + pb.bc.add(fd.constraint.RigidTie(nodes_A, name="tieA")) + pb.bc.add(fd.constraint.RigidTie(nodes_B, name="tieB")) + + pb.bc.add("Dirichlet", mesh.find_nodes("Z", 0.0), "Disp", 0) + # Push rigid blocks down — initial gap 0.20, dhat=0.03, push by 0.19 so + # we cross into the barrier zone and exercise the padded-matrix code path. + pb.bc.add("Dirichlet", "RigidDispZ", -0.19) + pb.set_nr_criterion("Displacement", tol=5e-3, max_subiter=15) + + expected_n_dof = pb.n_node_dof + pb.n_global_dof + assert pb.n_global_dof == 12 + + # Drive nlsolve through one increment. Before the fix this raised + # ``ValueError: inconsistent shapes`` from ``assembly_sum`` the moment + # IPC detected its first collision. + pb.nlsolve(dt=0.2, tmax=0.2, update_dt=True, print_info=0) + + assert ipc.global_matrix.shape == (expected_n_dof, expected_n_dof) + assert ipc.global_vector.shape == (expected_n_dof,) + assert solid.global_matrix.shape == ipc.global_matrix.shape + assert solid.global_vector.shape == ipc.global_vector.shape + + +def test_periodic_bc_plus_ipc_assembles_with_consistent_shape(fresh_3d_space): + """PeriodicBC + IPCContact: even with no contact, the global-DOF + padding path must produce matrices of size ``n_dof``, not + ``n_dof + n_global_dof``.""" + mesh = fd.mesh.box_mesh( + nx=4, + ny=4, + nz=4, + x_min=0, + x_max=1, + y_min=0, + y_max=1, + z_min=0, + z_max=1, + ) + + mat = fd.constitutivelaw.ElasticIsotrop(1e4, 0.3) + wf = fd.weakform.StressEquilibrium(mat, nlgeom=False) + solid = fd.Assembly.create(wf, mesh) + + surf = fd.mesh.extract_surface(mesh, quad2tri=True) + # No initial contact; we only need IPC's assemble path to be exercised + # alongside the PeriodicBC-driven global DOFs. + ipc = fd.constraint.IPCContact( + mesh, + surface_mesh=surf, + dhat=0.02, + dhat_is_relative=False, + use_ccd=False, + ) + assembly = fd.Assembly.sum(solid, ipc) + + pb = fd.problem.NonLinear(assembly) + pb.bc.add(fd.constraint.PeriodicBC(periodicity_type="small_strain", dim=3)) + # Pin one corner against rigid body translation + corner = mesh.nearest_node([0.0, 0.0, 0.0]) + pb.bc.add("Dirichlet", corner, "Disp", 0) + # Drive a small mean strain — small_strain BC exposes E_xx as a global DOF + pb.bc.add("Dirichlet", "E_xx", 0.005) + for v in ("E_yy", "E_zz", "E_xy", "E_xz", "E_yz"): + pb.bc.add("Dirichlet", v, 0.0) + + pb.set_nr_criterion("Displacement", tol=5e-3, max_subiter=10) + + expected_n_dof = pb.n_node_dof + pb.n_global_dof + assert pb.n_global_dof > 0 + + pb.nlsolve(dt=1.0, tmax=1.0, update_dt=True, print_info=0) + + assert ipc.global_matrix.shape == (expected_n_dof, expected_n_dof) + assert ipc.global_vector.shape == (expected_n_dof,) + assert solid.global_matrix.shape == ipc.global_matrix.shape + + +if __name__ == "__main__": + pytest.main([__file__, "-v"]) diff --git a/tests/test_merge_coincident_nodes.py b/tests/test_merge_coincident_nodes.py new file mode 100644 index 00000000..a1222a1d --- /dev/null +++ b/tests/test_merge_coincident_nodes.py @@ -0,0 +1,64 @@ +"""Regression tests for Mesh.merge_coincident_nodes. + +In particular, a point shared by three or more parts must collapse to a single +survivor — a single greedy pass would merge only one pair of the cluster and +leave the rest split. +""" + +import numpy as np +import pytest + +import fedoo as fd + + +def test_merge_pair(): + nodes = np.array([[0, 0, 0], [1e-9, 0, 0], [1, 0, 0], [1, 1, 0]], dtype=float) + elements = np.array([[0, 2, 3], [1, 3, 2]]) + m = fd.Mesh(nodes, elements, "tri3") + merged = m.merge_coincident_nodes(tol=1e-6) + assert merged == 1 + assert m.n_nodes == 3 + # The two coincident corners now reference the same surviving node. + assert m.elements[0, 0] == m.elements[1, 0] + + +def test_merge_triple_point_collapses_to_single_node(): + """Three parts meeting at one point must all share a single node.""" + eps = 1e-9 + nodes = np.array( + [ + [0, 0, 0], + [eps, 0, 0], + [0, eps, 0], # three near-coincident corners + [1, 0, 0], + [0, 1, 0], + [2, 0, 0], + [2, 1, 0], + [-1, 0, 0], + [-1, 1, 0], + ], + dtype=float, + ) + elements = np.array([[0, 3, 4], [1, 5, 6], [2, 7, 8]]) + m = fd.Mesh(nodes, elements, "tri3") + + merged = m.merge_coincident_nodes(tol=1e-6) + + assert merged == 2 # two pairs collapse the 3-node cluster to 1 + assert m.n_nodes == 7 # 9 - 2 + shared = m.elements[:, 0] + assert ( + len(set(shared.tolist())) == 1 + ), f"triple point not fully merged: corner ids {shared}" + + +def test_merge_noop_when_no_coincident_nodes(): + nodes = np.array([[0, 0, 0], [1, 0, 0], [0, 1, 0]], dtype=float) + elements = np.array([[0, 1, 2]]) + m = fd.Mesh(nodes, elements, "tri3") + assert m.merge_coincident_nodes(tol=1e-6) == 0 + assert m.n_nodes == 3 + + +if __name__ == "__main__": + pytest.main([__file__, "-v"]) diff --git a/tests/test_rigid_body_dynamics.py b/tests/test_rigid_body_dynamics.py new file mode 100644 index 00000000..8c9453ac --- /dev/null +++ b/tests/test_rigid_body_dynamics.py @@ -0,0 +1,141 @@ +"""Rotational inertia tests for dynamic rigid bodies.""" + +import numpy as np + +import fedoo as fd +from fedoo.constraint.rigid_body import RigidBodyAssembly +from simcoon import Rotation + + +def _make_assembly(inertia, rotvec): + space = fd.ModelingSpace("3D") + tie = fd.constraint.RigidTie(np.array([0]), center=np.zeros(3), use_quaternion=True) + tie._Q_base = Rotation.identity() + tie._angles_at_base = np.zeros(3) + state = {"rotvec": np.asarray(rotvec, dtype=float).copy()} + + def get_dof_ref(_problem): + return np.concatenate([np.zeros(3), state["rotvec"]]), None + + tie._get_dof_ref = get_dof_ref + assembly = RigidBodyAssembly( + mass=2.0, + inertia_tensor=np.asarray(inertia, dtype=float), + rigid_tie=tie, + space=space, + ) + assembly._dof_indices = np.arange(6) + return assembly, state + + +def test_non_spherical_inertia_adds_gyroscopic_force(): + inertia = np.diag([2.0, 3.0, 5.0]) + assembly, _ = _make_assembly(inertia, np.zeros(3)) + acceleration = np.array([0.1, -0.2, 0.3, 0.4, -0.5, 0.6]) + velocity = np.array([1.0, 2.0, -1.0, 0.7, -0.4, 1.2]) + + force = assembly.get_time_inertia_force(object(), acceleration, velocity) + + angular_velocity = velocity[3:] + expected_moment = inertia @ acceleration[3:] + np.cross( + angular_velocity, inertia @ angular_velocity + ) + np.testing.assert_allclose(force[:3], 2.0 * acceleration[:3]) + np.testing.assert_allclose(force[3:], expected_moment) + assert not np.allclose(np.cross(angular_velocity, inertia @ angular_velocity), 0.0) + + +def test_spherical_inertia_has_no_gyroscopic_force(): + inertia = 3.0 * np.eye(3) + assembly, _ = _make_assembly(inertia, np.zeros(3)) + acceleration = np.array([0.0, 0.0, 0.0, 0.4, -0.5, 0.6]) + velocity = np.array([0.0, 0.0, 0.0, 0.7, -0.4, 1.2]) + + force = assembly.get_time_inertia_force(object(), acceleration, velocity) + + np.testing.assert_allclose(force[3:], inertia @ acceleration[3:]) + + +def test_gyroscopic_inertia_tangent_matches_finite_difference(): + inertia = np.diag([2.0, 3.0, 5.0]) + rotvec = np.array([0.2, -0.15, 0.1]) + assembly, state = _make_assembly(inertia, rotvec) + acceleration = np.array([0.1, -0.2, 0.3, 0.4, -0.5, 0.6]) + velocity = np.array([1.0, 2.0, -1.0, 0.7, -0.4, 1.2]) + acceleration_factor = 8.0 + velocity_factor = 2.5 + + tangent = assembly.get_time_inertia_tangent( + object(), + acceleration, + velocity, + acceleration_factor, + velocity_factor, + ) + + h = 1e-7 + numerical = np.zeros((3, 3)) + reference = state["rotvec"].copy() + for axis in range(3): + perturbation = np.zeros(3) + perturbation[axis] = h + + state["rotvec"] = reference + perturbation + acceleration_plus = acceleration.copy() + velocity_plus = velocity.copy() + acceleration_plus[3 + axis] += acceleration_factor * h + velocity_plus[3 + axis] += velocity_factor * h + force_plus = assembly.get_time_inertia_force( + object(), acceleration_plus, velocity_plus + )[3:] + + state["rotvec"] = reference - perturbation + acceleration_minus = acceleration.copy() + velocity_minus = velocity.copy() + acceleration_minus[3 + axis] -= acceleration_factor * h + velocity_minus[3 + axis] -= velocity_factor * h + force_minus = assembly.get_time_inertia_force( + object(), acceleration_minus, velocity_minus + )[3:] + numerical[:, axis] = (force_plus - force_minus) / (2.0 * h) + + state["rotvec"] = reference + np.testing.assert_allclose(tangent[3:, 3:], numerical, rtol=1e-6, atol=1e-6) + + +def test_principal_axis_torque_matches_constant_angular_acceleration(): + space = fd.ModelingSpace("3D") + space.new_variable("DispX") + space.new_variable("DispY") + space.new_variable("DispZ") + space.new_vector("Disp", ("DispX", "DispY", "DispZ")) + nodes = np.array( + [ + [-0.5, -0.5, 0.0], + [0.5, -0.5, 0.0], + [0.5, 0.5, 0.0], + [-0.5, 0.5, 0.0], + ] + ) + mesh = fd.Mesh(nodes, np.array([[0, 1, 2, 3]]), "quad4") + inertia = np.diag([2.0, 3.0, 5.0]) + body = fd.constraint.RigidBody( + mesh, + mass=1.0, + inertia_tensor=inertia, + center_of_mass=np.zeros(3), + ) + torque = 4.0 + body.set_torque([torque, 0.0, 0.0]) + + end_time = 0.02 + body.solve( + dt=1e-3, + tmax=end_time, + print_info=0, + solver="direct_scipy", + ) + + expected_angle = 0.5 * torque / inertia[0, 0] * end_time**2 + rotation_vector = body.constraint.Q_total.as_rotvec() + np.testing.assert_allclose(rotation_vector, [expected_angle, 0.0, 0.0], atol=1e-10) diff --git a/tests/test_rigid_body_freefall.py b/tests/test_rigid_body_freefall.py new file mode 100644 index 00000000..d1734962 --- /dev/null +++ b/tests/test_rigid_body_freefall.py @@ -0,0 +1,86 @@ +"""Analytical validation of rigid body free fall under gravity.""" + +import numpy as np +import pytest + +import fedoo as fd + + +G = 9.81 + + +def _run_freefall(dt, t_end, mass=1.0, z0=1.0, radius=0.1): + try: + import pyvista as pv + except ImportError: # pragma: no cover + pytest.skip("pyvista not available") + + space = fd.ModelingSpace("3D") + space.new_variable("DispX") + space.new_variable("DispY") + space.new_variable("DispZ") + space.new_vector("Disp", ("DispX", "DispY", "DispZ")) + + mesh = fd.Mesh.from_pyvista( + pv.Sphere( + radius=radius, center=(0, 0, z0), theta_resolution=6, phi_resolution=6 + ) + ) + body = fd.constraint.RigidBody( + mesh, + mass=mass, + inertia_tensor=0.004 * np.eye(3), + center_of_mass=np.array([0.0, 0.0, z0]), + ) + body.set_force([0.0, 0.0, -mass * G]) + + pb = body.solve(dt=dt, tmax=t_end, print_info=0, solver="direct_scipy") + dof = pb.get_dof_solution() + idx = body.assembly._dof_indices + return z0 + dof[idx[2]] + + +@pytest.mark.parametrize("dt", [1e-3, 5e-4]) +def test_freefall_matches_analytical_solution(dt): + t_end = 0.3 + z_sim = _run_freefall(dt=dt, t_end=t_end) + z_exact = 1.0 - 0.5 * G * t_end**2 + # Newmark (β=0.25, γ=0.5) is energy-conserving and exact on quadratic + # trajectories up to round-off; 1e-4 m is ample margin. + assert abs(z_sim - z_exact) < 1e-4 + + +def test_freefall_no_horizontal_drift(): + t_end = 0.3 + try: + import pyvista as pv + except ImportError: # pragma: no cover + pytest.skip("pyvista not available") + + space = fd.ModelingSpace("3D") + space.new_variable("DispX") + space.new_variable("DispY") + space.new_variable("DispZ") + space.new_vector("Disp", ("DispX", "DispY", "DispZ")) + + mesh = fd.Mesh.from_pyvista( + pv.Sphere(radius=0.1, center=(0, 0, 1.0), theta_resolution=6, phi_resolution=6) + ) + body = fd.constraint.RigidBody( + mesh, + mass=1.0, + inertia_tensor=0.004 * np.eye(3), + center_of_mass=np.array([0.0, 0.0, 1.0]), + ) + body.set_force([0.0, 0.0, -G]) + + pb = body.solve(dt=1e-3, tmax=t_end, print_info=0, solver="direct_scipy") + dof = pb.get_dof_solution() + idx = body.assembly._dof_indices + dx, dy = dof[idx[0]], dof[idx[1]] + assert abs(dx) < 1e-10 + assert abs(dy) < 1e-10 + + +if __name__ == "__main__": + pytest.main([__file__]) diff --git a/tests/test_rigid_body_ipc_deformable.py b/tests/test_rigid_body_ipc_deformable.py new file mode 100644 index 00000000..d3af6ff5 --- /dev/null +++ b/tests/test_rigid_body_ipc_deformable.py @@ -0,0 +1,272 @@ +"""Regression tests for rigid-body coupling with a shared ``IPCContact``. + +A coarse version of `examples/contact/ipc/rigid_deformable_punch.py`. The +test asserts: + +1. Matrix/vector shapes are consistent across the assembly sum (regression + guard for the global-DOF padding bug fixed earlier on this branch). +2. The deformable plate actually compresses under the rigid piston — + the bug this entire feature exists to prevent. +3. Newton's third law: the IPC barrier gradient sums to zero between rigid + and deformable surface DOFs. +4. The RigidTie kinematic relation is satisfied exactly on the rigid + surface (max abs error = 0 across X/Y/Z). +5. ``set_static_obstacle`` and ``add_rigid_body`` are mutually exclusive + on a single body. +6. Generic nodal IPC followed by ``RigidTie`` MPC condensation works without + explicitly registering the body on ``IPCContact``. +""" + +import os + +os.environ.setdefault("KMP_DUPLICATE_LIB_OK", "TRUE") + +import numpy as np +import pytest + +import fedoo as fd + +ipctk = pytest.importorskip("ipctk") + + +@pytest.fixture +def fresh_3d_space(): + fd.Assembly.delete_memory() + fd.ModelingSpace("3D") + yield + fd.Assembly.delete_memory() + + +def _build_punch_problem(register_rigid_body=True): + """Disc + one rigid piston above. Returns (pb, ipc, solid, body, mesh, + n_disc, nodes_top).""" + disc = fd.mesh.box_mesh( + nx=6, + ny=6, + nz=3, + x_min=0.0, + x_max=1.0, + y_min=0.0, + y_max=1.0, + z_min=0.0, + z_max=0.10, + ) + top = fd.mesh.box_mesh( + nx=3, + ny=3, + nz=2, + x_min=0.30, + x_max=0.70, + y_min=0.30, + y_max=0.70, + z_min=0.15, + z_max=0.30, + ) + disc.add_element_set(np.arange(disc.n_elements), "disc") + top.add_element_set(np.arange(top.n_elements), "top") + mesh = fd.Mesh.stack(disc, top) + n_disc = disc.n_nodes + top_part = mesh.extract_elements("top") + nodes_top = top_part.parent_node_indices + + mat = fd.constitutivelaw.ElasticIsotrop(1e4, 0.3) + wf = fd.weakform.StressEquilibrium(mat, nlgeom=True) + solid = fd.Assembly.create(wf, mesh) + + surf = fd.mesh.extract_surface(mesh, quad2tri=True) + ipc = fd.constraint.IPCContact( + mesh, + surface_mesh=surf, + dhat=0.02, + dhat_is_relative=False, + use_ccd=False, + barrier_stiffness=1e6, + ) + + body = fd.constraint.RigidBody( + top_part, + mass=1.0, + inertia_tensor=0.01 * np.eye(3), + name="piston", + ) + if register_rigid_body: + ipc.add_rigid_body(body) + + assembly = fd.Assembly.sum(solid, body.assembly, ipc) + pb = fd.problem.NonLinear(assembly, nlgeom=True) + + # Clamp the disc's four side faces against rigid-body motion. + for axis, value in (("X", 0.0), ("X", 1.0), ("Y", 0.0), ("Y", 1.0)): + pb.bc.add("Dirichlet", mesh.find_nodes(axis, value), "Disp", 0) + + nc = body.constraint.node_cd + pb.bc.add("Dirichlet", nc[0], "RigidDispX", 0.0) + pb.bc.add("Dirichlet", nc[1], "RigidDispY", 0.0) + pb.bc.add("Dirichlet", nc[2], "RigidDispZ", -0.06) + pb.bc.add("Dirichlet", nc[3], "RigidRotX", 0.0) + pb.bc.add("Dirichlet", nc[4], "RigidRotY", 0.0) + pb.bc.add("Dirichlet", nc[5], "RigidRotZ", 0.0) + + pb.set_nr_criterion("Displacement", tol=5e-3, max_subiter=20) + return pb, ipc, solid, body, mesh, n_disc, nodes_top + + +def test_generic_ipc_uses_rigid_tie_mpc_condensation(fresh_3d_space): + pb, ipc, solid, body, mesh, n_disc, nodes_top = _build_punch_problem( + register_rigid_body=False + ) + + pb.nlsolve(dt=0.2, tmax=0.2, update_dt=True, print_info=0) + + assert not ipc._rigid_bodies + disp = pb.get_disp() + plate_top_nodes = np.where( + (mesh.nodes[:n_disc, 2] > 0.099) + & (mesh.nodes[:n_disc, 0] > 0.25) + & (mesh.nodes[:n_disc, 0] < 0.75) + & (mesh.nodes[:n_disc, 1] > 0.25) + & (mesh.nodes[:n_disc, 1] < 0.75) + )[0] + assert disp[2, plate_top_nodes].min() < -1e-3 + + q = pb.get_dof_solution()[body.assembly._dof_indices] + rotation, *_ = body.constraint._compute_rotation(q[3:]) + relative_position = mesh.nodes[nodes_top] - body.center_of_mass + expected = ( + relative_position @ rotation.T + + body.center_of_mass + + q[:3] + - mesh.nodes[nodes_top] + ) + np.testing.assert_allclose(disp[:, nodes_top].T, expected, atol=1e-9) + + +def test_shapes_consistent_across_assembly_sum(fresh_3d_space): + pb, ipc, solid, body, mesh, n_disc, nodes_top = _build_punch_problem() + pb.nlsolve(dt=0.2, tmax=0.2, update_dt=True, print_info=0) + + expected_n_dof = pb.n_node_dof + pb.n_global_dof + assert pb.n_global_dof == 6 + assert ipc.global_matrix.shape == (expected_n_dof, expected_n_dof) + assert ipc.global_vector.shape == (expected_n_dof,) + assert solid.global_matrix.shape == ipc.global_matrix.shape + + +def test_deformable_plate_actually_compresses(fresh_3d_space): + pb, ipc, solid, body, mesh, n_disc, nodes_top = _build_punch_problem() + pb.nlsolve(dt=0.2, tmax=0.2, update_dt=True, print_info=0) + + disp = pb.get_disp() + plate_top_nodes = np.where( + (mesh.nodes[:n_disc, 2] > 0.099) + & (mesh.nodes[:n_disc, 0] > 0.25) + & (mesh.nodes[:n_disc, 0] < 0.75) + & (mesh.nodes[:n_disc, 1] > 0.25) + & (mesh.nodes[:n_disc, 1] < 0.75) + )[0] + assert len(plate_top_nodes) > 0 + # The piston bottom (z=0.15) moves to z=0.09; with dhat=0.02 the plate + # top must compress to at least z = 0.09 - 0.02 + small slack. + assert disp[2, plate_top_nodes].min() < -1e-3, ( + f"Plate top did not compress under the punch " + f"(min dz = {disp[2, plate_top_nodes].min():+.4e}). " + "IPC is not coupling the rigid body to the deformable disc." + ) + + +def test_newton_third_law_on_ipc_gradient(fresh_3d_space): + pb, ipc, solid, body, mesh, n_disc, nodes_top = _build_punch_problem() + pb.nlsolve(dt=0.2, tmax=0.2, update_dt=True, print_info=0) + + # IPC barrier gradient summed onto each side via P. + P = ipc._scatter_matrix + grad_surf = ipc._barrier_potential.gradient( + ipc._collisions, ipc._collision_mesh, ipc._get_current_vertices(pb) + ) + F = -ipc._kappa * (P @ grad_surf) + + # Rigid side: 6-DOF block at body's global DOFs (forces only — first 3). + rigid_dofs = body.assembly._dof_indices[:3] + F_rigid = F[rigid_dofs] + + # Deformable side: sum the IPC Z-force at disc surface nodes. + surface_idx = ipc._surface_node_indices + disc_surf = surface_idx[np.isin(surface_idx, np.arange(n_disc))] + nvar = pb.space.nvar + F_disc = np.array([F[d * mesh.n_nodes + disc_surf].sum() for d in range(3)]) + + # Newton 3 (in the Z direction, which is where load lives): + assert abs(F_rigid[2] + F_disc[2]) < 1e-6 * max(abs(F_rigid[2]), 1.0), ( + f"Newton's 3rd law violated: F_rigid_z={F_rigid[2]:+.4e} " + f"F_disc_z={F_disc[2]:+.4e}" + ) + + +def test_rigid_tie_kinematic_error_is_zero(fresh_3d_space): + pb, ipc, solid, body, mesh, n_disc, nodes_top = _build_punch_problem() + pb.nlsolve(dt=0.2, tmax=0.2, update_dt=True, print_info=0) + + disp = pb.get_disp() # shape (3, n_nodes) + body_disp = disp[:, nodes_top].T # (n_body, 3) + + q = pb.get_dof_solution()[body.assembly._dof_indices] + R, *_ = body.constraint._compute_rotation(q[3:]) + r_ref = mesh.nodes[nodes_top] - body.center_of_mass + expected = r_ref @ R.T + body.center_of_mass + q[:3] - mesh.nodes[nodes_top] + assert np.allclose(body_disp, expected, atol=1e-9), ( + f"RigidTie kinematic relation violated: " + f"max abs error = {np.abs(body_disp - expected).max():.4e}" + ) + + +def test_static_obstacle_and_add_rigid_body_are_mutually_exclusive(fresh_3d_space): + block = fd.mesh.box_mesh( + nx=3, + ny=3, + nz=2, + x_min=0.3, + x_max=0.7, + y_min=0.3, + y_max=0.7, + z_min=0.2, + z_max=0.3, + ) + block_surf = fd.mesh.extract_surface(block, quad2tri=True) + obstacle_surf = fd.mesh.extract_surface( + fd.mesh.box_mesh( + nx=3, + ny=3, + nz=2, + x_min=0, + x_max=1, + y_min=0, + y_max=1, + z_min=-0.1, + z_max=0.0, + ), + quad2tri=True, + ) + body = fd.constraint.RigidBody( + block_surf, + mass=1.0, + inertia_tensor=0.01 * np.eye(3), + center_of_mass=block_surf.bounding_box.center, + name="b", + ) + body.set_static_obstacle(obstacle_surf, dhat=0.01, kappa=1e6) + + # Now try to plug the same body into a shared IPCContact — must refuse. + ipc = fd.constraint.IPCContact( + block, + surface_mesh=block_surf, + dhat=0.01, + dhat_is_relative=False, + use_ccd=False, + barrier_stiffness=1e6, + ) + with pytest.raises(RuntimeError, match=r"cannot use set_static_obstacle"): + ipc.add_rigid_body(body) + + +if __name__ == "__main__": + pytest.main([__file__, "-v"]) diff --git a/tests/test_rigid_body_kinematics.py b/tests/test_rigid_body_kinematics.py new file mode 100644 index 00000000..9bff9942 --- /dev/null +++ b/tests/test_rigid_body_kinematics.py @@ -0,0 +1,210 @@ +"""Unit tests for rigid body kinematics and the IPC contact Jacobian. + +Avoids ipctk dependencies by testing the RigidBodyAssembly geometry paths +(`_ipc_vertices`, `_build_ipc_jacobian`) against manual formulas. +""" + +import numpy as np +import pytest + +import fedoo as fd +from fedoo.constraint.rigid_body import RigidBodyAssembly + +from simcoon import Rotation # required dependency (fedoo imports it at load) + + +@pytest.fixture +def space(): + space = fd.ModelingSpace("3D") + space.new_variable("DispX") + space.new_variable("DispY") + space.new_variable("DispZ") + space.new_vector("Disp", ("DispX", "DispY", "DispZ")) + return space + + +def _make_assembly(rest_positions, obstacle_positions, center): + rt = fd.constraint.RigidTie(np.arange(len(rest_positions)), center=center) + rt.center = np.asarray(center, dtype=float) + asm = RigidBodyAssembly(mass=1.0, inertia_tensor=np.eye(3), rigid_tie=rt) + asm._ipc_rest_positions = np.asarray(rest_positions, dtype=float) + asm._ipc_obstacle_nodes = np.asarray(obstacle_positions, dtype=float) + asm._ipc_n_body = len(rest_positions) + return asm, rt + + +def test_ipc_vertices_pure_translation(space): + rest = np.array([[1.0, 0.0, 0.0], [0.0, 1.0, 0.0], [-1.0, 0.0, 0.0]]) + obst = np.array([[0.0, 0.0, -1.0]]) + center = np.array([0.0, 0.0, 0.0]) + asm, rt = _make_assembly(rest, obst, center) + + q = np.array([0.1, -0.2, 0.3, 0.0, 0.0, 0.0]) + verts = asm._ipc_vertices(q, rt) + + expected_body = rest + q[:3] + assert np.allclose(verts[:3], expected_body) + assert np.allclose(verts[3:], obst) + + +def test_ipc_vertices_pure_rotation_about_center(space): + rest = np.array([[1.0, 0.0, 0.0], [0.0, 1.0, 0.0], [0.0, 0.0, 1.0]]) + obst = np.zeros((0, 3)) + center = np.array([0.0, 0.0, 0.0]) + asm, rt = _make_assembly(rest, obst, center) + + # 90° about z — should rotate x into y, y into -x + q = np.array([0.0, 0.0, 0.0, 0.0, 0.0, np.pi / 2]) + verts = asm._ipc_vertices(q, rt) + + R_expected = Rotation.from_rotvec([0, 0, np.pi / 2]).as_matrix() + expected = rest @ R_expected.T + assert np.allclose(verts, expected, atol=1e-12) + + +def test_ipc_vertices_rotation_off_center(space): + rest = np.array([[1.0, 0.0, 0.0]]) + obst = np.zeros((0, 3)) + center = np.array([0.5, 0.0, 0.0]) + asm, rt = _make_assembly(rest, obst, center) + + # 180° about z around (0.5,0,0) maps (1,0,0) -> (0,0,0) + q = np.array([0.0, 0.0, 0.0, 0.0, 0.0, np.pi]) + verts = asm._ipc_vertices(q, rt) + assert np.allclose(verts[0], [0.0, 0.0, 0.0], atol=1e-12) + + +def test_build_ipc_jacobian_structure(space): + rest = np.array([[1.0, 0.0, 0.0], [0.0, 1.0, 0.0]]) + obst = np.array([[0.0, 0.0, -1.0], [1.0, 0.0, -1.0]]) + center = np.array([0.0, 0.0, 0.0]) + asm, rt = _make_assembly(rest, obst, center) + + J = asm._build_ipc_jacobian(rt, np.zeros(3)) + + assert J.shape == (12, 6) + # Translation block: body rows are identity per vertex; obstacle rows zero. + for i in range(2): + for d in range(3): + assert J[i * 3 + d, d] == 1.0 + assert np.all(J[6:, :3] == 0.0) # obstacle rows zero in translation cols + assert np.all(J[6:, 3:] == 0.0) # obstacle rows zero in rotation cols + + +def test_build_ipc_jacobian_matches_finite_difference(space): + rest = np.array( + [[1.0, 0.0, 0.0], [0.0, 1.0, 0.0], [0.0, 0.0, 1.0], [1.0, 1.0, 0.0]] + ) + obst = np.zeros((0, 3)) + center = np.array([0.2, -0.1, 0.0]) + asm, rt = _make_assembly(rest, obst, center) + + angles0 = np.array([0.1, -0.15, 0.2]) + J = asm._build_ipc_jacobian(rt, angles0) + + # Numerically differentiate vertex positions w.r.t. q = [dx,dy,dz,rx,ry,rz] + def vertex_positions(q): + return asm._ipc_vertices(q, rt).ravel() + + q0 = np.concatenate([np.zeros(3), angles0]) + h = 1e-6 + for k in range(6): + eps = np.zeros(6) + eps[k] = h + du_dq = (vertex_positions(q0 + eps) - vertex_positions(q0 - eps)) / (2 * h) + assert np.allclose(J[:, k], du_dq, atol=1e-6), f"column {k} mismatch" + + +def _make_rigid_body(dynamic=True): + nodes = np.array( + [ + [-0.5, -0.5, 0.0], + [0.5, -0.5, 0.0], + [0.5, 0.5, 0.0], + [-0.5, 0.5, 0.0], + ] + ) + mesh = fd.Mesh(nodes, np.array([[0, 1, 2, 3]]), "quad4") + kwargs = {} + if dynamic: + kwargs = {"mass": 1.0, "inertia_tensor": np.eye(3)} + return fd.constraint.RigidBody(mesh, dynamic=dynamic, **kwargs) + + +def test_rigid_body_rayleigh_damping_accepts_mass_and_stiffness_terms(space): + body = _make_rigid_body() + + body.set_rayleigh_damping(alpha=0.7, beta=0.2) + + assert body.assembly.dissipation == fd.time.RayleighDamping(alpha=0.7, beta=0.2) + + +def test_static_body_is_not_compiled_by_second_order_integrator(space): + body = _make_rigid_body(dynamic=False) + pb = fd.problem.NonLinear(body.assembly) + + assert body.constraint in pb.bc + assert pb.n_global_dof == 6 + + pb.set_time_integrator(fd.time.SECOND_ORDER, fd.time.Newmark()) + + pb.initialize() + + assert body.assembly.time_evolution is None + assert body.assembly.storage is None + assert body.assembly._time_integrator is None + assert not body.assembly._fedoo_time_integrated + + +def test_rigid_body_integrator_is_recompiled_for_each_problem(space): + body = _make_rigid_body() + + first = fd.problem.NonLinear(body.assembly) + first.set_time_integrator( + fd.time.SECOND_ORDER, fd.time.Newmark(beta=0.2, gamma=0.55) + ) + first.initialize() + first_adapter = body.assembly._time_integrator + assert first_adapter.beta == pytest.approx(0.2) + + second = fd.problem.NonLinear(body.assembly) + second.set_time_integrator( + fd.time.SECOND_ORDER, fd.time.Newmark(beta=0.3, gamma=0.6) + ) + second.initialize() + assert body.assembly._time_integrator is not first_adapter + assert body.assembly._time_integrator.beta == pytest.approx(0.3) + + static = fd.problem.NonLinear(body.assembly) + with pytest.warns(UserWarning, match="Assembly provider"): + static.initialize() + assert body.assembly._time_integrator is None + assert not body.assembly._fedoo_time_integrated + + +def test_problem_set_start_commits_quaternion_increment(space): + body = _make_rigid_body(dynamic=False) + pb = fd.problem.NonLinear(body.assembly) + pb.initialize() + rotation_indices = body.assembly.dof_indices[3:] + + first = np.array([0.2, 0.0, 0.0]) + pb._dU = np.zeros(pb.n_dof) + pb._dU[rotation_indices] = first + pb.set_start() + + second = np.array([0.0, -0.3, 0.1]) + pb._dU = np.zeros(pb.n_dof) + pb._dU[rotation_indices] = second + pb.set_start() + + expected = Rotation.from_rotvec(second) * Rotation.from_rotvec(first) + quaternion = body.constraint.Q_total.as_quat() + expected_quaternion = expected.as_quat() + assert np.allclose(quaternion, expected_quaternion) or np.allclose( + quaternion, -expected_quaternion + ) + + +if __name__ == "__main__": + pytest.main([__file__]) diff --git a/tests/test_rigid_tie_2d.py b/tests/test_rigid_tie_2d.py new file mode 100644 index 00000000..3ad9a096 --- /dev/null +++ b/tests/test_rigid_tie_2d.py @@ -0,0 +1,49 @@ +import numpy as np +from scipy import sparse + +import fedoo as fd + + +def test_rigid_tie_2d_helpers_and_array_like_center(): + space = fd.ModelingSpace("2D") + space.new_variable("DispX") + space.new_variable("DispY") + space.new_vector("Disp", ("DispX", "DispY")) + + nodes = np.array( + [ + [-1.0, -0.5], + [1.0, -0.5], + [1.0, 0.5], + [-1.0, 0.5], + ] + ) + mesh = fd.Mesh(nodes, np.array([[0, 1, 2, 3]]), "quad4") + problem = fd.Problem( + A=sparse.eye(mesh.n_nodes * space.nvar), mesh=mesh, space=space + ) + + center = [0.2, -0.1] + tied_nodes = np.arange(mesh.n_nodes) + control = fd.constraint.RigidTie2D(tied_nodes, center=center) + problem.bc.add(control) + problem.bc.add("Dirichlet", "RigidDispX", 0.3) + problem.bc.add("Dirichlet", "RigidDispY", -0.2) + problem.bc.add("Dirichlet", "RigidRotZ", 0.4) + problem._U = np.zeros(problem.n_dof) + problem._dU = np.zeros(problem.n_dof) + + problem.apply_boundary_conditions() + + assert isinstance(control.center, np.ndarray) + rotation, _ = control._compute_rotation(0.4) + expected = ( + (nodes - control.center) @ rotation.T + + control.center + + np.array([0.3, -0.2]) + - nodes + ) + np.testing.assert_allclose(problem._dU[control._disp_indices], expected) + + problem._dU[:] = 0.0 + control.pre_update(problem) diff --git a/tests/test_rigid_tie_derivatives.py b/tests/test_rigid_tie_derivatives.py new file mode 100644 index 00000000..74f64404 --- /dev/null +++ b/tests/test_rigid_tie_derivatives.py @@ -0,0 +1,104 @@ +"""Finite-difference validation of RigidTie rotation derivatives.""" + +import numpy as np +import pytest + +from fedoo.constraint.rigid_tie import RigidTie +from simcoon import Rotation + + +def _R(omega): + """Rotation matrix via the tie's total-rotation-vector mode.""" + rt = RigidTie([0, 1], use_quaternion=False) + R, _, _, _ = rt._compute_rotation(np.asarray(omega, dtype=float)) + return R + + +def _dR_analytic(omega): + rt = RigidTie([0, 1], use_quaternion=False) + _, *dR = rt._compute_rotation(np.asarray(omega, dtype=float)) + return np.stack(dR, axis=0) + + +def _dR_fd(omega, h=1e-6): + omega = np.asarray(omega, dtype=float) + out = np.empty((3, 3, 3)) + for k in range(3): + eps = np.zeros(3) + eps[k] = h + out[k] = (_R(omega + eps) - _R(omega - eps)) / (2 * h) + return out + + +@pytest.mark.parametrize( + "omega", + [ + np.zeros(3), + np.array([1e-12, 0.0, 0.0]), + np.array([0.1, 0.0, 0.0]), + np.array([0.0, 0.2, 0.0]), + np.array([0.0, 0.0, 0.3]), + np.array([0.3, -0.4, 0.5]), + np.array([np.pi / 4, np.pi / 6, -np.pi / 8]), + ], +) +def test_dR_drotvec_matches_finite_difference(omega): + analytic = _dR_analytic(omega) + numeric = _dR_fd(omega) + # FD error ~ O(h^2) with h=1e-6 → ~1e-8 in smooth regime + assert np.allclose(analytic, numeric, atol=1e-6) + + +def test_dR_drotvec_zero_limit_is_skew_basis(): + rt = RigidTie([0, 1], use_quaternion=False) + _, *dR = rt._compute_rotation(np.zeros(3)) + expected = ( + np.array([[0, 0, 0], [0, 0, -1], [0, 1, 0]]), + np.array([[0, 0, 1], [0, 0, 0], [-1, 0, 0]]), + np.array([[0, -1, 0], [1, 0, 0], [0, 0, 0]]), + ) + for got, exp in zip(dR, expected): + assert np.array_equal(got, exp) + + +def test_compute_rotation_identity_at_zero(): + rt = RigidTie([0, 1]) + R, _, _, _ = rt._compute_rotation(np.zeros(3)) + assert np.allclose(R, np.eye(3)) + + +def test_compute_rotation_is_orthogonal(): + rt = RigidTie([0, 1]) + for omega in [ + np.array([0.3, -0.4, 0.5]), + np.array([1.2, 0.0, 0.0]), + np.array([0.0, 2.5, -1.5]), + ]: + R, _, _, _ = rt._compute_rotation(omega) + assert np.allclose(R @ R.T, np.eye(3), atol=1e-12) + assert np.isclose(np.linalg.det(R), 1.0, atol=1e-12) + + +def test_incremental_derivatives_match_finite_difference_after_base_rotation(): + tie = RigidTie([0, 1], use_quaternion=True) + tie._Q_base = Rotation.from_rotvec([0.3, -0.2, 0.1]) + tie._angles_at_base = np.array([0.4, 0.1, -0.2]) + angles = tie._angles_at_base + np.array([0.05, -0.04, 0.03]) + _, *derivatives = tie._compute_rotation(angles) + + h = 1e-7 + numerical = [] + for axis in range(3): + perturbation = np.zeros(3) + perturbation[axis] = h + rotation_plus = tie._compute_rotation(angles + perturbation)[0] + rotation_minus = tie._compute_rotation(angles - perturbation)[0] + numerical.append((rotation_plus - rotation_minus) / (2.0 * h)) + + np.testing.assert_allclose( + np.stack(derivatives), np.stack(numerical), rtol=1e-7, atol=1e-7 + ) + + +if __name__ == "__main__": + pytest.main([__file__]) diff --git a/tests/test_rigid_tie_rollback.py b/tests/test_rigid_tie_rollback.py new file mode 100644 index 00000000..605eaf93 --- /dev/null +++ b/tests/test_rigid_tie_rollback.py @@ -0,0 +1,67 @@ +"""Lifecycle tests for multiplicative quaternion rotation updates.""" + +import numpy as np + +from fedoo.constraint.rigid_tie import RigidTie +from simcoon import Rotation + + +def _make_tie(angles): + tie = RigidTie(np.array([0, 1]), use_quaternion=True) + tie._Q_base = Rotation.identity() + tie._angles_at_base = np.zeros(3) + calls = iter(np.asarray(value, dtype=float) for value in angles) + + def get_dof_ref(_problem): + return np.concatenate([np.zeros(3), next(calls)]), None + + tie._get_dof_ref = get_dof_ref + return tie + + +def _same_rotation(left, right, atol=1e-14): + left_quaternion = left.as_quat() + right_quaternion = right.as_quat() + return np.allclose(left_quaternion, right_quaternion, atol=atol) or np.allclose( + left_quaternion, -right_quaternion, atol=atol + ) + + +def test_successive_increments_compose_multiplicatively(): + first = np.array([0.1, 0.0, 0.0]) + second = np.array([0.1, 0.2, 0.0]) + tie = _make_tie([first, second]) + + tie.set_start(None) + tie.set_start(None) + + expected = Rotation.from_rotvec(second - first) * Rotation.from_rotvec(first) + assert _same_rotation(tie.Q_total, expected) + np.testing.assert_array_equal(tie._angles_at_base, second) + + +def test_failed_increment_keeps_last_converged_orientation(): + converged = np.array([0.3, -0.4, 0.5]) + tie = _make_tie([converged]) + tie.set_start(None) + orientation = Rotation.from_quat(tie.Q_total.as_quat()) + + tie.to_start(None) + tie.to_start(None) + + assert _same_rotation(tie.Q_total, orientation) + + +def test_total_rotvec_mode_does_not_create_quaternion_state(): + tie = RigidTie(np.array([0, 1]), use_quaternion=False) + rotation, _, _, _ = tie._compute_rotation(np.array([0.2, -0.1, 0.4])) + + expected = Rotation.from_rotvec([0.2, -0.1, 0.4]).as_matrix() + np.testing.assert_allclose(rotation, expected) + assert tie.Q_total is None + + +def test_non_finite_increment_does_not_mutate_orientation(): + tie = _make_tie([[np.nan, 0.0, 0.0]]) + tie.set_start(None) + assert _same_rotation(tie.Q_total, Rotation.identity())