[BUG FIX] Add pressure-based buoyancy force transfer in SPH-rigid coupling (#2070)

genesis/engine/couplers/legacy_coupler.py

@@ -222,6 +222,8 @@ def _func_collide_with_rigid_geom_robust(
         pos_world,
         vel,
         mass,
+        particle_p,
+        particle_rest_rho,
         normal_prev,
         geom_idx,
         batch_idx,
@@ -274,6 +276,21 @@ def _func_collide_with_rigid_geom_robust(
                 rigid_global_info,
             )
 
+        # Pressure-based buoyancy force transfer: even when the particle is not actively colliding
+        # (rvel_normal_magnitude >= 0), its static pressure still exerts force on the rigid body.
+        # This is essential for correct buoyancy of submerged objects.
+        if signed_dist < 0 and particle_p > 0:
+            V = mass / particle_rest_rho
+            h = self.sph_solver._support_radius
+            buoyancy_force = -particle_p * V / h * normal_rigid
+            self.rigid_solver._func_apply_coupling_force(
+                pos_world,
+                buoyancy_force,
+                geoms_info.link_idx[geom_idx],
+                batch_idx,
+                links_state,
+            )
+
         # attraction force
         # if 0.001 < signed_dist < 0.01:
         #     vel = vel - normal_rigid * 0.1 * signed_dist
@@ -721,6 +738,8 @@ def sph_rigid(
                             self.sph_solver.particles_reordered[i_p, i_b].pos,
                             self.sph_solver.particles_reordered[i_p, i_b].vel,
                             self.sph_solver.particles_info_reordered[i_p, i_b].mass,
+                            self.sph_solver.particles_reordered[i_p, i_b].p,
+                            self.sph_solver.particles_info_reordered[i_p, i_b].rho,
                             self.sph_rigid_normal_reordered[i_p, i_g, i_b],
                             i_g,
                             i_b,

genesis/engine/solvers/sph_solver.py

@@ -632,6 +632,24 @@ def _density_solve(self, f: qd.i32):
 
         gs.logger.debug(f"DFSPH - iterations: {iteration} Avg density err: {avg_density_err:.4f} kg/m^3")
 
+    @qd.kernel
+    def _kernel_compute_dfsph_pressure(self):
+        """
+        Extract implicit pressure from DFSPH density solve residual and write to particles.p.
+
+        DFSPH does not compute explicit pressure via equation of state (unlike WCSPH). Instead, it
+        solves for velocity corrections that enforce incompressibility. The implicit pressure is
+        derived from the density residual (drho - 1.0) and the DFSPH factor (which encodes the
+        inverse squared kernel gradient sum, already scaled by 1/dt^2 during _density_solve):
+            p = -(drho - 1.0) * dfsph_factor * rho0
+        This gives p > 0 when the particle is compressed (drho > 1.0).
+        """
+        for i_p, i_b in qd.ndrange(self._n_particles, self._B):
+            if self.particles_ng_reordered[i_p, i_b].active:
+                rho0 = self.particles_info_reordered[i_p, i_b].rho
+                b_i = self.particles_reordered[i_p, i_b].drho - 1.0
+                self.particles_reordered[i_p, i_b].p = -b_i * self.particles_reordered[i_p, i_b].dfsph_factor * rho0
+
     # ------------------------------------------------------------------------------------
     # ------------------------------------- utils ----------------------------------------
     # ------------------------------------------------------------------------------------
@@ -701,6 +719,7 @@ def substep_pre_coupling(self, f):
                 self._kernel_compute_non_pressure_forces(f, self._sim.cur_t)
                 self._kernel_predict_velocity(f)
                 self._density_solve(f)
+                self._kernel_compute_dfsph_pressure()
 
     def substep_pre_coupling_grad(self, f):
         pass

tests/test_sph_buoyancy.py

@@ -0,0 +1,142 @@
+"""
+Analytical integration tests for SPH-rigid pressure-based buoyancy coupling.
+
+Validates that ``particles.p`` is populated with physically meaningful pressure
+values and that the rigid-body coupler transmits the resulting forces.
+
+1. Heavy-cube sinking
+   -------------------
+   A submerged rigid cube with density *rho_c > rho_f* sinks at less than
+   free-fall acceleration due to buoyancy + drag + collision response.
+   The free-fall displacement over *N* steps provides a rigorous upper bound.
+
+2. Hydrostatic pressure gradient (WCSPH)
+   --------------------------------------
+   For a settled column of WCSPH fluid the pressure approximately follows
+
+       dp/dz ~ rho * g   (≈ 9800 Pa/m),
+
+   confirming that ``particles.p`` holds physically meaningful values.
+"""
+
+import numpy as np
+import pytest
+
+import genesis as gs
+
+
+# ---------------------------------------------------------------------------
+# Heavy-cube sinking — upper bound: free-fall displacement
+# ---------------------------------------------------------------------------
+
+_RHO_FLUID = 1000.0
+_RHO_CUBE_HEAVY = 2000.0  # 2× fluid density
+_HEAVY_DT = 2e-3
+_HEAVY_SUBSTEPS = 4
+_HEAVY_N_STEPS = 100
+_T_SIM = _HEAVY_N_STEPS * _HEAVY_DT * _HEAVY_SUBSTEPS
+_FREE_FALL_DISP = 0.5 * 9.8 * _T_SIM ** 2  # m
+
+
+@pytest.mark.required
+def test_heavy_cube_sinks_slower_than_free_fall(show_viewer):
+    """A rigid cube (rho_c = 2 * rho_f) must sink significantly less than
+    the free-fall displacement, confirming the SPH-rigid coupling applies
+    a net upward force (collision + pressure buoyancy)."""
+    scene = gs.Scene(
+        sim_options=gs.options.SimOptions(dt=_HEAVY_DT, substeps=_HEAVY_SUBSTEPS),
+        sph_options=gs.options.SPHOptions(
+            lower_bound=(-0.5, -0.5, 0.0),
+            upper_bound=(0.5, 0.5, 1.0),
+            particle_size=0.02,
+            pressure_solver="WCSPH",
+        ),
+        show_viewer=show_viewer,
+    )
+    scene.add_entity(morph=gs.morphs.Plane())
+    scene.add_entity(
+        material=gs.materials.SPH.Liquid(sampler="regular", mu=0.05),
+        morph=gs.morphs.Box(pos=(0.0, 0.0, 0.35), size=(0.4, 0.4, 0.5)),
+    )
+    cube = scene.add_entity(
+        material=gs.materials.Rigid(rho=_RHO_CUBE_HEAVY),
+        morph=gs.morphs.Box(pos=(0.0, 0.0, 0.25), size=(0.1, 0.1, 0.1)),
+    )
+    scene.build()
+
+    init_z = float(cube.get_pos()[2])
+    for _ in range(_HEAVY_N_STEPS):
+        scene.step()
+    final_z = float(cube.get_pos()[2])
+    displacement = abs(final_z - init_z)
+
+    assert displacement < _FREE_FALL_DISP * 0.8, (
+        f"Displacement {displacement:.4f} m exceeds 80 % of free-fall "
+        f"({_FREE_FALL_DISP:.4f} m) — coupling force is missing."
+    )
+
+
+# ---------------------------------------------------------------------------
+# Hydrostatic pressure gradient in WCSPH
+# ---------------------------------------------------------------------------
+
+_MIN_CENTRE_PARTICLES = 8
+
+
+@pytest.mark.required
+def test_wcsph_hydrostatic_pressure_gradient(show_viewer):
+    """The WCSPH pressure field in a settled column should approximately
+    follow dp/dz = rho * g ≈ 9800 Pa/m."""
+    scene = _build_fluid_only_scene(show_viewer, "WCSPH")
+    sph = scene.sph_solver
+
+    p = sph.particles_reordered.p.to_numpy()[:, 0]
+    pos = sph.particles_reordered.pos.to_numpy()[:, 0, :]
+    x, y, z = pos[:, 0], pos[:, 1], pos[:, 2]
+
+    centre = (np.abs(x) < 0.04) & (np.abs(y) < 0.04) & (z > 0.02) & (z < 0.7)
+    z_c, p_c = z[centre], p[centre]
+
+    if len(z_c) < _MIN_CENTRE_PARTICLES:
+        pytest.skip(f"Too few centre-column particles ({len(z_c)}).")
+
+    z_surface = z_c.max()
+    depths = np.maximum(z_surface - z_c, 1e-12)
+    weights = np.clip(depths / depths.max(), 0.1, 1.0)
+    slope = np.polyfit(depths, p_c, 1, w=weights)[0]
+    ratio = slope / (1000.0 * 9.8)
+
+    assert 0.3 < ratio < 2.5, (
+        f"Pressure gradient dp/dz = {slope:.0f} Pa/m "
+        f"(expected ~9800 Pa/m, ratio={ratio:.2f})."
+    )
+
+
+# ---------------------------------------------------------------------------
+# Helper
+# ---------------------------------------------------------------------------
+
+
+def _build_fluid_only_scene(show_viewer, pressure_solver):
+    """Build and settle a fluid-only scene for pressure-field analysis."""
+    scene = gs.Scene(
+        sim_options=gs.options.SimOptions(dt=2e-3, substeps=4),
+        sph_options=gs.options.SPHOptions(
+            lower_bound=(-0.5, -0.5, 0.0),
+            upper_bound=(0.5, 0.5, 1.0),
+            particle_size=0.015,
+            pressure_solver=pressure_solver,
+        ),
+        show_viewer=show_viewer,
+    )
+    scene.add_entity(morph=gs.morphs.Plane())
+    scene.add_entity(
+        material=gs.materials.SPH.Liquid(sampler="regular", mu=0.05),
+        morph=gs.morphs.Box(pos=(0.0, 0.0, 0.4), size=(0.4, 0.4, 0.6)),
+    )
+    scene.build()
+
+    for _ in range(800):
+        scene.step()
+
+    return scene