diff --git a/.gitattributes b/.gitattributes index 0d7e218e2..7be42c85d 100644 --- a/.gitattributes +++ b/.gitattributes @@ -1,3 +1,3 @@ # These files should be ignored from GitHub's language statistics. -python/example.ipynb linguist-vendored +python/examples/*.ipynb linguist-vendored notebooks/*.ipynb linguist-vendored \ No newline at end of file diff --git a/.github/workflows/clang-format-check.yml b/.github/workflows/clang-format-check.yml index f3a5dd499..c7fbb5cb6 100644 --- a/.github/workflows/clang-format-check.yml +++ b/.github/workflows/clang-format-check.yml @@ -5,6 +5,7 @@ on: - '.github/workflows/clang-format-check.yml' - '.clang-format' - 'src/**' + - 'demo/**' - 'tests/src/**' - 'python/src/**' pull_request: @@ -12,6 +13,7 @@ on: - '.github/workflows/clang-format-check.yml' - '.clang-format' - 'src/**' + - 'demo/**' - 'tests/src/**' - 'python/src/**' jobs: @@ -22,6 +24,7 @@ jobs: matrix: path: - 'src' + - 'demo' - 'tests/src' - 'python/src' steps: diff --git a/.github/workflows/continuous.yml b/.github/workflows/continuous.yml index efcfcde4e..a2dc3995e 100644 --- a/.github/workflows/continuous.yml +++ b/.github/workflows/continuous.yml @@ -130,5 +130,8 @@ jobs: run: | cmake --build build -j ${{ steps.cpu-cores.outputs.count }} && ccache --show-stats + # We raise CTest's default 1500 s per-test timeout: "ESP potential 2D + # finite differences" alone takes ~1000 s in Windows Debug, and running + # alongside the demo simulator tests pushes it past the default. - name: Tests - run: cd build; ctest --verbose -j ${{ steps.cpu-cores.outputs.count }} + run: cd build; ctest --verbose --timeout 3600 -j ${{ steps.cpu-cores.outputs.count }} diff --git a/.github/workflows/coverage.yml b/.github/workflows/coverage.yml index e05462490..4355362ba 100644 --- a/.github/workflows/coverage.yml +++ b/.github/workflows/coverage.yml @@ -7,6 +7,7 @@ on: paths: - '.github/workflows/coverage.yml' - 'cmake/**' + - 'demo/**' - 'src/**' - 'tests/**' - 'CMakeLists.txt' @@ -65,6 +66,8 @@ jobs: cd build cmake .. \ -DIPC_TOOLKIT_BUILD_TESTS=ON \ + -DIPC_TOOLKIT_BUILD_DEMO=ON \ + -DIPC_TOOLKIT_WITH_GLTF=ON \ -DIPC_TOOLKIT_WITH_CODE_COVERAGE=ON \ -DSIMD_CXX_FLAGS=-march=x86-64-v3 \ -DCMAKE_BUILD_TYPE=Release diff --git a/.gitignore b/.gitignore index 149693837..e40f7b792 100644 --- a/.gitignore +++ b/.gitignore @@ -676,4 +676,7 @@ CMakeGraphVizOptions.cmake .claude graphify-out -__cmake_systeminformation \ No newline at end of file +__cmake_systeminformation +simulator_test.glb +**/.polyscope.ini +**/imgui.ini diff --git a/CMakeLists.txt b/CMakeLists.txt index 3f190f539..fdd5610ec 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -82,10 +82,12 @@ if(IPC_TOOLKIT_TOPLEVEL_PROJECT) endif() endif() -# If this is not the top-level project, we don't build tests or Python bindings -# by default. This is useful for projects that use IPC Toolkit as a submodule. -option(IPC_TOOLKIT_BUILD_TESTS "Build unit-tests" ${IPC_TOOLKIT_TOPLEVEL_PROJECT}) -option(IPC_TOOLKIT_BUILD_PYTHON "Build Python bindings" OFF) +# If this is not the top-level project, we don't build tests, Python bindings, +# or the demo simulator by default. This is useful for projects that use IPC +# Toolkit as a submodule. +option(IPC_TOOLKIT_BUILD_TESTS "Build unit-tests" ${IPC_TOOLKIT_TOPLEVEL_PROJECT}) +option(IPC_TOOLKIT_BUILD_PYTHON "Build Python bindings" OFF) +option(IPC_TOOLKIT_BUILD_DEMO "Build the demo simulator" ${IPC_TOOLKIT_TOPLEVEL_PROJECT}) option(IPC_TOOLKIT_WITH_CUDA "Enable CUDA CCD" OFF) option(IPC_TOOLKIT_WITH_SIMD "Enable SIMD" ON) @@ -93,6 +95,7 @@ option(IPC_TOOLKIT_WITH_RATIONAL_INTERSECTION "Use rational edge-triangle inters option(IPC_TOOLKIT_WITH_ROBIN_MAP "Use Tessil's robin-map rather than std maps" ON) option(IPC_TOOLKIT_WITH_ABSEIL "Use Abseil's hash functions" ON) option(IPC_TOOLKIT_WITH_FILIB "Use filib for interval arithmetic" ON) +option(IPC_TOOLKIT_WITH_GLTF "Enable glTF I/O (tinygltf)" OFF) option(IPC_TOOLKIT_WITH_INEXACT_CCD "Use the original inexact CCD method of IPC" OFF) option(IPC_TOOLKIT_WITH_PROFILER "Enable performance profiler" OFF) option(IPC_TOOLKIT_WITH_TRACY "Enable Tracy frame profiler" OFF) @@ -293,6 +296,12 @@ include(tinyad) # TODO: Make this a private dependency once we stop exposing TinyAD types in the public API. target_link_libraries(ipc_toolkit PUBLIC TinyAD::TinyAD) +# TinyGLTF +if(IPC_TOOLKIT_WITH_GLTF) + include(tinygltf) + target_link_libraries(ipc_toolkit PRIVATE tinygltf::tinygltf) +endif() + # CCD if(IPC_TOOLKIT_WITH_INEXACT_CCD) # Etienne Vouga's CTCD Library for the floating point root finding algorithm @@ -433,6 +442,16 @@ endif() # Use C++17 target_compile_features(ipc_toolkit PUBLIC cxx_std_17) +################################################################################ +# Demo simulator (application-side code: solver, problem glue, stepping loop) +################################################################################ + +# Added before the tests and Python bindings so both can optionally include +# the demo's tests and bindings. +if(IPC_TOOLKIT_BUILD_DEMO) + add_subdirectory(demo) +endif() + ################################################################################ # Tests ################################################################################ @@ -460,6 +479,17 @@ if(IPC_TOOLKIT_WITH_CODE_COVERAGE AND CMAKE_CXX_COMPILER_ID MATCHES "GNU|Clang") --coverage -fprofile-update=atomic ) + if(IPC_TOOLKIT_BUILD_DEMO) + target_compile_options(ipc_toolkit_demo PRIVATE + -g # generate debug info + --coverage # sets all required flags + -fprofile-update=atomic + ) + target_link_options(ipc_toolkit_demo PUBLIC + --coverage + -fprofile-update=atomic + ) + endif() endif() ################################################################################ diff --git a/CMakePresets.json b/CMakePresets.json index 437a9ecb1..022983f5c 100644 --- a/CMakePresets.json +++ b/CMakePresets.json @@ -72,7 +72,8 @@ "cacheVariables": { "IPC_TOOLKIT_BUILD_PYTHON": "ON", "IPC_TOOLKIT_BUILD_TESTS": "OFF", - "IPC_TOOLKIT_WITH_CUDA": "OFF" + "IPC_TOOLKIT_WITH_CUDA": "ON", + "IPC_TOOLKIT_WITH_GLTF": "ON" } }, { diff --git a/README.md b/README.md index cc4549551..e3c3626c9 100644 --- a/README.md +++ b/README.md @@ -26,7 +26,9 @@ IPC Toolkit is a set of reusable functions to integrate Incremental Potential Co ### Limitations -This is not a full simulation library. As such, it does not include any physics or solvers. For a full simulation implementation, we recommend [PolyFEM](https://polyfem.github.io/) (a finite element library) or [Rigid IPC](https://github.com/ipc-sim/rigid-ipc) (rigid-body dynamics), both of which utilize the IPC Toolkit. +This is not a full simulation library. As such, it does not include any solvers. For a full simulation implementation, we recommend [PolyFEM](https://polyfem.github.io/) (a finite element library) or [Rigid IPC](https://github.com/ipc-sim/rigid-ipc) (rigid-body dynamics), both of which utilize the IPC Toolkit. + +A small demo simulator ([`demo/`](demo)) shows how to assemble the toolkit's components (energies, time integration, and CCD) into a rigid/affine body simulator driven by [polysolve](https://github.com/polyfem/polysolve). ## Build diff --git a/cmake/recipes/polysolve.cmake b/cmake/recipes/polysolve.cmake new file mode 100644 index 000000000..927ff0ec3 --- /dev/null +++ b/cmake/recipes/polysolve.cmake @@ -0,0 +1,62 @@ +# PolySolve (https://github.com/polyfem/polysolve) +# License: MIT +if(TARGET polysolve::polysolve) + return() +endif() + +message(STATUS "Third-party: creating target 'polysolve::polysolve'") + +# Pre-register shared dependencies so this project's pins win over polysolve's +# older ones (both recipe sets guard on the existing targets): finite-diff +# 1.0.4 vs 1.0.2 (the tests need fd::finite_jacobian_tensor), nlohmann/json +# 3.12 vs 3.11, and spdlog 1.17.0 vs 1.9.2. +include(finite_diff) +include(json) +include(spdlog) + +include(CPM) +CPMAddPackage( + URI "gh:polyfem/polysolve#1b01e6cec13c6813ae48846d1f98654bbbf1402b" + OPTIONS + # Only the Eigen built-in linear solvers are needed for the demo simulator; + # disable the optional heavyweight backends and their dependency tails. + # (Accelerate also conflicts with Eigen's BLAS prototypes on recent macOS SDKs.) + "POLYSOLVE_WITH_ACCELERATE OFF" + "POLYSOLVE_WITH_AMGCL OFF" + "POLYSOLVE_WITH_CHOLMOD OFF" + "POLYSOLVE_WITH_HYPRE OFF" + "POLYSOLVE_WITH_MKL OFF" + "POLYSOLVE_WITH_PARDISO OFF" + "POLYSOLVE_WITH_SPECTRA OFF" + "POLYSOLVE_WITH_SPQR OFF" + "POLYSOLVE_WITH_SUPERLU OFF" + "POLYSOLVE_WITH_TESTS OFF" + "POLYSOLVE_WITH_UMFPACK OFF" +) + +# polysolve always lists its cuSolver backend (CuSolverDN.cu) as a source, even +# with POLYSOLVE_WITH_CUSOLVER off. That is harmless until we enable the CUDA +# language for our own kernels: CMake then compiles the file with nvcc, under +# polysolve's directory-wide MSVC flags (/experimental:external, /FS, ...), +# which nvcc reads as extra input files and rejects on Windows. We never use +# that backend, so we keep the file out of the build. +set_source_files_properties( + "${polysolve_SOURCE_DIR}/src/polysolve/linear/CuSolverDN.cu" + TARGET_DIRECTORY polysolve_linear + PROPERTIES HEADER_FILE_ONLY ON) + +# polysolve also hands consumers a bare /utf-8 on MSVC, which would reach nvcc +# in our CUDA tests the same way. We limit it to C++ sources; CUDA sources +# already get -Xcompiler=/utf-8 from ipc_toolkit. +get_target_property(_polysolve_options polysolve_linear INTERFACE_COMPILE_OPTIONS) +if(_polysolve_options) + list(TRANSFORM _polysolve_options + REPLACE "^/utf-8$" "$<$:/utf-8>") + set_target_properties(polysolve_linear PROPERTIES + INTERFACE_COMPILE_OPTIONS "${_polysolve_options}") +endif() +unset(_polysolve_options) + +# Folder name for IDE +set_target_properties(polysolve PROPERTIES FOLDER "ThirdParty") +set_target_properties(polysolve_linear PROPERTIES FOLDER "ThirdParty") diff --git a/cmake/recipes/tinygltf.cmake b/cmake/recipes/tinygltf.cmake new file mode 100644 index 000000000..d6b384ddf --- /dev/null +++ b/cmake/recipes/tinygltf.cmake @@ -0,0 +1,21 @@ +# TinyGLTF (https://github.com/syoyo/tinygltf) +# License: MIT +if(TARGET tinygltf::tinygltf) + return() +endif() + +message(STATUS "Third-party: creating target 'tinygltf::tinygltf'") + +include(CPM) +CPMAddPackage( + URI "gh:syoyo/tinygltf@2.9.6" + DOWNLOAD_ONLY TRUE +) + +add_library(tinygltf) +add_library(tinygltf::tinygltf ALIAS tinygltf) +target_sources(tinygltf PRIVATE ${tinygltf_SOURCE_DIR}/tiny_gltf.cc) +target_include_directories(tinygltf INTERFACE + $ + $ +) \ No newline at end of file diff --git a/demo/CMakeLists.txt b/demo/CMakeLists.txt new file mode 100644 index 000000000..79e8618d1 --- /dev/null +++ b/demo/CMakeLists.txt @@ -0,0 +1,52 @@ +################################################################################ +# IPC Toolkit Demo Simulator +# +# Application-side code intentionally kept out of the core library: the +# incremental potential presented to the (polysolve) nonlinear solver and the +# time-stepping loop. The core library only provides the modular components +# (energies, time integrators, potentials, and CCD); this target is the living +# proof that they are easy to assemble into a simulator. +################################################################################ + +set(SOURCES + src/kinematic_driver.cpp + src/kinematic_driver.hpp + src/kinematics.cpp + src/kinematics.hpp + src/simulator.cpp + src/simulator.hpp +) + +add_library(ipc_toolkit_demo ${SOURCES}) +add_library(ipc::toolkit::demo ALIAS ipc_toolkit_demo) + +target_include_directories(ipc_toolkit_demo PUBLIC "${CMAKE_CURRENT_SOURCE_DIR}/src") + +# The demo depends on the core library; the core library must never depend on +# the demo (or on any solver). +target_link_libraries(ipc_toolkit_demo PUBLIC ipc::toolkit) + +# polysolve (nonlinear solver with PolyFEM-grade Newton + line search). +# PUBLIC: the Simulator IS a polysolve::nonlinear::Problem. +include(polysolve) +target_link_libraries(ipc_toolkit_demo PUBLIC polysolve::polysolve) + +target_compile_features(ipc_toolkit_demo PUBLIC cxx_std_17) + +if(IPC_TOOLKIT_WITH_CUDA) + # The demo has no CUDA sources, but CMake still device-links it because it + # links the separably compiled core library. That bakes a resolved copy of + # the core's device code into libipc_toolkit_demo.a, and nvlink then fails to + # resolve the core's device functions when an executable (e.g. the tests) + # links both. We leave device linking to the final executable instead. + set_target_properties(ipc_toolkit_demo PROPERTIES CUDA_RESOLVE_DEVICE_SYMBOLS OFF) +endif() + +# Folder name for IDE +set_target_properties(ipc_toolkit_demo PROPERTIES FOLDER "DEMO") +source_group(TREE "${CMAKE_CURRENT_SOURCE_DIR}" FILES ${SOURCES}) + +# NOTE: The demo's tests live in the unified ipc_toolkit_tests binary +# (tests/src/tests/demo/) and its Python bindings in the core ipctk module as +# the ipctk.demo submodule (demo/python/simulator.cpp); both are added by +# their respective parent targets when IPC_TOOLKIT_BUILD_DEMO is ON. diff --git a/demo/README.md b/demo/README.md new file mode 100644 index 000000000..ab20c9bfe --- /dev/null +++ b/demo/README.md @@ -0,0 +1,255 @@ +# Demo Simulator + +A small rigid/affine body dynamics simulator built from IPC Toolkit components +(`demo/`, target `ipc_toolkit_demo`, CMake option +`IPC_TOOLKIT_BUILD_DEMO`). It exists to test the components, prototype +ideas, and serve as living proof that the toolkit is easy to integrate — it is +**not** part of the library. + +## The library/demo boundary + +The core library (`ipc::toolkit`) provides modular, dependency-light math: + +- Energy classes: the body (non-contact) terms — inertial, body-force, the + SO(dim) orthogonality penalty (affine only), and the augmented Lagrangian — + written once in **affine coordinates** `y = [p; vec(A)]` + (`ipc/dynamics/affine/`) and aggregated by `dynamics::BodyPotentials` behind a + `dynamics::ToAffine` change of variables (see "Rigid and affine dynamics" + below), plus the barrier and friction potentials. Stateless math with explicit + arguments (`energy(bodies, x)` / `gradient` / `hessian`). Every term except + inertia is a **pure, unscaled physical potential** (energy units); the demo + applies the time-integration scaling when it sums them (see below). +- Time integration (`ipc/dynamics/time_integration/`): BDF (state history + + `dt` → predicted state); it knows nothing about solvers or stepping loops. +- Contact, friction, adhesion potentials and CCD (unchanged). + +This demo (`ipc::toolkit::demo`) owns everything application-side, in two +source pairs: + +- `demo/src/simulator.hpp` — the `Simulator`: it *is* a + [polysolve](https://github.com/polyfem/polysolve) + `nonlinear::Problem` (energy/gradient/Hessian summed from the library's + `BodyPotentials` for the body terms plus the barrier and friction contact + terms, the toolkit's CCD routed into polysolve's line search via + `max_step_size`, and the joint-constraint change of variables x = Uz + [Chen et al. 2022, §4.1] applied around the interface), plus the time-stepping + loop, adaptive barrier stiffness, the friction/kinematic outer loop, pose + history, and a stored `polysolve::nonlinear::Solver`. +- `demo/src/kinematics.hpp` — how the simulation state maps to the world: + the rigid and affine parameterizations (in 2D and 3D), with their world-vertex + maps, chain rules, and the mode-appropriate collision candidates and CCD. The + DOF→pose map itself is the library's `dynamics::ToAffine`. Future + discretizations (FEM nodal DOFs, SPH particles) slot in as additional + `Kinematics` implementations. + +The core library never depends on the demo, on polysolve, or on any solver. + +## The incremental potential (objective units) + +The step minimizes an incremental potential kept uniformly in **kg·m²**: the +inertial term ½(x − x̃)ᵀM(x − x̃) is unscaled, and **every other term is +multiplied by the integrator's acceleration scaling** `s = (βΔt)²` (= Δt² for +implicit Euler) as the `Simulator` sums them. Because β changes while the BDF +order ramps up, the scaling is applied at evaluation time — never baked into +the term classes or into κ. The library's energy classes are therefore pure +potentials, and the adaptive barrier stiffness balances ∇E against `s·κ·∇B` +(see `initialize_barrier_stiffness`). Barrier stiffness κ is stored only in the +`BarrierPotential`, so the friction term automatically sees the current normal +forces. + +## Rigid and affine dynamics (one formulation) + +`Settings::body_dynamics` selects rigid-body dynamics (RBD) or affine-body +dynamics (ABD) [Lan et al. 2022]. Both run the *same* code: the body terms are +written once in affine coordinates `y = [p; vec(A)]` and reached through a +`dynamics::ToAffine` map that the chosen parameterization supplies — + +- **rigid** (`RigidToAffine`): the optimization DOFs are `x = [p; θ]` (θ a + rotation vector in 3D, a scalar angle in 2D) and `A = Q(θ) ∈ SO(dim)` is an + exact change of variables; each term's affine-space gradient/Hessian is pulled + back to `x` by its chain rule (with the rotation block projected to PSD once). +- **affine** (`AffineToAffine`): `x = y` (the identity); `A` is optimized + directly and kept near `SO(dim)` by the orthogonality penalty. + +`BodyPotentials` evaluates every body term at `y = to_affine(x)`, sums them with +the incremental-potential scaling, and applies this chain rule **once**. Contact +and friction are deliberately *not* part of it — they couple pairs of bodies +(off-diagonal Hessian blocks), so they keep their own vertex-space chain rule +(`Kinematics::map_vertex_*`), while the block-diagonal body terms share the +single `ToAffine` pullback. + +This is the main structural difference from Rigid IPC, which is rigid-only and +carries the rotation as a rotation vector everywhere — forces, inertia, and the +time integrator all act in `[p; θ]`. Here rigid bodies still *optimize* `[p; θ]` +each step (so rotations stay exact and the variable stays bounded, via the log +map), but the integrator state and every body energy live in the affine matrix +representation, so **rigid rotation is integrated in matrix space, as in ABD**. +Its recovered angle therefore differs from the linear-in-angle value by an +`O((Δtω)³)` per-step amount — the same higher-order term ABD and 3D already +carry — rather than being exact as in Rigid IPC's angle-space update. + +## Differences from the original Rigid IPC solver + +The solve matches Rigid IPC's [`newton_solver.cpp`](https://github.com/ipc-sim/rigid-ipc/blob/main/src/solvers/newton_solver.cpp) +[Ferguson et al. 2021] in everything that matters — PSD-projected Newton with +an `Eigen::SimplicialLDLT` direction solve, a CCD-filtered backtracking line +search that accepts any energy decrease, and the world-space +max-vertex-velocity stopping criterion (checked from the second iteration, +via the `step_norm()` override) with a gradient-norm early exit. The one +algorithmic difference is the **regularization schedule**: Rigid IPC adapted +a diagonal regularization coefficient continuously inside one Newton loop +(doubled on a failed solve, halved on success, persisting across iterations +and time steps); polysolve instead escalates between discrete strategies — +`ProjectedNewton` → `RegularizedProjectedNewton` (whose internal weight ramps +1e-8 → 1e8) — resetting each solve. Rigid IPC's gradient-descent failsafe is +also dropped: at barrier stiffness scales a −∇f direction is ~1e14 long, so +its CCD-capped line search never yields a usable step. + +The remaining non-default polysolve settings in `Settings::solver_params` +(`Backtracking` line search, no plain-Newton stage, `residual_tolerance` +effectively disabled, explicit `SimplicialLDLT`) exist to reproduce the +original behavior — see the comments in `simulator.hpp`. + +## Friction + +Set `Settings::friction_coefficient > 0` to enable lagged friction +[Ferguson et al. 2021]. Unlike the original's displacement formulation, the +friction potential is evaluated on **velocities** recovered from the time +integrator: `v = (V(x) − Σᵢ wᵢ V(xᵗ⁻ⁱ)) / (βΔt)`, using the same history +combination the BDF scheme uses for its own velocities (so it is correct — not +just consistent — at BDF order ≥ 2). `Settings::static_friction_speed_bound` is +therefore a true speed bound εᵥ (m/s), not a displacement. The tangent bases +and normal-force magnitudes are frozen (lagged) during each inner solve and +rebuilt between solves; `Settings::friction_iterations` controls the lagging: +`0` disables friction, `> 0` caps the number of solves per step (the default +`1` is a single lagged solve), and `< 0` iterates until the momentum balance +‖∇E + κ∇B + ∇D‖ falls below `Settings::momentum_balance_tol × bbox_diagonal` +(`1e-2` by default; or the loop stalls). + +> **Accuracy note.** The friction forces are proportional to the *lagged* +> barrier normal forces, so their accuracy follows the accuracy of the contact +> forces at the solved state. Under the default fast velocity-convergence +> criterion those forces are noisy (the barrier's force-vs-gap curve is steep +> near equilibrium). For quantitative friction — matching an analytic +> deceleration μg, for instance — iterate the lagging to convergence +> (`friction_iterations = -1`) **and** tighten `Settings::velocity_conv_tol` +> (e.g. `1e-3`) so the solver cannot stop while the contact forces are +> unbalanced. + +## Static and kinematic bodies + +Each body has a `type` (`RigidBody::Type::{DYNAMIC, KINEMATIC, STATIC}`, +default `DYNAMIC`): + +- **STATIC** bodies never move: all their DOFs are pinned and they contribute + no inertia, gravity, or external forces (they are still collision-active). +- **KINEMATIC** bodies are driven to a per-step target pose by an augmented + Lagrangian [Ferguson et al. 2021]. The per-body target policy lives in a + `KinematicDriver` owned by the `Simulator`: either a scripted absolute pose, + or (the default) the body's prescribed velocity integrated one step. They + respond to no forces but do push dynamic bodies through the barrier. A + driver's optional drive time counts down each step; when it elapses the body + converts to STATIC. + +Set a body's `type` (via `RigidBody::set_type`) before constructing the +`Simulator`; a `KINEMATIC` body gets a default velocity-driven driver, so +`set_type(KINEMATIC)` plus an initial velocity is all a velocity-driven body +needs. To script a body or give it a finite drive time, attach a driver after +construction: + +```cpp +(*bodies)[0].set_type(rigid::RigidBody::Type::KINEMATIC); +demo::Simulator sim(bodies, poses, dt, settings); +sim.set_kinematic_driver(0, demo::KinematicDriver::scripted(script)); +// or: demo::KinematicDriver::velocity_driven(/*max_time=*/2 * dt) +``` + +The augmented Lagrangian lives in the library +(`ipc::affine::AugmentedLagrangian`, evaluated in affine coordinates and reached +through the same `ToAffine` chain rule in rigid mode) as another body term +inside `BodyPotentials` (manual derivatives, no autodiff). The `Simulator` +drives it and the friction lagging in a **single outer loop**: each iteration is +one Newton solve followed +by an AL penalty/multiplier update (satisfied channels freeze their DOFs) and a +friction re-lag, until both converge (`Settings::max_outer_iterations` caps it, +with `al_initial_penalty` / `al_max_penalty` / `al_satisfied_progress` / +`al_stall_progress` controlling the schedule). + +Individual DOFs of a *dynamic* body can also be fixed by passing an +`is_dof_fixed` mask (`[position | rotation]`, world axes) to +`build_from_meshes`. Joints may not reference a STATIC, KINEMATIC, or +fixed-DOF body (they would be over-constrained); use `add_fixed_point` / +`add_fixed_body` to attach a body to the world instead. + +## Example + +```cpp +#include +#include + +using namespace ipc; + +// Assemble the scene from rest meshes (V, E, F) and initial poses +std::vector poses = { rigid::Pose(...) }; +auto bodies = rigid::RigidBodies::build_from_meshes( + { V }, { E }, { F }, /*densities=*/{ 1000.0 }, poses); +bodies->planes.emplace_back(Eigen::Vector3d(0, 1, 0), 0); // ground + +// Optionally make a body static or kinematic before constructing the sim +(*bodies)[0].set_type(rigid::RigidBody::Type::STATIC); + +// Sum the energy terms into an incremental potential and minimize it with +// polysolve's Newton solver, stepping in time with the library's BDF +// integrator and filtering the line search with the library's CCD. +demo::Simulator::Settings settings; +settings.friction_coefficient = 0.3; // optional lagged friction +demo::Simulator sim(bodies, poses, /*dt=*/0.01, settings); +sim.run(/*t_end=*/1.0); + +// Read back the trajectory (or write it to glTF with ipc/io/write_gltf.hpp +// when the library is built with IPC_TOOLKIT_WITH_GLTF=ON) +std::vector final_poses = sim.rigid_poses(); +``` + +The same scene runs in affine mode (`Settings::body_dynamics = AFFINE`) with +optional joint constraints (`affine::JointConstraints`). The polysolve solver +is configured through `Settings::solver_params` / +`Settings::linear_solver_params` (raw polysolve json; unset entries get the +defaults of polysolve's `nonlinear-solver-spec.json`). polysolve logs through +its own logger, silenced by default (it reports non-gradient stops — e.g., +every velocity-criterion stop — at error level and per-iteration timings at +debug); set `Settings::solver_log_level` to re-enable it. + +## Python + +Building with `IPC_TOOLKIT_BUILD_PYTHON=ON` (and the demo enabled) exposes the +simulator as the `ipctk.demo` submodule of the core module: + +```python +import ipctk + +settings = ipctk.demo.Simulator.Settings() +settings.friction_coefficient = 0.3 +settings.solver_params = {**settings.solver_params, "max_iterations": 50} +sim = ipctk.demo.Simulator(bodies, initial_poses, dt, settings) +sim.run(1.0) +``` + +Body classification is exposed on `ipctk.RigidBody` (`type`, `is_dof_fixed`, +`convert_to_static()`); kinematic driving is `ipctk.demo.KinematicDriver` +(`velocity_driven` / `scripted`) attached via +`Simulator.set_kinematic_driver(body, driver)`. See `python/examples/rigid.py` +and `python/examples/joints.py`. + +## Tests + +The demo's tests live in the unified `ipc_toolkit_tests` binary +(`tests/src/tests/demo/`) and are only compiled when the demo is enabled +(`IPC_TOOLKIT_BUILD_DEMO=ON`). They cover the stepping loop, joint constraints, +both dynamics models, friction (finite-difference derivatives plus analytic +stick/slip and deceleration), static/kinematic bodies (both modes), 2D +simulation, codimensional contact, and finite-difference checks of the +gradient/Hessian through the polysolve `Problem` interface. The `ToAffine` +change of variables and the `BodyPotentials` aggregator are finite-difference- +tested in `tests/src/tests/dynamics/` (`test_to_affine`, `test_body_potentials`), +and the augmented Lagrangian in `tests/src/tests/dynamics/affine/`. diff --git a/demo/python/simulator.cpp b/demo/python/simulator.cpp new file mode 100644 index 000000000..6f921ac51 --- /dev/null +++ b/demo/python/simulator.cpp @@ -0,0 +1,282 @@ +#include +#include +#include + +#include +#include +#include +#include + +namespace py = pybind11; +using namespace ipc; +using namespace ipc::dynamics; +using namespace ipc::demo; + +// Match the opaque declaration in dynamics/rigid/bodies.cpp so the bound +// "Poses" class is used consistently across translation units. +PYBIND11_MAKE_OPAQUE(std::vector) + +void define_simulator(py::module_& m) +{ + py::class_(m, "KinematicDriver", R"ipc_Qu8mg5v7( + Drives a KINEMATIC rigid body to a per-step target pose. + + Either follows a scripted sequence of absolute poses (one per step) + or, with no script, integrates the body's prescribed velocity. After + an optional maximum drive time the body converts to STATIC. + )ipc_Qu8mg5v7") + .def_static( + "velocity_driven", &KinematicDriver::velocity_driven, + "max_time"_a = std::numeric_limits::infinity(), + "A velocity-driven driver (target integrated from the body's " + "prescribed velocity each step).") + .def_static( + "scripted", + [](const std::vector& poses, const double max_time) { + return KinematicDriver::scripted( + std::deque(poses.begin(), poses.end()), + max_time); + }, + "poses"_a, "max_time"_a = std::numeric_limits::infinity(), + "A scripted driver (target is the front pose, advanced per step).") + .def_property_readonly("max_time", &KinematicDriver::max_time) + .def_property_readonly("is_scripted", &KinematicDriver::is_scripted); + + py::class_ simulator(m, "Simulator", R"ipc_Qu8mg5v7( + Body dynamics simulator with a rigid/affine toggle. + + The dynamics model is selected by Settings.body_dynamics: + RIGID [Ferguson et al. 2021] or AFFINE [Lan et al. 2022] (which also + supports linear equality joint constraints [Chen et al. 2022]). + )ipc_Qu8mg5v7"); + + py::enum_( + simulator, "BodyDynamics", + "The dynamics model used to simulate the bodies.") + .value( + "RIGID", Simulator::BodyDynamics::RIGID, + "Rigid body dynamics [Ferguson et al. 2021]: 6-DOF per body with " + "curved-trajectory (nonlinear) CCD.") + .value( + "AFFINE", Simulator::BodyDynamics::AFFINE, + "Affine body dynamics [Lan et al. 2022]: 12-DOF per body with a " + "stiff orthogonality potential and linear CCD.") + .export_values(); + + py::class_( + simulator, "FrictionPair", + "A coefficient of friction for the contacts between two specific " + "bodies, overriding Settings.friction_coefficient.") + .def( + py::init([](size_t body0, size_t body1, double coefficient) { + return Simulator::FrictionPair { body0, body1, coefficient }; + }), + "body0"_a, "body1"_a, "coefficient"_a) + .def_readwrite("body0", &Simulator::FrictionPair::body0) + .def_readwrite("body1", &Simulator::FrictionPair::body1) + .def_readwrite("coefficient", &Simulator::FrictionPair::coefficient); + + py::class_(simulator, "Settings") + .def(py::init<>()) + .def_readwrite( + "body_dynamics", &Simulator::Settings::body_dynamics, + "The dynamics model used for all bodies (RIGID or AFFINE).") + .def_readwrite( + "bdf_order", &Simulator::Settings::bdf_order, + "Order of the BDF time integrator (1 <= n <= 6; 1 is implicit " + "Euler). The effective order ramps up from 1 over the first n " + "steps.") + .def_readwrite("dhat", &Simulator::Settings::dhat) + .def_readwrite( + "minimum_separation_distance", + &Simulator::Settings::minimum_separation_distance) + .def_readwrite("gravity", &Simulator::Settings::gravity) + .def_readwrite( + "min_barrier_stiffness_scale", + &Simulator::Settings::min_barrier_stiffness_scale) + .def_readwrite( + "dhat_epsilon_scale", &Simulator::Settings::dhat_epsilon_scale) + .def_readwrite( + "friction_coefficient", &Simulator::Settings::friction_coefficient, + "Coefficient of friction (0 disables friction).") + .def_readwrite( + "friction_pairs", &Simulator::Settings::friction_pairs, + "Per-pair coefficients of friction (a list of FrictionPair). " + "Contacts between the two bodies of a pair use its coefficient " + "instead of friction_coefficient; contacts with an analytic plane " + "always use friction_coefficient. This returns a copy, so assign " + "the whole list rather than appending to it.") + .def_readwrite( + "static_friction_speed_bound", + &Simulator::Settings::static_friction_speed_bound, + "Smooth friction mollifier speed bound eps_v (m/s): sliding " + "speeds below this are treated as static.") + .def_readwrite( + "friction_iterations", &Simulator::Settings::friction_iterations, + "Friction lagging iterations: 0 disables friction; > 0 caps the " + "number of solves per step (1 = a single lagged solve); < 0 " + "iterates until the momentum balance converges (up to " + "max_outer_iterations).") + .def_readwrite( + "momentum_balance_tol", &Simulator::Settings::momentum_balance_tol, + "Friction lagging stops once the momentum balance falls below " + "this times the bounding box diagonal.") + .def_readwrite( + "max_outer_iterations", &Simulator::Settings::max_outer_iterations, + "Hard cap on the outer (friction-lagging / augmented Lagrangian) " + "solves per step.") + .def_readwrite( + "al_initial_penalty", &Simulator::Settings::al_initial_penalty, + "Initial penalty of the kinematic-body augmented Lagrangian, " + "relative to the body's inertia (reset each step, as are the " + "multipliers).") + .def_readwrite( + "al_max_penalty", &Simulator::Settings::al_max_penalty, + "Maximum AL penalty (stop doubling).") + .def_readwrite( + "al_satisfied_progress", + &Simulator::Settings::al_satisfied_progress, + "AL progress at which a channel is satisfied (DOFs freeze).") + .def_readwrite( + "al_stall_progress", &Simulator::Settings::al_stall_progress, + "AL progress below which the penalty doubles (otherwise the " + "multipliers update).") + .def_readwrite( + "orthogonality_stiffness", + &Simulator::Settings::orthogonality_stiffness, + "Stiffness of the orthogonality potential (affine mode only).") + .def_readwrite( + "use_area_weighting", &Simulator::Settings::use_area_weighting) + .def_readwrite( + "abort_on_convergence_failure", + &Simulator::Settings::abort_on_convergence_failure) + .def_readwrite( + "velocity_conv_tol", &Simulator::Settings::velocity_conv_tol, + "Velocity convergence tolerance: converged when the proposed " + "step moves every vertex slower than this x the bounding box " + "diagonal (world space). Set <= 0 to disable.") + .def_readwrite( + "solver_params", &Simulator::Settings::solver_params, + R"ipc_Qu8mg5v7( + polysolve nonlinear solver parameters (a dict). Entries not set + here get the defaults of polysolve's nonlinear-solver-spec.json. + + Note: this property returns a copy -- assign a whole dict, e.g. + ``settings.solver_params = {**settings.solver_params, "max_iterations": 50}``. + )ipc_Qu8mg5v7") + .def_readwrite( + "linear_solver_params", &Simulator::Settings::linear_solver_params, + "polysolve linear solver parameters (a dict; empty selects " + "polysolve's default Eigen solver). Returns a copy -- assign a " + "whole dict.") + .def_readwrite( + "solver_log_level", &Simulator::Settings::solver_log_level, + "Verbosity of polysolve's own logger (separate from the " + "toolkit's logger; silenced by default -- lower to, e.g., " + "ipctk.debug to see the solver's per-iteration reports)."); + + simulator + .def( + py::init< + const std::shared_ptr&, + const std::vector&, const double>(), + "bodies"_a, "initial_poses"_a, "dt"_a) + .def( + py::init< + const std::shared_ptr&, + const std::vector&, const double, + const Simulator::Settings&>(), + "bodies"_a, "initial_poses"_a, "dt"_a, "settings"_a) + .def( + py::init< + const std::shared_ptr&, + const std::vector&, + const std::vector&, const double, + const Simulator::Settings&>(), + R"ipc_Qu8mg5v7( + Create a simulator with initial velocities. + + Parameters: + bodies: Bodies in the simulation. + initial_poses: Initial poses of the bodies. + initial_velocities: Initial velocities: position is the linear velocity and rotation is the world-frame angular velocity. + dt: Time step. + settings: Simulation settings. + )ipc_Qu8mg5v7", + "bodies"_a, "initial_poses"_a, "initial_velocities"_a, "dt"_a, + "settings"_a = Simulator::Settings()) + .def( + py::init< + const std::shared_ptr&, + const std::vector&, + const std::shared_ptr&, const double, + const Simulator::Settings&>(), + R"ipc_Qu8mg5v7( + Create a simulator with joint constraints. + + Requires settings.body_dynamics == BodyDynamics.AFFINE + (material-point constraints are nonlinear in the rigid rotation + vectors). + )ipc_Qu8mg5v7", + "bodies"_a, "initial_poses"_a, "joints"_a, "dt"_a, + "settings"_a = Simulator::Settings()) + .def_property("gravity", &Simulator::gravity, &Simulator::set_gravity) + .def_property_readonly("settings", &Simulator::settings) + .def( + "run", &Simulator::run, "t_end"_a, + "callback"_a = std::function([](bool) {})) + .def("step", &Simulator::step) + .def("reset", &Simulator::reset) + .def( + "set_kinematic_driver", &Simulator::set_kinematic_driver, "body"_a, + "driver"_a, + "Attach a kinematic driver to a KINEMATIC body (call after " + "construction; the body must already be KINEMATIC).") + .def_property_readonly( + "pose_history", &Simulator::pose_history, + R"ipc_Qu8mg5v7( + Get the history of (affine) poses in the simulation. + + In rigid mode the rotation part is exactly R(θ). + + Returns: + A list of affine poses at each time step. + )ipc_Qu8mg5v7") + .def_property_readonly( + "rigid_pose_history", &Simulator::rigid_pose_history, + R"ipc_Qu8mg5v7( + Get the history of poses converted to rigid poses (log map). + + Exact in rigid mode (use for write_gltf); in affine mode the + rotation part of the pose must be (numerically) a rotation + matrix. + + Returns: + A list of poses at each time step. + )ipc_Qu8mg5v7") + .def_property_readonly( + "poses", &Simulator::poses, + R"ipc_Qu8mg5v7( + Get the current (most recent) affine poses. + + O(num_bodies) -- prefer this over ``pose_history[-1]`` for + per-frame access, which copies the entire history each call. + )ipc_Qu8mg5v7") + .def_property_readonly( + "rigid_poses", &Simulator::rigid_poses, + R"ipc_Qu8mg5v7( + Get the current (most recent) poses as rigid poses (log map). + + O(num_bodies) -- the per-frame counterpart to + ``rigid_pose_history``. Pass to ``RigidBodies.vertices`` for + rendering. + )ipc_Qu8mg5v7") + .def_property_readonly("t", &Simulator::t) + .def_property_readonly( + "num_contacts", + [](const Simulator& sim) { return sim.normal_collisions().size(); }, + "Number of active normal (barrier) contacts at the current state.") + .def_property_readonly( + "last_momentum_balance", &Simulator::last_momentum_balance, + "Momentum-balance residual of the last friction-lagging solve."); +} diff --git a/demo/src/kinematic_driver.cpp b/demo/src/kinematic_driver.cpp new file mode 100644 index 000000000..92f436126 --- /dev/null +++ b/demo/src/kinematic_driver.cpp @@ -0,0 +1,32 @@ +#include "kinematic_driver.hpp" + +namespace ipc::demo { + +rigid::Pose KinematicDriver::target( + const rigid::Pose& current, + const rigid::Pose& velocity, + const double dt) const +{ + if (is_scripted()) { + // Scripted absolute target pose (advanced each step). + return m_scripted_poses.front(); + } + + // Integrate the body's prescribed velocity: p̂ = p + dt·v and, for the + // rotation, θ̂ = θ + dt·ω in 2D (a linear space) or the exact exponential + // Q̂ = exp(dt·[ω]×)·Q in 3D. + rigid::Pose result = current; + result.position += dt * velocity.position; + + if (current.rotation.size() == 1) { + result.rotation(0) += dt * velocity.rotation(0); + } else { + result.rotation = rigid::rotation_matrix_to_vector( + rigid::rotation_vector_to_matrix(dt * velocity.rotation) + * rigid::rotation_vector_to_matrix(current.rotation)); + } + + return result; +} + +} // namespace ipc::demo diff --git a/demo/src/kinematic_driver.hpp b/demo/src/kinematic_driver.hpp new file mode 100644 index 000000000..f68e4c405 --- /dev/null +++ b/demo/src/kinematic_driver.hpp @@ -0,0 +1,94 @@ +#pragma once + +#include + +#include +#include + +namespace ipc::demo { + +/// @brief Drives a KINEMATIC rigid body to a per-step target pose. +/// +/// A kinematic body follows either a scripted sequence of absolute poses (one +/// per step) or, when no script is given, its own prescribed velocity. After +/// an optional maximum drive time the simulator converts the body to STATIC. +/// +/// The driver holds no simulation state beyond its script and remaining time; +/// the current pose and velocity are supplied by the caller (extracted from +/// the time integrator), so the driver is agnostic to the DOF layout. +class KinematicDriver { +public: + KinematicDriver() = default; + + /// @brief A velocity-driven driver (target integrated from the body's + /// prescribed velocity each step). + /// @param max_time Drive time before the body converts to STATIC. + static KinematicDriver velocity_driven( + const double max_time = std::numeric_limits::infinity()) + { + return KinematicDriver({}, max_time); + } + + /// @brief A scripted driver (target is the front pose, advanced per step). + /// @param poses Absolute target poses, one per step. + /// @param max_time Drive time before the body converts to STATIC. + static KinematicDriver scripted( + std::deque poses, + const double max_time = std::numeric_limits::infinity()) + { + return KinematicDriver(std::move(poses), max_time); + } + + /// @brief The target pose for this step. + /// @param current The body's current pose (position + rotation + /// vector/angle). + /// @param velocity The body's prescribed velocity (linear in .position, + /// angular ω in .rotation); used only when there is no script. + /// @param dt Time step. + rigid::Pose target( + const rigid::Pose& current, + const rigid::Pose& velocity, + const double dt) const; + + /// @brief Advance the driver by one step (pop the front scripted pose and + /// count down the drive time). + void step(const double dt) + { + m_max_time -= dt; + if (!m_scripted_poses.empty()) { + m_scripted_poses.pop_front(); + } + } + + /// @brief Whether the drive time has elapsed (the body should convert to + /// STATIC before this step). + bool expired(const double dt) const { return m_max_time < 0.5 * dt; } + + /// @brief Whether this driver follows a scripted pose sequence. + bool is_scripted() const { return !m_scripted_poses.empty(); } + + /// @brief Remaining drive time. + double max_time() const { return m_max_time; } + + /// @brief The scripted target poses (empty ⇒ velocity-driven). + const std::deque& scripted_poses() const + { + return m_scripted_poses; + } + +private: + KinematicDriver(std::deque poses, const double max_time) + : m_scripted_poses(std::move(poses)) + , m_max_time(max_time) + { + } + + /// @brief Scripted absolute target poses (one per step); empty ⇒ + /// velocity-driven. + std::deque m_scripted_poses; + + /// @brief Remaining drive time before the body converts to STATIC. + double m_max_time = std::numeric_limits::infinity(); +}; + +} // namespace ipc::demo diff --git a/demo/src/kinematics.cpp b/demo/src/kinematics.cpp new file mode 100644 index 000000000..89c433bf3 --- /dev/null +++ b/demo/src/kinematics.cpp @@ -0,0 +1,289 @@ +#include "kinematics.hpp" + +#include +#include +#include +#include +#include +#include + +#include + +namespace ipc::demo { + +Kinematics::Kinematics(const std::shared_ptr& bodies) + : m_bodies(bodies) +{ + assert(m_bodies != nullptr); +} + +Kinematics::~Kinematics() = default; + +namespace { + + /// @brief Rigid parameterization: x_i = [p; θ] (6-DOF in 3D with θ the + /// rotation vector; 3-DOF in 2D with θ the scalar angle). + class RigidKinematics : public Kinematics { + public: + explicit RigidKinematics( + const std::shared_ptr& bodies) + : Kinematics(bodies) + { + m_to_affine = std::make_shared( + bodies->dim(), bodies->num_bodies()); + } + + int pose_ndof() const { return m_bodies->dim() == 2 ? 3 : 6; } + + int ndof() const override + { + return pose_ndof() * int(m_bodies->num_bodies()); + } + + Eigen::MatrixXd + world_vertices(Eigen::ConstRef x) const override + { + return m_bodies->vertices(x); + } + + Eigen::VectorXd map_vertex_gradient( + Eigen::ConstRef x, + Eigen::ConstRef grad) const override + { + return m_bodies->to_rigid_dof( + rigid::Pose::to_poses(x, m_bodies->dim()), grad); + } + + Eigen::SparseMatrix map_vertex_hessian( + Eigen::ConstRef x, + Eigen::ConstRef grad, + const Eigen::SparseMatrix& hess) const override + { + return m_bodies->to_rigid_dof( + rigid::Pose::to_poses(x, m_bodies->dim()), grad, hess); + } + + std::vector + poses(Eigen::ConstRef x) const override + { + return m_to_affine->poses(x); + } + + Eigen::VectorXd + dof(const std::vector& poses) const override + { + return rigid::Pose::from_poses(poses); + } + + Eigen::VectorXd + to_integrator_state(Eigen::ConstRef x) const override + { + // The integrator state is affine-shaped [p; vec(Q)] per body; the + // log map keeps the optimization variable θ bounded on the way + // back. + return m_to_affine->to_affine(x); + } + + Eigen::VectorXd + from_integrator_state(Eigen::ConstRef X) const override + { + return m_to_affine->from_affine(X); + } + + void update_candidates( + Eigen::ConstRef x, + const double inflation_radius, + BroadPhase* broad_phase) override + { + // Discrete (distance) candidates through the rigid body-pair + // broad phase: reuses each body's static rest-frame BVH instead + // of rebuilding a world-space one [Ferguson et al. 2021]. + m_candidates.build( + *m_bodies, rigid::Pose::to_poses(x, m_bodies->dim()), + inflation_radius, broad_phase); + } + + void update_candidates( + Eigen::ConstRef x0, + Eigen::ConstRef x1, + const double inflation_radius, + BroadPhase* broad_phase) override + { + m_candidates.build( + *m_bodies, rigid::Pose::to_poses(x0, m_bodies->dim()), + rigid::Pose::to_poses(x1, m_bodies->dim()), inflation_radius, + broad_phase); + } + + double max_step_size( + Eigen::ConstRef x0, + Eigen::ConstRef x1, + const double inflation_radius, + BroadPhase* broad_phase, + const double min_distance) override + { + update_candidates(x0, x1, inflation_radius, broad_phase); + return m_candidates.compute_collision_free_stepsize( + *m_bodies, rigid::Pose::to_poses(x0, m_bodies->dim()), + rigid::Pose::to_poses(x1, m_bodies->dim()), min_distance, + m_ccd); + } + + const Candidates& candidates() const override { return m_candidates; } + + void clear_candidates() override { m_candidates.clear(); } + + private: + /// @brief Rigid collision candidates (rotational AABB inflation). + rigid::RigidCandidates m_candidates; + /// @brief Nonlinear CCD along the curved rigid trajectories. + /// The per-piece linear CCD is delegated to Additive CCD (as in the + /// affine path): much faster than Tight-Inclusion CCD per query, + /// though not provably conservative under floating-point rounding. + NonlinearCCD m_ccd { std::make_shared() }; + }; + + /// @brief Affine parameterization: x_i = [p; vec(A) column-major] + /// (12-DOF per body in 3D; 6-DOF in 2D). + class AffineKinematics : public Kinematics { + public: + explicit AffineKinematics( + const std::shared_ptr& bodies) + : Kinematics(bodies) + , m_J_all(affine::affine_jacobian(*bodies)) + { + m_to_affine = std::make_shared( + bodies->dim(), bodies->num_bodies()); + } + + int body_ndof() const + { + return m_bodies->dim() + m_bodies->dim() * m_bodies->dim(); + } + + int ndof() const override + { + return body_ndof() * int(m_bodies->num_bodies()); + } + + Eigen::MatrixXd + world_vertices(Eigen::ConstRef x) const override + { + return affine::vertices(*m_bodies, x); + } + + Eigen::VectorXd map_vertex_gradient( + Eigen::ConstRef x, + Eigen::ConstRef grad) const override + { + return m_J_all.transpose() * grad; + } + + Eigen::SparseMatrix map_vertex_hessian( + Eigen::ConstRef x, + Eigen::ConstRef grad, + const Eigen::SparseMatrix& hess) const override + { + // V(x) is linear in the affine DOFs, so there is no second-order + // term (grad is unused). + return m_J_all.transpose() * hess * m_J_all; + } + + std::vector + poses(Eigen::ConstRef x) const override + { + return m_to_affine->poses(x); + } + + Eigen::VectorXd + dof(const std::vector& poses) const override + { + const int dim = m_bodies->dim(); + Eigen::VectorXd x(body_ndof() * poses.size()); + for (size_t i = 0; i < poses.size(); ++i) { + x.segment(body_ndof() * i, dim) = poses[i].position; + x.segment(body_ndof() * i + dim, dim * dim) = + poses[i].rotation_matrix().reshaped(); + } + return x; + } + + Eigen::VectorXd + to_integrator_state(Eigen::ConstRef x) const override + { + return m_to_affine->to_affine(x); // identity: DOFs are the state + } + + Eigen::VectorXd + from_integrator_state(Eigen::ConstRef X) const override + { + return m_to_affine->from_affine(X); // identity: DOFs are the state + } + + void update_candidates( + Eigen::ConstRef x, + const double inflation_radius, + BroadPhase* broad_phase) override + { + m_candidates.build( + *m_bodies, world_vertices(x), inflation_radius, broad_phase); + } + + void update_candidates( + Eigen::ConstRef x0, + Eigen::ConstRef x1, + const double inflation_radius, + BroadPhase* broad_phase) override + { + m_candidates.build( + *m_bodies, world_vertices(x0), world_vertices(x1), + inflation_radius, broad_phase); + } + + double max_step_size( + Eigen::ConstRef x0, + Eigen::ConstRef x1, + const double inflation_radius, + BroadPhase* broad_phase, + const double min_distance) override + { + const Eigen::MatrixXd V0 = world_vertices(x0); + const Eigen::MatrixXd V1 = world_vertices(x1); + + m_candidates.build( + *m_bodies, V0, V1, inflation_radius, broad_phase); + + // The per-iterate vertex trajectories are linear in the step + // parameter, so linear CCD is exact [Lan et al. 2022, §4.1]. + return m_candidates.compute_collision_free_stepsize( + *m_bodies, V0, V1, min_distance, m_ccd); + } + + const Candidates& candidates() const override { return m_candidates; } + + void clear_candidates() override { m_candidates.clear(); } + + private: + /// @brief Constant Jacobian dV/dx (V is linear in the affine DOFs). + Eigen::SparseMatrix m_J_all; + /// @brief Collision candidates on the linear vertex trajectories. + Candidates m_candidates; + /// @brief Linear CCD (exact for affine per-iterate trajectories). + AdditiveCCD m_ccd; + }; + +} // namespace + +std::unique_ptr Kinematics::create_rigid( + const std::shared_ptr& bodies) +{ + return std::make_unique(bodies); +} + +std::unique_ptr Kinematics::create_affine( + const std::shared_ptr& bodies) +{ + return std::make_unique(bodies); +} + +} // namespace ipc::demo diff --git a/demo/src/kinematics.hpp b/demo/src/kinematics.hpp new file mode 100644 index 000000000..213921384 --- /dev/null +++ b/demo/src/kinematics.hpp @@ -0,0 +1,149 @@ +#pragma once + +#include +#include +#include + +#include +#include + +#include +#include + +namespace ipc { +class BroadPhase; +class Candidates; +} // namespace ipc + +namespace ipc::rigid { +class RigidBodies; +} // namespace ipc::rigid + +namespace ipc::demo { + +/// @brief The kinematics of a discretization: how the simulation state x +/// maps to the world. +/// +/// Encapsulates the DOF layout, the world-vertex map and its chain rule, the +/// bridge to the time-integrator state, and the discretization-appropriate +/// collision candidates and CCD — while all forces/energies live elsewhere. +/// Today this hides the differences between the rigid (6-DOF [p; θ] per +/// body) and affine (12-DOF [p; vec(A) column-major] per body) +/// parameterizations; future discretizations (e.g., FEM nodal DOFs or SPH +/// particles, where x is the vertex/particle positions and the world map is +/// the identity) slot in as additional implementations. +class Kinematics { +public: + virtual ~Kinematics(); + + /// @brief Create the rigid (6-DOF [p; θ] per body) DOF model. + static std::unique_ptr + create_rigid(const std::shared_ptr& bodies); + + /// @brief Create the affine (12-DOF [p; vec(A)] per body) DOF model. + static std::unique_ptr + create_affine(const std::shared_ptr& bodies); + + /// @brief Total number of optimization DOFs. + virtual int ndof() const = 0; + + // ----------------------------------------------------------------------- + // World map and chain rule + // ----------------------------------------------------------------------- + + /// @brief World-space collision-mesh vertices at x. + virtual Eigen::MatrixXd + world_vertices(Eigen::ConstRef x) const = 0; + + /// @brief Map a vertex-space gradient to the DOFs: Jᵀ ∇V. + virtual Eigen::VectorXd map_vertex_gradient( + Eigen::ConstRef x, + Eigen::ConstRef grad) const = 0; + + /// @brief Map a vertex-space Hessian to the DOFs: Jᵀ H J (+ the + /// second-order term Σ ∇V·d²V/dx² for the rigid parameterization; the + /// affine map is linear so its second-order term is zero). + virtual Eigen::SparseMatrix map_vertex_hessian( + Eigen::ConstRef x, + Eigen::ConstRef grad, + const Eigen::SparseMatrix& hess) const = 0; + + // ----------------------------------------------------------------------- + // Pose conversions + // ----------------------------------------------------------------------- + + /// @brief The to-affine map (DOFs → affine pose coordinates) and its chain + /// rule for this parameterization. + std::shared_ptr to_affine() const + { + return m_to_affine; + } + + /// @brief Per-body affine poses at x (exact for rigid: A = R(θ)). + virtual std::vector + poses(Eigen::ConstRef x) const = 0; + + /// @brief Build the DOF vector from rigid poses. + virtual Eigen::VectorXd + dof(const std::vector& poses) const = 0; + + // ----------------------------------------------------------------------- + // Time-integrator bridge + // ----------------------------------------------------------------------- + // The integrator state is affine-shaped ([p; vec(Q) column-major] per + // body) for both models; only the rigid model needs a conversion. + + /// @brief Convert the optimization DOFs to the integrator state. + virtual Eigen::VectorXd + to_integrator_state(Eigen::ConstRef x) const = 0; + + /// @brief Convert the integrator state to the optimization DOFs. + /// @note For the rigid model the log map yields the canonical rotation + /// vector (‖θ‖ ≤ π), which also keeps the optimization variable bounded. + virtual Eigen::VectorXd + from_integrator_state(Eigen::ConstRef X) const = 0; + + // ----------------------------------------------------------------------- + // Collision candidates and CCD + // ----------------------------------------------------------------------- + + /// @brief Build the discrete collision candidates at x. + virtual void update_candidates( + Eigen::ConstRef x, + const double inflation_radius, + BroadPhase* broad_phase) = 0; + + /// @brief Build the continuous collision candidates for the step x0 → x1. + virtual void update_candidates( + Eigen::ConstRef x0, + Eigen::ConstRef x1, + const double inflation_radius, + BroadPhase* broad_phase) = 0; + + /// @brief Compute a collision-free maximum step size from x0 to x1 ∈ [0, 1]. + /// Also updates the collision candidates for the swept interval. + virtual double max_step_size( + Eigen::ConstRef x0, + Eigen::ConstRef x1, + const double inflation_radius, + BroadPhase* broad_phase, + const double min_distance = 0.0) = 0; + + /// @brief The current collision candidates. + virtual const Candidates& candidates() const = 0; + + /// @brief Clear the collision candidates. + virtual void clear_candidates() = 0; + +protected: + explicit Kinematics( + const std::shared_ptr& bodies); + + /// @brief The bodies in the simulation. + std::shared_ptr m_bodies; + + /// @brief The to-affine map for this parameterization (set by subclasses). + std::shared_ptr m_to_affine; +}; + +} // namespace ipc::demo diff --git a/demo/src/simulator.cpp b/demo/src/simulator.cpp new file mode 100644 index 000000000..03c99240c --- /dev/null +++ b/demo/src/simulator.cpp @@ -0,0 +1,1337 @@ +#include "simulator.hpp" + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include // cross_product_matrix +#include // nearest_rotation +#include +#include +#include +#include + +#include +#include +#include +#include + +#include +#include +#include +#include +#include + +namespace ipc::demo { + +using namespace ipc::dynamics; + +namespace { + // TODO: Why does this not work with 0.5? + inline constexpr double INFLATION_RADIUS_MULTIPLIER = 1.0; + + /// While a kinematic target is unmet, the velocity criterion is this much + /// tighter (see Simulator::step_norm). + inline constexpr double AL_VELOCITY_CONV_TOL_SCALE = 0.1; + + std::vector + zero_velocities(const size_t num_bodies, const int dim) + { + return std::vector(num_bodies, rigid::Pose::identity(dim)); + } + + /// Zero the velocity components of a body's fixed DOFs. + rigid::Pose + free_velocity(rigid::Pose velocity, const rigid::RigidBody& body) + { + const auto& fixed = body.is_dof_fixed(); + const int dim = int(velocity.position.size()); + for (int k = 0; k < dim; ++k) { + if (fixed(k)) { + velocity.position(k) = 0; + } + } + for (int k = 0; k < velocity.rotation.size(); ++k) { + if (fixed(dim + k)) { + velocity.rotation(k) = 0; + } + } + return velocity; + } +} // namespace + +Simulator::Simulator( + const std::shared_ptr& bodies, + const std::vector& initial_poses, + const double dt) + : Simulator(bodies, initial_poses, dt, Settings()) +{ +} + +Simulator::Simulator( + const std::shared_ptr& bodies, + const std::vector& initial_poses, + const double dt, + const Settings& settings) + : Simulator( + bodies, + initial_poses, + zero_velocities(bodies->num_bodies(), bodies->dim()), + /*joints=*/nullptr, + dt, + settings) +{ +} + +Simulator::Simulator( + const std::shared_ptr& bodies, + const std::vector& initial_poses, + const std::vector& initial_velocities, + const double dt, + const Settings& settings) + : Simulator( + bodies, + initial_poses, + initial_velocities, + /*joints=*/nullptr, + dt, + settings) +{ +} + +Simulator::Simulator( + const std::shared_ptr& bodies, + const std::vector& initial_poses, + const std::shared_ptr& joints, + const double dt) + : Simulator(bodies, initial_poses, joints, dt, Settings()) +{ +} + +Simulator::Simulator( + const std::shared_ptr& bodies, + const std::vector& initial_poses, + const std::shared_ptr& joints, + const double dt, + const Settings& settings) + : Simulator( + bodies, + initial_poses, + zero_velocities(bodies->num_bodies(), bodies->dim()), + joints, + dt, + settings) +{ +} + +Simulator::Simulator( + const std::shared_ptr& bodies, + const std::vector& initial_poses, + const std::vector& initial_velocities, + const std::shared_ptr& joints, + const double dt, + const Settings& settings) + : m_settings(settings) + , m_bodies(bodies) + , m_joints(joints) + , m_initial_poses(initial_poses) + , m_initial_velocities(initial_velocities) +{ + assert(m_bodies != nullptr); + assert(initial_poses.size() == m_bodies->num_bodies()); + assert(initial_velocities.size() == m_bodies->num_bodies()); + + if (m_settings.gravity.size() != m_bodies->dim()) { + // The default gravity is 3D; truncate for 2D scenes. + logger().debug( + "truncating gravity to the scene dimension ({})", m_bodies->dim()); + m_settings.gravity = + VectorMax3d(m_settings.gravity.head(m_bodies->dim())); + } + + if (m_joints != nullptr && !m_joints->empty() + && m_settings.body_dynamics != BodyDynamics::AFFINE) { + throw std::invalid_argument( + "Joint constraints require affine body dynamics " + "(material-point constraints are nonlinear in the rigid " + "rotation vectors)"); + } + + switch (m_settings.body_dynamics) { + case BodyDynamics::RIGID: + m_kinematics = Kinematics::create_rigid(m_bodies); + break; + case BodyDynamics::AFFINE: + m_kinematics = Kinematics::create_affine(m_bodies); + break; + } + + m_pose_history.push_back( + m_kinematics->poses(m_kinematics->dof(initial_poses))); + + m_barrier_potential = std::make_shared( + m_settings.dhat, /*stiffness=*/1.0, /*use_physical_barrier=*/true); + m_normal_collisions = std::make_shared(); + m_normal_collisions->set_use_area_weighting(m_settings.use_area_weighting); + // The improved max approximator [Li et al. 2023] assumes area-weighted + // collisions; without area weighting use the original IPC set (which is + // also what the original Rigid IPC solver used). + m_normal_collisions->set_collision_set_type( + m_settings.use_area_weighting + ? NormalCollisions::CollisionSetType::IMPROVED_MAX_APPROX + : NormalCollisions::CollisionSetType::IPC); + m_broad_phase = std::make_unique(); + + if (friction_enabled() && m_settings.static_friction_speed_bound <= 0) { + throw std::invalid_argument( + "static_friction_speed_bound must be positive when friction is " + "enabled"); + } + // εᵥ is a true speed bound (the friction potential is evaluated on + // velocities); keep the potential constructible when friction is off. + m_friction_potential = std::make_shared( + std::max(m_settings.static_friction_speed_bound, 1e-16)); + m_tangential_collisions = std::make_shared(); + + initialize_time_integrator(initial_poses, initial_velocities, dt); + initialize_terms(); + + if (m_joints != nullptr && !m_joints->empty()) { + m_joints->finalize(); + } + + // Snapshot the body state so reset() can restore it (kinematic bodies + // mutate to STATIC on expiry). KINEMATIC bodies get a default + // velocity-driven driver; set_kinematic_driver() replaces it to script a + // body or set a finite drive time. + m_initial_body_types.resize(m_bodies->num_bodies()); + m_initial_is_dof_fixed.resize(m_bodies->num_bodies()); + m_kinematic_drivers.assign(m_bodies->num_bodies(), std::nullopt); + for (size_t i = 0; i < m_bodies->num_bodies(); ++i) { + const rigid::RigidBody& body = (*m_bodies)[i]; + m_initial_body_types[i] = body.type(); + m_initial_is_dof_fixed[i] = body.is_dof_fixed(); + if (body.type() == rigid::RigidBody::Type::KINEMATIC) { + m_kinematic_drivers[i] = KinematicDriver::velocity_driven(); + } + + if (is_affine()) { + // Partial rotational fixing has no per-entry vec(A) analogue. + const auto rot_fixed = body.is_dof_fixed().tail( + body.is_dof_fixed().size() - m_bodies->dim()); + if (rot_fixed.any() && !rot_fixed.all()) { + throw std::invalid_argument( + "Partial rotational DOF fixing is not supported in " + "affine mode (fix all rotational DOFs or none)"); + } + } + + // A joint anchored to a prescribed body is over-constrained: the + // joint RHS is fixed at the initial configuration while the script + // or pin moves/holds the body. + if (has_joints() && (!body.is_dynamic() || body.is_dof_fixed().any()) + && m_joints->is_body_constrained(i)) { + throw std::invalid_argument( + "Joints may not reference STATIC/KINEMATIC or fixed-DOF " + "bodies (use add_fixed_point/add_fixed_body instead)"); + } + } + + init_friction_pairs(); + m_initial_kinematic_drivers = m_kinematic_drivers; + + rebuild_dof_mask(); + + // Create the polysolve Newton solver (the tolerances are rescaled by the + // characteristic length; use the initial world bounding box diagonal). + const Eigen::MatrixXd V0 = + m_kinematics->world_vertices(m_kinematics->dof(initial_poses)); + m_bbox_diagonal = + (V0.colwise().maxCoeff() - V0.colwise().minCoeff()).norm(); + assert(m_bbox_diagonal > 0); + + // The velocity convergence criterion [Ferguson et al. 2021]: converged + // when the proposed step moves every vertex slower than the tolerance. + // The step_norm() override measures steps as world-space vertex + // displacements, so polysolve's Δx criterion compares against + // tol × bbox × dt. + nlohmann::json solver_params = m_settings.solver_params; + const bool use_velocity_criterion = m_settings.velocity_conv_tol > 0; + if (use_velocity_criterion && !solver_params.contains("x_delta_tol")) { + solver_params["x_delta_tol"] = + m_settings.velocity_conv_tol * m_bbox_diagonal * dt; + } + // We disable polysolve's absolute gradient criterion whenever the + // velocity criterion is on. Its threshold doesn't scale with the scene, + // so for light or small bodies it fires after the first Newton iteration + // and accepts unconverged steps. Without the velocity criterion it is + // the only absolute stop left, so we keep a fixed fallback there. + if (!solver_params.contains("grad_norm_tol")) { + solver_params["grad_norm_tol"] = use_velocity_criterion ? 0.0 : 1e-6; + } + + // polysolve logs through a dedicated logger so its chatter (a per-solve + // "Finished" report at error level whenever the stop was not the + // gradient criterion, per-iteration [timing] lines at debug, ...) can be + // silenced independently of the toolkit's logger. Created without + // registering it in spdlog's global registry (see ipc/utils/logger.cpp). + m_solver_logger = std::make_shared( + "polysolve", std::make_shared()); + m_solver_logger->set_level(m_settings.solver_log_level); + + m_solver = polysolve::nonlinear::Solver::create( + solver_params, m_settings.linear_solver_params, + /*characteristic_length=*/m_bbox_diagonal, *m_solver_logger); +} + +Simulator::~Simulator() = default; + +void Simulator::initialize_time_integrator( + const std::vector& poses, + const std::vector& velocities, + const double dt) +{ + const int dim = m_bodies->dim(); + + // The integrator state is affine-shaped [p (dim); vec(Q) (dim²)] per body + // in both models and dimensions (the rigid optimization DOFs [p; θ] are + // mapped to/from this state by the parameterization's to-affine map). + const int pos_ndof = dim; + const int rot_ndof = dim * dim; + const int state_ndof = pos_ndof + rot_ndof; + + Eigen::VectorXd x0 = + Eigen::VectorXd::Zero(state_ndof * m_bodies->num_bodies()); + Eigen::VectorXd v0 = + Eigen::VectorXd::Zero(state_ndof * m_bodies->num_bodies()); + for (size_t i = 0; i < m_bodies->num_bodies(); ++i) { + x0.segment(state_ndof * i, dim) = poses[i].position; + + const rigid::Pose velocity = + free_velocity(velocities[i], (*m_bodies)[i]); + + v0.segment(state_ndof * i, dim) = velocity.position; + + if (rot_ndof == 4) { + // 2D (both models): Q̇ = ω S Q with S the 2D skew generator. + const Eigen::Matrix2d Q_t0 = poses[i].rotation_matrix(); + x0.segment<4>(state_ndof * i + dim) = Q_t0.reshaped(); + v0.segment<4>(state_ndof * i + dim) = + (cross_product_matrix(velocity.rotation(0)) * Q_t0).reshaped(); + } else { + // Q̇ = [ω]× Q with ω the world-frame angular velocity + const Eigen::Matrix3d Q_t0 = poses[i].rotation_matrix(); + x0.segment<9>(state_ndof * i + 3) = Q_t0.reshaped(); + v0.segment<9>(state_ndof * i + 3) = + (cross_product_matrix(velocity.rotation) * Q_t0).reshaped(); + } + } + const Eigen::VectorXd a0 = + Eigen::VectorXd::Zero(state_ndof * m_bodies->num_bodies()); + + auto bdf = std::make_shared(m_settings.bdf_order); + bdf->set_dt(dt); + bdf->init(x0, v0, a0, m_bodies->num_bodies(), pos_ndof, rot_ndof); + m_time_integrator = bdf; +} + +// ---------------------------------------------------------------------------- +// Energy terms +// ---------------------------------------------------------------------------- + +void Simulator::initialize_terms() +{ + affine::AugmentedLagrangian::Params al_params; + al_params.initial_penalty = m_settings.al_initial_penalty; + al_params.max_penalty = m_settings.al_max_penalty; + al_params.satisfied_progress = m_settings.al_satisfied_progress; + al_params.stall_progress = m_settings.al_stall_progress; + + // The orthogonality penalty only applies to the affine (identity) map; the + // BodyPotentials ignores it for the rigid map. + m_body_potentials.emplace( + *m_bodies, m_time_integrator, m_kinematics->to_affine(), + m_settings.orthogonality_stiffness, al_params); + m_body_potentials->set_gravity(m_settings.gravity); +} + +void Simulator::update_terms() { m_body_potentials->update(*m_bodies); } + +void Simulator::set_gravity(Eigen::ConstRef gravity) +{ + m_settings.gravity = gravity; + if (m_body_potentials) { + m_body_potentials->set_gravity(gravity); + } +} + +// ---------------------------------------------------------------------------- +// Full-space (DOF) energy evaluation +// ---------------------------------------------------------------------------- + +double Simulator::energy(Eigen::ConstRef x) const +{ + // Every term except the inertia is a pure potential (energy units) and is + // scaled by (βΔt)² so the incremental potential is uniformly in kg·m² + // [Lan et al. 2022, Eq. 9]; β changes as the BDF order ramps up, so query + // the integrator on every evaluation. + const double s = m_time_integrator->acceleration_scaling(); + + // Inertia + s·(body forces + orthogonality + augmented Lagrangian), pulled + // back to the DOFs through one to-affine map. + double energy = m_body_potentials->energy(*m_bodies, x, s); + + energy += + s + * (*m_barrier_potential)( + *m_normal_collisions, *m_bodies, m_kinematics->world_vertices(x)); + + if (friction_enabled() && !m_tangential_collisions->empty()) { + // The friction term (βΔt)²·(βΔt)·D(v(x)) contributes the friction + // force −μλ to the equations of motion: its x-gradient is + // (βΔt)²·μλf₁t̂, matching the (βΔt)²-scaled force terms. + energy += + s * m_time_integrator->velocity_scaling() + * (*m_friction_potential)( + *m_tangential_collisions, *m_bodies, friction_velocities(x)); + } + + return energy; +} + +Eigen::VectorXd +Simulator::non_barrier_gradient(Eigen::ConstRef x) const +{ + // The physical body forces only (augmented Lagrangian excluded) so the + // adaptive barrier stiffness is seeded from the physical forces, as the + // original Rigid IPC solver does. + return m_body_potentials->gradient( + *m_bodies, x, m_time_integrator->acceleration_scaling(), + /*include_al=*/false); +} + +Eigen::VectorXd +Simulator::barrier_gradient(Eigen::ConstRef x) const +{ + return m_kinematics->map_vertex_gradient( + x, + m_barrier_potential->gradient( + *m_normal_collisions, *m_bodies, m_kinematics->world_vertices(x))); +} + +Eigen::MatrixXd +Simulator::friction_velocities(Eigen::ConstRef x) const +{ + // v(x) = (V(x) − Σᵢ wᵢ V(xᵗ⁻ⁱ)) / (βΔt): the time integrator's velocity + // formula applied to the world-vertex trajectories. + assert(m_friction_vertex_history.size() > 0); + return (m_kinematics->world_vertices(x) - m_friction_vertex_history) + / m_time_integrator->velocity_scaling(); +} + +Eigen::VectorXd +Simulator::friction_gradient(Eigen::ConstRef x) const +{ + if (!friction_enabled() || m_tangential_collisions->empty()) { + return Eigen::VectorXd::Zero(x.size()); + } + // ∇ₓ[(βΔt)³ D(v(x))] = (βΔt)² Jᵀ ∇ᵥD since ∂v/∂V = 1/(βΔt). + return m_time_integrator->acceleration_scaling() + * m_kinematics->map_vertex_gradient( + x, + m_friction_potential->gradient( + *m_tangential_collisions, *m_bodies, friction_velocities(x))); +} + +Eigen::SparseMatrix +Simulator::full_hessian(Eigen::ConstRef x) const +{ + const double s = m_time_integrator->acceleration_scaling(); + + // Inertia + s·(body forces + orthogonality + augmented Lagrangian), pulled + // back to the DOFs through one to-affine map. The rotation block is PSD- + // projected once (ABS for rigid, CLAMP for affine, matching the historical + // per-term choices). + const PSDProjectionMethod body_psd = m_project_to_psd + ? (is_affine() ? PSDProjectionMethod::CLAMP : PSDProjectionMethod::ABS) + : PSDProjectionMethod::NONE; + Eigen::SparseMatrix hess = m_body_potentials->hessian( + *m_bodies, x, s, body_psd, /*include_al=*/true); + + // Contact terms are projected to PSD per contact (mirrors Rigid IPC). + const PSDProjectionMethod contact_psd = m_project_to_psd + ? PSDProjectionMethod::CLAMP + : PSDProjectionMethod::NONE; + const Eigen::MatrixXd V = m_kinematics->world_vertices(x); + + // Barrier contact Hessian. + if (is_affine()) { + // Affine parameterization is linear: project the per-contact vertex + // Hessian (interior) and map with Jᵀ H J. + const Eigen::VectorXd barrier_grad = + m_barrier_potential->gradient(*m_normal_collisions, *m_bodies, V); + const Eigen::SparseMatrix barrier_hess = + m_barrier_potential->hessian( + *m_normal_collisions, *m_bodies, V, contact_psd); + hess = hess + + s + * m_kinematics->map_vertex_hessian( + x, barrier_grad, barrier_hess); + } else { + // Rigid: form each contact's rigid-DOF block (Jᵀ H J + geometric term) + // and CLAMP-project the completed block per contact (following Rigid + // IPC's apply_chain_rule). Projecting after the indefinite geometric + // term is what keeps Newton conditioned on stiff contact. + const std::vector poses = + rigid::Pose::to_poses(x, m_bodies->dim()); + hess = + hess + + rigid::per_contact_rigid_hessian( + *m_bodies, poses, *m_normal_collisions, *m_barrier_potential, V, + /*gn_scale=*/s, /*geom_scale=*/s, contact_psd); + } + + if (friction_enabled() && !m_tangential_collisions->empty()) { + // ∇²ₓ[(βΔt)³ D(v(x))] = (βΔt) Jᵀ ∇²ᵥD J + (βΔt)² Σ ∇ᵥD·d²V/dx² + // (∂v/∂V = 1/(βΔt) is constant): the Gauss–Newton term carries an extra + // 1/(βΔt) = 1/velocity_scaling relative to the geometric term. + const Eigen::MatrixXd velocities = friction_velocities(x); + const double gn_scale = s / m_time_integrator->velocity_scaling(); + if (is_affine()) { + const Eigen::VectorXd friction_grad = s + * m_friction_potential->gradient( + *m_tangential_collisions, *m_bodies, velocities); + const Eigen::SparseMatrix friction_hess = gn_scale + * m_friction_potential->hessian( + *m_tangential_collisions, *m_bodies, velocities, + contact_psd); + hess = hess + + m_kinematics->map_vertex_hessian( + x, friction_grad, friction_hess); + } else { + const std::vector poses = + rigid::Pose::to_poses(x, m_bodies->dim()); + hess = hess + + rigid::per_contact_rigid_hessian( + *m_bodies, poses, *m_tangential_collisions, + *m_friction_potential, velocities, gn_scale, + /*geom_scale=*/s, contact_psd); + } + } + + return hess; +} + +// ---------------------------------------------------------------------------- +// Joint reduction [Chen et al. 2022, §4.1] +// ---------------------------------------------------------------------------- + +bool Simulator::has_joints() const +{ + return m_joints != nullptr && !m_joints->empty(); +} + +Eigen::VectorXd Simulator::to_full(Eigen::ConstRef v) const +{ + return has_joints() ? m_joints->to_full(v) : Eigen::VectorXd(v); +} + +// ---------------------------------------------------------------------------- +// polysolve::nonlinear::Problem implementation +// ---------------------------------------------------------------------------- + +double Simulator::value(const TVector& v) { return energy(to_full(v)); } + +void Simulator::gradient(const TVector& v, TVector& grad) +{ + const Eigen::VectorXd x = to_full(v); + const double s = m_time_integrator->acceleration_scaling(); + // Body potentials incl. the augmented Lagrangian (non_barrier_gradient + // excludes it), plus the contact terms via the vertex-space chain rule. + grad = m_body_potentials->gradient(*m_bodies, x, s, /*include_al=*/true) + + s * barrier_gradient(x) + friction_gradient(x); + if (has_joints()) { + // The pinned entries never move: zero their gradient so the search + // direction leaves them fixed. + grad = m_joints->reduction_basis().transpose() * grad; + grad.head(m_joints->num_constraints()).setZero(); + } + // Same treatment for the masked DOFs (STATIC bodies, fixed DOFs, and + // AL-satisfied kinematic channels). + for (const int i : m_pinned_dofs) { + grad(i) = 0; + } +} + +void Simulator::hessian(const TVector& v, THessian& hess) +{ + hess = full_hessian(to_full(v)); + if (has_joints()) { + // Reduced Hessian UᵀHU. U is sparse (the identity outside the jointed + // DOFs), so this stays sparse. + const Eigen::SparseMatrix& U = m_joints->reduction_basis(); + hess = U.transpose() * hess * U; + } + + // The joint-constrained coordinates and the masked DOFs never move, so we + // replace their rows/columns with identity: S H S + (I − S) with a + // diagonal selector S, staying sparse. + std::vector pinned(hess.rows(), false); + if (has_joints()) { + std::fill_n(pinned.begin(), m_joints->num_constraints(), true); + } + for (const int i : m_pinned_dofs) { + pinned[i] = true; + } + if (std::none_of(pinned.begin(), pinned.end(), [](bool p) { return p; })) { + return; + } + + Eigen::SparseMatrix S(hess.rows(), hess.cols()); // NOLINT + std::vector> triplets; + triplets.reserve(hess.rows()); + for (int i = 0; i < hess.rows(); ++i) { + if (!pinned[i]) { + triplets.emplace_back(i, i, 1.0); + } + } + S.setFromTriplets(triplets.begin(), triplets.end()); + hess = S * hess * S; + for (int i = 0; i < hess.rows(); ++i) { + if (pinned[i]) { + hess.coeffRef(i, i) = 1.0; + } + } +} + +double Simulator::max_step_size(const TVector& v0, const TVector& v1) +{ + // CCD filter: also updates the collision candidates for the swept + // interval. The min-separation distance both widens the candidate + // inflation and is enforced as the CCD minimum gap. + const double dmin = m_settings.minimum_separation_distance; + return m_kinematics->max_step_size( + to_full(v0), to_full(v1), + INFLATION_RADIUS_MULTIPLIER * m_barrier_potential->dhat() + dmin, + m_broad_phase.get(), dmin); +} + +void Simulator::line_search_begin(const TVector& v0, const TVector& /*v1*/) +{ + // The candidate set may have changed in max_step_size; rebuild the + // collision set at the line-search start so the energy comparisons made + // by the line search are consistent. + update_collisions(to_full(v0), /*update_candidates=*/false); +} + +void Simulator::solution_changed(const TVector& v) +{ + // The solver has moved off this solve's starting iterate (the first + // call, from minimize()'s initialization, passes the start point + // itself; later calls come from line-search trials and accepted steps). + if (m_solver_v.size() != v.size() + || (v.array() != m_solver_v.array()).any()) { + m_solver_stepped = true; + } + + m_solver_v = v; + update_collisions(to_full(v), /*update_candidates=*/false); +} + +double Simulator::step_norm( + const TVector& delta_v, + const polysolve::nonlinear::NormType /*norm_type*/) const +{ + // Guard the velocity convergence criterion until at least one step has + // been taken this solve (the original Rigid IPC solver's `iter > 0` + // check): polysolve tests its Δx criterion on the *proposed* direction + // before the first step, so a quasi-static scene whose per-step motion + // (~g·dt²) is below the tolerance would otherwise freeze forever — the + // step is never taken, and the integrator then re-derives zero velocity. + if (!m_solver_stepped) { + return std::numeric_limits::infinity(); + } + // Maximum world-space vertex displacement of the proposed step. + const double displacement = m_solver_v.size() != delta_v.size() + ? delta_v.norm() // fallback: no base point available + : (m_kinematics->world_vertices(to_full(m_solver_v + delta_v)) + - m_kinematics->world_vertices(to_full(m_solver_v))) + .lpNorm(); + + // While the current iterate falls short of a kinematic target, we tighten + // the velocity criterion. The solve would then end in an augmented + // Lagrangian update, which folds whatever force is still unbalanced into + // the multipliers, so it needs an iterate near equilibrium. On stiff + // contact the default tolerance stops well short of that: the remaining + // Newton step is tiny while large forces are still out of balance. The + // kinematic body then overshoots its target, and the next solves spend + // their iterations pulling it back. On the piston, tightening saves about + // 9% of the Newton iterations; without it the scene still runs cleanly, + // just more slowly. + // + // Once the iterate meets the targets, the usual tolerance applies, as in + // Rigid IPC, whose convergence test only adds the equality constraints to + // the velocity criterion. Tightening the whole solve instead costs about + // as many iterations again as the solve itself on scenes with soft modes + // (e.g., pins spinning freely in their holes), where Newton's last digits + // come slowly. + if (al_active() && m_solver_v.size() == delta_v.size() + && !al_targets_met(to_full(m_solver_v))) { + return displacement / AL_VELOCITY_CONV_TOL_SCALE; + } + return displacement; +} + +void Simulator::post_step(const polysolve::nonlinear::PostStepData& data) +{ + const Eigen::VectorXd x = to_full(data.x); + + const double min_distance = m_normal_collisions->compute_minimum_distance( + *m_bodies, m_kinematics->world_vertices(x)); + + const double kappa = update_barrier_stiffness( + m_prev_min_distance, min_distance, m_max_barrier_stiffness, + m_barrier_potential->stiffness(), m_bbox_diagonal, + m_settings.dhat_epsilon_scale, m_settings.minimum_separation_distance); + + if (kappa != m_barrier_potential->stiffness()) { + logger().debug( + "updated barrier stiffness: κ={:g} min_distance={:g}", kappa, + sqrt(min_distance)); + m_barrier_potential->set_stiffness(kappa); + } + + m_prev_min_distance = min_distance; +} + +// ---------------------------------------------------------------------------- +// Stepping +// ---------------------------------------------------------------------------- + +bool Simulator::run( + const double t_end, const std::function& callback) +{ + if (!has_time_remaining(m_time_integrator->dt(), t_end)) { + logger().warn( + "simulation already complete: t={:g} t_end={:g}", m_t, t_end); + return false; // Simulation already complete + } + + bool step_succeeded = true; + while (step_succeeded + && has_time_remaining(m_time_integrator->dt(), t_end)) { + step_succeeded = step(); + callback(step_succeeded); + } + return step_succeeded; +} + +void Simulator::initialize_step() { update_terms(); } + +void Simulator::initialize_barrier_stiffness(Eigen::ConstRef x) +{ + const Eigen::MatrixXd V = m_kinematics->world_vertices(x); + m_bbox_diagonal = (V.colwise().maxCoeff() - V.colwise().minCoeff()).norm(); + assert(m_bbox_diagonal > 0); + + // We average the rigid mass matrix's diagonal over the free DOFs, as Rigid + // IPC does: the mass for each free translation and the principal moment of + // inertia for each free rotation. Averaging whole-body masses instead lets + // a heavy STATIC/KINEMATIC or DOF-locked body (e.g., the bike chain's + // driven sprocket) inflate κ_min, which is proportional to this value; on + // the bike chain that made κ 6× stiffer than Rigid IPC's and doubled the + // Newton iterations. Both parameterizations use this rigid definition, so + // the affine mode gets the same κ as the rigid one. + double average_mass = 0; + int num_free_dofs = 0; + for (size_t i = 0; i < m_bodies->num_bodies(); ++i) { + const rigid::RigidBody& body = (*m_bodies)[i]; + if (body.type() == rigid::RigidBody::Type::STATIC) { + continue; + } + const auto& fixed = body.is_dof_fixed(); + const int dim = m_bodies->dim(); + for (int k = 0; k < fixed.size(); ++k) { + if (!fixed(k)) { + average_mass += + k < dim ? body.mass() : body.moment_of_inertia()(k - dim); + ++num_free_dofs; + } + } + } + if (num_free_dofs > 0) { + average_mass /= num_free_dofs; + } else { + // Nothing can move, so κ is irrelevant. We still need a positive + // mass for initial_barrier_stiffness, so we fall back to the mean + // body mass. + for (size_t i = 0; i < m_bodies->num_bodies(); ++i) { + average_mass += (*m_bodies)[i].mass(); + } + average_mass /= m_bodies->num_bodies(); + } + + // Sum the gradients of every term except the barrier (as they enter the + // objective, i.e., scaled by (βΔt)² where applicable). + const Eigen::VectorXd grad_energy = non_barrier_gradient(x); + + // initial_barrier_stiffness works in the units of the *raw* barrier, but + // the barrier enters the objective scaled by (a) the potential's physical + // scaling dhat/units(dhat²) (if enabled) and (b) the (βΔt)² acceleration + // scaling applied by the simulator. Remove the stiffness and physical + // scaling from the gradient (both are linear), and convert the resulting + // stiffness back to the potential's units accounting for both scales. + const double dhat = m_barrier_potential->dhat(); + const double physical_scale = m_barrier_potential->use_physical_barrier() + ? (dhat / m_barrier_potential->barrier().units(dhat * dhat)) + : 1.0; + const double barrier_scale = + m_time_integrator->acceleration_scaling() * physical_scale; + + const Eigen::VectorXd grad_barrier = barrier_gradient(x) + / (m_barrier_potential->stiffness() * physical_scale); + + const double kappa = initial_barrier_stiffness( + m_bbox_diagonal, m_barrier_potential->barrier(), dhat, average_mass, + grad_energy, grad_barrier, m_max_barrier_stiffness, + m_settings.min_barrier_stiffness_scale, + m_settings.minimum_separation_distance); + m_barrier_potential->set_stiffness(kappa / barrier_scale); + m_max_barrier_stiffness /= barrier_scale; + + m_prev_min_distance = + m_normal_collisions->compute_minimum_distance(*m_bodies, V); + + logger().debug( + "initial barrier stiffness: κ={:g} κ_max={:g} min_distance={:g}", + m_barrier_potential->stiffness(), m_max_barrier_stiffness, + sqrt(m_prev_min_distance)); +} + +bool Simulator::step() +{ + // Convert expired kinematic bodies to STATIC before the terms refresh + // (the affine mass matrix depends on the types). + for (size_t i = 0; i < m_bodies->num_bodies(); ++i) { + if (m_kinematic_drivers[i].has_value() + && m_kinematic_drivers[i]->expired(m_time_integrator->dt())) { + logger().debug( + "kinematic body {} expired; converting to static", i); + (*m_bodies)[i].convert_to_static(); + m_kinematic_drivers[i].reset(); + } + } + + initialize_step(); + + // For the rigid model this maps the rotation matrices back to canonical + // rotation vectors (‖θ‖ ≤ π), keeping the optimization variable bounded. + Eigen::VectorXd x = + m_kinematics->from_integrator_state(m_time_integrator->x_prev(0)); + + update_collisions(x, /*update_candidates=*/true); + + // We lag the friction normal forces before re-seeding κ. The bodies at + // x were in equilibrium under the previous step's κ, so that κ gives the + // contact forces they actually carry. The re-seeded κ can differ from it + // by orders of magnitude (it tracks the bbox and the current gradients), + // and using it here would scale every friction force by the same factor. + // This matches Rigid IPC, which builds its friction set before the solver + // re-initializes κ. + initialize_friction_step(); + update_friction_collisions(x); + + initialize_barrier_stiffness(x); + + // Reset the augmented Lagrangian toward this step's kinematic targets and + // rebuild the DOF mask (types may have changed above). + al_init(x); + rebuild_dof_mask(); + + // Friction lagging convergence tolerance [Ferguson et al. 2021]; fixed + // for the whole step (initialize_barrier_stiffness refreshes the bbox). + const double eps_d = m_settings.momentum_balance_tol * m_bbox_diagonal; + m_last_momentum_balance = -1; + + // Solve in the reduced coordinates z = Vx with the constrained entries + // pinned when joints are present (this also projects x exactly onto the + // constraints). + Eigen::VectorXd v = x; + if (has_joints()) { + v = m_joints->to_reduced(x); + v.head(m_joints->num_constraints()) = m_joints->rhs(); + } + + // The unified outer loop: each iteration is a full Newton solve followed + // by (a) an augmented Lagrangian update while kinematic targets are + // unsatisfied and (b) a friction re-lag (rebuild the collision and + // friction sets at the new state) until the momentum balance + // ‖∇E + κ∇B + ∇D‖ ≤ eps_d [Ferguson et al. 2021]. + bool converged = true; + double prev_momentum_balance = std::numeric_limits::infinity(); + for (int outer_iter = 0;; ++outer_iter) { + m_solver_v = v; // base point for step_norm() until solution_changed + m_solver_stepped = false; // arm the velocity criterion after one step + + try { + m_solver->minimize(*this, v); + } catch (const std::exception& e) { + // polysolve throws on convergence failures (line search failure, + // iteration limit, non-finite energy, ...). + logger().warn("solve failed: {}", e.what()); + converged = false; + } + x = to_full(v); + + if (!converged) { + break; + } + + bool needs_more = false; + bool friction_needs_more = false; + + // Augmented Lagrangian channel: update the penalties/multipliers at + // the energy-converged state; satisfied channels freeze their DOFs. + if (al_active()) { + al_update(x); + rebuild_dof_mask(); + needs_more = al_active(); + } + + // Friction channel: re-lag and measure how far the solution is from + // a fixed point. + if (friction_enabled()) { + update_collisions(x, /*update_candidates=*/true); + update_friction_collisions(x); + + Eigen::VectorXd grad; + gradient(v, grad); + m_last_momentum_balance = grad.norm(); + logger().debug( + "friction lagging: iteration={:d} momentum_balance={:g} " + "eps_d={:g}", + outer_iter + 1, m_last_momentum_balance, eps_d); + + // Stop on convergence, on the iteration cap (normal termination, + // as in the original Rigid IPC), or when the momentum balance + // stalls: the inner solver's own stopping criteria bound the + // reachable gradient residual (e.g., a resting contact held by + // static friction stops on the velocity criterion with an + // O(force) residual), so require ≥10% improvement per lagging + // iteration to keep going. + const bool improving = + m_last_momentum_balance < 0.9 * prev_momentum_balance; + prev_momentum_balance = m_last_momentum_balance; + + friction_needs_more = m_last_momentum_balance > eps_d && improving + && (m_settings.friction_iterations < 0 + || outer_iter + 1 < m_settings.friction_iterations); + if (!friction_needs_more && m_last_momentum_balance > eps_d) { + logger().debug( + "friction lagging ended above tolerance: " + "momentum_balance={:g} eps_d={:g} iterations={:d}", + m_last_momentum_balance, eps_d, outer_iter + 1); + } + needs_more = needs_more || friction_needs_more; + } else if (needs_more) { + // The AL wants another solve: refresh the collision set at the + // new state (the friction branch above does this otherwise). + update_collisions(x, /*update_candidates=*/true); + } + + if (needs_more && outer_iter + 1 >= m_settings.max_outer_iterations) { + if (al_active()) { + // A kinematic body genuinely failed to reach its target. + logger().warn( + "augmented Lagrangian did not satisfy the kinematic " + "targets in {} outer iterations", + m_settings.max_outer_iterations); + converged = false; + } + needs_more = false; + } + if (!needs_more) { + break; + } + + // We re-seed the barrier stiffness only for a friction re-lag, which + // the original Rigid IPC solver runs as a fresh solve that starts by + // re-initializing κ. Rigid IPC updates the augmented Lagrangian inside + // a single solve instead, so κ keeps whatever it grew to while the + // kinematic bodies were driven toward their targets. We keep it too, + // so our solves follow the same κ schedule as Rigid IPC's. + if (friction_needs_more) { + initialize_barrier_stiffness(x); + } + } + + if (!converged) { + logger().warn("step failed to converge: t={:g}", m_t); + if (m_settings.abort_on_convergence_failure) { + return false; + } + } + + m_pose_history.push_back(m_kinematics->poses(x)); + + m_time_integrator->update(m_kinematics->to_integrator_state(x)); + + m_t += m_time_integrator->dt(); + + // Advance the kinematic scripts (pop scripted poses, count down the + // drive time). + step_kinematic_bodies(); + + return true; +} + +void Simulator::update_collisions( + Eigen::ConstRef x, bool update_candidates) +{ + const Eigen::MatrixXd V = m_kinematics->world_vertices(x); + const double dmin = m_settings.minimum_separation_distance; + + if (update_candidates) { + // Widen the broad-phase inflation to cover the min-separation gap. + m_kinematics->update_candidates( + x, INFLATION_RADIUS_MULTIPLIER * m_barrier_potential->dhat() + dmin, + m_broad_phase.get()); + } + + m_normal_collisions->build( + m_kinematics->candidates(), *m_bodies, V, m_barrier_potential->dhat(), + dmin); +} + +void Simulator::initialize_friction_step() +{ + if (!friction_enabled()) { + return; + } + + // Σᵢ wᵢ V(xᵗ⁻ⁱ): the history part of the integrator's velocity formula + // applied to the world-vertex trajectories. + const std::vector weights = + m_time_integrator->velocity_history_weights(); + assert(!weights.empty()); + m_friction_vertex_history = weights[0] + * m_kinematics->world_vertices( + m_kinematics->from_integrator_state(m_time_integrator->x_prev(0))); + for (size_t i = 1; i < weights.size(); ++i) { + m_friction_vertex_history += weights[i] + * m_kinematics->world_vertices(m_kinematics->from_integrator_state( + m_time_integrator->x_prev(i))); + } +} + +void Simulator::update_friction_collisions(Eigen::ConstRef x) +{ + if (!friction_enabled()) { + return; + } + + // The normal force magnitudes (κ through the barrier potential) and the + // tangent bases are baked in at x and frozen until the next rebuild. + m_tangential_collisions->build( + *m_bodies, m_kinematics->world_vertices(x), *m_normal_collisions, + *m_barrier_potential, m_settings.friction_coefficient); + apply_friction_pairs(); +} + +void Simulator::init_friction_pairs() +{ + m_pair_friction.clear(); + m_vertex_bodies.clear(); + if (m_settings.friction_pairs.empty()) { + return; + } + + for (const FrictionPair& pair : m_settings.friction_pairs) { + if (pair.body0 >= m_bodies->num_bodies() + || pair.body1 >= m_bodies->num_bodies()) { + throw std::invalid_argument( + fmt::format( + "friction pair ({}, {}) references a body that does not exist " + "(there are {} bodies)", + pair.body0, pair.body1, m_bodies->num_bodies())); + } + if (pair.coefficient < 0) { + throw std::invalid_argument( + fmt::format( + "friction pair ({}, {}) has a negative coefficient ({:g})", + pair.body0, pair.body1, pair.coefficient)); + } + m_pair_friction[std::minmax(pair.body0, pair.body1)] = pair.coefficient; + } + + // Rigid bodies collide with all of their vertices, so collision-mesh and + // full vertex indices coincide and body_vertex_start applies to both. + assert(m_bodies->num_vertices() == m_bodies->full_num_vertices()); + m_vertex_bodies.resize(m_bodies->num_vertices()); + for (size_t i = 0; i < m_bodies->num_bodies(); ++i) { + std::fill_n( + m_vertex_bodies.begin() + m_bodies->body_vertex_start(i), + m_bodies->body_num_vertices(i), i); + } +} + +void Simulator::apply_friction_pairs() +{ + if (m_pair_friction.empty()) { + return; + } + + for (size_t ci = 0; ci < m_tangential_collisions->size(); ++ci) { + TangentialCollision& collision = (*m_tangential_collisions)[ci]; + + // Every collision stencil lists one body's vertices before the + // other's, so the first and last vertices name the two bodies. A + // plane-vertex collision has a single vertex and keeps the global + // coefficient, since an analytic plane is not a body. + const auto ids = + collision.vertex_ids(m_bodies->edges(), m_bodies->faces()); + const auto* const last = std::find(ids.begin(), ids.end(), -1) - 1; + if (last == ids.begin()) { + continue; + } + const size_t b0 = m_vertex_bodies[ids.front()]; + const size_t b1 = m_vertex_bodies[*last]; + + const auto it = m_pair_friction.find(std::minmax(b0, b1)); + if (it != m_pair_friction.end()) { + collision.mu_s = collision.mu_k = it->second; + } + } + + // The friction potential reads the lagged effective coefficients, which + // build() copied from the coefficients we just replaced. + m_tangential_collisions->reset_lagged_anisotropic_friction_coefficients(); +} + +// ---------------------------------------------------------------------------- +// Kinematic/static bodies +// ---------------------------------------------------------------------------- + +bool Simulator::has_kinematic_bodies() const +{ + return m_bodies->count_type(rigid::RigidBody::Type::KINEMATIC) > 0; +} + +std::vector Simulator::kinematic_targets() const +{ + std::vector targets(m_bodies->num_bodies()); + + const double h = m_time_integrator->dt(); + + for (size_t i = 0; i < m_bodies->num_bodies(); ++i) { + // Skip non-kinematic bodies + if (!m_kinematic_drivers[i].has_value()) { + continue; + } + + affine::Pose pose = m_time_integrator->pose(i); + // Kinematic bodies track a rigid motion, so their A is a rotation — but + // only to the affine solver's convergence tolerance. Snap it to the + // exact nearest rotation before reading off the rigid rotation vector. + pose.rotation = nearest_rotation(pose.rotation); + + // The current pose (position + rotation vector/angle). + rigid::Pose current; + current.position = pose.position; + current.rotation = pose.rotation_vector(); + + // We drive the body at its prescribed (initial) velocity rather than + // the velocity it actually reached last step. The augmented + // Lagrangian accepts a step once it covers 99.9% of the way to the + // target, so under load the body falls slightly short each step. If we + // fed that achieved velocity back in, every shortfall would carry into + // all later steps and the body would steadily slow down (and keep any + // sideways drift it picked up). Using the prescribed velocity keeps + // each step's error to that step alone, as in Rigid IPC. + const rigid::Pose velocity = + free_velocity(m_initial_velocities[i], (*m_bodies)[i]); + + targets[i] = m_kinematic_drivers[i]->target(current, velocity, h); + } + return targets; +} + +void Simulator::al_init(Eigen::ConstRef x) +{ + const std::vector targets = kinematic_targets(); + m_body_potentials->init_augmented_lagrangian( + *m_bodies, x, targets, m_time_integrator->acceleration_scaling()); +} + +bool Simulator::al_active() const +{ + return m_body_potentials->augmented_lagrangian_active(); +} + +bool Simulator::al_targets_met(Eigen::ConstRef x) const +{ + const double satisfied = m_settings.al_satisfied_progress; + return (m_body_potentials->augmented_lagrangian_linear_satisfied() + || m_body_potentials->augmented_lagrangian_linear_progress( + *m_bodies, x) + >= satisfied) + && (m_body_potentials->augmented_lagrangian_angular_satisfied() + || m_body_potentials->augmented_lagrangian_angular_progress( + *m_bodies, x) + >= satisfied); +} + +void Simulator::al_update(Eigen::ConstRef x) +{ + m_body_potentials->update_augmented_lagrangian(*m_bodies, x); +} + +void Simulator::rebuild_dof_mask() +{ + m_pinned_dofs.clear(); + + const int dim = m_bodies->dim(); + const int body_ndof = is_affine() ? (dim + dim * dim) : (dim == 2 ? 3 : 6); + const bool linear_satisfied = + m_body_potentials->augmented_lagrangian_linear_satisfied(); + const bool angular_satisfied = + m_body_potentials->augmented_lagrangian_angular_satisfied(); + + std::vector pinned(body_ndof * m_bodies->num_bodies(), false); + for (size_t i = 0; i < m_bodies->num_bodies(); ++i) { + const rigid::RigidBody& body = (*m_bodies)[i]; + const auto& fixed = body.is_dof_fixed(); + const int pos_ndof = dim, rot_flags = fixed.size() - pos_ndof; + const bool kinematic = body.type() == rigid::RigidBody::Type::KINEMATIC; + const bool is_static = body.type() == rigid::RigidBody::Type::STATIC; + + // Positional DOFs (world axes; identical layout in both modes). + for (int k = 0; k < pos_ndof; ++k) { + if (is_static || fixed(k) || (kinematic && linear_satisfied)) { + pinned[body_ndof * i + k] = true; + } + } + + // Rotational DOFs. + const bool pin_rotation = is_static || (kinematic && angular_satisfied) + || fixed.tail(rot_flags).all(); + if (is_affine()) { + // Whole-rotation only (partial rejected at construction). + if (pin_rotation) { + for (int k = dim; k < body_ndof; ++k) { + pinned[body_ndof * i + k] = true; + } + } + } else { + for (int k = 0; k < rot_flags; ++k) { + if (pin_rotation || fixed(pos_ndof + k)) { + pinned[body_ndof * i + pos_ndof + k] = true; + } + } + } + } + + // Translate to solver coordinates (identity without joints; through the + // reduction's identity-on-unjointed-DOF property with joints). + for (size_t j = 0; j < pinned.size(); ++j) { + if (!pinned[j]) { + continue; + } + m_pinned_dofs.push_back( + has_joints() ? m_joints->free_reduced_index(int(j)) : int(j)); + } +} + +void Simulator::step_kinematic_bodies() +{ + const double dt = m_time_integrator->dt(); + for (auto& driver : m_kinematic_drivers) { + if (driver.has_value()) { + driver->step(dt); + } + } +} + +void Simulator::set_kinematic_driver( + const size_t body, const KinematicDriver& driver) +{ + if ((*m_bodies)[body].type() != rigid::RigidBody::Type::KINEMATIC) { + throw std::invalid_argument( + "set_kinematic_driver requires the body to be KINEMATIC"); + } + // Also update the reset() snapshot so reset() restores this driver. + m_kinematic_drivers[body] = driver; + m_initial_kinematic_drivers[body] = driver; +} + +// ---------------------------------------------------------------------------- +// Pose history +// ---------------------------------------------------------------------------- + +namespace { + std::vector + to_rigid_poses(const std::vector& affine_poses) + { + std::vector rigid_poses(affine_poses.size()); + for (size_t i = 0; i < affine_poses.size(); ++i) { + // In affine mode A is a rotation only to the solver's tolerance + // (a pinned kinematic body keeps whatever the last solve left), so + // we read the angle off the nearest rotation, as + // kinematic_targets() does. + affine::Pose pose = affine_poses[i]; + pose.rotation = nearest_rotation(pose.rotation); + rigid_poses[i].position = pose.position; + rigid_poses[i].rotation = pose.rotation_vector(); + } + return rigid_poses; + } +} // namespace + +std::vector Simulator::rigid_poses() const +{ + return to_rigid_poses(m_pose_history.back()); +} + +std::list> Simulator::rigid_pose_history() const +{ + std::list> history; + for (const auto& poses : m_pose_history) { + history.push_back(to_rigid_poses(poses)); + } + return history; +} + +void Simulator::reset() +{ + m_t = 0.0; + // Reset pose history to only contain the initial poses + m_pose_history.resize(1); + + // Rebuild the time integrator and the energy terms (the terms hold + // references to the integrator) + initialize_time_integrator( + m_initial_poses, m_initial_velocities, m_time_integrator->dt()); + initialize_terms(); + + m_kinematics->clear_candidates(); + m_normal_collisions->clear(); + m_tangential_collisions->clear(); + m_friction_vertex_history.resize(0, 0); + m_last_momentum_balance = -1; + + // Restore the body state and kinematic drivers (kinematic bodies mutate + // to STATIC on expiry). + for (size_t i = 0; i < m_bodies->num_bodies(); ++i) { + rigid::RigidBody& body = (*m_bodies)[i]; + body.set_type(m_initial_body_types[i]); + body.set_is_dof_fixed(m_initial_is_dof_fixed[i]); + } + m_kinematic_drivers = m_initial_kinematic_drivers; + rebuild_dof_mask(); +} +} // namespace ipc::demo diff --git a/demo/src/simulator.hpp b/demo/src/simulator.hpp new file mode 100644 index 000000000..5fb027739 --- /dev/null +++ b/demo/src/simulator.hpp @@ -0,0 +1,723 @@ +#pragma once + +#include "kinematic_driver.hpp" +#include "kinematics.hpp" + +#include +#include + +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace spdlog { +class logger; +} // namespace spdlog + +namespace ipc { +class BarrierPotential; +class BroadPhase; +class Candidates; +class FrictionPotential; +class NormalCollisions; +class TangentialCollisions; +} // namespace ipc + +namespace ipc::rigid { +class RigidBodies; +} // namespace ipc::rigid + +namespace ipc::affine { +class JointConstraints; +} // namespace ipc::affine + +#include + +namespace ipc::dynamics { +class ImplicitTimeIntegrator; +} // namespace ipc::dynamics + +namespace polysolve::nonlinear { +class Solver; +} // namespace polysolve::nonlinear + +namespace ipc::demo { + +/// @brief Body dynamics simulator with a rigid/affine toggle. +/// +/// Minimizes an incremental potential (inertia + body forces +/// [+ orthogonality in affine mode] + barrier) over the DOFs of the selected +/// dynamics model with polysolve's Newton solver: +/// - RIGID [Ferguson et al. 2021]: 6-DOF [p; θ] per body with +/// curved-trajectory (nonlinear) CCD. +/// - AFFINE [Lan et al. 2022]: 12-DOF [p; vec(A)] per body with a stiff +/// orthogonality potential and linear CCD (exact for affine trajectories); +/// supports linear equality joint constraints [Chen et al. 2022]. +/// +/// The simulator is itself the polysolve::nonlinear::Problem it solves: the +/// Problem interface (value/gradient/hessian, CCD max_step_size, collision +/// state hooks) is implemented below. When joint constraints are present the +/// Problem interface operates in the reduced coordinates z (x = Uz with the +/// first m entries pinned [Chen et al. 2022, §4.1]); otherwise its +/// coordinates are the DOFs x directly. +class Simulator : public polysolve::nonlinear::Problem { +public: + /// @brief The dynamics model used to simulate the bodies. + enum class BodyDynamics : std::uint8_t { + /// @brief Rigid body dynamics [Ferguson et al. 2021]: 6-DOF per body + /// ([position; rotation vector]) with curved-trajectory (nonlinear) + /// CCD. + RIGID, + /// @brief Affine body dynamics [Lan et al. 2022]: 12-DOF per body + /// ([position; vec(A) column-major]) with a stiff orthogonality + /// potential and linear CCD (exact for affine per-iterate + /// trajectories). + AFFINE, + }; + + /// @brief A coefficient of friction for the contacts between two specific + /// bodies, overriding Settings::friction_coefficient. + struct FrictionPair { + /// @brief Index of one of the bodies. + size_t body0 = 0; + /// @brief Index of the other body (the order does not matter). + size_t body1 = 0; + /// @brief Coefficient of friction between the two bodies. + double coefficient = 0; + }; + + /// @brief User-configurable simulation parameters. + struct Settings { + /// @brief The dynamics model used for all bodies. + BodyDynamics body_dynamics = BodyDynamics::RIGID; + /// @brief Order of the BDF time integrator (1 ≤ n ≤ 6; 1 is implicit + /// Euler). The effective order ramps up from 1 over the first n steps + /// as the required history accumulates. + int bdf_order = 1; + /// @brief Barrier activation distance. + double dhat = 1e-3; + /// @brief Minimum separation distance (dmin): the barrier keeps bodies + /// at least this far apart. 0 disables (contact allowed down to touch). + double minimum_separation_distance = 0; + /// @brief Gravitational acceleration. + VectorMax3d gravity = Eigen::Vector3d(0, -9.81, 0); + /// @brief Scale used to premultiply the minimum barrier stiffness. + double min_barrier_stiffness_scale = 1e11; + /// @brief Adaptive barrier stiffness updates if the minimum distance + /// is less than this fraction of the bounding box diagonal. + double dhat_epsilon_scale = 1e-9; + /// @brief Coefficient of friction (0 disables friction) + /// [Ferguson et al. 2021]. + /// @note Friction forces are proportional to the (lagged) barrier + /// normal forces, so their accuracy follows the contact-force + /// accuracy of the solve: for quantitative friction, iterate the + /// lagging to convergence (friction_iterations < 0) and tighten + /// velocity_conv_tol (e.g., 1e-3) so the solver cannot stop while + /// the contact forces are unbalanced. + double friction_coefficient = 0; + /// @brief Per-pair coefficients of friction. Contacts between the two + /// bodies of a pair use its coefficient instead of + /// friction_coefficient, so, for example, a greased pin can slide in + /// its sleeve while the sleeve still grips the wall around it. + /// Contacts with an analytic plane always use friction_coefficient. + std::vector friction_pairs; + /// @brief Smooth friction mollifier speed bound εᵥ (m/s): sliding + /// speeds below this are treated as static [Ferguson et al. 2021]. + double static_friction_speed_bound = 1e-3; + /// @brief Friction lagging iterations: the friction operators (tangent + /// bases and normal forces) are frozen during each solve and rebuilt + /// between solves. 0 disables friction; > 0 caps the number of solves + /// per step (1 = a single lagged solve); < 0 iterates until the + /// momentum balance ‖∇E + κ∇B + ∇D‖ ≤ + /// momentum_balance_tol·bbox_diagonal (up to max_outer_iterations). + int friction_iterations = 1; + /// @brief Friction lagging stops once the momentum balance + /// ‖∇E + κ∇B + ∇D‖ falls below this times the bounding box diagonal + /// [Ferguson et al. 2021]. + /// @note The momentum balance carries the bodies' masses and Δt², so + /// the default can be loose for heavy bodies under a small net force: + /// a block sliding slowly down a ramp, where friction nearly cancels + /// gravity, needs a much smaller value to match the analytic slide. + double momentum_balance_tol = 1e-2; + /// @brief Hard cap on the outer (friction-lagging / augmented + /// Lagrangian) solves per step. + int max_outer_iterations = 50; + /// @brief Initial penalty of the kinematic-body augmented Lagrangian, + /// relative to the body's inertia (reset each step, as are the + /// multipliers) [Ferguson et al. 2021]. + double al_initial_penalty = 1e3; + /// @brief Maximum AL penalty (stop doubling). + double al_max_penalty = 1e8; + /// @brief AL progress η at which a channel is satisfied (DOFs freeze). + double al_satisfied_progress = 0.999; + /// @brief AL progress below which the penalty doubles (otherwise the + /// multipliers update). + double al_stall_progress = 0.99; + /// @brief Stiffness of the orthogonality potential (affine mode). + /// @note The potential also scales by each body's volume and enters + /// the incremental potential as Δt²V⊥ [Lan et al. 2022, Eq. 9]. + /// Lan et al. recommend κ > 100 GPa to keep deformation negligible. + double orthogonality_stiffness = 1e11; + /// @brief Whether to use area weighting for the collision set. + /// @note Default off, matching the original Rigid IPC. + bool use_area_weighting = false; + /// @brief If true, stop the simulation when a step fails to converge. + bool abort_on_convergence_failure = true; + /// @brief Velocity convergence tolerance: converged when the proposed + /// Newton step moves every vertex slower than this × the bounding box + /// diagonal (per unit time) [Ferguson et al. 2021]. Measured in world + /// space through the step_norm() override and enforced through + /// polysolve's Δx criterion (solver_params["x_delta_tol"], set + /// automatically unless already present). While enabled, polysolve's + /// absolute gradient criterion (solver_params["grad_norm_tol"]) also + /// defaults to 0, so this is the only convergence test. Set ≤ 0 to + /// disable, which restores a grad_norm_tol of 1e-6. + double velocity_conv_tol = 1e-2; + /// @brief polysolve nonlinear solver parameters. + /// Validated by polysolve; entries not set here get the defaults of + /// https://github.com/polyfem/polysolve/blob/main/nonlinear-solver-spec.json + // NOTE: The strategy ladder starts PSD-projected (like the original + // Rigid IPC solver) instead of polysolve's default plain Newton, + // whose indefinite-Hessian direction is discarded at every + // contact step; gradient descent is excluded because its + // barrier-scaled directions (‖Δx‖ ~ 1e14) make the CCD-filtered + // line search pathologically slow and never produce a usable + // step. "Backtracking" accepts any energy decrease, matching the + // original Rigid IPC line search; Armijo's sufficient-decrease + // condition is unattainable along CCD-capped steps into the stiff + // barrier (the predicted decrease c·α·|gᵀd| dwarfs the achievable + // one), which stalls the solver at first contact. + // NOTE: residual_tolerance gates directions on the ABSOLUTE linear + // solve residual ‖HΔx + g‖, which is meaningless at our scales + // (κ ~ 1e11 puts ‖g‖ at ~1e13); it is effectively disabled, + // leaving the descent-direction check and the line search as the + // safety net (exactly the original Rigid IPC solver's guards). + // NOTE: We leave grad_norm_tol unset on purpose so the constructor + // can pick it. polysolve's gradient criterion is absolute, so + // any fixed value depends on the scene's mass and length + // scale: at 1e-6, a 7 g coin (gradients ~1e-11) stops after + // one Newton iteration every step. With the velocity + // criterion on, the constructor sets it to 0 so the velocity + // criterion alone decides convergence (as in the original + // Rigid IPC solver); otherwise it falls back to 1e-6. Set it + // here to override either choice. + nlohmann::json solver_params = { + { "solver", + { { { "type", "ProjectedNewton" }, + { "residual_tolerance", 1e100 } }, + { { "type", "RegularizedProjectedNewton" }, + { "residual_tolerance", 1e100 } } } }, + { "allow_non_grad_convergence", true }, + { "max_iterations", 100 }, + { "line_search", + { { "method", "Backtracking" }, { "min_step_size", 1e-12 } } }, + }; + /// @brief polysolve linear solver parameters. + /// @note SimplicialLDLT (a direct solver, as in the original Rigid IPC + /// solver) is selected explicitly: polysolve's fallback default + /// is the iterative Eigen::BiCGSTAB, which does not converge on + /// the severely ill-conditioned barrier systems (entries spanning + /// ~1e2 to ~1e14), causing spurious direction failures. + nlohmann::json linear_solver_params = { + { "solver", "Eigen::SimplicialLDLT" }, + }; + /// @brief Verbosity of polysolve's own logger (separate from the + /// toolkit's logger). + /// @note Silenced by default: polysolve logs a per-solve "Finished" + /// report at *error* level whenever the stop was not the gradient + /// criterion — i.e., on every velocity-criterion stop — and + /// per-iteration [timing] lines at debug. Lower this (e.g., to warn + /// or debug) to see them; solve failures are always reported through + /// the toolkit's logger regardless. + spdlog::level::level_enum solver_log_level = spdlog::level::info; + }; + + /// @brief Create a simulator. + /// @param bodies Bodies in the simulation (rest geometry and mass properties) + /// @param initial_poses Initial (rigid) poses of the bodies + /// @param dt Time step + Simulator( + const std::shared_ptr& bodies, + const std::vector& initial_poses, + const double dt); + + /// @copydoc Simulator(const std::shared_ptr&, const std::vector&, const double) + /// @param settings Simulation settings + Simulator( + const std::shared_ptr& bodies, + const std::vector& initial_poses, + const double dt, + const Settings& settings); + + /// @brief Create a simulator with initial velocities. + /// @param initial_velocities Initial velocities of the bodies: position + /// is the linear velocity and rotation is the world-frame angular + /// velocity ω (Q̇ = [ω]× Q). + Simulator( + const std::shared_ptr& bodies, + const std::vector& initial_poses, + const std::vector& initial_velocities, + const double dt, + const Settings& settings); + + /// @brief Create a simulator with joint constraints. + /// @param joints Linear equality joint constraints (finalized by this + /// constructor if not already). + /// @throws std::invalid_argument unless settings.body_dynamics == AFFINE + /// (material-point constraints are nonlinear in the rigid rotation + /// vectors). + Simulator( + const std::shared_ptr& bodies, + const std::vector& initial_poses, + const std::shared_ptr& joints, + const double dt); + + /// @copydoc Simulator(const std::shared_ptr&, const std::vector&, const std::shared_ptr&, const double) + Simulator( + const std::shared_ptr& bodies, + const std::vector& initial_poses, + const std::shared_ptr& joints, + const double dt, + const Settings& settings); + + /// @brief Create a simulator (master constructor). + Simulator( + const std::shared_ptr& bodies, + const std::vector& initial_poses, + const std::vector& initial_velocities, + const std::shared_ptr& joints, + const double dt, + const Settings& settings); + + ~Simulator(); + + /// @brief Run the simulation. + /// + /// @param t_end End time + /// @param callback Callback function to be called at each step. + /// + /// The callback function takes a boolean argument indicating whether the + /// step was successful (i.e., did not fail to converge). The callback is + /// called after each step, including the final step that reaches t_end. + /// + /// If the simulation is already complete (i.e., t >= t_end), the + /// simulation will not run and the callback will not be called. + /// + /// @return True if the simulation ran successfully, false if it was terminated (e.g., due to convergence failure) + bool run( + const double t_end, + const std::function& callback = [](bool) { }); + + /// @brief Step the simulation. + /// @return True if the step was successful, false if the simulation should be terminated (e.g., due to convergence failure) + bool step(); + + /// @brief Reset the simulation to time t=0. + void reset(); + + // ----------------------------------------------------------------------- + // polysolve::nonlinear::Problem implementation + // + // NOTE: When joint constraints are present these operate in the reduced + // coordinates z (x = Uz); otherwise in the DOFs x directly. + // ----------------------------------------------------------------------- + + /// @brief Compute the incremental potential at v. + double value(const TVector& v) override; + + /// @brief Compute the gradient of the incremental potential at v. + void gradient(const TVector& v, TVector& grad) override; + + /// @brief Compute the (sparse) Hessian of the incremental potential at v. + /// @note Projected to PSD per term unless set_project_to_psd(false). + void hessian(const TVector& v, THessian& hess) override; + + /// @brief Compute a collision-free maximum step size from v0 to v1 ∈ [0, 1]. + /// Also updates the collision candidates for the swept interval (CCD). + double max_step_size(const TVector& v0, const TVector& v1) override; + + /// @brief Rebuild the collision set at the line-search start (the + /// candidates may have changed in max_step_size). + void line_search_begin(const TVector& v0, const TVector& v1) override; + + /// @brief Rebuild the collision set at the new iterate. + void solution_changed(const TVector& v) override; + + /// @brief Adaptively update the barrier stiffness after an accepted iterate. + void post_step(const polysolve::nonlinear::PostStepData& data) override; + + /// @brief Set whether hessian() projects each term to PSD. + void set_project_to_psd(bool val) override { m_project_to_psd = val; } + + /// @brief Norm of a proposed step: the maximum world-space vertex + /// displacement it produces (from the current iterate). + /// + /// polysolve's Δx stopping criterion uses this norm, so with + /// Settings::velocity_conv_tol this reproduces the velocity convergence + /// criterion of Rigid IPC [Ferguson et al. 2021] in a space where a + /// single scalar tolerance is meaningful (raw DOF-space Δx mixes + /// translations with rotation-vector radians). While a kinematic target + /// is unmet, the norm is scaled up so the criterion is tighter, giving + /// the augmented Lagrangian update a converged iterate. + double step_norm( + const TVector& delta_v, + const polysolve::nonlinear::NormType norm_type) const override; + + // ----------------------------------------------------------------------- + // Collision handling + // ----------------------------------------------------------------------- + + /// @brief Update the simulation state before stepping (refreshes the energy terms). + void initialize_step(); + + /// @brief Update the collision sets based on the current state x. + /// @param x DOF vector. + /// @param update_candidates If true, also update the candidate collisions based on the current state. If false, only update the normal collision set based on the current candidate collisions. + void update_collisions( + Eigen::ConstRef x, bool update_candidates = true); + + /// @brief Capture the vertex-space velocity history for the friction term + /// (Σᵢ wᵢ V(xᵗ⁻ⁱ) from the time integrator's velocity formula). Called at + /// the start of every step; kept fixed through all lagging iterations. + void initialize_friction_step(); + + /// @brief Rebuild the tangential (friction) collision set at the lagged + /// state x (the normal collision set must be up to date at x). The + /// tangent bases and normal force magnitudes are baked in at x — the + /// friction lagging [Ferguson et al. 2021]. + void update_friction_collisions(Eigen::ConstRef x); + + /// @brief Whether any body is KINEMATIC. + bool has_kinematic_bodies() const; + + /// @brief Attach a kinematic driver to a KINEMATIC body. + /// + /// KINEMATIC bodies default to a velocity-driven driver at construction; + /// call this to script their motion or set a finite drive time. Call after + /// construction (the body must already be KINEMATIC). The driver is also + /// captured as the reset() state. + /// @param body Index of the body (must be KINEMATIC). + /// @param driver The kinematic driver. + void set_kinematic_driver(const size_t body, const KinematicDriver& driver); + + // ----------------------------------------------------------------------- + // Accessors + // ----------------------------------------------------------------------- + + /// @brief History of the (affine) poses at each time step. + /// @note In rigid mode the rotation part is exactly R(θ). + const std::list>& pose_history() const + { + return m_pose_history; + } + + /// @brief History of the poses converted to rigid poses (log map). + /// @note Exact in rigid mode; in affine mode the rotation part of the + /// pose must be (numerically) a rotation matrix. + std::list> rigid_pose_history() const; + + /// @brief The current (most recent) affine poses. + /// @note O(num_bodies); prefer this over pose_history().back() for + /// per-frame access, since the full history is copied on each access. + const std::vector& poses() const + { + return m_pose_history.back(); + } + + /// @brief The current (most recent) poses as rigid poses (log map). + /// @note O(num_bodies); the per-frame counterpart to rigid_pose_history(). + std::vector rigid_poses() const; + + const rigid::RigidBodies& bodies() const { return *m_bodies; } + + double t() const { return m_t; } + + const Settings& settings() const { return m_settings; } + + VectorMax3d gravity() const { return m_settings.gravity; } + void set_gravity(Eigen::ConstRef gravity); + + /// @brief Get the barrier potential for collision handling. + const BarrierPotential& barrier_potential() const + { + return *m_barrier_potential; + } + + /// @brief Get the friction potential (dissipative, velocity-based). + const FrictionPotential& friction_potential() const + { + return *m_friction_potential; + } + + /// @brief Get the tangential (friction) collision set. + const TangentialCollisions& tangential_collisions() const + { + return *m_tangential_collisions; + } + + /// @brief The momentum balance ‖∇E + κ∇B + ∇D‖ after the last friction + /// lagging iteration (-1 before any friction solve). + double last_momentum_balance() const { return m_last_momentum_balance; } + + /// @brief Get the candidate collisions for the current time step. + const Candidates& candidates() const { return m_kinematics->candidates(); } + + /// @brief Get the normal collision set for the current time step. + const NormalCollisions& normal_collisions() const + { + return *m_normal_collisions; + } + + /// @brief Get the DOF model (DOF layout, world map, and CCD strategy). + const Kinematics& kinematics() const { return *m_kinematics; } + +protected: + /// @brief Check if there is time remaining in the simulation to take another step. + /// + /// This function accounts for floating point precision issues by checking + /// if m_t is less than t_end and that the difference between m_t and t_end + /// is greater than a small fraction of dt. + /// + /// @param dt Time step size + /// @param t_end End time of the simulation + /// @return True if there is time remaining to take another step, false otherwise + bool has_time_remaining(const double dt, const double t_end) const + { + return m_t < t_end && std::abs(m_t - t_end) > dt * 1e-3; + } + + /// @brief Build the time-integrator state from poses and velocities. + void initialize_time_integrator( + const std::vector& poses, + const std::vector& velocities, + const double dt); + + /// @brief (Re)build the energy terms of the incremental potential. + /// Called on construction and reset because the terms hold a reference to + /// the time integrator. + void initialize_terms(); + + /// @brief Refresh the terms at the start of a time step (predicted + /// positions and body-force wrenches). + void update_terms(); + + /// @brief Compute the initial (adaptive) barrier stiffness for this step. + void initialize_barrier_stiffness(Eigen::ConstRef x); + + // ---- Joint reduction [Chen et al. 2022, §4.1] -------------------------- + + /// @brief Whether joint constraints are active. + bool has_joints() const; + + /// @brief Map the solver coordinates v to the full DOFs x (x = Uv when + /// joints are present; identity otherwise). + Eigen::VectorXd to_full(Eigen::ConstRef v) const; + + // ---- Full-space (DOF) energy evaluation --------------------------------- + + /// @brief Whether the affine terms are engaged (rigid otherwise). + bool is_affine() const + { + return m_settings.body_dynamics == BodyDynamics::AFFINE; + } + + /// @brief Total incremental potential at the full DOFs x. + double energy(Eigen::ConstRef x) const; + + /// @brief Gradient of every term except the barrier, as the terms enter + /// the objective (i.e., scaled by (βΔt)² where applicable; also used to + /// initialize the adaptive barrier stiffness). + Eigen::VectorXd + non_barrier_gradient(Eigen::ConstRef x) const; + + /// @brief Barrier energy gradient chained to the DOFs: Jᵀ ∇B. + /// @note Not scaled by the (βΔt)² acceleration scaling; callers apply it. + Eigen::VectorXd barrier_gradient(Eigen::ConstRef x) const; + + /// @brief Whether the friction term is active. + bool friction_enabled() const + { + const bool any_friction = m_settings.friction_coefficient > 0 + || std::any_of(m_settings.friction_pairs.begin(), + m_settings.friction_pairs.end(), + [](const FrictionPair& pair) { + return pair.coefficient > 0; + }); + return any_friction && m_settings.friction_iterations != 0; + } + + /// @brief Validate Settings::friction_pairs and build the lookups that + /// apply_friction_pairs() uses. + void init_friction_pairs(); + + /// @brief Overwrite the coefficient of friction of every tangential + /// collision between the two bodies of a Settings::friction_pairs entry. + void apply_friction_pairs(); + + /// @brief Per-vertex velocities at x from the time integrator's velocity + /// formula: (V(x) − Σᵢ wᵢ V(xᵗ⁻ⁱ)) / (βΔt). + Eigen::MatrixXd + friction_velocities(Eigen::ConstRef x) const; + + /// @brief Friction term gradient as it enters the objective: + /// (βΔt)² Jᵀ ∇ᵥD (zero when friction is disabled or contact-free). + Eigen::VectorXd friction_gradient(Eigen::ConstRef x) const; + + // ---- Kinematic/static bodies ------------------------------------------- + + /// @brief Per-body target poses for this step (KINEMATIC bodies: script + /// front or velocity-integrated; others: current pose, unused). + std::vector kinematic_targets() const; + + /// @brief Reset the augmented Lagrangian for a new step at state x. + void al_init(Eigen::ConstRef x); + + /// @brief Whether the augmented Lagrangian still has unsatisfied channels. + bool al_active() const; + + /// @brief Whether the state x already meets every unsatisfied channel's + /// kinematic targets, by the AL's own progress test. + bool al_targets_met(Eigen::ConstRef x) const; + + /// @brief Apply the AL update policy at the (energy-converged) state x. + void al_update(Eigen::ConstRef x); + + /// @brief Rebuild m_pinned_dofs (STATIC bodies, is_dof_fixed flags, and + /// AL-satisfied channels of KINEMATIC bodies) in solver coordinates. + void rebuild_dof_mask(); + + /// @brief Advance kinematic scripts and convert expired bodies to STATIC + /// (called at the end of every step). + void step_kinematic_bodies(); + + /// @brief Total Hessian at the full DOFs x (PSD projected per term if + /// m_project_to_psd). + Eigen::SparseMatrix + full_hessian(Eigen::ConstRef x) const; + + // ----------------------------------------------------------------------- + + /// @brief Simulation settings. + Settings m_settings; + + /// @brief Bodies in the simulation. + std::shared_ptr m_bodies; + + /// @brief DOF model strategy (rigid or affine parameterization). + std::shared_ptr m_kinematics; + + /// @brief Time integrator ([p; vec(Q)] per-body state for both models). + std::shared_ptr m_time_integrator; + + /// @brief Optional linear equality joint constraints (affine mode only). + std::shared_ptr m_joints; + + /// @brief History of poses at each time step. + std::list> m_pose_history; + + /// @brief Initial poses (used by reset()). + std::vector m_initial_poses; + + /// @brief Initial velocities (used by reset()). + std::vector m_initial_velocities; + + /// @brief Barrier potential for collision handling. + std::shared_ptr m_barrier_potential; + + /// @brief Normal collision set. + std::shared_ptr m_normal_collisions; + + /// @brief Friction (dissipative) potential; evaluated on velocities. + std::shared_ptr m_friction_potential; + + /// @brief Tangential (friction) collision set (lagged: rebuilt between + /// solves, frozen during each solve). + std::shared_ptr m_tangential_collisions; + + /// @brief Coefficient of friction per body pair (smaller index first), + /// from Settings::friction_pairs. + std::map, double> m_pair_friction; + + /// @brief The body each collision-mesh vertex belongs to (only filled + /// when there are friction pairs). + std::vector m_vertex_bodies; + + /// @brief Vertex-space velocity history Σᵢ wᵢ V(xᵗ⁻ⁱ) — the reference for + /// the friction velocities (captured at step start, fixed for the step). + Eigen::MatrixXd m_friction_vertex_history; + + /// @brief Momentum balance after the last friction lagging iteration. + double m_last_momentum_balance = -1.0; + + // ---- Kinematic/static bodies ------------------------------------------- + + /// @brief Pinned solver-coordinate indices (zero search direction): + /// STATIC bodies, fixed DOFs, and AL-satisfied kinematic channels. + std::vector m_pinned_dofs; + + /// @brief Per-body kinematic drivers (empty entry ⇒ not kinematic). + /// KINEMATIC bodies get a default velocity-driven driver at construction. + std::vector> m_kinematic_drivers; + + /// @brief Body state at construction (restored by reset()). + std::vector m_initial_body_types; + std::vector m_initial_is_dof_fixed; + std::vector> m_initial_kinematic_drivers; + + /// @brief Broad phase collision handler for efficiently finding potential collisions. + std::unique_ptr m_broad_phase; + + /// @brief Dedicated logger for polysolve (see Settings::solver_log_level). + /// @note Declared before m_solver: the solver holds a reference to it, so + /// it must be destroyed after the solver. + std::shared_ptr m_solver_logger; + + /// @brief polysolve Newton solver used to minimize the incremental potential. + std::unique_ptr m_solver; + + // ---- Energy terms (rebuilt by initialize_terms) ------------------------ + + /// @brief The body (non-contact) part of the incremental potential: inertia, + /// body forces, the orthogonality penalty (affine only), and the augmented + /// Lagrangian, all behind the parameterization's to-affine map. + std::optional m_body_potentials; + + /// @brief Whether hessian() projects each term to PSD (set by polysolve + /// per descent strategy). + bool m_project_to_psd = true; + + /// @brief The solver's current iterate (tracked through + /// solution_changed); the base point for step_norm(). + Eigen::VectorXd m_solver_v; + + /// @brief Whether the solver has moved off the starting iterate this + /// solve; until then step_norm() disables the velocity convergence + /// criterion so at least one Newton step is always taken (the original + /// Rigid IPC solver's `iter > 0` guard). + bool m_solver_stepped = false; + + // ----------------------------------------------------------------------- + + /// @brief Current simulation time. + double m_t = 0.0; + + /// @brief World bounding box diagonal (updated each step). + double m_bbox_diagonal = 1.0; + + /// @brief Maximum barrier stiffness (from initial_barrier_stiffness). + double m_max_barrier_stiffness = 0.0; + + /// @brief Minimum distance at the previous Newton iterate (adaptive κ). + double m_prev_min_distance = -1.0; +}; +} // namespace ipc::demo diff --git a/docs/source/cpp-api/ccd.rst b/docs/source/cpp-api/ccd.rst index 5157c0ff2..85adf5f36 100644 --- a/docs/source/cpp-api/ccd.rst +++ b/docs/source/cpp-api/ccd.rst @@ -11,6 +11,8 @@ Narrow Phase CCD .. doxygenclass:: ipc::NarrowPhaseCCD :allow-dot-graphs: +.. doxygenenum:: ipc::NarrowPhaseCCDMode + Tight Inclusion CCD ^^^^^^^^^^^^^^^^^^^ diff --git a/docs/source/python-api/ccd.rst b/docs/source/python-api/ccd.rst index d0499aee0..9f891a202 100644 --- a/docs/source/python-api/ccd.rst +++ b/docs/source/python-api/ccd.rst @@ -12,6 +12,8 @@ Narrow Phase CCD .. autoclasstoc:: +.. autoclass:: ipctk.NarrowPhaseCCDMode + Tight Inclusion CCD ^^^^^^^^^^^^^^^^^^^ diff --git a/notebooks/abd.ipynb b/notebooks/abd.ipynb new file mode 100644 index 000000000..c700a13d3 --- /dev/null +++ b/notebooks/abd.ipynb @@ -0,0 +1,167 @@ +{ + "cells": [ + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "import sympy\n", + "import numpy as np" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "A = sympy.symbols([f\"a_{{{i}{j}}}\" for i in range(1, 4) for j in \"xyz\"])\n", + "A = np.array(A).reshape(3, 3)\n", + "display(sympy.Matrix(A))" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "def frobenius_norm(A):\n", + " sum = 0\n", + " for i in range(A.shape[0]):\n", + " for j in range(A.shape[1]):\n", + " sum += A[i, j]**2\n", + " return sympy.sqrt(sum)\n", + "\n", + "standard_version = frobenius_norm(A @ A.T - np.eye(3))**2" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "paper_version = sum((A[i].dot(A[i]) - 1)**2 for i in range(3)) \\\n", + " + sum((A[i].dot(A[j]))**2 for i in range(3) for j in range(3) if i != j)" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "(standard_version - paper_version).simplify()" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "for i in range(9):\n", + " for j in range(9):\n", + " display(standard_version.diff(A[i//3, i%3]).diff(A[j//3, j%3]))" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "x = np.array(sympy.symbols([f\"{d}{i}\" for i in range(1, 5) for d in \"xyz\"])).reshape(-1, 3)\n", + "display(sympy.Matrix(x))" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "p = np.array(sympy.symbols(\"p_x p_y p_z\"))\n", + "dof = np.hstack((p, A.flatten(order=\"C\")))" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "J = np.zeros((x.size, 12), dtype=object)\n", + "\n", + "for i in range(x.shape[0]):\n", + " for j in range(x.shape[1]):\n", + " J[x.shape[0]*j + i, j] = 1\n", + "\n", + "# J[:, 3:] = np.kron(np.eye(3, dtype=int), x)\n", + "for i in range(x.shape[0]):\n", + " for j in range(x.shape[1]):\n", + " for k in range(A.shape[0]):\n", + " J[i + k * x.shape[0], j + k * x.shape[1] + p.size] = x[i, j]\n", + "\n", + "display(sympy.Matrix(J))\n", + "\n", + "linear_version = J @ dof\n", + "affine_version = (x @ A.T + p).flatten(order=\"F\")\n", + "\n", + "display(sympy.Matrix(linear_version))\n", + "display(sympy.Matrix(affine_version))\n", + "display(sympy.Matrix(linear_version - affine_version))\n", + "\n", + "display(sympy.Matrix(linear_version).diff(dof))" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "sympy.Function(\"f\")(*(J @ dof)).diff(dof[0])" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "display(sympy.Symbol(r\"\\rho\") * sympy.Matrix(J.T @ J) / x.shape[0])" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [] + } + ], + "metadata": { + "kernelspec": { + "display_name": "Python 3", + "language": "python", + "name": "python3" + }, + "language_info": { + "codemirror_mode": { + "name": "ipython", + "version": 3 + }, + "file_extension": ".py", + "mimetype": "text/x-python", + "name": "python", + "nbconvert_exporter": "python", + "pygments_lexer": "ipython3", + "version": "3.11.7" + } + }, + "nbformat": 4, + "nbformat_minor": 2 +} diff --git a/python/CMakeLists.txt b/python/CMakeLists.txt index 422727b53..212bfb874 100755 --- a/python/CMakeLists.txt +++ b/python/CMakeLists.txt @@ -21,6 +21,17 @@ target_include_directories(ipctk PRIVATE "${CMAKE_CURRENT_SOURCE_DIR}/src") target_link_libraries(ipctk PRIVATE ipc::toolkit) +# Demo simulator bindings (optional; exposed as the ipctk.demo submodule) +if(IPC_TOOLKIT_BUILD_DEMO) + target_sources(ipctk PRIVATE "${PROJECT_SOURCE_DIR}/demo/python/simulator.cpp") + target_link_libraries(ipctk PRIVATE ipc::toolkit::demo) + target_compile_definitions(ipctk PRIVATE IPCTK_WITH_DEMO) + + # nlohmann::json <-> Python dict conversion for the solver parameters + include(pybind11_json) + target_link_libraries(ipctk PRIVATE pybind11::json) +endif() + # libigl (binding igl::edges) include(libigl) target_link_libraries(ipctk PRIVATE igl::core igl::predicates) @@ -53,4 +64,9 @@ endif() # Folder name for IDE set_target_properties(ipctk PROPERTIES FOLDER "SRC") get_target_property(IPCTK_SOURCES ipctk SOURCES) -source_group(TREE "${PROJECT_SOURCE_DIR}/python" FILES ${IPCTK_SOURCES}) \ No newline at end of file +# The demo bindings live outside python/ (demo/python/); group them separately +list(FILTER IPCTK_SOURCES EXCLUDE REGEX "/demo/python/") +source_group(TREE "${PROJECT_SOURCE_DIR}/python" FILES ${IPCTK_SOURCES}) +if(IPC_TOOLKIT_BUILD_DEMO) + source_group("Source Files\\demo" FILES "${PROJECT_SOURCE_DIR}/demo/python/simulator.cpp") +endif() \ No newline at end of file diff --git a/python/_find_ipctk.py b/python/_find_ipctk.py index 03b4576bd..8d853d559 100644 --- a/python/_find_ipctk.py +++ b/python/_find_ipctk.py @@ -14,6 +14,7 @@ pathlib.Path(".").resolve(), pathlib.Path("python").resolve(), repo_root / "build" / "python", + repo_root / "build" / "python" / "python", repo_root / "build" / "release" / "python", repo_root / "build" / "debug" / "python", ] diff --git a/python/example.ipynb b/python/examples/example.ipynb similarity index 80% rename from python/example.ipynb rename to python/examples/example.ipynb index 984ab9b7c..ef853bf7a 100755 --- a/python/example.ipynb +++ b/python/examples/example.ipynb @@ -21,6 +21,7 @@ "import meshio\n", "\n", "import sys\n", + "\n", "sys.path.append(\"../notebooks\") # noqa\n", "\n", "from find_ipctk import ipctk\n", @@ -61,12 +62,14 @@ "dtypes = numpy.empty((xs.size, ys.size), dtype=int)\n", "for i, x in enumerate(xs):\n", " for j, y in enumerate(ys):\n", - " dtypes[j, i] = point_edge_distance_type(\n", - " numpy.array([x, y]), e0, e1)\n", - "fig = go.Figure(data=[\n", - " go.Scatter(x=[e0[0], e1[0]], y=[e0[1], e1[1]]),\n", - " go.Contour(z=dtypes, x=xs, y=ys)\n", - "], layout=go.Layout(width=400, height=400))\n", + " dtypes[j, i] = point_edge_distance_type(numpy.array([x, y]), e0, e1)\n", + "fig = go.Figure(\n", + " data=[\n", + " go.Scatter(x=[e0[0], e1[0]], y=[e0[1], e1[1]]),\n", + " go.Contour(z=dtypes, x=xs, y=ys),\n", + " ],\n", + " layout=go.Layout(width=400, height=400),\n", + ")\n", "fig.show()" ] }, @@ -85,10 +88,13 @@ "for i, x in enumerate(xs):\n", " for j, y in enumerate(ys):\n", " distances[j, i] = point_edge_distance(numpy.array([x, y]), e0, e1)\n", - "fig = go.Figure(data=[\n", - " go.Scatter(x=[e0[0], e1[0]], y=[e0[1], e1[1]]),\n", - " go.Contour(z=numpy.sqrt(distances), x=xs, y=ys)\n", - "], layout=go.Layout(width=800, height=800))\n", + "fig = go.Figure(\n", + " data=[\n", + " go.Scatter(x=[e0[0], e1[0]], y=[e0[1], e1[1]]),\n", + " go.Contour(z=numpy.sqrt(distances), x=xs, y=ys),\n", + " ],\n", + " layout=go.Layout(width=800, height=800),\n", + ")\n", "fig.show()" ] }, @@ -101,23 +107,23 @@ "source": [ "e0 = numpy.array([-1, -1], dtype=float)\n", "e1 = numpy.array([1, 1], dtype=float)\n", - "x, y = numpy.meshgrid(numpy.arange(-2, 2, .2), numpy.arange(-2, 2, .2))\n", + "x, y = numpy.meshgrid(numpy.arange(-2, 2, 0.2), numpy.arange(-2, 2, 0.2))\n", "u, v = numpy.empty(x.shape), numpy.empty(x.shape)\n", "d = numpy.empty(x.shape)\n", "for i in range(x.shape[0]):\n", " for j in range(x.shape[1]):\n", - " grad = point_edge_distance_gradient(\n", - " numpy.array([x[i, j], y[i, j]]), e0, e1)\n", + " grad = point_edge_distance_gradient(numpy.array([x[i, j], y[i, j]]), e0, e1)\n", " u[i, j] = grad[0]\n", " v[i, j] = grad[1]\n", - " d[i, j] = numpy.sqrt(point_edge_distance(\n", - " numpy.array([x[i, j], y[i, j]]), e0, e1))\n", + " d[i, j] = numpy.sqrt(\n", + " point_edge_distance(numpy.array([x[i, j], y[i, j]]), e0, e1)\n", + " )\n", "\n", "fig = ff.create_quiver(x, y, u, v, name=\"point_grad\")\n", "fig.add_trace(go.Scatter(x=[e0[0], e1[0]], y=[e0[1], e1[1]], name=\"edge\"))\n", "fig.update_layout(width=800, height=800)\n", "fig.show()\n", - "fig = ff.create_quiver(x, y, u/(2 * d), v / (2 * d), name=\"point_grad\")\n", + "fig = ff.create_quiver(x, y, u / (2 * d), v / (2 * d), name=\"point_grad\")\n", "fig.add_trace(go.Scatter(x=[e0[0], e1[0]], y=[e0[1], e1[1]], name=\"edge\"))\n", "fig.update_layout(width=800, height=800)\n", "fig.show()" @@ -132,12 +138,11 @@ "source": [ "e0 = numpy.array([-1, -1], dtype=float)\n", "e1 = numpy.array([1, 1], dtype=float)\n", - "x, y = numpy.meshgrid(numpy.arange(-2, 2, .2), numpy.arange(-2, 2, .2))\n", + "x, y = numpy.meshgrid(numpy.arange(-2, 2, 0.2), numpy.arange(-2, 2, 0.2))\n", "d_xx, d_xy, d_yx, d_yy = [numpy.empty(x.shape) for i in range(4)]\n", "for i in range(x.shape[0]):\n", " for j in range(x.shape[1]):\n", - " hess = point_edge_distance_hessian(\n", - " numpy.array([x[i, j], y[i, j]]), e0, e1)\n", + " hess = point_edge_distance_hessian(numpy.array([x[i, j], y[i, j]]), e0, e1)\n", " d_xx[i, j] = hess[0, 0]\n", " d_xy[i, j] = hess[0, 1]\n", " d_yx[i, j] = hess[1, 0]\n", @@ -164,10 +169,13 @@ "for i, x in enumerate(xs):\n", " for j, y in enumerate(ys):\n", " distances[j, i] = point_point_distance(numpy.array([x, y]), p)\n", - "fig = go.Figure(data=[\n", - " go.Scatter(x=[p[0]], y=[p[1]]),\n", - " go.Contour(z=numpy.sqrt(distances), x=xs, y=ys)\n", - "], layout=go.Layout(width=800, height=800))\n", + "fig = go.Figure(\n", + " data=[\n", + " go.Scatter(x=[p[0]], y=[p[1]]),\n", + " go.Contour(z=numpy.sqrt(distances), x=xs, y=ys),\n", + " ],\n", + " layout=go.Layout(width=800, height=800),\n", + ")\n", "fig.show()" ] }, @@ -179,23 +187,21 @@ "outputs": [], "source": [ "p = numpy.array([0, 0], dtype=float)\n", - "x, y = numpy.meshgrid(numpy.arange(-2, 2, .2), numpy.arange(-2, 2, .2))\n", + "x, y = numpy.meshgrid(numpy.arange(-2, 2, 0.2), numpy.arange(-2, 2, 0.2))\n", "u, v = numpy.empty(x.shape), numpy.empty(x.shape)\n", "d = numpy.empty(x.shape)\n", "for i in range(x.shape[0]):\n", " for j in range(x.shape[1]):\n", - " grad = point_point_distance_gradient(\n", - " numpy.array([x[i, j], y[i, j]]), p)\n", + " grad = point_point_distance_gradient(numpy.array([x[i, j], y[i, j]]), p)\n", " u[i, j] = grad[0]\n", " v[i, j] = grad[1]\n", - " d[j, i] = numpy.sqrt(point_point_distance(\n", - " numpy.array([x[i, j], y[i, j]]), p))\n", + " d[j, i] = numpy.sqrt(point_point_distance(numpy.array([x[i, j], y[i, j]]), p))\n", "\n", "fig = ff.create_quiver(x, y, u, v, name=\"point_grad\")\n", "fig.add_trace(go.Scatter(x=[p[0]], y=[p[1]], name=\"edge\"))\n", "fig.update_layout(width=800, height=800)\n", "fig.show()\n", - "fig = ff.create_quiver(x, y, u/(2 * d), v / (2 * d), name=\"point_grad\")\n", + "fig = ff.create_quiver(x, y, u / (2 * d), v / (2 * d), name=\"point_grad\")\n", "fig.add_trace(go.Scatter(x=[p[0]], y=[p[1]], name=\"edge\"))\n", "fig.update_layout(width=800, height=800)\n", "fig.show()" @@ -233,10 +239,13 @@ "for i, x in enumerate(xs):\n", " for j, y in enumerate(ys):\n", " distances[j, i] = point_line_distance(numpy.array([x, y]), e0, e1)\n", - "fig = go.Figure(data=[\n", - " go.Scatter(x=[e0[0], e1[0]], y=[e0[1], e1[1]]),\n", - " go.Contour(z=numpy.sqrt(distances), x=xs, y=ys)\n", - "], layout=go.Layout(width=800, height=800))\n", + "fig = go.Figure(\n", + " data=[\n", + " go.Scatter(x=[e0[0], e1[0]], y=[e0[1], e1[1]]),\n", + " go.Contour(z=numpy.sqrt(distances), x=xs, y=ys),\n", + " ],\n", + " layout=go.Layout(width=800, height=800),\n", + ")\n", "fig.show()" ] }, @@ -281,7 +290,13 @@ "b = numpy.vectorize(lambda x: barrier(x, dhat))(d)\n", "b_grad = numpy.vectorize(lambda x: barrier_first_derivative(x, dhat))(d)\n", "b_hess = numpy.vectorize(lambda x: barrier_second_derivative(x, dhat))(d)\n", - "fig = go.Figure(data=[go.Scatter(x=d, y=b, name=\"b(x)\"), go.Scatter(x=d, y=b_grad, name=r\"\\nabla b(x)\"), go.Scatter(x=d, y=b_hess, name=r\"\\nabla^2 b(x)\")])\n", + "fig = go.Figure(\n", + " data=[\n", + " go.Scatter(x=d, y=b, name=\"b(x)\"),\n", + " go.Scatter(x=d, y=b_grad, name=r\"\\nabla b(x)\"),\n", + " go.Scatter(x=d, y=b_hess, name=r\"\\nabla^2 b(x)\"),\n", + " ]\n", + ")\n", "fig.update_layout(yaxis_range=[-1000, 1000])\n", "fig.show()" ] @@ -305,7 +320,8 @@ "E = numpy.arange(10).reshape(-1, 2)\n", "F = numpy.arange(9).reshape(-1, 3)\n", "dhat = 1\n", - "B = BarrierPotential(dhat)" + "kappa = 1\n", + "B = BarrierPotential(dhat, kappa)" ] }, { @@ -332,8 +348,8 @@ "source": [ "cs = Candidates()\n", "num_candidates = 20\n", - "cs.ev_candidates = [EdgeVertexCandidate(i, i+1) for i in range(num_candidates)]\n", - "assert(len(cs) == num_candidates)\n", + "cs.ev_candidates = [EdgeVertexCandidate(i, i + 1) for i in range(num_candidates)]\n", + "assert len(cs) == num_candidates\n", "cs.ev_candidates" ] }, @@ -352,7 +368,7 @@ "metadata": {}, "outputs": [], "source": [ - "in_mesh = meshio.read(\"../tests/data/bunny.ply\")\n", + "in_mesh = meshio.read(\"../../tests/data/bunny.ply\")\n", "V = in_mesh.points\n", "F = in_mesh.cells[0].data\n", "E = edges(F)" @@ -389,7 +405,8 @@ "source": [ "cs = NormalCollisions()\n", "cs.build(mesh, V, 1e-2)\n", - "B = BarrierPotential(1e-2)" + "kappa = 1\n", + "B = BarrierPotential(1e-2, kappa)" ] }, { @@ -437,7 +454,7 @@ ], "metadata": { "kernelspec": { - "display_name": "Python 3.10.5 64-bit", + "display_name": "Python 3", "language": "python", "name": "python3" }, @@ -451,7 +468,7 @@ "name": "python", "nbconvert_exporter": "python", "pygments_lexer": "ipython3", - "version": "3.11.9" + "version": "3.13.9" }, "varInspector": { "cols": { @@ -481,11 +498,6 @@ "_Feature" ], "window_display": false - }, - "vscode": { - "interpreter": { - "hash": "e7370f93d1d0cde622a1f8e1c04877d8463912d04d973331ad4851f04de6915a" - } } }, "nbformat": 4, diff --git a/python/examples/joints.py b/python/examples/joints.py new file mode 100644 index 000000000..2a0e3ecb5 --- /dev/null +++ b/python/examples/joints.py @@ -0,0 +1,229 @@ +"""Double pendulum via Affine Body Dynamics joint constraints [Chen et al. 2022]. + +Two rods released from horizontal: the upper rod is pinned to a fixed point at +its left end, and the lower rod's left end is glued to the upper rod's free end +with a point connection. Under gravity the linkage swings chaotically. + +The dynamics are solved at a small timestep (dt = 1/600) for accuracy, but the +viewer only redraws every SUBSTEPS = 10 steps, so it still runs at ~60 fps. + +Joint constraints are linear equalities in the affine DOFs, so they require the +affine dynamics model (BodyDynamics.AFFINE). The two rods overlap at the shared +joint, so collision between them is disabled with a vertex-patch filter (there +is no collision filtering between jointed bodies otherwise). +""" + +import numpy as np +import polyscope as ps +from find_ipctk import ipctk +from polyscope import imgui + +ipctk.set_logger_level(ipctk.warn) + + +def box(half_extents): + """Axis-aligned box centered at the origin with outward-facing normals.""" + V = ( + np.array( + [ + [-1, -1, -1], + [1, -1, -1], + [1, 1, -1], + [-1, 1, -1], + [-1, -1, 1], + [1, -1, 1], + [1, 1, 1], + [-1, 1, 1], + ], + dtype=float, + ) + * half_extents + ) + F = np.array( + [ + [0, 2, 1], + [0, 3, 2], + [4, 5, 6], + [4, 6, 7], + [0, 1, 5], + [0, 5, 4], + [2, 3, 7], + [2, 7, 6], + [0, 4, 7], + [0, 7, 3], + [1, 2, 6], + [1, 6, 5], + ], + dtype=np.int32, + ) + return V, ipctk.edges(F), F + + +# --- Scene: two rods forming a double pendulum -------------------------------- + +half_length = 0.6 # each rod spans x in [-half_length, half_length] +y0 = 2.5 # height of the pivot + +anchor = np.array([0.0, y0, 0.0]) # fixed pivot (upper rod's left end) +joint = np.array([2 * half_length, y0, 0.0]) # upper's right end = lower's left + +V_rod, E_rod, F_rod = box([half_length, 0.06, 0.06]) +initial_poses = ipctk.Poses( + [ + # Upper rod: horizontal, left end at the pivot. + ipctk.Pose(np.array([half_length, y0, 0.0]), np.zeros(3)), + # Lower rod: horizontal, left end at the joint. + ipctk.Pose(np.array([3 * half_length, y0, 0.0]), np.zeros(3)), + ] +) + +bodies = ipctk.RigidBodies( + rest_positions=[V_rod, V_rod], + edges=[E_rod, E_rod], + faces=[F_rod, F_rod], + densities=np.full(2, 1000.0), + initial_poses=initial_poses, +) + +# The rods meet at the joint, so disable collision between them: putting all +# vertices in one patch blocks every same-patch (i.e. every) collision pair. +bodies.can_collide = ipctk.make_vertex_patches_filter( + np.zeros(2 * V_rod.shape[0], dtype=np.int32) +) + +joints = ipctk.JointConstraints(bodies, initial_poses) +joints.add_fixed_point(0, anchor) # pin the upper rod +joints.add_point_connection(0, 1, joint) # link lower rod to upper rod + +settings = ipctk.demo.Simulator.Settings() +settings.body_dynamics = ipctk.demo.Simulator.BodyDynamics.AFFINE # joints require affine DOFs + +# Use second-order BDF for more accurate dynamics (order 1 is implicit Euler). +# The effective order ramps up from 1 over the first two steps. +settings.bdf_order = 2 + +# Tighten the Newton convergence tolerance for accurate (near energy-conserving) +# dynamics. The default (1e-2) terminates each Newton solve while the velocity +# is still under-resolved; that small per-step error is dissipative, and since +# the tolerance is a fixed velocity independent of dt, it accumulates over more +# steps at a smaller dt -- so the default actually damps *more* at dt=1/600 than +# at dt=1/60. At 1e-4 the swing amplitude is conserved and, as expected, the +# smaller timestep is the less-damped one. +settings.velocity_conv_tol = 1e-4 + +# Solve at a small timestep for accuracy, but only redraw every SUBSTEPS steps +# so the viewer still runs at ~60 fps (600 steps/s ÷ 10 = 60 frames/s). +DT = 1 / 60.0 +SUBSTEPS = 10 +DT /= SUBSTEPS + +sim = ipctk.demo.Simulator( + bodies=bodies, + initial_poses=initial_poses, + joints=joints, + dt=DT, + settings=settings, +) + + +# build_from_meshes folds each body's principal-axis rotation into its rest +# frame, so track material points by their initial world position instead of +# assuming a body-frame axis. +lower_tip_rest = joint + np.array([2 * half_length, 0.0, 0.0]) + + +def track(body, world_point_at_rest): + """World position at the latest step of the material point that was at + ``world_point_at_rest`` in the initial configuration.""" + p0 = initial_poses[body] + R0 = np.array(p0.rotation_matrix()) + material = R0.T @ (np.asarray(world_point_at_rest) - np.array(p0.position)) + # sim.rigid_poses is the current frame only (O(num_bodies)); rigid_pose_history + # would copy the entire growing history on every call. + pose = sim.rigid_poses[body] + return pose.transform_vertices(material.reshape(1, 3))[0] + + +def joint_pos(): + """Current world position of the moving joint (upper rod's free end).""" + return track(0, joint) + + +def lower_tip(): + """World position of the lower rod's free (swinging) end.""" + return track(1, lower_tip_rest) + + +# --- Viewer ------------------------------------------------------------------- + +ps.init() +ps.set_give_focus_on_show(True) +ps.look_at((0.0, y0, 6.0), (2 * half_length, y0 - 1.0, 0.0)) + +ps_mesh = ps.register_surface_mesh( + "double pendulum", bodies.vertices(initial_poses), bodies.faces +) +ps_pivots = ps.register_point_cloud("pivots", np.vstack([anchor, joint])) +ps_pivots.set_color((0.9, 0.3, 0.1)) +ps_pivots.set_radius(0.012) + +# A fading trail of the swinging tip makes the chaotic motion visible. One +# point is recorded per solver step (SUBSTEPS per frame), so ~2 s of history. +TRAIL_LEN = 120 * SUBSTEPS +trail = np.tile(lower_tip(), (TRAIL_LEN, 1)) +ps_trail = ps.register_point_cloud("tip trail", trail) +ps_trail.set_color((0.1, 0.5, 0.9)) +ps_trail.set_radius(0.004) +ps_trail.add_scalar_quantity( + "age", np.linspace(0.0, 1.0, TRAIL_LEN), enabled=True, cmap="blues" +) + +playing = False + + +def push_trail(point): + """Append a tip position to the rolling trail buffer (recorded per step so + the fine sub-frame motion is captured).""" + global trail + trail = np.roll(trail, -1, axis=0) + trail[-1] = point + + +def render(): + # sim.rigid_poses is the current frame converted to the Poses type expected + # by bodies.vertices (O(num_bodies), unlike rigid_pose_history). + ps_mesh.update_vertex_positions(bodies.vertices(sim.rigid_poses)) + ps_pivots.update_point_positions(np.vstack([anchor, joint_pos()])) + ps_trail.update_point_positions(trail) + + +def advance(): + """Take SUBSTEPS simulation steps, recording the tip each step. Returns + False if a step failed to converge.""" + for _ in range(SUBSTEPS): + if not sim.step(): + return False + push_trail(lower_tip()) + return True + + +def callback(): + global playing, trail + if imgui.Button("Play" if not playing else "Pause") or imgui.IsKeyPressed( + imgui.ImGuiKey_Space + ): + playing = not playing + imgui.SameLine() + if imgui.Button("Step") or playing: + if not advance(): + playing = False + render() + imgui.SameLine() + if imgui.Button("Reset"): + sim.reset() + trail = np.tile(lower_tip(), (TRAIL_LEN, 1)) + render() + + +ps.set_user_callback(callback) +ps.show() diff --git a/python/examples/rigid.py b/python/examples/rigid.py new file mode 100644 index 000000000..aa123ce26 --- /dev/null +++ b/python/examples/rigid.py @@ -0,0 +1,351 @@ +"""Interactive rigid body simulation demo. + +Usage: + python rigid.py # built-in bunnies-in-a-bowl scene + python rigid.py scene.glb [settings.py] # glTF geometry + Python settings + +The glTF file only provides geometry: each mesh node becomes a body named after +its node, with a default density of 1000. Everything physical comes from the +optional settings file, a plain Python module that may define: + + dt Time step in seconds (default 1/60). + convert_planes Turn two-triangle squares into infinite planes (default True). + is_dof_fixed {body name: [x, y, z, rx, ry, rz] booleans}. These are read + while loading because fixing rotational DOFs changes how a + body's inertia is stored, so they cannot change afterwards. + center_of_mass {body name: [x, y, z]} in world coordinates. A body's pose + and rotation then refer to this point instead of its + computed center of mass. Also read while loading. + configure(scene) + Called after loading and before building the simulator. It + edits the real ipctk objects through the Scene below, so + anything the bindings expose can be set (see scenes/). +""" + +import pathlib +import runpy +import sys + +import meshio +import numpy as np +import polyscope as ps +from find_ipctk import ipctk +from polyscope import imgui +from scipy.spatial.transform import Rotation + +ipctk.set_logger_level(ipctk.debug) + + +class Scene: + """The state a settings file's configure(scene) may change. + + Attributes: + bodies: The ipctk.RigidBodies (set density, type, external_force, ...). + poses: The initial poses (aligned with bodies). + settings: The ipctk.demo.Simulator.Settings used to build the simulator. + Dict-valued fields (solver_params, linear_solver_params) return a + copy, so assign a whole dict back rather than editing in place. + joints: Optional ipctk.JointConstraints (affine dynamics only). + """ + + def __init__(self, bodies, poses): + self.bodies = bodies + self.poses = poses + self.settings = ipctk.demo.Simulator.Settings() + self.settings.body_dynamics = ipctk.demo.Simulator.BodyDynamics.RIGID + self.settings.bdf_order = 1 + self.joints = None + self._velocities = None + self._drivers = {} + + @property + def names(self): + """The body names, in body order.""" + return [self.bodies.body_name(i) for i in range(len(self.bodies))] + + def index(self, name): + """The index of the body called ``name``.""" + i = self.bodies.body_index(name) + if i is None: + raise KeyError(f"no body named {name!r}; the bodies are {self.names}") + return i + + def body(self, name): + """The ipctk.RigidBody called ``name`` (edits apply to the scene).""" + return self.bodies[self.index(name)] + + def velocity(self, name): + """The initial velocity of body ``name`` as a Pose (position is the + linear velocity, rotation the angular velocity, both in world space). + Edits apply to the scene; bodies start at rest unless set.""" + if self._velocities is None: + self._velocities = ipctk.Poses( + [ + ipctk.Pose(np.zeros(3), np.zeros(3)) + for _ in range(len(self.bodies)) + ] + ) + return self._velocities[self.index(name)] + + def drive(self, name, driver): + """Attach an ipctk.demo.KinematicDriver to the KINEMATIC body ``name``.""" + self._drivers[self.index(name)] = driver + + def build_simulator(self, dt): + if self.joints is not None and self._velocities is not None: + raise ValueError("the Simulator takes either joints or initial velocities") + kwargs = dict( + bodies=self.bodies, + initial_poses=self.poses, + dt=dt, + settings=self.settings, + ) + if self.joints is not None: + kwargs["joints"] = self.joints + elif self._velocities is not None: + kwargs["initial_velocities"] = self._velocities + sim = ipctk.demo.Simulator(**kwargs) + # Drivers attach after construction, once the body types are final. + for i, driver in self._drivers.items(): + sim.set_kinematic_driver(i, driver) + return sim + + +config = {} +if len(sys.argv) > 2: + # Run the settings file as a module and keep its globals. + config = runpy.run_path(sys.argv[2], run_name="__ipctk_settings__") +dt = config.get("dt", 1 / 60.0) + +if len(sys.argv) > 1: + if not hasattr(ipctk, "read_gltf"): + sys.exit( + "This ipctk build has no glTF support. Reconfigure it with " + "-DIPC_TOOLKIT_WITH_GLTF=ON and rebuild." + ) + bodies, initial_poses = ipctk.read_gltf( + sys.argv[1], + convert_planes=config.get("convert_planes", True), + is_dof_fixed=config.get("is_dof_fixed", {}), + center_of_mass=config.get("center_of_mass", {}), + ) +else: + mesh_names = [ + "bunny (lowpoly).ply", + "bunny (lowpoly).ply", + "bowl.ply", + ] + + rest_positions = [] + edges = [] + faces = [] + for mesh_name in mesh_names: + mesh = meshio.read( + pathlib.Path(__file__).parents[2] / "tests" / "data" / mesh_name + ) + rest_positions.append(mesh.points.astype("float64")) + edges.append(ipctk.edges(mesh.cells_dict["triangle"])) + faces.append(mesh.cells_dict["triangle"]) + + initial_poses = ipctk.Poses( + [ + ipctk.Pose(np.array([1.0, 1.5, 0]), np.random.random(3)), + ipctk.Pose(np.array([-1.0, 2.0, 0.0]), np.array([0.0, np.pi / 4, 0.0])), + ipctk.Pose(np.array([0.0, 1.1, 0.0]), np.zeros(3)), + ] + ) + + bodies = ipctk.RigidBodies( + rest_positions=rest_positions, + edges=edges, + faces=faces, + densities=np.full(len(mesh_names), 1000.0), + initial_poses=initial_poses, + ) + + # Add a ground plane at y = 0 for the bodies to rest on. `planes` is a + # read/write property that returns a copy, so assign the whole list. + bodies.planes = [ipctk.Hyperplane(np.array([0.0, 1.0, 0.0]), np.zeros(3))] + +scene = Scene(bodies, initial_poses) +if "configure" in config: + config["configure"](scene) + +ps.init() + +if len(bodies.planes) > 0: + ps.set_ground_plane_height_mode("manual") + ps.set_ground_plane_height(bodies.planes[0].origin()[1]) +else: + ps.set_ground_plane_mode("none") + +ps.set_give_focus_on_show(True) + +ps_mesh = ps.register_surface_mesh( + "rigid body", + bodies.vertices(initial_poses), + bodies.faces, +) + +# The bodies are merged into a single surface mesh, so map each face back to the +# body that owns it (via any of its vertices) to resolve picks to a body. +face_body = np.array([bodies.vertex_to_body(int(v)) for v in bodies.faces[:, 0]]) + +DYNAMIC_COLOR = np.array([0.85, 0.85, 0.9]) +STATIC_COLOR = np.array([0.85, 0.3, 0.3]) + + +def refresh_body_colors(): + """Color each body's faces by its type (static bodies stand out).""" + body_colors = np.array( + [ + STATIC_COLOR + if bodies[i].type == ipctk.RigidBody.Type.STATIC + else DYNAMIC_COLOR + for i in range(len(bodies)) + ] + ) + ps_mesh.add_color_quantity( + "body type", body_colors[face_body], defined_on="faces", enabled=True + ) + + +refresh_body_colors() + +ps_com = ps.register_point_cloud( + "rigid body coms", + np.vstack([initial_poses[i].position.reshape(-1, 3) for i in range(len(bodies))]), +) + +for d in range(3): + dim = np.zeros((len(bodies), 3)) + for i in range(len(bodies)): + dim[i, d] = 1 # 100 * bodies[i].moment_of_inertia[d] + R = Rotation.from_rotvec(initial_poses[i].rotation.copy()).as_matrix() + dim[i, :] = R @ dim[i, :] + ps_com.add_vector_quantity( + "xyz"[d], + dim, + enabled=True, + vectortype="standard", + ) + +sim = scene.build_simulator(dt) + +playing = False + +# Screen position where the right mouse button was pressed (to tell a click, +# which converts a body to static, from a drag, which pans the camera). +right_click_press = None + + +def _mouse_screen_coords(): + p = imgui.GetIO().MousePos + try: + return (float(p.x), float(p.y)) + except AttributeError: + return (float(p[0]), float(p[1])) + + +def make_body_static_under_cursor(screen_coords): + """Convert the body under the cursor to a STATIC (immovable) body.""" + pick = ps.pick(screen_coords=screen_coords) + if not pick.is_hit or pick.structure_name != "rigid body": + return + + # Resolve the picked element to one of the merged mesh's vertices, then to + # the body that owns it. + element_type = pick.structure_data.get("element_type") + index = pick.structure_data.get("index") + if index is None: + return + if element_type == "face": + vertex = int(bodies.faces[index][0]) + elif element_type == "vertex": + vertex = int(index) + elif element_type == "edge": + vertex = int(bodies.edges[index][0]) + else: + return + + body = bodies.vertex_to_body(vertex) + if bodies[body].type != ipctk.RigidBody.Type.STATIC: + # The simulator rebuilds its DOF mask from the body types every step, + # so the body freezes at its current pose on the next step. + bodies[body].convert_to_static() + print(f"Body {body} converted to STATIC") + refresh_body_colors() + + +def update_mesh(): + # sim.rigid_poses / sim.poses return only the current frame (O(num_bodies)); + # the *_history properties copy the entire growing history on each access. + poses = sim.rigid_poses + affine_poses = sim.poses + ps_mesh.update_vertex_positions(bodies.vertices(poses)) + ps_com.update_point_positions( + np.vstack([poses[i].position.reshape(-1, 3) for i in range(len(bodies))]) + ) + for d in range(3): + dim = np.zeros((len(bodies), 3)) + for i in range(len(bodies)): + dim[i, d] = 1 # 100 * bodies[i].moment_of_inertia[d] + R = np.array(affine_poses[i].rotation) + dim[i, :] = R @ dim[i, :] + ps_com.add_vector_quantity( + "xyz"[d], + dim, + enabled=True, + vectortype="standard", + ) + + +def callback(): + global playing, right_click_press + if imgui.Button("Play" if not playing else "Pause") or imgui.IsKeyPressed( + imgui.ImGuiKey_Space + ): + playing = not playing + imgui.SameLine() + if imgui.Button("Step") or playing: + if not sim.step(): + playing = False + update_mesh() + imgui.SameLine() + if imgui.Button("Reset"): + sim.reset() + update_mesh() + # reset() restores the original body types, so update the colors too. + refresh_body_colors() + imgui.SameLine() + if imgui.Button("Save"): + ipctk.write_gltf( + "output.glb", + bodies, + sim.rigid_pose_history, + dt, + ) + + imgui.TextUnformatted("Right-click a body to make it static.") + + # Right-click a body to make it static. A right *drag* still pans the + # camera, so only convert on a click that barely moved. + io = imgui.GetIO() + if imgui.IsMouseClicked(imgui.ImGuiMouseButton_Right): + right_click_press = _mouse_screen_coords() + if ( + imgui.IsMouseReleased(imgui.ImGuiMouseButton_Right) + and right_click_press is not None + ): + release = _mouse_screen_coords() + moved_sq = (release[0] - right_click_press[0]) ** 2 + ( + release[1] - right_click_press[1] + ) ** 2 + if moved_sq < 25.0 and not io.WantCaptureMouse: + make_body_static_under_cursor(right_click_press) + right_click_press = None + + +ps.set_user_callback(callback) + +ps.show() diff --git a/python/src/CMakeLists.txt b/python/src/CMakeLists.txt index b48bd0ffe..60746cf35 100644 --- a/python/src/CMakeLists.txt +++ b/python/src/CMakeLists.txt @@ -17,6 +17,7 @@ add_subdirectory(candidates) add_subdirectory(ccd) add_subdirectory(collisions) add_subdirectory(distance) +add_subdirectory(dynamics) add_subdirectory(friction) add_subdirectory(geometry) add_subdirectory(math) diff --git a/python/src/bindings.cpp b/python/src/bindings.cpp index 88642b749..cbddeb4bb 100644 --- a/python/src/bindings.cpp +++ b/python/src/bindings.cpp @@ -4,6 +4,11 @@ #include #include +#ifdef IPCTK_WITH_DEMO +// Defined in demo/python/simulator.cpp +void define_simulator(py::module_& m); +#endif + PYBIND11_MODULE(ipctk, m) { // py::options options; @@ -140,6 +145,18 @@ PYBIND11_MODULE(ipctk, m) define_collision_mesh(m); define_ipc(m); + // dynamics + define_rigid_bodies(m); // must precede the others (binds Pose/RigidBodies) + define_affine_joints(m); + +#ifdef IPCTK_WITH_DEMO + // demo simulator (demo/) + py::module_ demo = m.def_submodule( + "demo", + "Demo body-dynamics simulator built from IPC Toolkit components"); + define_simulator(demo); +#endif + // Deprecated names from before the smooth_contact -> gcp rename. We resolve // them in a module-level __getattr__ (PEP 562) instead of plain aliases, so // using an old name raises a DeprecationWarning. Python only calls this diff --git a/python/src/bindings.hpp b/python/src/bindings.hpp index fcd7775fa..033985a9f 100644 --- a/python/src/bindings.hpp +++ b/python/src/bindings.hpp @@ -7,6 +7,7 @@ #include #include #include +#include #include #include #include diff --git a/python/src/candidates/candidates.cpp b/python/src/candidates/candidates.cpp index 872665c2e..3f9630b83 100644 --- a/python/src/candidates/candidates.cpp +++ b/python/src/candidates/candidates.cpp @@ -263,7 +263,25 @@ void define_candidates(py::module_& m) )ipc_Qu8mg5v7", "mesh"_a, "vertices"_a, "is_active"_a = py::none()) .def( - "save_obj", &Candidates::save_obj, "filename"_a, "vertices"_a, + "write_obj", &Candidates::write_obj, "filename"_a, "vertices"_a, + "edges"_a, "faces"_a) + .def( + "save_obj", + [](const Candidates& self, const std::string& filename, + Eigen::ConstRef vertices, + Eigen::ConstRef edges, + Eigen::ConstRef faces) { + if (PyErr_WarnEx( + PyExc_DeprecationWarning, + "Candidates.save_obj is deprecated. Use " + "Candidates.write_obj instead.", + 1) + == -1) { + throw py::error_already_set(); + } + return self.write_obj(filename, vertices, edges, faces); + }, + "Deprecated: use write_obj instead.", "filename"_a, "vertices"_a, "edges"_a, "faces"_a) .def_readwrite("vv_candidates", &Candidates::vv_candidates) .def_readwrite("ev_candidates", &Candidates::ev_candidates) diff --git a/python/src/candidates/plane_vertex.cpp b/python/src/candidates/plane_vertex.cpp index 07f6eb30f..c5299807f 100644 --- a/python/src/candidates/plane_vertex.cpp +++ b/python/src/candidates/plane_vertex.cpp @@ -1,7 +1,7 @@ -#include - #include +#include + using namespace ipc; void define_plane_vertex_candidate(py::module_& m) @@ -15,4 +15,4 @@ void define_plane_vertex_candidate(py::module_& m) "plane", &PlaneVertexCandidate::plane, "Plane of the candidate") .def_readwrite( "vertex_id", &PlaneVertexCandidate::vertex_id, "ID of the vertex"); -} \ No newline at end of file +} diff --git a/python/src/ccd/additive_ccd.cpp b/python/src/ccd/additive_ccd.cpp index e57d94904..ca38e8ef6 100644 --- a/python/src/ccd/additive_ccd.cpp +++ b/python/src/ccd/additive_ccd.cpp @@ -6,7 +6,8 @@ using namespace ipc; void define_additive_ccd(py::module_& m) { - py::class_(m, "AdditiveCCD") + py::class_>( + m, "AdditiveCCD") .def( py::init(), R"ipc_Qu8mg5v7( @@ -22,6 +23,10 @@ void define_additive_ccd(py::module_& m) "DEFAULT_CONSERVATIVE_RESCALING", &AdditiveCCD::DEFAULT_CONSERVATIVE_RESCALING, "The default conservative rescaling value used to avoid taking steps exactly to impact. Value choosen to based on [Li et al. 2021].") + .def_readonly_static( + "COMPONENT_CONSERVATIVE_RESCALING", + &AdditiveCCD::COMPONENT_CONSERVATIVE_RESCALING, + "The conservative rescaling used for component queries (see NarrowPhaseCCDMode.COMPONENT); as raw as Additive CCD can safely get, and the member value is ignored.") .def_readwrite( "conservative_rescaling", &AdditiveCCD::conservative_rescaling, "The conservative rescaling value used to avoid taking steps exactly to impact."); diff --git a/python/src/ccd/inexact_ccd.cpp b/python/src/ccd/inexact_ccd.cpp index 264738142..89d733c73 100644 --- a/python/src/ccd/inexact_ccd.cpp +++ b/python/src/ccd/inexact_ccd.cpp @@ -9,7 +9,8 @@ void define_inexact_ccd(py::module_& m) #ifdef IPC_TOOLKIT_WITH_INEXACT_CCD using namespace ipc; - py::class_(m, "InexactCCD") + py::class_>( + m, "InexactCCD") .def( py::init(), R"ipc_Qu8mg5v7( diff --git a/python/src/ccd/narrow_phase_ccd.cpp b/python/src/ccd/narrow_phase_ccd.cpp index 1668a0c63..599bc3e08 100644 --- a/python/src/ccd/narrow_phase_ccd.cpp +++ b/python/src/ccd/narrow_phase_ccd.cpp @@ -2,8 +2,41 @@ #include +#include + using namespace ipc; +namespace { +/// @brief Whether a Python override accepts the `mode` keyword argument. +/// +/// Overrides written before NarrowPhaseCCDMode existed take no `mode` +/// parameter, so we only pass it to overrides that declare it (or accept +/// **kwargs). Older overrides keep working; they just never learn that a +/// query comes from a piecewise driver. +bool accepts_mode(const py::function& override) +{ + try { + const py::module_ inspect = py::module_::import("inspect"); + const py::object params = + inspect.attr("signature")(override).attr("parameters"); + if (params.contains("mode")) { + return true; + } + const py::object var_keyword = + inspect.attr("Parameter").attr("VAR_KEYWORD"); + for (const py::handle param : params.attr("values")()) { + const py::object kind = param.attr("kind"); + if (kind.equal(var_keyword)) { + return true; + } + } + return false; + } catch (const py::error_already_set&) { + return true; // No inspectable signature, so we pass the argument. + } +} +} // namespace + class PyNarrowPhaseCCD : public NarrowPhaseCCD { public: using NarrowPhaseCCD::NarrowPhaseCCD; // Inherit constructors @@ -14,13 +47,16 @@ class PyNarrowPhaseCCD : public NarrowPhaseCCD { Eigen::ConstRef p1_t1, double& toi, const double min_distance = 0.0, - const double tmax = 1.0) const override + const double tmax = 1.0, + const NarrowPhaseCCDMode mode = + NarrowPhaseCCDMode::STANDALONE) const override { py::gil_scoped_acquire gil; // Acquire GIL before calling Python code py::function override = py::get_override(this, "point_point_ccd"); if (override) { - const py::tuple obj = - override(p0_t0, p1_t0, p0_t1, p1_t1, min_distance, tmax); + const py::tuple obj = call_override( + 0, override, mode, p0_t0, p1_t0, p0_t1, p1_t1, min_distance, + tmax); toi = obj[1].cast(); return obj[0].cast(); } @@ -35,13 +71,16 @@ class PyNarrowPhaseCCD : public NarrowPhaseCCD { Eigen::ConstRef e1_t1, double& toi, const double min_distance = 0.0, - const double tmax = 1.0) const override + const double tmax = 1.0, + const NarrowPhaseCCDMode mode = + NarrowPhaseCCDMode::STANDALONE) const override { py::gil_scoped_acquire gil; // Acquire GIL before calling Python code py::function override = py::get_override(this, "point_edge_ccd"); if (override) { - const py::tuple obj = override( - p_t0, e0_t0, e1_t0, p_t1, e0_t1, e1_t1, min_distance, tmax); + const py::tuple obj = call_override( + 1, override, mode, p_t0, e0_t0, e1_t0, p_t1, e0_t1, e1_t1, + min_distance, tmax); toi = obj[1].cast(); return obj[0].cast(); } @@ -58,14 +97,16 @@ class PyNarrowPhaseCCD : public NarrowPhaseCCD { Eigen::ConstRef t2_t1, double& toi, const double min_distance = 0.0, - const double tmax = 1.0) const override + const double tmax = 1.0, + const NarrowPhaseCCDMode mode = + NarrowPhaseCCDMode::STANDALONE) const override { py::gil_scoped_acquire gil; // Acquire GIL before calling Python code py::function override = py::get_override(this, "point_triangle_ccd"); if (override) { - const py::tuple obj = override( - p_t0, t0_t0, t1_t0, t2_t0, p_t1, t0_t1, t1_t1, t2_t1, - min_distance, tmax); + const py::tuple obj = call_override( + 2, override, mode, p_t0, t0_t0, t1_t0, t2_t0, p_t1, t0_t1, + t1_t1, t2_t1, min_distance, tmax); toi = obj[1].cast(); return obj[0].cast(); } @@ -82,24 +123,66 @@ class PyNarrowPhaseCCD : public NarrowPhaseCCD { Eigen::ConstRef eb1_t1, double& toi, const double min_distance = 0.0, - const double tmax = 1.0) const override + const double tmax = 1.0, + const NarrowPhaseCCDMode mode = + NarrowPhaseCCDMode::STANDALONE) const override { py::gil_scoped_acquire gil; // Acquire GIL before calling Python code py::function override = py::get_override(this, "edge_edge_ccd"); if (override) { - const py::tuple obj = override( - ea0_t0, ea1_t0, eb0_t0, eb1_t0, ea0_t1, ea1_t1, eb0_t1, eb1_t1, - min_distance, tmax); + const py::tuple obj = call_override( + 3, override, mode, ea0_t0, ea1_t0, eb0_t0, eb1_t0, ea0_t1, + ea1_t1, eb0_t1, eb1_t1, min_distance, tmax); toi = obj[1].cast(); return obj[0].cast(); } throw std::runtime_error("pure virtual function called"); } + +private: + /// @brief Call a Python override, passing `mode` only if it accepts it. + /// @note Called with the GIL held, which also guards m_accepts_mode. + template + py::object call_override( + const int method, + const py::function& override, + const NarrowPhaseCCDMode mode, + Args&&... args) const + { + int8_t& accepts = m_accepts_mode[method]; + if (accepts < 0) { + accepts = accepts_mode(override) ? 1 : 0; + } + if (accepts) { + return override(std::forward(args)..., "mode"_a = mode); + } + return override(std::forward(args)...); + } + + /// @brief Per-method cache of accepts_mode() (-1 = unknown). + mutable std::array m_accepts_mode { { -1, -1, -1, -1 } }; }; void define_narrow_phase_ccd(py::module_& m) { - py::class_(m, "NarrowPhaseCCD") + py::enum_( + m, "NarrowPhaseCCDMode", "How a narrow-phase CCD query is used.") + .value( + "STANDALONE", NarrowPhaseCCDMode::STANDALONE, + "A standalone query: the method applies its own conservative " + "strategy.") + .value( + "COMPONENT", NarrowPhaseCCDMode::COMPONENT, + "The per-piece linear CCD inside a piecewise driver (e.g., " + "NonlinearCCD): return the raw time of impact.") + .value( + "COMPONENT_NO_ZERO_TOI", NarrowPhaseCCDMode::COMPONENT_NO_ZERO_TOI, + "Like COMPONENT, but a time of impact of exactly zero is " + "unacceptable."); + + py::class_< + NarrowPhaseCCD, PyNarrowPhaseCCD, std::shared_ptr>( + m, "NarrowPhaseCCD") .def(py::init<>()) .def( "point_point_ccd", @@ -107,14 +190,15 @@ void define_narrow_phase_ccd(py::module_& m) Eigen::ConstRef p1_t0, Eigen::ConstRef p0_t1, Eigen::ConstRef p1_t1, - const double min_distance = 0.0, const double tmax = 1.0) { + const double min_distance = 0.0, const double tmax = 1.0, + const NarrowPhaseCCDMode mode = NarrowPhaseCCDMode::STANDALONE) { double toi; bool r = self.point_point_ccd( - p0_t0, p1_t0, p0_t1, p1_t1, toi, min_distance, tmax); + p0_t0, p1_t0, p0_t1, p1_t1, toi, min_distance, tmax, mode); return std::make_tuple(r, toi); }, "p0_t0"_a, "p1_t0"_a, "p0_t1"_a, "p1_t1"_a, "min_distance"_a = 0.0, - "tmax"_a = 1.0) + "tmax"_a = 1.0, "mode"_a = NarrowPhaseCCDMode::STANDALONE) .def( "point_edge_ccd", [](const NarrowPhaseCCD& self, Eigen::ConstRef p_t0, @@ -123,15 +207,17 @@ void define_narrow_phase_ccd(py::module_& m) Eigen::ConstRef p_t1, Eigen::ConstRef e0_t1, Eigen::ConstRef e1_t1, - const double min_distance = 0.0, const double tmax = 1.0) { + const double min_distance = 0.0, const double tmax = 1.0, + const NarrowPhaseCCDMode mode = NarrowPhaseCCDMode::STANDALONE) { double toi; bool r = self.point_edge_ccd( p_t0, e0_t0, e1_t0, p_t1, e0_t1, e1_t1, toi, min_distance, - tmax); + tmax, mode); return std::make_tuple(r, toi); }, "p_t0"_a, "e0_t0"_a, "e1_t0"_a, "p_t1"_a, "e0_t1"_a, "e1_t1"_a, - "min_distance"_a = 0.0, "tmax"_a = 1.0) + "min_distance"_a = 0.0, "tmax"_a = 1.0, + "mode"_a = NarrowPhaseCCDMode::STANDALONE) .def( "point_triangle_ccd", [](const NarrowPhaseCCD& self, @@ -143,15 +229,17 @@ void define_narrow_phase_ccd(py::module_& m) Eigen::ConstRef t0_t1, Eigen::ConstRef t1_t1, Eigen::ConstRef t2_t1, - const double min_distance = 0.0, const double tmax = 1.0) { + const double min_distance = 0.0, const double tmax = 1.0, + const NarrowPhaseCCDMode mode = NarrowPhaseCCDMode::STANDALONE) { double toi; bool r = self.point_triangle_ccd( p_t0, t0_t0, t1_t0, t2_t0, p_t1, t0_t1, t1_t1, t2_t1, toi, - min_distance, tmax); + min_distance, tmax, mode); return std::make_tuple(r, toi); }, "p_t0"_a, "t0_t0"_a, "t1_t0"_a, "t2_t0"_a, "p_t1"_a, "t0_t1"_a, - "t1_t1"_a, "t2_t1"_a, "min_distance"_a = 0.0, "tmax"_a = 1.0) + "t1_t1"_a, "t2_t1"_a, "min_distance"_a = 0.0, "tmax"_a = 1.0, + "mode"_a = NarrowPhaseCCDMode::STANDALONE) .def( "edge_edge_ccd", [](const NarrowPhaseCCD& self, @@ -163,14 +251,15 @@ void define_narrow_phase_ccd(py::module_& m) Eigen::ConstRef ea1_t1, Eigen::ConstRef eb0_t1, Eigen::ConstRef eb1_t1, - const double min_distance = 0.0, const double tmax = 1.0) { + const double min_distance = 0.0, const double tmax = 1.0, + const NarrowPhaseCCDMode mode = NarrowPhaseCCDMode::STANDALONE) { double toi; bool r = self.edge_edge_ccd( ea0_t0, ea1_t0, eb0_t0, eb1_t0, ea0_t1, ea1_t1, eb0_t1, - eb1_t1, toi, min_distance, tmax); + eb1_t1, toi, min_distance, tmax, mode); return std::make_tuple(r, toi); }, "ea0_t0"_a, "ea1_t0"_a, "eb0_t0"_a, "eb1_t0"_a, "ea0_t1"_a, "ea1_t1"_a, "eb0_t1"_a, "eb1_t1"_a, "min_distance"_a = 0.0, - "tmax"_a = 1.0); + "tmax"_a = 1.0, "mode"_a = NarrowPhaseCCDMode::STANDALONE); } diff --git a/python/src/ccd/nonlinear_ccd.cpp b/python/src/ccd/nonlinear_ccd.cpp index f1adf3a94..402c5bcb2 100644 --- a/python/src/ccd/nonlinear_ccd.cpp +++ b/python/src/ccd/nonlinear_ccd.cpp @@ -79,19 +79,77 @@ void define_nonlinear_ccd(py::module_& m) py::class_(m, "NonlinearCCD") .def( - py::init(), - "tolerance"_a = NonlinearCCD::DEFAULT_TOLERANCE, + py::init([](std::shared_ptr narrow_phase_ccd, + const double conservative_rescaling) { + return NonlinearCCD( + std::move(narrow_phase_ccd), conservative_rescaling); + }), + R"ipc_Qu8mg5v7( + Construct a new NonlinearCCD object. + + Parameters: + narrow_phase_ccd: The narrow-phase CCD used for the per-piece linear CCD (if None, a default-constructed TightInclusionCCD is used). It is queried with a COMPONENT NarrowPhaseCCDMode and never modified, so it may be shared with standalone users. + conservative_rescaling: Fraction of the initial gap (above min_distance) a step may close: the CCD reports an impact once the distance shrinks to min_distance + (1 - conservative_rescaling) * (d0 - min_distance). + )ipc_Qu8mg5v7", + "narrow_phase_ccd"_a = nullptr, + "conservative_rescaling"_a = + NonlinearCCD::DEFAULT_CONSERVATIVE_RESCALING, + // A Python subclass of NarrowPhaseCCD loses its overrides once + // its Python object is collected, so we keep it alive for as long + // as this NonlinearCCD. + py::keep_alive<1, 2>()) + .def( + py::init([](const double tolerance, const long max_iterations, + const double conservative_rescaling) { + if (PyErr_WarnEx( + PyExc_DeprecationWarning, + "NonlinearCCD(tolerance, max_iterations, " + "conservative_rescaling) is deprecated; pass " + "narrow_phase_ccd=TightInclusionCCD(tolerance, " + "max_iterations) instead.", + 1) + == -1) { + throw py::error_already_set(); + } + return NonlinearCCD( + std::make_shared( + tolerance, max_iterations), + conservative_rescaling); + }), + R"ipc_Qu8mg5v7( + Construct a NonlinearCCD whose per-piece CCD is a TightInclusionCCD. + + Deprecated: pass narrow_phase_ccd=TightInclusionCCD(tolerance, max_iterations) instead. + + Parameters: + tolerance: Tight Inclusion's solver tolerance. + max_iterations: Tight Inclusion's maximum number of iterations. + conservative_rescaling: Fraction of the initial gap (above min_distance) a step may close: the CCD reports an impact once the distance shrinks to min_distance + (1 - conservative_rescaling) * (d0 - min_distance). + )ipc_Qu8mg5v7", + "tolerance"_a, "max_iterations"_a = NonlinearCCD::DEFAULT_MAX_ITERATIONS, "conservative_rescaling"_a = NonlinearCCD::DEFAULT_CONSERVATIVE_RESCALING) - .def_readwrite( - "tolerance", &NonlinearCCD::tolerance, "Solver tolerance.") - .def_readwrite( - "max_iterations", &NonlinearCCD::max_iterations, - "Maximum number of iterations.") .def_readwrite( "conservative_rescaling", &NonlinearCCD::conservative_rescaling, - "Conservative rescaling of the time of impact.") + "Fraction of the initial gap (above min_distance) a step may close.") + .def_property( + "narrow_phase_ccd", + [](const NonlinearCCD& self) { + // pybind11 cannot hold a shared_ptr with a + // shared_ptr holder, so we expose a mutable pointer. + return std::const_pointer_cast( + self.narrow_phase_ccd()); + }, + py::cpp_function( + [](NonlinearCCD& self, + std::shared_ptr narrow_phase_ccd) { + self.set_narrow_phase_ccd(std::move(narrow_phase_ccd)); + }, + py::keep_alive<1, 2>()), + "The narrow-phase CCD used for the per-piece linear CCD (never " + "None). Assigning None selects a default-constructed " + "TightInclusionCCD.") .def( "point_point_ccd", [](const NonlinearCCD& self, const NonlinearTrajectory& p0, @@ -224,7 +282,7 @@ void define_nonlinear_ccd(py::module_& m) linear_ccd: Perform linear CCD on a given time interval. tmax: Maximum time to check for collision. min_distance: Minimum separation distance between the objects. - conservative_rescaling: Conservative rescaling of the time of impact. + conservative_rescaling: Fraction of the initial gap (above min_distance) a step may close: the CCD reports an impact once the distance shrinks to min_distance + (1 - conservative_rescaling) * (d0 - min_distance). Returns: Tuple of: diff --git a/python/src/ccd/tight_inclusion_ccd.cpp b/python/src/ccd/tight_inclusion_ccd.cpp index 011bd2ea3..579a457d1 100644 --- a/python/src/ccd/tight_inclusion_ccd.cpp +++ b/python/src/ccd/tight_inclusion_ccd.cpp @@ -165,7 +165,9 @@ void define_tight_inclusion_ccd(py::module_& m) "min_corner"_a, "max_corner"_a, "is_vertex_face"_a, "using_minimum_separation"_a); - py::class_(m, "TightInclusionCCD") + py::class_< + TightInclusionCCD, NarrowPhaseCCD, std::shared_ptr>( + m, "TightInclusionCCD") .def( py::init(), R"ipc_Qu8mg5v7( diff --git a/python/src/collisions/normal/plane_vertex.cpp b/python/src/collisions/normal/plane_vertex.cpp index abfe60ed0..826fad460 100644 --- a/python/src/collisions/normal/plane_vertex.cpp +++ b/python/src/collisions/normal/plane_vertex.cpp @@ -1,3 +1,4 @@ +#include #include #include diff --git a/python/src/collisions/tangential/plane_vertex.cpp b/python/src/collisions/tangential/plane_vertex.cpp index 1a6ffe363..d700edb53 100644 --- a/python/src/collisions/tangential/plane_vertex.cpp +++ b/python/src/collisions/tangential/plane_vertex.cpp @@ -1,7 +1,7 @@ -#include - #include +#include + using namespace ipc; void define_plane_vertex_tangential_collision(py::module_& m) @@ -15,4 +15,4 @@ void define_plane_vertex_tangential_collision(py::module_& m) const PlaneVertexNormalCollision&, Eigen::ConstRef, const NormalPotential&>(), "collision"_a, "positions"_a, "normal_potential"_a); -} \ No newline at end of file +} diff --git a/python/src/dynamics/CMakeLists.txt b/python/src/dynamics/CMakeLists.txt new file mode 100644 index 000000000..e43145b22 --- /dev/null +++ b/python/src/dynamics/CMakeLists.txt @@ -0,0 +1,6 @@ +################################################################################ +# Subfolders +################################################################################ + +add_subdirectory(affine) +add_subdirectory(rigid) \ No newline at end of file diff --git a/python/src/dynamics/affine/CMakeLists.txt b/python/src/dynamics/affine/CMakeLists.txt new file mode 100644 index 000000000..d64f1012b --- /dev/null +++ b/python/src/dynamics/affine/CMakeLists.txt @@ -0,0 +1,5 @@ +set(SOURCES + joints.cpp +) + +target_sources(ipctk PRIVATE ${SOURCES}) diff --git a/python/src/dynamics/affine/bindings.hpp b/python/src/dynamics/affine/bindings.hpp new file mode 100644 index 000000000..a200fc3b5 --- /dev/null +++ b/python/src/dynamics/affine/bindings.hpp @@ -0,0 +1,6 @@ +#pragma once + +#include +namespace py = pybind11; + +void define_affine_joints(py::module_& m); diff --git a/python/src/dynamics/affine/joints.cpp b/python/src/dynamics/affine/joints.cpp new file mode 100644 index 000000000..53f66ec25 --- /dev/null +++ b/python/src/dynamics/affine/joints.cpp @@ -0,0 +1,69 @@ +#include + +#include +#include +#include + +#include + +namespace py = pybind11; +using namespace ipc; +using namespace ipc::affine; + +// Match the opaque declaration in dynamics/rigid/bodies.cpp so the bound +// "Poses" class is used consistently across translation units. +PYBIND11_MAKE_OPAQUE(std::vector) + +void define_affine_joints(py::module_& m) +{ + py::class_(m, "AffinePose") + .def(py::init<>()) + .def_readwrite("position", &affine::Pose::position) + .def_readwrite("rotation", &affine::Pose::rotation) + .def("rotation_vector", &affine::Pose::rotation_vector) + .def( + "transform_vertices", &affine::Pose::transform_vertices, + "vertices"_a) + .def("__repr__", [](const affine::Pose& p) { + return fmt::format( + "AffinePose(position={}, rotation={})", p.position, + p.rotation.reshaped().transpose()); + }); + + py::class_>( + m, "JointConstraints", R"ipc_Qu8mg5v7( + Linear equality joint constraints for affine bodies [Chen et al. 2022]. + + Anchor points are given in world space at the initial configuration + and converted to material coordinates using the initial poses. + )ipc_Qu8mg5v7") + .def( + py::init< + const std::shared_ptr&, + const std::vector&>(), + "bodies"_a, "initial_poses"_a) + .def( + "add_point_connection", &JointConstraints::add_point_connection, + "Glue a point of body_a to a point of body_b.", "body_a"_a, + "body_b"_a, "world_anchor"_a) + .def( + "add_fixed_point", &JointConstraints::add_fixed_point, + "Fix a material point of a body at its initial world position.", + "body"_a, "world_anchor"_a) + .def( + "add_hinge", &JointConstraints::add_hinge, + "Hinge between two bodies about the axis through two points.", + "body_a"_a, "body_b"_a, "world_axis_p0"_a, "world_axis_p1"_a) + .def( + "add_sliding_plane", &JointConstraints::add_sliding_plane, + "Constrain a material point of a body to a plane.", "body"_a, + "world_anchor"_a, "normal"_a) + .def( + "add_fixed_body", &JointConstraints::add_fixed_body, + "Fix all 12 DOFs of a body at their initial values.", "body"_a) + .def_property_readonly( + "num_constraints", &JointConstraints::num_constraints) + .def( + "residual", &JointConstraints::residual, + "Evaluate the constraint residual Cx - s.", "x"_a); +} diff --git a/python/src/dynamics/bindings.hpp b/python/src/dynamics/bindings.hpp new file mode 100644 index 000000000..0fc34d7fb --- /dev/null +++ b/python/src/dynamics/bindings.hpp @@ -0,0 +1,2 @@ +#include +#include diff --git a/python/src/dynamics/rigid/CMakeLists.txt b/python/src/dynamics/rigid/CMakeLists.txt new file mode 100644 index 000000000..80147e7a6 --- /dev/null +++ b/python/src/dynamics/rigid/CMakeLists.txt @@ -0,0 +1,5 @@ +set(SOURCES + bodies.cpp +) + +target_sources(ipctk PRIVATE ${SOURCES}) \ No newline at end of file diff --git a/python/src/dynamics/rigid/bindings.hpp b/python/src/dynamics/rigid/bindings.hpp new file mode 100644 index 000000000..9e9d38741 --- /dev/null +++ b/python/src/dynamics/rigid/bindings.hpp @@ -0,0 +1,6 @@ +#pragma once + +#include +namespace py = pybind11; + +void define_rigid_bodies(py::module_& m); \ No newline at end of file diff --git a/python/src/dynamics/rigid/bodies.cpp b/python/src/dynamics/rigid/bodies.cpp new file mode 100644 index 000000000..5719418dd --- /dev/null +++ b/python/src/dynamics/rigid/bodies.cpp @@ -0,0 +1,189 @@ +#include + +#include +#include +#ifdef IPC_TOOLKIT_WITH_GLTF +#include +#include +#endif + +#include +#include + +namespace py = pybind11; +using namespace ipc; +using namespace ipc::rigid; + +PYBIND11_MAKE_OPAQUE(std::vector) + +void define_rigid_bodies(py::module_& m) +{ + // module_local(false): stl_bind types are module-local by default, which + // would prevent any other extension module embedding the toolkit from + // returning Poses to Python. + py::bind_vector>(m, "Poses", py::module_local(false)) + .def("__repr__", [](const std::vector& poses) { + std::string repr = "Poses(["; + for (const auto& pose : poses) { + repr += fmt::format( + "Pose(position={}, rotation={}){}", pose.position, + pose.rotation, (&pose != &poses.back() ? ", " : "")); + } + repr += "])"; + return repr; + }); + + py::class_(m, "Pose") + .def(py::init<>()) + .def( + py::init< + Eigen::ConstRef, Eigen::ConstRef>(), + "position"_a, "rotation"_a) + .def("rotation_matrix", &Pose::rotation_matrix) + .def("transform_vertices", &Pose::transform_vertices, "vertices"_a) + .def(py::self * py::self) + .def_readwrite("position", &Pose::position) + .def_readwrite("rotation", &Pose::rotation) + .def("__repr__", [](const Pose& p) { + return fmt::format( + "Pose(position={}, rotation={})", p.position, p.rotation); + }); + + py::class_ rigid_body(m, "RigidBody"); + + py::enum_(rigid_body, "Type", "How a body is simulated.") + .value("STATIC", RigidBody::Type::STATIC, "Does not move.") + .value( + "KINEMATIC", RigidBody::Type::KINEMATIC, + "Moves along a prescribed script/velocity but does not respond " + "to forces.") + .value( + "DYNAMIC", RigidBody::Type::DYNAMIC, + "Moves and responds to forces.") + .export_values(); + + rigid_body + .def( + py::init< + Eigen::Ref, Eigen::ConstRef, + Eigen::ConstRef, const double, Pose&, + const VectorMax6b&, const std::optional&>(), + "vertices"_a, "edges"_a, "faces"_a, "density"_a, "initial_pose"_a, + "is_dof_fixed"_a = VectorMax6b(), "center_of_mass"_a = std::nullopt) + .def_property_readonly("mass", &RigidBody::mass) + .def_property_readonly("volume", &RigidBody::volume) + .def_property( + "density", &RigidBody::density, &RigidBody::set_density, + "Uniform density. Setting it rescales the mass and inertia; a " + "running Simulator picks up the change at its next step.") + .def_property_readonly( + "moment_of_inertia", &RigidBody::moment_of_inertia) + .def_property_readonly( + "second_moment", &RigidBody::second_moment, + "Diagonal second-moment matrix ∫ρ x̄x̄ᵀ in the principal frame.") + .def_property_readonly( + "principal_rotation", &RigidBody::principal_rotation, + "Rotation from the principal axes to the input frame.") + .def_property( + "external_force", &RigidBody::external_force, + &RigidBody::set_external_force, + "Constant external force/torque (position = world force, rotation = " + "world torque); folded into the incremental potential each step.") + .def_property( + "type", &RigidBody::type, &RigidBody::set_type, + "How this body is simulated (STATIC/KINEMATIC/DYNAMIC).") + .def_property_readonly( + "is_dof_fixed", &RigidBody::is_dof_fixed, + "Per-DOF fixed flags ([position | rotation]; set at " + "construction).") + .def( + "convert_to_static", &RigidBody::convert_to_static, + "Convert this body to a STATIC body (all DOF fixed). " + "Kinematic scripting/driving lives on the demo Simulator " + "(see ipctk.demo.KinematicDriver / Simulator.set_kinematic_driver)."); + + py::class_, CollisionMesh>( + m, "RigidBodies") + .def( + py::init(&RigidBodies::build_from_meshes), "rest_positions"_a, + "edges"_a, "faces"_a, "densities"_a, "initial_poses"_a, + "convert_planes"_a = false, + "is_dof_fixed"_a = std::vector(), + "names"_a = std::vector(), + "center_of_mass"_a = std::vector>()) + .def( + "vertices", + py::overload_cast&>( + &RigidBodies::vertices, py::const_), + R"ipc_Qu8mg5v7( + Compute the vertex positions from the poses of the rigid bodies. + + Parameters: + poses: The poses of the rigid bodies. + + Returns: + The vertex positions of the rigid bodies (#V × dim). + )ipc_Qu8mg5v7", + "poses"_a) + .def_property_readonly("num_bodies", &RigidBodies::num_bodies) + .def("__len__", &RigidBodies::num_bodies) + .def( + "__getitem__", py::overload_cast(&RigidBodies::operator[]), + // reference_internal: return a live reference tied to the parent's + // lifetime so mutations (e.g. body.type = STATIC) persist. + py::return_value_policy::reference_internal, "index"_a) + .def( + "vertex_to_body", &RigidBodies::vertex_to_body, + "Get the body index for a given collision-mesh vertex index.", + "vertex_id"_a) + .def( + "body_name", &RigidBodies::body_name, + "Get the name of the i-th rigid body.", "index"_a) + .def( + "body_index", &RigidBodies::body_index, + "Look up a rigid body index by name (None if not found).", + "name"_a); + +#ifdef IPC_TOOLKIT_WITH_GLTF + m.def( + "write_gltf", &rigid::write_gltf, R"ipc_Qu8mg5v7( + Write a sequence of rigid body poses to a glTF file. + + Parameters: + filename: The output glTF filename. + bodies: The rigid bodies to write. + poses: A list of poses for each timestep. + timestep: The time interval between each pose in seconds. + embed_buffers: Whether to embed the binary buffers in the glTF file. + write_binary: Whether to write a binary .glb file (true) or a text .gltf file (false). + prettyPrint: Whether to pretty-print the JSON content. + + Returns: + True if successful, false otherwise. + )ipc_Qu8mg5v7", + "filename"_a, "bodies"_a, "poses"_a, "timestep"_a, + "embed_buffers"_a = true, "write_binary"_a = true, + "prettyPrint"_a = true); + + m.def( + "read_gltf", &rigid::read_gltf, R"ipc_Qu8mg5v7( + Read a rigid body scene from a glTF file and return a RigidBodies object. + + Only geometry is read: each mesh node becomes a body named after its + node, with a default density of 1000. Set physical properties (density, + type, external_force, ...) on the returned bodies. + + Parameters: + filename: The input glTF filename. + convert_planes: Whether to convert plane primitives in the glTF file to infinite planes in the RigidBodies object. If false, plane primitives will be ignored. + is_dof_fixed: Per-DOF fixed flags ([position | rotation]) keyed by node name. These are construction-time only, since fixing rotational DOFs changes the frame the inertia is expressed in. + center_of_mass: Prescribed centers of mass keyed by node name, in the file's world coordinates. A body's pose and rotation then refer to this point instead of its computed center of mass (exact only for STATIC and KINEMATIC bodies, whose inertia does not enter the dynamics). Construction-time only, like is_dof_fixed. + + Returns: + A pair containing the RigidBodies object and a vector of initial poses for each body. + )ipc_Qu8mg5v7", + "filename"_a, "convert_planes"_a = false, + "is_dof_fixed"_a = std::unordered_map(), + "center_of_mass"_a = std::unordered_map()); +#endif +} diff --git a/python/tests/test_ccd.py b/python/tests/test_ccd.py index 19c500055..721e744dd 100644 --- a/python/tests/test_ccd.py +++ b/python/tests/test_ccd.py @@ -1,3 +1,6 @@ +import gc +import warnings + import numpy as np from find_ipctk import ipctk from ipctk.filib import Interval @@ -27,12 +30,28 @@ def __init__(self): ipctk.NarrowPhaseCCD.__init__(self) def point_point_ccd( - self, p0_t0, p1_t0, p0_t1, p1_t1, min_distance=0.0, tmax=1.0 + self, + p0_t0, + p1_t0, + p0_t1, + p1_t1, + min_distance=0.0, + tmax=1.0, + mode=ipctk.NarrowPhaseCCDMode.STANDALONE, ): return True, 0.0 def point_edge_ccd( - self, p_t0, e0_t0, e1_t0, p_t1, e0_t1, e1_t1, min_distance=0.0, tmax=1.0 + self, + p_t0, + e0_t0, + e1_t0, + p_t1, + e0_t1, + e1_t1, + min_distance=0.0, + tmax=1.0, + mode=ipctk.NarrowPhaseCCDMode.STANDALONE, ): return True, 0.0 @@ -48,6 +67,7 @@ def point_triangle_ccd( t2_t1, min_distance=0.0, tmax=1.0, + mode=ipctk.NarrowPhaseCCDMode.STANDALONE, ): return True, 0.0 @@ -63,6 +83,7 @@ def edge_edge_ccd( eb1_t1, min_distance=0.0, tmax=1.0, + mode=ipctk.NarrowPhaseCCDMode.STANDALONE, ): return True, 0.0 @@ -83,6 +104,96 @@ def edge_edge_ccd( assert 0 <= toi <= 1 +class _LinearTrajectory(ipctk.NonlinearTrajectory): + def __init__(self, p0, p1): + ipctk.NonlinearTrajectory.__init__(self) + self.p0 = p0 + self.p1 = p1 + + def __call__(self, t): + return (self.p1 - self.p0) * t + self.p0 + + def max_distance_from_linear(self, t0, t1): + return 0 + + +def _crossing_points(): + return ( + _LinearTrajectory(np.array([-1.0, 0, 0]), np.array([1.0, 0, 0])), + _LinearTrajectory(np.array([0, -1.0, 0]), np.array([0, 1.0, 0])), + ) + + +def test_custom_ccd_without_mode_parameter(): + # Overrides written before NarrowPhaseCCDMode existed take no `mode`. + # They must keep working, including as NonlinearCCD's per-piece CCD and + # after the Python object is otherwise unreferenced. + calls = [] + + class LegacyNarrowPhaseCCD(ipctk.NarrowPhaseCCD): + def __init__(self): + ipctk.NarrowPhaseCCD.__init__(self) + + def point_point_ccd( + self, p0_t0, p1_t0, p0_t1, p1_t1, min_distance=0.0, tmax=1.0 + ): + calls.append(min_distance) + return False, 1.0 + + ccd = ipctk.NonlinearCCD(narrow_phase_ccd=LegacyNarrowPhaseCCD()) + gc.collect() + + is_colliding, _ = ccd.point_point_ccd(*_crossing_points()) + + assert not is_colliding + assert len(calls) > 0 + + +def test_custom_ccd_receives_component_mode(): + modes = [] + + class ModeAwareNarrowPhaseCCD(ipctk.NarrowPhaseCCD): + def __init__(self): + ipctk.NarrowPhaseCCD.__init__(self) + + def point_point_ccd( + self, + p0_t0, + p1_t0, + p0_t1, + p1_t1, + min_distance=0.0, + tmax=1.0, + mode=ipctk.NarrowPhaseCCDMode.STANDALONE, + ): + modes.append(mode) + return False, 1.0 + + narrow_phase_ccd = ModeAwareNarrowPhaseCCD() + ccd = ipctk.NonlinearCCD(narrow_phase_ccd=narrow_phase_ccd) + ccd.point_point_ccd(*_crossing_points()) + + # The first piece starts separated, so a zero time of impact is ruled out. + assert modes[0] == ipctk.NarrowPhaseCCDMode.COMPONENT_NO_ZERO_TOI + assert ipctk.NarrowPhaseCCDMode.STANDALONE not in modes + + # Sharing the object with NonlinearCCD leaves standalone queries (made + # through the C++ interface) alone. + modes.clear() + ipctk.NarrowPhaseCCD.point_point_ccd(narrow_phase_ccd, *[np.zeros(3)] * 4) + assert modes == [ipctk.NarrowPhaseCCDMode.STANDALONE] + + +def test_nonlinear_ccd_tolerance_constructor_is_deprecated(): + with warnings.catch_warnings(record=True) as caught: + warnings.simplefilter("always") + ccd = ipctk.NonlinearCCD(1e-6, 1000, 0.5) + + assert any(issubclass(w.category, DeprecationWarning) for w in caught) + assert ccd.conservative_rescaling == 0.5 + assert isinstance(ccd.narrow_phase_ccd, ipctk.TightInclusionCCD) + + def test_custom_broad_phase(): V0, E, F = load_mesh("two-cubes-close.ply") V1, E, F = load_mesh("two-cubes-intersecting.ply") @@ -155,9 +266,13 @@ def max_distance_from_linear(self, t0: float, t1: float): is_colliding, toi = ccd.point_point_ccd(p0, p1) + # The points meet at t = 0.5, but the CCD stops early once the gap + # shrinks to (1 - conservative_rescaling) of its initial value. assert is_colliding - assert 0 < toi < 1 - assert np.abs(toi - 0.5) < 1e-6 + assert 0 < toi <= 0.5 + d0 = np.linalg.norm(p0(0) - p1(0)) + target = (1 - ccd.conservative_rescaling) * d0 + assert np.linalg.norm(p0(toi) - p1(toi)) >= target # class IntervalLinearTrajectory(ipctk.IntervalNonlinearTrajectory): # def __init__(self, p0, p1): @@ -220,8 +335,8 @@ def max_distance_from_linear(self, t0, t1): e0 = Rigid2DTrajectory(np.array([-1, 0]), np.zeros(2), np.zeros(2), 0, np.pi) e1 = Rigid2DTrajectory(np.array([1, 0]), np.zeros(2), np.zeros(2), 0, np.pi) - # increase the conservative_rescaling from 0.8 to 0.9 to get a more accurate estimate - ccd.conservative_rescaling = 0.9 + # increase the conservative_rescaling from 0.8 to 0.99 to get a more accurate estimate + ccd.conservative_rescaling = 0.99 collision, toi = ccd.point_edge_ccd(p, e0, e1) assert collision diff --git a/src/ipc/CMakeLists.txt b/src/ipc/CMakeLists.txt index 676d818f6..9a010d767 100644 --- a/src/ipc/CMakeLists.txt +++ b/src/ipc/CMakeLists.txt @@ -20,8 +20,10 @@ add_subdirectory(candidates) add_subdirectory(ccd) add_subdirectory(collisions) add_subdirectory(distance) +add_subdirectory(dynamics) add_subdirectory(friction) add_subdirectory(geometry) +add_subdirectory(io) add_subdirectory(math) add_subdirectory(ogc) add_subdirectory(potentials) diff --git a/src/ipc/broad_phase/lbvh.cpp b/src/ipc/broad_phase/lbvh.cpp index 1b4e054b0..95e7d3400 100644 --- a/src/ipc/broad_phase/lbvh.cpp +++ b/src/ipc/broad_phase/lbvh.cpp @@ -333,7 +333,8 @@ namespace { bool triangular, bool use_simd = true> void independent_traversal( - const LBVH::Nodes& source, + const LBVH::Node* source_leaves, + const size_t n_source_leaves, const LBVH::Nodes& target, const LBVH::RightmostLeaves& rightmost_leaves, const std::function& can_collide, @@ -350,10 +351,6 @@ namespace { constexpr size_t GRAIN_SIZE = 1; #endif - // Calculate the offset to the first leaf node in the source BVH. - const size_t source_leaf_offset = source.size() / 2; - const size_t n_source_leaves = source_leaf_offset + 1; - const size_t n_tasks = n_source_leaves / SIMD_SIZE + (n_source_leaves % SIMD_SIZE != 0); @@ -371,14 +368,14 @@ namespace { if constexpr (use_simd) { assert(actual_end - idx >= 1); traverse_lbvh_simd( - &source[source_leaf_offset + idx], idx, + source_leaves + idx, idx, std::min(SIMD_SIZE, actual_end - idx), target, rightmost_leaves, can_collide, local_candidates); } else { #endif traverse_lbvh( - source[source_leaf_offset + idx], idx, target, - rightmost_leaves, can_collide, local_candidates); + source_leaves[idx], idx, target, rightmost_leaves, + can_collide, local_candidates); #ifdef IPC_TOOLKIT_WITH_SIMD } #endif @@ -389,13 +386,14 @@ namespace { template void LBVH::detect_candidates( - const Nodes& source, + const Node* source_leaves, + const size_t n_source_leaves, const Nodes& target, const RightmostLeaves& rightmost_leaves, const std::function& can_collide, std::vector& candidates) { - if (source.empty() || target.empty()) { + if (n_source_leaves == 0 || target.empty()) { return; } @@ -404,7 +402,8 @@ void LBVH::detect_candidates( { IPC_TOOLKIT_PROFILE_BLOCK("traverse"); independent_traversal( - source, target, rightmost_leaves, can_collide, storage); + source_leaves, n_source_leaves, target, rightmost_leaves, + can_collide, storage); } { @@ -505,6 +504,152 @@ void LBVH::detect_face_face_candidates( std::bind(&LBVH::can_faces_collide, this, _1, _2), candidates); } +// ============================================================================ +// Two-tree queries against leaves. +// +// The other set's leaves are the source (they are iterated) and this BVH is +// the target (its tree is traversed), so this BVH should be the one with +// more primitives to take advantage of the log(n) traversal. The +// rightmost_leaves argument is only used for triangular (self-collision) +// traversal, so an empty vector is passed here. + +namespace { + LBVH::Node make_leaf( + const Eigen::Array3d& box_min, + const Eigen::Array3d& box_max, + const size_t primitive_id) + { + LBVH::Node leaf; + details::set_inflated_aabb(box_min, box_max, leaf); + leaf.primitive_id = static_cast(primitive_id); + leaf.is_inner_marker = 0; + return leaf; + } +} // namespace + +void LBVH::Leaves::build( + const AABBs& vertex_boxes, + Eigen::ConstRef _edges, + Eigen::ConstRef _faces) +{ + vertices.resize(vertex_boxes.size()); + tbb::parallel_for(size_t(0), size_t(vertex_boxes.size()), [&](size_t i) { + vertices[i] = make_leaf(vertex_boxes[i].min, vertex_boxes[i].max, i); + }); + + edges.resize(_edges.rows()); + tbb::parallel_for(size_t(0), size_t(_edges.rows()), [&](size_t i) { + const AABB& a = vertex_boxes[_edges(i, 0)]; + const AABB& b = vertex_boxes[_edges(i, 1)]; + edges[i] = make_leaf(a.min.min(b.min), a.max.max(b.max), i); + }); + + faces.resize(_faces.rows()); + tbb::parallel_for(size_t(0), size_t(_faces.rows()), [&](size_t i) { + const AABB& a = vertex_boxes[_faces(i, 0)]; + const AABB& b = vertex_boxes[_faces(i, 1)]; + const AABB& c = vertex_boxes[_faces(i, 2)]; + faces[i] = make_leaf( + a.min.min(b.min).min(c.min), a.max.max(b.max).max(c.max), i); + }); +} + +void LBVH::detect_vertex_vertex_candidates( + const Leaves& other, + const std::function& can_collide, + std::vector& candidates) const +{ + if (!has_vertices()) { + return; + } + + IPC_TOOLKIT_PROFILE_BLOCK("LBVH::detect_vertex_vertex_candidates(leaves)"); + + detect_candidates( + other.vertices.data(), other.vertices.size(), vertex_bvh, + /*rightmost_leaves=*/ {}, can_collide, candidates); +} + +void LBVH::detect_edge_vertex_candidates( + const Leaves& other, + const std::function& can_collide, + std::vector& candidates) const +{ + if (!has_edges()) { + return; + } + + IPC_TOOLKIT_PROFILE_BLOCK("LBVH::detect_edge_vertex_candidates(leaves)"); + + detect_candidates( + other.vertices.data(), other.vertices.size(), edge_bvh, + /*rightmost_leaves=*/ {}, can_collide, candidates); +} + +void LBVH::detect_vertex_edge_candidates( + const Leaves& other, + const std::function& can_collide, + std::vector& candidates) const +{ + if (!has_vertices()) { + return; + } + + IPC_TOOLKIT_PROFILE_BLOCK("LBVH::detect_vertex_edge_candidates(leaves)"); + + detect_candidates( + other.edges.data(), other.edges.size(), vertex_bvh, + /*rightmost_leaves=*/ {}, can_collide, candidates); +} + +void LBVH::detect_edge_edge_candidates( + const Leaves& other, + const std::function& can_collide, + std::vector& candidates) const +{ + if (!has_edges()) { + return; + } + + IPC_TOOLKIT_PROFILE_BLOCK("LBVH::detect_edge_edge_candidates(leaves)"); + + detect_candidates( + other.edges.data(), other.edges.size(), edge_bvh, + /*rightmost_leaves=*/ {}, can_collide, candidates); +} + +void LBVH::detect_face_vertex_candidates( + const Leaves& other, + const std::function& can_collide, + std::vector& candidates) const +{ + if (!has_faces()) { + return; + } + + IPC_TOOLKIT_PROFILE_BLOCK("LBVH::detect_face_vertex_candidates(leaves)"); + + detect_candidates( + other.vertices.data(), other.vertices.size(), face_bvh, + /*rightmost_leaves=*/ {}, can_collide, candidates); +} + +void LBVH::detect_vertex_face_candidates( + const Leaves& other, + const std::function& can_collide, + std::vector& candidates) const +{ + if (!has_vertices()) { + return; + } + + IPC_TOOLKIT_PROFILE_BLOCK("LBVH::detect_vertex_face_candidates(leaves)"); + + detect_candidates( + other.faces.data(), other.faces.size(), vertex_bvh, + /*rightmost_leaves=*/ {}, can_collide, candidates); +} + // ============================================================================ bool LBVH::can_edge_vertex_collide(size_t ei, size_t vi) const diff --git a/src/ipc/broad_phase/lbvh.hpp b/src/ipc/broad_phase/lbvh.hpp index f8e0fc5b8..d4cde2cbc 100644 --- a/src/ipc/broad_phase/lbvh.hpp +++ b/src/ipc/broad_phase/lbvh.hpp @@ -179,6 +179,97 @@ class LBVH : public BroadPhase { const Nodes& edge_nodes() const { return edge_bvh; } const Nodes& face_nodes() const { return face_bvh; } + /// @brief The leaf nodes of an LBVH, without its hierarchy. + /// + /// A two-tree query descends only this LBVH's hierarchy and iterates the + /// other side's leaves, so the other side never needs a hierarchy of its + /// own. We use this to query a small, short-lived set of boxes (e.g., one + /// body's swept boxes) against a prebuilt LBVH. Building a full LBVH for + /// such a set costs Morton codes, a sort, and a bottom-up pass per tree, + /// and for a handful of boxes that fixed cost dwarfs the query itself. + struct Leaves { + /// @brief One leaf per vertex box. + Nodes vertices; + /// @brief One leaf per edge, bounding its two vertex boxes. + Nodes edges; + /// @brief One leaf per face, bounding its three vertex boxes. + Nodes faces; + + /// @brief Build the leaves from vertex boxes. + /// + /// Edge and face boxes are the unions of their vertices' boxes, as in + /// LBVH::build. We build serially because a Leaves set is meant for a + /// few boxes, where a parallel launch costs more than the work. + /// + /// @param vertex_boxes The vertex boxes. + /// @param edges The edges, as rows of vertex ids into vertex_boxes. + /// @param faces The faces, as rows of vertex ids into vertex_boxes. + void build( + const AABBs& vertex_boxes, + Eigen::ConstRef edges, + Eigen::ConstRef faces); + }; + + // ------------------------------------------------------------------------- + // Two-tree queries between this LBVH and another set's leaves. + // Candidate ids are each side's local primitive ids. The can_collide + // callback receives ids in the same order as the emitted candidate. + + /// @brief Find candidate collisions between this BVH's vertices and another set's vertices. + /// @param[in] other The other set's leaves, containing the second set of vertices. + /// @param[in] can_collide Filter callback receiving (this_vertex_id, other_vertex_id). + /// @param[out] candidates The candidates (vertex0_id ∈ this, vertex1_id ∈ other). + void detect_vertex_vertex_candidates( + const Leaves& other, + const std::function& can_collide, + std::vector& candidates) const; + + /// @brief Find candidate collisions between this BVH's edges and another set's vertices. + /// @param[in] other The other set's leaves, containing the vertices. + /// @param[in] can_collide Filter callback receiving (this_edge_id, other_vertex_id). + /// @param[out] candidates The candidates (edge_id ∈ this, vertex_id ∈ other). + void detect_edge_vertex_candidates( + const Leaves& other, + const std::function& can_collide, + std::vector& candidates) const; + + /// @brief Find candidate collisions between this BVH's vertices and another set's edges. + /// @param[in] other The other set's leaves, containing the edges. + /// @param[in] can_collide Filter callback receiving (other_edge_id, this_vertex_id). + /// @param[out] candidates The candidates (edge_id ∈ other, vertex_id ∈ this). + void detect_vertex_edge_candidates( + const Leaves& other, + const std::function& can_collide, + std::vector& candidates) const; + + /// @brief Find candidate collisions between this BVH's edges and another set's edges. + /// @param[in] other The other set's leaves, containing the second set of edges. + /// @param[in] can_collide Filter callback receiving (this_edge_id, other_edge_id). + /// @param[out] candidates The candidates (edge0_id ∈ this, edge1_id ∈ other). + void detect_edge_edge_candidates( + const Leaves& other, + const std::function& can_collide, + std::vector& candidates) const; + + /// @brief Find candidate collisions between this BVH's faces and another set's vertices. + /// @param[in] other The other set's leaves, containing the vertices. + /// @param[in] can_collide Filter callback receiving (this_face_id, other_vertex_id). + /// @param[out] candidates The candidates (face_id ∈ this, vertex_id ∈ other). + void detect_face_vertex_candidates( + const Leaves& other, + const std::function& can_collide, + std::vector& candidates) const; + + /// @brief Find candidate collisions between this BVH's vertices and another set's faces. + /// @param[in] other The other set's leaves, containing the faces. + /// @param[in] can_collide Filter callback receiving (other_face_id, this_vertex_id). + /// @param[out] candidates The candidates (face_id ∈ other, vertex_id ∈ this). + void detect_vertex_face_candidates( + const Leaves& other, + const std::function& can_collide, + std::vector& candidates) const; + // ------------------------------------------------------------------------- + protected: /// @brief Build the broad phase for collision detection. /// @note Assumes the vertex_boxes have been built. @@ -215,6 +306,39 @@ class LBVH : public BroadPhase { const Nodes& target, const RightmostLeaves& rightmost_leaves, const std::function& can_collide, + std::vector& candidates) + { + if (source.empty()) { + return; + } + // An LBVH over n primitives stores its n - 1 internal nodes first and + // its n leaves after them, so the leaves start at size / 2. + const size_t leaf_offset = source.size() / 2; + detect_candidates( + source.data() + leaf_offset, leaf_offset + 1, target, + rightmost_leaves, can_collide, candidates); + } + + /// @brief Detect candidate collisions between a set of leaves and a LBVH. + /// @tparam Candidate Type of candidate collision. + /// @tparam swap_order Whether to swap the order of box id with the LBVH id when adding to the candidates. + /// @tparam triangular Whether to consider (i, j) and (j, i) as the same. + /// @param[in] source_leaves The leaves to query with, contiguous in memory. + /// @param[in] n_source_leaves The number of leaves. + /// @param[in] target The LBVH to traverse. + /// @param[in] rightmost_leaves The rightmost leaf indices for the target LBVH (used for self-collision skip). + /// @param[in] can_collide Function to determine if two primitives can collide given their ids. + /// @param[out] candidates The candidate collisions. + template < + typename Candidate, + bool swap_order = false, + bool triangular = false> + static void detect_candidates( + const Node* source_leaves, + const size_t n_source_leaves, + const Nodes& target, + const RightmostLeaves& rightmost_leaves, + const std::function& can_collide, std::vector& candidates); /// @brief Detect candidate collisions between a single LBVH and itself. diff --git a/src/ipc/candidates/candidates.cpp b/src/ipc/candidates/candidates.cpp index b9da17d8e..262b3ebf6 100644 --- a/src/ipc/candidates/candidates.cpp +++ b/src/ipc/candidates/candidates.cpp @@ -8,10 +8,10 @@ #include #include #include +#include #include #include #include -#include #include #include @@ -725,7 +725,7 @@ std::vector Candidates::edge_edge_to_edge_vertex( // ============================================================================ -bool Candidates::save_obj( +bool Candidates::write_obj( const std::string& filename, Eigen::ConstRef vertices, Eigen::ConstRef edges, @@ -736,13 +736,17 @@ bool Candidates::save_obj( return false; } int v_offset = 0; - ipc::save_obj(obj, vertices, edges, faces, vv_candidates, v_offset); + ipc::write_candidates_obj( + obj, vertices, edges, faces, vv_candidates, v_offset); v_offset += vv_candidates.size() * 2; - ipc::save_obj(obj, vertices, edges, faces, ev_candidates, v_offset); + ipc::write_candidates_obj( + obj, vertices, edges, faces, ev_candidates, v_offset); v_offset += ev_candidates.size() * 3; - ipc::save_obj(obj, vertices, edges, faces, ee_candidates, v_offset); + ipc::write_candidates_obj( + obj, vertices, edges, faces, ee_candidates, v_offset); v_offset += ee_candidates.size() * 4; - ipc::save_obj(obj, vertices, faces, faces, fv_candidates, v_offset); + ipc::write_candidates_obj( + obj, vertices, faces, faces, fv_candidates, v_offset); return true; } diff --git a/src/ipc/candidates/candidates.hpp b/src/ipc/candidates/candidates.hpp index 839c3f7c5..c02cd931b 100644 --- a/src/ipc/candidates/candidates.hpp +++ b/src/ipc/candidates/candidates.hpp @@ -237,18 +237,30 @@ class Candidates { // == Save candidates to file ============================================= - /// @brief Save the collision candidates to an OBJ file. - /// @param filename The name of the file to save the candidates to. + /// @brief Write the collision candidates to an OBJ file. + /// @param filename The name of the file to write the candidates to. /// @param vertices Collision mesh vertex positions (rowwise). /// @param edges Collision mesh edge indices (rowwise). /// @param faces Collision mesh face indices (rowwise). - /// @return True if the file was saved successfully. - bool save_obj( + /// @return True if the file was written successfully. + bool write_obj( const std::string& filename, Eigen::ConstRef vertices, Eigen::ConstRef edges, Eigen::ConstRef faces) const; + /// @brief Write the collision candidates to an OBJ file. + /// @deprecated Use write_obj instead. + [[deprecated("Use Candidates::write_obj instead.")]] + bool save_obj( + const std::string& filename, + Eigen::ConstRef vertices, + Eigen::ConstRef edges, + Eigen::ConstRef faces) const + { + return write_obj(filename, vertices, edges, faces); + } + /// @brief Build per-primitive adjacency lists from the candidates. /// @note Required before calling any of the *_set() queries below, which /// return empty lists until then. diff --git a/src/ipc/candidates/plane_vertex.hpp b/src/ipc/candidates/plane_vertex.hpp index 349186134..a45860fe8 100644 --- a/src/ipc/candidates/plane_vertex.hpp +++ b/src/ipc/candidates/plane_vertex.hpp @@ -93,4 +93,4 @@ class PlaneVertexCandidate : virtual public CollisionStencil { Eigen::ConstRef positions) const override; }; -} // namespace ipc \ No newline at end of file +} // namespace ipc diff --git a/src/ipc/ccd/additive_ccd.cpp b/src/ipc/ccd/additive_ccd.cpp index 0cd0abf21..cd1a83e8f 100644 --- a/src/ipc/ccd/additive_ccd.cpp +++ b/src/ipc/ccd/additive_ccd.cpp @@ -22,6 +22,7 @@ #include "additive_ccd.hpp" +#include #include #include #include @@ -68,6 +69,41 @@ AdditiveCCD::AdditiveCCD( conservative_rescaling = _conservative_rescaling; } +bool AdditiveCCD::handle_initial_proximity( + const double initial_distance_sq, + double& min_distance, + const NarrowPhaseCCDMode mode, + double& toi) const +{ + if (initial_distance_sq > min_distance * min_distance) { + return false; // initial distance clears the minimum separation + } + + const double initial_distance = std::sqrt(initial_distance_sq); + if (mode == NarrowPhaseCCDMode::COMPONENT_NO_ZERO_TOI + && initial_distance > 0) { + // The piecewise driver cannot accept toi = 0, so we trade separation + // margin for a positive TOI. + min_distance = + shrink_min_distance_below(initial_distance, min_distance); + return false; + } + + if (mode == NarrowPhaseCCDMode::COMPONENT) { + // Expected per-piece situation: the driver's linearization error can + // exceed the current distance; it handles toi = 0 by subdividing. + logger().trace( + "Initial distance {} ≤ d_min={}, returning toi=0!", + initial_distance, min_distance); + } else { + logger().warn( + "Initial distance {} ≤ d_min={}, returning toi=0!", + initial_distance, min_distance); + } + toi = 0; + return true; +} + template bool AdditiveCCD::additive_ccd( VectorMax12d x, // mutable copy @@ -76,8 +112,15 @@ bool AdditiveCCD::additive_ccd( const double max_disp_mag, double& toi, const double min_distance, - const double tmax) const + const double tmax, + const NarrowPhaseCCDMode mode) const { + // As the per-piece linear CCD inside a piecewise driver, which supplies + // its own conservative margins, we use the least conservative rescaling + // that still terminates (see COMPONENT_CONSERVATIVE_RESCALING). + const double conservative_rescaling = mode == NarrowPhaseCCDMode::STANDALONE + ? this->conservative_rescaling + : COMPONENT_CONSERVATIVE_RESCALING; assert(conservative_rescaling > 0 && conservative_rescaling <= 1); const double min_distance_sq = min_distance * min_distance; @@ -134,17 +177,16 @@ bool AdditiveCCD::point_point_ccd( Eigen::ConstRef p1_t1, double& toi, const double min_distance, - const double tmax) const + const double tmax, + const NarrowPhaseCCDMode mode) const { const int dim = p0_t0.size(); assert(dim == p1_t0.size() && dim == p0_t1.size() && dim == p1_t1.size()); const double initial_distance = point_point_distance(p0_t0, p1_t0); - if (initial_distance <= min_distance * min_distance) { - logger().warn( - "Initial distance {} ≤ d_min={}, returning toi=0!", - std::sqrt(initial_distance), min_distance); - toi = 0; + double adj_min_distance = min_distance; + if (handle_initial_proximity( + initial_distance, adj_min_distance, mode, toi)) { return true; } @@ -165,7 +207,8 @@ bool AdditiveCCD::point_point_ccd( const VectorMax12d dx = stack(dp0, dp1); return additive_ccd( - x, dx, distance_squared, max_disp_mag, toi, min_distance, tmax); + x, dx, distance_squared, max_disp_mag, toi, adj_min_distance, tmax, + mode); } bool AdditiveCCD::point_edge_ccd( @@ -177,18 +220,17 @@ bool AdditiveCCD::point_edge_ccd( Eigen::ConstRef e1_t1, double& toi, const double min_distance, - const double tmax) const + const double tmax, + const NarrowPhaseCCDMode mode) const { const int dim = p_t0.size(); assert(dim == e0_t0.size() && dim == e1_t0.size()); assert(dim == p_t1.size() && dim == e0_t1.size() && dim == e1_t1.size()); const double initial_distance = point_edge_distance(p_t0, e0_t0, e1_t0); - if (initial_distance <= min_distance * min_distance) { - logger().warn( - "Initial distance {} ≤ d_min={}, returning toi=0!", - std::sqrt(initial_distance), min_distance); - toi = 0; + double adj_min_distance = min_distance; + if (handle_initial_proximity( + initial_distance, adj_min_distance, mode, toi)) { return true; } @@ -212,7 +254,8 @@ bool AdditiveCCD::point_edge_ccd( const VectorMax12d dx = stack(dp, de0, de1); return additive_ccd( - x, dx, distance_squared, max_disp_mag, toi, min_distance, tmax); + x, dx, distance_squared, max_disp_mag, toi, adj_min_distance, tmax, + mode); } bool AdditiveCCD::point_triangle_ccd( @@ -226,15 +269,14 @@ bool AdditiveCCD::point_triangle_ccd( Eigen::ConstRef t2_t1, double& toi, const double min_distance, - const double tmax) const + const double tmax, + const NarrowPhaseCCDMode mode) const { const double initial_distance = point_triangle_distance(p_t0, t0_t0, t1_t0, t2_t0); - if (initial_distance <= min_distance * min_distance) { - logger().warn( - "Initial distance {} ≤ d_min={}, returning toi=0!", - std::sqrt(initial_distance), min_distance); - toi = 0; + double adj_min_distance = min_distance; + if (handle_initial_proximity( + initial_distance, adj_min_distance, mode, toi)) { return true; } @@ -260,7 +302,8 @@ bool AdditiveCCD::point_triangle_ccd( const VectorMax12d dx = stack(dp, dt0, dt1, dt2); return additive_ccd( - x, dx, distance_squared, max_disp_mag, toi, min_distance, tmax); + x, dx, distance_squared, max_disp_mag, toi, adj_min_distance, tmax, + mode); } bool AdditiveCCD::edge_edge_ccd( @@ -274,15 +317,14 @@ bool AdditiveCCD::edge_edge_ccd( Eigen::ConstRef eb1_t1, double& toi, const double min_distance, - const double tmax) const + const double tmax, + const NarrowPhaseCCDMode mode) const { const double initial_distance = edge_edge_distance(ea0_t0, ea1_t0, eb0_t0, eb1_t0); - if (initial_distance <= min_distance * min_distance) { - logger().warn( - "Initial distance {} ≤ d_min={}, returning toi=0!", - std::sqrt(initial_distance), min_distance); - toi = 0; + double adj_min_distance = min_distance; + if (handle_initial_proximity( + initial_distance, adj_min_distance, mode, toi)) { return true; } @@ -299,7 +341,7 @@ bool AdditiveCCD::edge_edge_ccd( return false; } - const double min_distance_sq = min_distance * min_distance; + const double min_distance_sq = adj_min_distance * adj_min_distance; auto distance_squared = [min_distance_sq](Eigen::ConstRef x) { const auto& ea0 = x.head<3>(); const auto& ea1 = x.segment<3>(3); @@ -321,7 +363,8 @@ bool AdditiveCCD::edge_edge_ccd( const VectorMax12d dx = stack(dea0, dea1, deb0, deb1); return additive_ccd( - x, dx, distance_squared, max_disp_mag, toi, min_distance, tmax); + x, dx, distance_squared, max_disp_mag, toi, adj_min_distance, tmax, + mode); } } // namespace ipc \ No newline at end of file diff --git a/src/ipc/ccd/additive_ccd.hpp b/src/ipc/ccd/additive_ccd.hpp index 412758671..4046c3469 100644 --- a/src/ipc/ccd/additive_ccd.hpp +++ b/src/ipc/ccd/additive_ccd.hpp @@ -24,6 +24,13 @@ class AdditiveCCD : public NarrowPhaseCCD { /// The default conservative rescaling value used to avoid taking steps /// exactly to impact. Value choosen to based on [Li et al. 2021]. static constexpr double DEFAULT_CONSERVATIVE_RESCALING = 0.9; + /// The conservative rescaling used for component queries (see + /// NarrowPhaseCCDMode::COMPONENT). Additive CCD terminates by advancing + /// until the remaining gap shrinks to (1 - conservative_rescaling) times + /// the initial gap, so a truly raw TOI (rescaling -> 1) never terminates + /// and values above 0.99 behave badly; 0.99 is as raw as it can safely + /// get. Component queries ignore the member value in favor of this. + static constexpr double COMPONENT_CONSERVATIVE_RESCALING = 0.99; /// @brief Construct a new AdditiveCCD object. /// @param max_iterations The maximum number of iterations to use for the CCD algorithm. If set to UNLIMITTED_ITERATIONS, the algorithm will run until it converges. @@ -40,6 +47,7 @@ class AdditiveCCD : public NarrowPhaseCCD { /// @param[out] toi The time of impact between the two points. /// @param min_distance The minimum distance between two objects. /// @param tmax The maximum time to check for collisions. + /// @param mode How the query is used (see NarrowPhaseCCDMode). /// @return True if a collision was detected, false otherwise. bool point_point_ccd( Eigen::ConstRef p0_t0, @@ -48,7 +56,9 @@ class AdditiveCCD : public NarrowPhaseCCD { Eigen::ConstRef p1_t1, double& toi, const double min_distance = 0.0, - const double tmax = 1.0) const override; + const double tmax = 1.0, + const NarrowPhaseCCDMode mode = + NarrowPhaseCCDMode::STANDALONE) const override; /// @brief Computes the time of impact between a point and an edge using continuous collision detection. /// @param p_t0 The initial position of the point. @@ -60,6 +70,7 @@ class AdditiveCCD : public NarrowPhaseCCD { /// @param[out] toi The time of impact between the point and the edge. /// @param min_distance The minimum distance between two objects. /// @param tmax The maximum time to check for collisions. + /// @param mode How the query is used (see NarrowPhaseCCDMode). /// @return True if a collision was detected, false otherwise. bool point_edge_ccd( Eigen::ConstRef p_t0, @@ -70,7 +81,9 @@ class AdditiveCCD : public NarrowPhaseCCD { Eigen::ConstRef e1_t1, double& toi, const double min_distance = 0.0, - const double tmax = 1.0) const override; + const double tmax = 1.0, + const NarrowPhaseCCDMode mode = + NarrowPhaseCCDMode::STANDALONE) const override; /// @brief Computes the time of impact between a point and a triangle using continuous collision detection. /// @param p_t0 The initial position of the point. @@ -84,6 +97,7 @@ class AdditiveCCD : public NarrowPhaseCCD { /// @param[out] toi The time of impact between the point and the triangle. /// @param min_distance The minimum distance between two objects. /// @param tmax The maximum time to check for collisions. + /// @param mode How the query is used (see NarrowPhaseCCDMode). /// @return True if a collision was detected, false otherwise. bool point_triangle_ccd( Eigen::ConstRef p_t0, @@ -96,7 +110,9 @@ class AdditiveCCD : public NarrowPhaseCCD { Eigen::ConstRef t2_t1, double& toi, const double min_distance = 0.0, - const double tmax = 1.0) const override; + const double tmax = 1.0, + const NarrowPhaseCCDMode mode = + NarrowPhaseCCDMode::STANDALONE) const override; /// @brief Computes the time of impact between two edges using continuous collision detection. /// @param ea0_t0 The initial position of the first endpoint of the first edge. @@ -110,6 +126,7 @@ class AdditiveCCD : public NarrowPhaseCCD { /// @param[out] toi The time of impact between the two edges. /// @param min_distance The minimum distance between two objects. /// @param tmax The maximum time to check for collisions. + /// @param mode How the query is used (see NarrowPhaseCCDMode). /// @return True if a collision was detected, false otherwise. bool edge_edge_ccd( Eigen::ConstRef ea0_t0, @@ -122,12 +139,34 @@ class AdditiveCCD : public NarrowPhaseCCD { Eigen::ConstRef eb1_t1, double& toi, const double min_distance = 0.0, - const double tmax = 1.0) const override; + const double tmax = 1.0, + const NarrowPhaseCCDMode mode = + NarrowPhaseCCDMode::STANDALONE) const override; /// @brief Maximum number of iterations. long max_iterations; private: + /// @brief Handle a query whose initial distance is within the minimum separation. + /// + /// For COMPONENT_NO_ZERO_TOI queries with a positive initial distance, the + /// minimum separation is halved until the initial distance clears it, + /// trading separation margin for a positive TOI (mirroring Tight + /// Inclusion's small-TOI rerun). Otherwise this reports toi = 0, at trace + /// level for COMPONENT queries (the piecewise driver handles it by + /// subdividing) and at warn level for STANDALONE queries. + /// + /// @param initial_distance_sq The squared initial distance between the objects. + /// @param[in,out] min_distance The minimum separation (possibly reduced). + /// @param mode How the query is used (see NarrowPhaseCCDMode). + /// @param[out] toi Set to 0 when returning true. + /// @return True if the query should return early (toi = 0), false to continue. + bool handle_initial_proximity( + const double initial_distance_sq, + double& min_distance, + const NarrowPhaseCCDMode mode, + double& toi) const; + /// @brief Computes the time of impact between two objects using additive continuous collision detection. /// @param x Initial positions /// @param dx Displacements @@ -136,6 +175,7 @@ class AdditiveCCD : public NarrowPhaseCCD { /// @param[out] toi The time of impact between the two objects. /// @param min_distance The minimum distance between the objects. /// @param tmax The maximum time to check for collisions. + /// @param mode How the query is used (see NarrowPhaseCCDMode). /// @return True if a collision was detected, false otherwise. template bool additive_ccd( @@ -145,7 +185,8 @@ class AdditiveCCD : public NarrowPhaseCCD { const double max_disp_mag, double& toi, const double min_distance = 0.0, - const double tmax = 1.0) const; + const double tmax = 1.0, + const NarrowPhaseCCDMode mode = NarrowPhaseCCDMode::STANDALONE) const; }; } // namespace ipc \ No newline at end of file diff --git a/src/ipc/ccd/check_initial_distance.hpp b/src/ipc/ccd/check_initial_distance.hpp index 3ec8e85e9..e6329ec0f 100644 --- a/src/ipc/ccd/check_initial_distance.hpp +++ b/src/ipc/ccd/check_initial_distance.hpp @@ -2,6 +2,8 @@ #include +#include + namespace ipc { inline bool check_initial_distance( @@ -20,4 +22,26 @@ inline bool check_initial_distance( return true; } +/// @brief Shrink a minimum separation until a positive initial distance clears it. +/// +/// A component query that cannot report toi = 0 trades separation margin for +/// a positive time of impact: we halve the minimum separation until the +/// initial distance exceeds it (mirroring Tight Inclusion's small-TOI rerun). +/// +/// @param initial_distance The initial (unsquared) distance between the objects. +/// @param min_distance The requested minimum separation. +/// @return The minimum separation, halved just enough to fall below a positive initial_distance (unchanged otherwise). +inline double +shrink_min_distance_below(const double initial_distance, double min_distance) +{ + if (initial_distance <= 0 || initial_distance > min_distance) { + return min_distance; + } + // min_distance / 2^k < initial_distance for the smallest such k ≥ 1. + const int k = + static_cast(std::floor(std::log2(min_distance / initial_distance))) + + 1; + return std::ldexp(min_distance, -k); // halve k times +} + } // namespace ipc \ No newline at end of file diff --git a/src/ipc/ccd/inexact_ccd.cpp b/src/ipc/ccd/inexact_ccd.cpp index 09df70b82..1f1e0db4a 100644 --- a/src/ipc/ccd/inexact_ccd.cpp +++ b/src/ipc/ccd/inexact_ccd.cpp @@ -22,8 +22,22 @@ bool InexactCCD::ccd_strategy( const double min_distance, const double initial_distance, const double conservative_rescaling, - double& toi) + double& toi, + const NarrowPhaseCCDMode mode) { + if (mode != NarrowPhaseCCDMode::STANDALONE) { + // As the per-piece linear CCD inside a piecewise driver (e.g., + // NonlinearCCD), the driver supplies its own conservative margins, so + // we run the CCD directly with the caller's minimum separation (no + // inflation, rerun, or rescaling). This CCD has no zero-TOI option, so + // when zero is unacceptable we shrink the separation until the initial + // distance clears it. + const double d = mode == NarrowPhaseCCDMode::COMPONENT_NO_ZERO_TOI + ? shrink_min_distance_below(initial_distance, min_distance) + : min_distance; + return ccd(d, toi); + } + if (check_initial_distance(initial_distance, min_distance, toi)) { return true; } @@ -57,7 +71,8 @@ bool InexactCCD::point_point_ccd_3D( Eigen::ConstRef p1_t1, double& toi, const double min_distance, - const double tmax) const + const double tmax, + const NarrowPhaseCCDMode mode) const { assert(tmax >= 0 && tmax <= 1.0); @@ -73,7 +88,7 @@ bool InexactCCD::point_point_ccd_3D( }; return ccd_strategy( - ccd, min_distance, initial_distance, conservative_rescaling, toi); + ccd, min_distance, initial_distance, conservative_rescaling, toi, mode); } bool InexactCCD::point_point_ccd( @@ -83,14 +98,15 @@ bool InexactCCD::point_point_ccd( Eigen::ConstRef p1_t1, double& toi, const double min_distance, - const double tmax) const + const double tmax, + const NarrowPhaseCCDMode mode) const { assert(p0_t0.size() == p1_t0.size()); assert(p0_t0.size() == p0_t1.size()); assert(p0_t0.size() == p1_t1.size()); return point_point_ccd_3D( to_3D(p0_t0), to_3D(p1_t0), to_3D(p0_t1), to_3D(p1_t1), toi, - min_distance, tmax); + min_distance, tmax, mode); } bool InexactCCD::point_edge_ccd_3D( @@ -102,7 +118,8 @@ bool InexactCCD::point_edge_ccd_3D( Eigen::ConstRef e1_t1, double& toi, const double min_distance, - const double tmax) const + const double tmax, + const NarrowPhaseCCDMode mode) const { assert(tmax >= 0 && tmax <= 1.0); @@ -119,7 +136,7 @@ bool InexactCCD::point_edge_ccd_3D( }; return ccd_strategy( - ccd, min_distance, initial_distance, conservative_rescaling, toi); + ccd, min_distance, initial_distance, conservative_rescaling, toi, mode); } bool InexactCCD::point_edge_ccd( @@ -131,14 +148,15 @@ bool InexactCCD::point_edge_ccd( Eigen::ConstRef e1_t1, double& toi, const double min_distance, - const double tmax) const + const double tmax, + const NarrowPhaseCCDMode mode) const { assert(p_t1.size() == p_t0.size()); assert(e0_t0.size() == p_t0.size() && e1_t0.size() == p_t0.size()); assert(e0_t1.size() == p_t0.size() && e1_t1.size() == p_t0.size()); return point_edge_ccd_3D( to_3D(p_t0), to_3D(e0_t0), to_3D(e1_t0), to_3D(p_t1), to_3D(e0_t1), - to_3D(e1_t1), toi, min_distance, tmax); + to_3D(e1_t1), toi, min_distance, tmax, mode); } bool InexactCCD::edge_edge_ccd( @@ -152,7 +170,8 @@ bool InexactCCD::edge_edge_ccd( Eigen::ConstRef eb1_t1, double& toi, const double min_distance, - const double tmax) const + const double tmax, + const NarrowPhaseCCDMode mode) const { assert(tmax >= 0 && tmax <= 1.0); @@ -171,7 +190,7 @@ bool InexactCCD::edge_edge_ccd( }; return ccd_strategy( - ccd, min_distance, initial_distance, conservative_rescaling, toi); + ccd, min_distance, initial_distance, conservative_rescaling, toi, mode); } bool InexactCCD::point_triangle_ccd( @@ -185,7 +204,8 @@ bool InexactCCD::point_triangle_ccd( Eigen::ConstRef t2_t1, double& toi, const double min_distance, - const double tmax) const + const double tmax, + const NarrowPhaseCCDMode mode) const { assert(tmax >= 0 && tmax <= 1.0); @@ -204,7 +224,7 @@ bool InexactCCD::point_triangle_ccd( }; return ccd_strategy( - ccd, min_distance, initial_distance, conservative_rescaling, toi); + ccd, min_distance, initial_distance, conservative_rescaling, toi, mode); } } // namespace ipc diff --git a/src/ipc/ccd/inexact_ccd.hpp b/src/ipc/ccd/inexact_ccd.hpp index 7f36964b3..a4496be3e 100644 --- a/src/ipc/ccd/inexact_ccd.hpp +++ b/src/ipc/ccd/inexact_ccd.hpp @@ -31,6 +31,7 @@ class InexactCCD : public NarrowPhaseCCD { /// @param[out] toi The time of impact between the two points. /// @param[in] min_distance The minimum distance between the points. /// @param[in] tmax The maximum time to check for collisions. + /// @param[in] mode How the query is used (see NarrowPhaseCCDMode). /// @return True if a collision was detected, false otherwise. bool point_point_ccd( Eigen::ConstRef p0_t0, @@ -39,7 +40,9 @@ class InexactCCD : public NarrowPhaseCCD { Eigen::ConstRef p1_t1, double& toi, const double min_distance = 0.0, - const double tmax = 1.0) const override; + const double tmax = 1.0, + const NarrowPhaseCCDMode mode = + NarrowPhaseCCDMode::STANDALONE) const override; /// @brief Computes the time of impact between a point and an edge using continuous collision detection. /// @param[in] p_t0 The initial position of the point. @@ -51,6 +54,7 @@ class InexactCCD : public NarrowPhaseCCD { /// @param[out] toi The time of impact between the point and the edge. /// @param[in] min_distance The minimum distance between the objects. /// @param[in] tmax The maximum time to check for collisions. + /// @param[in] mode How the query is used (see NarrowPhaseCCDMode). /// @return True if a collision was detected, false otherwise. bool point_edge_ccd( Eigen::ConstRef p_t0, @@ -61,7 +65,9 @@ class InexactCCD : public NarrowPhaseCCD { Eigen::ConstRef e1_t1, double& toi, const double min_distance = 0.0, - const double tmax = 1.0) const override; + const double tmax = 1.0, + const NarrowPhaseCCDMode mode = + NarrowPhaseCCDMode::STANDALONE) const override; /// @brief Computes the time of impact between two edges in 3D using continuous collision detection. /// @param[in] ea0_t0 The initial position of the first endpoint of the first edge. @@ -75,6 +81,7 @@ class InexactCCD : public NarrowPhaseCCD { /// @param[out] toi The time of impact between the two edges. /// @param[in] min_distance The minimum distance between the objects. /// @param[in] tmax The maximum time to check for collisions. + /// @param[in] mode How the query is used (see NarrowPhaseCCDMode). /// @return True if a collision was detected, false otherwise. bool edge_edge_ccd( Eigen::ConstRef ea0_t0, @@ -87,7 +94,9 @@ class InexactCCD : public NarrowPhaseCCD { Eigen::ConstRef eb1_t1, double& toi, const double min_distance = 0.0, - const double tmax = 1.0) const override; + const double tmax = 1.0, + const NarrowPhaseCCDMode mode = + NarrowPhaseCCDMode::STANDALONE) const override; /// @brief Computes the time of impact between a point and a triangle in 3D using continuous collision detection. /// @param[in] p_t0 The initial position of the point. @@ -101,6 +110,7 @@ class InexactCCD : public NarrowPhaseCCD { /// @param[out] toi The time of impact between the point and the triangle. /// @param[in] min_distance The minimum distance between the objects. /// @param[in] tmax The maximum time to check for collisions. + /// @param[in] mode How the query is used (see NarrowPhaseCCDMode). /// @return True if a collision was detected, false otherwise. bool point_triangle_ccd( Eigen::ConstRef p_t0, @@ -113,7 +123,9 @@ class InexactCCD : public NarrowPhaseCCD { Eigen::ConstRef t2_t1, double& toi, const double min_distance = 0.0, - const double tmax = 1.0) const override; + const double tmax = 1.0, + const NarrowPhaseCCDMode mode = + NarrowPhaseCCDMode::STANDALONE) const override; private: /// @brief Computes the time of impact between two points in 3D using continuous collision detection. @@ -124,6 +136,7 @@ class InexactCCD : public NarrowPhaseCCD { /// @param[out] toi The time of impact between the two points. /// @param[in] min_distance The minimum distance between the objects. /// @param[in] tmax The maximum time to check for collisions. + /// @param[in] mode How the query is used (see NarrowPhaseCCDMode). /// @return True if a collision was detected, false otherwise. bool point_point_ccd_3D( Eigen::ConstRef p0_t0, @@ -132,7 +145,8 @@ class InexactCCD : public NarrowPhaseCCD { Eigen::ConstRef p1_t1, double& toi, const double min_distance = 0.0, - const double tmax = 1.0) const; + const double tmax = 1.0, + const NarrowPhaseCCDMode mode = NarrowPhaseCCDMode::STANDALONE) const; /// @brief Computes the time of impact between a point and an edge in 3D using continuous collision detection. /// @param[in] p_t0 The initial position of the point. @@ -144,6 +158,7 @@ class InexactCCD : public NarrowPhaseCCD { /// @param[out] toi The time of impact between the point and the edge. /// @param[in] min_distance The minimum distance between the objects. /// @param[in] tmax The maximum time to check for collisions. + /// @param[in] mode How the query is used (see NarrowPhaseCCDMode). /// @return True if a collision was detected, false otherwise. bool point_edge_ccd_3D( Eigen::ConstRef p_t0, @@ -154,7 +169,8 @@ class InexactCCD : public NarrowPhaseCCD { Eigen::ConstRef e1_t1, double& toi, const double min_distance = 0.0, - const double tmax = 1.0) const; + const double tmax = 1.0, + const NarrowPhaseCCDMode mode = NarrowPhaseCCDMode::STANDALONE) const; /// @brief Perform the CCD strategy outlined by Li et al. [2020]. /// @param[in] ccd The continuous collision detection function. @@ -162,6 +178,7 @@ class InexactCCD : public NarrowPhaseCCD { /// @param[in] initial_distance The initial distance between the objects. /// @param[in] conservative_rescaling The conservative rescaling of the time of impact. /// @param[out] toi Output time of impact. + /// @param[in] mode How the query is used. Outside STANDALONE the strategy is skipped and ccd runs once with min_distance. /// @return True if a collision was detected, false otherwise. static bool ccd_strategy( const std::function& @@ -169,7 +186,8 @@ class InexactCCD : public NarrowPhaseCCD { const double min_distance, const double initial_distance, const double conservative_rescaling, - double& toi); + double& toi, + const NarrowPhaseCCDMode mode = NarrowPhaseCCDMode::STANDALONE); }; } // namespace ipc diff --git a/src/ipc/ccd/narrow_phase_ccd.hpp b/src/ipc/ccd/narrow_phase_ccd.hpp index cc6d67625..fbaa07791 100644 --- a/src/ipc/ccd/narrow_phase_ccd.hpp +++ b/src/ipc/ccd/narrow_phase_ccd.hpp @@ -2,8 +2,26 @@ #include +#include + namespace ipc { +/// @brief How a narrow-phase CCD query is used. +enum class NarrowPhaseCCDMode : uint8_t { + /// A standalone query: the method applies its own conservative strategy + /// (minimum-distance inflation, small-TOI reruns, and rescaling). + STANDALONE, + /// The per-piece linear CCD inside a piecewise driver (e.g., + /// NonlinearCCD), which supplies its own conservative margins (a target + /// separation inflated by the linearization error as the minimum + /// separation, and subdivision on zero TOI). The method skips its + /// standalone strategy and returns the raw time of impact. + COMPONENT, + /// Like COMPONENT, but the caller cannot accept a time of impact of + /// exactly zero (it knows the start of the piece is separated). + COMPONENT_NO_ZERO_TOI, +}; + /// @brief Narrow Phase Continuous Collision Detection (CCD) interface. class NarrowPhaseCCD { public: @@ -19,6 +37,7 @@ class NarrowPhaseCCD { /// @param toi The time of impact. /// @param min_distance The minimum distance between the two points. /// @param tmax The maximum time to check for collision. + /// @param mode How the query is used (see NarrowPhaseCCDMode). /// @return True if a collision was detected, false otherwise. virtual bool point_point_ccd( Eigen::ConstRef p0_t0, @@ -27,7 +46,9 @@ class NarrowPhaseCCD { Eigen::ConstRef p1_t1, double& toi, const double min_distance = 0.0, - const double tmax = 1.0) const = 0; + const double tmax = 1.0, + const NarrowPhaseCCDMode mode = + NarrowPhaseCCDMode::STANDALONE) const = 0; /// @brief Perform narrow phase CCD between a point and a linear edge. /// @param p_t0 The starting position of the point. @@ -39,6 +60,7 @@ class NarrowPhaseCCD { /// @param toi The time of impact. /// @param min_distance The minimum distance between the point and the edge. /// @param tmax The maximum time to check for collision. + /// @param mode How the query is used (see NarrowPhaseCCDMode). /// @return True if a collision was detected, false otherwise. virtual bool point_edge_ccd( Eigen::ConstRef p_t0, @@ -49,7 +71,9 @@ class NarrowPhaseCCD { Eigen::ConstRef e1_t1, double& toi, const double min_distance = 0.0, - const double tmax = 1.0) const = 0; + const double tmax = 1.0, + const NarrowPhaseCCDMode mode = + NarrowPhaseCCDMode::STANDALONE) const = 0; /// @brief Perform narrow phase CCD between a point and a linear triangle. /// @param p_t0 The starting position of the point. @@ -63,6 +87,7 @@ class NarrowPhaseCCD { /// @param toi The time of impact. /// @param min_distance The minimum distance between the point and the triangle. /// @param tmax The maximum time to check for collision. + /// @param mode How the query is used (see NarrowPhaseCCDMode). /// @return True if a collision was detected, false otherwise. virtual bool point_triangle_ccd( Eigen::ConstRef p_t0, @@ -75,7 +100,9 @@ class NarrowPhaseCCD { Eigen::ConstRef t2_t1, double& toi, const double min_distance = 0.0, - const double tmax = 1.0) const = 0; + const double tmax = 1.0, + const NarrowPhaseCCDMode mode = + NarrowPhaseCCDMode::STANDALONE) const = 0; /// @brief Perform narrow phase CCD between two linear edges. /// @param ea0_t0 The starting position of the first edge's first endpoint. @@ -89,6 +116,7 @@ class NarrowPhaseCCD { /// @param toi The time of impact. /// @param min_distance The minimum distance between the two edges. /// @param tmax The maximum time to check for collision. + /// @param mode How the query is used (see NarrowPhaseCCDMode). /// @return True if a collision was detected, false otherwise. virtual bool edge_edge_ccd( Eigen::ConstRef ea0_t0, @@ -101,7 +129,9 @@ class NarrowPhaseCCD { Eigen::ConstRef eb1_t1, double& toi, const double min_distance = 0.0, - const double tmax = 1.0) const = 0; + const double tmax = 1.0, + const NarrowPhaseCCDMode mode = + NarrowPhaseCCDMode::STANDALONE) const = 0; /// @brief The conservative rescaling value used to avoid taking steps exactly to impact. /// diff --git a/src/ipc/ccd/nonlinear_ccd.cpp b/src/ipc/ccd/nonlinear_ccd.cpp index 23246e668..3a5e5c89f 100644 --- a/src/ipc/ccd/nonlinear_ccd.cpp +++ b/src/ipc/ccd/nonlinear_ccd.cpp @@ -6,8 +6,6 @@ #include #include -#include - #include // #define USE_FIXED_PIECES @@ -51,14 +49,40 @@ double IntervalNonlinearTrajectory::max_distance_from_linear( // ============================================================================ +namespace { + /// The mode of a per-piece query made by the piecewise driver. + NarrowPhaseCCDMode component_mode(const bool no_zero_toi) + { + return no_zero_toi ? NarrowPhaseCCDMode::COMPONENT_NO_ZERO_TOI + : NarrowPhaseCCDMode::COMPONENT; + } +} // namespace + NonlinearCCD::NonlinearCCD( - const double _tolerance, - const long _max_iterations, + std::shared_ptr _narrow_phase_ccd, const double _conservative_rescaling) - : tolerance(_tolerance) - , max_iterations(_max_iterations) - , conservative_rescaling(_conservative_rescaling) + : conservative_rescaling(_conservative_rescaling) +{ + set_narrow_phase_ccd(std::move(_narrow_phase_ccd)); +} + +NonlinearCCD::NonlinearCCD( + const double tolerance, + const long max_iterations, + const double _conservative_rescaling) + : NonlinearCCD( + std::make_shared(tolerance, max_iterations), + _conservative_rescaling) +{ +} + +void NonlinearCCD::set_narrow_phase_ccd( + std::shared_ptr _narrow_phase_ccd) { + if (_narrow_phase_ccd == nullptr) { + _narrow_phase_ccd = std::make_shared(); + } + m_narrow_phase_ccd = std::move(_narrow_phase_ccd); } bool NonlinearCCD::conservative_piecewise_linear_ccd( @@ -82,6 +106,15 @@ bool NonlinearCCD::conservative_piecewise_linear_ccd( } assert(distance_t0 > min_distance); + // We report an impact once the distance shrinks to the target separation, + // which keeps (1 - conservative_rescaling) of the initial gap above + // min_distance. Each piece's linear CCD stops at this target (inflated by + // the piece's linearization error), so the returned toi is as large as + // possible while every point along the trajectory up to it stays at + // least this far apart. + const double target_distance = min_distance + + (1 - conservative_rescaling) * (distance_t0 - min_distance); + double ti0 = 0; std::stack ts; @@ -101,24 +134,17 @@ bool NonlinearCCD::conservative_piecewise_linear_ccd( const double distance_ti0 = distance(ti0); - // If distance has decreased by a factor and the toi is not near zero, - // then we can call this a collision. - if (distance_ti0 < (1 - conservative_rescaling) * distance_t0 - && ti0 >= TightInclusionCCD::SMALL_TOI) { - toi = ti0; - logger().trace( - "Distance small enough distance_ti0={:g}; toi={:g}", - distance_ti0, toi); - return true; - } - double min_distance_linear = max_distance_from_linear(ti0, ti1); #ifndef USE_FIXED_PIECES - // Check if the minimum distance is too large and we need to subdivide - // (Large distances cause the slow CCD) - if ((min_distance_linear - >= std::min((1 - conservative_rescaling) * distance_ti0, 0.01)) + // We subdivide while the linearization error is large compared to the + // slack the piece has above the target separation. A large error + // inflates the minimum separation, which slows the linear CCD and can + // swallow the slack entirely (reporting toi = 0 at the piece start). + if ((min_distance_linear >= std::min( + (1 - conservative_rescaling) + * (distance_ti0 - target_distance), + 0.01)) && (num_subdivisions < MAX_NUM_SUBDIVISIONS || ti0 == 0)) { logger().trace( "Subdividing at ti=[{:g}, {:g}] min_distance={:g} distance_ti0={:g}", @@ -129,7 +155,7 @@ bool NonlinearCCD::conservative_piecewise_linear_ccd( } #endif - min_distance_linear += min_distance; + min_distance_linear += target_distance; const bool is_impacting = linear_ccd( ti0, ti1, min_distance_linear, /*no_zero_toi=*/ti0 == 0, toi); @@ -143,7 +169,8 @@ bool NonlinearCCD::conservative_piecewise_linear_ccd( if (is_impacting) { toi = (ti1 - ti0) * toi + ti0; if (toi == 0) { - // This is impossible because distance_t0 > min_distance + // The linearization error swallowed the slack above the + // target separation, so we split the piece to shrink it. ts.push((ti1 + ti0) / 2); num_subdivisions++; continue; @@ -172,24 +199,14 @@ bool NonlinearCCD::point_point_ccd( return sqrt(point_point_distance(p0(t), p1(t))); }, [&](const double t0, const double t1) { - return std::max( - p0.max_distance_from_linear(t0, t1), - p1.max_distance_from_linear(t0, t1)); + return p0.max_distance_from_linear(t0, t1) + + p1.max_distance_from_linear(t0, t1); }, [&](const double ti0, const double ti1, const double _min_distance, const bool no_zero_toi, double& _toi) { - double output_tolerance; - return ticcd::edgeEdgeCCD( - to_3D(p0(ti0)), to_3D(p0(ti0)), to_3D(p1(ti0)), to_3D(p1(ti0)), - to_3D(p0(ti1)), to_3D(p0(ti1)), to_3D(p1(ti1)), to_3D(p1(ti1)), - Eigen::Array3d::Constant(-1), // rounding error (auto) - _min_distance, // minimum separation distance - _toi, // time of impact - tolerance, // delta - 1.0, // maximum time to check - max_iterations, // maximum number of iterations - output_tolerance, // delta_actual - no_zero_toi); // no zero toi + return m_narrow_phase_ccd->point_point_ccd( + p0(ti0), p1(ti0), p0(ti1), p1(ti1), _toi, _min_distance, + /*tmax=*/1.0, component_mode(no_zero_toi)); }, toi, min_distance, tmax, conservative_rescaling); } @@ -214,18 +231,9 @@ bool NonlinearCCD::point_edge_ccd( }, [&](const double ti0, const double ti1, const double _min_distance, const bool no_zero_toi, double& _toi) { - double output_tolerance; - return ticcd::edgeEdgeCCD( - to_3D(p(ti0)), to_3D(p(ti0)), to_3D(e0(ti0)), to_3D(e1(ti0)), - to_3D(p(ti1)), to_3D(p(ti1)), to_3D(e0(ti1)), to_3D(e1(ti1)), - Eigen::Array3d::Constant(-1), // rounding error (auto) - _min_distance, // minimum separation distance - _toi, // time of impact - tolerance, // delta - 1.0, // maximum time to check - max_iterations, // maximum number of iterations - output_tolerance, // delta_actual - no_zero_toi); // no zero toi + return m_narrow_phase_ccd->point_edge_ccd( + p(ti0), e0(ti0), e1(ti0), p(ti1), e0(ti1), e1(ti1), _toi, + _min_distance, /*tmax=*/1.0, component_mode(no_zero_toi)); }, toi, min_distance, tmax, conservative_rescaling); } @@ -253,18 +261,10 @@ bool NonlinearCCD::edge_edge_ccd( }, [&](const double ti0, const double ti1, const double _min_distance, const bool no_zero_toi, double& _toi) { - double output_tolerance; - return ticcd::edgeEdgeCCD( + return m_narrow_phase_ccd->edge_edge_ccd( ea0(ti0), ea1(ti0), eb0(ti0), eb1(ti0), // - ea0(ti1), ea1(ti1), eb0(ti1), eb1(ti1), - Eigen::Array3d::Constant(-1), // rounding error (auto) - _min_distance, // minimum separation distance - _toi, // time of impact - tolerance, // delta - 1.0, // maximum time to check - max_iterations, // maximum number of iterations - output_tolerance, // delta_actual - no_zero_toi); // no zero toi + ea0(ti1), ea1(ti1), eb0(ti1), eb1(ti1), // + _toi, _min_distance, /*tmax=*/1.0, component_mode(no_zero_toi)); }, toi, min_distance, tmax, conservative_rescaling); } @@ -291,18 +291,10 @@ bool NonlinearCCD::point_triangle_ccd( }, [&](const double ti0, const double ti1, const double _min_distance, const bool no_zero_toi, double& _toi) { - double output_tolerance; - return ticcd::vertexFaceCCD( + return m_narrow_phase_ccd->point_triangle_ccd( p(ti0), t0(ti0), t1(ti0), t2(ti0), // - p(ti1), t0(ti1), t1(ti1), t2(ti1), - Eigen::Array3d::Constant(-1), // rounding error (auto) - _min_distance, // minimum separation distance - _toi, // time of impact - tolerance, // delta - 1.0, // maximum time to check - max_iterations, // maximum number of iterations - output_tolerance, // delta_actual - no_zero_toi); // no zero toi + p(ti1), t0(ti1), t1(ti1), t2(ti1), // + _toi, _min_distance, /*tmax=*/1.0, component_mode(no_zero_toi)); }, toi, min_distance, tmax, conservative_rescaling); } diff --git a/src/ipc/ccd/nonlinear_ccd.hpp b/src/ipc/ccd/nonlinear_ccd.hpp index dee5c1ffa..984d6e889 100644 --- a/src/ipc/ccd/nonlinear_ccd.hpp +++ b/src/ipc/ccd/nonlinear_ccd.hpp @@ -1,12 +1,14 @@ #pragma once #include +#include #include #ifdef IPC_TOOLKIT_WITH_FILIB #include #endif #include +#include namespace ipc { @@ -50,16 +52,32 @@ class IntervalNonlinearTrajectory : virtual public NonlinearTrajectory { class NonlinearCCD { public: - /// The default tolerance used with Tight-Inclusion CCD. + /// The default tolerance of the TightInclusionCCD built by the deprecated + /// (tolerance, max_iterations) constructor. static constexpr double DEFAULT_TOLERANCE = 1e-6; - /// The default maximum number of iterations used with Tight-Inclusion CCD. + /// The default maximum number of iterations of the TightInclusionCCD + /// built by the deprecated (tolerance, max_iterations) constructor. static constexpr long DEFAULT_MAX_ITERATIONS = 10'000'000L; - /// The default conservative rescaling value used to avoid taking steps - /// exactly to impact. + /// The default conservative rescaling, which keeps 20% of the initial gap + /// so steps never go exactly to impact. static constexpr double DEFAULT_CONSERVATIVE_RESCALING = 0.8; - NonlinearCCD( - const double tolerance = DEFAULT_TOLERANCE, + /// @brief Construct a new NonlinearCCD object. + /// @param narrow_phase_ccd The narrow-phase CCD used for the per-piece linear CCD (if null, a default-constructed TightInclusionCCD is used). Each per-piece query is made with a COMPONENT NarrowPhaseCCDMode, so the object is not modified and may be shared with standalone users. + /// @param conservative_rescaling Fraction of the initial gap (above min_distance) a step may close: the CCD reports an impact once the distance shrinks to min_distance + (1 - conservative_rescaling) * (d₀ - min_distance). + explicit NonlinearCCD( + std::shared_ptr narrow_phase_ccd = nullptr, + const double conservative_rescaling = DEFAULT_CONSERVATIVE_RESCALING); + + /// @brief Construct a NonlinearCCD whose per-piece CCD is a TightInclusionCCD. + /// @deprecated Use NonlinearCCD(std::make_shared(tolerance, max_iterations), conservative_rescaling). + /// @param tolerance Tight Inclusion's solver tolerance. + /// @param max_iterations Tight Inclusion's maximum number of iterations. + /// @param conservative_rescaling Fraction of the initial gap (above min_distance) a step may close: the CCD reports an impact once the distance shrinks to min_distance + (1 - conservative_rescaling) * (d₀ - min_distance). + [[deprecated( + "Pass a std::make_shared(tolerance, max_iterations) as the narrow-phase CCD instead.")]] + explicit NonlinearCCD( + const double tolerance, const long max_iterations = DEFAULT_MAX_ITERATIONS, const double conservative_rescaling = DEFAULT_CONSERVATIVE_RESCALING); @@ -131,24 +149,34 @@ class NonlinearCCD { const double min_distance = 0.0, const double tmax = 1.0) const; - /// @brief Solver tolerance. - double tolerance; + /// @brief Set the narrow-phase CCD used for the per-piece linear CCD. + /// @param narrow_phase_ccd The narrow-phase CCD to use (if null, a default-constructed TightInclusionCCD is used). + void set_narrow_phase_ccd( + std::shared_ptr narrow_phase_ccd); - /// @brief Maximum number of iterations. - long max_iterations; + /// @brief The narrow-phase CCD used for the per-piece linear CCD (never null). + std::shared_ptr narrow_phase_ccd() const + { + return m_narrow_phase_ccd; + } - /// @brief Conservative rescaling of the time of impact. + /// @brief Fraction of the initial gap (above min_distance) a step may close: the CCD reports an impact once the distance shrinks to min_distance + (1 - conservative_rescaling) * (d₀ - min_distance). double conservative_rescaling; +private: + /// @brief The narrow-phase CCD used for the per-piece linear CCD (never null). + /// @note The piecewise driver passes each piece's linearization error as the minimum separation distance, so the narrow phase must support minimum separation (both TightInclusionCCD and AdditiveCCD do). + std::shared_ptr m_narrow_phase_ccd; + public: /// @brief Perform conservative piecewise linear CCD of a nonlinear trajectories. /// @param[in] distance Return the distance for a given time in [0, 1]. /// @param[in] max_distance_from_linear Return the maximum distance from the linearized trajectory for a given time interval. - /// @param[in] linear_ccd Perform linear CCD on a given time interval. + /// @param[in] linear_ccd Perform linear CCD on a given time interval. Receives the piece interval, the target separation inflated by the piece's linearization error, and a no_zero_toi flag (true on the first piece, whose start is known to be more than min_distance apart). /// @param[out] toi Output time of impact. /// @param[in] min_distance Minimum separation distance between the objects. /// @param[in] tmax Maximum time to check for collision. - /// @param[in] conservative_rescaling Conservative rescaling of the time of impact. + /// @param[in] conservative_rescaling Fraction of the initial gap (above min_distance) a step may close: the CCD reports an impact once the distance shrinks to min_distance + (1 - conservative_rescaling) * (d₀ - min_distance). /// @return True if a collision was detected, false otherwise. static bool conservative_piecewise_linear_ccd( const std::function& distance, diff --git a/src/ipc/ccd/tight_inclusion_ccd.cpp b/src/ipc/ccd/tight_inclusion_ccd.cpp index e13563d2e..f5985059e 100644 --- a/src/ipc/ccd/tight_inclusion_ccd.cpp +++ b/src/ipc/ccd/tight_inclusion_ccd.cpp @@ -36,8 +36,19 @@ bool TightInclusionCCD::ccd_strategy( const double min_distance, const double initial_distance, const double conservative_rescaling, - double& toi) + double& toi, + const NarrowPhaseCCDMode mode) { + if (mode != NarrowPhaseCCDMode::STANDALONE) { + // As the per-piece linear CCD inside a piecewise driver (e.g., + // NonlinearCCD), the driver supplies its own conservative margins, so + // we run Tight Inclusion directly with the caller's minimum separation + // and zero-TOI requirement (no inflation, rerun, or rescaling). + return ccd( + min_distance, mode == NarrowPhaseCCDMode::COMPONENT_NO_ZERO_TOI, + toi); + } + if (check_initial_distance(initial_distance, min_distance, toi)) { return true; } @@ -81,7 +92,8 @@ bool TightInclusionCCD::point_point_ccd_3D( Eigen::ConstRef p1_t1, double& toi, const double min_distance, - const double tmax) const + const double tmax, + const NarrowPhaseCCDMode mode) const { assert(tmax >= 0 && tmax <= 1.0); @@ -94,10 +106,11 @@ bool TightInclusionCCD::point_point_ccd_3D( const double adjusted_tolerance = std::min( INITIAL_DISTANCE_TOLERANCE_SCALE * initial_distance, tolerance); - const auto ccd = [&](const double _min_distance, const bool no_zero_toi, + const auto ccd = [&](const double _min_distance, const bool _no_zero_toi, double& _toi) -> bool { - const long _max_iterations = - no_zero_toi ? TIGHT_INCLUSION_UNLIMITED_ITERATIONS : max_iterations; + const long _max_iterations = _no_zero_toi + ? TIGHT_INCLUSION_UNLIMITED_ITERATIONS + : max_iterations; double output_tolerance; // NOTE: Use degenerate edge-edge @@ -110,7 +123,7 @@ bool TightInclusionCCD::point_point_ccd_3D( tmax, // maximum time to check _max_iterations, // maximum number of iterations output_tolerance, // delta_actual - no_zero_toi); + _no_zero_toi); if (adjusted_tolerance < output_tolerance && toi < SMALL_TOI) { logger().trace( @@ -124,7 +137,7 @@ bool TightInclusionCCD::point_point_ccd_3D( }; return ccd_strategy( - ccd, min_distance, initial_distance, conservative_rescaling, toi); + ccd, min_distance, initial_distance, conservative_rescaling, toi, mode); } bool TightInclusionCCD::point_point_ccd( @@ -134,14 +147,15 @@ bool TightInclusionCCD::point_point_ccd( Eigen::ConstRef p1_t1, double& toi, const double min_distance, - const double tmax) const + const double tmax, + const NarrowPhaseCCDMode mode) const { assert(p0_t0.size() == p1_t0.size()); assert(p0_t0.size() == p0_t1.size()); assert(p0_t0.size() == p1_t1.size()); return point_point_ccd_3D( to_3D(p0_t0), to_3D(p1_t0), to_3D(p0_t1), to_3D(p1_t1), toi, - min_distance, tmax); + min_distance, tmax, mode); } bool TightInclusionCCD::point_edge_ccd_3D( @@ -153,7 +167,8 @@ bool TightInclusionCCD::point_edge_ccd_3D( Eigen::ConstRef e1_t1, double& toi, const double min_distance, - const double tmax) const + const double tmax, + const NarrowPhaseCCDMode mode) const { assert(tmax >= 0 && tmax <= 1.0); @@ -167,10 +182,11 @@ bool TightInclusionCCD::point_edge_ccd_3D( const double adjusted_tolerance = std::min( INITIAL_DISTANCE_TOLERANCE_SCALE * initial_distance, tolerance); - const auto ccd = [&](const double _min_distance, const bool no_zero_toi, + const auto ccd = [&](const double _min_distance, const bool _no_zero_toi, double& _toi) -> bool { - const long _max_iterations = - no_zero_toi ? TIGHT_INCLUSION_UNLIMITED_ITERATIONS : max_iterations; + const long _max_iterations = _no_zero_toi + ? TIGHT_INCLUSION_UNLIMITED_ITERATIONS + : max_iterations; double output_tolerance = tolerance; // NOTE: Use degenerate edge-edge @@ -183,7 +199,7 @@ bool TightInclusionCCD::point_edge_ccd_3D( tmax, // maximum time to check _max_iterations, // maximum number of iterations output_tolerance, // delta_actual - no_zero_toi); + _no_zero_toi); if (adjusted_tolerance < output_tolerance && toi < SMALL_TOI) { logger().trace( @@ -197,7 +213,7 @@ bool TightInclusionCCD::point_edge_ccd_3D( }; return ccd_strategy( - ccd, min_distance, initial_distance, conservative_rescaling, toi); + ccd, min_distance, initial_distance, conservative_rescaling, toi, mode); } bool TightInclusionCCD::point_edge_ccd( @@ -209,14 +225,15 @@ bool TightInclusionCCD::point_edge_ccd( Eigen::ConstRef e1_t1, double& toi, const double min_distance, - const double tmax) const + const double tmax, + const NarrowPhaseCCDMode mode) const { assert(p_t1.size() == p_t0.size()); assert(e0_t0.size() == p_t0.size() && e1_t0.size() == p_t0.size()); assert(e0_t1.size() == p_t0.size() && e1_t1.size() == p_t0.size()); return point_edge_ccd_3D( to_3D(p_t0), to_3D(e0_t0), to_3D(e1_t0), to_3D(p_t1), to_3D(e0_t1), - to_3D(e1_t1), toi, min_distance, tmax); + to_3D(e1_t1), toi, min_distance, tmax, mode); } bool TightInclusionCCD::edge_edge_ccd( @@ -230,7 +247,8 @@ bool TightInclusionCCD::edge_edge_ccd( Eigen::ConstRef eb1_t1, double& toi, const double min_distance, - const double tmax) const + const double tmax, + const NarrowPhaseCCDMode mode) const { assert(tmax >= 0 && tmax <= 1.0); @@ -245,10 +263,11 @@ bool TightInclusionCCD::edge_edge_ccd( const double adjusted_tolerance = std::min( INITIAL_DISTANCE_TOLERANCE_SCALE * initial_distance, tolerance); - const auto ccd = [&](const double _min_distance, const bool no_zero_toi, + const auto ccd = [&](const double _min_distance, const bool _no_zero_toi, double& _toi) -> bool { - const long _max_iterations = - no_zero_toi ? TIGHT_INCLUSION_UNLIMITED_ITERATIONS : max_iterations; + const long _max_iterations = _no_zero_toi + ? TIGHT_INCLUSION_UNLIMITED_ITERATIONS + : max_iterations; double output_tolerance; bool is_impacting = ticcd::edgeEdgeCCD( @@ -260,7 +279,7 @@ bool TightInclusionCCD::edge_edge_ccd( tmax, // maximum time to check _max_iterations, // maximum number of iterations output_tolerance, // delta_actual - no_zero_toi); + _no_zero_toi); if (adjusted_tolerance < output_tolerance && toi < SMALL_TOI) { logger().trace( @@ -274,7 +293,7 @@ bool TightInclusionCCD::edge_edge_ccd( }; return ccd_strategy( - ccd, min_distance, initial_distance, conservative_rescaling, toi); + ccd, min_distance, initial_distance, conservative_rescaling, toi, mode); } bool TightInclusionCCD::point_triangle_ccd( @@ -288,7 +307,8 @@ bool TightInclusionCCD::point_triangle_ccd( Eigen::ConstRef t2_t1, double& toi, const double min_distance, - const double tmax) const + const double tmax, + const NarrowPhaseCCDMode mode) const { assert(tmax >= 0 && tmax <= 1.0); @@ -303,10 +323,11 @@ bool TightInclusionCCD::point_triangle_ccd( const double adjusted_tolerance = std::min( INITIAL_DISTANCE_TOLERANCE_SCALE * initial_distance, tolerance); - const auto ccd = [&](const double _min_distance, const bool no_zero_toi, + const auto ccd = [&](const double _min_distance, const bool _no_zero_toi, double& _toi) -> bool { - const long _max_iterations = - no_zero_toi ? TIGHT_INCLUSION_UNLIMITED_ITERATIONS : max_iterations; + const long _max_iterations = _no_zero_toi + ? TIGHT_INCLUSION_UNLIMITED_ITERATIONS + : max_iterations; double output_tolerance; bool is_impacting = ticcd::vertexFaceCCD( @@ -318,7 +339,7 @@ bool TightInclusionCCD::point_triangle_ccd( tmax, // maximum time to check _max_iterations, // maximum number of iterations output_tolerance, // delta_actual - no_zero_toi); + _no_zero_toi); if (adjusted_tolerance < output_tolerance && toi < SMALL_TOI) { logger().trace( @@ -332,7 +353,7 @@ bool TightInclusionCCD::point_triangle_ccd( }; return ccd_strategy( - ccd, min_distance, initial_distance, conservative_rescaling, toi); + ccd, min_distance, initial_distance, conservative_rescaling, toi, mode); } } // namespace ipc \ No newline at end of file diff --git a/src/ipc/ccd/tight_inclusion_ccd.hpp b/src/ipc/ccd/tight_inclusion_ccd.hpp index 08b434a0d..4728256ba 100644 --- a/src/ipc/ccd/tight_inclusion_ccd.hpp +++ b/src/ipc/ccd/tight_inclusion_ccd.hpp @@ -35,6 +35,7 @@ class TightInclusionCCD : public NarrowPhaseCCD { /// @param[out] toi The time of impact between the two points. /// @param[in] min_distance The minimum distance between the points. /// @param[in] tmax The maximum time to check for collisions. + /// @param[in] mode How the query is used (see NarrowPhaseCCDMode). /// @return True if a collision was detected, false otherwise. bool point_point_ccd( Eigen::ConstRef p0_t0, @@ -43,7 +44,9 @@ class TightInclusionCCD : public NarrowPhaseCCD { Eigen::ConstRef p1_t1, double& toi, const double min_distance = 0.0, - const double tmax = 1.0) const override; + const double tmax = 1.0, + const NarrowPhaseCCDMode mode = + NarrowPhaseCCDMode::STANDALONE) const override; /// @brief Computes the time of impact between a point and an edge using continuous collision detection. /// @param[in] p_t0 The initial position of the point. @@ -55,6 +58,7 @@ class TightInclusionCCD : public NarrowPhaseCCD { /// @param[out] toi The time of impact between the point and the edge. /// @param[in] min_distance The minimum distance between the objects. /// @param[in] tmax The maximum time to check for collisions. + /// @param[in] mode How the query is used (see NarrowPhaseCCDMode). /// @return True if a collision was detected, false otherwise. bool point_edge_ccd( Eigen::ConstRef p_t0, @@ -65,7 +69,9 @@ class TightInclusionCCD : public NarrowPhaseCCD { Eigen::ConstRef e1_t1, double& toi, const double min_distance = 0.0, - const double tmax = 1.0) const override; + const double tmax = 1.0, + const NarrowPhaseCCDMode mode = + NarrowPhaseCCDMode::STANDALONE) const override; /// @brief Computes the time of impact between two edges in 3D using continuous collision detection. /// @param[in] ea0_t0 The initial position of the first endpoint of the first edge. @@ -79,6 +85,7 @@ class TightInclusionCCD : public NarrowPhaseCCD { /// @param[out] toi The time of impact between the two edges. /// @param[in] min_distance The minimum distance between the objects. /// @param[in] tmax The maximum time to check for collisions. + /// @param[in] mode How the query is used (see NarrowPhaseCCDMode). /// @return True if a collision was detected, false otherwise. bool edge_edge_ccd( Eigen::ConstRef ea0_t0, @@ -91,7 +98,9 @@ class TightInclusionCCD : public NarrowPhaseCCD { Eigen::ConstRef eb1_t1, double& toi, const double min_distance = 0.0, - const double tmax = 1.0) const override; + const double tmax = 1.0, + const NarrowPhaseCCDMode mode = + NarrowPhaseCCDMode::STANDALONE) const override; /// @brief Computes the time of impact between a point and a triangle in 3D using continuous collision detection. /// @param[in] p_t0 The initial position of the point. @@ -105,6 +114,7 @@ class TightInclusionCCD : public NarrowPhaseCCD { /// @param[out] toi The time of impact between the point and the triangle. /// @param[in] min_distance The minimum distance between the objects. /// @param[in] tmax The maximum time to check for collisions. + /// @param[in] mode How the query is used (see NarrowPhaseCCDMode). /// @return True if a collision was detected, false otherwise. bool point_triangle_ccd( Eigen::ConstRef p_t0, @@ -117,7 +127,9 @@ class TightInclusionCCD : public NarrowPhaseCCD { Eigen::ConstRef t2_t1, double& toi, const double min_distance = 0.0, - const double tmax = 1.0) const override; + const double tmax = 1.0, + const NarrowPhaseCCDMode mode = + NarrowPhaseCCDMode::STANDALONE) const override; /// @brief Solver tolerance. double tolerance; @@ -134,6 +146,7 @@ class TightInclusionCCD : public NarrowPhaseCCD { /// @param[out] toi The time of impact between the two points. /// @param[in] min_distance The minimum distance between the objects. /// @param[in] tmax The maximum time to check for collisions. + /// @param[in] mode How the query is used (see NarrowPhaseCCDMode). /// @return True if a collision was detected, false otherwise. bool point_point_ccd_3D( Eigen::ConstRef p0_t0, @@ -142,7 +155,8 @@ class TightInclusionCCD : public NarrowPhaseCCD { Eigen::ConstRef p1_t1, double& toi, const double min_distance = 0.0, - const double tmax = 1.0) const; + const double tmax = 1.0, + const NarrowPhaseCCDMode mode = NarrowPhaseCCDMode::STANDALONE) const; /// @brief Computes the time of impact between a point and an edge in 3D using continuous collision detection. /// @param[in] p_t0 The initial position of the point. @@ -154,6 +168,7 @@ class TightInclusionCCD : public NarrowPhaseCCD { /// @param[out] toi The time of impact between the point and the edge. /// @param[in] min_distance The minimum distance between the objects. /// @param[in] tmax The maximum time to check for collisions. + /// @param[in] mode How the query is used (see NarrowPhaseCCDMode). /// @return True if a collision was detected, false otherwise. bool point_edge_ccd_3D( Eigen::ConstRef p_t0, @@ -164,7 +179,8 @@ class TightInclusionCCD : public NarrowPhaseCCD { Eigen::ConstRef e1_t1, double& toi, const double min_distance = 0.0, - const double tmax = 1.0) const; + const double tmax = 1.0, + const NarrowPhaseCCDMode mode = NarrowPhaseCCDMode::STANDALONE) const; /// @brief Perform the CCD strategy outlined by Li et al. [2020]. /// @param[in] ccd The continuous collision detection function. @@ -172,6 +188,7 @@ class TightInclusionCCD : public NarrowPhaseCCD { /// @param[in] initial_distance The initial distance between the objects. /// @param[in] conservative_rescaling The conservative rescaling of the time of impact. /// @param[out] toi Output time of impact. + /// @param[in] mode How the query is used. Outside STANDALONE the strategy is skipped and ccd runs once with min_distance. /// @return True if a collision was detected, false otherwise. static bool ccd_strategy( const std::function full_positions) const; /// @brief Compute the vertex positions from vertex displacements on the full mesh. diff --git a/src/ipc/collisions/tangential/plane_vertex.cpp b/src/ipc/collisions/tangential/plane_vertex.cpp index a7a0bc49a..5182feba7 100644 --- a/src/ipc/collisions/tangential/plane_vertex.cpp +++ b/src/ipc/collisions/tangential/plane_vertex.cpp @@ -93,4 +93,4 @@ PlaneVertexTangentialCollision::relative_velocity_dx_dbeta( dim() * ndof(), _closest_point.size()); } -} // namespace ipc \ No newline at end of file +} // namespace ipc diff --git a/src/ipc/collisions/tangential/plane_vertex.hpp b/src/ipc/collisions/tangential/plane_vertex.hpp index c87c76fb1..6c52c5f7e 100644 --- a/src/ipc/collisions/tangential/plane_vertex.hpp +++ b/src/ipc/collisions/tangential/plane_vertex.hpp @@ -49,4 +49,4 @@ class PlaneVertexTangentialCollision : public PlaneVertexCandidate, Eigen::ConstRef closest_point) const override; }; -} // namespace ipc \ No newline at end of file +} // namespace ipc diff --git a/src/ipc/config.hpp.in b/src/ipc/config.hpp.in index cd46c0619..a527f0ec4 100644 --- a/src/ipc/config.hpp.in +++ b/src/ipc/config.hpp.in @@ -17,6 +17,7 @@ #cmakedefine IPC_TOOLKIT_WITH_ROBIN_MAP #cmakedefine IPC_TOOLKIT_WITH_ABSEIL #cmakedefine IPC_TOOLKIT_WITH_FILIB +#cmakedefine IPC_TOOLKIT_WITH_GLTF #cmakedefine IPC_TOOLKIT_WITH_PROFILER #cmakedefine IPC_TOOLKIT_WITH_TRACY #cmakedefine IPC_TOOLKIT_WITH_MESHFEM_SPARSE diff --git a/src/ipc/dynamics/CMakeLists.txt b/src/ipc/dynamics/CMakeLists.txt new file mode 100644 index 000000000..b10e9b506 --- /dev/null +++ b/src/ipc/dynamics/CMakeLists.txt @@ -0,0 +1,17 @@ +set(SOURCES + body_potentials.cpp + body_potentials.hpp + to_affine.cpp + to_affine.hpp +) + +source_group(TREE "${CMAKE_CURRENT_SOURCE_DIR}" PREFIX "Source Files" FILES ${SOURCES}) +target_sources(ipc_toolkit PRIVATE ${SOURCES}) + +################################################################################ +# Subfolders +################################################################################ + +add_subdirectory(affine) +add_subdirectory(rigid) +add_subdirectory(time_integration) diff --git a/src/ipc/dynamics/affine/CMakeLists.txt b/src/ipc/dynamics/affine/CMakeLists.txt new file mode 100644 index 000000000..cb5ec9418 --- /dev/null +++ b/src/ipc/dynamics/affine/CMakeLists.txt @@ -0,0 +1,23 @@ +set(SOURCES + affine_dof.cpp + affine_dof.hpp + augmented_lagrangian.cpp + augmented_lagrangian.hpp + body_forces.cpp + body_forces.hpp + inertial_term.cpp + inertial_term.hpp + joints.cpp + joints.hpp + orthogonality_potential.cpp + orthogonality_potential.hpp + pose.cpp + pose.hpp +) + +source_group(TREE "${CMAKE_CURRENT_SOURCE_DIR}" PREFIX "Source Files" FILES ${SOURCES}) +target_sources(ipc_toolkit PRIVATE ${SOURCES}) + +################################################################################ +# Subfolders +################################################################################ \ No newline at end of file diff --git a/src/ipc/dynamics/affine/affine_dof.cpp b/src/ipc/dynamics/affine/affine_dof.cpp new file mode 100644 index 000000000..f6bea6486 --- /dev/null +++ b/src/ipc/dynamics/affine/affine_dof.cpp @@ -0,0 +1,73 @@ +#include "affine_dof.hpp" + +namespace ipc::affine { + +affine::Pose +dof_to_pose(Eigen::ConstRef x, const size_t i, const int dim) +{ + const int ndof = dim + dim * dim; + assert((i + 1) * ndof <= size_t(x.size())); + + affine::Pose pose; + pose.position = x.segment(i * ndof, dim); + pose.rotation = x.segment(i * ndof + dim, dim * dim).reshaped(dim, dim); + return pose; +} + +std::vector +dof_to_poses(Eigen::ConstRef x, const int dim) +{ + const int ndof = dim + dim * dim; + assert(x.size() % ndof == 0); + + std::vector poses(x.size() / ndof); + for (size_t i = 0; i < poses.size(); i++) { + poses[i] = dof_to_pose(x, i, dim); + } + return poses; +} + +Eigen::MatrixXd +vertices(const rigid::RigidBodies& bodies, Eigen::ConstRef x) +{ + const int dim = bodies.dim(); + + Eigen::MatrixXd V(bodies.num_vertices(), dim); + for (size_t i = 0; i < bodies.num_bodies(); ++i) { + V.middleRows(bodies.body_vertex_start(i), bodies.body_num_vertices(i)) = + dof_to_pose(x, i, dim).transform_vertices( + bodies.body_rest_positions(i)); + } + return V; +} + +Eigen::SparseMatrix affine_jacobian(const rigid::RigidBodies& bodies) +{ + const int dim = bodies.dim(); + const int ndof = dim + dim * dim; + + std::vector> triplets; + triplets.reserve(bodies.num_vertices() * dim * (dim + 1)); + + for (size_t i = 0; i < bodies.num_bodies(); ++i) { + const Eigen::SparseMatrix J_i = + affine::Pose::dof_jacobian(bodies.body_rest_positions(i)); + + const index_t row_start = dim * bodies.body_vertex_start(i); + const index_t col_start = ndof * i; + for (int k = 0; k < J_i.outerSize(); ++k) { + for (Eigen::SparseMatrix::InnerIterator it(J_i, k); it; + ++it) { + triplets.emplace_back( + row_start + it.row(), col_start + it.col(), it.value()); + } + } + } + + Eigen::SparseMatrix J( + dim * bodies.num_vertices(), ndof * bodies.num_bodies()); + J.setFromTriplets(triplets.begin(), triplets.end()); + return J; +} + +} // namespace ipc::affine diff --git a/src/ipc/dynamics/affine/affine_dof.hpp b/src/ipc/dynamics/affine/affine_dof.hpp new file mode 100644 index 000000000..b56566df9 --- /dev/null +++ b/src/ipc/dynamics/affine/affine_dof.hpp @@ -0,0 +1,27 @@ +#pragma once + +#include +#include + +namespace ipc::affine { + +/// @brief Extract the affine pose of body i from the stacked DOF vector. +/// DOF layout per body: [p (dim); vec(A) column-major (dim²)]. +affine::Pose +dof_to_pose(Eigen::ConstRef x, const size_t i, const int dim); + +/// @brief Extract all affine poses from the stacked DOF vector. +std::vector +dof_to_poses(Eigen::ConstRef x, const int dim); + +/// @brief Compute the world-space collision mesh vertices from the affine DOFs. +Eigen::MatrixXd +vertices(const rigid::RigidBodies& bodies, Eigen::ConstRef x); + +/// @brief The block-diagonal Jacobian dV/dx of the collision mesh vertices +/// with respect to the affine DOFs. +/// @note V(x) is linear in the affine DOFs, so this is constant and the +/// barrier chain rule is simply Jᵀ g / Jᵀ H J with no second-order term. +Eigen::SparseMatrix affine_jacobian(const rigid::RigidBodies& bodies); + +} // namespace ipc::affine diff --git a/src/ipc/dynamics/affine/augmented_lagrangian.cpp b/src/ipc/dynamics/affine/augmented_lagrangian.cpp new file mode 100644 index 000000000..2c91102f8 --- /dev/null +++ b/src/ipc/dynamics/affine/augmented_lagrangian.cpp @@ -0,0 +1,313 @@ +#include "augmented_lagrangian.hpp" + +#include + +namespace ipc::affine { + +namespace { + /// W = J ⊗ I: diagonal of the angular ABD mass block for A(k, j) DOFs + /// (DOF layout ndof·i + dim + k + dim·j ⇒ weight J(j)). + Eigen::VectorXd angular_weights(const rigid::RigidBody& body) // NOLINT + { + const int dim = body.principal_rotation().rows(); + Eigen::VectorXd w(dim * dim); + for (int j = 0; j < dim; ++j) { + w.segment(dim * j, dim).array() = + body.second_moment().diagonal()(j); + } + return w; + } + + /// Progress η = 1 − √(d²/d₀²) with an absolute floor (see the rigid AL). + double progress( + const double distance_sq, + const double start_distance_sq, + const double tol) + { + const double tol_sq = tol * tol; + if (start_distance_sq <= tol_sq) { + return distance_sq <= tol_sq ? 1.0 : 0.0; + } + return 1.0 - std::sqrt(distance_sq / start_distance_sq); + } +} // namespace + +void AugmentedLagrangian::init( + const rigid::RigidBodies& bodies, + Eigen::ConstRef x0, + const std::vector& targets, + const double multiplier_scaling) +{ + assert(targets.size() == bodies.num_bodies()); + assert(multiplier_scaling > 0); + m_dim = bodies.dim(); + assert( + x0.size() + == (m_dim + m_dim * m_dim) * Eigen::Index(bodies.num_bodies())); + + m_x0 = x0; + m_targets = targets; + m_multiplier_scaling = multiplier_scaling; + + // Every step starts from zero multipliers (see the class description). + m_lambda.assign(targets.size(), VectorMax3d::Zero(m_dim)); + m_lambda_A.assign(targets.size(), Eigen::VectorXd::Zero(m_dim * m_dim)); + + m_target_rotations.resize(targets.size()); + m_angular_weights.resize(targets.size()); + m_num_kinematic = 0; + for (size_t i = 0; i < targets.size(); ++i) { + // Only kinematic bodies have a meaningful target rotation; the rest + // are left default (never read — see the KINEMATIC guards below). + if (bodies[i].type() == rigid::RigidBody::Type::KINEMATIC) { + m_target_rotations[i] = targets[i].rotation_matrix(); + m_angular_weights[i] = angular_weights(bodies[i]); + ++m_num_kinematic; + } + } + + m_kappa_q = m_params.initial_penalty; + m_kappa_A = m_params.initial_penalty; + + m_linear_satisfied = m_num_kinematic == 0; + m_angular_satisfied = m_num_kinematic == 0; + + m_prev_eta_q = -std::numeric_limits::infinity(); + m_prev_eta_A = -std::numeric_limits::infinity(); +} + +VectorMax12d AugmentedLagrangian::target_dof(const size_t body_id) const +{ + VectorMax12d y(m_dim + m_dim * m_dim); + y.head(m_dim) = m_targets[body_id].position; + y.tail(m_dim * m_dim) = m_target_rotations[body_id].reshaped(); + return y; +} + +// ---- Progress metrics --------------------------------------------------- + +double AugmentedLagrangian::linear_progress( + const rigid::RigidBodies& bodies, Eigen::ConstRef x) const +{ + double distance_sq = 0, start_distance_sq = 0; + for (size_t i = 0; i < bodies.num_bodies(); ++i) { + if (bodies[i].type() != rigid::RigidBody::Type::KINEMATIC) { + continue; + } + const int ndof = m_dim + m_dim * m_dim; + const auto& q_hat = m_targets[i].position; + distance_sq += (q_hat - x.segment(ndof * i, m_dim)).squaredNorm(); + start_distance_sq += + (q_hat - m_x0.segment(ndof * i, m_dim)).squaredNorm(); + } + + double scale = 0; + for (size_t i = 0; i < bodies.num_bodies(); ++i) { + if (bodies[i].type() == rigid::RigidBody::Type::KINEMATIC) { + scale = std::max(scale, bodies[i].bounding_radius()); + } + } + return progress( + distance_sq, start_distance_sq, + (1.0 - m_params.satisfied_progress) * std::max(scale, 1e-12)); +} + +double AugmentedLagrangian::angular_progress( + const rigid::RigidBodies& bodies, Eigen::ConstRef x) const +{ + double distance_sq = 0, start_distance_sq = 0; + for (size_t i = 0; i < bodies.num_bodies(); ++i) { + if (bodies[i].type() != rigid::RigidBody::Type::KINEMATIC) { + continue; + } + const int ndof = m_dim + m_dim * m_dim; + const Eigen::VectorXd a_hat = m_target_rotations[i].reshaped(); + distance_sq += + (a_hat - x.segment(ndof * i + m_dim, m_dim * m_dim)).squaredNorm(); + start_distance_sq += + (a_hat - m_x0.segment(ndof * i + m_dim, m_dim * m_dim)) + .squaredNorm(); + } + + // A matrices have O(1) Frobenius scale. + return progress( + distance_sq, start_distance_sq, 1.0 - m_params.satisfied_progress); +} + +// ---- Update policy -------------------------------------------------------- + +void AugmentedLagrangian::update( + const rigid::RigidBodies& bodies, Eigen::ConstRef x) +{ + // At an energy-converged x the penalty and multiplier together supply the + // constraint force, so folding the penalty's share into the multiplier + // leaves λ equal to the full reaction. + // + // Near the target, the multiplier update alone normally closes the gap + // in one solve. It cannot when the load on the body moves between solves + // (e.g., friction re-lagged in between), because λ then chases a moving + // reaction and η can hover just short of satisfied indefinitely. So when + // the gap stops shrinking, we also double the penalty, which makes the + // body proportionally less sensitive to the change in load. + const auto stagnated = [&](const double eta, const double prev_eta) { + return 1 - eta > m_params.min_gap_reduction * (1 - prev_eta); + }; + + if (!m_linear_satisfied) { + const double eta_q = linear_progress(bodies, x); + if (eta_q >= m_params.satisfied_progress) { + m_linear_satisfied = true; + } else if ( + eta_q < m_params.stall_progress + && m_kappa_q < m_params.max_penalty) { + m_kappa_q *= 2; + } else { + update_linear_multipliers(bodies, x); + if (stagnated(eta_q, m_prev_eta_q) + && m_kappa_q < m_params.max_penalty) { + m_kappa_q *= 2; + } + } + m_prev_eta_q = eta_q; + logger().debug( + "augmented lagrangian (linear): η_q={:g} κ_q={:g} satisfied={}", + eta_q, m_kappa_q, m_linear_satisfied); + } + + if (!m_angular_satisfied) { + const double eta_A = angular_progress(bodies, x); // NOLINT + if (eta_A >= m_params.satisfied_progress) { + m_angular_satisfied = true; + } else if ( + eta_A < m_params.stall_progress + && m_kappa_A < m_params.max_penalty) { + m_kappa_A *= 2; + } else { + update_angular_multipliers(bodies, x); + if (stagnated(eta_A, m_prev_eta_A) + && m_kappa_A < m_params.max_penalty) { + m_kappa_A *= 2; + } + } + m_prev_eta_A = eta_A; + logger().debug( + "augmented lagrangian (angular): η_A={:g} κ_A={:g} satisfied={}", + eta_A, m_kappa_A, m_angular_satisfied); + } +} + +void AugmentedLagrangian::update_linear_multipliers( + const rigid::RigidBodies& bodies, Eigen::ConstRef x) +{ + const int ndof = m_dim + m_dim * m_dim; + for (size_t i = 0; i < bodies.num_bodies(); ++i) { + if (bodies[i].type() != rigid::RigidBody::Type::KINEMATIC) { + continue; + } + m_lambda[i] -= m_kappa_q * bodies[i].mass() + * (x.segment(ndof * i, m_dim) - m_targets[i].position) + / m_multiplier_scaling; + } +} + +void AugmentedLagrangian::update_angular_multipliers( + const rigid::RigidBodies& bodies, Eigen::ConstRef x) +{ + const int ndof = m_dim + m_dim * m_dim; + for (size_t i = 0; i < bodies.num_bodies(); ++i) { + if (bodies[i].type() != rigid::RigidBody::Type::KINEMATIC) { + continue; + } + const Eigen::VectorXd da = x.segment(ndof * i + m_dim, m_dim * m_dim) + - Eigen::VectorXd(m_target_rotations[i].reshaped()); + m_lambda_A[i] -= m_kappa_A * m_angular_weights[i].cwiseProduct(da) + / m_multiplier_scaling; + } +} + +// ---- Cumulative energy -------------------------------------------------- + +double AugmentedLagrangian::operator()( + const rigid::RigidBodies& bodies, Eigen::ConstRef x) const +{ + if (!active()) { + return 0.0; + } + + double energy = 0.0; + for (size_t i = 0; i < bodies.num_bodies(); ++i) { + if (bodies[i].type() != rigid::RigidBody::Type::KINEMATIC) { + continue; + } + const int ndof = m_dim + m_dim * m_dim; + const VectorMax3d dq = + x.segment(ndof * i, m_dim) - m_targets[i].position; + energy += 0.5 * m_kappa_q * bodies[i].mass() * dq.squaredNorm() + - m_multiplier_scaling * m_lambda[i].dot(dq); + + const Eigen::VectorXd da = x.segment(ndof * i + m_dim, m_dim * m_dim) + - Eigen::VectorXd(m_target_rotations[i].reshaped()); + const Eigen::VectorXd& w = m_angular_weights[i]; + energy += 0.5 * m_kappa_A * da.dot(w.cwiseProduct(da)) + - m_multiplier_scaling * m_lambda_A[i].dot(da); + } + return energy; +} + +Eigen::VectorXd AugmentedLagrangian::gradient( + const rigid::RigidBodies& bodies, Eigen::ConstRef x) const +{ + Eigen::VectorXd grad = Eigen::VectorXd::Zero(x.size()); + if (!active()) { + return grad; + } + + for (size_t i = 0; i < bodies.num_bodies(); ++i) { + if (bodies[i].type() != rigid::RigidBody::Type::KINEMATIC) { + continue; + } + const int ndof = m_dim + m_dim * m_dim; + const VectorMax3d dq = + x.segment(ndof * i, m_dim) - m_targets[i].position; + grad.segment(ndof * i, m_dim) = m_kappa_q * bodies[i].mass() * dq + - m_multiplier_scaling * m_lambda[i]; + + const Eigen::VectorXd da = x.segment(ndof * i + m_dim, m_dim * m_dim) + - Eigen::VectorXd(m_target_rotations[i].reshaped()); + const Eigen::VectorXd& w = m_angular_weights[i]; + grad.segment(ndof * i + m_dim, m_dim * m_dim) = + m_kappa_A * w.cwiseProduct(da) + - m_multiplier_scaling * m_lambda_A[i]; + } + return grad; +} + +Eigen::SparseMatrix AugmentedLagrangian::hessian( + const rigid::RigidBodies& bodies, Eigen::ConstRef x) const +{ + Eigen::SparseMatrix hess(x.size(), x.size()); + if (!active()) { + return hess; + } + + std::vector> triplets; + for (size_t i = 0; i < bodies.num_bodies(); ++i) { + if (bodies[i].type() != rigid::RigidBody::Type::KINEMATIC) { + continue; + } + const int ndof = m_dim + m_dim * m_dim; + for (int k = 0; k < m_dim; ++k) { + triplets.emplace_back( + ndof * i + k, ndof * i + k, m_kappa_q * bodies[i].mass()); + } + const Eigen::VectorXd& w = m_angular_weights[i]; + for (int k = 0; k < m_dim * m_dim; ++k) { + triplets.emplace_back( + ndof * i + m_dim + k, ndof * i + m_dim + k, m_kappa_A * w(k)); + } + } + hess.setFromTriplets(triplets.begin(), triplets.end()); + return hess; +} + +} // namespace ipc::affine diff --git a/src/ipc/dynamics/affine/augmented_lagrangian.hpp b/src/ipc/dynamics/affine/augmented_lagrangian.hpp new file mode 100644 index 000000000..9b10a8ec8 --- /dev/null +++ b/src/ipc/dynamics/affine/augmented_lagrangian.hpp @@ -0,0 +1,166 @@ +#pragma once + +#include + +#include + +namespace ipc::affine { + +/// @brief Augmented Lagrangian driving KINEMATIC bodies to per-step target +/// poses in affine coordinates [Ferguson et al. 2021]. The rigid +/// parameterization reaches it through the to-affine map's chain rule. +/// +/// With y = [p; vec(A)] per body and the exact ABD mass blocks +/// M = blkdiag(m I₃, W) with W = J ⊗ I₃ (diagonal), the two channels are +/// \f[ +/// E = \tfrac{\kappa_q}{2} m \|q - \hat{q}\|^2 +/// - s\, \lambda^\top (q - \hat{q}) +/// + \tfrac{\kappa_Q}{2} (a - \hat{a})^\top W (a - \hat{a}) +/// - s\, \lambda_A^\top (a - \hat{a}), +/// \f] +/// where â = vec(R(θ̂)) embeds the rigid target rotation and s = (βΔt)² is +/// the time integrator's acceleration scaling. Fully quadratic with constant +/// PSD Hessian. We use Rigid IPC's method-of-multipliers schedule, and also +/// double the penalty when progress stagnates short of the target. +/// +/// The two halves are scaled differently on purpose. The penalty is a +/// numerical device, so we weigh it against inertia (no s): the κ needed to +/// reach a target then shrinks with the time step instead of growing like +/// 1/Δt. The multipliers carry s like contact and friction do, so λ is the +/// constraint's reaction force (and λ_A its generalized force on vec(A)) in +/// physical units. +/// +/// Like Rigid IPC, we start every step with zero multipliers rather than the +/// previous step's reaction. A reaction can change abruptly between steps +/// (e.g., a crank at dead center as its piston reverses), and a stale one +/// then pushes the body the wrong way, so the next solves spend iterations +/// undoing it. On the piston, starting from zero saves about 8% of the Newton +/// iterations. +class AugmentedLagrangian { +public: + struct Params { + double initial_penalty = 1e3; + double max_penalty = 1e8; + double satisfied_progress = 0.999; + double stall_progress = 0.99; + /// Between the stall and satisfied thresholds, the penalty still + /// doubles when the remaining gap 1 − η fails to shrink below this + /// fraction of the previous update's gap. + double min_gap_reduction = 0.5; + }; + + AugmentedLagrangian() = default; + + explicit AugmentedLagrangian(const Params& params) : m_params(params) { } + + /// @brief Start a new step: capture the start state and targets, and + /// reset the penalties, multipliers, and satisfied flags. + /// @param bodies The collection of bodies. + /// @param x0 Step-start affine DOFs (dim + dim² per body). + /// @param targets Per-body target rigid poses (only KINEMATIC used). + /// @param multiplier_scaling The acceleration scaling s = (βΔt)² that + /// makes the multipliers physical forces. + void init( + const rigid::RigidBodies& bodies, + Eigen::ConstRef x0, + const std::vector& targets, + const double multiplier_scaling = 1); + + /// @brief The constraint's reaction force on a body's position, as of the + /// last update this step (zero before then). Only valid after init(). + const VectorMax3d& linear_multiplier(const size_t body_id) const + { + return m_lambda[body_id]; + } + + /// @brief Whether any kinematic channel is still unsatisfied. + bool active() const + { + return m_num_kinematic > 0 + && !(m_linear_satisfied && m_angular_satisfied); + } + + /// @brief Linear progress η_q ∈ (-∞, 1] toward the targets. + double linear_progress( + const rigid::RigidBodies& bodies, + Eigen::ConstRef x) const; + + /// @brief Angular progress η_A ∈ (-∞, 1] toward the targets. + double angular_progress( + const rigid::RigidBodies& bodies, + Eigen::ConstRef x) const; + + /// @brief Apply the update policy at the (energy-converged) state x. + void update( + const rigid::RigidBodies& bodies, Eigen::ConstRef x); + + bool linear_satisfied() const { return m_linear_satisfied; } + bool angular_satisfied() const { return m_angular_satisfied; } + + double linear_penalty() const { return m_kappa_q; } + double angular_penalty() const { return m_kappa_A; } + + // ---- Cumulative functions ----------------------------------------------- + + double operator()( + const rigid::RigidBodies& bodies, + Eigen::ConstRef x) const; + + Eigen::VectorXd gradient( + const rigid::RigidBodies& bodies, + Eigen::ConstRef x) const; + + /// @brief The Hessian (block diagonal, constant per step, PSD). + Eigen::SparseMatrix hessian( + const rigid::RigidBodies& bodies, + Eigen::ConstRef x) const; + +private: + /// @brief Target affine DOFs [p̂; vec(R(θ̂))] per body (dim + dim²). + VectorMax12d target_dof(const size_t body_id) const; + + /// @brief Method-of-multipliers update: fold the linear penalty's force + /// at x into λ, i.e., λ ← λ − κ_q m (q − q̂) / s. + void update_linear_multipliers( + const rigid::RigidBodies& bodies, Eigen::ConstRef x); + + /// @brief λ_A ← λ_A − κ_A W (a − â) / s. + void update_angular_multipliers( + const rigid::RigidBodies& bodies, Eigen::ConstRef x); + + Params m_params; + + size_t m_num_kinematic = 0; + + /// @brief Scene dimension (set at init). + int m_dim = 3; + + /// @brief Step-start affine DOFs (η denominators). + Eigen::VectorXd m_x0; + /// @brief Target rigid poses. + std::vector m_targets; + /// @brief Target rotation matrices (cached). + std::vector m_target_rotations; + /// @brief Diagonal of each kinematic body's angular mass block W (cached). + std::vector m_angular_weights; + + double m_kappa_q = 1e3; + double m_kappa_A = 1e3; // NOLINT(readability-identifier-naming) + + /// @brief The acceleration scaling s = (βΔt)² on the multiplier terms. + double m_multiplier_scaling = 1; + + /// @brief Per-body multipliers (λ linear ∈ R^dim, λ_A angular ∈ R^dim²), + /// zeroed at the start of every step. + std::vector m_lambda; + std::vector m_lambda_A; // NOLINT + + bool m_linear_satisfied = false; + bool m_angular_satisfied = false; + + /// @brief Each channel's progress at the previous update this step. + double m_prev_eta_q = -std::numeric_limits::infinity(); + double m_prev_eta_A = -std::numeric_limits::infinity(); // NOLINT +}; + +} // namespace ipc::affine diff --git a/src/ipc/dynamics/affine/body_forces.cpp b/src/ipc/dynamics/affine/body_forces.cpp new file mode 100644 index 000000000..5e058ff0a --- /dev/null +++ b/src/ipc/dynamics/affine/body_forces.cpp @@ -0,0 +1,48 @@ +#include "body_forces.hpp" + +#include + +namespace ipc::affine { + +void BodyForces::update( + const rigid::RigidBodies& bodies, const std::vector& poses) +{ + assert(poses.size() == bodies.num_bodies()); + + const int dim = bodies.dim(); + const int ndof = dim + dim * dim; + + m_wrench = Eigen::VectorXd::Zero(ndof * bodies.num_bodies()); + for (size_t i = 0; i < bodies.num_bodies(); ++i) { + if (!bodies[i].is_dynamic()) { + continue; // no gravity or external forces + } + m_wrench.segment(ndof * i, dim) = + -(bodies[i].mass() * gravity().head(dim) + + bodies[i].external_force().position); + + const auto& torque = bodies[i].external_force().rotation; + if (torque.isZero()) { + continue; + } + + // We pull the world-space torque τ back to the affine matrix A with + // the linear energy E(A) = −⟨½ [τ]× A₀, A⟩, where A₀ is the step-start + // A. Perturbing A₀ by a small world rotation δφ, A = (I + [δφ]×) A₀, + // gives δE = −tr(½ A₀ᵀ [τ]×ᵀ [δφ]× A₀) = −τ·δφ, so the generalized + // torque is exactly τ regardless of the body's orientation. The ½ + // comes from tr([a]×ᵀ [b]×) = 2 a·b. + const auto& A = poses[i].rotation; + if (dim == 3) { + const Eigen::Matrix3d W = + 0.5 * cross_product_matrix(torque) * Eigen::Matrix3d(A); + m_wrench.segment<9>(ndof * i + 3) = -W.reshaped(); + } else { + const Eigen::Matrix2d W = + 0.5 * cross_product_matrix(torque(0)) * Eigen::Matrix2d(A); + m_wrench.segment<4>(ndof * i + 2) = -W.reshaped(); + } + } +} + +} // namespace ipc::affine diff --git a/src/ipc/dynamics/affine/body_forces.hpp b/src/ipc/dynamics/affine/body_forces.hpp new file mode 100644 index 000000000..1719ada0a --- /dev/null +++ b/src/ipc/dynamics/affine/body_forces.hpp @@ -0,0 +1,59 @@ +#pragma once + +#include +#include + +namespace ipc::affine { + +/// @brief Gravitational and external forces/torques on affine bodies. +/// +/// The energy is linear in the affine DOFs: E(x) = wᵀx with a per-body wrench +/// w = [−(m g + f_ext); −vec(½ [τ]× A₀)] (column-major), where τ is the +/// world-space torque and A₀ the step-start affine matrix. The torque term +/// applies a generalized torque of exactly τ at A₀. +/// +/// This is a pure potential (no time-integrator scaling): the simulator is +/// responsible for scaling it into the incremental potential (by (βΔt)²). +class BodyForces { +public: + BodyForces() = default; + + VectorMax3d gravity() const { return m_gravity; } + void set_gravity(Eigen::ConstRef gravity) + { + m_gravity = gravity; + } + + /// @brief Recompute the per-body wrenches from the current state. + /// @param bodies The collection of bodies. + /// @param poses The poses of the bodies at the start of the step (used to + /// transform world-space torques into body space). + void update( + const rigid::RigidBodies& bodies, + const std::vector& poses); + + double operator()( + const rigid::RigidBodies& bodies, + Eigen::ConstRef x) const + { + return m_wrench.dot(x); + } + + Eigen::VectorXd gradient( + const rigid::RigidBodies& bodies, + Eigen::ConstRef x) const + { + return m_wrench; + } + + // The Hessian is identically zero. + +private: + /// @brief Gravitational acceleration + VectorMax3d m_gravity = Eigen::Vector3d(0, -9.81, 0); + + /// @brief Stacked per-body wrenches (dim + dim² DOF per body) + Eigen::VectorXd m_wrench; +}; + +} // namespace ipc::affine diff --git a/src/ipc/dynamics/affine/inertial_term.cpp b/src/ipc/dynamics/affine/inertial_term.cpp new file mode 100644 index 000000000..4c8e7eb9f --- /dev/null +++ b/src/ipc/dynamics/affine/inertial_term.cpp @@ -0,0 +1,87 @@ +#include "inertial_term.hpp" + +#include + +namespace ipc::affine { + +InertialTerm::InertialTerm( + const rigid::RigidBodies& bodies, + const std::shared_ptr& + _time_integrator) + : time_integrator(_time_integrator) +{ + build_mass_matrix(bodies); +} + +void InertialTerm::build_mass_matrix(const rigid::RigidBodies& bodies) +{ + // The exact ABD mass matrix: because the rest positions are COM-centered + // in the principal inertia frame, ∫ρ x̄ dV = 0 and ∫ρ x̄x̄ᵀ dV = J + // (diagonal), so with q = [p; vec(A) column-major] the inertial energy + // ½m‖p − p̂‖² + ½tr((A − Â) J (A − Â)ᵀ) has mass matrix + // M = blkdiag(m I, J ⊗ I). + // Non-DYNAMIC bodies contribute no inertia (zero blocks): their motion is + // fully prescribed (pinned DOFs and/or the augmented Lagrangian). + const int dim = bodies.dim(); + const int ndof = dim + dim * dim; + m_effective_masses = effective_masses(bodies); + std::vector> triplets; + for (int i = 0; i < bodies.num_bodies(); i++) { + if (!bodies[i].is_dynamic()) { + continue; + } + const double mass = bodies[i].mass(); + const auto& J = bodies[i].second_moment().diagonal(); + assert(J.size() == dim); + for (int k = 0; k < dim; k++) { + triplets.emplace_back(i * ndof + k, i * ndof + k, mass); + for (int j = 0; j < dim; j++) { + const int dof = i * ndof + dim + k + dim * j; // A(k, j) + triplets.emplace_back(dof, dof, J(j)); + } + } + } + m_mass.resize(ndof * bodies.num_bodies(), ndof * bodies.num_bodies()); + m_mass.setFromTriplets(triplets.begin(), triplets.end()); +} + +void InertialTerm::update(const rigid::RigidBodies& bodies) +{ + // Update the predicted positions from the current time integrator state + m_x_hat = time_integrator->predicted_positions(); + // Body types can change at runtime (convert_to_static), and with them the + // zero blocks of non-DYNAMIC bodies. That is rare, so we only rebuild the + // mass matrix when a body's type or mass actually changed. + if (effective_masses(bodies) != m_effective_masses) { + build_mass_matrix(bodies); + } +} + +std::vector +InertialTerm::effective_masses(const rigid::RigidBodies& bodies) +{ + std::vector masses(bodies.num_bodies()); + for (size_t i = 0; i < bodies.num_bodies(); i++) { + masses[i] = bodies[i].is_dynamic() ? bodies[i].mass() : 0.0; + } + return masses; +} + +// ---- Cumulative functions --------------------------------------------------- + +double InertialTerm::operator()( + const rigid::RigidBodies& bodies, Eigen::ConstRef x) const +{ + assert(x.size() == m_x_hat.size()); + Eigen::VectorXd dx = x - m_x_hat; + return 0.5 * (dx.transpose() * m_mass * dx)(0, 0); +} + +Eigen::VectorXd InertialTerm::gradient( + const rigid::RigidBodies& bodies, Eigen::ConstRef x) const +{ + assert(x.size() == m_x_hat.size()); + return m_mass * (x - m_x_hat); +} + +} // namespace ipc::affine \ No newline at end of file diff --git a/src/ipc/dynamics/affine/inertial_term.hpp b/src/ipc/dynamics/affine/inertial_term.hpp new file mode 100644 index 000000000..ee3c13ef2 --- /dev/null +++ b/src/ipc/dynamics/affine/inertial_term.hpp @@ -0,0 +1,69 @@ +#pragma once + +#include + +namespace ipc::dynamics { +class ImplicitTimeIntegrator; +} + +namespace ipc::affine { + +/// @brief Class representing the term ... +class InertialTerm { +public: + InertialTerm( + const rigid::RigidBodies& bodies, + const std::shared_ptr& + time_integrator); + + /// @brief Update the predicted poses of the rigid bodies. + /// @param bodies The collection of rigid bodies. + void update(const rigid::RigidBodies& bodies); + + // ---- Cumulative functions ----------------------------------------------- + + /// @brief Compute the total energy for all rigid bodies. + /// @param bodies The collection of rigid bodies. + /// @param x The DOFs of the rigid bodies, where the first 3 entries are the positions and the last 3 entries are the rotations. + /// @return The total energy of the rigid bodies. + double operator()( + const rigid::RigidBodies& bodies, + Eigen::ConstRef x) const; + + /// @brief Compute the gradient of the total energy for all rigid bodies. + /// @param bodies The collection of rigid bodies. + /// @param x The DOFs of the rigid bodies, where the first 3 entries are the positions and the last 3 entries are the rotations. + /// @return The gradient of the total energy of the rigid bodies. + Eigen::VectorXd gradient( + const rigid::RigidBodies& bodies, + Eigen::ConstRef x) const; + + /// @brief The (constant, sparse) mass matrix — also the Hessian. + const Eigen::SparseMatrix& mass_matrix() const { return m_mass; } + +private: + /// @brief (Re)build the block-diagonal ABD mass matrix (zero blocks for + /// non-DYNAMIC bodies). + void build_mass_matrix(const rigid::RigidBodies& bodies); + + /// @brief Each body's mass if it is DYNAMIC, else zero: everything the + /// mass matrix depends on that can change after construction (the second + /// moment scales with the mass). + static std::vector + effective_masses(const rigid::RigidBodies& bodies); + + const std::shared_ptr + time_integrator; + + /// Mass matrix for the entire system (block diagonal with each block + /// corresponding to a rigid body) + Eigen::SparseMatrix m_mass; + + /// Cached predicted poses for the rigid body + Eigen::VectorXd m_x_hat; + + /// The effective_masses() the mass matrix was built from + std::vector m_effective_masses; +}; + +} // namespace ipc::affine \ No newline at end of file diff --git a/src/ipc/dynamics/affine/joints.cpp b/src/ipc/dynamics/affine/joints.cpp new file mode 100644 index 000000000..86771028e --- /dev/null +++ b/src/ipc/dynamics/affine/joints.cpp @@ -0,0 +1,335 @@ +#include "joints.hpp" + +#include + +#include +#include +#include +#include + +namespace ipc::affine { + +JointConstraints::JointConstraints( + const std::shared_ptr& bodies, + const std::vector& initial_poses) + : m_bodies(bodies) + , m_initial_poses(initial_poses) +{ + assert(initial_poses.size() == m_bodies->num_bodies()); +} + +VectorMax3d JointConstraints::world_to_material( + const size_t body, Eigen::ConstRef world_point) const +{ + assert(body < m_bodies->num_bodies()); + const rigid::Pose& pose = m_initial_poses[body]; + // x̄ = A⁻¹ (p_world − p); A ∈ SO(dim) initially, so A⁻¹ = Aᵀ + return pose.rotation_matrix().transpose() * (world_point - pose.position); +} + +void JointConstraints::check_can_add(const size_t body) const +{ + // These are runtime checks rather than asserts because the Python + // bindings reach them from release builds, where a bad index would write + // out of bounds and a late joint would be silently ignored. + if (m_finalized) { + throw std::logic_error( + "joint constraints cannot be added after finalize() (the " + "Simulator finalizes them on construction)"); + } + if (body >= m_bodies->num_bodies()) { + throw std::out_of_range( + "joint body index " + std::to_string(body) + " is out of range (" + + std::to_string(m_bodies->num_bodies()) + " bodies)"); + } +} + +int JointConstraints::body_ndof() const +{ + const int dim = m_bodies->dim(); + return dim + dim * dim; +} + +void JointConstraints::add_point_coefficients( + Eigen::Ref row, + const size_t body, + Eigen::ConstRef material_point, + const int k, + const double sign) const +{ + const int dim = m_bodies->dim(); + const int ndof = body_ndof(); + // p(x)_k = x[ndof·b + k] + Σⱼ x[ndof·b + dim + k + dim·j] x̄ⱼ + row(ndof * body + k) += sign; + for (int j = 0; j < dim; j++) { + row(ndof * body + dim + k + dim * j) += sign * material_point(j); + } +} + +void JointConstraints::add_point_connection( + const size_t body_a, + const size_t body_b, + Eigen::ConstRef world_anchor) +{ + check_can_add(body_a); + check_can_add(body_b); + if (body_a == body_b) { + throw std::invalid_argument( + "a point connection needs two distinct bodies"); + } + const VectorMax3d xa = world_to_material(body_a, world_anchor); + const VectorMax3d xb = world_to_material(body_b, world_anchor); + const int n = body_ndof() * m_bodies->num_bodies(); + for (int k = 0; k < m_bodies->dim(); k++) { + Eigen::RowVectorXd row = Eigen::RowVectorXd::Zero(n); + add_point_coefficients(row, body_a, xa, k, 1.0); + add_point_coefficients(row, body_b, xb, k, -1.0); + m_rows.push_back(row); + m_rhs.push_back(0.0); + } +} + +void JointConstraints::add_fixed_point( + const size_t body, Eigen::ConstRef world_anchor) +{ + check_can_add(body); + const VectorMax3d xb = world_to_material(body, world_anchor); + const int n = body_ndof() * m_bodies->num_bodies(); + for (int k = 0; k < m_bodies->dim(); k++) { + Eigen::RowVectorXd row = Eigen::RowVectorXd::Zero(n); + add_point_coefficients(row, body, xb, k, 1.0); + m_rows.push_back(row); + m_rhs.push_back(world_anchor(k)); + } +} + +void JointConstraints::add_hinge( + const size_t body_a, + const size_t body_b, + Eigen::ConstRef world_axis_p0, + Eigen::ConstRef world_axis_p1) +{ + if (m_bodies->dim() == 2) { + // A 2D "hinge" is a point connection; two anchors would be redundant + // rows (and trip the redundancy check in finalize()). + throw std::invalid_argument( + "add_hinge is 3D-only; use add_point_connection in 2D"); + } + add_point_connection(body_a, body_b, world_axis_p0); + add_point_connection(body_a, body_b, world_axis_p1); +} + +void JointConstraints::add_sliding_plane( + const size_t body, + Eigen::ConstRef world_anchor, + Eigen::ConstRef normal) +{ + check_can_add(body); + const VectorMax3d xb = world_to_material(body, world_anchor); + const VectorMax3d n_hat = normal.normalized(); + const int n = body_ndof() * m_bodies->num_bodies(); + Eigen::RowVectorXd row = Eigen::RowVectorXd::Zero(n); + for (int k = 0; k < m_bodies->dim(); k++) { + add_point_coefficients(row, body, xb, k, n_hat(k)); + } + m_rows.push_back(row); + m_rhs.push_back(n_hat.dot(world_anchor)); +} + +void JointConstraints::add_fixed_body(const size_t body) +{ + check_can_add(body); + const int dim = m_bodies->dim(); + const int ndof = body_ndof(); + const int n = ndof * m_bodies->num_bodies(); + const rigid::Pose& pose = m_initial_poses[body]; + const MatrixMax3d A = pose.rotation_matrix(); + for (int d = 0; d < ndof; d++) { + Eigen::RowVectorXd row = Eigen::RowVectorXd::Zero(n); + row(ndof * body + d) = 1.0; + m_rows.push_back(row); + m_rhs.push_back( + d < dim ? pose.position(d) : A.reshaped()(d - dim)); // column-major + } +} + +void JointConstraints::finalize() +{ + if (m_finalized) { + return; + } + + const int n = body_ndof() * int(m_bodies->num_bodies()); + const int m = int(m_rows.size()); + + // Only the DOFs some constraint touches take part in the elimination; + // every other DOF maps to itself. Restricting to these "active" columns + // keeps the change of variables sparse: identity outside the jointed + // bodies, so its cost scales with the joints, not the scene. + std::vector active; + for (int j = 0; j < n; j++) { + if (std::any_of(m_rows.begin(), m_rows.end(), [&](const auto& row) { + return row(j) != 0; + })) { + active.push_back(j); + } + } + const int na = int(active.size()); + if (m > na) { + throw std::runtime_error( + "more joint constraints than constrained DOFs; joints are " + "redundant"); + } + + // Assemble the active columns of C (m×na) and s + Eigen::MatrixXd C(m, na); + Eigen::VectorXd s(m); + for (int i = 0; i < m; i++) { + for (int j = 0; j < na; j++) { + C(i, j) = m_rows[i](active[j]); + } + s(i) = m_rhs[i]; + } + + // Gaussian elimination with column pivoting: make C upper triangular in a + // permuted DOF order so that V = [C̃; 0 I] is a full-rank upper-triangular + // basis extension [Chen et al. 2022, §4.1]. + std::vector perm(na); + std::iota(perm.begin(), perm.end(), 0); + + for (int i = 0; i < m; i++) { + // Eliminate the previous pivots from row i + for (int k = 0; k < i; k++) { + const double f = C(i, k) / C(k, k); + if (f != 0) { + C.row(i) -= f * C.row(k); + s(i) -= f * s(k); + } + } + // Swap the largest remaining coefficient onto the diagonal + int j_star; + const double pivot = C.row(i).tail(na - i).cwiseAbs().maxCoeff(&j_star); + j_star += i; + if (pivot < 1e-12) { + logger().error( + "joint constraint row {} is linearly dependent; joints are " + "redundant", + i); + throw std::runtime_error("redundant joint constraints"); + } + if (j_star != i) { + C.col(i).swap(C.col(j_star)); + std::swap(perm[i], perm[j_star]); + } + } + + // In the permuted active order V = [[C₁, C₂], [0, I]] with C₁ (m×m) upper + // triangular, so U = V⁻¹ = [[C₁⁻¹, −C₁⁻¹C₂], [0, I]]. + const Eigen::MatrixXd C1_inv = + C.leftCols(m).triangularView().solve( + Eigen::MatrixXd::Identity(m, m)); + const Eigen::MatrixXd W = -C1_inv * C.rightCols(na - m); + + // The reduced coordinates are ordered [pivots (m); remaining active DOFs; + // inactive DOFs], i.e., z_k = (V x)_k with x_full[perm_full[k]] the k-th + // permuted DOF. + std::vector perm_full; + perm_full.reserve(n); + for (int k = 0; k < na; k++) { + perm_full.push_back(active[perm[k]]); + } + { + std::vector is_active(n, false); + for (const int j : active) { + is_active[j] = true; + } + for (int j = 0; j < n; j++) { + if (!is_active[j]) { + perm_full.push_back(j); + } + } + } + + std::vector> u_triplets, v_triplets; + u_triplets.reserve(size_t(m) * na + n); + v_triplets.reserve(size_t(m) * na + n); + for (int i = 0; i < m; i++) { + for (int k = 0; k < na; k++) { + // z_i = Σₖ C̃(i, k) x[perm_full[k]] + if (C(i, k) != 0) { + v_triplets.emplace_back(i, perm_full[k], C(i, k)); + } + // x[perm_full[i]] = Σₖ U(i, k) z_k + const double u = k < m ? C1_inv(i, k) : W(i, k - m); + if (u != 0) { + u_triplets.emplace_back(perm_full[i], k, u); + } + } + } + for (int k = m; k < n; k++) { + v_triplets.emplace_back(k, perm_full[k], 1.0); + u_triplets.emplace_back(perm_full[k], k, 1.0); + } + + m_u.resize(n, n); + m_u.setFromTriplets(u_triplets.begin(), u_triplets.end()); + m_v.resize(n, n); + m_v.setFromTriplets(v_triplets.begin(), v_triplets.end()); + + m_s = s; + + m_perm_inv.resize(n); + for (int k = 0; k < n; k++) { + m_perm_inv[perm_full[k]] = k; + } + + m_finalized = true; +} + +bool JointConstraints::is_body_constrained(const size_t body) const +{ + assert(body < m_bodies->num_bodies()); + const int ndof = body_ndof(); + return std::any_of(m_rows.begin(), m_rows.end(), [&](const auto& row) { + return !row.segment(ndof * body, ndof).isZero(); + }); +} + +int JointConstraints::free_reduced_index(const int full_dof) const +{ + assert(m_finalized); +#ifndef NDEBUG + for (const auto& row : m_rows) { + assert(row(full_dof) == 0.0); + } +#endif + const int k = m_perm_inv[full_dof]; + assert(k >= int(num_constraints())); + return k; +} + +Eigen::VectorXd +JointConstraints::to_reduced(Eigen::ConstRef x) const +{ + assert(m_finalized); + return m_v * x; +} + +Eigen::VectorXd +JointConstraints::to_full(Eigen::ConstRef z) const +{ + assert(m_finalized); + return m_u * z; +} + +Eigen::VectorXd +JointConstraints::residual(Eigen::ConstRef x) const +{ + Eigen::VectorXd r(m_rows.size()); + for (size_t i = 0; i < m_rows.size(); i++) { + r(i) = m_rows[i].dot(x) - m_rhs[i]; + } + return r; +} + +} // namespace ipc::affine diff --git a/src/ipc/dynamics/affine/joints.hpp b/src/ipc/dynamics/affine/joints.hpp new file mode 100644 index 000000000..26938d468 --- /dev/null +++ b/src/ipc/dynamics/affine/joints.hpp @@ -0,0 +1,134 @@ +#pragma once + +#include + +namespace ipc::affine { + +/// @brief Linear equality joint constraints for affine bodies. +/// +/// A material point p(x) = A x̄ + p is *linear* in the affine DOFs +/// [p; vec(A)], so the common articulation joints (point connection, fixed +/// point, hinge, sliding plane) are linear equality constraints Cx = s +/// [Chen et al. 2022, §4.1]. They are enforced exactly via a change of +/// variables x = Uz: build an upper-triangular basis extension V of the +/// constraint rows (Gaussian elimination with column pivoting), set U = V⁻¹, +/// and pin the first m entries of z = Vx to s (Dirichlet-style) during the +/// Newton solve. U and V are sparse: they act as the identity on every DOF no +/// constraint touches. +/// +/// @throws std::out_of_range / std::invalid_argument from the add_* methods +/// for an invalid body, and std::logic_error once finalize() has run. +/// +/// Anchor points are given in world space at the initial configuration and +/// converted to material coordinates using the initial poses. +class JointConstraints { +public: + /// @param bodies The bodies (for DOF counts and dimensions). + /// @param initial_poses The initial poses used to convert world-space + /// anchors to material coordinates. + JointConstraints( + const std::shared_ptr& bodies, + const std::vector& initial_poses); + + /// @brief Glue a point of body_a to a point of body_b (dim rows). + /// @param body_a Index of the first body. + /// @param body_b Index of the second body. + /// @param world_anchor The common point in world space (initially). + void add_point_connection( + const size_t body_a, + const size_t body_b, + Eigen::ConstRef world_anchor); + + /// @brief Fix a material point of a body at its initial world position (dim rows). + void add_fixed_point( + const size_t body, Eigen::ConstRef world_anchor); + + /// @brief Hinge between two bodies about the axis through two points + /// (two point connections; 6 rows). + /// @throws std::invalid_argument in 2D (use add_point_connection). + void add_hinge( + const size_t body_a, + const size_t body_b, + Eigen::ConstRef world_axis_p0, + Eigen::ConstRef world_axis_p1); + + /// @brief Constrain a material point of a body to the plane through + /// world_anchor with the given normal (1 row). + void add_sliding_plane( + const size_t body, + Eigen::ConstRef world_anchor, + Eigen::ConstRef normal); + + /// @brief Fix all DOFs of a body at their initial values (dim + dim² rows). + void add_fixed_body(const size_t body); + + /// @brief Number of constraint rows m. + size_t num_constraints() const { return m_rows.size(); } + + bool empty() const { return m_rows.empty(); } + + /// @brief Build the change-of-variable matrices. Call after adding all + /// joints (the Simulator calls this on construction). + void finalize(); + + // -- Post-finalize interface ---------------------------------------------- + + /// @brief Map full DOFs to reduced coordinates: z = Vx. + Eigen::VectorXd to_reduced(Eigen::ConstRef x) const; + + /// @brief Map reduced coordinates to full DOFs: x = Uz. + Eigen::VectorXd to_full(Eigen::ConstRef z) const; + + /// @brief The change-of-variable matrix U (x = Uz), sparse. + const Eigen::SparseMatrix& reduction_basis() const { return m_u; } + + /// @brief The pinned values s of the first m reduced coordinates. + const Eigen::VectorXd& rhs() const { return m_s; } + + /// @brief Evaluate the constraint residual Cx − s. + Eigen::VectorXd residual(Eigen::ConstRef x) const; + + /// @brief Whether any joint references the given body's DOFs. + bool is_body_constrained(const size_t body) const; + + /// @brief The reduced coordinate z index of an un-jointed full DOF + /// (identity under the reduction: z_k = x_full_dof exactly). + /// @note Only valid after finalize() and only for DOFs whose column is + /// zero in every constraint row (asserted). + int free_reduced_index(const int full_dof) const; + +private: + /// @brief Add the coefficients of world-point coordinate k of a material + /// point x̄ on a body to a constraint row. + void add_point_coefficients( + Eigen::Ref row, + const size_t body, + Eigen::ConstRef material_point, + const int k, + const double sign) const; + + /// @brief Throw unless joints can still be added and @p body is valid. + void check_can_add(const size_t body) const; + + /// @brief Convert a world-space anchor to body material coordinates. + VectorMax3d world_to_material( + const size_t body, Eigen::ConstRef world_point) const; + + /// @brief The DOFs per body (dim + dim²). + int body_ndof() const; + + std::shared_ptr m_bodies; + std::vector m_initial_poses; + + /// Constraint rows of C (each of size ndof) and right-hand sides + std::vector m_rows; + std::vector m_rhs; + + bool m_finalized = false; + Eigen::SparseMatrix m_u; ///< x = Uz (permutation folded in) + Eigen::SparseMatrix m_v; ///< z = Vx (permutation folded in) + Eigen::VectorXd m_s; ///< pinned values of z.head(m) + std::vector m_perm_inv; ///< full DOF index → permuted (z) index +}; + +} // namespace ipc::affine diff --git a/src/ipc/dynamics/affine/orthogonality_potential.cpp b/src/ipc/dynamics/affine/orthogonality_potential.cpp new file mode 100644 index 000000000..ea9ff9be9 --- /dev/null +++ b/src/ipc/dynamics/affine/orthogonality_potential.cpp @@ -0,0 +1,156 @@ +#include "orthogonality_potential.hpp" + +#include +#include +#include +#include +#include + +namespace ipc::affine { + +double OrthogonalityPotential::operator()( + const rigid::RigidBodies& bodies, Eigen::ConstRef x) const +{ + if (bodies.num_bodies() == 0) { + return 0; + } + + const int ndof = x.size() / bodies.num_bodies(); + + return tbb::parallel_reduce( + tbb::blocked_range(size_t(0), bodies.num_bodies()), 0.0, + [&](const tbb::blocked_range& r, double local_sum) { + for (size_t i = r.begin(); i < r.end(); ++i) { + local_sum += operator()(bodies[i], x.segment(i * ndof, ndof)); + } + return local_sum; + }, + std::plus()); +} + +Eigen::VectorXd OrthogonalityPotential::gradient( + const rigid::RigidBodies& bodies, Eigen::ConstRef x) const +{ + if (bodies.num_bodies() == 0) { + return Eigen::VectorXd(); + } + + const int ndof = x.size() / bodies.num_bodies(); + + Eigen::VectorXd grad(x.size()); + tbb::parallel_for( + tbb::blocked_range(size_t(0), bodies.num_bodies()), + [&](const tbb::blocked_range& r) { + for (size_t i = r.begin(); i < r.end(); ++i) { + grad.segment(ndof * i, ndof) = + gradient(bodies[i], x.segment(i * ndof, ndof)); + } + }); + return grad; +} + +Eigen::SparseMatrix OrthogonalityPotential::hessian( + const rigid::RigidBodies& bodies, + Eigen::ConstRef x, + const PSDProjectionMethod project_hessian_to_psd) const +{ + if (bodies.num_bodies() == 0) { + return Eigen::SparseMatrix(); + } + + const int ndof = x.size() / bodies.num_bodies(); + + tbb::enumerable_thread_specific>> + storage; + + tbb::parallel_for( + tbb::blocked_range(size_t(0), bodies.num_bodies()), + [&](const tbb::blocked_range& r) { + auto& hess_triplets = storage.local(); + + for (size_t i = r.begin(); i < r.end(); ++i) { + const MatrixMax12d local_hess = hessian( + bodies[i], x.segment(i * ndof, ndof), + project_hessian_to_psd); + for (size_t hi = 0; hi < local_hess.rows(); ++hi) { + for (size_t hj = 0; hj < local_hess.rows(); ++hj) { + hess_triplets.emplace_back( + ndof * i + hi, ndof * i + hj, local_hess(hi, hj)); + } + } + } + }); + + Eigen::SparseMatrix hess(x.size(), x.size()); + for (const auto& local_hess_triplets : storage) { + Eigen::SparseMatrix local_hess(x.size(), x.size()); + local_hess.setFromTriplets( + local_hess_triplets.begin(), local_hess_triplets.end()); + hess += local_hess; + } + return hess; +} + +// -- Single body methods --------------------------------------------- + +double OrthogonalityPotential::operator()( + const rigid::RigidBody& body, Eigen::ConstRef x) const +{ + const int dim = body.principal_rotation().rows(); + const auto& A = x.tail(dim * dim).reshaped(dim, dim); + const auto I = MatrixMax3d::Identity(dim, dim); + return stiffness * body.volume() * (A * A.transpose() - I).squaredNorm(); +} + +VectorMax12d OrthogonalityPotential::gradient( + const rigid::RigidBody& body, Eigen::ConstRef x) const +{ + VectorMax12d grad = VectorMax12d::Zero(x.size()); + + const int dim = body.principal_rotation().rows(); + const auto& A = x.tail(dim * dim).reshaped(dim, dim); + const auto I = MatrixMax3d::Identity(dim, dim); + + const MatrixMax3d G = + stiffness * body.volume() * 4 * (A * A.transpose() - I) * A; + grad.tail(A.size()) = G.reshaped(); + + return grad; +} + +MatrixMax12d OrthogonalityPotential::hessian( + const rigid::RigidBody& body, + Eigen::ConstRef x, + const PSDProjectionMethod project_hessian_to_psd) const +{ + const int dim = body.principal_rotation().rows(); + const auto& A = x.tail(dim * dim).reshaped(dim, dim); + const auto I = MatrixMax3d::Identity(dim, dim); + + MatrixMax12d hess = MatrixMax12d::Zero(x.size(), x.size()); + // NOTE: top left block is with respect to p (translation) and is zero + + for (int i = 0; i < A.cols(); i++) { + for (int j = 0; j < A.cols(); j++) { + auto hess_aij = hess.block(i * dim + dim, j * dim + dim, dim, dim); + + // Extra outer product term if i == j + hess_aij += (int(i == j) + 1) * A.col(j) * A.col(i).transpose() + + (A.col(i).dot(A.col(j)) - int(i == j)) * I; + + if (i == j) { + for (int k = 0; k < A.cols(); k++) { + if (i != k) { // Already added the i == k term above + hess_aij += A.col(k) * A.col(k).transpose(); + } + } + } + + hess_aij *= 4 * stiffness * body.volume(); + } + } + + return project_to_psd(hess, project_hessian_to_psd); +} + +} // namespace ipc::affine \ No newline at end of file diff --git a/src/ipc/dynamics/affine/orthogonality_potential.hpp b/src/ipc/dynamics/affine/orthogonality_potential.hpp new file mode 100644 index 000000000..55cfa9b0e --- /dev/null +++ b/src/ipc/dynamics/affine/orthogonality_potential.hpp @@ -0,0 +1,44 @@ +#pragma once + +#include + +namespace ipc::affine { + +class OrthogonalityPotential { +public: + OrthogonalityPotential(const double stiffness) : stiffness(stiffness) { } + + // ---- Cumulative functions ----------------------------------------------- + + double operator()( + const rigid::RigidBodies& bodies, + Eigen::ConstRef x) const; + + Eigen::VectorXd gradient( + const rigid::RigidBodies& bodies, + Eigen::ConstRef x) const; + + Eigen::SparseMatrix hessian( + const rigid::RigidBodies& bodies, + Eigen::ConstRef x, + const PSDProjectionMethod project_hessian_to_psd = + PSDProjectionMethod::NONE) const; + + // ---- Per-body functions ------------------------------------------------- + + double operator()( + const rigid::RigidBody& body, Eigen::ConstRef x) const; + + VectorMax12d gradient( + const rigid::RigidBody& body, Eigen::ConstRef x) const; + + MatrixMax12d hessian( + const rigid::RigidBody& body, + Eigen::ConstRef x, + const PSDProjectionMethod project_hessian_to_psd = + PSDProjectionMethod::NONE) const; + + double stiffness; +}; + +} // namespace ipc::affine \ No newline at end of file diff --git a/src/ipc/dynamics/affine/pose.cpp b/src/ipc/dynamics/affine/pose.cpp new file mode 100644 index 000000000..a5899307d --- /dev/null +++ b/src/ipc/dynamics/affine/pose.cpp @@ -0,0 +1,61 @@ +#include "pose.hpp" + +#include + +namespace ipc::affine { + +VectorMax3d Pose::rotation_vector() const +{ + assert(rotation.rows() == rotation.cols()); + assert(rotation.rows() == 2 || rotation.rows() == 3); + assert(rotation.isUnitary(1e-9)); // R must be an actual rotation + if (rotation.rows() == 2) { + // For 2D, return the angle + VectorMax3d angle(1); + // rotation(1, 0) = sin(θ), rotation(0, 0) = cos(θ) + // Thus, θ = atan2(sin(θ), cos(θ)) + angle(0) = std::atan2(rotation(1, 0), rotation(0, 0)); + return angle; + } else { + // For 3D, return the rotation vector + return rigid::rotation_matrix_to_vector(rotation); + } +} + +void Pose::set_rotation_vector(Eigen::ConstRef theta) +{ + assert(theta.size() == 1 || theta.size() == 3); + if (theta.size() == 1) { + rotation = rigid::rotation_angle_to_matrix(theta(0)); + } else { + // For 3D, convert the rotation vector to a rotation matrix + rotation = rigid::rotation_vector_to_matrix(theta); + } +} + +Eigen::SparseMatrix +Pose::dof_jacobian(Eigen::ConstRef rest_positions) +{ + const int dim = rest_positions.cols(); + + std::vector> triplets; + triplets.reserve(rest_positions.rows() * dim * (dim + 1)); + + for (int i = 0; i < rest_positions.rows(); i++) { + for (int k = 0; k < dim; k++) { + // ∂(world_{i,k}) / ∂p_k = 1 + triplets.emplace_back(dim * i + k, k, 1); + // world_{i,k} = p_k + Σ_j A(k, j) x̄(i, j) + for (int j = 0; j < dim; j++) { + triplets.emplace_back( + dim * i + k, dim + k + dim * j, rest_positions(i, j)); + } + } + } + + Eigen::SparseMatrix J(rest_positions.size(), dim + dim * dim); + J.setFromTriplets(triplets.begin(), triplets.end()); + return J; +} + +} // namespace ipc::affine diff --git a/src/ipc/dynamics/affine/pose.hpp b/src/ipc/dynamics/affine/pose.hpp new file mode 100644 index 000000000..c4db690c6 --- /dev/null +++ b/src/ipc/dynamics/affine/pose.hpp @@ -0,0 +1,64 @@ +#pragma once + +#include + +#include + +namespace ipc::affine { + +/// @brief An affine body pose: world = A x̄ + p. +/// +/// @p rotation is a dim×dim matrix A — a rotation matrix for rigid bodies (RBD) +/// or a general affine map for affine bodies (ABD). This is the shared +/// "affine-coordinate" representation of a body's configuration. +struct Pose { + /// @brief Position (translation) of the body. + VectorMax3d position; + /// @brief The dim×dim affine/rotation matrix A. + MatrixMax3d rotation; + + /// @brief Recover the rotation vector (3D) or angle (2D) from A. + /// @note Assumes A ∈ SO(dim). This holds for rigid bodies but NOT for + /// affine bodies, whose A is only softly orthogonal — do not call + /// this on a raw affine A. Use the affine pose directly (e.g. + /// transform_vertices) instead of projecting to a rigid rotation. + /// @return The rotation vector (3D) or 1-vector angle (2D). + VectorMax3d rotation_vector() const; + + /// @brief Set A from a rotation vector (3D) or angle (2D). + /// @param theta The rotation vector (3D) or angle (2D). + void set_rotation_vector(Eigen::ConstRef theta); + + /// @brief Transform vertices from local to world coordinates (A x̄ + p). + /// @param V The vertices to transform (each row is a vertex). + /// @return The transformed vertices. + Eigen::MatrixXd transform_vertices(Eigen::ConstRef V) const + { + // Compute: A x̄ + p + // transpose because V is row-ordered + return (V * rotation.transpose()).rowwise() + position.transpose(); + } + + /// @brief Compute the Jacobian of the transformed vertices w.r.t. the DOFs. + /// @param V The vertices to transform (each row is a vertex). + /// @return The Jacobian matrix of size (num_vertices * dim) x ndof. + Eigen::SparseMatrix + transform_vertices_jacobian(Eigen::ConstRef V) const + { + return dof_jacobian(V); + } + + /// @brief Compute the Jacobian of the transformed vertices w.r.t. the DOFs. + /// + /// DOF layout: q = [p (dim); vec(A) column-major (dim²)], i.e., + /// q[dim + k + dim·j] = A(k, j) — matching the integrator's state layout + /// and OrthogonalityPotential. Rows are vertex-major (dim·i + k), matching + /// the collision-mesh gradient layout. + /// + /// @param rest_positions The rest vertices (each row is a vertex). + /// @return The Jacobian matrix of size (num_vertices * dim) x (ndof). + static Eigen::SparseMatrix + dof_jacobian(Eigen::ConstRef rest_positions); +}; + +} // namespace ipc::affine diff --git a/src/ipc/dynamics/body_potentials.cpp b/src/ipc/dynamics/body_potentials.cpp new file mode 100644 index 000000000..fedac5210 --- /dev/null +++ b/src/ipc/dynamics/body_potentials.cpp @@ -0,0 +1,124 @@ +#include "body_potentials.hpp" + +#include + +namespace ipc::dynamics { + +BodyPotentials::BodyPotentials( + const rigid::RigidBodies& bodies, + std::shared_ptr time_integrator, + std::shared_ptr to_affine, + const double orthogonality_stiffness, + const affine::AugmentedLagrangian::Params& al_params) + : m_time_integrator(std::move(time_integrator)) + , m_to_affine(std::move(to_affine)) + , m_inertial(bodies, m_time_integrator) + , m_al(al_params) +{ + // The orthogonality penalty keeps A near SO(dim); it is only meaningful for + // the affine (identity) parameterization — rigid rotations are exact. + if (m_to_affine->is_identity()) { + m_orthogonality.emplace(orthogonality_stiffness); + } +} + +void BodyPotentials::update(const rigid::RigidBodies& bodies) +{ + m_inertial.update(bodies); + + // Start-of-step poses used to transform world torques into body space. + std::vector poses(bodies.num_bodies()); + for (size_t i = 0; i < bodies.num_bodies(); ++i) { + poses[i] = m_time_integrator->pose(i); + } + m_body_forces.update(bodies, poses); +} + +Eigen::VectorXd BodyPotentials::affine_gradient( + const rigid::RigidBodies& bodies, + Eigen::ConstRef y, + const double scaling, + const bool include_al) const +{ + Eigen::VectorXd g = m_inertial.gradient(bodies, y) + + scaling * m_body_forces.gradient(bodies, y); + if (m_orthogonality) { + g += scaling * m_orthogonality->gradient(bodies, y); + } + // The AL weighs its penalty against inertia and scales its multipliers + // itself, so it enters unscaled (see AugmentedLagrangian). + if (include_al && m_al.active()) { + g += m_al.gradient(bodies, y); + } + return g; +} + +double BodyPotentials::energy( + const rigid::RigidBodies& bodies, + Eigen::ConstRef x, + const double scaling, + const bool include_al) const +{ + const Eigen::VectorXd y = m_to_affine->to_affine(x); + + double E = m_inertial(bodies, y) + scaling * m_body_forces(bodies, y); + if (m_orthogonality) { + E += scaling * (*m_orthogonality)(bodies, y); + } + if (include_al && m_al.active()) { + E += m_al(bodies, y); + } + return E; +} + +Eigen::VectorXd BodyPotentials::gradient( + const rigid::RigidBodies& bodies, + Eigen::ConstRef x, + const double scaling, + const bool include_al) const +{ + const Eigen::VectorXd y = m_to_affine->to_affine(x); + return m_to_affine->apply_gradient( + x, affine_gradient(bodies, y, scaling, include_al)); +} + +Eigen::SparseMatrix BodyPotentials::hessian( + const rigid::RigidBodies& bodies, + Eigen::ConstRef x, + const double scaling, + const PSDProjectionMethod project_hessian_to_psd, + const bool include_al) const +{ + const Eigen::VectorXd y = m_to_affine->to_affine(x); + + // Affine-coordinate Hessian (body forces are linear → zero Hessian). The + // term Hessians are left unprojected; apply_hessian projects once. + Eigen::SparseMatrix H = m_inertial.mass_matrix(); + if (m_orthogonality) { + H += scaling + * m_orthogonality->hessian(bodies, y, PSDProjectionMethod::NONE); + } + if (include_al && m_al.active()) { + H += m_al.hessian(bodies, y); + } + + const Eigen::VectorXd g = affine_gradient(bodies, y, scaling, include_al); + return m_to_affine->apply_hessian(x, g, H, project_hessian_to_psd); +} + +void BodyPotentials::init_augmented_lagrangian( + const rigid::RigidBodies& bodies, + Eigen::ConstRef x0, + const std::vector& targets, + const double multiplier_scaling) +{ + m_al.init(bodies, m_to_affine->to_affine(x0), targets, multiplier_scaling); +} + +void BodyPotentials::update_augmented_lagrangian( + const rigid::RigidBodies& bodies, Eigen::ConstRef x) +{ + m_al.update(bodies, m_to_affine->to_affine(x)); +} + +} // namespace ipc::dynamics diff --git a/src/ipc/dynamics/body_potentials.hpp b/src/ipc/dynamics/body_potentials.hpp new file mode 100644 index 000000000..6fadea201 --- /dev/null +++ b/src/ipc/dynamics/body_potentials.hpp @@ -0,0 +1,148 @@ +#pragma once + +#include +#include +#include +#include +#include + +#include +#include + +namespace ipc::dynamics { + +class ImplicitTimeIntegrator; + +/// @brief The body (non-contact) part of the incremental potential, expressed +/// in the optimization DOFs x through a single to-affine map. +/// +/// It owns the per-body affine-coordinate terms — inertia, body forces, the +/// SO(dim) orthogonality penalty (affine bodies only), and the augmented +/// Lagrangian — evaluates each at y = to_affine(x), sums their +/// gradients/Hessians (with the incremental-potential scaling: inertia +/// unscaled, the physical potentials × s), and applies the to-affine chain +/// rule once: +/// ∇ₓE = (dy/dx)ᵀ (g_inertia + s (g_bf + g_orth) [+ g_al]) +/// ∇²ₓE = (dy/dx)ᵀ H (dy/dx) + Σₖ gₖ d²yₖ/dx². +/// The augmented Lagrangian applies s to its own multiplier terms and weighs +/// its penalty against inertia, so it is added unscaled. +/// +/// These are the block-diagonal-per-body terms; pairwise contact/friction use +/// the vertex-space chain rule and are added separately. +/// +/// The augmented Lagrangian is separable via @p include_al so the adaptive +/// barrier stiffness can be seeded from the physical forces only. +class BodyPotentials { +public: + BodyPotentials( + const rigid::RigidBodies& bodies, + std::shared_ptr time_integrator, + std::shared_ptr to_affine, + const double orthogonality_stiffness, + const affine::AugmentedLagrangian::Params& al_params = {}); + + VectorMax3d gravity() const { return m_body_forces.gravity(); } + void set_gravity(Eigen::ConstRef gravity) + { + m_body_forces.set_gravity(gravity); + } + + /// @brief Refresh the predicted state and body-force torques for a new step. + void update(const rigid::RigidBodies& bodies); + + // ---- Incremental-potential contribution --------------------------------- + + double energy( + const rigid::RigidBodies& bodies, + Eigen::ConstRef x, + const double scaling, + const bool include_al = true) const; + + Eigen::VectorXd gradient( + const rigid::RigidBodies& bodies, + Eigen::ConstRef x, + const double scaling, + const bool include_al = true) const; + + Eigen::SparseMatrix hessian( + const rigid::RigidBodies& bodies, + Eigen::ConstRef x, + const double scaling, + const PSDProjectionMethod project_hessian_to_psd = + PSDProjectionMethod::NONE, + const bool include_al = true) const; + + // ---- Augmented Lagrangian lifecycle (operates in affine coordinates) ---- + + /// @brief Start a step's augmented Lagrangian for the kinematic bodies. + /// @param bodies The collection of bodies. + /// @param x0 Step-start affine DOFs. + /// @param targets Per-body target rigid poses (only KINEMATIC used). + /// @param multiplier_scaling The acceleration scaling (βΔt)² applied to + /// the multiplier terms (the penalty is weighed against inertia). + void init_augmented_lagrangian( + const rigid::RigidBodies& bodies, + Eigen::ConstRef x0, + const std::vector& targets, + const double multiplier_scaling); + + void update_augmented_lagrangian( + const rigid::RigidBodies& bodies, Eigen::ConstRef x); + + bool augmented_lagrangian_active() const { return m_al.active(); } + bool augmented_lagrangian_linear_satisfied() const + { + return m_al.linear_satisfied(); + } + bool augmented_lagrangian_angular_satisfied() const + { + return m_al.angular_satisfied(); + } + + double augmented_lagrangian_linear_progress( + const rigid::RigidBodies& bodies, + Eigen::ConstRef x) const + { + return m_al.linear_progress(bodies, m_to_affine->to_affine(x)); + } + double augmented_lagrangian_angular_progress( + const rigid::RigidBodies& bodies, + Eigen::ConstRef x) const + { + return m_al.angular_progress(bodies, m_to_affine->to_affine(x)); + } + + affine::AugmentedLagrangian& augmented_lagrangian() { return m_al; } + const affine::AugmentedLagrangian& augmented_lagrangian() const + { + return m_al; + } + + // ---- Accessors ---------------------------------------------------------- + + const ToAffine& to_affine() const { return *m_to_affine; } + + /// @brief The constant affine-coordinate inertial (mass) matrix. + const Eigen::SparseMatrix& mass_matrix() const + { + return m_inertial.mass_matrix(); + } + +private: + /// @brief The summed affine-coordinate gradient (inertia + s·(rest)). + Eigen::VectorXd affine_gradient( + const rigid::RigidBodies& bodies, + Eigen::ConstRef y, + const double scaling, + const bool include_al) const; + + std::shared_ptr m_time_integrator; + std::shared_ptr m_to_affine; + + affine::InertialTerm m_inertial; + affine::BodyForces m_body_forces; + affine::AugmentedLagrangian m_al; + std::optional m_orthogonality; +}; + +} // namespace ipc::dynamics diff --git a/src/ipc/dynamics/rigid/CMakeLists.txt b/src/ipc/dynamics/rigid/CMakeLists.txt new file mode 100644 index 000000000..006d79cbc --- /dev/null +++ b/src/ipc/dynamics/rigid/CMakeLists.txt @@ -0,0 +1,23 @@ +set(SOURCES + mass.cpp + mass.hpp + pose.cpp + pose.hpp + rigid_bodies.cpp + rigid_bodies.hpp + rigid_body.cpp + rigid_body.hpp + rigid_candidates.cpp + rigid_candidates.hpp + rigid_contact_hessian.cpp + rigid_contact_hessian.hpp + rigid_trajectory.cpp + rigid_trajectory.hpp +) + +source_group(TREE "${CMAKE_CURRENT_SOURCE_DIR}" PREFIX "Source Files" FILES ${SOURCES}) +target_sources(ipc_toolkit PRIVATE ${SOURCES}) + +################################################################################ +# Subfolders +################################################################################ \ No newline at end of file diff --git a/src/ipc/dynamics/rigid/mass.cpp b/src/ipc/dynamics/rigid/mass.cpp new file mode 100644 index 000000000..a79bb7b59 --- /dev/null +++ b/src/ipc/dynamics/rigid/mass.cpp @@ -0,0 +1,285 @@ +#include "mass.hpp" + +#include + +#include + +namespace ipc::rigid { + +namespace { + + /// Lumped-mass properties for edge meshes and point clouds (both + /// dimensions): edge-Voronoi masses when edges are given, unit mass per + /// point otherwise. + /// + /// In 2D the returned inertia is the full 2x2 second moment + /// ∫ρ x̄x̄ᵀ about the center of mass (the scalar moment about z is its + /// trace). In 3D it is the classical inertia tensor Σ mᵢ (‖r‖² I − r rᵀ) + /// about the center of mass. + void compute_mass_properties_lumped( + Eigen::ConstRef vertices, + Eigen::ConstRef edges, + const double density, + double& total_mass, + VectorMax3d& center, + MatrixMax3d& inertia) + { + assert(vertices.size() > 0); + const int dim = vertices.cols(); + + Eigen::VectorXd masses; + if (edges.rows() > 0) { + assert(edges.cols() == 2); + Eigen::SparseMatrix mass_matrix; + construct_mass_matrix(vertices, edges, mass_matrix); + masses = mass_matrix.diagonal(); + } else { + // Point cloud: unit mass per point. + masses = Eigen::VectorXd::Ones(vertices.rows()); + } + + total_mass = masses.sum(); + if (total_mass == 0) { + center.setZero(dim); + } else { + center = + (masses.asDiagonal() * vertices).colwise().sum() / total_mass; + } + + inertia.setZero(dim, dim); + for (long i = 0; i < vertices.rows(); i++) { + const VectorMax3d r = vertices.row(i).transpose() - center; + if (dim == 2) { + // Second moment Σ mᵢ r rᵀ + inertia += masses(i) * r * r.transpose(); + } else { + // Classical tensor Σ mᵢ (‖r‖² I − r rᵀ) + inertia += masses(i) + * (r.squaredNorm() * Eigen::Matrix3d::Identity() + - r * r.transpose()); + } + } + + // Total mass above is the paremeter length of the edges, so we need to + // multiply by the density to get the total mass in the correct units. + total_mass *= density; + // Same for the inertia. + inertia *= density; + } + + // Based on ChTriangleMeshConnected.cpp::ComputeMassProperties from Chrono: + // Copyright (c) 2016, Project Chrono Development Team + // All rights reserved. + // https://github.com/projectchrono/chrono/blob/main/LICENSE + // + // This requires the mesh to be closed, watertight, with proper triangle + // orientation. + bool compute_mass_properties_3D( + Eigen::ConstRef vertices, + Eigen::ConstRef faces, + const double density, + double& total_mass, + VectorMax3d& center, + MatrixMax3d& inertia) + { + assert(vertices.cols() == 3); + assert(faces.rows() > 0 && faces.cols() == 3); + + // order: 1, x, y, z, x^2, y^2, z^2, xy, yz, zx + std::array integral {}; + + for (int i = 0; i < faces.rows(); i++) { + // Get vertices of triangle i. + const Eigen::Vector3d& v0 = vertices.row(faces(i, 0)); + const Eigen::Vector3d& v1 = vertices.row(faces(i, 1)); + const Eigen::Vector3d& v2 = vertices.row(faces(i, 2)); + + // Get cross product of edges and normal vector. + const Eigen::Vector3d& V1mV0 = v1 - v0; + const Eigen::Vector3d& V2mV0 = v2 - v0; + const Eigen::Vector3d& N = V1mV0.cross(V2mV0); + + // Compute integral terms. + double tmp0, tmp1, tmp2; + double f1x, f2x, f3x, g0x, g1x, g2x; + tmp0 = v0.x() + v1.x(); + f1x = tmp0 + v2.x(); + tmp1 = v0.x() * v0.x(); + tmp2 = tmp1 + v1.x() * tmp0; + f2x = tmp2 + v2.x() * f1x; + f3x = v0.x() * tmp1 + v1.x() * tmp2 + v2.x() * f2x; + g0x = f2x + v0.x() * (f1x + v0.x()); + g1x = f2x + v1.x() * (f1x + v1.x()); + g2x = f2x + v2.x() * (f1x + v2.x()); + + double f1y, f2y, f3y, g0y, g1y, g2y; + tmp0 = v0.y() + v1.y(); + f1y = tmp0 + v2.y(); + tmp1 = v0.y() * v0.y(); + tmp2 = tmp1 + v1.y() * tmp0; + f2y = tmp2 + v2.y() * f1y; + f3y = v0.y() * tmp1 + v1.y() * tmp2 + v2.y() * f2y; + g0y = f2y + v0.y() * (f1y + v0.y()); + g1y = f2y + v1.y() * (f1y + v1.y()); + g2y = f2y + v2.y() * (f1y + v2.y()); + + double f1z, f2z, f3z, g0z, g1z, g2z; + tmp0 = v0.z() + v1.z(); + f1z = tmp0 + v2.z(); + tmp1 = v0.z() * v0.z(); + tmp2 = tmp1 + v1.z() * tmp0; + f2z = tmp2 + v2.z() * f1z; + f3z = v0.z() * tmp1 + v1.z() * tmp2 + v2.z() * f2z; + g0z = f2z + v0.z() * (f1z + v0.z()); + g1z = f2z + v1.z() * (f1z + v1.z()); + g2z = f2z + v2.z() * (f1z + v2.z()); + + // Update integrals. + integral[0] += N.x() * f1x; + integral[1] += N.x() * f2x; + integral[2] += N.y() * f2y; + integral[3] += N.z() * f2z; + integral[4] += N.x() * f3x; + integral[5] += N.y() * f3y; + integral[6] += N.z() * f3z; + integral[7] += N.x() * (v0.y() * g0x + v1.y() * g1x + v2.y() * g2x); + integral[8] += N.y() * (v0.z() * g0y + v1.z() * g1y + v2.z() * g2y); + integral[9] += N.z() * (v0.x() * g0z + v1.x() * g1z + v2.x() * g2z); + } + + constexpr double inv_6 = 1.0 / 6.0; + constexpr double inv_24 = 1.0 / 24.0; + constexpr double inv_60 = 1.0 / 60.0; + constexpr double inv_120 = 1.0 / 120.0; + integral[0] *= inv_6; + integral[1] *= inv_24; + integral[2] *= inv_24; + integral[3] *= inv_24; + integral[4] *= inv_60; + integral[5] *= inv_60; + integral[6] *= inv_60; + integral[7] *= inv_120; + integral[8] *= inv_120; + integral[9] *= inv_120; + + // total_mass + total_mass = integral[0]; + if (total_mass <= 0 || !std::isfinite(total_mass)) { + // 3D mass computation only works for closed meshes! + return false; + } + assert(total_mass > 0); + + // center of mass + center = + Eigen::Vector3d(integral[1], integral[2], integral[3]) / total_mass; + + // inertia relative to world origin + inertia.resize(3, 3); + inertia(0, 0) = integral[5] + integral[6]; + inertia(0, 1) = -integral[7]; + inertia(0, 2) = -integral[9]; + inertia(1, 0) = inertia(0, 1); + inertia(1, 1) = integral[4] + integral[6]; + inertia(1, 2) = -integral[8]; + inertia(2, 0) = inertia(0, 2); + inertia(2, 1) = inertia(1, 2); + inertia(2, 2) = integral[4] + integral[5]; + + // inertia relative to center of mass + inertia(0, 0) -= + total_mass * (center.y() * center.y() + center.z() * center.z()); + inertia(0, 1) += total_mass * center.x() * center.y(); + inertia(0, 2) += total_mass * center.z() * center.x(); + inertia(1, 0) = inertia(0, 1); + inertia(1, 1) -= + total_mass * (center.z() * center.z() + center.x() * center.x()); + inertia(1, 2) += total_mass * center.y() * center.z(); + inertia(2, 0) = inertia(0, 2); + inertia(2, 1) = inertia(1, 2); + inertia(2, 2) -= + total_mass * (center.x() * center.x() + center.y() * center.y()); + + // Total mass above is the volume of the mesh, so we need to multiply by + // the density to get the total mass in the correct units. + total_mass *= density; + // Same for the inertia. + inertia *= density; + + return true; + } + +} // namespace + +void compute_mass_properties( + Eigen::ConstRef vertices, + Eigen::ConstRef facets, + const double density, + double& total_mass, + VectorMax3d& center, + MatrixMax3d& inertia) +{ + assert(facets.cols() <= 3); + if (vertices.cols() == 2 || facets.size() == 0 || facets.cols() != 3) { + // 2D meshes (facets = edges), 3D edge meshes, and point clouds. + if (facets.cols() == 2) { + compute_mass_properties_lumped( + vertices, facets, density, total_mass, center, inertia); + } else { + compute_mass_properties_lumped( + vertices, Eigen::MatrixXi(0, 2), density, total_mass, center, + inertia); + } + } else if ( + facets.rows() < 4 // Not enough faces to form a closed mesh + || !compute_mass_properties_3D( + vertices, facets, density, total_mass, center, inertia)) { + // If the mesh is not closed, fall back to lumped point masses. + compute_mass_properties_lumped( + vertices, Eigen::MatrixXi(0, 2), density, total_mass, center, + inertia); + } +} + +// Construct the sparse mass matrix for the given mesh (V, F). +void construct_mass_matrix( + Eigen::ConstRef vertices, + Eigen::ConstRef facets, + Eigen::SparseMatrix& mass_matrix) +{ + if (facets.rows() == 0) { + // Point cloud: unit mass per point. + mass_matrix.resize(vertices.rows(), vertices.rows()); + mass_matrix.setIdentity(); + } else if (vertices.cols() == 2 || facets.cols() == 2) { + assert(facets.cols() == 2); + Eigen::VectorXd vertex_masses = Eigen::VectorXd::Zero(vertices.rows()); + for (long i = 0; i < facets.rows(); i++) { + const double edge_length = + (vertices.row(facets(i, 1)) - vertices.row(facets(i, 0))) + .norm(); + // Add Voronoi areas to the vertex weight + vertex_masses(facets(i, 0)) += edge_length / 2; + vertex_masses(facets(i, 1)) += edge_length / 2; + } + std::vector> triplets; + triplets.reserve(vertices.rows()); + for (long i = 0; i < vertices.rows(); i++) { + triplets.emplace_back(i, i, vertex_masses(i)); + } + mass_matrix.resize(vertices.rows(), vertices.rows()); + mass_matrix.setFromTriplets(triplets.begin(), triplets.end()); + } else if (facets.cols() == 3) { + assert(vertices.cols() == 3); // Only use triangles in 3D + // libigl is precompiled for plain matrices, so we pass copies. + igl::massmatrix( + Eigen::MatrixXd(vertices), Eigen::MatrixXi(facets), + igl::MassMatrixType::MASSMATRIX_TYPE_VORONOI, mass_matrix); + } else { + // Probably a point cloud + mass_matrix.resize(vertices.rows(), vertices.rows()); + mass_matrix.setIdentity(); + } +} + +} // namespace ipc::rigid diff --git a/src/ipc/dynamics/rigid/mass.hpp b/src/ipc/dynamics/rigid/mass.hpp new file mode 100644 index 000000000..9591bd8c5 --- /dev/null +++ b/src/ipc/dynamics/rigid/mass.hpp @@ -0,0 +1,39 @@ +#pragma once + +#include + +#include +#include + +namespace ipc::rigid { + +/// @brief Compute the total mass, center of mass, and moment of inertia +/// @note Convention: in 3D the returned inertia is the classical inertia +/// tensor about the center of mass; in 2D it is the full 2×2 second moment +/// ∫ρ x̄x̄ᵀ about the center of mass (the scalar moment about z is its +/// trace). +/// @param vertices Vertices of the mesh +/// @param facets Facets (2D: edges; 3D: triangles; edge networks and point +/// clouds get lumped masses) of the mesh +/// @param density Uniform mass density of the body +/// @param total_mass Total mass of the mesh +/// @param center_of_mass Center of mass of the mesh +/// @param moment_of_inertia Moment of inertia of the mesh +void compute_mass_properties( + Eigen::ConstRef vertices, + Eigen::ConstRef facets, + const double density, + double& total_mass, + VectorMax3d& center_of_mass, + MatrixMax3d& moment_of_inertia); + +/// @brief Construct the sparse mass matrix for the given mesh (V, E). +/// @param vertices Vertices of the mesh +/// @param facets Facets (2D: edges; 3D: triangles) of the mesh +/// @param mass_matrix Sparse mass matrix of the mesh +void construct_mass_matrix( + Eigen::ConstRef vertices, + Eigen::ConstRef facets, + Eigen::SparseMatrix& mass_matrix); + +} // namespace ipc::rigid diff --git a/src/ipc/dynamics/rigid/pose.cpp b/src/ipc/dynamics/rigid/pose.cpp new file mode 100644 index 000000000..92fdb9b1d --- /dev/null +++ b/src/ipc/dynamics/rigid/pose.cpp @@ -0,0 +1,314 @@ +#include "pose.hpp" + +#include +#include +#include + +namespace ipc::rigid { + +Eigen::Matrix3d +rotation_vector_to_matrix(Eigen::ConstRef theta) +{ + const double sinc_angle = sinc_norm_x(theta); + const double sinc_half_angle = sinc_norm_x((theta / 2).eval()); + const Eigen::Matrix3d K = cross_product_matrix(theta); + const Eigen::Matrix3d K2 = K * K; + Eigen::Matrix3d R = + sinc_angle * K + 0.5 * sinc_half_angle * sinc_half_angle * K2; + R.diagonal().array() += 1.0; + return R; +} + +Eigen::Matrix +rotation_vector_to_matrix_jacobian(Eigen::ConstRef theta) +{ + assert(theta.size() == 3); // Only valid for 3D rotation vectors + + const double s = sinc_norm_x(theta); + const double sh = sinc_norm_x((theta / 2).eval()); + const double sh_sq = sh * sh; + const Eigen::Matrix3d K = cross_product_matrix(theta); + + const Eigen::Vector3d ds = sinc_norm_x_grad(theta); + const Eigen::Vector3d dsh = 0.5 * sinc_norm_x_grad((theta / 2).eval()); + + std::array dK; + dK[0] << 0, 0, 0, 0, 0, -1, 0, 1, 0; + dK[1] << 0, 0, 1, 0, 0, 0, -1, 0, 0; + dK[2] << 0, -1, 0, 1, 0, 0, 0, 0, 0; + + std::array s_dK; + s_dK[0] << 0, 0, 0, 0, 0, -s, 0, s, 0; + s_dK[1] << 0, 0, s, 0, 0, 0, -s, 0, 0; + s_dK[2] << 0, -s, 0, s, 0, 0, 0, 0, 0; + + const Eigen::Matrix3d sh_K2 = sh * K * K; + + Eigen::Matrix dR; + for (int i = 0; i < 3; ++i) { + const Eigen::Matrix3d K_dKi = K * dK[i]; + dR.col(i) = (ds(i) * K + s_dK[i] + dsh(i) * sh_K2 + + (0.5 * sh_sq) * (K_dKi + K_dKi.transpose())) + .reshaped(); + } + + return dR; +} + +Eigen::Matrix +rotation_vector_to_matrix_hessian(Eigen::ConstRef theta) +{ + assert(theta.size() == 3); // Only valid for 3D rotation vectors + + const double sh = sinc_norm_x(theta / 2); + const double sh_sq = sh * sh; + const Eigen::Matrix3d K = cross_product_matrix(theta); + + const Eigen::Vector3d ds = sinc_norm_x_grad(theta); + const Eigen::Vector3d dsh = 0.5 * sinc_norm_x_grad(theta / 2); + const Eigen::Matrix3d d2s = sinc_norm_x_hess(theta); + const Eigen::Matrix3d d2sh = 0.25 * sinc_norm_x_hess(theta / 2); + + std::array dK; + dK[0] << 0, 0, 0, 0, 0, -1, 0, 1, 0; + dK[1] << 0, 0, 1, 0, 0, 0, -1, 0, 0; + dK[2] << 0, -1, 0, 1, 0, 0, 0, 0, 0; + + // NOTE: ∂²K/∂θ² = 0 + + const Eigen::Matrix3d K2 = K * K; + + std::array K_dK; + K_dK[0] = K * dK[0]; + K_dK[1] = K * dK[1]; + K_dK[2] = K * dK[2]; + + std::array K_dK_plus_K_dK_T; + K_dK_plus_K_dK_T[0] = K_dK[0] + K_dK[0].transpose(); + K_dK_plus_K_dK_T[1] = K_dK[1] + K_dK[1].transpose(); + K_dK_plus_K_dK_T[2] = K_dK[2] + K_dK[2].transpose(); + + // Flatten in column-major order + Eigen::Matrix d2R; + for (int j = 0; j < 3; ++j) { + const Eigen::Matrix3d K_dKj = K * dK[j]; + for (int i = j; i < 3; ++i) { + const Eigen::Matrix3d dKj_dKi = dK[j] * dK[i]; + d2R.col(j * 3 + i) = + (d2s(i, j) * K + ds(i) * dK[j] + ds(j) * dK[i] + + (d2sh(i, j) * sh + dsh(i) * dsh(j)) * K2 + + (sh * dsh(i)) * K_dK_plus_K_dK_T[j] + + (sh * dsh(j)) * K_dK_plus_K_dK_T[i] + + (0.5 * sh_sq) * (dKj_dKi + dKj_dKi.transpose())) + .reshaped(); + if (i != j) { + d2R.col(i * 3 + j) = d2R.col(j * 3 + i); // Symmetric + } + } + } + return d2R; +} + +Eigen::Vector3d rotation_matrix_to_vector(Eigen::ConstRef R) +{ +#if false // NOLINT + // Eigen does this conversion by going from SO(3) -> Quaternion -> 𝔰𝔬(3), + // but we can do it directly from SO(3) -> 𝔰𝔬(3). In random benchmarking, + // this is about 2x faster than the Eigen implementation. However, this + // might be less accurate for some edge cases as it requires a sin and acos + // where as Eigen's approach uses atan2 and sqrt. + + assert(R.isUnitary(1e-9)); // Ensure it's a rotation matrix + + Eigen::Vector3d r; + // θ ∈ [0, π) + double theta = acos((std::clamp(R.trace(), -1.0, 3.0) - 1) / 2.0); + assert(std::isfinite(theta)); + + // R = I + sin(θ)K + (1 - cos(θ))K² where K is the cross-product matrix of + // r/θ + if (theta <= 1e-5) { + // θ ≈ 0 ⟹ R ≈ I + θK + // No need to divide by sin(θ)≈θ since the off-diagonals are ±rᵢθ + r.x() = (R(2, 1) - R(1, 2)) / 2; + r.y() = (R(0, 2) - R(2, 0)) / 2; + r.z() = (R(1, 0) - R(0, 1)) / 2; + // Θ is already part of the off-diagonal elements, so no need to scale + } else if (theta >= PI - 1e-5) { + // θ ≈ π ⟹ R ≈ I - (θ - π)K + 2K² + // Ensure positive for sqrt + assert(R(0, 0) > -1); + r.x() = sqrt((R(0, 0) + 1) / 2); + // Determine the other components based on the first component + // R₀,₁ = 2r₀r₁+(θ-π)r₂, R₀,₂ = 2r₀r₂-(θ-π)r₁, R₁,₂ = 2r₁r₂+(θ-π)r₀ + r.y() = R(0, 1) / (2 * r.x()); + r.z() = R(0, 2) / (2 * r.x()); + // r.normalize(); // Normalize just in case + r.array() *= theta; // Scale by the angle + } else { + const double sin_theta = sin(theta); + r.x() = (R(2, 1) - R(1, 2)) / (2 * sin_theta); + r.y() = (R(0, 2) - R(2, 0)) / (2 * sin_theta); + r.z() = (R(1, 0) - R(0, 1)) / (2 * sin_theta); + // r.normalize(); // Normalize just in case + r *= theta; // Scale by the angle + } + assert(r.array().isFinite().all()); + +#ifndef NDEBUG + Eigen::AngleAxisd eigen(R); + assert( + (eigen.angle() * eigen.axis()).isApprox(r, 1e-4) + || (theta >= PI - 1e-5 // Eigen can flip the sign of angles close to π + && (-eigen.angle() * eigen.axis()).isApprox(r, 1e-4))); +#endif + + return r; +#else + // Use Eigen's built-in function for conversion + Eigen::AngleAxisd angle_axis(R); + return angle_axis.angle() * angle_axis.axis(); +#endif +} + +MatrixMax +rotation_to_matrix_jacobian(Eigen::ConstRef theta) +{ + assert(theta.size() == 1 || theta.size() == 3); + if (theta.size() == 1) { + // 2D rotation matrix R(θ) = [cos -sin; sin cos] (column-major vec): + // d vec(R)/dθ = [-sin, cos, -cos, -sin]ᵀ + MatrixMax dR(4, 1); + const double dcos = -std::sin(theta(0)); + const double dsin = std::cos(theta(0)); + dR(0, 0) = dcos; + dR(1, 0) = dsin; + dR(2, 0) = -dsin; + dR(3, 0) = dcos; + return dR; + } else { + return rotation_vector_to_matrix_jacobian(theta); + } +} + +MatrixMax +rotation_to_matrix_hessian(Eigen::ConstRef theta) +{ + assert(theta.size() == 1 || theta.size() == 3); + if (theta.size() == 1) { + // 2D: d² vec(R)/dθ² = -vec(R) = [-cos, -sin, sin, -cos]ᵀ + MatrixMax d2R(4, 1); + const double d2cos = -std::cos(theta(0)); + const double d2sin = -std::sin(theta(0)); + d2R(0, 0) = d2cos; + d2R(1, 0) = d2sin; + d2R(2, 0) = -d2sin; + d2R(3, 0) = d2cos; + return d2R; + } else { + return rotation_vector_to_matrix_hessian(theta); + } +} + +Eigen::MatrixXd +Pose::transform_vertices_jacobian(Eigen::ConstRef V) const +{ + const int dim = V.cols(); + assert(dim == position.size()); + + Eigen::MatrixXd jac = Eigen::MatrixXd::Zero(V.size(), ndof()); + + // Derivative w.r.t. position is identity + for (int i = 0; i < V.rows(); ++i) { + jac.block(i * dim, 0, dim, dim).setIdentity(); + } + + // Precompute dR/dθ + const MatrixMax dR_dtheta = + rotation_to_matrix_jacobian(rotation); + + // Derivative w.r.t. rotation + for (int j = 0; j < rotation.size(); ++j) { + jac.block(0, dim + j, V.size(), 1) = + (V * dR_dtheta.col(j).reshaped(dim, dim).transpose()) + .reshaped(); + } + + return jac; +} + +Eigen::MatrixXd +Pose::transform_vertices_hessian(Eigen::ConstRef V) const +{ + const int dim = V.cols(); + const int ndof = position.size() + rotation.size(); + assert(dim == position.size()); + + Eigen::MatrixXd hess = Eigen::MatrixXd::Zero(V.size(), ndof * ndof); + + // Derivative of position Jacobian is zero, so only need to compute + // derivatives involving rotation. + + // Precompute d^2R/dθ^2 + const MatrixMax d2R_dtheta2 = + rotation_to_matrix_hessian(rotation); + + // Derivative w.r.t. rotation + for (int j = position.size(); j < ndof; ++j) { + for (int i = position.size(); i < ndof; ++i) { + const int k = (j - dim) * rotation.size() + (i - dim); + hess.col(j * ndof + i) = + (V * d2R_dtheta2.col(k).reshaped(dim, dim).transpose()) + .reshaped(); + } + } + + return hess; +} + +MatrixMax +Pose::transform_vertex_jacobian(Eigen::ConstRef v) const +{ + const int dim = v.size(); + assert(dim == position.size()); + + MatrixMax jac(dim, ndof()); + + // Derivative w.r.t. position is identity + jac.leftCols(dim).setIdentity(); + + // Derivative w.r.t. rotation: d(R v)/dθⱼ = (dR/dθⱼ) v + const MatrixMax dR_dtheta = + rotation_to_matrix_jacobian(rotation); + for (int j = 0; j < rotation.size(); ++j) { + jac.col(dim + j) = dR_dtheta.col(j).reshaped(dim, dim) * v; + } + + return jac; +} + +MatrixMax +Pose::transform_vertex_hessian(Eigen::ConstRef v) const +{ + const int dim = v.size(); + const int ndof = this->ndof(); + assert(dim == position.size()); + + // The position Jacobian is constant, so only the rotation-rotation + // columns are nonzero. + MatrixMax hess = + MatrixMax::Zero(dim, ndof * ndof); + + const MatrixMax d2R_dtheta2 = + rotation_to_matrix_hessian(rotation); + for (int j = dim; j < ndof; ++j) { + for (int i = dim; i < ndof; ++i) { + const int k = (j - dim) * rotation.size() + (i - dim); + hess.col(j * ndof + i) = d2R_dtheta2.col(k).reshaped(dim, dim) * v; + } + } + + return hess; +} + +} // namespace ipc::rigid \ No newline at end of file diff --git a/src/ipc/dynamics/rigid/pose.hpp b/src/ipc/dynamics/rigid/pose.hpp new file mode 100644 index 000000000..d8b77e05a --- /dev/null +++ b/src/ipc/dynamics/rigid/pose.hpp @@ -0,0 +1,292 @@ +#pragma once + +#include +#include + +#include + +namespace ipc::rigid { + +/// @brief Convert a 2D rotation angle to a rotation matrix. +/// @param theta The rotation angle (counterclockwise, in radians) +/// @return The rotation matrix [[cos θ, −sin θ], [sin θ, cos θ]] +inline Eigen::Matrix2d rotation_angle_to_matrix(const double theta) +{ + Eigen::Matrix2d R; + R << std::cos(theta), -std::sin(theta), std::sin(theta), std::cos(theta); + return R; +} + +/// @brief Convert from a 3D rotation vector to a rotation matrix. +/// @param theta The rotation vector +/// @return The rotation matrix corresponding to the rotation vector +Eigen::Matrix3d +rotation_vector_to_matrix(Eigen::ConstRef theta); + +/// @brief Compute the Jacobian of the rotation matrix with respect to the rotation vector. +/// @param theta The rotation vector +/// @return The Jacobian matrix +Eigen::Matrix +rotation_vector_to_matrix_jacobian(Eigen::ConstRef theta); + +/// @brief Compute the Hessian of the rotation matrix with respect to the rotation vector. +/// +/// This is a 9x9 matrix where each row corresponds to the second derivative of +/// each element of the rotation matrix with respect to each component of the +/// rotation vector. +/// +/// @param theta The rotation vector +/// @return The Hessian matrix +Eigen::Matrix +rotation_vector_to_matrix_hessian(Eigen::ConstRef theta); + +/// @brief Convert from a 3D rotation matrix to a rotation vector. +/// @param R The rotation matrix +/// @return The rotation vector corresponding to the rotation matrix +Eigen::Vector3d rotation_matrix_to_vector(Eigen::ConstRef R); + +/// @brief Jacobian of the rotation matrix w.r.t. the rotation parameters, +/// dispatching on the dimension. +/// +/// For a rotation parameterized by @p theta (a scalar angle in 2D or a rotation +/// vector in 3D), this returns d vec(R)/d theta as a (dim²)×rot_ndof matrix: +/// 2D → 4×1 (so(2) exp map, single generator), 3D → 9×3 (Rodrigues). +/// +/// @param theta The rotation parameters (size 1 in 2D, size 3 in 3D) +/// @return The Jacobian, sized 4×1 (2D) or 9×3 (3D) +MatrixMax +rotation_to_matrix_jacobian(Eigen::ConstRef theta); + +/// @brief Hessian of the rotation matrix w.r.t. the rotation parameters, +/// dispatching on the dimension. +/// +/// Returns d² vec(R)/d theta² flattened as a (dim²)×(rot_ndof²) matrix: +/// 2D → 4×1 (d²R/dθ² = −R), 3D → 9×9. +/// +/// @param theta The rotation parameters (size 1 in 2D, size 3 in 3D) +/// @return The Hessian, sized 4×1 (2D) or 9×9 (3D) +MatrixMax +rotation_to_matrix_hessian(Eigen::ConstRef theta); + +// ---------------------------------------------------------------------------- + +struct Pose { + /// @brief Position of the rigid body + VectorMax3d position; + /// @brief Rotation of the rigid body (rotation vector for 3D, angle for 1D) + VectorMax3d rotation; + + /// @brief Default constructor. + Pose() = default; + + /// @brief Construct a pose from position and rotation vectors. + /// @param _position The position vector + /// @param _rotation The rotation vector + Pose( + Eigen::ConstRef _position, + Eigen::ConstRef _rotation) + : position(_position) + , rotation(_rotation) + { + } + + /// @brief Construct a pose from a concatenated vector. + /// @param x The concatenated position and rotation vector + explicit Pose(Eigen::ConstRef x) + { + assert(x.size() == 6 || x.size() == 3); + if (x.size() == 3) { + position = x.head<2>(); + rotation = x.tail<1>(); + } else { + position = x.head<3>(); + rotation = x.tail<3>(); + } + } + + /// @brief Construct an identity pose with zero position and rotation. + /// @param dim The dimension of the pose (2 or 3) + /// @return Identity pose with zero position and rotation + static Pose identity(const int dim) + { + assert(dim == 2 || dim == 3); + // Rotation DOF: 2D uses a single angle, 3D uses a rotation vector. + const int rot_ndof = dim == 2 ? 1 : 3; + return Pose(VectorMax3d::Zero(dim), VectorMax3d::Zero(rot_ndof)); + } + + int ndof() const { return position.size() + rotation.size(); } + + /// @brief Construct a rotation matrix from the rotation vector. + /// @return The rotation matrix corresponding to the rotation vector + MatrixMax3d rotation_matrix() const + { + assert(rotation.size() == 1 || rotation.size() == 3); + if (rotation.size() == 1) { + return rotation_angle_to_matrix(rotation(0)); + } else { + // For 3D, convert the rotation vector to a rotation matrix + return rotation_vector_to_matrix(rotation); + } + } + + /// @brief Construct a quaternion from the rotation vector. + /// @return The quaternion corresponding to the rotation vector + Eigen::Quaternion quaternion() const + { + assert(rotation.size() == 3); + double angle = rotation.norm(); + if (angle == 0.0) { + return Eigen::Quaternion::Identity(); + } + Eigen::Vector3d axis = rotation / angle; + return Eigen::Quaternion(Eigen::AngleAxis(angle, axis)); + } + + /// @brief Clamp the rotation angle to the range [-π, π] to prevent numerical issues. + /// This is important for optimization to ensure that the rotation does not + /// grow unbounded and cause numerical instability. + void clamp_rotation_to_pi() + { + double angle = rotation.norm(); + + // 1. Small angle approximation / Zero check + if (angle < std::numeric_limits::epsilon()) { + return; + } + + // 2. Extract unit axis + VectorMax3d axis = rotation / angle; + + // 3. Wrap angle to [-π, π] using remainder + angle = std::remainder(angle, 2.0 * PI); + + // 4. Canonicalize π: Ensure -π < angle <= π + // This prevents the representation from jumping between π and -π + if (angle <= -PI) { + angle = PI; + axis = -axis; // Flip axis to keep angle positive + } + + rotation = angle * axis; + } + + /// @brief Transform vertices from local to world coordinates using the pose. + /// @param V The vertices to transform (each row is a vertex) + /// @return The transformed vertices + Eigen::MatrixXd transform_vertices(Eigen::ConstRef V) const + { + // Compute: R(θ) V + p + // transpose because x is row-ordered + return (V * rotation_matrix().transpose()).rowwise() + + position.transpose(); + } + + /// @brief Compute the Jacobian of the transformed vertices with respect to the pose. + /// @param V The vertices to transform (each row is a vertex) + /// @return The Jacobian matrix of size (num_vertices * dim) x ndof + Eigen::MatrixXd + transform_vertices_jacobian(Eigen::ConstRef V) const; + + /// @brief Compute the Hessian of the transformed vertices with respect to the pose. + /// @param V The vertices to transform (each row is a vertex) + /// @return The Hessian matrix of size (num_vertices * dim) x (ndof * ndof) + Eigen::MatrixXd + transform_vertices_hessian(Eigen::ConstRef V) const; + + /// @brief Compute the Jacobian of a single transformed vertex with respect to the pose. + /// + /// This is the fixed-size counterpart of transform_vertices_jacobian for + /// per-contact code: the result lives on the stack, so we avoid a heap + /// allocation for every stencil vertex. + /// + /// @param v The vertex to transform (in body coordinates) + /// @return The Jacobian matrix of size dim x ndof + MatrixMax + transform_vertex_jacobian(Eigen::ConstRef v) const; + + /// @brief Compute the Hessian of a single transformed vertex with respect to the pose. + /// + /// This is the fixed-size counterpart of transform_vertices_hessian. Column + /// j * ndof + i holds the second derivative with respect to DOFs i and j. + /// + /// @param v The vertex to transform (in body coordinates) + /// @return The Hessian matrix of size dim x (ndof * ndof) + MatrixMax + transform_vertex_hessian(Eigen::ConstRef v) const; + + /// @brief Compute the inverse of the pose. + /// @return The inverse pose + Pose inverse() const + { + Pose inv; + const MatrixMax3d R_inv = rotation_matrix().transpose(); + inv.position = -(R_inv * position); + if (position.size() == 2) { + // Negate angle directly + inv.rotation = -rotation; + } else { + // Convert inverse rotation to a vector + inv.rotation = rotation_matrix_to_vector(R_inv); + } + return inv; + } + + /// @brief Combine two poses. + /// @param a The first pose + /// @param b The second pose + /// @return The combined pose + friend Pose operator*(const Pose& a, const Pose& b) + { + Pose c; + + const MatrixMax3d Ra = a.rotation_matrix(); + + c.position = Ra * b.position + a.position; + + if (c.position.size() == 2) { + // Combine angles directly + c.rotation = a.rotation + b.rotation; + } else { + // Combine rotation matrices multiplicatively + c.rotation = rotation_matrix_to_vector(Ra * b.rotation_matrix()); + } + + return c; + } + + /// @brief Check if two poses are equal. + /// @param a The first pose + /// @param b The second pose + /// @return True if the poses are equal, false otherwise + friend bool operator==(const Pose& a, const Pose& b) + { + return a.position == b.position && a.rotation == b.rotation; + } + + static std::vector + to_poses(Eigen::ConstRef x, const int dim) + { + const int pose_ndof = dim == 2 ? 3 : 6; + assert(x.size() % pose_ndof == 0); + std::vector poses(x.size() / pose_ndof); + for (size_t i = 0; i < poses.size(); ++i) { + poses[i] = Pose(x.segment(i * pose_ndof, pose_ndof)); + } + return poses; + } + + static Eigen::VectorXd from_poses(const std::vector& poses) + { + const int pose_ndof = poses[0].ndof(); + Eigen::VectorXd x(poses.size() * pose_ndof); + for (size_t i = 0; i < poses.size(); ++i) { + assert(poses[i].ndof() == pose_ndof); + x.segment(i * pose_ndof, pose_ndof) << poses[i].position, + poses[i].rotation; + } + return x; + } +}; + +} // namespace ipc::rigid \ No newline at end of file diff --git a/src/ipc/dynamics/rigid/rigid_bodies.cpp b/src/ipc/dynamics/rigid/rigid_bodies.cpp new file mode 100644 index 000000000..ec967350b --- /dev/null +++ b/src/ipc/dynamics/rigid/rigid_bodies.cpp @@ -0,0 +1,438 @@ +#include "rigid_bodies.hpp" + +#include +#include +#include +#include + +#include +#include + +#include + +namespace ipc::rigid { + +Eigen::MatrixXd +RigidBodies::vertices(const std::vector& poses) const +{ + assert(poses.size() == num_bodies()); + Eigen::MatrixXd V(num_vertices(), dim()); + for (size_t i = 0; i < num_bodies(); ++i) { + const index_t start = body_vertex_starts[i]; + const index_t end = body_vertex_starts[i + 1]; + V.middleRows(start, end - start) = poses[i].transform_vertices( + rest_positions().middleRows(body_vertex_starts[i], end - start)); + } + return V; +} + +std::shared_ptr RigidBodies::build_from_meshes( + const std::vector& rest_positions, + const std::vector& edges, + const std::vector& faces, + const std::vector& densities, + std::vector& initial_poses, + const bool convert_planes, + const std::vector& is_dof_fixed, + const std::vector& names, + const std::vector>& center_of_mass) +{ + assert(rest_positions.size() == edges.size()); + assert(rest_positions.size() == faces.size()); + assert(rest_positions.size() == densities.size()); + assert( + is_dof_fixed.empty() || is_dof_fixed.size() == rest_positions.size()); + assert(names.empty() || names.size() == rest_positions.size()); + assert( + center_of_mass.empty() + || center_of_mass.size() == rest_positions.size()); + + if (rest_positions.empty()) { + return nullptr; + } + + unordered_set plane_bodies; + std::vector> planes; + if (convert_planes && rest_positions[0].cols() == 3) { + for (size_t i = 0; i < faces.size(); ++i) { + // Must have exactly 2 faces and 4 vertices to be considered a plane + // body. This is a heuristic to identify bodies that are essentially + // flat and can be treated as planes for collision purposes. + if (faces[i].rows() != 2 || rest_positions[i].rows() != 4 + || edges[i].rows() != 5) { + continue; + } + + // Check if the two faces form a square: + std::array edge_lengths; + for (size_t j = 0; j < edges[i].rows(); ++j) { + const auto& e = edges[i].row(j); + edge_lengths[j] = + (rest_positions[i].row(e(0)) - rest_positions[i].row(e(1))) + .norm(); + } + std::sort(edge_lengths.begin(), edge_lengths.end()); + if (std::abs(edge_lengths[0] - edge_lengths[3]) > 1e-6) { + // Not all edges are the same length, so not a square + continue; + } + + const Eigen::Vector3d n0 = triangle_normal( + rest_positions[i].row(faces[i](0, 0)), + rest_positions[i].row(faces[i](0, 1)), + rest_positions[i].row(faces[i](0, 2))); + const Eigen::Vector3d n1 = triangle_normal( + rest_positions[i].row(faces[i](1, 0)), + rest_positions[i].row(faces[i](1, 1)), + rest_positions[i].row(faces[i](1, 2))); + if (std::abs(n0.dot(n1)) > 1.0 - 1e-6) { + plane_bodies.insert(i); + Eigen::Matrix3d R = initial_poses[i].rotation_matrix(); + planes.emplace_back( + R * n0, + R * rest_positions[i].colwise().mean().transpose() + + initial_poses[i].position); + } + } + } + const int num_bodies = rest_positions.size() - plane_bodies.size(); + + size_t num_vertices = 0, num_edges = 0, num_faces = 0; + std::vector body_vertex_starts(num_bodies + 1); + std::vector body_edge_starts(num_bodies + 1); + std::vector body_face_starts(num_bodies + 1); + body_vertex_starts[0] = body_edge_starts[0] = body_face_starts[0] = 0; + + for (size_t i = 0, j = 0; i < rest_positions.size(); ++i) { + if (plane_bodies.count(i) > 0) { + logger().info("Body {} is identified as a plane body", i); + continue; + } + body_vertex_starts[j + 1] = (num_vertices += rest_positions[i].rows()); + body_edge_starts[j + 1] = (num_edges += edges[i].rows()); + body_face_starts[j + 1] = (num_faces += faces[i].rows()); + ++j; + } + + Eigen::MatrixXd concat_rest_positions( + num_vertices, rest_positions[0].cols()); + Eigen::MatrixXi concat_edges(num_edges, 2); + Eigen::MatrixXi concat_faces(num_faces, 3); + + for (size_t i = 0, j = 0; i < rest_positions.size(); ++i) { + assert(rest_positions[i].size() > 0); + if (plane_bodies.count(i) > 0) { + continue; + } + concat_rest_positions.middleRows( + body_vertex_starts[j], rest_positions[i].rows()) = + rest_positions[i]; + if (edges[i].size() > 0) { + concat_edges.middleRows(body_edge_starts[j], edges[i].rows()) = + edges[i].array() + body_vertex_starts[j]; + } + if (faces[i].size() > 0) { + concat_faces.middleRows(body_face_starts[j], faces[i].rows()) = + faces[i].array() + body_vertex_starts[j]; + } + ++j; + } + + { + size_t idx = 0; + auto new_end = std::remove_if( + initial_poses.begin(), initial_poses.end(), + [&](const Pose&) { return plane_bodies.count(idx++) > 0; }); + initial_poses.erase(new_end, initial_poses.end()); + } + + // The constructor indexes the per-body inputs by body, so we drop the + // plane entries to keep them aligned with the remaining meshes. + std::vector nonplane_densities; + nonplane_densities.reserve(num_bodies); + for (size_t i = 0; i < densities.size(); ++i) { + if (plane_bodies.count(i) == 0) { + nonplane_densities.push_back(densities[i]); + } + } + + std::vector nonplane_is_dof_fixed; + if (!is_dof_fixed.empty()) { + for (size_t i = 0; i < is_dof_fixed.size(); ++i) { + if (plane_bodies.count(i) == 0) { + nonplane_is_dof_fixed.push_back(is_dof_fixed[i]); + } + } + } + + std::vector> nonplane_center_of_mass; + if (!center_of_mass.empty()) { + for (size_t i = 0; i < center_of_mass.size(); ++i) { + if (plane_bodies.count(i) == 0) { + nonplane_center_of_mass.push_back(center_of_mass[i]); + } + } + } + + std::shared_ptr bodies = std::make_shared( + concat_rest_positions, concat_edges, concat_faces, body_vertex_starts, + body_edge_starts, body_face_starts, nonplane_densities, initial_poses, + nonplane_is_dof_fixed, nonplane_center_of_mass); + + bodies->planes = std::move(planes); + + // Override the default names with the caller-provided names (pruning plane + // bodies to stay aligned with the built bodies). + if (!names.empty()) { + std::vector nonplane_names; + nonplane_names.reserve(num_bodies); + for (size_t i = 0; i < names.size(); ++i) { + if (plane_bodies.count(i) == 0) { + nonplane_names.push_back(names[i]); + } + } + bodies->assign_body_names(nonplane_names); + } + + return bodies; +} + +RigidBodies::RigidBodies( + Eigen::ConstRef _rest_positions, + Eigen::ConstRef _edges, + Eigen::ConstRef _faces, + const std::vector& _body_vertex_starts, + const std::vector& _body_edge_starts, + const std::vector& _body_face_starts, + const std::vector& densities, + std::vector& initial_poses, + const std::vector& is_dof_fixed, + const std::vector>& center_of_mass) + : CollisionMesh(_rest_positions, _edges, _faces) + , body_vertex_starts(_body_vertex_starts) + , body_edge_starts(_body_edge_starts) + , body_face_starts(_body_face_starts) +{ + assert(body_vertex_starts.size() == body_edge_starts.size()); + assert(body_edge_starts.size() == body_face_starts.size()); + assert(body_vertex_starts.back() == num_vertices()); + assert(body_edge_starts.back() == num_edges()); + assert(body_face_starts.back() == num_faces()); + assert(initial_poses.size() == body_vertex_starts.size() - 1); + assert( + is_dof_fixed.empty() + || is_dof_fixed.size() == body_vertex_starts.size() - 1); + assert( + center_of_mass.empty() + || center_of_mass.size() == body_vertex_starts.size() - 1); + + bodies.reserve(body_vertex_starts.size() - 1); + for (size_t i = 0; i < body_vertex_starts.size() - 1; ++i) { + bodies.emplace_back( + m_rest_positions.middleRows( + body_vertex_starts[i], + body_vertex_starts[i + 1] - body_vertex_starts[i]), + edges().middleRows( + body_edge_starts[i], + body_edge_starts[i + 1] - body_edge_starts[i]) + .array() + - body_vertex_starts[i], + faces().middleRows( + body_face_starts[i], + body_face_starts[i + 1] - body_face_starts[i]) + .array() + - body_vertex_starts[i], + densities[i], initial_poses[i], + is_dof_fixed.empty() ? VectorMax6b() : is_dof_fixed[i], + center_of_mass.empty() ? std::nullopt : center_of_mass[i]); + logger().info( + "Initial pose: position={}, rotation={}", + initial_poses[i].position.transpose(), + initial_poses[i].rotation.transpose()); + } + + // Per-body codimensional element counts (the CollisionMesh base infers + // codim vertices/edges from the concatenated connectivity). + m_body_num_codim_vertices.assign(num_bodies(), 0); + m_body_num_codim_edges.assign(num_bodies(), 0); + for (size_t i = 0; i < num_bodies(); ++i) { + for (index_t v = body_vertex_starts[i]; v < body_vertex_starts[i + 1]; + ++v) { + m_body_num_codim_vertices[i] += is_codim_vertex(v); + } + for (index_t e = body_edge_starts[i]; e < body_edge_starts[i + 1]; + ++e) { + m_body_num_codim_edges[i] += is_codim_edge(e); + } + } + + assert(body_vertex_starts.size() > 0); + // The filter copies the body offsets instead of capturing `this`, so it + // stays valid when the collision mesh is copied or moved. + this->can_collide = [starts = body_vertex_starts](size_t vi, size_t vj) { + return body_containing_vertex(starts, index_t(vi)) + != body_containing_vertex(starts, index_t(vj)); + }; + assert(!this->can_collide(0, 0)); + + // Default names ("Body{i}"); build_from_meshes overrides these when the + // caller supplies names. + assign_body_names({}); +} + +std::vector +RigidBodies::make_unique_names(const std::vector& names) +{ + std::vector unique_names; + unique_names.reserve(names.size()); + std::unordered_set taken; + + for (size_t i = 0; i < names.size(); ++i) { + std::string name = + names[i].empty() ? ("Body" + std::to_string(i)) : names[i]; + + // Disambiguate duplicates so every name maps to a unique body. + if (taken.count(name) > 0) { + std::string unique = name + "." + std::to_string(i); + while (taken.count(unique) > 0) { + unique += "_"; + } + name = std::move(unique); + } + + taken.insert(name); + unique_names.push_back(std::move(name)); + } + return unique_names; +} + +void RigidBodies::assign_body_names(const std::vector& names) +{ + std::vector padded = names; + padded.resize(num_bodies()); // missing names become "Body{i}" + m_body_names = make_unique_names(padded); + + m_name_to_index.clear(); + m_name_to_index.reserve(num_bodies()); + for (size_t i = 0; i < num_bodies(); ++i) { + m_name_to_index.emplace(m_body_names[i], i); + } +} + +Eigen::VectorXd RigidBodies::to_rigid_dof( + const std::vector& poses, Eigen::ConstRef g) const +{ + assert(poses.size() == num_bodies()); + assert(g.size() == ndof()); + + const int ndof_per_body = poses[0].ndof(); + Eigen::VectorXd result(poses.size() * ndof_per_body); + + // Apply the chain rule to map the gradient from the collision mesh to the + // rigid degrees of freedom. + for (size_t i = 0; i < bodies.size(); ++i) { + assert(poses[i].ndof() == ndof_per_body); + const index_t start = body_vertex_starts[i]; + const index_t end = body_vertex_starts[i + 1]; + const index_t nV = end - start; + + result.segment(i * ndof_per_body, ndof_per_body) = + poses[i] + .transform_vertices_jacobian( + rest_positions().middleRows(start, end - start)) + .transpose() + * g.segment(dim() * start, dim() * nV); + } + + return result; +} + +Eigen::SparseMatrix RigidBodies::to_rigid_dof( + const std::vector& poses, + Eigen::ConstRef g, + const Eigen::SparseMatrix& H) const +{ + assert(poses.size() == num_bodies()); + assert(g.size() == ndof()); + assert(H.rows() == ndof() && H.cols() == ndof()); + + const int ndof_per_body = poses[0].ndof(); + const int n = poses.size() * ndof_per_body; + + // 1. Find the body pairs coupled in H by scanning its nonzero structure. + // Diagonal blocks are always needed for the second-order term. + std::set> coupled; + for (index_t i = 0; i < index_t(num_bodies()); ++i) { + coupled.emplace(i, i); + } + for (int k = 0; k < H.outerSize(); ++k) { + for (Eigen::SparseMatrix::InnerIterator it(H, k); it; ++it) { + index_t bi = vertex_to_body(it.row() / dim()); + index_t bj = vertex_to_body(it.col() / dim()); + if (bi > bj) { + std::swap(bi, bj); + } + coupled.emplace(bi, bj); + } + } + + // 2. Precompute the per-body Jacobians (and Hessians for the diagonal) + std::vector dV_dx(bodies.size()); + for (size_t i = 0; i < bodies.size(); ++i) { + assert(poses[i].ndof() == ndof_per_body); + dV_dx[i] = poses[i].transform_vertices_jacobian(body_rest_positions(i)); + } + + // 3. Compute the coupled blocks and assemble the sparse result + std::vector> triplets; + triplets.reserve(coupled.size() * ndof_per_body * ndof_per_body * 2); + + const auto emplace_block = [&](const index_t bi, const index_t bj, + Eigen::ConstRef block) { + for (int c = 0; c < ndof_per_body; ++c) { + for (int r = 0; r < ndof_per_body; ++r) { + if (block(r, c) != 0) { + triplets.emplace_back( + bi * ndof_per_body + r, bj * ndof_per_body + c, + block(r, c)); + } + } + } + }; + + for (const auto& [bi, bj] : coupled) { + // Keep the Hessian block sparse to avoid densifying large blocks + const Eigen::SparseMatrix H_ij = H.block( + dim() * body_vertex_starts[bi], dim() * body_vertex_starts[bj], + dim() * body_num_vertices(bi), dim() * body_num_vertices(bj)); + + MatrixMax6d block = dV_dx[bi].transpose() * (H_ij * dV_dx[bj]); + + if (bi == bj) { + const auto gi = g.segment( + dim() * body_vertex_starts[bi], dim() * body_num_vertices(bi)); + if (!gi.isZero()) { + // Second-order term of the transformation (diagonal blocks) + const Eigen::MatrixXd d2V_dx2 = + poses[bi].transform_vertices_hessian( + body_rest_positions(bi)); + for (int jj = 0; jj < ndof_per_body; ++jj) { + for (int ii = 0; ii < ndof_per_body; ++ii) { + block(ii, jj) += + gi.dot(d2V_dx2.col(ii * ndof_per_body + jj)); + } + } + } + emplace_block(bi, bi, block); + } else { + emplace_block(bi, bj, block); + emplace_block(bj, bi, block.transpose()); + } + } + + Eigen::SparseMatrix result(n, n); + result.setFromTriplets(triplets.begin(), triplets.end()); + return result; +} + +} // namespace ipc::rigid \ No newline at end of file diff --git a/src/ipc/dynamics/rigid/rigid_bodies.hpp b/src/ipc/dynamics/rigid/rigid_bodies.hpp new file mode 100644 index 000000000..fd07ec5b6 --- /dev/null +++ b/src/ipc/dynamics/rigid/rigid_bodies.hpp @@ -0,0 +1,315 @@ +#pragma once + +#include + +#include +#include +#include +#include +#include + +namespace ipc::affine { +struct Pose; +} // namespace ipc::affine + +namespace ipc::rigid { + +class RigidBodies : public CollisionMesh { + RigidBodies() = delete; + +public: + /// @brief Construct a RigidBodies object from the given mesh data and rigid body information. + /// @param rest_positions A matrix of vertex positions for all rigid bodies concatenated together. + /// @param edges A matrix of edge indices for all rigid bodies concatenated together. + /// @param faces A matrix of face indices for all rigid bodies concatenated together. + /// @param body_vertex_starts A vector of start indices for the vertices of each rigid body in the concatenated rest_positions matrix. + /// @param body_edge_starts A vector of start indices for the edges of each rigid body in the concatenated edges matrix. + /// @param body_face_starts A vector of start indices for the faces of each rigid body in the concatenated faces matrix. + /// @param densities A vector of densities for each rigid body. + /// @param initial_poses A vector of initial poses for each rigid body. + /// @param is_dof_fixed Optional per-body fixed-DOF flags (empty means all free). + /// @param center_of_mass Optional per-body prescribed centers of mass, in each body's rest frame (empty, or an empty entry, means computed from the mesh; see RigidBody). + RigidBodies( + Eigen::ConstRef rest_positions, + Eigen::ConstRef edges, + Eigen::ConstRef faces, + const std::vector& body_vertex_starts, + const std::vector& body_edge_starts, + const std::vector& body_face_starts, + const std::vector& densities, + std::vector& initial_poses, + const std::vector& is_dof_fixed = {}, + const std::vector>& center_of_mass = {}); + + /// @brief Build a RigidBodies object from a set of meshes, where each mesh corresponds to a rigid body. + /// @param rest_positions A vector of vertex positions for each rigid body mesh. + /// @param edges A vector of edge indices for each rigid body mesh. + /// @param faces A vector of face indices for each rigid body mesh. + /// @param densities A vector of densities for each rigid body mesh. + /// @param initial_poses A vector of initial poses for each rigid body mesh. + /// @param convert_planes If true, any mesh with exactly two coplanar faces will be converted to a plane. + /// @param is_dof_fixed Optional per-body fixed-DOF flags (empty means all free). + /// @param names Optional per-body names (empty means auto-generated). Plane + /// bodies are pruned from the names alongside the geometry, and any + /// empty or duplicate name is replaced with a unique default. + /// @param center_of_mass Optional per-body prescribed centers of mass, in each mesh's rest frame (empty, or an empty entry, means computed from the mesh; see RigidBody). + /// @return A shared pointer to the constructed RigidBodies object + static std::shared_ptr build_from_meshes( + const std::vector& rest_positions, + const std::vector& edges, + const std::vector& faces, + const std::vector& densities, + std::vector& initial_poses, + const bool convert_planes = false, + const std::vector& is_dof_fixed = {}, + const std::vector& names = {}, + const std::vector>& center_of_mass = {}); + + /// @brief Get the vertices of the collision mesh given the poses of each rigid body. + /// @param poses A vector of poses for each rigid body. + /// @return A matrix of vertex positions for the collision mesh. + Eigen::MatrixXd vertices(const std::vector& poses) const + { + assert(poses.size() == num_bodies()); + Eigen::MatrixXd V(num_vertices(), dim()); + for (size_t i = 0; i < num_bodies(); ++i) { + const index_t start = body_vertex_starts[i]; + const index_t end = body_vertex_starts[i + 1]; + V.middleRows(start, end - start) = poses[i].transform_vertices( + rest_positions().middleRows( + body_vertex_starts[i], end - start)); + } + return V; + } + + /// @brief Get the collision-mesh vertices given the affine pose of each + /// body (world = A x̄ + p), preserving any affine deformation. + /// @note For affine bodies use this rather than projecting to a rigid pose, + /// which would discard the stretch/shear in A. + /// @param poses A vector of affine poses for each body. + /// @return A matrix of vertex positions for the collision mesh. + Eigen::MatrixXd vertices(const std::vector& poses) const; + + /// @brief Get the vertices of the collision mesh given the full positions or poses. + /// @param full_positions_or_poses A matrix of full positions (size: num_vertices() × dim()) or a matrix of full poses (size: num_bodies() × 1). + /// @return A matrix of vertex positions for the collision mesh. + Eigen::MatrixXd vertices( + Eigen::ConstRef full_positions_or_poses) const override + { + if (full_positions_or_poses.cols() == dim()) { + assert(full_positions_or_poses.rows() == full_num_vertices()); + return CollisionMesh::vertices(full_positions_or_poses); + } else { + assert(full_positions_or_poses.cols() == 1); + return vertices(Pose::to_poses(full_positions_or_poses, dim())); + } + } + + /// @brief Get the rigid body at index i. + /// @param i Index of the rigid body. + /// @return Reference to the rigid body at index i. + RigidBody& operator[](size_t i) { return bodies[i]; } + + /// @brief Get the rigid body at index i. + /// @param i Index of the rigid body. + /// @return Const reference to the rigid body at index i. + const RigidBody& operator[](size_t i) const { return bodies[i]; } + + /// @brief Get the number of rigid bodies in the system. + /// @return Number of rigid bodies. + size_t num_bodies() const { return bodies.size(); } + + /// @brief Get the name of the i-th rigid body. + /// @note Names are unique; unnamed bodies default to "Body{i}". + const std::string& body_name(size_t i) const { return m_body_names[i]; } + + /// @brief Look up a rigid body index by name. + /// @param name The body name to look up. + /// @return The body index, or std::nullopt if no body has that name. + std::optional body_index(const std::string& name) const + { + const auto it = m_name_to_index.find(name); + if (it == m_name_to_index.end()) { + return std::nullopt; + } + return it->second; + } + + /// @brief Number of codimensional (in no edge) vertices of body i. + size_t body_num_codim_vertices(size_t i) const + { + return m_body_num_codim_vertices[i]; + } + + /// @brief Number of codimensional (in no face) edges of body i. + size_t body_num_codim_edges(size_t i) const + { + return m_body_num_codim_edges[i]; + } + + /// @brief Count the bodies of the given type. + size_t count_type(const RigidBody::Type type) const + { + return std::count_if( + bodies.begin(), bodies.end(), + [type](const RigidBody& body) { return body.type() == type; }); + } + + /// @brief Get the number of vertices in the i-th rigid body mesh. + /// @param i Index of the rigid body mesh. + /// @return Number of vertices in the i-th rigid body mesh. + size_t body_num_vertices(size_t i) const + { + return body_vertex_starts[i + 1] - body_vertex_starts[i]; + } + + /// @brief Get the number of edges in the i-th rigid body mesh. + /// @param i Index of the rigid body mesh. + /// @return Number of edges in the i-th rigid body mesh. + size_t body_num_edges(size_t i) const + { + return body_edge_starts[i + 1] - body_edge_starts[i]; + } + + /// @brief Get the number of faces in the i-th rigid body mesh. + /// @param i Index of the rigid body mesh. + /// @return Number of faces in the i-th rigid body mesh. + size_t body_num_faces(size_t i) const + { + return body_face_starts[i + 1] - body_face_starts[i]; + } + + /// @brief Get the vertices of the i-th rigid body mesh. + /// @param i Index of the rigid body mesh. + /// @return Vertices of the i-th rigid body mesh. + auto body_rest_positions(size_t i) const + { + return rest_positions().middleRows( + body_vertex_starts[i], body_num_vertices(i)); + } + + /// @brief Get the vertices of the i-th rigid body mesh. + /// @param i Index of the rigid body mesh. + /// @param pose Pose of the i-th rigid body. + /// @return Vertices of the i-th rigid body mesh. + Eigen::MatrixXd body_vertices(size_t i, const Pose& pose) const + { + return pose.transform_vertices(body_rest_positions(i)); + } + + /// @brief Get the edges of the i-th rigid body mesh. + /// @note Returns indices in the local body vertex indexing. + /// @param i Index of the rigid body mesh. + /// @return Edges of the i-th rigid body mesh. + Eigen::MatrixXi body_edges(size_t i) const + { + return edges() + .middleRows(body_edge_starts[i], body_num_edges(i)) + .array() + - body_vertex_starts[i]; + } + + /// @brief Get the faces of the i-th rigid body mesh. + /// @note Returns indices in the local body vertex indexing. + /// @param i Index of the rigid body mesh. + /// @return Faces of the i-th rigid body mesh. + Eigen::MatrixXi body_faces(size_t i) const + { + return faces() + .middleRows(body_face_starts[i], body_num_faces(i)) + .array() + - body_vertex_starts[i]; + } + + /// @brief Map a vector quantity on the collision mesh to the rigid degrees of freedom. + /// This is useful for mapping gradients from the collision mesh to the + /// rigid degrees of freedom (i.e., applies the chain-rule). + /// @param poses Poses of the rigid bodies. + /// @param g Vector quantity on the collision mesh with size equal to ndof(). + /// @return Vector quantity on the full mesh with size equal to full_ndof(). + Eigen::VectorXd to_rigid_dof( + const std::vector& poses, + Eigen::ConstRef g) const; + + /// @brief Map a Hessian on the collision mesh to the rigid degrees of freedom. + /// + /// Applies the chain rule JᵀHJ plus the second-order term Σ g·d²V/dx² on + /// the diagonal blocks. Only computes the per-body-pair blocks for body + /// pairs that are actually coupled in H (plus the diagonal blocks). + /// + /// @param poses The poses of the rigid bodies. + /// @param g Gradient on the collision mesh with size equal to ndof(). + /// @param H Hessian on the collision mesh with size equal to ndof() × ndof(). + /// @return Sparse Hessian on the rigid degrees of freedom. + Eigen::SparseMatrix to_rigid_dof( + const std::vector& poses, + Eigen::ConstRef g, + const Eigen::SparseMatrix& H) const; + + /// @brief Get the start index of the i-th body's vertices in the collision mesh. + index_t body_vertex_start(size_t i) const { return body_vertex_starts[i]; } + + /// @brief Get the start index of the i-th body's edges in the collision mesh. + index_t body_edge_start(size_t i) const { return body_edge_starts[i]; } + + /// @brief Get the start index of the i-th body's faces in the collision mesh. + index_t body_face_start(size_t i) const { return body_face_starts[i]; } + + /// @brief Get the body index for a given vertex index. + index_t vertex_to_body(index_t vi) const + { + assert(vi >= 0 && vi < num_vertices()); + return body_containing_vertex(body_vertex_starts, vi); + } + + /// @brief Make a list of body names unique, the same way bodies are named. + /// + /// Empty names become "Body{i}" and a repeated name gets a ".{i}" suffix, + /// where i is the name's position in @p names. + /// @param names The requested names. + /// @return Unique names, aligned with @p names. + static std::vector + make_unique_names(const std::vector& names); + +private: + /// @brief The body containing vertex vi, given each body's first vertex. + static index_t body_containing_vertex( + const std::vector& body_vertex_starts, const index_t vi) + { + // Find the first element GREATER than vi + auto it = std::upper_bound( + body_vertex_starts.begin(), body_vertex_starts.end(), vi); + + // The body index is (iterator distance - 1) + // std::distance returns the number of elements from the start to 'it' + return static_cast( + std::distance(body_vertex_starts.begin(), it)) + - 1; + } + + /// @brief Populate @ref m_body_names / @ref m_name_to_index (num_bodies() + /// entries). Empty or missing names default to "Body{i}"; duplicates are + /// disambiguated with a ".{i}" suffix so every name is unique. + void assign_body_names(const std::vector& names); + + /// @brief Rigid bodies in the system + std::vector bodies; + + // Start indices of vertices for each body + std::vector body_vertex_starts; + std::vector body_edge_starts; + std::vector body_face_starts; + + /// Per-body codimensional element counts (used to skip the codim + /// broad-phase passes for surface-mesh body pairs). + std::vector m_body_num_codim_vertices; + std::vector m_body_num_codim_edges; + + /// Per-body names (aligned with @ref bodies); unique, defaulting to + /// "Body{i}" for unnamed bodies. Populated by @ref build_from_meshes. + std::vector m_body_names; + /// Reverse lookup from body name to index. + std::unordered_map m_name_to_index; +}; + +} // namespace ipc::rigid \ No newline at end of file diff --git a/src/ipc/dynamics/rigid/rigid_body.cpp b/src/ipc/dynamics/rigid/rigid_body.cpp new file mode 100644 index 000000000..96052ef5c --- /dev/null +++ b/src/ipc/dynamics/rigid/rigid_body.cpp @@ -0,0 +1,215 @@ +#include "rigid_body.hpp" + +#include +#include +#include + +namespace ipc::rigid { + +namespace { + void center_vertices( + Eigen::Ref vertices, + Eigen::ConstRef edges, + Eigen::ConstRef faces, + Pose& pose) + { + // compute the center of mass several times to get more accurate + for (int i = 0; i < 10; i++) { + double mass; + VectorMax3d com; + MatrixMax3d inertia; + compute_mass_properties( + vertices, + (vertices.cols() == 2 || faces.size() == 0) ? edges : faces, + 1.0, // density (1.0 because we only want the center of mass) + mass, com, inertia); + vertices.rowwise() -= com.transpose(); + pose.position += com; + if (com.squaredNorm() < 1e-8) { + break; + } + } + } + + /// Convert the classical principal moments of inertia to the diagonal + /// second-moment matrix J = ∫ρ x̄x̄ᵀ (3D only; in 2D the second moment is + /// computed directly). + inline Eigen::DiagonalMatrix + second_moment_from_inertia(Eigen::ConstRef I) // NOLINT + { + assert(I.size() == 3); + Eigen::DiagonalMatrix J(3); // NOLINT + J.diagonal() << 0.5 * (-I.x() + I.y() + I.z()), + 0.5 * (I.x() - I.y() + I.z()), 0.5 * (I.x() + I.y() - I.z()); + return J; + } +} // namespace + +RigidBody::RigidBody( + Eigen::Ref vertices, + Eigen::ConstRef edges, + Eigen::ConstRef faces, + const double density, + Pose& initial_pose, + const VectorMax6b& is_dof_fixed, + const std::optional& center_of_mass) +{ + assert(vertices.size() > 0); + assert(edges.size() == 0 || edges.cols() == 2); + assert(faces.size() == 0 || faces.cols() == 3); + + const int dim = vertices.cols(); + assert(dim == 2 || dim == 3); + + const int ndof = dim == 2 ? 3 : 6; + assert(is_dof_fixed.size() == 0 || is_dof_fixed.size() == ndof); + m_is_dof_fixed = is_dof_fixed.size() == ndof + ? is_dof_fixed + : VectorMax6b(VectorMax6b::Zero(ndof)); + + // 1. Center the vertices, so the mass properties are computed correctly + // TODO: This should not be necessary. Determine why the mass properties + // are not computed correctly without centering the vertices. + Pose centering_pose = Pose::identity(dim); + center_vertices(vertices, edges, faces, centering_pose); + + // 2. Compute the mass properties + VectorMax3d computed_center_of_mass; + MatrixMax3d inertia_tensor; + compute_mass_properties( + vertices, (dim == 2 || faces.size() == 0) ? edges : faces, density, + m_mass, computed_center_of_mass, inertia_tensor); + m_volume = m_mass / density; + + // A prescribed center of mass moves the body's origin, the point its pose + // and rotation refer to, from the computed center of mass to the given + // one. We move the inertia with it by the parallel axis theorem, since + // compute_mass_properties returns it about the computed center of mass. + if (center_of_mass) { + assert(center_of_mass->size() == dim); + const VectorMax3d d = *center_of_mass - centering_pose.position; + vertices.rowwise() -= d.transpose(); + centering_pose.position += d; + if (dim == 3) { + // Classical inertia tensor: I + m(‖d‖²·Id − d dᵀ) + inertia_tensor += m_mass + * (d.squaredNorm() * Eigen::Matrix3d::Identity() + - d * d.transpose()); + } else { + // Second moment ∫ρ x̄x̄ᵀ: S + m d dᵀ + inertia_tensor += m_mass * d * d.transpose(); + } + } + + // 3. Convert the inertia tensor to the principal axes moments of inertia + const int num_rot_dof_fixed = + dim == 3 ? int(m_is_dof_fixed.tail(3).count()) : 0; + if (dim == 3 && num_rot_dof_fixed == 2) { + // Rotation is confined to a single world axis: keep the world-frame + // moments of inertia and skip the principal-axes rotation R₀ so the + // free rotation-vector component stays aligned with its world axis. + m_moment_of_inertia = inertia_tensor.diagonal(); + m_principal_rotation = Eigen::Matrix3d::Identity(); + + // We keep only the diagonal of the world-frame second moment. This is + // exact for rotation about one world axis: the inertial energy only + // sees the trace of the second moment's block orthogonal to that + // axis, so the dropped off-diagonal terms never contribute. + m_second_moment = second_moment_from_inertia(m_moment_of_inertia); + } else if (dim == 3) { + if (num_rot_dof_fixed == 1) { + logger().warn( + "Rigid body dynamics with two free rotational DOF has not " + "been tested thoroughly."); + } + // This computation is taken from ProjectChrono: https://bit.ly/2RpbTl1 + // The eigen values of the inertia tensor are the principal moments + // of inertia, which are the diagonal elements of the diagonalized + // inertia tensor. The eigenvectors are the principal axes of the + // inertia tensor, which are the columns of the rotation matrix R₀. + Eigen::SelfAdjointEigenSolver solver; + + // Remove small values from the inertia tensor to avoid numerical + // issues in the eigen decomposition + const double threshold = 1e-16 * inertia_tensor.maxCoeff(); + inertia_tensor = (inertia_tensor.array().abs() < threshold) + .select(0.0, inertia_tensor); + + solver.compute(inertia_tensor); + assert(solver.info() == Eigen::Success); + + // The principal moments of inertia are the eigenvalues of the inertia + // tensor. + m_moment_of_inertia = solver.eigenvalues(); + if ((m_moment_of_inertia.array() < 0).any()) { + logger().warn( + "Negative moments of inertia ({}), inverting.", + m_moment_of_inertia); + // This typically only happens with negative ε inertias + m_moment_of_inertia = m_moment_of_inertia.array().abs(); + } + + // The rotation from the principal inertial frame to the input world + // frame. + m_principal_rotation = solver.eigenvectors(); + + // Ensure that we have an orientation preserving transform + if (m_principal_rotation.determinant() < 0.0) { + m_principal_rotation.col(0) *= -1.0; + } + assert(m_principal_rotation.isUnitary(1e-9)); + + // Remove the initial rotation from the rest vertices + vertices = vertices * m_principal_rotation; + + // Store the initial rotation in the pose (R = RᵢR₀) + Eigen::AngleAxisd r = + Eigen::AngleAxisd(Eigen::Matrix3d(m_principal_rotation)); + centering_pose.rotation = r.angle() * r.axis(); + + // Compute the diagonal second moment in the principal frame from the + // classical inertia tensor. + m_second_moment = second_moment_from_inertia(m_moment_of_inertia); + } else { + // 2D: the inertia is the full 2×2 second moment ∫ρ x̄x̄ᵀ (about the + // COM). Mirror the 3D principal-frame handling: diagonalize, fold the + // principal rotation into the pose, and rotate the rest vertices. + Eigen::SelfAdjointEigenSolver solver; + solver.compute(Eigen::Matrix2d(inertia_tensor)); + assert(solver.info() == Eigen::Success); + + Eigen::Matrix2d R0 = solver.eigenvectors(); // NOLINT + if (R0.determinant() < 0.0) { + R0.col(0) *= -1.0; + } + assert(R0.isUnitary(1e-9)); + m_principal_rotation = R0; + + // Remove the initial rotation from the rest vertices + vertices = vertices * m_principal_rotation; + + // Store the initial rotation angle in the pose + centering_pose.rotation.resize(1); + centering_pose.rotation(0) = std::atan2(R0(1, 0), R0(0, 0)); + + // Diagonal second moment in the principal frame + m_second_moment = Eigen::DiagonalMatrix( + solver.eigenvalues().cwiseAbs()); + + // Scalar moment about z: I_z = ∫ρ(x² + y²) = tr(∫ρ x̄x̄ᵀ) + m_moment_of_inertia.resize(1); + m_moment_of_inertia(0) = m_second_moment.diagonal().sum(); + } + + // 4. Apply the centering pose to the initial pose + initial_pose = initial_pose * centering_pose; + + m_external_force = Pose::identity(dim); + + m_bounding_radius = vertices.rowwise().norm().maxCoeff(); + + m_bvh = std::make_shared(); + m_bvh->build(vertices, edges, faces); +} + +} // namespace ipc::rigid \ No newline at end of file diff --git a/src/ipc/dynamics/rigid/rigid_body.hpp b/src/ipc/dynamics/rigid/rigid_body.hpp new file mode 100644 index 000000000..151453b29 --- /dev/null +++ b/src/ipc/dynamics/rigid/rigid_body.hpp @@ -0,0 +1,203 @@ +#pragma once + +#include +#include +#include + +#include + +namespace ipc { +class LBVH; +} + +namespace ipc::rigid { + +class RigidBody { +public: + enum class Type : uint8_t { + /// @brief Static rigid body, does not move + STATIC, + /// @brief Kinematic rigid body, moves but does not respond to forces + KINEMATIC, + /// @brief Dynamic rigid body, moves and responds to forces + DYNAMIC + }; + +public: + /// @brief Construct a rigid body from a mesh and density. + /// @param vertices Vertices of the mesh (will be modified to be centered at the center of mass) + /// @param edges Edges of the mesh (2D) + /// @param faces Faces of the mesh (3D) or empty (2D) + /// @param density Density of the rigid body + /// @param initial_pose Initial pose of the rigid body (will be modified to account for the center of mass) + /// @param is_dof_fixed Per-DOF fixed flags ([position | rotation]; empty means all free). Positional flags refer to world axes; rotational flags to the components of the rotation vector. + /// @param center_of_mass Optional center of mass to use instead of the one computed from the mesh, in the frame of the input vertices. The body's pose and rotation then refer to this point, and its inertia is moved to it by the parallel axis theorem. The inertial term assumes the body's origin is its true center of mass, so this is exact only for bodies whose inertia does not enter the dynamics (STATIC and KINEMATIC ones). + RigidBody( + Eigen::Ref vertices, + Eigen::ConstRef edges, + Eigen::ConstRef faces, + const double density, + Pose& initial_pose, + const VectorMax6b& is_dof_fixed = VectorMax6b(), + const std::optional& center_of_mass = std::nullopt); + + // ---- Getters ------------------------------------------------------------ + + /// @brief The measure of the body that its mass is computed from. + /// + /// This is the enclosed volume for a closed 3D triangle mesh. Otherwise + /// the mass is lumped, and this is the total lumped measure: the edge + /// length for 2D and edge meshes, or one unit per vertex for point clouds + /// and open surfaces. + double volume() const { return m_volume; } + + /// @brief The total mass (density × volume()). + double mass() const { return m_mass; } + + /// @brief The uniform density (mass() / volume()). + double density() const { return m_mass / m_volume; } + + /// @brief The principal moments of inertia about the center of mass. + /// + /// In 3D these are the inertia tensor's eigenvalues, ordered to match the + /// axes of principal_rotation(). In 2D this is a single value, the moment + /// about the out-of-plane axis. When exactly two rotational DOFs are + /// fixed in 3D, these are instead the diagonal of the world-frame inertia + /// tensor. + const VectorMax3d& moment_of_inertia() const { return m_moment_of_inertia; } + + /// @brief The diagonal second-moment matrix ∫ρ x̄x̄ᵀ in the principal frame (2×2 in 2D, 3×3 in 3D). + /// + /// This is the J in the affine mass matrix blkdiag(m I, J ⊗ I). When + /// exactly two rotational DOFs are fixed in 3D, it is the diagonal of the + /// world-frame second moment instead. + const auto& second_moment() const { return m_second_moment; } + + /// @brief The rotation from the principal axes to the input frame. + /// + /// At construction we express the rest vertices in the principal frame + /// and fold this rotation into the initial pose, so world positions are + /// unchanged. We keep it to convert back to the input frame (e.g., when + /// writing glTF). It is the identity when exactly two rotational DOFs are + /// fixed in 3D, so the free rotation component stays on its world axis. + const MatrixMax3d& principal_rotation() const + { + return m_principal_rotation; + } + + /// @brief The constant external load applied to the body. + /// + /// The position holds the world-space force and the rotation holds the + /// world-space torque (a scalar in 2D). The simulator folds it into the + /// incremental potential each step, for DYNAMIC bodies only. + const Pose& external_force() const { return m_external_force; } + + /// @brief The static bounding volume hierarchy of the body's elements. + /// + /// We build it once at construction from the rest vertices, in the body + /// frame, so the rigid broad phase can query a moving body without + /// rebuilding it. + std::shared_ptr bvh() const { return m_bvh; } + + /// @brief The largest distance from the center of mass to a rest vertex. + /// + /// The body stays inside a sphere of this radius about its position in + /// every orientation, which is what the body-level broad phase uses. + double bounding_radius() const { return m_bounding_radius; } + + /// @brief How the body is simulated (STATIC, KINEMATIC, or DYNAMIC). + Type type() const { return m_type; } + + /// @brief Whether the body is DYNAMIC, i.e., responds to forces and + /// contact. + bool is_dynamic() const { return m_type == Type::DYNAMIC; } + + /// @brief Per-DOF fixed flags ([position | rotation] of size ndof). + const VectorMax6b& is_dof_fixed() const { return m_is_dof_fixed; } + + // ---- Setters ------------------------------------------------------------ + + void set_external_force(const Pose& external_force) + { + assert( + external_force.position.size() == m_external_force.position.size()); + assert( + external_force.rotation.size() == m_external_force.rotation.size()); + m_external_force = external_force; + } + + void set_type(const Type type) { m_type = type; } + + /// @brief Change the body's (uniform) density, rescaling its mass and inertia. + /// + /// Mass and inertia are both linear in density, and the principal axes do + /// not depend on it, so we rescale them instead of recomputing the mass + /// properties from the mesh. + /// + /// @note A running simulator picks up the new mass matrix at its next time step. + /// @param density New density (must be positive). + void set_density(const double density) + { + assert(density > 0); + const double scale = density / this->density(); + m_mass *= scale; + m_moment_of_inertia *= scale; + m_second_moment.diagonal() *= scale; + } + + /// @brief Restore the fixed-DOF flags (used by Simulator::reset()). + /// @warning Must match the construction-time flags: fixing rotational + /// DOFs changes the inertia handling, which is baked at construction. + void set_is_dof_fixed(const VectorMax6b& is_dof_fixed) + { + assert(is_dof_fixed.size() == m_is_dof_fixed.size()); + m_is_dof_fixed = is_dof_fixed; + } + + /// @brief Convert this body to a STATIC body (all DOF fixed). + /// @note Velocities need no explicit zeroing here: they live in the time + /// integrator's history and decay to zero once the DOF are pinned. + void convert_to_static() + { + m_type = Type::STATIC; + m_is_dof_fixed.setOnes(); + } + +private: + /// @brief Volume of the rigid body + double m_volume; + + /// @brief Total mass of the rigid body + double m_mass; + + /// @brief Moment of inertia measured with respect to the principal axes + VectorMax3d m_moment_of_inertia; + + /// @brief Diagonal second-moment matrix ∫ρ x̄x̄ᵀ in the principal frame + Eigen::DiagonalMatrix m_second_moment; + + /// @brief Rotation matrix from the principal axes to the world frame + /// This also stored in initial_pose.rotation upon construction. + /// This is useful for converting to and from input world coordinates. + /// @note Maybe this should be a rotation vector instead? + /// @note Maybe we should store position as well? + MatrixMax3d m_principal_rotation; + + /// @brief External force and torque applied to the rigid body + Pose m_external_force; + + /// @brief Statically constructed bounding volume hierarchy for collision detection + /// @note This is defined in the inertial reference frame + std::shared_ptr m_bvh; + + /// @brief Bounding radius of the rigid body + double m_bounding_radius; + + /// @brief How this body is simulated (STATIC/KINEMATIC/DYNAMIC). + Type m_type = Type::DYNAMIC; + + /// @brief Per-DOF fixed flags ([position | rotation] of size ndof). + VectorMax6b m_is_dof_fixed; +}; + +} // namespace ipc::rigid \ No newline at end of file diff --git a/src/ipc/dynamics/rigid/rigid_candidates.cpp b/src/ipc/dynamics/rigid/rigid_candidates.cpp new file mode 100644 index 000000000..575f85599 --- /dev/null +++ b/src/ipc/dynamics/rigid/rigid_candidates.cpp @@ -0,0 +1,601 @@ +#include "rigid_candidates.hpp" + +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include + +#include + +namespace ipc::rigid { + +const NonlinearCCD& default_nonlinear_ccd() +{ + // We share one instance instead of allocating a fresh TightInclusionCCD + // for every defaulted argument. Queries never modify it, so concurrent + // use is safe. + static const NonlinearCCD ccd; + return ccd; +} + +void RigidCandidates::build( + const RigidBodies& bodies, + const std::vector& poses, + const double inflation_radius, + BroadPhase* broad_phase) +{ + build(bodies, poses, poses, inflation_radius, broad_phase); +} + +void RigidCandidates::build( + const RigidBodies& bodies, + const std::vector& poses_t0, + const std::vector& poses_t1, + const double inflation_radius, + BroadPhase* broad_phase) +{ + assert(poses_t0.size() == bodies.num_bodies()); + assert(poses_t1.size() == bodies.num_bodies()); + + clear(); + + // A body participates in contact resolution only if it is DYNAMIC (free to + // respond to contact forces). STATIC and KINEMATIC bodies have prescribed + // trajectories, and analytic planes are fixed, so a contact between two + // non-dynamic participants (static-static / static-kinematic / + // kinematic-kinematic, and any of those against a plane) can never be + // resolved — and when such bodies overlap by construction (a common fixture + // pattern) the barrier evaluates on a negative distance and blows up to + // inf. Keep a candidate pair only if at least one participant is dynamic. + const auto is_dynamic = [&bodies](const index_t b) { + const RigidBody& body = bodies[b]; + const VectorMax6b& fixed = body.is_dof_fixed(); + const bool all_dof_fixed = fixed.size() > 0 && fixed.all(); + return body.type() == RigidBody::Type::DYNAMIC && !all_dof_fixed; + }; + + // World-space vertex positions at the interval endpoints (for the + // plane-vertex candidates below; body-body candidates work in relative + // body space and never need world-space vertices). + const Eigen::MatrixXd vertices_t0 = bodies.vertices(poses_t0); + const Eigen::MatrixXd vertices_t1 = bodies.vertices(poses_t1); + + // Planes to vertices: the plane distance is affine along the chord, so it + // suffices to check the endpoint distances inflated by the deviation bound. + for (const auto& plane : bodies.planes) { + for (index_t vi = 0; vi < bodies.num_vertices(); ++vi) { + const index_t b = bodies.vertex_to_body(vi); + // The plane is fixed; keep only vertices of dynamic bodies. + if (!is_dynamic(b)) { + continue; + } + const double deviation = rigid_chord_deviation( + bodies.rest_positions().row(vi).norm(), + (poses_t1[b].rotation - poses_t0[b].rotation).norm()); + const double d_t0 = plane.signedDistance(vertices_t0.row(vi)); + const double d_t1 = plane.signedDistance(vertices_t1.row(vi)); + if (d_t0 < inflation_radius + deviation + || d_t1 < inflation_radius + deviation) { + pv_candidates.emplace_back(plane, vi); + } + } + } + + if (bodies.num_bodies() < 2) { + return; // No body-body collisions possible + } + + std::unique_ptr default_broad_phase; + if (broad_phase == nullptr) { + default_broad_phase = make_default_broad_phase(); + broad_phase = default_broad_phase.get(); + } + + const int dim = poses_t0[0].position.size(); + + // 1. Broad phase between bodies + + // a. Build body AABBs (bounding sphere swept between the two positions) + AABBs body_boxes(bodies.num_bodies()); + for (int i = 0; i < bodies.num_bodies(); i++) { + const double r = bodies[i].bounding_radius() + inflation_radius; + body_boxes[i] = AABB( + AABB::from_point(poses_t0[i].position, r), + AABB::from_point(poses_t1[i].position, r)); + } + + // b. Build the broad phase. The shared broad-phase object may carry a + // *vertex*-level filter from a previous standard build (Candidates::build + // sets can_vertices_collide to mesh.can_collide and leaves it set). Here + // the "vertices" are bodies, so that filter must not be applied: feeding + // body indices to a vertex filter silently drops body pairs (e.g., the + // same-body vertex exclusion rejects low body indices because the + // corresponding vertices belong to the same body). We use an accept-all + // filter for the body-level pass. + // + // The guard restores the caller's filter and drops the body boxes on + // every exit path, including exceptions, so the caller's broad phase + // never keeps body-level state that a later vertex query would misread. + struct BodyPassGuard { + BroadPhase& broad_phase; + CollisionFilter caller_filter; + ~BodyPassGuard() + { + broad_phase.clear(); + broad_phase.can_vertices_collide = std::move(caller_filter); + } + }; + + std::vector body_candidates; + { + BodyPassGuard guard { *broad_phase, broad_phase->can_vertices_collide }; + // TODO: Implement a mechanism to allow the caller to specify a + // body-level filter (e.g., for jointed bodies). + broad_phase->can_vertices_collide = CollisionFilter(); // accept all + broad_phase->build( + body_boxes, /*edges=*/Eigen::MatrixXi(), + /*faces=*/Eigen::MatrixXi(), dim); + + // c. Detect body-body candidates: bodies are stored as "vertices," so + // we can reuse vertex-vertex candidate detection. + broad_phase->detect_vertex_vertex_candidates(body_candidates); + } + + // 2. Broad phase between colliding bodies + // NOTE: The loop over body pairs is serial because each two-tree query is + // internally parallelized. + for (const VertexVertexCandidate& body_candidate : body_candidates) { + auto [body_i, body_j] = body_candidate; // Body indices + + // Skip pairs where neither body is dynamic (static/kinematic in any + // combination): a contact between two prescribed/fixed bodies can never + // be resolved. + if (!is_dynamic(body_i) && !is_dynamic(body_j)) { + continue; + } + + // Ensure bi has more vertices than bj to take advantage of log(n) + // traversal (bj's leaves are iterated against bi's tree). + if (bodies.body_num_vertices(body_i) + < bodies.body_num_vertices(body_j)) { + std::swap(body_i, body_j); + } + + // a. Build the boxes for body bj's swept trajectory inside the (rest) + // body space of body bi. + const Pose body_j_to_i_t0 = + poses_t0[body_i].inverse() * poses_t0[body_j]; + const Eigen::MatrixXd j_vertices_t0 = + bodies.body_vertices(body_j, body_j_to_i_t0); + + const Pose body_j_to_i_t1 = + poses_t1[body_i].inverse() * poses_t1[body_j]; + const Eigen::MatrixXd j_vertices_t1 = + bodies.body_vertices(body_j, body_j_to_i_t1); + + // The relative trajectory is nonlinear, so the endpoint boxes are + // inflated by a conservative bound on the deviation of the relative + // curve from its chord (see relative_rigid_chord_deviation). Body i's + // rest-frame BVH boxes are not inflated, so the inflation radius is + // applied twice on this side (one-sided Minkowski growth). + const double dtheta_i = + (poses_t1[body_i].rotation - poses_t0[body_i].rotation).norm(); + const double dtheta_j = + (poses_t1[body_j].rotation - poses_t0[body_j].rotation).norm(); + const VectorMax3d dq_t0 = + poses_t0[body_j].position - poses_t0[body_i].position; + const VectorMax3d dq_t1 = + poses_t1[body_j].position - poses_t1[body_i].position; + const double max_translation = std::max(dq_t0.norm(), dq_t1.norm()); + const double translation_change = (dq_t1 - dq_t0).norm(); + + const auto j_rest_positions = bodies.body_rest_positions(body_j); + + AABBs body_j_vertex_boxes(bodies.body_num_vertices(body_j)); + tbb::parallel_for(size_t(0), body_j_vertex_boxes.size(), [&](size_t v) { + const double deviation = relative_rigid_chord_deviation( + j_rest_positions.row(v).norm(), dtheta_i, dtheta_j, + max_translation, translation_change); + body_j_vertex_boxes[v] = AABB::from_point( + j_vertices_t0.row(v).transpose(), + j_vertices_t1.row(v).transpose(), + deviation + 2 * inflation_radius); + }); + + // b. Build leaves for body bj's swept boxes. Edge/face boxes are unions + // of the vertex boxes, which is conservative because every point of + // a moving edge/face is a convex combination of its vertices' + // trajectories. The two-tree traversal only iterates bj's side, so + // we skip building a hierarchy for it: on scenes with hundreds of + // small bodies, that build's fixed cost dominated this loop. + LBVH::Leaves body_j_leaves; + body_j_leaves.build( + body_j_vertex_boxes, bodies.body_edges(body_j), + bodies.body_faces(body_j)); + + // c. Detect candidates between body bi and bj using the static + // rest-frame BVH of body bi. Local per-body element ids are mapped + // to global collision mesh ids using the body start offsets. + const LBVH& body_i_bvh = *bodies[body_i].bvh(); + + const index_t vi_start = bodies.body_vertex_start(body_i); + const index_t ei_start = bodies.body_edge_start(body_i); + const index_t fi_start = bodies.body_face_start(body_i); + const index_t vj_start = bodies.body_vertex_start(body_j); + const index_t ej_start = bodies.body_edge_start(body_j); + const index_t fj_start = bodies.body_face_start(body_j); + + // Vertex-level filters mirroring the single-tree traversal's + // can_*_collide semantics, but with global ids so the collision mesh's + // can_collide (e.g., a vertex-patch filter between jointed bodies) is + // honored. Elements of distinct bodies never share endpoints, so the + // shared-endpoint exclusions are unnecessary here. + const Eigen::MatrixXi& E = bodies.edges(); + const Eigen::MatrixXi& F = bodies.faces(); + const auto can_vv = [&](const index_t va, const index_t vb) { + return bodies.can_collide(va, vb); + }; + const auto can_ev = [&](const index_t e, const index_t v) { + return can_vv(E(e, 0), v) || can_vv(E(e, 1), v); + }; + const auto can_ee = [&](const index_t ea, const index_t eb) { + return can_ev(ea, E(eb, 0)) || can_ev(ea, E(eb, 1)); + }; + const auto can_fv = [&](const index_t f, const index_t v) { + return can_vv(F(f, 0), v) || can_vv(F(f, 1), v) + || can_vv(F(f, 2), v); + }; + + if (dim == 2) { + // (eᵢ, vⱼ): receives (this_edge_id, other_vertex_id) + std::vector ev; + body_i_bvh.detect_edge_vertex_candidates( + body_j_leaves, + [&](size_t ei, size_t vj) { + return can_ev(ei_start + ei, vj_start + vj); + }, + ev); + for (const EdgeVertexCandidate& c : ev) { + ev_candidates.emplace_back( + ei_start + c.edge_id, vj_start + c.vertex_id); + } + + // (eⱼ, vᵢ): receives (other_edge_id, this_vertex_id) + ev.clear(); + body_i_bvh.detect_vertex_edge_candidates( + body_j_leaves, + [&](size_t ej, size_t vi) { + return can_ev(ej_start + ej, vi_start + vi); + }, + ev); + for (const EdgeVertexCandidate& c : ev) { + ev_candidates.emplace_back( + ej_start + c.edge_id, vi_start + c.vertex_id); + } + } else { + // (eᵢ, eⱼ): receives (this_edge_id, other_edge_id) + std::vector ee; + body_i_bvh.detect_edge_edge_candidates( + body_j_leaves, + [&](size_t ei, size_t ej) { + return can_ee(ei_start + ei, ej_start + ej); + }, + ee); + for (const EdgeEdgeCandidate& c : ee) { + ee_candidates.emplace_back( + ei_start + c.edge0_id, ej_start + c.edge1_id); + } + + // (fᵢ, vⱼ): receives (this_face_id, other_vertex_id) + std::vector fv; + body_i_bvh.detect_face_vertex_candidates( + body_j_leaves, + [&](size_t fi, size_t vj) { + return can_fv(fi_start + fi, vj_start + vj); + }, + fv); + for (const FaceVertexCandidate& c : fv) { + fv_candidates.emplace_back( + fi_start + c.face_id, vj_start + c.vertex_id); + } + + // (fⱼ, vᵢ): receives (other_face_id, this_vertex_id) + fv.clear(); + body_i_bvh.detect_vertex_face_candidates( + body_j_leaves, + [&](size_t fj, size_t vi) { + return can_fv(fj_start + fj, vi_start + vi); + }, + fv); + for (const FaceVertexCandidate& c : fv) { + fv_candidates.emplace_back( + fj_start + c.face_id, vi_start + c.vertex_id); + } + } + + // Codimensional candidates, mirroring the single-mesh logic of + // Candidates::build: codim-vertex × codim-vertex pairs (narrow-phased + // with point-point CCD) and, in 3D, codim-edge × codim-vertex pairs + // (a codim edge against a non-codim vertex is already covered by the + // ee/fv passes above, and a codim vertex against a face-adjacent edge + // by fv). The passes are skipped entirely for surface-mesh pairs. + const bool i_has_cv = bodies.body_num_codim_vertices(body_i) > 0; + const bool j_has_cv = bodies.body_num_codim_vertices(body_j) > 0; + + if (i_has_cv && j_has_cv) { + // (vᵢ, vⱼ): receives (this_vertex_id, other_vertex_id) + std::vector vv; + body_i_bvh.detect_vertex_vertex_candidates( + body_j_leaves, + [&](size_t vi, size_t vj) { + return bodies.is_codim_vertex(vi_start + vi) + && bodies.is_codim_vertex(vj_start + vj) + && can_vv(vi_start + vi, vj_start + vj); + }, + vv); + for (const VertexVertexCandidate& c : vv) { + vv_candidates.emplace_back( + vi_start + c.vertex0_id, vj_start + c.vertex1_id); + } + } + + if (dim == 3) { + if (bodies.body_num_codim_edges(body_i) > 0 && j_has_cv) { + // (eᵢ, vⱼ): receives (this_edge_id, other_vertex_id) + std::vector ev; + body_i_bvh.detect_edge_vertex_candidates( + body_j_leaves, + [&](size_t ei, size_t vj) { + return bodies.is_codim_edge(ei_start + ei) + && bodies.is_codim_vertex(vj_start + vj) + && can_ev(ei_start + ei, vj_start + vj); + }, + ev); + for (const EdgeVertexCandidate& c : ev) { + ev_candidates.emplace_back( + ei_start + c.edge_id, vj_start + c.vertex_id); + } + } + + if (i_has_cv && bodies.body_num_codim_edges(body_j) > 0) { + // (eⱼ, vᵢ): receives (other_edge_id, this_vertex_id) + std::vector ev; + body_i_bvh.detect_vertex_edge_candidates( + body_j_leaves, + [&](size_t ej, size_t vi) { + return bodies.is_codim_edge(ej_start + ej) + && bodies.is_codim_vertex(vi_start + vi) + && can_ev(ej_start + ej, vi_start + vi); + }, + ev); + for (const EdgeVertexCandidate& c : ev) { + ev_candidates.emplace_back( + ej_start + c.edge_id, vi_start + c.vertex_id); + } + } + } + } +} + +// ============================================================================ +// Narrow phase + +namespace { + /// @brief Nonlinear CCD between a rigidly-moving point and a static plane. + /// + /// Uses the conservative piecewise-linear CCD driver: within each + /// linearized piece the position is affine in t, so the plane distance is + /// affine and attains its minimum at an endpoint; the crossing time of the + /// (deviation-inflated) minimum separation is computed exactly. + bool rigid_point_plane_ccd( + const Eigen::Hyperplane& plane, + const RigidTrajectory& p, + double& toi, + const double min_distance, + const double tmax, + const double conservative_rescaling) + { + return NonlinearCCD::conservative_piecewise_linear_ccd( + [&](const double t) { + return std::abs(plane.signedDistance(p(t))); + }, + [&](const double t0, const double t1) { + return p.max_distance_from_linear(t0, t1); + }, + [&](const double ti0, const double ti1, const double min_d, + const bool no_zero_toi, double& piece_toi) { + const double d0 = plane.signedDistance(p(ti0)); + const double d1 = plane.signedDistance(p(ti1)); + // Measure from the side the point starts on + const double sign = d0 >= 0 ? 1.0 : -1.0; + const double e0 = sign * d0; + const double e1 = sign * d1; + if (e1 >= min_d) { + return false; // Affine distance is minimized at an endpoint + } + piece_toi = e0 > e1 + ? std::max((e0 - min_d) / (e0 - e1), 0.0) + : 0.0; // Let the driver subdivide degenerate cases + return true; + }, + toi, min_distance, tmax, conservative_rescaling); + } +} // namespace + +bool RigidCandidates::candidate_ccd( + const size_t i, + const RigidBodies& bodies, + const std::vector& poses_t0, + const std::vector& poses_t1, + const double min_distance, + const double tmax, + const NonlinearCCD& nonlinear_ccd, + double& toi) const +{ + const auto trajectory = [&](const index_t vi) { + const index_t b = bodies.vertex_to_body(vi); + return RigidTrajectory( + bodies.rest_positions().row(vi).transpose(), poses_t0[b], + poses_t1[b]); + }; + + const Eigen::MatrixXi& E = bodies.edges(); + const Eigen::MatrixXi& F = bodies.faces(); + + size_t j = i; // Match the ordering of Candidates::operator[] + if (j < vv_candidates.size()) { + const VertexVertexCandidate& c = vv_candidates[j]; + return nonlinear_ccd.point_point_ccd( + trajectory(c.vertex0_id), trajectory(c.vertex1_id), toi, + min_distance, tmax); + } + j -= vv_candidates.size(); + if (j < ev_candidates.size()) { + const EdgeVertexCandidate& c = ev_candidates[j]; + return nonlinear_ccd.point_edge_ccd( + trajectory(c.vertex_id), trajectory(E(c.edge_id, 0)), + trajectory(E(c.edge_id, 1)), toi, min_distance, tmax); + } + j -= ev_candidates.size(); + if (j < ee_candidates.size()) { + const EdgeEdgeCandidate& c = ee_candidates[j]; + return nonlinear_ccd.edge_edge_ccd( + trajectory(E(c.edge0_id, 0)), trajectory(E(c.edge0_id, 1)), + trajectory(E(c.edge1_id, 0)), trajectory(E(c.edge1_id, 1)), toi, + min_distance, tmax); + } + j -= ee_candidates.size(); + if (j < fv_candidates.size()) { + const FaceVertexCandidate& c = fv_candidates[j]; + return nonlinear_ccd.point_triangle_ccd( + trajectory(c.vertex_id), trajectory(F(c.face_id, 0)), + trajectory(F(c.face_id, 1)), trajectory(F(c.face_id, 2)), toi, + min_distance, tmax); + } + j -= fv_candidates.size(); + assert(j < pv_candidates.size()); + const PlaneVertexCandidate& c = pv_candidates[j]; + return rigid_point_plane_ccd( + c.plane, trajectory(c.vertex_id), toi, min_distance, tmax, + nonlinear_ccd.conservative_rescaling); +} + +bool RigidCandidates::is_step_collision_free( + const RigidBodies& bodies, + const std::vector& poses_t0, + const std::vector& poses_t1, + const double min_distance, + const NonlinearCCD& nonlinear_ccd) const +{ + assert(poses_t0.size() == bodies.num_bodies()); + assert(poses_t1.size() == bodies.num_bodies()); + + // Narrow phase + for (size_t i = 0; i < size(); i++) { + double toi; + if (candidate_ccd( + i, bodies, poses_t0, poses_t1, min_distance, /*tmax=*/1.0, + nonlinear_ccd, toi)) { + return false; + } + } + + return true; +} + +double RigidCandidates::compute_collision_free_stepsize( + const RigidBodies& bodies, + const std::vector& poses_t0, + const std::vector& poses_t1, + const double min_distance, + const NonlinearCCD& nonlinear_ccd) const +{ + assert(poses_t0.size() == bodies.num_bodies()); + assert(poses_t1.size() == bodies.num_bodies()); + + if (empty()) { + return 1; // No possible collisions, so can take full step. + } + + std::atomic earliest_toi(1.0); + + tbb::parallel_for(size_t(0), size(), [&](size_t i) { + const double tmax = earliest_toi.load(std::memory_order_relaxed); + + double toi = std::numeric_limits::infinity(); // output + const bool are_colliding = candidate_ccd( + i, bodies, poses_t0, poses_t1, min_distance, tmax, nonlinear_ccd, + toi); + + if (are_colliding) { + // Update the earliest time of impact (TOI) atomically + double prev = earliest_toi.load(std::memory_order_relaxed); + while (toi < prev + && !earliest_toi.compare_exchange_weak( + prev, toi, std::memory_order_relaxed)) { } + } + }); + + const double result = earliest_toi.load(std::memory_order_relaxed); + assert(result >= 0 && result <= 1.0); + return result; +} + +double RigidCandidates::compute_noncandidate_conservative_stepsize( + const RigidBodies& bodies, + const std::vector& poses_t0, + const std::vector& poses_t1, + const double dhat) const +{ + assert(poses_t0.size() == bodies.num_bodies()); + assert(poses_t1.size() == bodies.num_bodies()); + + // Bound each vertex's path length by ‖Δp‖ + ‖Δθ‖‖x̄‖ (see + // rigid_chord_deviation for the rotational velocity bound). + Eigen::MatrixXd displacement_bounds(bodies.num_vertices(), 1); + for (index_t vi = 0; vi < bodies.num_vertices(); ++vi) { + const index_t b = bodies.vertex_to_body(vi); + displacement_bounds(vi, 0) = + (poses_t1[b].position - poses_t0[b].position).norm() + + (poses_t1[b].rotation - poses_t0[b].rotation).norm() + * bodies.rest_positions().row(vi).norm(); + } + + return Candidates::compute_noncandidate_conservative_stepsize( + bodies, displacement_bounds, dhat); +} + +double RigidCandidates::compute_cfl_stepsize( + const RigidBodies& bodies, + const std::vector& poses_t0, + const std::vector& poses_t1, + const double dhat, + const double min_distance, + BroadPhase* broad_phase, + const NonlinearCCD& nonlinear_ccd) const +{ + const double alpha_C = this->compute_collision_free_stepsize( + bodies, poses_t0, poses_t1, min_distance, nonlinear_ccd); + + const double alpha_F = this->compute_noncandidate_conservative_stepsize( + bodies, poses_t0, poses_t1, dhat); + + // If alpha_F < 0.5 * alpha_C, then we should do full CCD. + if (alpha_F < 0.5 * alpha_C) { + RigidCandidates full_candidates; + full_candidates.build( + bodies, poses_t0, poses_t1, /*inflation_radius=*/0, broad_phase); + return full_candidates.compute_collision_free_stepsize( + bodies, poses_t0, poses_t1, min_distance, nonlinear_ccd); + } + return std::min(alpha_C, alpha_F); +} + +} // namespace ipc::rigid diff --git a/src/ipc/dynamics/rigid/rigid_candidates.hpp b/src/ipc/dynamics/rigid/rigid_candidates.hpp new file mode 100644 index 000000000..f498a623b --- /dev/null +++ b/src/ipc/dynamics/rigid/rigid_candidates.hpp @@ -0,0 +1,129 @@ +#pragma once + +#include +#include +#include + +namespace ipc::rigid { + +/// @brief The NonlinearCCD used when a RigidCandidates query is not given one. +/// @note A single shared instance, so defaulted arguments do not allocate. +const NonlinearCCD& default_nonlinear_ccd(); + +/// @brief A class for storing and managing collision candidates. +class RigidCandidates : public Candidates { +public: + RigidCandidates() = default; + + using Candidates::build; + + /// @brief Initialize the set of discrete collision detection candidates. + /// @param bodies The rigid bodies. + /// @param poses The poses of the rigid bodies. + /// @param inflation_radius Amount to inflate the bounding boxes. + /// @param broad_phase Broad phase method to use. + void build( + const RigidBodies& bodies, + const std::vector& poses, + const double inflation_radius = 0, + BroadPhase* broad_phase = nullptr); + + /// @brief Initialize the set of continuous collision detection candidates. + /// @note Assumes the trajectory is linear. + /// @param bodies The rigid bodies. + /// @param poses_t0 The starting poses of the rigid bodies. + /// @param poses_t1 The ending poses of the rigid bodies. + /// @param inflation_radius Amount to inflate the bounding boxes. + /// @param broad_phase Broad phase method to use. + void build( + const RigidBodies& bodies, + const std::vector& poses_t0, + const std::vector& poses_t1, + const double inflation_radius = 0, + BroadPhase* broad_phase = nullptr); + + /// @brief Determine if the step is collision free from the set of candidates. + /// @note Uses the nonlinear rigid trajectories of the bodies. + /// @param bodies The rigid bodies. + /// @param poses_t0 The starting poses of the rigid bodies. + /// @param poses_t1 The ending poses of the rigid bodies. + /// @param min_distance The minimum distance allowable between any two elements. + /// @param nonlinear_ccd The nonlinear narrow phase CCD algorithm to use. + /// @returns True if no collisions occur. + bool is_step_collision_free( + const RigidBodies& bodies, + const std::vector& poses_t0, + const std::vector& poses_t1, + const double min_distance = 0.0, + const NonlinearCCD& nonlinear_ccd = default_nonlinear_ccd()) const; + + /// @brief Computes a maximal step size that is collision free using the set of collision candidates. + /// @note Uses the nonlinear rigid trajectories of the bodies. + /// @param bodies The rigid bodies. + /// @param poses_t0 The starting poses of the rigid bodies. Assumed to be intersection free. + /// @param poses_t1 The ending poses of the rigid bodies. + /// @param min_distance The minimum distance allowable between any two elements. + /// @param nonlinear_ccd The nonlinear narrow phase CCD algorithm to use. + /// @returns A step-size \f$\in [0, 1]\f$ that is collision free. A value of 1.0 if a full step and 0.0 is no step. + double compute_collision_free_stepsize( + const RigidBodies& bodies, + const std::vector& poses_t0, + const std::vector& poses_t1, + const double min_distance = 0.0, + const NonlinearCCD& nonlinear_ccd = default_nonlinear_ccd()) const; + + /// @brief Computes a conservative bound on the largest-feasible step size for surface primitives not in collision. + /// + /// Each vertex's displacement is bounded by the arc-length bound + /// ‖Δp‖ + ‖Δθ‖‖x̄‖ of its rigid trajectory. + /// + /// @param bodies The rigid bodies. + /// @param poses_t0 The starting poses of the rigid bodies. + /// @param poses_t1 The ending poses of the rigid bodies. + /// @param dhat Barrier activation distance. + double compute_noncandidate_conservative_stepsize( + const RigidBodies& bodies, + const std::vector& poses_t0, + const std::vector& poses_t1, + const double dhat) const; + + /// @brief Computes a CFL-inspired CCD maximum step step size. + /// @param bodies The rigid bodies. + /// @param poses_t0 The starting poses of the rigid bodies. + /// @param poses_t1 The ending poses of the rigid bodies. + /// @param dhat Barrier activation distance. + /// @param min_distance The minimum distance allowable between any two elements. + /// @param broad_phase The broad phase algorithm to use. + /// @param nonlinear_ccd The nonlinear narrow phase CCD algorithm to use. + double compute_cfl_stepsize( + const RigidBodies& bodies, + const std::vector& poses_t0, + const std::vector& poses_t1, + const double dhat, + const double min_distance = 0.0, + BroadPhase* broad_phase = nullptr, + const NonlinearCCD& nonlinear_ccd = default_nonlinear_ccd()) const; + +private: + /// @brief Perform narrow-phase nonlinear CCD on the i-th candidate. + /// @param[in] i Index of the candidate (same ordering as operator[]). + /// @param[in] bodies The rigid bodies. + /// @param[in] poses_t0 The starting poses of the rigid bodies. + /// @param[in] poses_t1 The ending poses of the rigid bodies. + /// @param[in] min_distance The minimum distance allowable between any two elements. + /// @param[in] tmax Maximum time (normalized) to look for collisions. + /// @param[in] nonlinear_ccd The nonlinear narrow phase CCD algorithm to use. + /// @param[out] toi Computed time of impact (normalized). + /// @return If the candidate had a collision over the time interval. + bool candidate_ccd( + const size_t i, + const RigidBodies& bodies, + const std::vector& poses_t0, + const std::vector& poses_t1, + const double min_distance, + const double tmax, + const NonlinearCCD& nonlinear_ccd, + double& toi) const; +}; + +} // namespace ipc::rigid diff --git a/src/ipc/dynamics/rigid/rigid_contact_hessian.cpp b/src/ipc/dynamics/rigid/rigid_contact_hessian.cpp new file mode 100644 index 000000000..74b5bd967 --- /dev/null +++ b/src/ipc/dynamics/rigid/rigid_contact_hessian.cpp @@ -0,0 +1,174 @@ +#include "rigid_contact_hessian.hpp" + +#include +#include +#include +#include + +#include +#include + +#include + +namespace ipc::rigid { + +template +Eigen::SparseMatrix per_contact_rigid_hessian( + const RigidBodies& bodies, + const std::vector& poses, + const TCollisions& collisions, + const TPotential& potential, + Eigen::ConstRef X, + const double gn_scale, + const double geom_scale, + const PSDProjectionMethod project) +{ + const int dim = bodies.dim(); + const int rb_ndof = poses.empty() ? 0 : poses[0].ndof(); + const int n = int(poses.size()) * rb_ndof; + const auto& E = bodies.edges(); + const auto& F = bodies.faces(); + + if (collisions.size() == 0) { + return Eigen::SparseMatrix(n, n); + } + + using Triplets = std::vector>; + tbb::enumerable_thread_specific storage; + + tbb::parallel_for(size_t(0), collisions.size(), [&](size_t ci) { + Triplets& triplets = storage.local(); + const auto& c = collisions[ci]; + const int nv = c.num_vertices(); + + // Vertex-space stencil derivatives; the interior stencil Hessian is + // left UNPROJECTED (mirrors Rigid IPC's compute_potential_hessian with + // project_hessian_to_psd=false) — only the completed rigid-DOF block is + // projected below. + const auto s_dof = c.dof(X, E, F); + const auto grad_f = potential.gradient(c, s_dof); + const auto hess_f = + potential.hessian(c, s_dof, PSDProjectionMethod::NONE); + + const std::array vids = c.vertex_ids(E, F); + + // Map each stencil vertex to its body; collect the ≤2 unique bodies. + std::array body_ids = { { -1, -1 } }; + std::array local_slot = { { 0, 0, 0, 0 } }; + int nb = 0; + for (int i = 0; i < nv; ++i) { + const index_t b = bodies.vertex_to_body(vids[i]); + int slot = -1; + for (int k = 0; k < nb; ++k) { + if (body_ids[k] == b) { + slot = k; + break; + } + } + if (slot < 0) { + slot = nb; + body_ids[nb++] = b; + } + local_slot[i] = slot; + } + + // Jacobian of the stencil vertices w.r.t. the (≤2) bodies' rigid DOFs. + // A stencil has at most 4 vertices × 3 coordinates = 12 rows, and the + // ≤2 bodies have at most 2 × 6 rigid DOFs = 12 columns, so every local + // matrix here fits in a stack-allocated MatrixMax12d. + MatrixMax12d jac_Vi = MatrixMax12d::Zero(nv * dim, nb * rb_ndof); + for (int i = 0; i < nv; ++i) { + const index_t b = bodies.vertex_to_body(vids[i]); + const index_t lv = vids[i] - bodies.body_vertex_start(b); + jac_Vi.block(i * dim, local_slot[i] * rb_ndof, dim, rb_ndof) = + poses[b].transform_vertex_jacobian( + bodies.body_rest_positions(b).row(lv).transpose()); + } + + // Gauss–Newton term (keeps the two-body coupling). + MatrixMax12d local = gn_scale * (jac_Vi.transpose() * hess_f * jac_Vi); + + // Second-order geometric term on the diagonal body blocks only (a + // vertex of body A has zero second derivative w.r.t. body B's DOFs). + for (int i = 0; i < nv; ++i) { + const auto gi = grad_f.segment(i * dim, dim); + if (gi.isZero()) { + continue; + } + const index_t b = bodies.vertex_to_body(vids[i]); + const index_t lv = vids[i] - bodies.body_vertex_start(b); + const MatrixMax d2V = + poses[b].transform_vertex_hessian( + bodies.body_rest_positions(b).row(lv).transpose()); + const int off = local_slot[i] * rb_ndof; + for (int jj = 0; jj < rb_ndof; ++jj) { + for (int ii = 0; ii < rb_ndof; ++ii) { + local(off + ii, off + jj) += + geom_scale * gi.dot(d2V.col(ii * rb_ndof + jj)); + } + } + } + + // Project the completed local rigid-DOF block (mirrors + // apply_chain_rule). + if (project != PSDProjectionMethod::NONE) { + local = project_to_psd(local, project); + } + + // Scatter into the global rigid-DOF Hessian. + for (int a = 0; a < nb; ++a) { + for (int b2 = 0; b2 < nb; ++b2) { + for (int r = 0; r < rb_ndof; ++r) { + for (int col = 0; col < rb_ndof; ++col) { + const double v = + local(a * rb_ndof + r, b2 * rb_ndof + col); + if (v != 0.0) { + triplets.emplace_back( + body_ids[a] * rb_ndof + r, + body_ids[b2] * rb_ndof + col, v); + } + } + } + } + } + }); + + Triplets all; + size_t total = 0; + for (const auto& t : storage) { + total += t.size(); + } + all.reserve(total); + for (const auto& t : storage) { + all.insert(all.end(), t.begin(), t.end()); + } + + Eigen::SparseMatrix H(n, n); + H.setFromTriplets(all.begin(), all.end()); + return H; +} + +// Explicit instantiations for the two contact potentials used by the demo. +template Eigen::SparseMatrix +per_contact_rigid_hessian( + const RigidBodies&, + const std::vector&, + const NormalCollisions&, + const BarrierPotential&, + Eigen::ConstRef, + const double, + const double, + const PSDProjectionMethod); + +template Eigen::SparseMatrix +per_contact_rigid_hessian( + const RigidBodies&, + const std::vector&, + const TangentialCollisions&, + const FrictionPotential&, + Eigen::ConstRef, + const double, + const double, + const PSDProjectionMethod); + +} // namespace ipc::rigid diff --git a/src/ipc/dynamics/rigid/rigid_contact_hessian.hpp b/src/ipc/dynamics/rigid/rigid_contact_hessian.hpp new file mode 100644 index 000000000..30fda6779 --- /dev/null +++ b/src/ipc/dynamics/rigid/rigid_contact_hessian.hpp @@ -0,0 +1,65 @@ +#pragma once + +#include +#include +#include + +#include +#include + +#include + +namespace ipc::rigid { + +/// @brief Assemble a collision potential's Hessian in rigid DOFs, projecting +/// each contact's local block to PSD — a 1-to-1 port of the original Rigid IPC +/// `apply_chain_rule` (Ferguson et al. 2021). +/// +/// For each collision the local rigid-DOF Hessian spanning its ≤2 bodies is +/// gn_scale · Jᵢᵀ H_stencil Jᵢ + geom_scale · Σ_v (d²Vᵥ/dx²)·grad_stencil,v +/// (Gauss–Newton term with the two-body coupling, plus the indefinite +/// second-order geometric term on the diagonal body blocks). That completed +/// local block is projected with @p project and scattered into the global +/// sparse Hessian. +/// +/// This is a 1-to-1 port of Rigid IPC: the interior vertex-space stencil +/// Hessian is left UNPROJECTED, and only the completed rigid-DOF block is +/// projected — *after* adding the indefinite geometric term. Projecting there +/// (rather than the vertex-space block before the geometric term) is what keeps +/// Newton conditioned on stiff, high-mass-ratio contact (e.g. dense piles), +/// cutting iteration counts from hundreds to a handful. +/// +/// Defined for (NormalCollisions, BarrierPotential) and +/// (TangentialCollisions, FrictionPotential) via explicit instantiation. +/// +/// @param bodies The rigid bodies (collision mesh + per-body transforms). +/// @param poses The current pose of each body. +/// @param collisions The collision set. +/// @param potential The matching potential. +/// @param X Vertex-space quantity the stencil DOFs are read from: world +/// vertices for the barrier, velocities for friction. +/// @param gn_scale Scale applied to the Gauss–Newton term Jᵀ H J. +/// @param geom_scale Scale applied to the geometric term Σ d²V/dx²·grad. +/// @param project PSD projection of each per-contact local block (CLAMP mirrors +/// Rigid IPC; NONE disables projection). +/// @return The potential Hessian in rigid DOFs (size ndof × ndof). +/// +/// @note We assemble with plain triplets here instead of going through +/// Potential::assemble_hessian and a HessianAssembler. Those backends scatter +/// per-vertex dim×dim blocks, but our blocks are per body (6×6 in 3D), which +/// the MeshFEMSparse backend does not support. The tradeoff is small: the +/// result is only (6·#bodies)², so assembly is cheap next to the per-contact +/// derivative evaluation, and we never build the large vertex-space Hessian +/// that the block backend was designed to speed up. +template +Eigen::SparseMatrix per_contact_rigid_hessian( + const RigidBodies& bodies, + const std::vector& poses, + const TCollisions& collisions, + const TPotential& potential, + Eigen::ConstRef X, + const double gn_scale, + const double geom_scale, + const PSDProjectionMethod project); + +} // namespace ipc::rigid diff --git a/src/ipc/dynamics/rigid/rigid_trajectory.cpp b/src/ipc/dynamics/rigid/rigid_trajectory.cpp new file mode 100644 index 000000000..6142dd92a --- /dev/null +++ b/src/ipc/dynamics/rigid/rigid_trajectory.cpp @@ -0,0 +1,34 @@ +#include "rigid_trajectory.hpp" + +namespace ipc::rigid { + +RigidTrajectory::RigidTrajectory( + Eigen::ConstRef rest_position, + const Pose& pose_t0, + const Pose& pose_t1) + : m_rest_position(rest_position) + , m_pose_t0(pose_t0) + , m_pose_t1(pose_t1) + , m_radius(rest_position.norm()) + , m_dtheta_norm((pose_t1.rotation - pose_t0.rotation).norm()) +{ + assert(pose_t0.position.size() == rest_position.size()); + assert(pose_t1.position.size() == rest_position.size()); + assert(pose_t0.rotation.size() == pose_t1.rotation.size()); +} + +VectorMax3d RigidTrajectory::operator()(const double t) const +{ + const Pose pose( + (1 - t) * m_pose_t0.position + t * m_pose_t1.position, + (1 - t) * m_pose_t0.rotation + t * m_pose_t1.rotation); + return pose.rotation_matrix() * m_rest_position + pose.position; +} + +double RigidTrajectory::max_distance_from_linear( + const double t0, const double t1) const +{ + return rigid_chord_deviation(m_radius, (t1 - t0) * m_dtheta_norm); +} + +} // namespace ipc::rigid diff --git a/src/ipc/dynamics/rigid/rigid_trajectory.hpp b/src/ipc/dynamics/rigid/rigid_trajectory.hpp new file mode 100644 index 000000000..4491d4201 --- /dev/null +++ b/src/ipc/dynamics/rigid/rigid_trajectory.hpp @@ -0,0 +1,111 @@ +#pragma once + +#include +#include + +#include + +namespace ipc::rigid { + +/// @brief Bound the deviation of a rigidly-carried point from the chord of its +/// trajectory. +/// +/// Consider a point at body-frame position x̄ carried by a rigid motion with +/// rotation vector interpolated linearly: f(s) = R(θa + s δθ) x̄, s ∈ [0, 1]. +/// The deviation from the endpoint linear interpolant l(s) = (1-s) f(0) + s +/// f(1) is bounded by the classic C² interpolation-error bound +/// ‖f(s) − l(s)‖ ≤ ⅛ max_s ‖f″(s)‖. +/// Using the Fréchet-integral representation of d/ds exp([·]×) — where each +/// factor is an orthogonal matrix (spectral norm 1) times [δθ]× (spectral norm +/// ‖δθ‖) — gives ‖f″(s)‖ ≤ ‖δθ‖² ‖x̄‖ *without any fixed-axis assumption*, so +/// the bound is conservative even when θa and θa + δθ are not parallel. In 2D +/// the axis is fixed and this is the familiar sagitta bound +/// r (1 − cos(δ/2)) ≤ r δ²/8. Since both the curve and its chord lie in the +/// ball of radius ‖x̄‖ about the rotation center, the deviation never exceeds +/// 2‖x̄‖, which caps the bound for large rotations. +/// +/// See also: Redon et al. [2002], "Fast Continuous Collision Detection between +/// Rigid Bodies." +/// +/// @param radius Distance ‖x̄‖ of the point from the rotation center +/// @param dtheta Norm of the rotation-vector change ‖δθ‖ over the interval +/// @return Conservative bound on max_s ‖f(s) − l(s)‖ +inline double rigid_chord_deviation(const double radius, const double dtheta) +{ + return std::min(radius * dtheta * dtheta / 8, 2 * radius); +} + +/// @brief Bound the chord deviation of a point on body j expressed in body i's +/// rest frame. +/// +/// The relative curve is x(t) = Rᵢ(t)ᵀ (Rⱼ(t) x̄ + Δq(t)) with Δq(t) = pⱼ(t) − +/// pᵢ(t) affine in t. Applying the same ⅛ max‖x″‖ interpolation-error bound as +/// rigid_chord_deviation with the product rule x″ = A″u + 2A′u′ + Au″ (A = +/// Rᵢᵀ, u = Rⱼ x̄ + Δq) and the Fréchet bounds ‖A′‖ ≤ ‖δθᵢ‖, ‖A″‖ ≤ ‖δθᵢ‖² +/// gives +/// dev = ⅛ [ (‖δθᵢ‖ + ‖δθⱼ‖)² ‖x̄‖ +/// + ‖δθᵢ‖ (‖δθᵢ‖ max‖Δq‖ + 2 ‖Δq(1) − Δq(0)‖) ]. +/// Both the curve and its chord lie in the ball of radius ‖x̄‖ + max‖Δq‖ about +/// the origin, capping the deviation at twice that. When body i is static +/// (δθᵢ = 0) this degenerates to the world-space rigid_chord_deviation bound. +/// +/// @param radius Distance ‖x̄‖ of the point from body j's rotation center +/// @param dtheta_i Norm of body i's rotation-vector change over the interval +/// @param dtheta_j Norm of body j's rotation-vector change over the interval +/// @param max_translation Maximum of ‖Δq‖ over the interval (attained at an endpoint since Δq is affine) +/// @param translation_change Norm of the relative translation change ‖Δq(1) − Δq(0)‖ over the interval +/// @return Conservative bound on the deviation of the relative curve from its chord +inline double relative_rigid_chord_deviation( + const double radius, + const double dtheta_i, + const double dtheta_j, + const double max_translation, + const double translation_change) +{ + const double dtheta_sum = dtheta_i + dtheta_j; + const double dev = + (dtheta_sum * dtheta_sum * radius + + dtheta_i * (dtheta_i * max_translation + 2 * translation_change)) + / 8; + return std::min(dev, 2 * (radius + max_translation)); +} + +/// @brief The trajectory of a point carried by a rigid body. +/// +/// The body moves from pose_t0 to pose_t1 with position and rotation vector +/// interpolated linearly (matching the incremental potential's pose +/// parameterization): +/// x(t) = R((1−t) θ₀ + t θ₁) x̄ + (1−t) p₀ + t p₁. +class RigidTrajectory : virtual public NonlinearTrajectory { +public: + /// @brief Construct the trajectory of a point on a rigid body. + /// @param rest_position The body-frame (rest) position x̄ of the point + /// @param pose_t0 The pose of the body at the start of the time step + /// @param pose_t1 The pose of the body at the end of the time step + RigidTrajectory( + Eigen::ConstRef rest_position, + const Pose& pose_t0, + const Pose& pose_t1); + + /// @brief Compute the point's position at time t ∈ [0, 1]. + VectorMax3d operator()(const double t) const override; + + /// @brief Conservative bound on the deviation from the linearized trajectory. + /// + /// The translational part is affine in t, so it matches its chord exactly + /// and only the rotational part contributes (see rigid_chord_deviation). + /// + /// @param t0 Start time of the sub-interval + /// @param t1 End time of the sub-interval + double + max_distance_from_linear(const double t0, const double t1) const override; + +private: + VectorMax3d m_rest_position; + Pose m_pose_t0; + Pose m_pose_t1; + double m_radius; ///< ‖x̄‖ (cached) + double m_dtheta_norm; ///< ‖θ₁ − θ₀‖ (cached) +}; + +} // namespace ipc::rigid diff --git a/src/ipc/dynamics/time_integration/CMakeLists.txt b/src/ipc/dynamics/time_integration/CMakeLists.txt new file mode 100644 index 000000000..0f4563d08 --- /dev/null +++ b/src/ipc/dynamics/time_integration/CMakeLists.txt @@ -0,0 +1,9 @@ +set(SOURCES + bdf.cpp + bdf.hpp + time_integrator.cpp + time_integrator.hpp +) + +source_group(TREE "${CMAKE_CURRENT_SOURCE_DIR}" PREFIX "Source Files" FILES ${SOURCES}) +target_sources(ipc_toolkit PRIVATE ${SOURCES}) diff --git a/src/ipc/dynamics/time_integration/bdf.cpp b/src/ipc/dynamics/time_integration/bdf.cpp new file mode 100644 index 000000000..212212576 --- /dev/null +++ b/src/ipc/dynamics/time_integration/bdf.cpp @@ -0,0 +1,112 @@ +#include "bdf.hpp" + +#include + +#include +#include + +namespace ipc::dynamics { + +void BDF::set_target_order(const int target_order) +{ + if (target_order < 1 || target_order > 6) { + log_and_throw_error("BDF order must be 1 ≤ n ≤ 6"); + } + m_target_order = target_order; +} + +const std::vector& BDF::alphas(const int i) +{ + static const std::array, 6> alphas = { { + { 1 }, + { 4.0 / 3.0, -1.0 / 3.0 }, + { 18.0 / 11.0, -9.0 / 11.0, 2.0 / 11.0 }, + { 48.0 / 25.0, -36.0 / 25.0, 16.0 / 25.0, -3.0 / 25.0 }, + { 300.0 / 137.0, -300.0 / 137.0, 200.0 / 137.0, -75.0 / 137.0, + 12.0 / 137.0 }, + { 360.0 / 147.0, -450.0 / 147.0, 400.0 / 147.0, -225.0 / 147.0, + 72.0 / 147.0, -10.0 / 147.0 }, + } }; + assert(i >= 0 && i < alphas.size()); + return alphas[i]; +} + +double BDF::betas(const int i) +{ + static const std::array betas = { { + 1.0, + 2.0 / 3.0, + 6.0 / 11.0, + 12.0 / 25.0, + 60.0 / 137.0, + 60.0 / 147.0, + } }; + assert(i >= 0 && i < betas.size()); + return betas[i]; +} + +Eigen::VectorXd BDF::weighted_sum_x_prevs() const +{ + const std::vector& alpha = alphas(order() - 1); + + Eigen::VectorXd sum = alpha[0] * x_prev(0); + for (int i = 1; i < order(); i++) { + sum += alpha[i] * x_prev(i); + } + return sum; +} + +Eigen::VectorXd BDF::weighted_sum_v_prevs() const +{ + const std::vector& alpha = alphas(order() - 1); + + Eigen::VectorXd sum = alpha[0] * v_prev(0); + for (int i = 1; i < order(); i++) { + sum += alpha[i] * v_prev(i); + } + return sum; +} + +Eigen::VectorXd BDF::predicted_positions() const +{ + return weighted_sum_x_prevs() + + betas(order() - 1) * dt() * weighted_sum_v_prevs(); +} + +Eigen::VectorXd BDF::compute_position(Eigen::ConstRef v) const +{ + return weighted_sum_x_prevs() + beta_dt() * v; +} + +Eigen::VectorXd BDF::compute_velocity(Eigen::ConstRef x) const +{ + return (x - weighted_sum_x_prevs()) / beta_dt(); +} + +Eigen::VectorXd +BDF::compute_acceleration(Eigen::ConstRef v) const +{ + return (v - weighted_sum_v_prevs()) / beta_dt(); +} + +double BDF::acceleration_scaling() const +{ + const double beta_dt = this->beta_dt(); + return beta_dt * beta_dt; +} + +double BDF::dvdx(const unsigned prev_ti) const +{ + if (prev_ti == 0) { + return 1 / beta_dt(); + } + // prev_ti = i ≥ 1 refers to x_prev(i - 1), whose weight is αᵢ₋₁. + if (int(prev_ti) > order()) { + return 0; + } + return -alphas(order() - 1)[prev_ti - 1] / beta_dt(); +} + +double BDF::beta_dt() const { return betas(order() - 1) * dt(); } + +} // namespace ipc::dynamics diff --git a/src/ipc/dynamics/time_integration/bdf.hpp b/src/ipc/dynamics/time_integration/bdf.hpp new file mode 100644 index 000000000..5d903fb43 --- /dev/null +++ b/src/ipc/dynamics/time_integration/bdf.hpp @@ -0,0 +1,92 @@ +#pragma once + +#include + +namespace ipc::dynamics { + +/// @brief Backward Differentiation Formulas (BDF) of order 1 ≤ n ≤ 6. +/// \f[ +/// x^{t+1} = \left(\sum_{i=0}^{n-1} \alpha_i x^{t-i}\right) + \beta\Delta +/// t\, v^{t+1}, \quad v^{t+1} = \left(\sum_{i=0}^{n-1} \alpha_i +/// v^{t-i}\right) + \beta\Delta t\, a^{t+1} +/// \f] +/// BDF-1 is the standard implicit (backward) Euler method. +/// @see https://en.wikipedia.org/wiki/Backward_differentiation_formula +class BDF : public ImplicitTimeIntegrator { +public: + BDF() = default; + + /// @brief Construct a BDF integrator of the given target order. + explicit BDF(const int target_order) { set_target_order(target_order); } + + /// @brief Predicted positions + /// \f$\hat{x} = \sum_i \alpha_i x^{t-i} + \beta\Delta t \sum_i \alpha_i + /// v^{t-i}\f$. + Eigen::VectorXd predicted_positions() const override; + + /// @brief \f$x = \sum_i \alpha_i x^{t-i} + \beta\Delta t\, v\f$. + Eigen::VectorXd + compute_position(Eigen::ConstRef v) const override; + + /// @brief \f$v = (x - \sum_i \alpha_i x^{t-i}) / (\beta\Delta t)\f$. + Eigen::VectorXd + compute_velocity(Eigen::ConstRef x) const override; + + /// @brief \f$a = (v - \sum_i \alpha_i v^{t-i}) / (\beta\Delta t)\f$. + Eigen::VectorXd + compute_acceleration(Eigen::ConstRef v) const override; + + /// @brief \f$(\beta\Delta t)^2\f$. + double acceleration_scaling() const override; + + /// @brief \f$\beta\Delta t\f$. + double velocity_scaling() const override { return beta_dt(); } + + /// @brief The \f$\alpha\f$ coefficients of the current order. + std::vector velocity_history_weights() const override + { + return alphas(order() - 1); + } + + /// @brief \f$\partial v/\partial x = 1/(\beta\Delta t)\f$ for prev_ti = 0 + /// and \f$\partial v/\partial x^{t-i} = -\alpha_i/(\beta\Delta t)\f$ for + /// prev_ti = i + 1 (x^{t-i} = x_prev(i)); zero beyond the current order. + double dvdx(const unsigned prev_ti = 0) const override; + + /// @brief The requested (maximum) BDF order. + int target_order() const { return m_target_order; } + + /// @brief Set the requested BDF order (1 ≤ n ≤ 6). + /// @note Must be set before init(); it determines the history buffer size. + void set_target_order(const int target_order); + + /// @brief The current order (min of the target and the available history). + int order() const + { + return std::min(m_target_order, available_steps()); + } + + /// @brief \f$\sum_{i=0}^{n-1} \alpha_i x^{t-i}\f$. + Eigen::VectorXd weighted_sum_x_prevs() const; + + /// @brief \f$\sum_{i=0}^{n-1} \alpha_i v^{t-i}\f$. + Eigen::VectorXd weighted_sum_v_prevs() const; + + /// @brief \f$\beta\Delta t\f$. + double beta_dt() const; + + /// @brief The \f$\alpha\f$ coefficients for order-`i+1` BDF. + static const std::vector& alphas(const int i); + + /// @brief The \f$\beta\f$ coefficient for order-`i+1` BDF. + static double betas(const int i); + +protected: + unsigned max_steps() const override { return unsigned(m_target_order); } + +private: + /// @brief The requested (maximum) BDF order. + int m_target_order = 1; +}; + +} // namespace ipc::dynamics diff --git a/src/ipc/dynamics/time_integration/time_integrator.cpp b/src/ipc/dynamics/time_integration/time_integrator.cpp new file mode 100644 index 000000000..c70e6e88c --- /dev/null +++ b/src/ipc/dynamics/time_integration/time_integrator.cpp @@ -0,0 +1,142 @@ +#include "time_integrator.hpp" + +#include +#include + +#include + +namespace ipc::dynamics { + +namespace { + /// @brief Modulus that is always non-negative (unlike %). + inline int true_mod(const int a, const int b) { return (a % b + b) % b; } +} // namespace + +void ImplicitTimeIntegrator::init( + Eigen::ConstRef x_prev, + Eigen::ConstRef v_prev, + Eigen::ConstRef a_prev, + const size_t num_bodies, + const int pos_ndof, + const int rot_ndof) +{ + assert(x_prev.size() == v_prev.size()); + assert(v_prev.size() == a_prev.size()); + + m_num_bodies = num_bodies; + + if (pos_ndof > 0 && rot_ndof > 0) { + assert(x_prev.size() == num_bodies * size_t(pos_ndof + rot_ndof)); + m_pos_ndof = pos_ndof, m_rot_ndof = rot_ndof; + } else { + m_pos_ndof = 3, m_rot_ndof = 9; // Default to the 3D case + // NOTE: This inference is ambiguous (e.g., a multiple of 4 bodies in + // 2D); pass the layout explicitly when decoding poses matters. + if (x_prev.size() != num_bodies * (m_pos_ndof + m_rot_ndof)) { + m_pos_ndof = 2, m_rot_ndof = 4; // 2D case: [p (2); vec(A) (4)] + } + } + // If neither body layout matches exactly, x_prev is not affine-shaped + // (e.g., a generic state used only for the raw multistep formulas below); + // pose()/predicted_pose() are only meaningful when the layout does fit, + // so we don't require it here. + + m_available_steps = 1; + m_current_ptr = 0; + + m_x_prevs.resize(x_prev.size(), max_steps()); + m_v_prevs.resize(v_prev.size(), max_steps()); + m_a_prevs.resize(a_prev.size(), max_steps()); + + m_x_prevs.col(m_current_ptr) = x_prev; + m_v_prevs.col(m_current_ptr) = v_prev; + m_a_prevs.col(m_current_ptr) = a_prev; +} + +void ImplicitTimeIntegrator::update(Eigen::ConstRef x) +{ + assert(x.size() == m_x_prevs.rows()); + + const Eigen::VectorXd v = compute_velocity(x); + const Eigen::VectorXd a = compute_acceleration(v); + + m_current_ptr = (m_current_ptr + 1) % max_steps(); + + m_x_prevs.col(m_current_ptr) = x; + m_v_prevs.col(m_current_ptr) = v; + m_a_prevs.col(m_current_ptr) = a; + + m_available_steps = std::min(m_available_steps + 1, max_steps()); +} + +void ImplicitTimeIntegrator::set_dt(const double dt) +{ + m_dt = dt; + m_available_steps = 1; // Reset the history: a new dt invalidates it. +} + +Eigen::Map +ImplicitTimeIntegrator::x_prev(const int i) const +{ + assert(i < available_steps()); + const int ptr = true_mod(int(m_current_ptr) - i, max_steps()); + return Eigen::Map( + m_x_prevs.data() + ptr * m_x_prevs.rows(), m_x_prevs.rows()); +} + +Eigen::Map +ImplicitTimeIntegrator::v_prev(const int i) const +{ + assert(i < available_steps()); + const int ptr = true_mod(int(m_current_ptr) - i, max_steps()); + return Eigen::Map( + m_v_prevs.data() + ptr * m_v_prevs.rows(), m_v_prevs.rows()); +} + +Eigen::Map +ImplicitTimeIntegrator::a_prev(const int i) const +{ + assert(i < available_steps()); + const int ptr = true_mod(int(m_current_ptr) - i, max_steps()); + return Eigen::Map( + m_a_prevs.data() + ptr * m_a_prevs.rows(), m_a_prevs.rows()); +} + +affine::Pose ImplicitTimeIntegrator::pose( + Eigen::ConstRef x, const size_t i) const +{ + affine::Pose pose; + + pose.position = x.segment(i * (m_pos_ndof + m_rot_ndof), m_pos_ndof); + if (m_rot_ndof == 4) { + // 2D: vec(A) column-major. + pose.rotation = + x.segment(i * (m_pos_ndof + m_rot_ndof) + m_pos_ndof, m_rot_ndof) + .reshaped(2, 2); + } else { + assert(m_rot_ndof == 9); + pose.rotation = + x.segment(i * (m_pos_ndof + m_rot_ndof) + m_pos_ndof, m_rot_ndof) + .reshaped(3, 3); + } + + return pose; +} + +std::vector ImplicitTimeIntegrator::predicted_pose() const +{ + const Eigen::VectorXd x_hat = predicted_positions(); + + std::vector predicted(m_num_bodies); + tbb::parallel_for( + tbb::blocked_range(0, m_num_bodies), + [&](const tbb::blocked_range& r) { + for (size_t i = r.begin(); i < r.end(); ++i) { + predicted[i] = pose(x_hat, i); + } + }); + + return predicted; +} + +} // namespace ipc::dynamics diff --git a/src/ipc/dynamics/time_integration/time_integrator.hpp b/src/ipc/dynamics/time_integration/time_integrator.hpp new file mode 100644 index 000000000..5ecd1f4c5 --- /dev/null +++ b/src/ipc/dynamics/time_integration/time_integrator.hpp @@ -0,0 +1,161 @@ +#pragma once + +#include +#include + +#include + +#include + +namespace ipc::dynamics { + +/// @brief Implicit time integrator of the second-order ODE M ẍ = f(x). +/// +/// The step is posed as the minimization of an incremental potential +/// \f[ +/// E(x) = \tfrac12 (x - \hat{x})^\top M (x - \hat{x}) + s\, V(x), +/// \f] +/// where \f$\hat{x} =\f$ predicted_positions() and \f$s =\f$ +/// acceleration_scaling(); the minimizer satisfies +/// \f$M(x-\hat{x}) = s\, f(x)\f$ with \f$f = -\nabla V\f$. +/// +/// A ring buffer of the previous positions, velocities, and accelerations is +/// stored so multi-step schemes (e.g., BDF-n) can be implemented. The state is +/// affine-shaped ([p; vec(Q) column-major] per body); the pose() helpers decode +/// it into affine::Pose. +class ImplicitTimeIntegrator { +public: + virtual ~ImplicitTimeIntegrator() = default; + + /// @brief Initialize the integrator with the previous x, v, and a. + /// @param x_prev Previous positions. + /// @param v_prev Previous velocities. + /// @param a_prev Previous accelerations. + /// @param num_bodies Number of bodies (used to decode the affine-shaped + /// state into poses). + /// @param pos_ndof Positional DOFs per body (2 or 3); -1 infers the + /// layout from the state size (fragile for 2D — pass it explicitly). + /// @param rot_ndof Rotational DOFs per body (4 = vec(2×2), 9 = vec(3×3)); + /// -1 infers. + /// @note For multi-step schemes, the scheme's parameters (e.g., the BDF + /// order) must be set before calling init() since they determine the + /// history buffer size. + void init( + Eigen::ConstRef x_prev, + Eigen::ConstRef v_prev, + Eigen::ConstRef a_prev, + const size_t num_bodies, + const int pos_ndof = -1, + const int rot_ndof = -1); + + /// @brief Advance the stored history with a newly-accepted state x. + /// @param x The new positions. + void update(Eigen::ConstRef x); + + // ----------------------------------------------------------------------- + // Scheme-specific integration formulas + // ----------------------------------------------------------------------- + + /// @brief Predicted positions \f$\hat{x}\f$ used in the inertia term. + virtual Eigen::VectorXd predicted_positions() const = 0; + + /// @brief Compute the current position given the current velocity. + virtual Eigen::VectorXd + compute_position(Eigen::ConstRef v) const = 0; + + /// @brief Compute the current velocity given the current position. + virtual Eigen::VectorXd + compute_velocity(Eigen::ConstRef x) const = 0; + + /// @brief Compute the current acceleration given the current velocity. + virtual Eigen::VectorXd + compute_acceleration(Eigen::ConstRef v) const = 0; + + /// @brief Scaling of the potential forces in the incremental potential + /// (\f$(\beta\Delta t)^2\f$ for BDF; \f$\Delta t^2\f$ for implicit Euler). + virtual double acceleration_scaling() const = 0; + + /// @brief First-order (velocity) force scaling (\f$\beta\Delta t\f$ for BDF). + virtual double velocity_scaling() const = 0; + + /// @brief Weights \f$w_i\f$ of the history combination in the velocity + /// formula \f$v = (x - \sum_i w_i x^{t-i}) / \text{velocity\_scaling()}\f$. + /// @return One weight per used history step (most recent first). + virtual std::vector velocity_history_weights() const = 0; + + /// @brief Derivative of the velocity with respect to a position in its + /// formula: prev_ti = 0 is the current position, and prev_ti = i ≥ 1 is + /// x_prev(i - 1). + virtual double dvdx(const unsigned prev_ti = 0) const = 0; + + // ----------------------------------------------------------------------- + // Time step + // ----------------------------------------------------------------------- + + /// @brief The time step size. + double dt() const { return m_dt; } + + /// @brief Set the time step size (resets the accumulated history). + void set_dt(const double dt); + + // ----------------------------------------------------------------------- + // History access (0 → most recent, 1 → previous, ...) + // ----------------------------------------------------------------------- + + /// @brief The i-th previous positions. + Eigen::Map x_prev(const int i = 0) const; + /// @brief The i-th previous velocities. + Eigen::Map v_prev(const int i = 0) const; + /// @brief The i-th previous accelerations. + Eigen::Map a_prev(const int i = 0) const; + + // ----------------------------------------------------------------------- + // Pose decoding of the affine-shaped state + // ----------------------------------------------------------------------- + + /// @brief Decode body i's affine pose from a state vector x. + affine::Pose pose(Eigen::ConstRef x, const size_t i) const; + + /// @brief Decode body i's affine pose from the most recent state. + affine::Pose pose(const size_t i) const { return pose(x_prev(0), i); } + + /// @brief Decode all bodies' poses from predicted_positions(). + std::vector predicted_pose() const; + + /// @brief Number of bodies. + size_t num_bodies() const { return m_num_bodies; } + /// @brief Position DOFs per body (2 or 3). + size_t pos_ndof() const { return m_pos_ndof; } + /// @brief Rotation DOFs per body (4 = vec(2×2), 9 = vec(3×3)). + size_t rot_ndof() const { return m_rot_ndof; } + +protected: + /// @brief The current number of stored history steps. + int available_steps() const { return m_available_steps; } + + /// @brief The maximum number of history steps the scheme requires. + virtual unsigned max_steps() const = 0; + +private: + /// @brief Ring buffers of the previous positions/velocities/accelerations + /// (one column per stored step). + Eigen::MatrixXd m_x_prevs; + Eigen::MatrixXd m_v_prevs; + Eigen::MatrixXd m_a_prevs; + + /// @brief Time step size. + double m_dt = 1.0 / 60.0; + + /// @brief Column index of the most recent stored step. + unsigned m_current_ptr = 0; + + /// @brief Number of stored history steps (≤ max_steps()). + unsigned m_available_steps = 1; + + /// @brief Number of bodies and the per-body state layout. + size_t m_num_bodies = 0; + size_t m_pos_ndof = 3; + size_t m_rot_ndof = 9; +}; + +} // namespace ipc::dynamics diff --git a/src/ipc/dynamics/to_affine.cpp b/src/ipc/dynamics/to_affine.cpp new file mode 100644 index 000000000..7bbc27416 --- /dev/null +++ b/src/ipc/dynamics/to_affine.cpp @@ -0,0 +1,207 @@ +#include "to_affine.hpp" + +#include + +namespace ipc::dynamics { + +namespace { + /// Extract the dense (n×n) diagonal block starting at @p offset from a + /// (column-major) sparse matrix that is block diagonal per body. + /// @note n ≤ affine_ndof ≤ 12. + MatrixMax12d extract_diagonal_block( + const Eigen::SparseMatrix& H, const int offset, const int n) + { + MatrixMax12d block = MatrixMax12d::Zero(n, n); + for (int c = 0; c < n; ++c) { + for (Eigen::SparseMatrix::InnerIterator it(H, offset + c); + it; ++it) { + const int r = int(it.row()) - offset; + if (r >= 0 && r < n) { + block(r, c) = it.value(); + } + } + } + return block; + } + + /// Assemble a block-diagonal sparse matrix from per-body dense blocks. + Eigen::SparseMatrix assemble_block_diagonal( + const std::vector& blocks, const int block_ndof) + { + std::vector> triplets; + triplets.reserve(blocks.size() * block_ndof * block_ndof); + for (size_t i = 0; i < blocks.size(); ++i) { + const int offset = int(i) * block_ndof; + for (int c = 0; c < block_ndof; ++c) { + for (int r = 0; r < block_ndof; ++r) { + const double v = blocks[i](r, c); + if (v != 0.0) { + triplets.emplace_back(offset + r, offset + c, v); + } + } + } + } + Eigen::SparseMatrix H( + blocks.size() * block_ndof, blocks.size() * block_ndof); + H.setFromTriplets(triplets.begin(), triplets.end()); + return H; + } +} // namespace + +// ---- Rigid to-affine map --------------------------------------------- + +Eigen::VectorXd +RigidToAffine::to_affine(Eigen::ConstRef x) const +{ + const int d = m_dim, rndof = reduced_ndof(), andof = affine_ndof(); + assert(x.size() == int(m_num_bodies) * rndof); + + Eigen::VectorXd y(andof * m_num_bodies); + for (size_t i = 0; i < m_num_bodies; ++i) { + affine::Pose pose; + const VectorMax3d theta = x.segment(i * rndof + d, rot_ndof()); + pose.set_rotation_vector(theta); + y.segment(i * andof, d) = x.segment(i * rndof, d); + y.segment(i * andof + d, d * d) = pose.rotation.reshaped(); + } + return y; +} + +Eigen::VectorXd +RigidToAffine::from_affine(Eigen::ConstRef y) const +{ + const int d = m_dim, rndof = reduced_ndof(), andof = affine_ndof(); + assert(y.size() == int(m_num_bodies) * andof); + + Eigen::VectorXd x(rndof * m_num_bodies); + for (size_t i = 0; i < m_num_bodies; ++i) { + affine::Pose pose; + pose.rotation = y.segment(i * andof + d, d * d).reshaped(d, d); + x.segment(i * rndof, d) = y.segment(i * andof, d); + x.segment(i * rndof + d, rot_ndof()) = pose.rotation_vector(); + } + return x; +} + +std::vector +RigidToAffine::poses(Eigen::ConstRef x) const +{ + const int d = m_dim, rndof = reduced_ndof(); + std::vector out(m_num_bodies); + for (size_t i = 0; i < m_num_bodies; ++i) { + out[i].position = x.segment(i * rndof, d); + const VectorMax3d theta = x.segment(i * rndof + d, rot_ndof()); + out[i].set_rotation_vector(theta); + } + return out; +} + +Eigen::VectorXd RigidToAffine::apply_gradient( + Eigen::ConstRef x, + Eigen::ConstRef g_affine) const +{ + const int d = m_dim, rot = rot_ndof(), rndof = reduced_ndof(), + andof = affine_ndof(); + assert(g_affine.size() == int(m_num_bodies) * andof); + + Eigen::VectorXd g(rndof * m_num_bodies); + for (size_t i = 0; i < m_num_bodies; ++i) { + const VectorMax3d theta = x.segment(i * rndof + d, rot); + const MatrixMax dQ = + rigid::rotation_to_matrix_jacobian(theta); + + // Position block: dy/dx = I + g.segment(i * rndof, d) = g_affine.segment(i * andof, d); + // Rotation block: (dvec(Q)/dθ)ᵀ · g_A + g.segment(i * rndof + d, rot) = + dQ.transpose() * g_affine.segment(i * andof + d, d * d); + } + return g; +} + +Eigen::SparseMatrix RigidToAffine::apply_hessian( + Eigen::ConstRef x, + Eigen::ConstRef g_affine, + const Eigen::SparseMatrix& H_affine, + const PSDProjectionMethod project_hessian_to_psd) const +{ + const int d = m_dim, rot = rot_ndof(), rndof = reduced_ndof(), + andof = affine_ndof(); + assert(H_affine.rows() == int(m_num_bodies) * andof); + + std::vector blocks(m_num_bodies); + for (size_t i = 0; i < m_num_bodies; ++i) { + const MatrixMax12d Ha = + extract_diagonal_block(H_affine, int(i) * andof, andof); + + const VectorMax3d theta = x.segment(i * rndof + d, rot); + const MatrixMax dQ = + rigid::rotation_to_matrix_jacobian(theta); + const MatrixMax d2Q = + rigid::rotation_to_matrix_hessian(theta); + + MatrixMax6d H = MatrixMax6d::Zero(rndof, rndof); + // pos-pos: dy/dp = I, so this block passes straight through. + H.topLeftCorner(d, d) = Ha.topLeftCorner(d, d); + // pos-rot and rot-pos: zero for every body term we sum today, but we + // still chain them so the result is exact for any affine Hessian. + H.block(0, d, d, rot) = Ha.block(0, d, d, d * d) * dQ; + H.block(d, 0, rot, d) = dQ.transpose() * Ha.block(d, 0, d * d, d); + // rot-rot: (dQ)ᵀ Haa (dQ) + Σₖ (g_A)ₖ d²Qₖ/dθ² + H.block(d, d, rot, rot) = + dQ.transpose() * Ha.block(d, d, d * d, d * d) * dQ + + (d2Q.transpose() * g_affine.segment(i * andof + d, d * d)) + .reshaped(rot, rot); + + // We project the whole per-body block instead of only rot-rot. When + // the coupling is zero the two agree, because the position block is + // already PSD. When it is not zero, projecting only rot-rot would not + // make the block PSD. The extra cost is one eigendecomposition of a + // ≤6×6 matrix per body. + blocks[i] = project_to_psd(H, project_hessian_to_psd); + } + return assemble_block_diagonal(blocks, rndof); +} + +// ---- Affine (identity) to-affine map --------------------------------- + +std::vector +AffineToAffine::poses(Eigen::ConstRef x) const +{ + const int d = m_dim, andof = affine_ndof(); + std::vector out(m_num_bodies); + for (size_t i = 0; i < m_num_bodies; ++i) { + out[i].position = x.segment(i * andof, d); + out[i].rotation = x.segment(i * andof + d, d * d).reshaped(d, d); + } + return out; +} + +Eigen::SparseMatrix AffineToAffine::apply_hessian( + Eigen::ConstRef x, + Eigen::ConstRef g_affine, + const Eigen::SparseMatrix& H_affine, + const PSDProjectionMethod project_hessian_to_psd) const +{ + // x ≡ y, so the Hessian is unchanged apart from the (optional) PSD + // projection of the affine-matrix block of each body. + if (project_hessian_to_psd == PSDProjectionMethod::NONE) { + return H_affine; + } + + const int d = m_dim, andof = affine_ndof(); + std::vector blocks(m_num_bodies); + for (size_t i = 0; i < m_num_bodies; ++i) { + MatrixMax12d Ha = + extract_diagonal_block(H_affine, int(i) * andof, andof); + // Project the vec(A) block (position block is PSD by construction). + const MatrixMax A_block = + Ha.bottomRightCorner(d * d, d * d); + Ha.bottomRightCorner(d * d, d * d) = + project_to_psd(A_block, project_hessian_to_psd); + blocks[i] = Ha; + } + return assemble_block_diagonal(blocks, andof); +} + +} // namespace ipc::dynamics diff --git a/src/ipc/dynamics/to_affine.hpp b/src/ipc/dynamics/to_affine.hpp new file mode 100644 index 000000000..e72856611 --- /dev/null +++ b/src/ipc/dynamics/to_affine.hpp @@ -0,0 +1,148 @@ +#pragma once + +#include +#include + +#include +#include + +#include + +namespace ipc::dynamics { + +/// @brief The map from the optimization DOFs x to the affine coordinates +/// y = [p; vec(A) column-major] per body, and the chain rule that pulls a +/// potential's affine-coordinate derivatives back to x. +/// +/// Two parameterizations share the same affine-coordinate potentials and differ +/// only here: +/// - Rigid (RBD): x = [p; θ] per body, A = Q(θ) (a change of variables that +/// enforces A ∈ SO(dim) exactly). +/// - Affine (ABD): x = y (the identity), A optimized directly. +/// +/// The affine and vertex representations are block diagonal per body; every +/// method operates block-by-body on stacked vectors/Hessians. +class ToAffine { +public: + virtual ~ToAffine() = default; + + int dim() const { return m_dim; } + size_t num_bodies() const { return m_num_bodies; } + int pos_ndof() const { return m_dim; } + /// @brief Reduced rotation DOFs per body (1|3 for rigid, dim² for affine). + virtual int rot_ndof() const = 0; + int reduced_ndof() const { return pos_ndof() + rot_ndof(); } + int affine_ndof() const { return m_dim + m_dim * m_dim; } + /// @brief Whether x ≡ y (affine parameterization). + virtual bool is_identity() const { return false; } + + /// @brief Affine coordinates y(x): [p; vec(A)] per body. + virtual Eigen::VectorXd + to_affine(Eigen::ConstRef x) const = 0; + + /// @brief Reduced DOFs x(y). For rigid this is the log map (canonical θ). + virtual Eigen::VectorXd + from_affine(Eigen::ConstRef y) const = 0; + + /// @brief Per-body affine poses at x (exact for rigid: A = R(θ)). + virtual std::vector + poses(Eigen::ConstRef x) const = 0; + + /// @brief Pull an affine-coordinate gradient back to x: (dy/dx)ᵀ g. + /// @param x The reduced DOFs. + /// @param g_affine The gradient in affine coordinates (size affine_ndof·N). + /// @return The gradient in reduced coordinates (size reduced_ndof·N). + virtual Eigen::VectorXd apply_gradient( + Eigen::ConstRef x, + Eigen::ConstRef g_affine) const = 0; + + /// @brief Pull an affine-coordinate Hessian back to x: + /// (dy/dx)ᵀ H (dy/dx) + Σₖ (g)ₖ d²yₖ/dx², projecting the rotation block to + /// PSD once (per body) with @p project_hessian_to_psd. + /// @note Assumes H_affine is block diagonal per body. + virtual Eigen::SparseMatrix apply_hessian( + Eigen::ConstRef x, + Eigen::ConstRef g_affine, + const Eigen::SparseMatrix& H_affine, + const PSDProjectionMethod project_hessian_to_psd = + PSDProjectionMethod::NONE) const = 0; + +protected: + ToAffine(const int dim, const size_t num_bodies) + : m_dim(dim) + , m_num_bodies(num_bodies) + { + } + + int m_dim; + size_t m_num_bodies; +}; + +/// @brief The rigid to-affine map x = [p; θ] → y = [p; vec(Q(θ))]. +class RigidToAffine : public ToAffine { +public: + RigidToAffine(const int dim, const size_t num_bodies) + : ToAffine(dim, num_bodies) + { + } + + int rot_ndof() const override { return m_dim == 2 ? 1 : 3; } + + Eigen::VectorXd + to_affine(Eigen::ConstRef x) const override; + Eigen::VectorXd + from_affine(Eigen::ConstRef y) const override; + std::vector + poses(Eigen::ConstRef x) const override; + + Eigen::VectorXd apply_gradient( + Eigen::ConstRef x, + Eigen::ConstRef g_affine) const override; + + Eigen::SparseMatrix apply_hessian( + Eigen::ConstRef x, + Eigen::ConstRef g_affine, + const Eigen::SparseMatrix& H_affine, + const PSDProjectionMethod project_hessian_to_psd = + PSDProjectionMethod::NONE) const override; +}; + +/// @brief The identity to-affine map x ≡ y (affine bodies). +class AffineToAffine : public ToAffine { +public: + AffineToAffine(const int dim, const size_t num_bodies) + : ToAffine(dim, num_bodies) + { + } + + int rot_ndof() const override { return m_dim * m_dim; } + bool is_identity() const override { return true; } + + Eigen::VectorXd to_affine(Eigen::ConstRef x) const override + { + return x; + } + Eigen::VectorXd + from_affine(Eigen::ConstRef y) const override + { + return y; + } + std::vector + poses(Eigen::ConstRef x) const override; + + Eigen::VectorXd apply_gradient( + Eigen::ConstRef x, + Eigen::ConstRef g_affine) const override + { + return g_affine; + } + + Eigen::SparseMatrix apply_hessian( + Eigen::ConstRef x, + Eigen::ConstRef g_affine, + const Eigen::SparseMatrix& H_affine, + const PSDProjectionMethod project_hessian_to_psd = + PSDProjectionMethod::NONE) const override; +}; + +} // namespace ipc::dynamics diff --git a/src/ipc/esp/collisions/esp_quadrature.hpp b/src/ipc/esp/collisions/esp_quadrature.hpp index 1a8e429ca..031cd18b4 100644 --- a/src/ipc/esp/collisions/esp_quadrature.hpp +++ b/src/ipc/esp/collisions/esp_quadrature.hpp @@ -1,5 +1,6 @@ #pragma once +#include #include #include @@ -699,11 +700,6 @@ lobatto_compute(int n1, std::vector& x, std::vector& w) Output, double W[N], the weights. */ { - // M_PI is not standard C++: MSVC only defines it when _USE_MATH_DEFINES is - // set before , which a consumer of this header cannot be relied on - // to do. - static constexpr double pi_v = 3.14159265358979323846; - int i; int j; double test; @@ -731,7 +727,7 @@ lobatto_compute(int n1, std::vector& x, std::vector& w) Initial estimate for the abscissas is the Chebyshev-Gauss-Lobatto nodes. */ for (i = 0; i < n; i++) { - x[i] = cos(pi_v * static_cast(i) / static_cast(n - 1)); + x[i] = cos(PI * static_cast(i) / static_cast(n - 1)); } std::vector xold(n); diff --git a/src/ipc/geometry/normal.hpp b/src/ipc/geometry/normal.hpp index 39d1c6d5b..c76267ead 100644 --- a/src/ipc/geometry/normal.hpp +++ b/src/ipc/geometry/normal.hpp @@ -541,6 +541,20 @@ cross_product_matrix(const Eigen::MatrixBase& v) return detail::cross_product_matrix(v); } +/// @brief Cross product matrix for a 2D (scalar) angular quantity. +/// +/// In 2D an angular velocity or torque is a scalar ω about the out-of-plane +/// axis, and ω × v = ω S v with the skew generator S = [[0, −1], [1, 0]]. +/// +/// @param omega The scalar angular quantity. +/// @return The 2×2 cross product matrix ω S. +inline Eigen::Matrix2d cross_product_matrix(const double omega) +{ + Eigen::Matrix2d m; + m << 0, -omega, omega, 0; + return m; +} + /// @brief Computes the unnormalized normal vector of a point-line pair. /// @param p The vertex position. /// @param e0 The start position of the line. diff --git a/src/ipc/io/CMakeLists.txt b/src/ipc/io/CMakeLists.txt new file mode 100644 index 000000000..24384864a --- /dev/null +++ b/src/ipc/io/CMakeLists.txt @@ -0,0 +1,15 @@ +set(SOURCES + write_candidates_obj.cpp + write_candidates_obj.hpp +) + +if(IPC_TOOLKIT_WITH_GLTF) + list(APPEND SOURCES + read_gltf.cpp + read_gltf.hpp + write_gltf.cpp + write_gltf.hpp + ) +endif() + +target_sources(ipc_toolkit PRIVATE ${SOURCES}) \ No newline at end of file diff --git a/src/ipc/io/read_gltf.cpp b/src/ipc/io/read_gltf.cpp new file mode 100644 index 000000000..fdebb7251 --- /dev/null +++ b/src/ipc/io/read_gltf.cpp @@ -0,0 +1,419 @@ +#include "read_gltf.hpp" + +#include +#include // nearest_rotation +#include + +#include +#include +#include +#include + +#include +#include +#include + +namespace ipc::rigid { + +namespace { + + /// @brief The node's transform relative to its parent. + /// + /// glTF stores this either as a column-major 4×4 matrix or as separate + /// translation, rotation, and scale (TRS), applied as T·R·S. + Eigen::Affine3d local_transform(const tinygltf::Node& node) + { + Eigen::Affine3d transform = Eigen::Affine3d::Identity(); + if (node.matrix.size() == 16) { + transform.matrix() = + Eigen::Map(node.matrix.data()); + return transform; + } + if (node.translation.size() == 3) { + transform.translate( + Eigen::Vector3d( + node.translation[0], node.translation[1], + node.translation[2])); + } + if (node.rotation.size() == 4) { + // NOTE: glTF uses (x, y, z, w) + transform.rotate( + Eigen::Quaterniond( + node.rotation[3], node.rotation[0], node.rotation[1], + node.rotation[2])); + } + if (node.scale.size() == 3) { + transform.scale( + Eigen::Vector3d(node.scale[0], node.scale[1], node.scale[2])); + } + return transform; + } + + /// @brief Append one primitive's elements to (V, F, E). + /// + /// Triangles go to F and lines to E (points add vertices only). The + /// primitive's indices are offset by the vertices already in V, so a + /// multi-primitive mesh becomes one indexed mesh. + /// + /// @return False (with a warning) if the primitive cannot be read as a + /// dense float triangle, line, or point list; V, F, and E are then + /// left unchanged. + bool extract_mesh_data( + const tinygltf::Model& model, + const tinygltf::Primitive& primitive, + Eigen::MatrixXd& V, + Eigen::MatrixXi& F, + Eigen::MatrixXi& E) + { + // tinygltf reports an unspecified mode as -1 (glTF's default is + // TRIANGLES). + int simplex_size; + switch (primitive.mode) { + case -1: + case TINYGLTF_MODE_TRIANGLES: + simplex_size = 3; + break; + case TINYGLTF_MODE_LINE: + simplex_size = 2; + break; + case TINYGLTF_MODE_POINTS: + simplex_size = 1; + break; + default: + logger().warn( + "[GLTF] skipping primitive with mode {} (only triangles, " + "lines, and points are supported)", + primitive.mode); + return false; + } + + // --- 1. Vertices (POSITION) --- + const auto position = primitive.attributes.find("POSITION"); + if (position == primitive.attributes.end()) { + logger().warn("[GLTF] skipping primitive without POSITION"); + return false; + } + const tinygltf::Accessor& vAccessor = model.accessors[position->second]; + if (vAccessor.componentType != TINYGLTF_COMPONENT_TYPE_FLOAT + || vAccessor.type != TINYGLTF_TYPE_VEC3 || vAccessor.bufferView < 0 + || vAccessor.sparse.isSparse) { + logger().warn( + "[GLTF] skipping primitive whose POSITION is not a dense float " + "VEC3 accessor"); + return false; + } + const tinygltf::BufferView& vBufferView = + model.bufferViews[vAccessor.bufferView]; + const tinygltf::Buffer& vBuffer = model.buffers[vBufferView.buffer]; + + // The stride is usually 3 floats, but interleaved data is larger. + const int vByteStride = vAccessor.ByteStride(vBufferView); + if (vByteStride <= 0) { + logger().warn("[GLTF] skipping primitive with an invalid stride"); + return false; + } + const unsigned char* vData = + &vBuffer.data[vBufferView.byteOffset + vAccessor.byteOffset]; + + // --- 2. Elements (INDICES) --- + // An unindexed primitive lists each element's vertices in order. + const size_t num_indices = primitive.indices < 0 + ? vAccessor.count + : model.accessors[primitive.indices].count; + if (num_indices % simplex_size != 0) { + logger().warn( + "[GLTF] skipping primitive with {} indices (not a list of " + "{}-vertex elements)", + num_indices, simplex_size); + return false; + } + + std::vector indices(num_indices); + if (primitive.indices < 0) { + std::iota(indices.begin(), indices.end(), 0); + } else { + const tinygltf::Accessor& iAccessor = + model.accessors[primitive.indices]; + if (iAccessor.bufferView < 0) { + logger().warn( + "[GLTF] skipping primitive whose indices have no buffer"); + return false; + } + const tinygltf::BufferView& iBufferView = + model.bufferViews[iAccessor.bufferView]; + const unsigned char* iData = + &model.buffers[iBufferView.buffer] + .data[iBufferView.byteOffset + iAccessor.byteOffset]; + for (size_t i = 0; i < num_indices; ++i) { + switch (iAccessor.componentType) { + case TINYGLTF_COMPONENT_TYPE_UNSIGNED_INT: + indices[i] = + int(reinterpret_cast(iData)[i]); + break; + case TINYGLTF_COMPONENT_TYPE_UNSIGNED_SHORT: + indices[i] = + int(reinterpret_cast(iData)[i]); + break; + case TINYGLTF_COMPONENT_TYPE_UNSIGNED_BYTE: + indices[i] = int(iData[i]); + break; + default: + logger().warn( + "[GLTF] skipping primitive with index component type " + "{}", + iAccessor.componentType); + return false; + } + } + } + + // --- 3. Append --- + const Eigen::Index vOffset = V.rows(); + V.conservativeResize(vOffset + Eigen::Index(vAccessor.count), 3); + for (size_t i = 0; i < vAccessor.count; ++i) { + const auto* p = + reinterpret_cast(vData + i * size_t(vByteStride)); + V.row(vOffset + Eigen::Index(i)) << double(p[0]), double(p[1]), + double(p[2]); + } + + if (simplex_size > 1) { + Eigen::MatrixXi& elements = simplex_size == 3 ? F : E; + const Eigen::Index offset = elements.rows(); + const size_t num_elements = num_indices / simplex_size; + elements.conservativeResize( + offset + Eigen::Index(num_elements), simplex_size); + for (size_t i = 0; i < num_elements; ++i) { + for (int j = 0; j < simplex_size; ++j) { + elements(offset + Eigen::Index(i), j) = + int(vOffset) + indices[simplex_size * i + j]; + } + } + } + return true; + } + + void process_node( + const tinygltf::Model& model, + const tinygltf::Node& node, + const Eigen::Affine3d& parent_transform, + std::vector& rest_positions, + std::vector& edges, + std::vector& faces, + std::vector& densities, + std::vector& initial_poses, + std::vector& names) + { + // A node's transform is relative to its parent, so we accumulate the + // world transform down the hierarchy. + const Eigen::Affine3d transform = + parent_transform * local_transform(node); + const Eigen::Matrix3d L = transform.linear(); + + // A node scaled to zero on every axis (directly or by an ancestor) has + // no geometry left, so we skip the body entirely (but still recurse + // to its children). + const bool zero_scale = L.cwiseAbs().maxCoeff() < 1e-12; + + if (node.mesh > -1 && zero_scale) { + logger().info("Skipping zero-scale body: {}", node.name); + } else if (node.mesh > -1) { + const tinygltf::Mesh& mesh = model.meshes[node.mesh]; + logger().info("Found Body: {} (Mesh: {})", node.name, mesh.name); + + Eigen::MatrixXd V(0, 3); + Eigen::MatrixXi F(0, 3), line_edges(0, 2); + for (const auto& primitive : mesh.primitives) { + extract_mesh_data(model, primitive, V, F, line_edges); + } + + if (V.rows() == 0) { + logger().warn( + "[GLTF] skipping body {} with no readable primitives", + node.name); + } else { + // We split the world linear map into a rotation and a + // stretch, L = R·S (polar decomposition). The stretch (scale + // and any shear from non-uniform parent scales) is baked into + // the rest positions, and the rotation goes into the pose. + const Eigen::Matrix3d R = nearest_rotation(L); + const Eigen::Matrix3d S = R.transpose() * L; + if (!S.isIdentity(1e-12)) { + V = V * S.transpose(); + } + if (L.determinant() < 0) { + // A mirroring transform flips the triangles inside out, + // so we restore their winding. + F.col(1).swap(F.col(2)); + } + + Pose pose = Pose::identity(3); + pose.position = transform.translation(); + pose.rotation = rotation_matrix_to_vector(R); + logger().info( + " - Position: {}, Rotation: {}", pose.position.transpose(), + pose.rotation.transpose()); + + // The body's edges are its triangles' edges plus any line + // primitives (codimensional edges). + Eigen::MatrixXi E(0, 2); + if (F.rows() > 0) { // igl::edges does not accept empty faces + igl::edges(F, E); + } + if (line_edges.rows() > 0) { + E.conservativeResize(E.rows() + line_edges.rows(), 2); + E.bottomRows(line_edges.rows()) = line_edges; + } + + rest_positions.push_back(std::move(V)); + faces.push_back(std::move(F)); + edges.push_back(std::move(E)); + initial_poses.push_back(std::move(pose)); + + // We only read geometry here. Physical properties (density, + // body type, velocities, ...) are set by the caller after + // loading, so every body starts with a default density. + densities.emplace_back(1e3); + + // We prefer the node name and fall back to the mesh name. An + // empty name lets RigidBodies generate a unique "Body{i}". + names.emplace_back(!node.name.empty() ? node.name : mesh.name); + } + } + + // Recursively process children (important for nested objects/stacks) + for (int childIdx : node.children) { + process_node( + model, model.nodes[childIdx], transform, rest_positions, edges, + faces, densities, initial_poses, names); + } + } + + /// @brief The root nodes to load: the default scene's, or every node that + /// is no other node's child when the file has no usable scene. + std::vector root_nodes(const tinygltf::Model& model) + { + const int scene = model.defaultScene > -1 ? model.defaultScene : 0; + if (scene < int(model.scenes.size())) { + logger().info( + "Loading scene: {} with {} root nodes.", + model.scenes[scene].name, model.scenes[scene].nodes.size()); + return model.scenes[scene].nodes; + } + + logger().warn("[GLTF] no scene to load; using every root node"); + std::unordered_set children; + for (const tinygltf::Node& node : model.nodes) { + children.insert(node.children.begin(), node.children.end()); + } + std::vector roots; + for (int i = 0; i < int(model.nodes.size()); ++i) { + if (children.count(i) == 0) { + roots.push_back(i); + } + } + return roots; + } + +} // namespace + +std::pair, std::vector> read_gltf( + const std::string& filename, + const bool convert_planes, + const std::unordered_map& is_dof_fixed, + const std::unordered_map& center_of_mass) +{ + tinygltf::Model model; + tinygltf::TinyGLTF loader; + std::string err; + std::string warn; + + // .glb is the binary format, .gltf is the ASCII/JSON format + bool ret; + if (filename.size() >= 4 + && filename.compare(filename.size() - 4, 4, ".glb") == 0) { + ret = loader.LoadBinaryFromFile(&model, &err, &warn, filename); + } else { + ret = loader.LoadASCIIFromFile(&model, &err, &warn, filename); + } + + if (!warn.empty()) { + logger().warn("[GLTF] {}", warn); + } + + if (!err.empty()) { + logger().error("[GLTF] {}", err); + } + + if (!ret) { + logger().error("Failed to parse glTF file: {}", filename); + return { nullptr, {} }; + } + + std::vector rest_positions; + std::vector edges; + std::vector faces; + std::vector densities; + std::vector initial_poses; + std::vector names; + + for (int nodeIdx : root_nodes(model)) { + process_node( + model, model.nodes[nodeIdx], Eigen::Affine3d::Identity(), + rest_positions, edges, faces, densities, initial_poses, names); + } + + // We make the names unique here, over every mesh node in file order, + // rather than leaving it to build_from_meshes, which only sees the bodies + // left after plane pruning. That way a body's name depends only on the + // file, so callers can predict it and use it as a key below. + names = RigidBodies::make_unique_names(names); + + // Look up each body's DOF mask by name; an empty mask leaves every DOF + // free. We warn about unmatched names since they are most likely typos. + std::vector body_is_dof_fixed(names.size()); + for (const auto& [name, mask] : is_dof_fixed) { + bool found = false; + for (size_t i = 0; i < names.size(); ++i) { + if (names[i] == name) { + body_is_dof_fixed[i] = mask; + found = true; + } + } + if (!found) { + logger().warn( + "is_dof_fixed: no body named '{}' in {}", name, filename); + } + } + + // Likewise for the prescribed centers of mass. We take them in world + // coordinates, so they do not depend on where the file puts each node's + // origin, and move them into the body's rest frame: a rest position v + // sits at R v + t in the world. + std::vector> body_center_of_mass(names.size()); + for (const auto& [name, com] : center_of_mass) { + bool found = false; + for (size_t i = 0; i < names.size(); ++i) { + if (names[i] == name) { + body_center_of_mass[i] = + initial_poses[i].rotation_matrix().transpose() + * (com - initial_poses[i].position); + found = true; + } + } + if (!found) { + logger().warn( + "center_of_mass: no body named '{}' in {}", name, filename); + } + } + + // build_from_meshes prunes plane bodies from initial_poses, names, the DOF + // masks, and the centers of mass in place, so they all stay aligned with + // the returned bodies. + std::shared_ptr bodies = RigidBodies::build_from_meshes( + rest_positions, edges, faces, densities, initial_poses, convert_planes, + body_is_dof_fixed, names, body_center_of_mass); + return { bodies, initial_poses }; +} + +} // namespace ipc::rigid \ No newline at end of file diff --git a/src/ipc/io/read_gltf.hpp b/src/ipc/io/read_gltf.hpp new file mode 100644 index 000000000..a5f698292 --- /dev/null +++ b/src/ipc/io/read_gltf.hpp @@ -0,0 +1,44 @@ +#pragma once + +#include + +#include +#include +#include +#include + +namespace ipc::rigid { + +// Forward declarations +class RigidBodies; +struct Pose; + +/// @brief Read a rigid body scene from a glTF file and return a RigidBodies object. +/// +/// We only read geometry: each mesh node becomes a body named after its node +/// (or mesh), made unique with RigidBodies::make_unique_names over the mesh +/// nodes in file order. A node's world transform composes its ancestors' +/// transforms (TRS or matrix); its rotation becomes the body's pose and the +/// remaining stretch (scale, plus any shear) is baked into the rest +/// positions. All triangle, line, and point primitives of a mesh are merged +/// into one body (lines become codimensional edges), and nodes scaled to zero +/// are skipped. Every body gets a default density of +/// 1000; set physical properties on the returned bodies. +/// +/// The exceptions are @p is_dof_fixed and @p center_of_mass. Fixing exactly +/// two rotational DOFs changes the frame a body's inertia is expressed in, and +/// a prescribed center of mass moves the body's origin; both are baked into +/// the body at construction, so we take them here instead of through setters. +/// +/// @param filename The input glTF filename. +/// @param convert_planes If true, any mesh with exactly two coplanar faces will be converted to a plane. +/// @param is_dof_fixed Per-DOF fixed flags ([position | rotation]) keyed by body name. Bodies not listed have all DOFs free. +/// @param center_of_mass Prescribed centers of mass keyed by body name, in the file's world coordinates. Bodies not listed use the center of mass computed from their mesh. See RigidBody. +/// @return A pair containing the RigidBodies object and a vector of initial poses for each body. +std::pair, std::vector> read_gltf( + const std::string& filename, + const bool convert_planes = false, + const std::unordered_map& is_dof_fixed = {}, + const std::unordered_map& center_of_mass = {}); + +} // namespace ipc::rigid diff --git a/src/ipc/utils/save_obj.cpp b/src/ipc/io/write_candidates_obj.cpp similarity index 95% rename from src/ipc/utils/save_obj.cpp rename to src/ipc/io/write_candidates_obj.cpp index f8d82795d..d65981d17 100644 --- a/src/ipc/utils/save_obj.cpp +++ b/src/ipc/io/write_candidates_obj.cpp @@ -1,4 +1,4 @@ -#include "save_obj.hpp" +#include "write_candidates_obj.hpp" #include #include @@ -10,7 +10,7 @@ namespace ipc { template <> -void save_obj( +void write_candidates_obj( std::ostream& out, Eigen::ConstRef V, Eigen::ConstRef /*unused*/, @@ -26,7 +26,7 @@ void save_obj( } template <> -void save_obj( +void write_candidates_obj( std::ostream& out, Eigen::ConstRef V, Eigen::ConstRef E, @@ -46,7 +46,7 @@ void save_obj( } template <> -void save_obj( +void write_candidates_obj( std::ostream& out, Eigen::ConstRef V, Eigen::ConstRef E, @@ -68,7 +68,7 @@ void save_obj( } template <> -void save_obj( +void write_candidates_obj( std::ostream& out, Eigen::ConstRef V, Eigen::ConstRef E, @@ -89,7 +89,7 @@ void save_obj( } template <> -void save_obj( +void write_candidates_obj( std::ostream& out, Eigen::ConstRef V, Eigen::ConstRef E, diff --git a/src/ipc/io/write_candidates_obj.hpp b/src/ipc/io/write_candidates_obj.hpp new file mode 100644 index 000000000..dc8adfad6 --- /dev/null +++ b/src/ipc/io/write_candidates_obj.hpp @@ -0,0 +1,36 @@ +#pragma once + +#include + +#include +#include +#include + +namespace ipc { + +template +void write_candidates_obj( + std::ostream& out, + Eigen::ConstRef V, + Eigen::ConstRef E, + Eigen::ConstRef F, + const std::vector& candidates, + const int v_offset = 0); + +template +bool write_candidates_obj( + const std::string& filename, + Eigen::ConstRef V, + Eigen::ConstRef E, + Eigen::ConstRef F, + const std::vector& candidates) +{ + std::ofstream obj(filename); + if (!obj.is_open()) { + return false; + } + write_candidates_obj(obj, V, E, F, candidates); + return true; +} + +} // namespace ipc diff --git a/src/ipc/io/write_gltf.cpp b/src/ipc/io/write_gltf.cpp new file mode 100644 index 000000000..8b5c3fe6d --- /dev/null +++ b/src/ipc/io/write_gltf.cpp @@ -0,0 +1,419 @@ +#include "write_gltf.hpp" + +#include + +#include + +#include +#include +#include + +namespace ipc::rigid { + +bool write_gltf( + const std::string& filename, + const RigidBodies& bodies, + const std::list>& poses, + double timestep, + bool embed_buffers, + bool write_binary, + bool prettyPrint) +{ + typedef float Float; + constexpr int float_component_type = std::is_same::value + ? TINYGLTF_COMPONENT_TYPE_FLOAT + : TINYGLTF_COMPONENT_TYPE_DOUBLE; + + using namespace tinygltf; + + assert(bodies.dim() == 3); + size_t num_bodies = bodies.num_bodies(); + size_t num_planes = bodies.planes.size(); + assert(poses.size() > 0); + size_t num_steps = poses.size(); + + // Each body's primitive: triangles when it has faces, otherwise its edges + // as lines, otherwise its vertices as points. glTF forbids empty accessors + // and buffer views, so a faceless (codimensional) body must still write a + // non-empty index list. + std::vector> body_indices(num_bodies); + std::vector body_modes(num_bodies); + for (size_t i = 0; i < num_bodies; i++) { + Eigen::MatrixXi elements; + if (bodies.body_num_faces(i) > 0) { + elements = bodies.body_faces(i); + body_modes[i] = TINYGLTF_MODE_TRIANGLES; + } else if (bodies.body_num_edges(i) > 0) { + elements = bodies.body_edges(i); + body_modes[i] = TINYGLTF_MODE_LINE; + } else { + elements = Eigen::VectorXi::LinSpaced( + bodies.body_num_vertices(i), 0, + int(bodies.body_num_vertices(i)) - 1); + body_modes[i] = TINYGLTF_MODE_POINTS; + } + // Row-major so each element's indices are contiguous. + for (int r = 0; r < elements.rows(); r++) { + for (int c = 0; c < elements.cols(); c++) { + body_indices[i].push_back( + static_cast(elements(r, c))); + } + } + } + + Model model; + model.defaultScene = 0; + + model.asset.version = "2.0"; + model.asset.generator = "RigidIPC"; + + Scene& scene = model.scenes.emplace_back(); + scene.name = "RigidIPCSimulation"; + scene.nodes.resize(num_bodies + num_planes); + std::iota(scene.nodes.begin(), scene.nodes.end(), 0); + + model.nodes.resize(num_bodies + num_planes); + model.animations.resize(1); + Animation& animation = model.animations[0]; + animation.name = "Simulation"; + animation.channels.resize(2 * num_bodies); + animation.samplers.resize(2 * num_bodies); + model.meshes.resize(num_bodies + num_planes); + // Four accessors per body: vertices, faces, translations, and rotations + // Plus two accessors per analytic plane: vertices and faces + model.accessors.resize(4 * num_bodies + 1 + 2 * num_planes); + { + Accessor& accessor = model.accessors[2 * num_bodies]; + accessor.name = "Times"; + accessor.bufferView = 2 * num_bodies; + accessor.componentType = float_component_type; + accessor.count = num_steps; + // Time samples are i·Δt for i ∈ [0, num_steps) + accessor.minValues.push_back(0.0); + accessor.maxValues.push_back((num_steps - 1) * timestep); + accessor.type = TINYGLTF_TYPE_SCALAR; + } + + for (int i = 0; i < num_bodies; i++) { + Node& node = model.nodes[i]; + node.mesh = i; + std::string body_name = "Body" + std::to_string(i); + // We name the node after the body so the name round-trips through + // read_gltf; the internal accessor/bufferView labels stay indexed. + node.name = bodies.body_name(i); + node.translation = { { poses.front()[i].position.x(), + poses.front()[i].position.y(), + poses.front()[i].position.z() } }; + // The vertices are written in the body (R0) frame and the animation + // rotates them by pose.quaternion() * R0⁻¹ each frame. We set the + // node's static rotation to the first frame's value so the initial + // orientation round-trips through read_gltf, which reads the static + // node transform rather than the animation. + const Eigen::Quaternion q0( + Eigen::Matrix3d(bodies[i].principal_rotation())); + const Eigen::Quaternion q = + poses.front()[i].quaternion() * q0.inverse(); + node.rotation = { { q.x(), q.y(), q.z(), q.w() } }; + + animation.channels[2 * i + 0].sampler = 2 * i; + animation.channels[2 * i + 0].target_node = i; + animation.channels[2 * i + 0].target_path = "translation"; + animation.channels[2 * i + 1].sampler = 2 * i + 1; + animation.channels[2 * i + 1].target_node = i; + animation.channels[2 * i + 1].target_path = "rotation"; + + animation.samplers[2 * i + 0].input = 2 * num_bodies; + animation.samplers[2 * i + 0].output = 2 * num_bodies + 2 * i + 1; + animation.samplers[2 * i + 0].interpolation = "LINEAR"; + animation.samplers[2 * i + 1].input = 2 * num_bodies; + animation.samplers[2 * i + 1].output = 2 * num_bodies + 2 * i + 2; + animation.samplers[2 * i + 1].interpolation = "LINEAR"; + + Mesh& mesh = model.meshes[i]; + mesh.name = body_name; + Primitive& primitive = mesh.primitives.emplace_back(); + primitive.attributes["POSITION"] = 2 * i; + primitive.indices = 2 * i + 1; + primitive.mode = body_modes[i]; + + Accessor* accessor = &model.accessors[2 * i]; + accessor->name = body_name + "Vertices"; + accessor->bufferView = 2 * i; + accessor->componentType = float_component_type; + accessor->count = bodies.body_num_vertices(i); + // The glTF 2.0 spec requires min/max on POSITION accessors. Compute + // them from the float-cast values actually written to the buffer. + { + const Eigen::MatrixXf V = + (bodies.body_rest_positions(i) + * bodies[i].principal_rotation().transpose()) + .cast(); + for (int d = 0; d < 3; d++) { + accessor->minValues.push_back(V.col(d).minCoeff()); + accessor->maxValues.push_back(V.col(d).maxCoeff()); + } + } + accessor->type = TINYGLTF_TYPE_VEC3; + + accessor = &model.accessors[2 * i + 1]; + accessor->name = body_name + "Faces"; + accessor->bufferView = 2 * i + 1; + accessor->componentType = TINYGLTF_COMPONENT_TYPE_UNSIGNED_INT; + accessor->count = body_indices[i].size(); + accessor->type = TINYGLTF_TYPE_SCALAR; + + accessor = &model.accessors[2 * num_bodies + 2 * i + 1]; + accessor->name = body_name + "Translations"; + accessor->bufferView = 2 * num_bodies + 2 * i + 1; + accessor->componentType = float_component_type; + accessor->count = num_steps; + accessor->type = TINYGLTF_TYPE_VEC3; + + accessor = &model.accessors[2 * num_bodies + 2 * i + 2]; + accessor->name = body_name + "Rotations"; + accessor->bufferView = 2 * num_bodies + 2 * i + 2; + accessor->componentType = float_component_type; + accessor->count = num_steps; + accessor->type = TINYGLTF_TYPE_VEC4; + } + + // Create meshes, nodes, and accessors for analytic planes + for (size_t p = 0; p < num_planes; ++p) { + const auto& plane = bodies.planes[p]; + size_t mesh_index = num_bodies + p; + + Node& node = model.nodes[mesh_index]; + node.mesh = static_cast(mesh_index); + std::string plane_name = "Plane" + std::to_string(p); + node.name = plane_name; + Eigen::Vector3d origin = -plane.offset() * plane.normal(); + node.translation = { { origin.x(), origin.y(), origin.z() } }; + Eigen::Quaternion q = Eigen::Quaternion::Identity(); + node.rotation = { { q.x(), q.y(), q.z(), q.w() } }; + + Mesh& mesh = model.meshes[mesh_index]; + mesh.name = plane_name; + Primitive& primitive = mesh.primitives.emplace_back(); + const int vert_accessor_idx = + static_cast(4 * num_bodies + 1 + 2 * p); + const int face_accessor_idx = vert_accessor_idx + 1; + primitive.attributes["POSITION"] = vert_accessor_idx; + primitive.indices = face_accessor_idx; + primitive.mode = TINYGLTF_MODE_TRIANGLES; + + Accessor* accessor = &model.accessors[vert_accessor_idx]; + accessor->name = plane_name + "Vertices"; + accessor->bufferView = 4 * num_bodies + 1 + 2 * p; + accessor->componentType = float_component_type; + accessor->count = 4; + accessor->type = TINYGLTF_TYPE_VEC3; + + accessor = &model.accessors[face_accessor_idx]; + accessor->name = plane_name + "Faces"; + accessor->bufferView = 4 * num_bodies + 1 + 2 * p + 1; + accessor->componentType = TINYGLTF_COMPONENT_TYPE_UNSIGNED_INT; + accessor->count = 6; + accessor->type = TINYGLTF_TYPE_SCALAR; + } + + /////////////////////////////////////////////////////////////////////////// + + model.bufferViews.resize(4 * num_bodies + 1 + 2 * num_planes); + size_t byte_offset = 0; + for (int i = 0; i < num_bodies; i++) { + std::string body_name = "Body" + std::to_string(i); + + BufferView* buffer_view = &model.bufferViews[2 * i]; + buffer_view->name = body_name + "Vertices"; + buffer_view->buffer = 0; + buffer_view->byteLength = + sizeof(Float) * bodies.body_rest_positions(i).size(); + buffer_view->byteOffset = byte_offset; + byte_offset += buffer_view->byteLength; + + buffer_view = &model.bufferViews[2 * i + 1]; + buffer_view->name = body_name + "Faces"; + buffer_view->buffer = 0; + buffer_view->byteLength = sizeof(unsigned int) * body_indices[i].size(); + buffer_view->byteOffset = byte_offset; + byte_offset += buffer_view->byteLength; + } + { + BufferView& buffer_view = model.bufferViews[2 * num_bodies]; + buffer_view.name = "Times"; + buffer_view.buffer = 0; + buffer_view.byteLength = num_steps * sizeof(Float); + buffer_view.byteOffset = byte_offset; + byte_offset += buffer_view.byteLength; + } + for (int i = 0; i < num_bodies; i++) { + std::string body_name = "Body" + std::to_string(i); + + BufferView* buffer_view = + &model.bufferViews[2 * num_bodies + 2 * i + 1]; + buffer_view->name = body_name + "Translations"; + buffer_view->buffer = 0; + buffer_view->byteLength = 3 * sizeof(Float) * num_steps; + buffer_view->byteOffset = byte_offset; + byte_offset += buffer_view->byteLength; + + buffer_view = &model.bufferViews[2 * num_bodies + 2 * i + 2]; + buffer_view->name = body_name + "Rotations"; + buffer_view->buffer = 0; + buffer_view->byteLength = 4 * sizeof(Float) * num_steps; + buffer_view->byteOffset = byte_offset; + byte_offset += buffer_view->byteLength; + } + + // Add bufferViews for analytic planes (each plane: 4 vertices, 2 triangles) + for (size_t p = 0; p < num_planes; ++p) { + BufferView* buffer_view = + &model.bufferViews[4 * num_bodies + 1 + 2 * p]; + buffer_view->name = "PlaneVertices" + std::to_string(p); + buffer_view->buffer = 0; + buffer_view->byteLength = sizeof(Float) * 3 * 4; // 4 vertices + buffer_view->byteOffset = byte_offset; + byte_offset += buffer_view->byteLength; + + buffer_view = &model.bufferViews[4 * num_bodies + 1 + 2 * p + 1]; + buffer_view->name = "PlaneFaces" + std::to_string(p); + buffer_view->buffer = 0; + buffer_view->byteLength = sizeof(unsigned int) * 6; // 2 triangles + buffer_view->byteOffset = byte_offset; + byte_offset += buffer_view->byteLength; + } + + /////////////////////////////////////////////////////////////////////////// + + std::vector byte_data(byte_offset); + size_t byte_i = 0; + for (int i = 0; i < num_bodies; i++) { + // NOTE: Apply the initial rotation to the vertices + Eigen::MatrixXd V = bodies.body_rest_positions(i) + * bodies[i].principal_rotation().transpose(); + for (int r = 0; r < V.rows(); r++) { + for (int c = 0; c < V.cols(); c++) { + const auto v = static_cast(V(r, c)); + std::memcpy(&byte_data[byte_i], &v, sizeof(Float)); + byte_i += sizeof(Float); + } + } + + for (const unsigned int index : body_indices[i]) { + std::memcpy(&byte_data[byte_i], &index, sizeof(unsigned int)); + byte_i += sizeof(unsigned int); + } + } + for (int i = 0; i < num_steps; i++) { + const auto t = static_cast(i * timestep); + std::memcpy(&byte_data[byte_i], &t, sizeof(Float)); + byte_i += sizeof(Float); + } + for (int i = 0; i < num_bodies; i++) { + for (const auto& poses_j : poses) { + Eigen::Vector3d p = poses_j[i].position; + for (int d = 0; d < p.size(); d++) { + const auto pd = static_cast(p[d]); + std::memcpy(&byte_data[byte_i], &pd, sizeof(Float)); + byte_i += sizeof(Float); + } + } + + const Eigen::Quaternion q0( + Eigen::Matrix3d(bodies[i].principal_rotation())); + for (const auto& poses_j : poses) { + // NOTE: Apply the inverse of the initial rotation to the + // quaternion to get the rotation relative to the input orientation. + Eigen::Quaternion quat = + poses_j[i].quaternion() * q0.inverse(); + Eigen::Vector4d q(quat.x(), quat.y(), quat.z(), quat.w()); + for (int d = 0; d < q.size(); d++) { + const auto qd = static_cast(q[d]); + std::memcpy(&byte_data[byte_i], &qd, sizeof(Float)); + byte_i += sizeof(Float); + } + } + } + + // Write analytic plane vertex and face data + if (num_planes > 0) { + // Compute scene bounding box (apply initial rotations to body vertices) + Eigen::Vector3d scene_min = + Eigen::Vector3d::Constant(std::numeric_limits::infinity()); + Eigen::Vector3d scene_max = + Eigen::Vector3d::Constant(-std::numeric_limits::infinity()); + for (int i = 0; i < num_bodies; ++i) { + Eigen::MatrixXd V = bodies.body_rest_positions(i) + * bodies[i].principal_rotation().transpose(); + for (int r = 0; r < V.rows(); ++r) { + scene_min = scene_min.cwiseMin(V.row(r).transpose()); + scene_max = scene_max.cwiseMax(V.row(r).transpose()); + } + } + // Size the ground quad to the scene (≈5× the bounding-box diagonal) so + // the plane spans the scene without dwarfing small scenes like the + // bolt. half_size is the half-extent, so the full width is 2·half_size. + const double diag = (scene_max - scene_min).norm(); + const double half_size = diag > 0.0 ? 2.5 * diag : 2.5; + + for (size_t p = 0; p < num_planes; ++p) { + const auto& plane = bodies.planes[p]; + Eigen::Vector3d n = plane.normal().normalized(); + Eigen::Vector3d ref = (std::abs(n.x()) < 0.9) + ? Eigen::Vector3d::UnitX() + : Eigen::Vector3d::UnitY(); + Eigen::Vector3d u = n.cross(ref).normalized(); + Eigen::Vector3d v = n.cross(u).normalized(); + + // vertices relative to plane origin (so node translation places + // them) + Eigen::Vector3d origin = -plane.offset() * plane.normal(); + std::array verts { { + origin + (u * half_size + v * half_size), + origin + (-u * half_size + v * half_size), + origin + (-u * half_size + -v * half_size), + origin + (u * half_size + -v * half_size), + } }; + + // write vertex positions as floats (local coordinates = verts - + // origin) + Eigen::Vector3f local_min = + Eigen::Vector3f::Constant(std::numeric_limits::max()); + Eigen::Vector3f local_max = + Eigen::Vector3f::Constant(std::numeric_limits::lowest()); + for (int vi = 0; vi < 4; ++vi) { + Eigen::Vector3d local = verts[vi] - origin; + for (int d = 0; d < 3; ++d) { + Float vd = static_cast(local[d]); + local_min[d] = std::min(local_min[d], vd); + local_max[d] = std::max(local_max[d], vd); + std::memcpy(&byte_data[byte_i], &vd, sizeof(Float)); + byte_i += sizeof(Float); + } + } + + // The glTF 2.0 spec requires min/max on POSITION accessors + Accessor& vert_accessor = + model.accessors[4 * num_bodies + 1 + 2 * p]; + for (int d = 0; d < 3; ++d) { + vert_accessor.minValues.push_back(local_min[d]); + vert_accessor.maxValues.push_back(local_max[d]); + } + + unsigned int faces[6] = { 0, 1, 2, 0, 2, 3 }; + for (int fi = 0; fi < 6; ++fi) { + unsigned int f = faces[fi]; + std::memcpy(&byte_data[byte_i], &f, sizeof(unsigned int)); + byte_i += sizeof(unsigned int); + } + } + } + assert(byte_i == byte_data.size()); + + model.buffers.emplace_back().data = byte_data; + return TinyGLTF().WriteGltfSceneToFile( + &model, filename, + /*embedImages=*/true, embed_buffers, prettyPrint, write_binary); +} + +} // namespace ipc::rigid diff --git a/src/ipc/io/write_gltf.hpp b/src/ipc/io/write_gltf.hpp new file mode 100644 index 000000000..ee9203902 --- /dev/null +++ b/src/ipc/io/write_gltf.hpp @@ -0,0 +1,31 @@ +#pragma once + +#include +#include +#include + +namespace ipc::rigid { + +// Forward declarations +class RigidBodies; +struct Pose; + +/// @brief Write a sequence of rigid body poses to a glTF file. +/// @param filename The output glTF filename. +/// @param bodies The rigid bodies to write. +/// @param poses A list of poses for each timestep. +/// @param timestep The time interval between each pose in seconds. +/// @param embed_buffers Whether to embed the binary buffers in the glTF file. +/// @param write_binary Whether to write a binary .glb file (true) or a text .gltf file (false). +/// @param prettyPrint Whether to pretty-print the JSON content. +/// @return True if successful, false otherwise. +bool write_gltf( + const std::string& filename, + const RigidBodies& bodies, + const std::list>& poses, + double timestep, + bool embed_buffers = true, + bool write_binary = true, + bool prettyPrint = true); + +} // namespace ipc::rigid diff --git a/src/ipc/math/CMakeLists.txt b/src/ipc/math/CMakeLists.txt index 3715b35bb..3c2edb686 100644 --- a/src/ipc/math/CMakeLists.txt +++ b/src/ipc/math/CMakeLists.txt @@ -1,4 +1,5 @@ set(SOURCES + constants.hpp heaviside_type.hpp interval.cpp interval.hpp @@ -7,6 +8,8 @@ set(SOURCES math.tpp morton.hpp scalar_math.hpp + sinc.cpp + sinc.hpp ) -target_sources(ipc_toolkit PRIVATE ${SOURCES}) \ No newline at end of file +target_sources(ipc_toolkit PRIVATE ${SOURCES}) diff --git a/src/ipc/math/constants.hpp b/src/ipc/math/constants.hpp new file mode 100644 index 000000000..2968ae56d --- /dev/null +++ b/src/ipc/math/constants.hpp @@ -0,0 +1,21 @@ +#pragma once + +// We include first so M_PI is visible whenever the platform provides +// it, regardless of what the including file pulled in before us. +#include // IWYU pragma: keep (provides M_PI where available) + +namespace ipc { + +// M_PI is not standard C++ (MSVC only defines it when _USE_MATH_DEFINES is +// set before ), so we use it when available and fall back to our own +// literal otherwise, following libigl's igl/PI.h. We avoid EIGEN_PI because it +// is a long double on GCC, and narrowing it trips -Werror=float-conversion. +#ifdef M_PI +/// @brief π +constexpr double PI = M_PI; +#else +/// @brief π +constexpr double PI = 3.1415926535897932384626433832795; +#endif + +} // namespace ipc diff --git a/src/ipc/math/math.cpp b/src/ipc/math/math.cpp index 6ab30e342..c5f5c2fe4 100644 --- a/src/ipc/math/math.cpp +++ b/src/ipc/math/math.cpp @@ -4,9 +4,45 @@ #include #include +#include + namespace ipc { namespace { + // Rotation-variant SVD for a fixed-size square matrix. Folds any reflection + // into the smallest singular value so that U and V are proper rotations + // (det = +1), following the standard signed SVD. + template + void svd_rv( + const Eigen::Matrix& A, + Eigen::Matrix& U, + Eigen::Matrix& V, + Eigen::Vector& sigma) + { + using MatrixType = Eigen::Matrix; + // Compute-time U/V options (runtime options are deprecated in Eigen). + constexpr int COMPUTE_FULL_UV = + Eigen::ComputeFullU | Eigen::ComputeFullV; + + Eigen::JacobiSVD svd(A); + + U = svd.matrixU(); + V = svd.matrixV(); + sigma = svd.singularValues(); + + MatrixType L = MatrixType::Identity(); + L(dim - 1, dim - 1) = (U * V.transpose()).determinant(); + + const double detU = U.determinant(); + const double detV = V.determinant(); + if (detU < 0 && detV > 0) { + U = U * L; + } else if (detU > 0 && detV < 0) { + V = V * L; + } + sigma = L * sigma; + } + /* Mathematica script @@ -20,6 +56,23 @@ namespace { } // namespace +MatrixMax3d nearest_rotation(Eigen::ConstRef A) +{ + assert(A.rows() == A.cols()); + if (A.rows() == 2) { + Eigen::Matrix2d U, V; + Eigen::Vector2d sigma; + svd_rv<2>(Eigen::Matrix2d(A), U, V, sigma); + return U * V.transpose(); + } else { + assert(A.rows() == 3); + Eigen::Matrix3d U, V; + Eigen::Vector3d sigma; + svd_rv<3>(Eigen::Matrix3d(A), U, V, sigma); + return U * V.transpose(); + } +} + HeavisideType OrientationTypes::compute_type( const double val, const double alpha, const double beta) { diff --git a/src/ipc/math/math.hpp b/src/ipc/math/math.hpp index 18a98bf3b..a79413635 100644 --- a/src/ipc/math/math.hpp +++ b/src/ipc/math/math.hpp @@ -136,6 +136,11 @@ HessianType<9> negative_orientation_penalty_hess( const double alpha, const double beta); +/// @brief The nearest rotation (in SO(dim)) to a square matrix A. +/// @param A The (square) matrix (dim ∈ {2, 3}). +/// @return The rotation factor R = U Vᵀ of the rotation-variant SVD. +MatrixMax3d nearest_rotation(Eigen::ConstRef A); + } // namespace ipc #include "math.tpp" diff --git a/src/ipc/math/sinc.cpp b/src/ipc/math/sinc.cpp new file mode 100644 index 000000000..7a127f061 --- /dev/null +++ b/src/ipc/math/sinc.cpp @@ -0,0 +1,155 @@ +#include "sinc.hpp" + +#include +#include + +namespace ipc { + +namespace { + // We use these bounds because for example 1 + x^2 = 1 for x < sqrt(ϵ). + constexpr double TAYLOR_0_BOUND = std::numeric_limits::epsilon(); + const double TAYLOR_2_BOUND = std::sqrt(TAYLOR_0_BOUND); + const double TAYLOR_N_BOUND = std::sqrt(TAYLOR_2_BOUND); + +#ifdef IPC_TOOLKIT_WITH_FILIB + // WARNING: Assumes x is a single value and uses interval arithmetic to + // account for rounding. + filib::Interval sinc_interval_taylor(double x_double) + { + const filib::Interval x(x_double); + + if (std::abs(x.INF) >= TAYLOR_N_BOUND) { + return sin(x) / x; + } + + // approximation by taylor series in x at 0 up to order 5 + // 1 - x² / 6 + x⁴ / 120 = 1 + x² / 6 * (x² / 20 - 1) + const filib::Interval squared_x = sqr(x); + return 1.0 + squared_x / 6.0 * (squared_x / 20.0 - 1.0); + } +#endif + + // Compute sinc'(x) / x + inline double dsinc_over_x(double x) + { + constexpr double EPS = 1e-4; + + double x2 = x * x; + if (std::abs(x) > EPS) { + return (x * std::cos(x) - std::sin(x)) / (x2 * x); + } + + // approximation by taylor series in x at 0 up to order 5 + return x2 * (-x2 / 840.0 + 1.0 / 30.0) - 1.0 / 3.0; + } + + // Compute sinc"(x) / x² - sinc'(x) / x³ + inline double ddsinc_over_x2_minus_dsinc_over_x3(double x) + { + constexpr double EPS = 0.1; + + double x2 = x * x; + double x4 = x2 * x2; + if (std::abs(x) > EPS) { + return ((3 - x2) * std::sin(x) - 3 * x * std::cos(x)) / (x4 * x); + } + + // approximation by taylor series in x at 0 up to order 5 + return x4 / 7560.0 - x2 / 210.0 + 1.0 / 15.0; + } +} // namespace + +double sinc(const double x) +{ + if (std::abs(x) >= TAYLOR_N_BOUND) { + return std::sin(x) / x; + } + + // approximation by taylor series in x at 0 up to order 1 + double result = 1; + + if (std::abs(x) >= TAYLOR_0_BOUND) { + const double squared_x = x * x; + + // approximation by taylor series in x at 0 up to order 3 + result -= squared_x / 6.0; + + if (std::abs(x) >= TAYLOR_2_BOUND) { + // approximation by taylor series in x at 0 up to order 5 + result += (squared_x * squared_x) / 120.0; + } + } + + return result; +} + +#ifdef IPC_TOOLKIT_WITH_FILIB +filib::Interval sinc(const filib::Interval& x) +{ + // Define two regions and use even symmetry of sinc. + + // A bound on sinc where it is monotonic ([0, ~4.4934]) + constexpr double MONOTONIC_BOUND = 4.4934094579; + + // A conservative lower bound for sinc(x) + // https://www.wolframalpha.com/input/?i=min+sin%28x%29%2Fx+between+4+and+5 + const filib::Interval bounds(-0.217233628211221659, 1.0); + + filib::Interval y = filib::Interval::empty(), x_pos = x; + if (x.INF < 0) { + if (x.SUP <= 0) { + return sinc(-x); // sinc is an even function + } + // Split + y = sinc(filib::Interval(0, -x.INF)); + x_pos = filib::Interval(0, x.SUP); + } + + // Split the domain into two intervals: + // 1) x ∩ [0, MONOTONIC_BOUND] + // 2) x ∩ [MONOTONIC_BOUND, ∞) + + // Case 1 (Monotonic): + filib::Interval x_gt_monotonic = x_pos; + if (x_pos.INF <= MONOTONIC_BOUND) { + filib::Interval x_monotonic = x_pos; + if (x_monotonic.SUP > MONOTONIC_BOUND) { + x_monotonic = filib::Interval(x_monotonic.INF, MONOTONIC_BOUND); + x_gt_monotonic = filib::Interval(MONOTONIC_BOUND, x_pos.SUP); + } else { + x_gt_monotonic = filib::Interval::empty(); + } + + // sinc is monotonically decreasing, so flip lower() and upper(). + filib::Interval monotonic_y = (sinc_interval_taylor(x_monotonic.SUP) + | sinc_interval_taylor(x_monotonic.INF)) + & bounds; + + // Combine the two intervals as a convex hull + y |= monotonic_y; + } + + // Case 2 (Not necessarily monotonic): + if (!empty(x_gt_monotonic)) { + // x_gt_monotonic is larger than one, so the division should be well + // behaved. + y |= sin(x_gt_monotonic) / x_gt_monotonic; + } + + return intsec(y, bounds); +} +#endif + +VectorMax3d sinc_norm_x_grad(Eigen::ConstRef x) +{ + return dsinc_over_x(x.norm()) * x; +} + +MatrixMax3d sinc_norm_x_hess(Eigen::ConstRef x) +{ + double norm_x = x.norm(); + return ddsinc_over_x2_minus_dsinc_over_x3(norm_x) * x * x.transpose() + + dsinc_over_x(norm_x) * MatrixMax3d::Identity(x.size(), x.size()); +} + +} // namespace ipc diff --git a/src/ipc/math/sinc.hpp b/src/ipc/math/sinc.hpp new file mode 100644 index 000000000..b31f61e70 --- /dev/null +++ b/src/ipc/math/sinc.hpp @@ -0,0 +1,45 @@ +#pragma once + +#include +#include + +namespace ipc { + +/// @brief Compute the sinc function: \f$ \frac{\sin(x)}{x} \f$ +/// @param x The value for which to compute the sinc function +/// @return The value of sinc(x) +double sinc(const double x); + +#ifdef IPC_TOOLKIT_WITH_FILIB + +/// @brief Compute the sinc function: \f$ \frac{\sin(x)}{x} \f$ over an interval +/// @param x The interval for which to compute the sinc function +/// @return The interval of sinc(x) +filib::Interval sinc(const filib::Interval& x); + +#endif + +/// @brief Compute the sinc of the norm of a vector (\f$\operatorname{sinc}(\|x\|)\f$) +/// @tparam T The type of the elements of the vector +/// @param x The vector for which to compute the sinc of the norm +/// @return The value of \f$\operatorname{sinc}(\|x\|)\f$ +template T sinc_norm_x(Eigen::ConstRef> x) +{ + if constexpr (std::is_same_v) { + return sinc(x.norm()); + } else { + return sinc(norm(x)); + } +} + +/// @brief Compute the gradient of the sinc of the norm of a vector +/// @param x The vector for which to compute the gradient of the sinc of the norm +/// @return The gradient of the sinc of the norm of the vector +VectorMax3d sinc_norm_x_grad(Eigen::ConstRef x); + +/// @brief Compute the Hessian of the sinc of the norm of a vector +/// @param x The vector for which to compute the Hessian of the sinc of the norm +/// @return The Hessian of the sinc of the norm of the vector +MatrixMax3d sinc_norm_x_hess(Eigen::ConstRef x); + +} // namespace ipc \ No newline at end of file diff --git a/src/ipc/utils/CMakeLists.txt b/src/ipc/utils/CMakeLists.txt index 3e28d09bc..b7235ecbe 100644 --- a/src/ipc/utils/CMakeLists.txt +++ b/src/ipc/utils/CMakeLists.txt @@ -16,9 +16,8 @@ set(SOURCES merge_thread_local.hpp profiler.cpp profiler.hpp - simd.hpp - save_obj.cpp save_obj.hpp + simd.hpp unordered_map_and_set.hpp vertex_to_min_edge.cpp vertex_to_min_edge.hpp diff --git a/src/ipc/utils/logger.cpp b/src/ipc/utils/logger.cpp index a953f6f84..1e0dd2d52 100644 --- a/src/ipc/utils/logger.cpp +++ b/src/ipc/utils/logger.cpp @@ -23,11 +23,14 @@ spdlog::logger& logger() if (get_shared_logger()) { return *get_shared_logger(); } else { - // When using factory methods provided by spdlog (_st and _mt - // functions), names must be unique, since the logger is registered - // globally. Otherwise, you will need to create the logger manually. See + // Create the logger manually instead of using spdlog's factory + // methods (_st and _mt functions): the factories register the logger + // in spdlog's global registry, whose name-uniqueness check fails when + // several shared libraries each embed a copy of the toolkit (e.g., + // two Python modules). See // https://github.com/gabime/spdlog/wiki/2.-Creating-loggers - static auto default_logger = spdlog::stdout_color_mt("ipctk"); + static auto default_logger = std::make_shared( + "ipctk", std::make_shared()); return *default_logger; } } diff --git a/src/ipc/utils/save_obj.hpp b/src/ipc/utils/save_obj.hpp index 819b323f2..4195509a4 100644 --- a/src/ipc/utils/save_obj.hpp +++ b/src/ipc/utils/save_obj.hpp @@ -1,23 +1,30 @@ #pragma once -#include +// Deprecated: this header moved to , and +// save_obj was renamed to write_candidates_obj. We keep these forwarding +// overloads so existing code builds (with a deprecation warning). -#include -#include -#include +#include namespace ipc { template +[[deprecated( + "Use ipc::write_candidates_obj from instead.")]] void save_obj( std::ostream& out, Eigen::ConstRef V, Eigen::ConstRef E, Eigen::ConstRef F, const std::vector& candidates, - const int v_offset = 0); + const int v_offset = 0) +{ + write_candidates_obj(out, V, E, F, candidates, v_offset); +} template +[[deprecated( + "Use ipc::write_candidates_obj from instead.")]] bool save_obj( const std::string& filename, Eigen::ConstRef V, @@ -25,12 +32,7 @@ bool save_obj( Eigen::ConstRef F, const std::vector& candidates) { - std::ofstream obj(filename); - if (!obj.is_open()) { - return false; - } - save_obj(obj, V, E, F, candidates); - return true; + return write_candidates_obj(filename, V, E, F, candidates); } } // namespace ipc diff --git a/tests/CMakeLists.txt b/tests/CMakeLists.txt index 0c2ea4e80..710c0bf56 100644 --- a/tests/CMakeLists.txt +++ b/tests/CMakeLists.txt @@ -48,6 +48,11 @@ source_group(TREE "${PROJECT_SOURCE_DIR}/tests" FILES ${IPC_TOOLKIT_TESTS_SOURCE target_link_libraries(ipc_toolkit_tests PRIVATE ipc::toolkit) +# Demo simulator (optional; adds the simulator tests in src/tests/demo/) +if(IPC_TOOLKIT_BUILD_DEMO) + target_link_libraries(ipc_toolkit_tests PRIVATE ipc::toolkit::demo) +endif() + # libigl (use igl::edges) include(libigl) target_link_libraries(ipc_toolkit_tests PRIVATE igl::core) diff --git a/tests/src/tests/CMakeLists.txt b/tests/src/tests/CMakeLists.txt index c9c1ea62d..c7d95a93c 100644 --- a/tests/src/tests/CMakeLists.txt +++ b/tests/src/tests/CMakeLists.txt @@ -35,9 +35,12 @@ add_subdirectory(broad_phase) add_subdirectory(candidates) add_subdirectory(ccd) add_subdirectory(collisions) +add_subdirectory(demo) add_subdirectory(distance) +add_subdirectory(dynamics) add_subdirectory(friction) add_subdirectory(geometry) +add_subdirectory(io) add_subdirectory(math) add_subdirectory(ogc) add_subdirectory(potential) diff --git a/tests/src/tests/broad_phase/test_lbvh.cpp b/tests/src/tests/broad_phase/test_lbvh.cpp index f1a630823..293d13984 100644 --- a/tests/src/tests/broad_phase/test_lbvh.cpp +++ b/tests/src/tests/broad_phase/test_lbvh.cpp @@ -231,6 +231,141 @@ TEST_CASE("LBVH::detect_*_candidates", "[broad_phase][lbvh]") #endif } +TEST_CASE( + "Querying an LBVH with bare leaves finds every overlapping box pair", + "[broad_phase][lbvh]") +{ + const std::string mesh = + GENERATE("two-cubes-close.ply", "bunny (lowpoly).ply"); + + Eigen::MatrixXd vertices; + Eigen::MatrixXi edges, faces; + REQUIRE(tests::load_mesh(mesh, vertices, edges, faces)); + + // The other set is the same mesh swept straight through itself along a + // diagonal, so every swept vertex box contains its original vertex and + // every query type has candidates. + const double diagonal = + (vertices.colwise().maxCoeff() - vertices.colwise().minCoeff()).norm(); + const Eigen::RowVector3d offset = + 0.01 * diagonal * Eigen::RowVector3d(1, -1, 0.5).normalized(); + constexpr double inflation_radius = 1e-3; + + LBVH lbvh; + lbvh.build(vertices, edges, faces, inflation_radius); + + AABBs this_vertex_boxes, this_edge_boxes, this_face_boxes; + build_vertex_boxes(vertices, this_vertex_boxes, inflation_radius); + build_edge_boxes(this_vertex_boxes, edges, this_edge_boxes); + build_face_boxes(this_vertex_boxes, faces, this_face_boxes); + + AABBs other_vertex_boxes, other_edge_boxes, other_face_boxes; + build_vertex_boxes( + vertices.rowwise() - offset, vertices.rowwise() + offset, + other_vertex_boxes, inflation_radius); + build_edge_boxes(other_vertex_boxes, edges, other_edge_boxes); + build_face_boxes(other_vertex_boxes, faces, other_face_boxes); + + LBVH::Leaves other_leaves; + other_leaves.build(other_vertex_boxes, edges, faces); + + // The LBVH rounds its boxes outward to floats, so the pairs it finds sit + // between the exact (double-precision) overlaps and the overlaps of boxes + // padded by far more than that rounding. We compare (this id, other id) + // pairs rather than candidates because the vertex-vertex and edge-edge + // candidates compare as unordered pairs, which would merge (i, j) with + // (j, i) even though i and j index different sets here. + using Pairs = std::vector>; + const double pad = 1e-6 * diagonal; + const auto check = [&](const AABBs& a, const AABBs& b, Pairs found) { + Pairs exact, padded; + for (size_t i = 0; i < a.size(); ++i) { + for (size_t j = 0; j < b.size(); ++j) { + const double gap = + (a[i].min - b[j].max).max(b[j].min - a[i].max).maxCoeff(); + if (gap <= 0) { + exact.emplace_back(i, j); + } + if (gap <= pad) { + padded.emplace_back(i, j); + } + } + } + std::sort(found.begin(), found.end()); + REQUIRE(!exact.empty()); + CHECK(std::adjacent_find(found.begin(), found.end()) == found.end()); + CHECK( + std::includes( + found.begin(), found.end(), exact.begin(), exact.end())); + CHECK( + std::includes( + padded.begin(), padded.end(), found.begin(), found.end())); + }; + + const auto accept_all = [](size_t, size_t) { return true; }; + // Each query's candidates, as (this id, other id) pairs. + const auto pairs = [](const auto& candidates, auto to_pair) { + Pairs out; + for (const auto& c : candidates) { + out.push_back(to_pair(c)); + } + return out; + }; + { + std::vector found; + lbvh.detect_vertex_vertex_candidates(other_leaves, accept_all, found); + check( + this_vertex_boxes, other_vertex_boxes, + pairs(found, [](const auto& c) { + return std::make_pair(c.vertex0_id, c.vertex1_id); + })); + } + { + std::vector found; + lbvh.detect_edge_vertex_candidates(other_leaves, accept_all, found); + check( + this_edge_boxes, other_vertex_boxes, + pairs(found, [](const auto& c) { + return std::make_pair(c.edge_id, c.vertex_id); + })); + } + { + std::vector found; + lbvh.detect_vertex_edge_candidates(other_leaves, accept_all, found); + check( + this_vertex_boxes, other_edge_boxes, + pairs(found, [](const auto& c) { + return std::make_pair(c.vertex_id, c.edge_id); + })); + } + { + std::vector found; + lbvh.detect_edge_edge_candidates(other_leaves, accept_all, found); + check( + this_edge_boxes, other_edge_boxes, pairs(found, [](const auto& c) { + return std::make_pair(c.edge0_id, c.edge1_id); + })); + } + { + std::vector found; + lbvh.detect_face_vertex_candidates(other_leaves, accept_all, found); + check( + this_face_boxes, other_vertex_boxes, + pairs(found, [](const auto& c) { + return std::make_pair(c.face_id, c.vertex_id); + })); + } + { + std::vector found; + lbvh.detect_vertex_face_candidates(other_leaves, accept_all, found); + check( + this_vertex_boxes, other_face_boxes, + pairs(found, [](const auto& c) { + return std::make_pair(c.vertex_id, c.face_id); + })); + } +} + TEST_CASE("LBVH single-primitive trees", "[broad_phase][lbvh]") { // A BVH over a single primitive is one node, which is both the root and a diff --git a/tests/src/tests/candidates/test_candidates.cpp b/tests/src/tests/candidates/test_candidates.cpp index 32af0d84f..9507e8f1f 100644 --- a/tests/src/tests/candidates/test_candidates.cpp +++ b/tests/src/tests/candidates/test_candidates.cpp @@ -5,8 +5,8 @@ #include #include #include +#include -#include #include #include @@ -233,8 +233,8 @@ TEST_CASE( "Candidate adjacency lists join candidate partners with mesh neighbors", "[candidates]") { - const double c = std::cos(2 * igl::PI / 3), s = std::sin(2 * igl::PI / 3); - const double r = std::cos(igl::PI / 3), q = std::sin(igl::PI / 3); + const double c = std::cos(2 * ipc::PI / 3), s = std::sin(2 * ipc::PI / 3); + const double r = std::cos(ipc::PI / 3), q = std::sin(ipc::PI / 3); Eigen::MatrixXd V(8, 3); V << 0, 0, 0.05, 1, 0, 1, c, s, 1, c, -s, 1, // 0, 0, -0.05, r, q, -1, -1, 0, -1, r, -q, -1; diff --git a/tests/src/tests/ccd/test_nonlinear_ccd.cpp b/tests/src/tests/ccd/test_nonlinear_ccd.cpp index 6236c5009..21086e313 100644 --- a/tests/src/tests/ccd/test_nonlinear_ccd.cpp +++ b/tests/src/tests/ccd/test_nonlinear_ccd.cpp @@ -2,10 +2,11 @@ #include #include +#include #include +#include #include - -#include +#include using namespace ipc; @@ -30,7 +31,7 @@ class RotationalTrajectory : virtual public NonlinearTrajectory { double max_distance_from_linear(const double t0, const double t1) const override { - if (z_angular_velocity * (t1 - t0) >= 2 * igl::PI) { + if (z_angular_velocity * (t1 - t0) >= 2 * ipc::PI) { // This is the most conservative estimate return 2 * (point - center).norm(); // 2 * radius } @@ -141,7 +142,7 @@ class Rigid2DTrajectory : virtual public ipc::NonlinearTrajectory { double max_distance_from_linear(const double t0, const double t1) const override { - if (delta_rotation * (t1 - t0) >= 2 * igl::PI) { + if (delta_rotation * (t1 - t0) >= 2 * ipc::PI) { // This is the most conservative estimate return 2 * position.norm(); // 2 * radius } @@ -169,18 +170,18 @@ TEST_CASE("Nonlinear Point-Point CCD", "[ccd][nonlinear][point-point]") SECTION("Analytic max") { p1 = std::make_unique( - Eigen::Vector2d(1, 0), Eigen::Vector2d::Zero(), igl::PI); + Eigen::Vector2d(1, 0), Eigen::Vector2d::Zero(), ipc::PI); } #ifdef IPC_TOOLKIT_WITH_FILIB SECTION("Interval max") { p1 = std::make_unique( - Eigen::Vector2d(1, 0), Eigen::Vector2d::Zero(), igl::PI); + Eigen::Vector2d(1, 0), Eigen::Vector2d::Zero(), ipc::PI); } #endif NonlinearCCD ccd; - ccd.conservative_rescaling = 0.9; + ccd.conservative_rescaling = 0.99; double toi; bool collision = ccd.point_point_ccd(p0, *p1, toi); @@ -190,16 +191,55 @@ TEST_CASE("Nonlinear Point-Point CCD", "[ccd][nonlinear][point-point]") CHECK(toi == Catch::Approx(0.5).margin(1e-2)); } +TEST_CASE( + "Nonlinear CCD stops once the gap shrinks to its target", + "[ccd][nonlinear][point-point]") +{ + const double s = GENERATE(0.5, 0.8, 0.9); + // A point swings around the unit circle toward a static point. At y = 1 + // they collide at t = 0.5. At y = 1.05 they never touch, but they still + // come within 0.05 of each other, which closes more than s of the gap. + const double y = GENERATE(1.0, 1.05); + + const StaticTrajectory p0(Eigen::Vector2d(0, y)); + const RotationalTrajectory p1( + Eigen::Vector2d(1, 0), Eigen::Vector2d::Zero(), ipc::PI); + const auto distance = [&](const double t) { + return (p0(t) - p1(t)).norm(); + }; + const double target = (1 - s) * distance(0); + + const bool use_additive = GENERATE(false, true); + NonlinearCCD ccd( + use_additive ? std::shared_ptr( + std::make_shared()) + : std::make_shared()); + ccd.conservative_rescaling = s; + + double toi; + const bool collision = ccd.point_point_ccd(p0, p1, toi); + + // We expect the step to stop before the closest approach, keeping at + // least (1 - s) of the initial gap but not much more. Additive CCD stops + // within a few percent of the target. Tight Inclusion measures the + // minimum separation in the infinity norm, so it can stop while the + // Euclidean distance is still up to √2 times the target in 2D. + CHECK(collision); + CHECK(toi <= 0.5); + CHECK(distance(toi) >= target); + CHECK(distance(toi) <= (use_additive ? 1.05 : std::sqrt(2.0)) * target); +} + TEST_CASE("Nonlinear Point-Edge CCD", "[ccd][nonlinear][point-edge]") { const StaticTrajectory p(Eigen::Vector2d(0, 0.5)); const RotationalTrajectory e0( - Eigen::Vector2d(-1, 0), Eigen::Vector2d::Zero(), 2 * igl::PI); + Eigen::Vector2d(-1, 0), Eigen::Vector2d::Zero(), 2 * ipc::PI); const RotationalTrajectory e1( - Eigen::Vector2d(1, 0), Eigen::Vector2d::Zero(), 2 * igl::PI); + Eigen::Vector2d(1, 0), Eigen::Vector2d::Zero(), 2 * ipc::PI); NonlinearCCD ccd; - ccd.conservative_rescaling = 0.9; + ccd.conservative_rescaling = 0.99; double toi; bool collision = ccd.point_edge_ccd(p, e0, e1, toi); @@ -219,13 +259,13 @@ TEST_CASE("Rigid 2D Trajectory", "[ccd][nonlinear][point-edge]") // Rotating edge const Rigid2DTrajectory e0( Eigen::Vector2d(-1, 0), Eigen::Vector2d::Zero(), - Eigen::Vector2d::Zero(), 0, igl::PI); + Eigen::Vector2d::Zero(), 0, ipc::PI); const Rigid2DTrajectory e1( Eigen::Vector2d(+1, 0), Eigen::Vector2d::Zero(), - Eigen::Vector2d::Zero(), 0, igl::PI); + Eigen::Vector2d::Zero(), 0, ipc::PI); NonlinearCCD ccd; - ccd.conservative_rescaling = 0.9; + ccd.conservative_rescaling = 0.99; double toi; bool collision = ccd.point_edge_ccd(p, e0, e1, toi); @@ -238,14 +278,17 @@ TEST_CASE("Rigid 2D Trajectory", "[ccd][nonlinear][point-edge]") TEST_CASE("Nonlinear Edge-Edge CCD", "[ccd][nonlinear][edge-edge]") { const RotationalTrajectory ea0( - Eigen::Vector3d(-1, 0, 0), Eigen::Vector3d::Zero(), 2 * igl::PI); + Eigen::Vector3d(-1, 0, 0), Eigen::Vector3d::Zero(), 2 * ipc::PI); const RotationalTrajectory ea1( - Eigen::Vector3d(1, 0, 0), Eigen::Vector3d::Zero(), 2 * igl::PI); + Eigen::Vector3d(1, 0, 0), Eigen::Vector3d::Zero(), 2 * ipc::PI); const StaticTrajectory eb0(Eigen::Vector3d(-1, 0.5, 0)); const StaticTrajectory eb1(Eigen::Vector3d(1, 0.5, 0)); + NonlinearCCD ccd; + ccd.conservative_rescaling = 0.99; + double toi; - bool collision = NonlinearCCD().edge_edge_ccd(ea0, ea1, eb0, eb1, toi); + bool collision = ccd.edge_edge_ccd(ea0, ea1, eb0, eb1, toi); CHECK(collision); CHECK(toi <= 30 / 360.0); @@ -257,15 +300,15 @@ TEST_CASE("Nonlinear Point-Triangle CCD", "[ccd][nonlinear][point-triangle]") const double x = GENERATE(-0.1, 0, 0.1); const RotationalTrajectory t0( - Eigen::Vector3d(1, 0, 0), Eigen::Vector3d::Zero(), igl::PI); + Eigen::Vector3d(1, 0, 0), Eigen::Vector3d::Zero(), ipc::PI); const RotationalTrajectory t1( - Eigen::Vector3d(x, 0, 1), Eigen::Vector3d::Zero(), igl::PI); + Eigen::Vector3d(x, 0, 1), Eigen::Vector3d::Zero(), ipc::PI); const RotationalTrajectory t2( - Eigen::Vector3d(x, 0, -1), Eigen::Vector3d::Zero(), igl::PI); + Eigen::Vector3d(x, 0, -1), Eigen::Vector3d::Zero(), ipc::PI); const StaticTrajectory p(Eigen::Vector3d(0, 0.5, 0)); NonlinearCCD ccd; - ccd.conservative_rescaling = 0.9; + ccd.conservative_rescaling = 0.99; double toi; bool collision = ccd.point_triangle_ccd(p, t0, t1, t2, toi); diff --git a/tests/src/tests/collisions/test_obstacles.cpp b/tests/src/tests/collisions/test_obstacles.cpp index eda83bfff..7b38dfa97 100644 --- a/tests/src/tests/collisions/test_obstacles.cpp +++ b/tests/src/tests/collisions/test_obstacles.cpp @@ -9,9 +9,9 @@ #include #include #include +#include #include -#include #include @@ -28,9 +28,9 @@ CollisionMesh star_mesh(const std::vector& obstacle_vertex) constexpr double gap = 0.05; Eigen::MatrixXd V(8, 3); for (int i = 0; i < 3; i++) { - const double a = 2 * igl::PI * i / 3; + const double a = 2 * ipc::PI * i / 3; V.row(i) << std::cos(a), std::sin(a), gap; - V.row(4 + i) << std::cos(a + igl::PI / 3), std::sin(a + igl::PI / 3), 0; + V.row(4 + i) << std::cos(a + ipc::PI / 3), std::sin(a + ipc::PI / 3), 0; } V.row(3) << 0, 0, gap + 1; V.row(7) << 0, 0, -1; diff --git a/tests/src/tests/demo/CMakeLists.txt b/tests/src/tests/demo/CMakeLists.txt new file mode 100644 index 000000000..786d3d558 --- /dev/null +++ b/tests/src/tests/demo/CMakeLists.txt @@ -0,0 +1,17 @@ +# Tests for the demo simulator (demo/); only built when the demo is. +if(NOT IPC_TOOLKIT_BUILD_DEMO) + return() +endif() + +set(SOURCES + test_2d_simulator.cpp + test_codim.cpp + test_affine_simulator.cpp + test_friction_simulator.cpp + test_joints.cpp + test_rigid_simulator.cpp + test_static_kinematic.cpp +) + +source_group(TREE "${CMAKE_CURRENT_SOURCE_DIR}" PREFIX "Source Files" FILES ${SOURCES}) +target_sources(ipc_toolkit_tests PRIVATE ${SOURCES}) diff --git a/tests/src/tests/demo/test_2d_simulator.cpp b/tests/src/tests/demo/test_2d_simulator.cpp new file mode 100644 index 000000000..d673a465d --- /dev/null +++ b/tests/src/tests/demo/test_2d_simulator.cpp @@ -0,0 +1,336 @@ +// End-to-end 2D simulation tests (rigid and affine). + +#include +#include +#include +#include + +#include + +#include +#include +#include +#include +#include + +#include + +#include + +using namespace ipc; +using namespace ipc::rigid; +using ipc::demo::Simulator; + +namespace { + +/// A unit square outline (4 vertices, 4 edges, no faces). +void unit_square(Eigen::MatrixXd& V, Eigen::MatrixXi& E, Eigen::MatrixXi& F) +{ + V.resize(4, 2); + V << -0.5, -0.5, 0.5, -0.5, 0.5, 0.5, -0.5, 0.5; + E.resize(4, 2); + E << 0, 1, 1, 2, 2, 3, 3, 0; + F.resize(0, 3); +} + +Simulator::BodyDynamics generate_mode() +{ + return GENERATE( + Simulator::BodyDynamics::RIGID, Simulator::BodyDynamics::AFFINE); +} + +} // namespace + +TEST_CASE("2D ballistic motion", "[2d][rigid][simulator]") +{ + Eigen::MatrixXd V; + Eigen::MatrixXi E, F; + unit_square(V, E, F); + + std::vector initial_poses(1, Pose::identity(2)); + initial_poses[0].position = Eigen::Vector2d(0, 100.0); // free flight + + auto bodies = RigidBodies::build_from_meshes( + { V }, { E }, { F }, { 1000.0 }, initial_poses); + + std::vector initial_velocities(1, Pose::identity(2)); + initial_velocities[0].position = Eigen::Vector2d(1.0, 2.0); + initial_velocities[0].rotation = VectorMax3d::Constant(1, 0.7); // ω + + Simulator::Settings settings; + settings.solver_params["grad_norm_tol"] = 1e-10; + + const double dt = 0.01; + Simulator sim(bodies, initial_poses, initial_velocities, dt, settings); + + const Eigen::Vector2d p0 = initial_poses[0].position; + const double theta0 = initial_poses[0].rotation(0); + const Eigen::Vector2d v0 = initial_velocities[0].position; + const double omega = initial_velocities[0].rotation(0); + const Eigen::Vector2d g = settings.gravity.head<2>(); + + const int n = 10; + for (int i = 0; i < n; ++i) { + REQUIRE(sim.step()); + } + + // Implicit Euler: pₙ = p₀ + n dt v₀ + dt² g n(n+1)/2. + const Eigen::Vector2d pn_expected = + p0 + n * dt * v0 + dt * dt * g * (n * (n + 1) / 2.0); + CHECK( + (Eigen::Vector2d(sim.poses()[0].position) - pn_expected).norm() + == Catch::Approx(0).margin(1e-6)); + + const Pose pose_n = sim.rigid_poses()[0]; + // θₙ ≈ θ₀ + n dt ω. The rotation is integrated in matrix space (as in 3D), + // so the recovered angle differs from the linear-in-angle value by an + // O((dt ω)³) per-step amount; check with a correspondingly looser margin. + const double theta_expected = + std::remainder(theta0 + n * dt * omega, 2 * ipc::PI); + CHECK(pose_n.rotation(0) == Catch::Approx(theta_expected).margin(1e-4)); +} + +TEST_CASE("2D simulator gradient and hessian", "[2d][simulator]") +{ + const auto mode = generate_mode(); + CAPTURE(int(mode)); + + Eigen::MatrixXd V; + Eigen::MatrixXi E, F; + unit_square(V, E, F); + + Simulator::Settings settings; + settings.body_dynamics = mode; + // Exercise the smooth (area-weighted / IMPROVED_MAX_APPROX) collision set: + // the default is now the conventional IPC set (rigid-ipc parity), whose + // non-smooth max approximation makes a near-boundary FD gradient check + // fail. + settings.use_area_weighting = true; + + // Two dynamic squares in contact (0.5·dhat gap). NOTE: both dynamic — + // a STATIC body's pinned DOFs are zeroed in gradient() by design, which + // would (correctly) disagree with the FD of value(). + std::vector initial_poses(2, Pose::identity(2)); + initial_poses[1].position = Eigen::Vector2d(0.1, 1.0 + 0.5 * settings.dhat); + + auto bodies = RigidBodies::build_from_meshes( + { V, V }, { E, E }, { F, F }, { 1000.0, 1000.0 }, initial_poses); + + Simulator sim(bodies, initial_poses, /*dt=*/0.01, settings); + + const Eigen::VectorXd x0 = sim.kinematics().dof(initial_poses); + sim.initialize_step(); + sim.update_collisions(x0); + REQUIRE(!sim.normal_collisions().empty()); + + // Perturb the dynamic body (translation + rotation) to exercise the 2D + // chain rule (including the rigid second-order term). + Eigen::VectorXd x = x0; + const int body_ndof = int(x.size()) / 2; + x(body_ndof) += 1e-4; + x(body_ndof + int(bodies->dim())) += 1e-3; + + Eigen::VectorXd grad; + sim.gradient(x, grad); + + Eigen::VectorXd fd_grad; + fd::finite_gradient( + x, [&](const Eigen::VectorXd& y) { return sim.value(y); }, fd_grad); + CHECK(fd::compare_gradient(grad, fd_grad)); + if (!fd::compare_gradient(grad, fd_grad)) { + std::cout << "analytic:\n" << grad << "\n\nnumeric:\n" << fd_grad; + } + + sim.set_project_to_psd(false); + Eigen::SparseMatrix hess_sparse; + sim.hessian(x, hess_sparse); + const Eigen::MatrixXd hess = hess_sparse; + + Eigen::MatrixXd fd_hess; + fd::finite_jacobian( + x, + [&](const Eigen::VectorXd& y) { + Eigen::VectorXd g; + sim.gradient(y, g); + return g; + }, + fd_hess); + CHECK(fd::compare_hessian(hess, fd_hess, 1e-2)); +} + +TEST_CASE("2D box drop", "[2d][simulator]") +{ + const auto mode = generate_mode(); + CAPTURE(int(mode)); + + Eigen::MatrixXd V; + Eigen::MatrixXi E, F; + unit_square(V, E, F); + + // A static floor square and a dynamic square dropped from above. + std::vector initial_poses(2, Pose::identity(2)); + initial_poses[1].position = Eigen::Vector2d(0.1, 1.3); + + auto bodies = RigidBodies::build_from_meshes( + { V, V }, { E, E }, { F, F }, { 1000.0, 1000.0 }, initial_poses); + (*bodies)[0].set_type(RigidBody::Type::STATIC); + + Simulator::Settings settings; + settings.body_dynamics = mode; + + Simulator sim(bodies, initial_poses, /*dt=*/0.01, settings); + REQUIRE(sim.run(/*t_end=*/0.75)); + + // The dynamic square rests on top of the floor (no tunneling, no + // penetration): its center stays about one edge length above the floor. + CHECK(sim.poses()[1].position(1) > 0.99); + CHECK(sim.poses()[1].position(1) < 1.1); + + if (mode == Simulator::BodyDynamics::AFFINE) { + // A stays (numerically) a rotation. + const MatrixMax3d A = sim.poses()[1].rotation; + CHECK( + (A.transpose() * A - Eigen::Matrix2d::Identity()).norm() + == Catch::Approx(0).margin(1e-4)); + } +} + +TEST_CASE("2D friction slide", "[2d][friction][simulator]") +{ + const double mu = 0.3, g = 9.81, v0 = 1.0; + + Eigen::MatrixXd V; + Eigen::MatrixXi E, F; + unit_square(V, E, F); + + Simulator::Settings settings; + settings.friction_coefficient = mu; + settings.friction_iterations = -1; + // We converge each Newton solve tightly so the friction lagging samples + // force-balanced normal forces. At 1e-3, the first solve of a step stops + // on the velocity criterion with the contact still far out of balance, so + // the box dips into the floor and the lagging iterations oscillate. Every + // few steps the lagging then stalls on an iterate with an inflated normal + // force, and the extra friction stops the box about 20% short. + settings.velocity_conv_tol = 1e-4; + // Area-weighted (smooth) collision set for a tight friction-force check; + // the default is now the conventional IPC set (rigid-ipc parity). + settings.use_area_weighting = true; + + // Wide static floor (three squares would be nicer, but a stretched one + // works): scale the floor square horizontally. + Eigen::MatrixXd V_floor = V; + V_floor.col(0) *= 10.0; + + std::vector initial_poses(2, Pose::identity(2)); + initial_poses[1].position = Eigen::Vector2d(-2, 1.0 + 1.1 * settings.dhat); + + auto bodies = RigidBodies::build_from_meshes( + { V_floor, V }, { E, E }, { F, F }, { 1000.0, 1000.0 }, initial_poses); + (*bodies)[0].set_type(RigidBody::Type::STATIC); + + std::vector initial_velocities(2, Pose::identity(2)); + initial_velocities[1].position = Eigen::Vector2d(v0, 0); + + const double dt = 0.01; + Simulator sim(bodies, initial_poses, initial_velocities, dt, settings); + + const double x_start = initial_poses[1].position(0); + const double stopping_time = v0 / (mu * g); + const double stopping_dist = v0 * v0 / (2 * mu * g); + + REQUIRE(sim.run(/*t_end=*/2 * stopping_time)); + const double slide = sim.poses()[1].position(0) - x_start; + CHECK(slide == Catch::Approx(stopping_dist).margin(3 * v0 * dt)); +} + +TEST_CASE("2D kinematic body", "[2d][kinematic][simulator]") +{ + const auto mode = generate_mode(); + CAPTURE(int(mode)); + + Eigen::MatrixXd V; + Eigen::MatrixXi E, F; + unit_square(V, E, F); + + std::vector initial_poses(1, Pose::identity(2)); + auto bodies = RigidBodies::build_from_meshes( + { V }, { E }, { F }, { 1000.0 }, initial_poses); + (*bodies)[0].set_type(RigidBody::Type::KINEMATIC); + + std::vector initial_velocities(1, Pose::identity(2)); + initial_velocities[0].position = Eigen::Vector2d(1.0, 0.5); + initial_velocities[0].rotation = VectorMax3d::Constant(1, 0.3); + + Simulator::Settings settings; + settings.body_dynamics = mode; + + const double dt = 0.01; + Simulator sim(bodies, initial_poses, initial_velocities, dt, settings); + + const Eigen::Vector2d p0 = initial_poses[0].position; + const double theta0 = initial_poses[0].rotation(0); + + const int n = 10; + for (int i = 0; i < n; ++i) { + REQUIRE(sim.step()); + } + + const Eigen::Vector2d p_expected = p0 + n * dt * Eigen::Vector2d(1.0, 0.5); + CHECK( + (Eigen::Vector2d(sim.poses()[0].position) - p_expected).norm() + < 2e-3 * n * dt); + CHECK( + sim.rigid_poses()[0].rotation(0) + == Catch::Approx(theta0 + n * dt * 0.3).margin(5e-3)); +} + +TEST_CASE("2D joints", "[2d][affine][joints][simulator]") +{ + Eigen::MatrixXd V; + Eigen::MatrixXi E, F; + unit_square(V, E, F); + + std::vector initial_poses(1, Pose::identity(2)); + auto bodies = RigidBodies::build_from_meshes( + { V }, { E }, { F }, { 1000.0 }, initial_poses); + + auto joints = + std::make_shared(bodies, initial_poses); + + // A 2D hinge is a point connection. + CHECK_THROWS_AS( + joints->add_hinge( + 0, 0, Eigen::Vector2d(0, 0.5), Eigen::Vector2d(0, -0.5)), + std::invalid_argument); + + // Pendulum: pin the top-left corner of the square; it swings under + // gravity, keeping the pinned point fixed. + const Eigen::Vector2d pivot = + initial_poses[0].rotation_matrix() * Eigen::Vector2d(-0.5, 0.5) + + Eigen::Vector2d(initial_poses[0].position); + joints->add_fixed_point(0, pivot); + + Simulator::Settings settings; + settings.body_dynamics = Simulator::BodyDynamics::AFFINE; + + Simulator sim(bodies, initial_poses, joints, /*dt=*/0.01, settings); + + double max_swing = 0; + for (int i = 0; i < 50; ++i) { + REQUIRE(sim.step()); + // The pinned material point stays at the pivot. + const auto& pose = sim.poses()[0]; + const Eigen::Vector2d p = Eigen::Matrix2d(pose.rotation) + * (initial_poses[0].rotation_matrix().transpose() + * (pivot - Eigen::Vector2d(initial_poses[0].position))) + + Eigen::Vector2d(pose.position); + CHECK((p - pivot).norm() == Catch::Approx(0).margin(1e-8)); + max_swing = std::max( + max_swing, + std::abs( + Eigen::Vector2d(pose.position).x() + - initial_poses[0].position(0))); + } + CHECK(max_swing > 0.01); // it actually swings +} diff --git a/tests/src/tests/demo/test_affine_simulator.cpp b/tests/src/tests/demo/test_affine_simulator.cpp new file mode 100644 index 000000000..8dab6162c --- /dev/null +++ b/tests/src/tests/demo/test_affine_simulator.cpp @@ -0,0 +1,179 @@ +#include +#include + +#include + +#include +#include +#include +#include +#include + +#include + +#include +#include + +using namespace ipc; +using namespace ipc::affine; +using BodyDynamics = ipc::demo::Simulator::BodyDynamics; +using ipc::demo::Simulator; + +// NOTE: The inertial-term convention (the affine mass matrix equals the rigid +// trace form on rigid states) is covered by the change-of-variables and body- +// potentials unit tests ([to_affine], [body_potentials]). + +TEST_CASE("Affine DOF jacobian", "[affine][dof]") +{ + Eigen::MatrixXd V; + Eigen::MatrixXi E, F; + tests::load_mesh("cube.ply", V, E, F); + + std::vector initial_poses(2, rigid::Pose::identity(3)); + initial_poses[1].position = Eigen::Vector3d(2, 0, 0); + auto bodies = rigid::RigidBodies::build_from_meshes( + { V, V }, { E, E }, { F, F }, { 1000.0, 1000.0 }, initial_poses); + + const Eigen::SparseMatrix J = affine_jacobian(*bodies); + + const Eigen::VectorXd x = Eigen::VectorXd::Random(24); + + // V(x) is linear, so the FD jacobian must match exactly + Eigen::MatrixXd J_fd; + fd::finite_jacobian( + x, + [&](const Eigen::VectorXd& x_) -> Eigen::VectorXd { + return vertices(*bodies, x_).reshaped(); + }, + J_fd); + + CHECK(fd::compare_jacobian(Eigen::MatrixXd(J), J_fd)); +} + +TEST_CASE("Affine simulator gradient and hessian", "[affine][simulator]") +{ + Eigen::MatrixXd V; + Eigen::MatrixXi E, F; + tests::load_mesh("cube.ply", V, E, F); + + // Two cubes close enough that the barrier is active + std::vector initial_poses(2, rigid::Pose::identity(3)); + initial_poses[1].position = Eigen::Vector3d(1.0005, 0, 0); + + auto bodies = rigid::RigidBodies::build_from_meshes( + { V, V }, { E, E }, { F, F }, { 1000.0, 1000.0 }, initial_poses); + + // Use a mild orthogonality stiffness: derivative correctness is + // scale-independent, and the paper-recommended κ dwarfs the other terms, + // hurting the finite-difference conditioning. + Simulator::Settings settings; + settings.body_dynamics = BodyDynamics::AFFINE; + settings.orthogonality_stiffness = 1e6; + + Simulator sim(bodies, initial_poses, /*dt=*/0.01, settings); + + // Perturbed affine state (slightly non-rigid A) + Eigen::VectorXd x(24); + for (size_t i = 0; i < 2; ++i) { + x.segment<3>(12 * i) = initial_poses[i].position; + x.segment<9>(12 * i + 3) = + initial_poses[i].rotation_matrix().reshaped(); + } + // Deterministic pseudo-random perturbation: an unlucky perturbation can + // separate the cubes beyond dhat, deactivating the barrier this test + // requires. std::rand() is not used here because it is implementation- + // defined and seeding it with std::srand(0) does not reproduce the same + // sequence across compilers/platforms (e.g., MSVC vs. glibc/libc++), + // which previously made this test flaky on Windows CI. + Eigen::VectorXd perturbation(24); + for (int i = 0; i < perturbation.size(); ++i) { + perturbation(i) = std::sin(12.9898 * (i + 1)); + } + x += 0.001 * perturbation; + + sim.initialize_step(); + sim.update_collisions(x); + REQUIRE(sim.normal_collisions().size() > 0); + + // --- Gradient ----------------------------------------------------------- + + Eigen::VectorXd g; + sim.gradient(x, g); + + Eigen::VectorXd fd_g; + fd::finite_gradient( + x, [&](const Eigen::VectorXd& x_) { return sim.value(x_); }, fd_g); + + CHECK(fd::compare_gradient(g, fd_g)); + if (!fd::compare_gradient(g, fd_g)) { + std::cout << "analytic:\n" << g.transpose() << "\n\n"; + std::cout << "numerical:\n" << fd_g.transpose() << "\n\n"; + } + + // --- Hessian + // -------------------------------------------------------------- + + sim.set_project_to_psd(false); // FD comparison needs the exact Hessian + Eigen::SparseMatrix H_sparse; + sim.hessian(x, H_sparse); + const Eigen::MatrixXd H = H_sparse; + + Eigen::MatrixXd fd_H; + fd::finite_jacobian( + x, + [&](const Eigen::VectorXd& x_) { + Eigen::VectorXd g_; + sim.gradient(x_, g_); + return g_; + }, + fd_H); + + CHECK(fd::compare_hessian(H, fd_H)); + if (!fd::compare_hessian(H, fd_H)) { + std::cout << "analytic:\n" << H << "\n\n"; + std::cout << "numerical:\n" << fd_H << "\n\n"; + } +} + +TEST_CASE("Affine simulator cube drop", "[affine][simulator]") +{ + Eigen::MatrixXd V; + Eigen::MatrixXi E, F; + tests::load_mesh("cube.ply", V, E, F); + + std::vector initial_poses(1, rigid::Pose::identity(3)); + initial_poses[0].position = Eigen::Vector3d(0, 0.6, 0); + + auto bodies = rigid::RigidBodies::build_from_meshes( + { V }, { E }, { F }, { 1000.0 }, initial_poses); + bodies->planes.emplace_back(Eigen::Vector3d(0, 1, 0), 0); // ground y = 0 + + Simulator::Settings settings; // paper-recommended default stiffness + settings.body_dynamics = BodyDynamics::AFFINE; + + Simulator sim(bodies, initial_poses, /*dt=*/0.01, settings); + + for (int i = 0; i < 25; ++i) { + REQUIRE(sim.step()); + + const affine::Pose& pose = sim.pose_history().back()[0]; + + // A stays near SO(3) under the stiff orthogonality potential + const Eigen::Matrix3d AtA_minus_I = + pose.rotation.transpose() * pose.rotation + - Eigen::Matrix3d::Identity(); + CHECK(AtA_minus_I.norm() < 0.01); + } + + // The cube must rest on (not penetrate) the ground + const Eigen::MatrixXd V_final = vertices(*bodies, [&] { + Eigen::VectorXd x(12); + const affine::Pose& pose = sim.pose_history().back()[0]; + x.head<3>() = pose.position; + x.tail<9>() = pose.rotation.reshaped(); + return x; + }()); + CHECK((V_final.col(1).array() > -1e-10).all()); + // It fell and is near the ground + CHECK(V_final.col(1).minCoeff() < 0.1); +} diff --git a/tests/src/tests/demo/test_codim.cpp b/tests/src/tests/demo/test_codim.cpp new file mode 100644 index 000000000..2612f0b3b --- /dev/null +++ b/tests/src/tests/demo/test_codim.cpp @@ -0,0 +1,240 @@ +// Codimensional (point-cloud / segment-net) contact tests for the rigid +// body-pair broad phase. + +#include +#include + +#include + +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include + +using namespace ipc; +using namespace ipc::rigid; + +namespace { + +/// A 3×3 grid of isolated points in the z = 0 plane (3D) with a small tetra +/// offset so the inertia is nonsingular. +void point_grid(Eigen::MatrixXd& V, Eigen::MatrixXi& E, Eigen::MatrixXi& F) +{ + V.resize(10, 3); + int r = 0; + for (int i = -1; i <= 1; ++i) { + for (int j = -1; j <= 1; ++j) { + V.row(r++) = Eigen::RowVector3d(0.5 * i, 0, 0.5 * j); + } + } + V.row(r++) = Eigen::RowVector3d(0, 0.3, 0); // break planarity + E.resize(0, 2); + F.resize(0, 3); +} + +/// A zig-zag segment net (edges only, no faces). +void segment_net(Eigen::MatrixXd& V, Eigen::MatrixXi& E, Eigen::MatrixXi& F) +{ + V.resize(5, 3); + V << -1, 0, 0, -0.5, 0.2, 0, 0, 0, 0, 0.5, 0.2, 0, 1, 0, 0; + E.resize(4, 2); + E << 0, 1, 1, 2, 2, 3, 3, 4; + F.resize(0, 3); +} + +} // namespace + +TEST_CASE("Codim broad phase completeness", "[codim][rigid][candidates]") +{ + Eigen::MatrixXd V_cloud; + Eigen::MatrixXi E_cloud, F_cloud; + point_grid(V_cloud, E_cloud, F_cloud); + + Eigen::MatrixXd V_net; + Eigen::MatrixXi E_net, F_net; + segment_net(V_net, E_net, F_net); + + // Cloud falls straight down through the net. + std::vector poses_t0(2, Pose::identity(3)); + poses_t0[0].position = Eigen::Vector3d(0, 1.0, 0); // cloud + poses_t0[1].position = Eigen::Vector3d::Zero(); // net + + auto bodies = RigidBodies::build_from_meshes( + { V_cloud, V_net }, { E_cloud, E_net }, { F_cloud, F_net }, + { 1000.0, 1000.0 }, poses_t0); + + REQUIRE(bodies->body_num_codim_vertices(0) == 10); + REQUIRE(bodies->body_num_codim_edges(1) == 4); + + std::vector poses_t1 = poses_t0; + poses_t1[0].position.y() -= 2.0; // sweep through the net + + const double inflation = 1e-3; + RigidCandidates candidates; + candidates.build(*bodies, poses_t0, poses_t1, inflation); + + // Sampled ground truth: every (codim edge, cloud vertex) pair that comes + // within the inflation radius at some sampled time must be a candidate. + std::set> close_ev; + const int n_samples = 200; + for (int s = 0; s <= n_samples; ++s) { + const double t = s / double(n_samples); + std::vector poses(2); + for (int b = 0; b < 2; ++b) { + poses[b] = Pose( + (1 - t) * poses_t0[b].position + t * poses_t1[b].position, + poses_t0[b].rotation); + } + const Eigen::MatrixXd V = bodies->vertices(poses); + for (index_t e = 0; e < bodies->edges().rows(); ++e) { + for (index_t v = 0; v < 10; ++v) { // cloud vertices are 0..9 + const double d_sq = point_edge_distance( + V.row(v), V.row(bodies->edges()(e, 0)), + V.row(bodies->edges()(e, 1))); + if (d_sq <= inflation * inflation) { + close_ev.emplace(e, v); + } + } + } + } + REQUIRE(!close_ev.empty()); + + std::set> found_ev; + for (const auto& c : candidates.ev_candidates) { + found_ev.emplace(c.edge_id, c.vertex_id); + } + for (const auto& pair : close_ev) { + CAPTURE(pair.first, pair.second); + CHECK(found_ev.count(pair) == 1); + } +} + +TEST_CASE( + "Codim vertex-vertex candidates and CCD", "[codim][rigid][candidates]") +{ + Eigen::MatrixXd V_cloud; + Eigen::MatrixXi E_cloud, F_cloud; + point_grid(V_cloud, E_cloud, F_cloud); + + // Two identical clouds, one falling straight onto the other: aligned + // points must generate vertex-vertex candidates and point-point CCD must + // stop the sweep. + std::vector poses_t0(2, Pose::identity(3)); + poses_t0[0].position = Eigen::Vector3d(0, 1.0, 0); + + auto bodies = RigidBodies::build_from_meshes( + { V_cloud, V_cloud }, { E_cloud, E_cloud }, { F_cloud, F_cloud }, + { 1000.0, 1000.0 }, poses_t0); + + std::vector poses_t1 = poses_t0; + poses_t1[0].position.y() -= 2.0; // full pass-through if unimpeded + + RigidCandidates candidates; + candidates.build(*bodies, poses_t0, poses_t1, /*inflation_radius=*/1e-3); + CHECK(!candidates.vv_candidates.empty()); + + NonlinearCCD ccd; + const double alpha = candidates.compute_collision_free_stepsize( + *bodies, poses_t0, poses_t1, /*min_distance=*/0, ccd); + CHECK(alpha < 1.0); // the clouds may not pass through each other + + // The clamped step keeps all point pairs separated. + std::vector poses_alpha(2); + for (int b = 0; b < 2; ++b) { + poses_alpha[b] = Pose( + (1 - alpha) * poses_t0[b].position + alpha * poses_t1[b].position, + poses_t0[b].rotation); + } + const Eigen::MatrixXd V = bodies->vertices(poses_alpha); + double min_gap = std::numeric_limits::infinity(); + for (int v = 0; v < 10; ++v) { + // Strictly separated (squared distance > 0). The CCD stops once the + // gap shrinks to its conservative target, so the gap left over is a + // fraction of the initial one. + const double d = point_point_distance(V.row(v), V.row(10 + v)); + CHECK(d > 0); + min_gap = std::min(min_gap, std::sqrt(d)); + } + + // The discrete (distance) overload also produces the vv candidates once + // its inflation radius reaches across the gap the CCD left. + RigidCandidates discrete; + discrete.build(*bodies, poses_alpha, /*inflation_radius=*/2 * min_gap); + CHECK(!discrete.vv_candidates.empty()); +} + +TEST_CASE("Codim point cloud drop", "[codim][simulator]") +{ + using ipc::demo::Simulator; + + Eigen::MatrixXd V_cloud; + Eigen::MatrixXi E_cloud, F_cloud; + point_grid(V_cloud, E_cloud, F_cloud); + + // A point cloud dropped onto an identical static cloud: point-point + // contacts must stop it. + std::vector initial_poses(2, Pose::identity(3)); + initial_poses[0].position = Eigen::Vector3d(0, 0.2, 0); + + // We let the falling cloud move only vertically. Points resting on points + // is an unstable equilibrium: once the clouds are offset sideways by more + // than the minimum separation, the points can pass each other without + // ever coming within it, so neither CCD nor the barrier has anything to + // stop. Whether a free cloud stays balanced would then depend on whether + // the solver keeps every Newton step exactly symmetric, which is not what + // this test is about. + VectorMax6b vertical_only(6); + vertical_only << true, false, true, true, true, true; + + auto bodies = RigidBodies::build_from_meshes( + { V_cloud, V_cloud }, { E_cloud, E_cloud }, { F_cloud, F_cloud }, + { 1000.0, 1000.0 }, initial_poses, /*convert_planes=*/false, + { vertical_only, VectorMax6b::Zero(6) }); + (*bodies)[1].set_type(RigidBody::Type::STATIC); + + Simulator sim(bodies, initial_poses, /*dt=*/0.01); + REQUIRE(sim.run(/*t_end=*/0.5)); + + // The falling cloud rests on (strictly above) the static one. + const Eigen::MatrixXd V = bodies->vertices(sim.rigid_pose_history().back()); + double min_d_sq = std::numeric_limits::infinity(); + for (int v = 0; v < 10; ++v) { + min_d_sq = + std::min(min_d_sq, point_point_distance(V.row(v), V.row(10 + v))); + } + CHECK(min_d_sq > 0); + CHECK(sim.poses()[0].position(1) > 0); // did not fall through +} + +TEST_CASE("2D codim point drop", "[2d][codim][simulator]") +{ + using ipc::demo::Simulator; + + // Two isolated 2D points (one falling, one static below). + Eigen::MatrixXd V(3, 2); + V << -0.5, 0, 0.5, 0, 0, 0.4; // three points (nonsingular inertia) + const Eigen::MatrixXi E(0, 2), F(0, 3); + + std::vector initial_poses(2, Pose::identity(2)); + initial_poses[0].position = Eigen::Vector2d(0, 0.5); + + auto bodies = RigidBodies::build_from_meshes( + { V, V }, { E, E }, { F, F }, { 1000.0, 1000.0 }, initial_poses); + (*bodies)[1].set_type(RigidBody::Type::STATIC); + + REQUIRE(bodies->body_num_codim_vertices(0) == 3); + + Simulator sim(bodies, initial_poses, /*dt=*/0.01); + REQUIRE(sim.run(/*t_end=*/0.4)); + + // Aligned point pairs collide (vertex-vertex): the falling triple rests + // above the static one. + CHECK(sim.poses()[0].position(1) > 0); +} diff --git a/tests/src/tests/demo/test_friction_simulator.cpp b/tests/src/tests/demo/test_friction_simulator.cpp new file mode 100644 index 000000000..f8793b84c --- /dev/null +++ b/tests/src/tests/demo/test_friction_simulator.cpp @@ -0,0 +1,442 @@ +// Tests for the velocity-based lagged friction in the demo simulator. + +#include +#include +#include +#include + +#include + +#include +#include +#include +#include +#include +#include + +#include + +#include + +using namespace ipc; +using namespace ipc::rigid; +using ipc::demo::Simulator; + +namespace { + +/// A cube above the ground plane y = 0 with an initial gap of +/// gap_factor·dhat. Use gap_factor < 1 to start in contact (barrier active); +/// use gap_factor slightly > 1 so contact engages naturally within a step — +/// placing a body deep inside the activation distance with a freshly seeded +/// stiff barrier produces unphysically large (lagged) normal forces. +std::shared_ptr cube_on_plane( + std::vector& initial_poses, + const double dhat, + const double density, + const double gap_factor) +{ + Eigen::MatrixXd V; + Eigen::MatrixXi E, F; + REQUIRE(tests::load_mesh("cube.ply", V, E, F)); + + initial_poses.assign(1, Pose::identity(3)); + initial_poses[0].position.y() = -V.col(1).minCoeff() + gap_factor * dhat; + + auto bodies = RigidBodies::build_from_meshes( + { V }, { E }, { F }, { density }, initial_poses); + bodies->planes.emplace_back(Eigen::Vector3d(0, 1, 0), 0); // ground y = 0 + return bodies; +} + +} // namespace + +TEST_CASE( + "Friction simulator gradient and hessian", + "[friction][simulator][gradient][hessian]") +{ + const bool affine = GENERATE(false, true); + // Sliding speed regime: well below and well above the eps_v = 1e-3 kink + // (finite differences are unreliable at the kink itself). + const double offset = GENERATE(1e-6, 1e-3); + + CAPTURE(affine, offset); + + Simulator::Settings settings; + settings.body_dynamics = affine ? Simulator::BodyDynamics::AFFINE + : Simulator::BodyDynamics::RIGID; + settings.friction_coefficient = 0.5; + + std::vector initial_poses; + auto bodies = + cube_on_plane(initial_poses, settings.dhat, 1000.0, /*gap_factor=*/0.5); + + const double dt = 0.01; + Simulator sim(bodies, initial_poses, dt, settings); + + // Lag the collision and friction sets at the initial state. + const Eigen::VectorXd x0 = sim.kinematics().dof(initial_poses); + sim.initialize_step(); + sim.update_collisions(x0); + REQUIRE(!sim.normal_collisions().empty()); + sim.initialize_friction_step(); + sim.update_friction_collisions(x0); + REQUIRE(!sim.tangential_collisions().empty()); + + // Slide tangentially (and rotate slightly in rigid mode to exercise the + // second-order chain-rule term). + Eigen::VectorXd x = x0; + x(0) += offset; // tangential slide along x + if (!affine) { + x(5) += 0.1 * offset; // rotation about z + } else { + x(4) += 0.1 * offset; // perturb A + } + + Eigen::VectorXd grad; + sim.gradient(x, grad); + + Eigen::VectorXd fd_grad; + fd::finite_gradient( + x, [&](const Eigen::VectorXd& y) { return sim.value(y); }, fd_grad); + + CHECK(fd::compare_gradient(grad, fd_grad)); + if (!fd::compare_gradient(grad, fd_grad)) { + std::cout << "analytic:\n" << grad << "\n\n"; + std::cout << "numerical:\n" << fd_grad << "\n\n"; + } + + sim.set_project_to_psd(false); // FD comparison needs the exact Hessian + Eigen::SparseMatrix hess_sparse; + sim.hessian(x, hess_sparse); + const Eigen::MatrixXd hess = hess_sparse; + + Eigen::MatrixXd fd_hess; + fd::finite_jacobian( + x, + [&](const Eigen::VectorXd& y) { + Eigen::VectorXd g; + sim.gradient(y, g); + return g; + }, + fd_hess); + + // NOTE: 1e-2 tolerance: the FD of the gradient carries roundoff noise + // of O(1) from the ~1e8-scale (affine) mass-matrix entries, while the + // friction/barrier entries being verified are O(1)-O(1e3). + CHECK(fd::compare_hessian(hess, fd_hess, 1e-2)); + if (!fd::compare_hessian(hess, fd_hess, 1e-2)) { + std::cout << "analytic:\n" << hess << "\n\n"; + std::cout << "numerical:\n" << fd_hess << "\n\n"; + } +} + +TEST_CASE("Friction incline stick and slip", "[friction][simulator]") +{ + const bool affine = GENERATE(false, true); + CAPTURE(affine); + + // Tilt gravity instead of the plane: an incline of angle θ. + const double theta = 20.0 * ipc::PI / 180.0; // tan θ ≈ 0.364 + const double g = 9.81; + + Simulator::Settings settings; + settings.body_dynamics = affine ? Simulator::BodyDynamics::AFFINE + : Simulator::BodyDynamics::RIGID; + settings.gravity = + Eigen::Vector3d(g * std::sin(theta), -g * std::cos(theta), 0); + // Iterate the friction lagging to convergence: with a single lagged + // solve, the normal forces are sampled at velocity-criterion-accurate + // (not force-balanced) states, where the stiff barrier makes them noisy. + settings.friction_iterations = -1; + // ... and tighten the velocity criterion so its displacement tolerance is + // below the barrier force-well width: otherwise the solver can quit while + // a corner is jammed deep in the barrier (huge lagged normal force). + settings.velocity_conv_tol = 1e-3; + + SECTION("sticks when mu > tan(theta)") + { + settings.friction_coefficient = 0.5; + } + SECTION("slides when mu < tan(theta)") + { + settings.friction_coefficient = 0.1; + } + + std::vector initial_poses; + auto bodies = + cube_on_plane(initial_poses, settings.dhat, 1000.0, /*gap_factor=*/1.1); + + const double dt = 0.01, t_end = 1.0; + Simulator sim(bodies, initial_poses, dt, settings); + + const Eigen::Vector3d p0 = initial_poses[0].position; + + // Sample the position at three times past the landing transient and + // measure the steady-sliding acceleration by central differences, + // isolating the physics from the landing/settling transient. + REQUIRE(sim.run(0.5 * t_end)); + const double x1 = (sim.poses()[0].position - p0).x(); + REQUIRE(sim.run(0.75 * t_end)); + const double xm = (sim.poses()[0].position - p0).x(); + REQUIRE(sim.run(t_end)); + const double x2 = (sim.poses()[0].position - p0).x(); + + const double h = 0.25 * t_end; + const double accel = (x2 - 2 * xm + x1) / (h * h); + + if (settings.friction_coefficient > std::tan(theta)) { + // Static friction holds the cube in place (allow settling on the + // order of the barrier activation distance). + CHECK(std::abs(x2) < 10 * settings.dhat); + } else { + // Steady kinetic sliding: a = g (sin(theta) - mu cos(theta)). + const double expected = g + * (std::sin(theta) + - settings.friction_coefficient * std::cos(theta)); + CHECK(accel == Catch::Approx(expected).epsilon(0.05)); + } +} + +// The hard gate for the velocity-formulation scaling: a cube sliding on a +// flat plane decelerates at exactly μg, so it stops after v₀²/(2μg). +TEST_CASE("Friction sliding deceleration", "[friction][simulator]") +{ + const int bdf_order = GENERATE(1, 2); + CAPTURE(bdf_order); + + const double mu = 0.3, g = 9.81, v0 = 1.0; + + Simulator::Settings settings; + settings.friction_coefficient = mu; + settings.bdf_order = bdf_order; + settings.friction_iterations = -1; // see the incline test's notes + settings.velocity_conv_tol = 1e-3; + + std::vector initial_poses; + auto bodies = + cube_on_plane(initial_poses, settings.dhat, 1000.0, /*gap_factor=*/1.1); + + std::vector initial_velocities(1, Pose::identity(3)); + initial_velocities[0].position = Eigen::Vector3d(v0, 0, 0); + + const double dt = 0.01; + Simulator sim(bodies, initial_poses, initial_velocities, dt, settings); + + const Eigen::Vector3d p0 = initial_poses[0].position; + const double stopping_time = v0 / (mu * g); // ≈ 0.34 s + const double stopping_dist = v0 * v0 / (2 * mu * g); // ≈ 0.17 m + + REQUIRE(sim.run(/*t_end=*/2 * stopping_time)); + const double slide = (sim.poses()[0].position - p0).x(); + + // First-order time discretization error is O(v₀ dt) per the ½aΔt²-style + // offset of the stopping distance. + CHECK(slide == Catch::Approx(stopping_dist).margin(3 * v0 * dt)); + + // ... and it stays stopped (static friction holds). + const Eigen::Vector3d p_stop = sim.poses()[0].position; + REQUIRE(sim.run(/*t_end=*/2 * stopping_time + 0.2)); + CHECK( + (sim.poses()[0].position - p_stop).norm() + < settings.static_friction_speed_bound * 0.2); +} + +// Two cubes slide side by side across a static slab with the same initial +// speed. A friction pair makes the first cube's contact with the slab +// frictionless, so it keeps its speed while the second cube, which uses the +// global coefficient, stops after v₀²/(2μg). +TEST_CASE( + "Friction pairs override the global coefficient between two bodies", + "[friction][simulator]") +{ + const double mu = 0.3, g = 9.81, v0 = 1.0; + + Eigen::MatrixXd V; + Eigen::MatrixXi E, F; + REQUIRE(tests::load_mesh("cube.ply", V, E, F)); + const double half = -V.col(1).minCoeff(); + + // A wide, unit-thick slab with its top face at y = 0. + Eigen::MatrixXd V_slab = V; + V_slab.col(0) *= 10; + V_slab.col(2) *= 10; + + Simulator::Settings settings; + settings.friction_coefficient = mu; + settings.friction_iterations = -1; + settings.velocity_conv_tol = 1e-3; + + std::vector initial_poses(3, Pose::identity(3)); + initial_poses[0].position.y() = -half; + for (int i : { 1, 2 }) { + initial_poses[i].position = Eigen::Vector3d( + -2.0, half + 1.1 * settings.dhat, i == 1 ? -2.0 : 2.0); + } + + auto bodies = RigidBodies::build_from_meshes( + { V_slab, V, V }, { E, E, E }, { F, F, F }, { 1000.0, 1000.0, 1000.0 }, + initial_poses); + (*bodies)[0].set_type(RigidBody::Type::STATIC); + + settings.friction_pairs = { { /*body0=*/1, /*body1=*/0, /*mu=*/0.0 } }; + + std::vector initial_velocities(3, Pose::identity(3)); + initial_velocities[1].position = Eigen::Vector3d(v0, 0, 0); + initial_velocities[2].position = Eigen::Vector3d(v0, 0, 0); + + const double dt = 0.01; + Simulator sim(bodies, initial_poses, initial_velocities, dt, settings); + + // Run well past the time the gripped cube needs to stop, then measure both + // cubes' speeds over one more step. + const double stopping_time = v0 / (mu * g); + REQUIRE(sim.run(/*t_end=*/2 * stopping_time)); + const Eigen::Vector3d free_start = sim.poses()[1].position; + const Eigen::Vector3d gripped_start = sim.poses()[2].position; + REQUIRE(sim.step()); + const Eigen::Vector3d free_velocity = + (sim.poses()[1].position - free_start) / dt; + const Eigen::Vector3d gripped_velocity = + (sim.poses()[2].position - gripped_start) / dt; + + CHECK(free_velocity.x() == Catch::Approx(v0).epsilon(1e-2)); + CHECK(gripped_velocity.norm() < 1e-2 * v0); +} + +TEST_CASE("Friction pairs reject unknown bodies", "[friction][simulator]") +{ + std::vector initial_poses; + auto bodies = cube_on_plane(initial_poses, 1e-3, 1000.0, 1.1); + + Simulator::Settings settings; + settings.friction_coefficient = 0.3; + settings.friction_pairs = { { /*body0=*/0, /*body1=*/1, /*mu=*/0.1 } }; + + CHECK_THROWS_AS( + Simulator(bodies, initial_poses, 0.01, settings), + std::invalid_argument); +} + +// A step's friction forces are lagged from the state it starts at, so they +// must be the contact forces that state actually carries: the ones the +// previous step was solved with. A cube sliding faster than εᵥ then loses +// exactly μλΔt/m of speed per step. The barrier stiffness is re-seeded every +// step, so we add a second cube flying away from the scene. It grows the +// bounding box, and with it the stiffness, on every step, which is exactly +// when lagging with the re-seeded stiffness would scale the friction. +TEST_CASE( + "Sliding friction uses the contact forces the start state carries", + "[friction][simulator]") +{ + const double mu = 0.3, dt = 0.01; + + Simulator::Settings settings; + settings.friction_coefficient = mu; + settings.friction_iterations = 1; // one lagged solve per step + settings.velocity_conv_tol = 1e-4; + + Eigen::MatrixXd V; + Eigen::MatrixXi E, F; + REQUIRE(tests::load_mesh("cube.ply", V, E, F)); + + std::vector initial_poses(2, Pose::identity(3)); + initial_poses[0].position.y() = -V.col(1).minCoeff() + 0.9 * settings.dhat; + initial_poses[1].position = Eigen::Vector3d(5, 5, 0); + + auto bodies = RigidBodies::build_from_meshes( + { V, V }, { E, E }, { F, F }, { 1000.0, 1000.0 }, initial_poses); + bodies->planes.emplace_back(Eigen::Vector3d(0, 1, 0), 0); // ground y = 0 + + std::vector initial_velocities(2, Pose::identity(3)); + initial_velocities[0].position = Eigen::Vector3d(1, 0, 0); // sliding + initial_velocities[1].position = Eigen::Vector3d(0, 50, 0); // escaping + + Simulator sim(bodies, initial_poses, initial_velocities, dt, settings); + const double mass = sim.bodies()[0].mass(); + + double x_prev = sim.rigid_poses()[0].position.x(); + for (int i = 0; i < 3; ++i) { + x_prev = sim.rigid_poses()[0].position.x(); + REQUIRE(sim.step()); + } + + for (int i = 0; i < 5; ++i) { + CAPTURE(i); + + // The contact forces at the end of the previous step, with the + // stiffness it was solved with. + const BarrierPotential solved_with = sim.barrier_potential(); + TangentialCollisions start; + start.build( + sim.bodies(), sim.bodies().vertices(sim.rigid_poses()), + sim.normal_collisions(), solved_with, mu); + double normal_force = 0; + for (size_t j = 0; j < start.size(); ++j) { + normal_force += start[j].normal_force_magnitude; + } + REQUIRE(normal_force > 0); + + const double x = sim.rigid_poses()[0].position.x(); + const double v = (x - x_prev) / dt; + REQUIRE(sim.step()); + const double x_next = sim.rigid_poses()[0].position.x(); + const double v_next = (x_next - x) / dt; + x_prev = x; + + // The stiffness really did change, or this would test nothing. + CHECK(sim.barrier_potential().stiffness() != solved_with.stiffness()); + + CHECK( + v - v_next + == Catch::Approx(mu * normal_force * dt / mass).epsilon(1e-4)); + } +} + +TEST_CASE("Friction lagging semantics", "[friction][simulator]") +{ + Simulator::Settings settings; + std::vector initial_poses; + + SECTION("unlimited lagging converges the momentum balance") + { + settings.friction_coefficient = 0.3; + settings.friction_iterations = -1; + + auto bodies = cube_on_plane( + initial_poses, settings.dhat, 1000.0, /*gap_factor=*/1.1); + std::vector initial_velocities(1, Pose::identity(3)); + initial_velocities[0].position = Eigen::Vector3d(1.0, 0, 0); + + Simulator sim( + bodies, initial_poses, initial_velocities, /*dt=*/0.01, settings); + + for (int i = 0; i < 5; ++i) { + REQUIRE(sim.step()); + // The lagging ran and measured the momentum balance. (It may + // legitimately end above eps_d when the inner solver's own + // stopping criteria bound the reachable residual.) + CHECK(sim.last_momentum_balance() >= 0); + } + } + + SECTION("friction_iterations == 0 disables friction entirely") + { + auto run_sim = [&](const double mu, const int iterations) { + std::vector poses; + Simulator::Settings s; + s.friction_coefficient = mu; + s.friction_iterations = iterations; + auto bodies = + cube_on_plane(poses, s.dhat, 1000.0, /*gap_factor=*/1.1); + std::vector velocities(1, Pose::identity(3)); + velocities[0].position = Eigen::Vector3d(1.0, 0, 0); + Simulator sim(bodies, poses, velocities, /*dt=*/0.01, s); + REQUIRE(sim.run(/*t_end=*/0.2)); + return sim.poses()[0].position; + }; + + const Eigen::Vector3d with_disabled_friction = run_sim(0.3, 0); + const Eigen::Vector3d without_friction = run_sim(0.0, 1); + CHECK((with_disabled_friction - without_friction).norm() == 0.0); + } +} diff --git a/tests/src/tests/demo/test_joints.cpp b/tests/src/tests/demo/test_joints.cpp new file mode 100644 index 000000000..e6a257937 --- /dev/null +++ b/tests/src/tests/demo/test_joints.cpp @@ -0,0 +1,267 @@ +#include +#include + +#include + +#include +#include +#include +#include + +using namespace ipc; +using namespace ipc::affine; +using BodyDynamics = ipc::demo::Simulator::BodyDynamics; +using ipc::demo::Simulator; + +namespace { +/// World position of a world-frame anchor (defined at the initial pose) at +/// the current affine DOFs. +Eigen::Vector3d anchor_position( + const rigid::RigidBodies& bodies, + const std::vector& initial_poses, + Eigen::ConstRef x, + const size_t body, + Eigen::ConstRef world_anchor) +{ + const rigid::Pose& p0 = initial_poses[body]; + const Eigen::Vector3d material = + p0.rotation_matrix().transpose() * (world_anchor - p0.position); + const Eigen::Vector3d p = x.segment<3>(12 * body); + const Eigen::Matrix3d A = x.segment<9>(12 * body + 3).reshaped(3, 3); + return A * material + p; +} + +Eigen::VectorXd poses_to_dof(const std::vector& poses) +{ + Eigen::VectorXd x(12 * poses.size()); + for (size_t i = 0; i < poses.size(); i++) { + x.segment<3>(12 * i) = poses[i].position; + x.segment<9>(12 * i + 3) = poses[i].rotation.reshaped(); + } + return x; +} +} // namespace + +TEST_CASE("Joints require affine dynamics", "[affine][joints][simulator]") +{ + Eigen::MatrixXd V; + Eigen::MatrixXi E, F; + tests::load_mesh("cube.ply", V, E, F); + + std::vector initial_poses(1, rigid::Pose::identity(3)); + auto bodies = rigid::RigidBodies::build_from_meshes( + { V }, { E }, { F }, { 1000.0 }, initial_poses); + + auto joints = std::make_shared(bodies, initial_poses); + joints->add_fixed_point(0, Eigen::Vector3d(-0.5, 0, 0)); + + // Material-point constraints are nonlinear in the rigid rotation vectors + Simulator::Settings settings; + settings.body_dynamics = BodyDynamics::RIGID; + CHECK_THROWS_AS( + Simulator(bodies, initial_poses, joints, /*dt=*/0.01, settings), + std::invalid_argument); +} + +TEST_CASE("Joint change of variables", "[affine][joints]") +{ + Eigen::MatrixXd V; + Eigen::MatrixXi E, F; + tests::load_mesh("cube.ply", V, E, F); + + // A third, unjointed body must pass through the reduction untouched. + const int num_bodies = GENERATE(2, 3); + CAPTURE(num_bodies); + + std::vector initial_poses( + num_bodies, rigid::Pose::identity(3)); + for (int i = 1; i < num_bodies; i++) { + initial_poses[i].position = Eigen::Vector3d(i, 0, 0); + } + auto bodies = rigid::RigidBodies::build_from_meshes( + std::vector(num_bodies, V), + std::vector(num_bodies, E), + std::vector(num_bodies, F), + std::vector(num_bodies, 1000.0), initial_poses); + + JointConstraints joints(bodies, initial_poses); + joints.add_point_connection(0, 1, Eigen::Vector3d(0.5, 0, 0)); + joints.add_sliding_plane( + 1, Eigen::Vector3d(1, 0, 0), Eigen::Vector3d(0, 0, 1)); + REQUIRE(joints.num_constraints() == 4); + joints.finalize(); + + const int n = 12 * num_bodies; + const int m = int(joints.num_constraints()); + + // U V = I (round trip) + for (int trial = 0; trial < 5; trial++) { + const Eigen::VectorXd x = Eigen::VectorXd::Random(n); + CHECK( + (joints.to_full(joints.to_reduced(x)) - x).norm() + == Catch::Approx(0).margin(1e-12)); + } + + // Any z with pinned head satisfies the constraints exactly + for (int trial = 0; trial < 5; trial++) { + Eigen::VectorXd z = Eigen::VectorXd::Random(n); + z.head(m) = joints.rhs(); + const Eigen::VectorXd x = joints.to_full(z); + CHECK(joints.residual(x).norm() == Catch::Approx(0).margin(1e-10)); + } + + // The initial configuration satisfies the constraints + const Eigen::VectorXd x0 = poses_to_dof([&] { + std::vector poses(num_bodies); + for (int i = 0; i < num_bodies; i++) { + poses[i].position = initial_poses[i].position; + poses[i].rotation = initial_poses[i].rotation_matrix(); + } + return poses; + }()); + CHECK(joints.residual(x0).norm() == Catch::Approx(0).margin(1e-12)); +} + +TEST_CASE("Joints reject invalid requests", "[affine][joints]") +{ + Eigen::MatrixXd V; + Eigen::MatrixXi E, F; + tests::load_mesh("cube.ply", V, E, F); + + std::vector initial_poses(2, rigid::Pose::identity(3)); + initial_poses[1].position = Eigen::Vector3d(2, 0, 0); + auto bodies = rigid::RigidBodies::build_from_meshes( + { V, V }, { E, E }, { F, F }, { 1000.0, 1000.0 }, initial_poses); + + JointConstraints joints(bodies, initial_poses); + const Eigen::Vector3d anchor(1, 0, 0); + + CHECK_THROWS_AS(joints.add_fixed_point(2, anchor), std::out_of_range); + CHECK_THROWS_AS( + joints.add_point_connection(0, 5, anchor), std::out_of_range); + CHECK_THROWS_AS( + joints.add_point_connection(1, 1, anchor), std::invalid_argument); + CHECK(joints.empty()); + + // Once finalized (as the Simulator does on construction), a late joint + // would be silently ignored, so it is rejected instead. + joints.add_point_connection(0, 1, anchor); + joints.finalize(); + CHECK_THROWS_AS(joints.add_fixed_body(0), std::logic_error); + CHECK(joints.num_constraints() == 3); +} + +TEST_CASE("Affine pendulum on a fixed point", "[affine][joints][simulator]") +{ + Eigen::MatrixXd V; + Eigen::MatrixXi E, F; + tests::load_mesh("cube.ply", V, E, F); + + // Rod hanging horizontally from one end: it should swing down + Eigen::MatrixXd V_rod = V; + V_rod.col(0) *= 2.0; + + std::vector initial_poses(1, rigid::Pose::identity(3)); + auto bodies = rigid::RigidBodies::build_from_meshes( + { V_rod }, { E }, { F }, { 1000.0 }, initial_poses); + + const Eigen::Vector3d anchor(-1.0, 0, 0); // rod end + + auto joints = std::make_shared(bodies, initial_poses); + joints->add_fixed_point(0, anchor); + + Simulator::Settings settings; // paper-recommended default stiffness + settings.body_dynamics = BodyDynamics::AFFINE; + + Simulator sim(bodies, initial_poses, joints, /*dt=*/0.01, settings); + + for (int i = 0; i < 25; i++) { + REQUIRE(sim.step()); + + const Eigen::VectorXd x = + poses_to_dof({ sim.pose_history().back()[0] }); + + // The anchor point stays exactly fixed + const Eigen::Vector3d p = + anchor_position(*bodies, initial_poses, x, 0, anchor); + CHECK((p - anchor).norm() == Catch::Approx(0).margin(1e-8)); + + // A stays near SO(3) + const Eigen::Matrix3d A = x.segment<9>(3).reshaped(3, 3); + CHECK((A.transpose() * A - Eigen::Matrix3d::Identity()).norm() < 0.01); + } + + // The free end fell (pendulum swings down) + const Eigen::VectorXd x_final = + poses_to_dof({ sim.pose_history().back()[0] }); + const Eigen::Vector3d free_end = anchor_position( + *bodies, initial_poses, x_final, 0, Eigen::Vector3d(1.0, 0, 0)); + CHECK(free_end.y() < -0.1); + // ...and stays at distance 2 from the anchor (rigidity) + CHECK((free_end - anchor).norm() == Catch::Approx(2.0).margin(0.02)); +} + +TEST_CASE("Affine hinge door", "[affine][joints][simulator]") +{ + Eigen::MatrixXd V; + Eigen::MatrixXi E, F; + tests::load_mesh("cube.ply", V, E, F); + + // Body 0: fixed frame; body 1: door attached along a vertical hinge in + // the gap between the bodies (they must not start in contact — the + // barrier requires initial separation). Sideways gravity swings the door + // about the axis. + std::vector initial_poses(2, rigid::Pose::identity(3)); + initial_poses[1].position = Eigen::Vector3d(1.2, 0, 0); + + auto bodies = rigid::RigidBodies::build_from_meshes( + { V, V }, { E, E }, { F, F }, { 1000.0, 1000.0 }, initial_poses); + + const Eigen::Vector3d hinge_p0(0.6, -0.5, 0); + const Eigen::Vector3d hinge_p1(0.6, 0.5, 0); + + auto joints = std::make_shared(bodies, initial_poses); + joints->add_fixed_body(0); + joints->add_hinge(0, 1, hinge_p0, hinge_p1); + + Simulator::Settings settings; // paper-recommended default stiffness + settings.body_dynamics = BodyDynamics::AFFINE; + settings.gravity = Eigen::Vector3d(0, 0, -10.0); // push the door sideways + + Simulator sim(bodies, initial_poses, joints, /*dt=*/0.01, settings); + + for (int i = 0; i < 25; i++) { + REQUIRE(sim.step()); + + Eigen::VectorXd x(24); + for (int b = 0; b < 2; b++) { + const affine::Pose& pose = sim.pose_history().back()[b]; + x.segment<3>(12 * b) = pose.position; + x.segment<9>(12 * b + 3) = pose.rotation.reshaped(); + } + + // Body 0 stays fixed + CHECK( + (x.head<12>() - poses_to_dof({ [&] { + affine::Pose pose; + pose.position = initial_poses[0].position; + pose.rotation = initial_poses[0].rotation_matrix(); + return pose; + }() })) + .norm() + == Catch::Approx(0).margin(1e-8)); + + // Both hinge anchor pairs coincide + for (const auto& hp : { hinge_p0, hinge_p1 }) { + const Eigen::Vector3d p_frame = + anchor_position(*bodies, initial_poses, x, 0, hp); + const Eigen::Vector3d p_door = + anchor_position(*bodies, initial_poses, x, 1, hp); + CHECK((p_frame - p_door).norm() == Catch::Approx(0).margin(1e-8)); + } + } + + // The door swung about the hinge: its center moved in z + const affine::Pose& door = sim.pose_history().back()[1]; + CHECK(std::abs(door.position.z()) > 0.05); +} diff --git a/tests/src/tests/demo/test_rigid_simulator.cpp b/tests/src/tests/demo/test_rigid_simulator.cpp new file mode 100644 index 000000000..324dbd5f7 --- /dev/null +++ b/tests/src/tests/demo/test_rigid_simulator.cpp @@ -0,0 +1,349 @@ +// Test the mass utilities. + +#include +#include +#include +#include + +#include + +#include +#include // has_intersections +#include +#include +#ifdef IPC_TOOLKIT_WITH_GLTF +#include +#endif +#include +#include + +#include + +#include + +using namespace ipc; +using namespace ipc::rigid; +using ipc::demo::Simulator; + +namespace ipc::tests { +void simulator_test(Simulator& sim) +{ + Eigen::VectorXd x(18); + x << -0.31691, 0.358068, 0.0934453, 1.15926, 2.13156, 2.6372, // + 0.32609, 0.41217, 0.256955, 0.515056, 1.56975, 0.399389, // + 0.00250424, 0.11456, -0.00196945, 0.289872, -2.08816, 2.06641; + + // --- Check barrier potential gradient ------------------------------------ + + Eigen::MatrixXd V = sim.bodies().vertices(Pose::to_poses(x, 3)); + + std::vector poses(sim.bodies().num_bodies()); + for (size_t i = 0; i < sim.bodies().num_bodies(); ++i) { + poses[i] = Pose(x.segment<6>(6 * i)); + } + + sim.initialize_step(); + sim.update_collisions(x); + REQUIRE(sim.normal_collisions().size() > 0); + + Eigen::VectorXd tmp = sim.barrier_potential().gradient( + sim.normal_collisions(), sim.bodies(), V); + + Eigen::VectorXd fd_tmp; + fd::finite_gradient( + V.reshaped(), + [&](const Eigen::VectorXd& x) -> double { + return sim.barrier_potential()( + sim.normal_collisions(), sim.bodies(), + x.reshaped(V.rows(), V.cols())); + }, + fd_tmp); + + CHECK(fd::compare_gradient(tmp, fd_tmp)); + if (!fd::compare_gradient(tmp, fd_tmp)) { + std::cout << "analytic:\n" << tmp.transpose() << "\n\n"; + std::cout << "numerical:\n" << fd_tmp.transpose() << "\n\n"; + } + + // --- Check total gradient and hessian ------------------------------------ + + Eigen::VectorXd g; + sim.gradient(x, g); + + Eigen::VectorXd fd_g; + fd::finite_gradient( + x, [&](const Eigen::VectorXd& x) { return sim.value(x); }, fd_g); + + CHECK(fd::compare_gradient(g, fd_g)); + if (!fd::compare_gradient(g, fd_g)) { + std::cout << "analytic:\n" << g << "\n\n"; + std::cout << "numerical:\n" << fd_g << "\n\n"; + } + + sim.set_project_to_psd(false); // FD comparison needs the exact Hessian + Eigen::SparseMatrix H_sparse; + sim.hessian(x, H_sparse); + const Eigen::MatrixXd H = H_sparse; + + Eigen::MatrixXd fd_H; + fd::finite_jacobian( + x, + [&](const Eigen::VectorXd& x) { + Eigen::VectorXd g_; + sim.gradient(x, g_); + return g_; + }, + fd_H); + + CHECK(fd::compare_hessian(H, fd_H)); + if (!fd::compare_hessian(H, fd_H)) { + std::cout << "analytic:\n" << H << "\n\n"; + std::cout << "numerical:\n" << fd_H << "\n\n"; + } + + sim.reset(); +} +} // namespace ipc::tests + +TEST_CASE("Rigid body simulator", "[.][rigid]") +{ + Eigen::MatrixXd V_bunny; + Eigen::MatrixXi E_bunny, F_bunny; + REQUIRE(tests::load_mesh("bunny (lowpoly).ply", V_bunny, E_bunny, F_bunny)); + + Eigen::MatrixXd V_bowl; + Eigen::MatrixXi E_bowl, F_bowl; + REQUIRE(tests::load_mesh("bowl.ply", V_bowl, E_bowl, F_bowl)); + + std::vector initial_poses(3); + initial_poses[0].position = Eigen::Vector3d(1.0, 1.5, 0); + initial_poses[0].rotation = Eigen::Vector3d::Zero(); + initial_poses[1].position = Eigen::Vector3d(-1.0, 2.0, 0.0); + initial_poses[1].rotation = Eigen::Vector3d(0.0, ipc::PI / 4, 0.0); + initial_poses[2].position = Eigen::Vector3d(0.0, 1.1, 0.0); + initial_poses[2].rotation = Eigen::Vector3d(0.0, 0.0, 0.0); + + auto bodies = RigidBodies::build_from_meshes( + std::vector { V_bunny, V_bunny, V_bowl }, + std::vector { E_bunny, E_bunny, E_bowl }, + std::vector { F_bunny, F_bunny, F_bowl }, + /*densisties=*/ { { 1000.0, 1000.0, 1000.0 } }, initial_poses); + bodies->planes.emplace_back(Eigen::Vector3d(0, 1, 0), 0); + + double dt = 0.1; + double tend = 10.0; + int n_steps = int(tend / dt); + Simulator sim(bodies, initial_poses, dt); + + // --- Check simulator gradient and hessian -------------------------------- + SECTION("Simulator gradient and hessian") + { + ipc::tests::simulator_test(sim); + } + + // --- Test stepping and running the simulation ---------------------------- + + SECTION("Simulator stepping and running") + { + int n_calls = 0; + auto callback = [&](bool success) { + n_calls++; + CHECK(success); // Ensure all steps succeed + }; + + REQUIRE(sim.step()); + CHECK(sim.t() == Catch::Approx(dt)); + REQUIRE(sim.run(tend, callback)); + CHECK(n_calls == n_steps - 1); + CHECK(sim.t() == Catch::Approx(tend)); + CHECK(!sim.run( + tend)); // Simulation already completed, should return false + CHECK(n_calls == n_steps - 1); // Callback should not be called again + +#ifdef IPC_TOOLKIT_WITH_GLTF + write_gltf("simulator_test.glb", *bodies, sim.rigid_pose_history(), dt); +#endif + + sim.reset(); + CHECK(sim.t() == 0.0); + } +} + +TEST_CASE("Rigid simulator ballistic motion", "[rigid][simulator]") +{ + Eigen::MatrixXd V; + Eigen::MatrixXi E, F; + tests::load_mesh("cube.ply", V, E, F); + + std::vector initial_poses(1, Pose::identity(3)); + initial_poses[0].position = Eigen::Vector3d(0, 100.0, 0); // free flight + + auto bodies = RigidBodies::build_from_meshes( + { V }, { E }, { F }, { 1000.0 }, initial_poses); + + std::vector initial_velocities(1, Pose::identity(3)); + initial_velocities[0].position = Eigen::Vector3d(1.0, 2.0, -0.5); // v₀ + initial_velocities[0].rotation = Eigen::Vector3d(0, 0, 0.7); // ω + + Simulator::Settings settings; + settings.solver_params["grad_norm_tol"] = 1e-8; + + const double dt = 0.01; + Simulator sim(bodies, initial_poses, initial_velocities, dt, settings); + + // NOTE: build_from_meshes folds R₀ into initial_poses, so capture the + // world rotation after construction. + const Eigen::Vector3d p0 = initial_poses[0].position; + const Eigen::Matrix3d Q0 = initial_poses[0].rotation_matrix(); + const Eigen::Vector3d v0 = initial_velocities[0].position; + const Eigen::Vector3d omega = initial_velocities[0].rotation; + const Eigen::Vector3d g = settings.gravity; + + REQUIRE(sim.step()); + const affine::Pose pose_1 = sim.pose_history().back()[0]; + + // Implicit Euler translation: p₁ = p₀ + dt v₀ + dt² g + const Eigen::Vector3d p1_expected = p0 + dt * v0 + dt * dt * g; + CHECK( + (pose_1.position - p1_expected).norm() + == Catch::Approx(0).margin(1e-6)); + + // Rotation: for a cube J ∝ I, so the minimizer of the inertial term is the + // polar factor of Q̂ = (I + dt[ω]×) Q₀, i.e., a rotation about ω̂ by + // atan(dt‖ω‖). + const Eigen::Matrix3d Q1_expected = + Eigen::AngleAxisd(std::atan(dt * omega.norm()), omega.normalized()) + .toRotationMatrix() + * Q0; + CHECK( + (pose_1.rotation - Q1_expected).norm() + == Catch::Approx(0).margin(1e-5)); + + // rigid_pose_history() is the exact log map in rigid mode + const Pose rigid_pose_1 = sim.rigid_pose_history().back()[0]; + CHECK(rigid_pose_1.position == pose_1.position); + CHECK( + (rigid_pose_1.rotation_matrix() - pose_1.rotation).norm() + == Catch::Approx(0).margin(1e-12)); + + // Multi-step translation closed form: + // pₙ = p₀ + n dt v₀ + dt² g n(n+1)/2 + const int n = 10; + for (int i = 1; i < n; i++) { + REQUIRE(sim.step()); + } + const Eigen::Vector3d pn_expected = + p0 + n * dt * v0 + dt * dt * g * (n * (n + 1) / 2.0); + CHECK( + (sim.pose_history().back()[0].position - pn_expected).norm() + == Catch::Approx(0).margin(1e-5)); + + // reset() must restore the initial velocities (and rebuild the body + // forces with the new time integrator). + sim.reset(); + CHECK(sim.t() == 0.0); + REQUIRE(sim.step()); + const affine::Pose pose_1_again = sim.pose_history().back()[0]; + CHECK( + (pose_1_again.position - pose_1.position).norm() + == Catch::Approx(0).margin(1e-10)); + CHECK( + (pose_1_again.rotation - pose_1.rotation).norm() + == Catch::Approx(0).margin(1e-10)); +} + +TEST_CASE("Rigid simulator settings", "[rigid][simulator]") +{ + Eigen::MatrixXd V; + Eigen::MatrixXi E, F; + tests::load_mesh("cube.ply", V, E, F); + + std::vector initial_poses(1, Pose::identity(3)); + auto bodies = RigidBodies::build_from_meshes( + { V }, { E }, { F }, { 1000.0 }, initial_poses); + + Simulator::Settings settings; + settings.dhat = 0.05; + settings.gravity = Eigen::Vector3d(0, -1.62, 0); // moon gravity + settings.solver_params["max_iterations"] = 42; + + Simulator sim(bodies, initial_poses, /*dt=*/0.01, settings); + + CHECK(sim.barrier_potential().dhat() == 0.05); + CHECK(sim.gravity() == Eigen::Vector3d(0, -1.62, 0)); + CHECK(sim.settings().solver_params["max_iterations"] == 42); + + sim.set_gravity(Eigen::Vector3d(0, -9.81, 0)); + CHECK(sim.gravity() == Eigen::Vector3d(0, -9.81, 0)); +} + +// Regression test: a bunny dropped off-center onto the bowl rim used to tunnel +// through the thin shell because the rigid continuous broad phase missed the +// bunny-bowl candidate pair. The narrow phase (CCD) is only as good as the +// candidates it is given, so the simulation must stay intersection-free. +TEST_CASE("Rigid simulator no tunneling into bowl", "[rigid][simulator]") +{ + Eigen::MatrixXd V_bunny, V_bowl; + Eigen::MatrixXi E_bunny, F_bunny, E_bowl, F_bowl; + REQUIRE(tests::load_mesh("bunny (lowpoly).ply", V_bunny, E_bunny, F_bunny)); + REQUIRE(tests::load_mesh("bowl.ply", V_bowl, E_bowl, F_bowl)); + + // Bunny off to the side so it falls onto the sloped rim/wall of the bowl. + std::vector initial_poses(2, Pose::identity(3)); + initial_poses[0].position = Eigen::Vector3d(1.0, 1.5, 0.0); // bunny + initial_poses[1].position = Eigen::Vector3d(0.0, 1.1, 0.0); // bowl + + auto bodies = RigidBodies::build_from_meshes( + { V_bunny, V_bowl }, { E_bunny, E_bowl }, { F_bunny, F_bowl }, + { 1000.0, 1000.0 }, initial_poses); + bodies->planes.emplace_back(Eigen::Vector3d(0, 1, 0), 0); // ground y = 0 + + Simulator sim(bodies, initial_poses, /*dt=*/1.0 / 60.0); + + // First contact with the bowl occurs around t ≈ 0.5; run past it. + for (int i = 0; i < 60; ++i) { + REQUIRE(sim.step()); + const Eigen::MatrixXd V = + bodies->vertices(sim.rigid_pose_history().back()); + CHECK(!has_intersections(*bodies, V)); + } +} + +TEST_CASE("Rigid simulator BDF order", "[rigid][simulator]") +{ + Eigen::MatrixXd V; + Eigen::MatrixXi E, F; + tests::load_mesh("cube.ply", V, E, F); + + std::vector initial_poses(1, Pose::identity(3)); + initial_poses[0].position = Eigen::Vector3d(0, 100.0, 0); + + auto bodies = RigidBodies::build_from_meshes( + { V }, { E }, { F }, { 1000.0 }, initial_poses); + + std::vector initial_velocities(1, Pose::identity(3)); + initial_velocities[0].position = Eigen::Vector3d(1.0, -2.0, 0.5); // v₀ + + // Constant-velocity free flight (no gravity, no torque) is a linear + // trajectory, which any BDF order integrates exactly. + const int order = GENERATE(1, 2, 3, 4); + Simulator::Settings settings; + settings.bdf_order = order; + settings.gravity.setZero(); + settings.solver_params["grad_norm_tol"] = 1e-12; + + const double dt = 0.01; + const Eigen::Vector3d p0 = initial_poses[0].position; + const Eigen::Vector3d v0 = initial_velocities[0].position; + + Simulator sim(bodies, initial_poses, initial_velocities, dt, settings); + + const int n = 10; + for (int i = 0; i < n; ++i) { + REQUIRE(sim.step()); + } + + // pₙ = p₀ + n·dt·v₀ exactly, for any BDF order. + const Eigen::Vector3d pn = sim.pose_history().back()[0].position; + CHECK((pn - (p0 + n * dt * v0)).norm() == Catch::Approx(0).margin(1e-9)); +} diff --git a/tests/src/tests/demo/test_static_kinematic.cpp b/tests/src/tests/demo/test_static_kinematic.cpp new file mode 100644 index 000000000..62c21ecb7 --- /dev/null +++ b/tests/src/tests/demo/test_static_kinematic.cpp @@ -0,0 +1,430 @@ +// Scenario tests for STATIC and KINEMATIC bodies (DOF masking + augmented +// Lagrangian), run in both RIGID and AFFINE modes. + +#include +#include +#include +#include + +#include + +#include +#include +#include // has_intersections +#include + +using namespace ipc; +using namespace ipc::rigid; +using ipc::demo::Simulator; + +namespace { + +Simulator::BodyDynamics generate_mode() +{ + return GENERATE( + Simulator::BodyDynamics::RIGID, Simulator::BodyDynamics::AFFINE); +} + +void load_cube(Eigen::MatrixXd& V, Eigen::MatrixXi& E, Eigen::MatrixXi& F) +{ + REQUIRE(tests::load_mesh("cube.ply", V, E, F)); +} + +} // namespace + +TEST_CASE("Static body support", "[static][simulator]") +{ + const auto mode = generate_mode(); + CAPTURE(int(mode)); + + Eigen::MatrixXd V; + Eigen::MatrixXi E, F; + load_cube(V, E, F); + + // A static floor cube and a dynamic cube dropped from slightly above. + std::vector initial_poses(2, Pose::identity(3)); + initial_poses[1].position = Eigen::Vector3d(0.1, 1.1, 0.05); + + auto bodies = RigidBodies::build_from_meshes( + { V, V }, { E, E }, { F, F }, { 1000.0, 1000.0 }, initial_poses); + (*bodies)[0].set_type(RigidBody::Type::STATIC); + + Simulator::Settings settings; + settings.body_dynamics = mode; + + Simulator sim(bodies, initial_poses, /*dt=*/0.01, settings); + + const affine::Pose floor_pose_0 = sim.poses()[0]; + + REQUIRE(sim.run(/*t_end=*/0.5)); + + // The static floor never moves (bitwise). + CHECK(sim.poses()[0].position == floor_pose_0.position); + CHECK(sim.poses()[0].rotation == floor_pose_0.rotation); + + // The dynamic cube rests on top without intersecting. + CHECK(sim.poses()[1].position.y() > 0.9); + CHECK(!has_intersections( + *bodies, bodies->vertices(sim.pose_history().back()))); +} + +TEST_CASE("All bodies static", "[static][simulator]") +{ + const auto mode = generate_mode(); + CAPTURE(int(mode)); + + Eigen::MatrixXd V; + Eigen::MatrixXi E, F; + load_cube(V, E, F); + + std::vector initial_poses(1, Pose::identity(3)); + auto bodies = RigidBodies::build_from_meshes( + { V }, { E }, { F }, { 1000.0 }, initial_poses); + (*bodies)[0].set_type(RigidBody::Type::STATIC); + + Simulator::Settings settings; + settings.body_dynamics = mode; + + Simulator sim(bodies, initial_poses, /*dt=*/0.01, settings); + const affine::Pose pose_0 = sim.poses()[0]; + + REQUIRE(sim.run(/*t_end=*/0.1)); + CHECK(sim.poses()[0].position == pose_0.position); + CHECK(sim.poses()[0].rotation == pose_0.rotation); +} + +TEST_CASE("Kinematic body velocity driven", "[kinematic][simulator]") +{ + const auto mode = generate_mode(); + CAPTURE(int(mode)); + + Eigen::MatrixXd V; + Eigen::MatrixXi E, F; + load_cube(V, E, F); + + std::vector initial_poses(1, Pose::identity(3)); + auto bodies = RigidBodies::build_from_meshes( + { V }, { E }, { F }, { 1000.0 }, initial_poses); + (*bodies)[0].set_type(RigidBody::Type::KINEMATIC); + + std::vector initial_velocities(1, Pose::identity(3)); + initial_velocities[0].position = Eigen::Vector3d(1.0, 0.5, 0); + initial_velocities[0].rotation = Eigen::Vector3d(0, 0, 0.3); + + Simulator::Settings settings; + settings.body_dynamics = mode; + // Gravity must not affect a kinematic body (no inertia/body forces). + + const double dt = 0.01; + Simulator sim(bodies, initial_poses, initial_velocities, dt, settings); + + const Eigen::Vector3d p0 = initial_poses[0].position; + const Eigen::Matrix3d Q0 = initial_poses[0].rotation_matrix(); + const Eigen::Vector3d v = initial_velocities[0].position; + const Eigen::Vector3d omega = initial_velocities[0].rotation; + + const int n = 20; + for (int i = 0; i < n; ++i) { + REQUIRE(sim.step()); + } + + // Constant-velocity drive: pₙ = p₀ + n·h·v (within the AL satisfaction + // tolerance 1 − 0.999 per step) — and no gravity sag. + const Eigen::Vector3d p_expected = p0 + n * dt * v; + CHECK( + (sim.poses()[0].position - p_expected).norm() + < 2e-3 * n * dt * v.norm()); + + // Rotation follows exp(n h [ω]×) Q₀. + const Eigen::Matrix3d Q_expected = + Eigen::AngleAxisd(n * dt * omega.norm(), omega.normalized()) + .toRotationMatrix() + * Q0; + CHECK((sim.poses()[0].rotation - Q_expected).norm() < 5e-3); +} + +TEST_CASE( + "Kinematic body keeps its prescribed velocity under load", + "[kinematic][simulator]") +{ + const auto mode = generate_mode(); + CAPTURE(int(mode)); + + Eigen::MatrixXd V; + Eigen::MatrixXi E, F; + load_cube(V, E, F); + + // A kinematic cube lifts a much heavier cube resting on top of it. The + // load resists every step, so any shortfall in reaching a step's target + // shows up as lost speed if it carries over to the next step. + std::vector initial_poses(2, Pose::identity(3)); + initial_poses[1].position = Eigen::Vector3d(0, 1.001, 0); + + auto bodies = RigidBodies::build_from_meshes( + { V, V }, { E, E }, { F, F }, { 1000.0, 50000.0 }, initial_poses); + (*bodies)[0].set_type(RigidBody::Type::KINEMATIC); + + std::vector initial_velocities(2, Pose::identity(3)); + initial_velocities[0].position = Eigen::Vector3d(0, 0.5, 0); + + Simulator::Settings settings; + settings.body_dynamics = mode; + + const double dt = 0.01; + Simulator sim(bodies, initial_poses, initial_velocities, dt, settings); + + const Eigen::Vector3d p0 = initial_poses[0].position; + const Eigen::Vector3d v = initial_velocities[0].position; + + const int n = 100; + Eigen::Vector3d p_prev = p0; + for (int i = 0; i < n; ++i) { + p_prev = sim.poses()[0].position; + REQUIRE(sim.step()); + } + + // Each step may stop short of its target by the AL satisfaction tolerance + // (0.1% of the step), but the next step must still aim for the full + // prescribed displacement, so the error stays at the per-step tolerance + // instead of compounding. + const Eigen::Vector3d p = sim.poses()[0].position; + CHECK((p - p_prev - dt * v).norm() < 2e-3 * dt * v.norm()); + CHECK((p - (p0 + n * dt * v)).norm() < 2e-3 * n * dt * v.norm()); +} + +TEST_CASE("Kinematic body scripted poses", "[kinematic][simulator]") +{ + const auto mode = generate_mode(); + CAPTURE(int(mode)); + + Eigen::MatrixXd V; + Eigen::MatrixXi E, F; + load_cube(V, E, F); + + std::vector initial_poses(1, Pose::identity(3)); + auto bodies = RigidBodies::build_from_meshes( + { V }, { E }, { F }, { 1000.0 }, initial_poses); + (*bodies)[0].set_type(RigidBody::Type::KINEMATIC); + + // Scripted sinusoidal path (absolute poses, one per step). + // NOTE: build_from_meshes folds the principal-axes rotation R₀ into + // initial_poses; compose the script with the post-build pose. + const int n = 10; + const double dt = 0.01; + std::deque script; + for (int i = 1; i <= n; ++i) { + Pose pose = initial_poses[0]; + pose.position += + Eigen::Vector3d(0.1 * i * dt, 0.05 * std::sin(0.5 * i), 0); + script.push_back(pose); + } + Simulator::Settings settings; + settings.body_dynamics = mode; + + Simulator sim(bodies, initial_poses, dt, settings); + sim.set_kinematic_driver(0, demo::KinematicDriver::scripted(script)); + + for (int i = 0; i < n; ++i) { + const Pose& target = script[i]; + REQUIRE(sim.step()); + // Within the AL satisfaction tolerance of the scripted pose. + CHECK( + (sim.poses()[0].position - target.position).norm() + < 1e-3 * std::max(1.0, target.position.norm())); + } +} + +TEST_CASE("Kinematic body pushes dynamic body", "[kinematic][simulator]") +{ + const auto mode = generate_mode(); + CAPTURE(int(mode)); + + Eigen::MatrixXd V; + Eigen::MatrixXi E, F; + load_cube(V, E, F); + + // Kinematic cube slides toward a dynamic cube resting to its right. + std::vector initial_poses(2, Pose::identity(3)); + initial_poses[1].position = Eigen::Vector3d(1.2, 0, 0); + + auto bodies = RigidBodies::build_from_meshes( + { V, V }, { E, E }, { F, F }, { 1000.0, 1000.0 }, initial_poses); + (*bodies)[0].set_type(RigidBody::Type::KINEMATIC); + + std::vector initial_velocities(2, Pose::identity(3)); + initial_velocities[0].position = Eigen::Vector3d(1.0, 0, 0); + + Simulator::Settings settings; + settings.body_dynamics = mode; + settings.gravity.setZero(); // isolate the push + + const double dt = 0.01; + Simulator sim(bodies, initial_poses, initial_velocities, dt, settings); + + const double x1_start = initial_poses[1].position.x(); + for (int i = 0; i < 50; ++i) { + CAPTURE(i); + REQUIRE(sim.step()); + CHECK(!has_intersections( + *bodies, bodies->vertices(sim.pose_history().back()))); + } + + // The kinematic cube advanced ~0.5 m; the gap was 0.2 m, so the dynamic + // cube must have been pushed forward without tunneling. + CHECK(sim.poses()[0].position.x() > 0.45); + CHECK(sim.poses()[1].position.x() > x1_start + 0.2); + CHECK( + sim.poses()[1].position.x() - sim.poses()[0].position.x() + > 0.99); // still at least one cube width apart (no tunneling) +} + +TEST_CASE("Partial DOF fixing", "[static][simulator]") +{ + Eigen::MatrixXd V; + Eigen::MatrixXi E, F; + load_cube(V, E, F); + + // Fix the rotation of a cube; push it off-center so an unconstrained + // cube would spin. + std::vector initial_poses(1, Pose::identity(3)); + VectorMax6b is_dof_fixed = VectorMax6b::Zero(6); + is_dof_fixed.tail<3>().setOnes(); // fix all rotation + + auto bodies = RigidBodies::build_from_meshes( + { V }, { E }, { F }, { 1000.0 }, initial_poses, + /*convert_planes=*/false, { is_dof_fixed }); + + // Off-center force: a torque plus a linear force. + (*bodies)[0].set_external_force( + Pose(Eigen::Vector3d(500.0, 0, 0), Eigen::Vector3d(0, 0, 800.0))); + + const auto mode = generate_mode(); + CAPTURE(int(mode)); + Simulator::Settings settings; + settings.body_dynamics = mode; + settings.gravity.setZero(); + + Simulator sim(bodies, initial_poses, /*dt=*/0.01, settings); + + const affine::Pose pose_0 = sim.poses()[0]; + REQUIRE(sim.run(/*t_end=*/0.2)); + + // Orientation unchanged; position responded to the force. + CHECK( + (sim.poses()[0].rotation - pose_0.rotation).norm() + == Catch::Approx(0).margin(1e-9)); + CHECK(sim.poses()[0].position.x() > pose_0.position.x() + 1e-4); +} + +TEST_CASE("Kinematic max time expiry", "[kinematic][simulator]") +{ + const auto mode = generate_mode(); + CAPTURE(int(mode)); + + Eigen::MatrixXd V; + Eigen::MatrixXi E, F; + load_cube(V, E, F); + + std::vector initial_poses(1, Pose::identity(3)); + auto bodies = RigidBodies::build_from_meshes( + { V }, { E }, { F }, { 1000.0 }, initial_poses); + (*bodies)[0].set_type(RigidBody::Type::KINEMATIC); + + const double dt = 0.01; + + std::vector initial_velocities(1, Pose::identity(3)); + initial_velocities[0].position = Eigen::Vector3d(1.0, 0, 0); + + Simulator::Settings settings; + settings.body_dynamics = mode; + settings.gravity.setZero(); + + Simulator sim(bodies, initial_poses, initial_velocities, dt, settings); + sim.set_kinematic_driver( + 0, demo::KinematicDriver::velocity_driven(/*max_time=*/2 * dt)); + + REQUIRE(sim.step()); // kinematic step 1 + REQUIRE(sim.step()); // kinematic step 2 (max time reached) + CHECK((*bodies)[0].type() == RigidBody::Type::KINEMATIC); + + REQUIRE(sim.step()); // converts to STATIC at step start + CHECK((*bodies)[0].type() == RigidBody::Type::STATIC); + + const affine::Pose pose_frozen = sim.poses()[0]; + REQUIRE(sim.step()); + CHECK(sim.poses()[0].position == pose_frozen.position); + CHECK(sim.poses()[0].rotation == pose_frozen.rotation); + + // reset() restores the KINEMATIC type (and re-arms the driver, so the + // body drives again and expires a second time). + sim.reset(); + CHECK((*bodies)[0].type() == RigidBody::Type::KINEMATIC); + REQUIRE(sim.step()); + REQUIRE(sim.step()); + CHECK((*bodies)[0].type() == RigidBody::Type::KINEMATIC); + REQUIRE(sim.step()); + CHECK((*bodies)[0].type() == RigidBody::Type::STATIC); +} + +TEST_CASE("Joints reject non-dynamic bodies", "[kinematic][joints][simulator]") +{ + Eigen::MatrixXd V; + Eigen::MatrixXi E, F; + load_cube(V, E, F); + + std::vector initial_poses(2, Pose::identity(3)); + initial_poses[1].position = Eigen::Vector3d(1.5, 0, 0); + + auto bodies = RigidBodies::build_from_meshes( + { V, V }, { E, E }, { F, F }, { 1000.0, 1000.0 }, initial_poses); + (*bodies)[0].set_type(RigidBody::Type::KINEMATIC); + + auto joints = + std::make_shared(bodies, initial_poses); + joints->add_point_connection(0, 1, Eigen::Vector3d(0.75, 0, 0)); + + Simulator::Settings settings; + settings.body_dynamics = Simulator::BodyDynamics::AFFINE; + + CHECK_THROWS_AS( + Simulator(bodies, initial_poses, joints, /*dt=*/0.01, settings), + std::invalid_argument); +} + +TEST_CASE("Minimum separation distance", "[minimum_separation][simulator]") +{ + Eigen::MatrixXd V; + Eigen::MatrixXi E, F; + load_cube(V, E, F); + + // Drop a dynamic cube onto a static floor cube and return its resting + // height for a given minimum-separation distance. + const auto settle_y = [&](const double dmin) { + std::vector poses(2, Pose::identity(3)); + poses[1].position = Eigen::Vector3d(0.0, 1.1, 0.0); + + auto bodies = RigidBodies::build_from_meshes( + { V, V }, { E, E }, { F, F }, { 1000.0, 1000.0 }, poses); + (*bodies)[0].set_type(RigidBody::Type::STATIC); + + Simulator::Settings settings; + settings.minimum_separation_distance = dmin; + // Area-weighted (smooth) collision set: the default is now the + // conventional IPC set (rigid-ipc parity), whose non-smooth max + // approximation struggles to converge at large min-separation (0.1). + settings.use_area_weighting = true; + + Simulator sim(bodies, poses, /*dt=*/0.01, settings); + REQUIRE(sim.run(/*t_end=*/1.0)); + return sim.poses()[1].position.y(); + }; + + const double y_touch = settle_y(0.0); + const double dmin = 0.1; + const double y_gap = settle_y(dmin); + + // A nonzero min-separation makes the dropped cube rest ~dmin higher (the + // barrier keeps the bodies at least dmin apart). + CHECK(y_gap > y_touch + 0.5 * dmin); + CHECK(y_gap == Catch::Approx(y_touch + dmin).margin(0.03)); +} diff --git a/tests/src/tests/distance/test_edge_edge.cpp b/tests/src/tests/distance/test_edge_edge.cpp index 25bc87b54..8b8f3c590 100644 --- a/tests/src/tests/distance/test_edge_edge.cpp +++ b/tests/src/tests/distance/test_edge_edge.cpp @@ -12,9 +12,9 @@ #include #include #include +#include #include -#include #include using namespace ipc; @@ -218,7 +218,7 @@ TEST_CASE("Edge-edge distance degenerate case", "[distance][edge-edge]") double theta = GENERATE(-2, -1.5, -1, -0.123124, 0, 0.2342352, 0.5, 1, 1.5, 2, 50, 51) - * igl::PI; + * ipc::PI; Eigen::Matrix3d R = Eigen::AngleAxisd(theta, Eigen::Vector3d::UnitY()).toRotationMatrix(); @@ -331,13 +331,13 @@ TEST_CASE( SECTION("Almost parallel") { double exponent = GENERATE(range(-6, 3)); - angle = pow(10, exponent) * igl::PI / 180.0; + angle = pow(10, exponent) * ipc::PI / 180.0; } // SECTION("Parallel") { angle = 0; } Eigen::Vector3d e00(-1.0, 0, 0), e01(1.0, 0, 0), e10(cos(angle), 1, sin(angle)), - e11(cos(angle + igl::PI), 1, sin(angle + igl::PI)); + e11(cos(angle + ipc::PI), 1, sin(angle + ipc::PI)); double distance = edge_edge_distance(e00, e01, e10, e11); const Vector12d grad = edge_edge_distance_gradient(e00, e01, e10, e11); diff --git a/tests/src/tests/dynamics/CMakeLists.txt b/tests/src/tests/dynamics/CMakeLists.txt new file mode 100644 index 000000000..60ab69817 --- /dev/null +++ b/tests/src/tests/dynamics/CMakeLists.txt @@ -0,0 +1,15 @@ +set(SOURCES + test_bdf.cpp + test_body_potentials.cpp + test_to_affine.cpp +) + +source_group(TREE "${CMAKE_CURRENT_SOURCE_DIR}" PREFIX "Source Files" FILES ${SOURCES}) +target_sources(ipc_toolkit_tests PRIVATE ${SOURCES}) + +################################################################################ +# Subfolders +################################################################################ + +add_subdirectory(affine) +add_subdirectory(rigid) \ No newline at end of file diff --git a/tests/src/tests/dynamics/affine/CMakeLists.txt b/tests/src/tests/dynamics/affine/CMakeLists.txt new file mode 100644 index 000000000..55bf02695 --- /dev/null +++ b/tests/src/tests/dynamics/affine/CMakeLists.txt @@ -0,0 +1,11 @@ +set(SOURCES + test_augmented_lagrangian.cpp + test_orthogonality_potential.cpp +) + +source_group(TREE "${CMAKE_CURRENT_SOURCE_DIR}" PREFIX "Source Files" FILES ${SOURCES}) +target_sources(ipc_toolkit_tests PRIVATE ${SOURCES}) + +################################################################################ +# Subfolders +################################################################################ \ No newline at end of file diff --git a/tests/src/tests/dynamics/affine/test_augmented_lagrangian.cpp b/tests/src/tests/dynamics/affine/test_augmented_lagrangian.cpp new file mode 100644 index 000000000..bc8213500 --- /dev/null +++ b/tests/src/tests/dynamics/affine/test_augmented_lagrangian.cpp @@ -0,0 +1,209 @@ +// Finite-difference and policy tests for the augmented Lagrangian +// (manual derivatives — no autodiff). + +#include + +#include + +#include + +#include +#include + +using namespace ipc; +using namespace ipc::affine; + +namespace { + +std::shared_ptr +kinematic_cube(std::vector& initial_poses) +{ + Eigen::MatrixXd V; + Eigen::MatrixXi E, F; + REQUIRE(tests::load_mesh("cube.ply", V, E, F)); + + // Non-uniform scaling so J has distinct entries. + V.col(0).array() *= 0.5; + V.col(2).array() *= 2.0; + + initial_poses.assign(1, rigid::Pose::identity(3)); + auto bodies = rigid::RigidBodies::build_from_meshes( + { V }, { E }, { F }, { 1000.0 }, initial_poses); + (*bodies)[0].set_type(rigid::RigidBody::Type::KINEMATIC); + return bodies; +} + +/// Affine DOFs [p; vec(R)] of a rigid pose. +Eigen::VectorXd affine_dof(const rigid::Pose& pose) +{ + Eigen::VectorXd x(12); + x.head<3>() = pose.position; + x.tail<9>() = pose.rotation_matrix().reshaped(); + return x; +} + +} // namespace + +TEST_CASE( + "Affine augmented Lagrangian gradient and hessian", + "[affine][augmented_lagrangian][gradient][hessian]") +{ + std::srand(0); + + std::vector initial_poses; + auto bodies = kinematic_cube(initial_poses); + + // Unreachable satisfied threshold and always-update stall threshold so + // update() takes the multiplier branch (making the multipliers nonzero). + AugmentedLagrangian::Params params; + params.satisfied_progress = 2.0; + params.stall_progress = -10.0; + params.initial_penalty = GENERATE(1e3, 1e6); + + AugmentedLagrangian al(params); + + rigid::Pose target = initial_poses[0]; + target.position += Eigen::Vector3d(0.1, -0.2, 0.3); + target.rotation = Eigen::Vector3d(0.4, -0.1, 0.2); + + const Eigen::VectorXd x0 = affine_dof(initial_poses[0]); + al.init(*bodies, x0, { target }); + REQUIRE(al.active()); + + // Make the multipliers nonzero (multiplier-update branch). + Eigen::VectorXd x_mid = x0; + x_mid.head<3>() += Eigen::Vector3d(0.05, -0.1, 0.15); + x_mid.tail<9>() += 0.05 * Eigen::VectorXd::Random(9); + al.update(*bodies, x_mid); + + // FD check at a third state. + Eigen::VectorXd x = x0; + x.head<3>() += 0.1 * Eigen::Vector3d::Random(); + x.tail<9>() += 0.1 * Eigen::VectorXd::Random(9); + + const Eigen::VectorXd grad = al.gradient(*bodies, x); + Eigen::VectorXd fd_grad; + fd::finite_gradient( + x, [&](const Eigen::VectorXd& y) { return al(*bodies, y); }, fd_grad); + CHECK(fd::compare_gradient(grad, fd_grad)); + + const Eigen::MatrixXd hess = al.hessian(*bodies, x); + Eigen::MatrixXd fd_hess; + fd::finite_jacobian( + x, [&](const Eigen::VectorXd& y) { return al.gradient(*bodies, y); }, + fd_hess); + CHECK(fd::compare_hessian(hess, fd_hess)); +} + +// A kinematic body pushed by a constant load F (the energy −s F·q, like +// gravity) reaches its target, whatever the time step. The next step starts +// over, so the constraint exerts no leftover force at its new target. +TEST_CASE( + "Affine augmented Lagrangian reaches its target under load and starts " + "each step without a leftover force", + "[affine][augmented_lagrangian]") +{ + std::vector initial_poses; + auto bodies = kinematic_cube(initial_poses); + const double m = (*bodies)[0].mass(); + + const double dt = GENERATE(1e-2, 1e-3); + const double s = dt * dt; + const Eigen::Vector3d F(0, -5e4, 2e4); + + AugmentedLagrangian al((AugmentedLagrangian::Params())); + + // Minimizing E_AL(q) − s F·q over the position alone (the rotation stays + // on target): κ m (q − q̂) − s λ − s F = 0. + const auto solve = [&](const rigid::Pose& target) { + Eigen::VectorXd x = affine_dof(target); + x.head<3>() += + s * (al.linear_multiplier(0) + F) / (al.linear_penalty() * m); + return x; + }; + + rigid::Pose target = initial_poses[0]; + target.position += Eigen::Vector3d(0, 0.5 * dt, 0); + al.init(*bodies, affine_dof(initial_poses[0]), { target }, s); + for (int i = 0; i < 50 && al.active(); ++i) { + al.update(*bodies, solve(target)); + } + CHECK(al.linear_satisfied()); + + // Next step: no reaction carries over, so the constraint is at rest at + // its new target. + const rigid::Pose start = target; + target.position += Eigen::Vector3d(0, 0.5 * dt, 0); + al.init(*bodies, affine_dof(start), { target }, s); + CHECK(al.gradient(*bodies, affine_dof(target)).isZero()); +} + +TEST_CASE( + "Affine augmented Lagrangian policy", "[affine][augmented_lagrangian]") +{ + std::vector initial_poses; + auto bodies = kinematic_cube(initial_poses); + + AugmentedLagrangian al((AugmentedLagrangian::Params())); + + rigid::Pose target = initial_poses[0]; + target.position += Eigen::Vector3d(1, 0, 0); + target.rotation = Eigen::Vector3d(0, 0.5, 0); + + const Eigen::VectorXd x0 = affine_dof(initial_poses[0]); + al.init(*bodies, x0, { target }); + REQUIRE(al.active()); + CHECK(al.linear_penalty() == 1e3); + + // η at the start is 0. + CHECK(al.linear_progress(*bodies, x0) == Catch::Approx(0.0)); + CHECK(al.angular_progress(*bodies, x0) == Catch::Approx(0.0)); + + // η at the target is 1 → both channels satisfy. + const Eigen::VectorXd x_target = affine_dof(target); + CHECK(al.linear_progress(*bodies, x_target) == Catch::Approx(1.0)); + CHECK(al.angular_progress(*bodies, x_target) == Catch::Approx(1.0)); + al.update(*bodies, x_target); + CHECK(al.linear_satisfied()); + CHECK(al.angular_satisfied()); + CHECK(!al.active()); + + // Once inactive, the energy is identically zero. + CHECK(al(*bodies, x0) == 0.0); + CHECK(al.gradient(*bodies, x0).isZero()); + + // Re-init resets; no progress → the penalty doubles up to the cap. + al.init(*bodies, x0, { target }); + REQUIRE(al.active()); + double prev_kappa = al.linear_penalty(); + for (int i = 0; i < 25; ++i) { + al.update(*bodies, x0); // η = 0 < stall_progress + // The doubling stops once κ ≥ max_penalty (checked before doubling, + // so κ can land at up to 2×max). + CHECK(al.linear_penalty() <= 2e8); + CHECK(al.linear_penalty() >= prev_kappa); + prev_kappa = al.linear_penalty(); + } + const double capped = al.linear_penalty(); + CHECK(capped >= 1e8); + al.update(*bodies, x0); + CHECK(al.linear_penalty() == capped); // no further doubling + + // Progress that hovers just short of satisfied (e.g., because the load + // shifts between solves) still raises the penalty. + al.init(*bodies, x0, { target }); + Eigen::VectorXd x_near = x_target; + x_near.head<3>() += 0.005 * (x0.head<3>() - x_target.head<3>()); + REQUIRE(al.linear_progress(*bodies, x_near) == Catch::Approx(0.995)); + for (int i = 0; i < 3; ++i) { + al.update(*bodies, x_near); + } + CHECK(!al.linear_satisfied()); + CHECK(al.linear_penalty() > 1e3); + + // Non-kinematic bodies contribute nothing. + (*bodies)[0].set_type(rigid::RigidBody::Type::DYNAMIC); + al.init(*bodies, x0, { target }); + CHECK(!al.active()); + CHECK(al(*bodies, x0) == 0.0); +} diff --git a/tests/src/tests/dynamics/affine/test_orthogonality_potential.cpp b/tests/src/tests/dynamics/affine/test_orthogonality_potential.cpp new file mode 100644 index 000000000..b455a766e --- /dev/null +++ b/tests/src/tests/dynamics/affine/test_orthogonality_potential.cpp @@ -0,0 +1,118 @@ +#include + +#include + +#include + +#include + +#include + +using namespace ipc; +using namespace ipc::rigid; +using namespace ipc::affine; + +// Helper function to generate a RigidBody +auto rigid_bodies() +{ + Eigen::MatrixXd V; + Eigen::MatrixXi E, F; + REQUIRE(tests::load_mesh("cube.ply", V, E, F)); + + const double L = 0.5, W = 1.0, H = 2.0; + V.col(0).array() *= L; + V.col(1).array() *= W; + V.col(2).array() *= H; + + const double density = GENERATE(1.0, 2.0, 3.0); + + std::vector initial_poses; + initial_poses.push_back( + rigid::Pose::identity(3)); // Initial pose at the origin + + auto bodies = RigidBodies::build_from_meshes( + { V }, { E }, { F }, { density }, initial_poses); + + (*bodies)[0].set_external_force( + rigid::Pose(Eigen::Vector3d::Random(), Eigen::Vector3d::Random())); + + return bodies; +} + +TEST_CASE("Orthogonality potential", "[affine]") +{ + auto bodies = rigid_bodies(); + + OrthogonalityPotential V_perp(1); + + VectorMax12d x = VectorMax12d::Random(12); + + double energy = V_perp(*bodies, x); + + // Since we don't have a ground truth, we can only check if the energy is a + // valid number + CHECK(std::isfinite(energy)); + + VectorMax12d analytical_gradient; + MatrixMax12d analytical_hessian; + + { + analytical_gradient = V_perp.gradient(*bodies, x); + REQUIRE(analytical_gradient.squaredNorm() > 1e-8); + + // Compute the gradient using finite differences + auto f = [&](const Eigen::VectorXd& x_arg) { + return V_perp(*bodies, x_arg); + }; + Eigen::VectorXd numerical_gradient; + fd::finite_gradient(x, f, numerical_gradient); + + CHECK(fd::compare_gradient(analytical_gradient, numerical_gradient)); + if (!fd::compare_gradient(analytical_gradient, numerical_gradient)) { + std::cout << "Analytical Gradient:\n" + << analytical_gradient << "\n\n"; + std::cout << "Numerical Gradient:\n" + << numerical_gradient << "\n\n"; + } + } + + { + analytical_hessian = V_perp.hessian(*bodies, x); + + // Numerical hessian calculation + auto f = [&](const Eigen::VectorXd& x_arg) { + return V_perp.gradient(*bodies, x_arg); + }; + Eigen::MatrixXd numerical_hessian; + fd::finite_jacobian(x, f, numerical_hessian); + + // Compare analytical and numerical Hessians + CHECK(fd::compare_jacobian(analytical_hessian, numerical_hessian)); + if (!fd::compare_jacobian(analytical_hessian, numerical_hessian)) { + std::cout << "Analytical Hessian:\n" + << analytical_hessian << "\n\n"; + std::cout << "Numerical Hessian:\n" << numerical_hessian << "\n\n"; + } + } + + // Newton direction + { + Eigen::VectorXd newton_direction = + -analytical_hessian.ldlt().solve(analytical_gradient); + // std::cout << "Newton Direction:\n" << newton_direction << "\n\n"; + + if (newton_direction.dot(analytical_gradient) < 0.0) { + CHECK(true); + } else { + analytical_hessian = + V_perp.hessian(*bodies, x, PSDProjectionMethod::ABS); + + newton_direction = + -analytical_hessian.ldlt().solve(analytical_gradient); + + // std::cout << "Newton Direction:\n" << newton_direction << "\n\n"; + + CHECK(newton_direction.dot(analytical_gradient) < 0.0); + } + } +} \ No newline at end of file diff --git a/tests/src/tests/dynamics/rigid/CMakeLists.txt b/tests/src/tests/dynamics/rigid/CMakeLists.txt new file mode 100644 index 000000000..f4934321b --- /dev/null +++ b/tests/src/tests/dynamics/rigid/CMakeLists.txt @@ -0,0 +1,16 @@ +set(SOURCES + test_mass.cpp + test_pose.cpp + test_rigid_bodies.cpp + test_rigid_body.cpp + test_rigid_candidates.cpp + test_rigid_contact_hessian.cpp + test_rigid_trajectory.cpp +) + +source_group(TREE "${CMAKE_CURRENT_SOURCE_DIR}" PREFIX "Source Files" FILES ${SOURCES}) +target_sources(ipc_toolkit_tests PRIVATE ${SOURCES}) + +################################################################################ +# Subfolders +################################################################################ diff --git a/tests/src/tests/dynamics/rigid/test_mass.cpp b/tests/src/tests/dynamics/rigid/test_mass.cpp new file mode 100644 index 000000000..4f806189b --- /dev/null +++ b/tests/src/tests/dynamics/rigid/test_mass.cpp @@ -0,0 +1,143 @@ +// Test the mass utilities. + +#include +#include +#include + +#include + +#include + +#include + +using namespace ipc; +using namespace ipc::rigid; + +TEST_CASE("Mass properties", "[rigid][mass]") +{ + Eigen::MatrixXd V; + Eigen::MatrixXi E, F; + + const double density = GENERATE(1.0, 2.0, 3.0); + + double expected_mass = -1; + Eigen::Vector3d expected_center; + Eigen::Matrix3d expected_inertia; + SECTION("Cube") + { + REQUIRE(tests::load_mesh("cube.ply", V, E, F)); + + const double L = 0.5, W = 1.0, H = 2.0; + + V.col(0).array() *= L; + V.col(1).array() *= W; + V.col(2).array() *= H; + + expected_mass = density * L * W * H; + + expected_center.setZero(); + + const double Ixx = expected_mass * (W * W + H * H) / 12.0; + const double Iyy = expected_mass * (L * L + H * H) / 12.0; + const double Izz = expected_mass * (L * L + W * W) / 12.0; + expected_inertia = Eigen::DiagonalMatrix(Ixx, Iyy, Izz); + } + SECTION("Bunny (Low-Poly)") + { + REQUIRE(tests::load_mesh("bunny (lowpoly).ply", V, E, F)); + double volume; + igl::moments(V, F, volume, expected_center, expected_inertia); + expected_center /= volume; + expected_inertia.array() *= density; + expected_mass = density * volume; + } + SECTION("Bunny") + { + REQUIRE(tests::load_mesh("bunny.ply", V, E, F)); + double volume; + igl::moments(V, F, volume, expected_center, expected_inertia); + expected_center /= volume; + expected_inertia.array() *= density; + expected_mass = density * volume; + } + SECTION("Bowl") + { + REQUIRE(tests::load_mesh("bowl.ply", V, E, F)); + double volume; + igl::moments(V, F, volume, expected_center, expected_inertia); + expected_center /= volume; + expected_inertia.array() *= density; + expected_mass = density * volume; + } + + double total_mass; + VectorMax3d center; + MatrixMax3d inertia; + compute_mass_properties(V, F, density, total_mass, center, inertia); + + REQUIRE(total_mass == Catch::Approx(expected_mass).margin(1e-6)); + { + CAPTURE(density, center, expected_center); + REQUIRE(center.isApprox(expected_center, 1e-6)); + } + { + CAPTURE(density, inertia, expected_inertia); + REQUIRE(inertia.isApprox(expected_inertia, 1e-6)); + } +} + +TEST_CASE("2D mass properties", "[2d][mass]") +{ + // A W×H rectangle outline with edge-Voronoi lumped masses. + const double W = 2.0, H = 1.0, density = GENERATE(1.0, 100.0); + + Eigen::MatrixXd V(4, 2); + V << -W / 2, -H / 2, W / 2, -H / 2, W / 2, H / 2, -W / 2, H / 2; + Eigen::MatrixXi E(4, 2); + E << 0, 1, 1, 2, 2, 3, 3, 0; + + double mass; + ipc::VectorMax3d center; + ipc::MatrixMax3d inertia; + ipc::rigid::compute_mass_properties(V, E, density, mass, center, inertia); + + // Mass = density × perimeter; centered at the origin. + CHECK(mass == Catch::Approx(density * 2 * (W + H))); + CHECK(center.norm() == Catch::Approx(0).margin(1e-12)); + + // Second moment ∫ρ x̄x̄ᵀ of 4 lumped corner masses m_i = ρ(W + H)/2 at + // (±W/2, ±H/2). + const double corner_mass = density * (W + H) / 2; + REQUIRE(inertia.rows() == 2); + CHECK(inertia(0, 0) == Catch::Approx(4 * corner_mass * W * W / 4)); + CHECK(inertia(1, 1) == Catch::Approx(4 * corner_mass * H * H / 4)); + CHECK(inertia(0, 1) == Catch::Approx(0).margin(1e-10)); +} + +TEST_CASE("2D point cloud mass properties", "[2d][mass]") +{ + Eigen::MatrixXd V(3, 2); + V << 0, 0, 2, 0, 0, 2; + const Eigen::MatrixXi E(0, 2); + + double mass; + ipc::VectorMax3d center; + ipc::MatrixMax3d inertia; + ipc::rigid::compute_mass_properties(V, E, 1.0, mass, center, inertia); + + CHECK(mass == Catch::Approx(3.0)); // unit mass per point + CHECK(center.x() == Catch::Approx(2.0 / 3.0)); + CHECK(center.y() == Catch::Approx(2.0 / 3.0)); + REQUIRE(inertia.rows() == 2); + // Second moment about the COM: Σ (vᵢ − c)(vᵢ − c)ᵀ + double sxx = 0, syy = 0, sxy = 0; + for (int i = 0; i < 3; ++i) { + const Eigen::Vector2d r = V.row(i).transpose() - center; + sxx += r.x() * r.x(); + syy += r.y() * r.y(); + sxy += r.x() * r.y(); + } + CHECK(inertia(0, 0) == Catch::Approx(sxx)); + CHECK(inertia(1, 1) == Catch::Approx(syy)); + CHECK(inertia(0, 1) == Catch::Approx(sxy)); +} diff --git a/tests/src/tests/dynamics/rigid/test_pose.cpp b/tests/src/tests/dynamics/rigid/test_pose.cpp new file mode 100644 index 000000000..d7afab0e0 --- /dev/null +++ b/tests/src/tests/dynamics/rigid/test_pose.cpp @@ -0,0 +1,282 @@ +#include +#include + +#include +#include + +#include +#include +#include + +#include + +using namespace ipc; +using namespace ipc::rigid; + +TEST_CASE("so(3) -> SO(3)", "[rigid][pose]") +{ + double angle; + Eigen::Vector3d axis; + + SECTION("zero") + { + angle = 0; + axis = Eigen::Vector3d::Random(); + } + SECTION("small angle") + { + angle = GENERATE(take(100, random(0.0, 1e-6))); + axis = Eigen::Vector3d::Random(); + } + SECTION("random") + { + angle = GENERATE(take(100, random(0.0, 2 * ipc::PI))); + axis = Eigen::Vector3d::Random(); + } + axis.normalize(); + + const Eigen::Matrix3d R_expected = + Eigen::AngleAxisd(angle, axis).toRotationMatrix(); + + const Eigen::Vector3d theta = angle * axis; + const Eigen::Matrix3d R_actual = rotation_vector_to_matrix(theta); + + CHECK((R_actual - R_expected).norm() == Catch::Approx(0).margin(1e-12)); +} + +TEST_CASE("Benchmark so(3) -> SO(3)", "[!benchmark][rigid][pose]") +{ + Eigen::Vector3d theta = Eigen::Vector3d::Random(); + + BENCHMARK("Compute R") { return rotation_vector_to_matrix(theta); }; + + BENCHMARK("Compute ∇R") + { + return rotation_vector_to_matrix_jacobian(theta); + }; + + BENCHMARK("Compute ∇²R") + { + return rotation_vector_to_matrix_hessian(theta); + }; +} + +#if false +TEST_CASE("Interval so(3) -> SO(3)", "[!benchmark][physics][pose]") +{ + using namespace ipc::rigid; + double angle; + Eigen::Vector3d axis; + + SECTION("zero") + { + angle = 0; + axis = Eigen::Vector3d::Random(); + } + SECTION("random") + { + angle = GENERATE(take(1, random(0.0, 2 * ipc::PI))); + axis = Eigen::Vector3d::Random(); + } + axis.normalize(); + + Pose p = Pose::Zero(3); + p.rotation = angle * axis; + BENCHMARK("Double so(3) -> SO(3)") + { + Eigen::Matrix3d R = p.construct_rotation_matrix(); + }; + Pose pI = p.cast(); + BENCHMARK("Interval so(3) -> SO(3)") + { + Matrix3I R = pI.construct_rotation_matrix(); + }; +} +#endif + +TEST_CASE("so(3) -> SO(3) derivatives", "[rigid][pose]") +{ + double angle; + Eigen::Vector3d axis; + + SECTION("zero") + { + angle = 0; + axis = Eigen::Vector3d::Random(); + } + SECTION("small angle") + { + angle = GENERATE(take(100, random(0.0, 1e-6))); + axis = Eigen::Vector3d::Random(); + } + SECTION("random") + { + angle = GENERATE(take(100, random(0.0, 2 * ipc::PI))); + axis = Eigen::Vector3d::Random(); + } + axis.normalize(); + + Eigen::Vector3d theta = angle * axis; + + { + Eigen::Matrix J_analytic = + rotation_vector_to_matrix_jacobian(theta); + + auto f = [&](const Eigen::VectorXd& x) -> Eigen::MatrixXd { + return rotation_vector_to_matrix(x); + }; + + Eigen::MatrixXd J_numerical; + fd::finite_jacobian_tensor<3>(theta, f, J_numerical); + + CHECK(fd::compare_jacobian(J_analytic, J_numerical)); + if (!fd::compare_jacobian(J_analytic, J_numerical)) { + std::cout << "J_analytic:\n" << J_analytic << "\n\n"; + std::cout << "J_numerical:\n" << J_numerical << "\n\n"; + } + } + + { + Eigen::Matrix H_analytic = + rotation_vector_to_matrix_hessian(theta); + + auto f = [&](const Eigen::VectorXd& x) -> Eigen::MatrixXd { + return rotation_vector_to_matrix_jacobian(x); + }; + + Eigen::MatrixXd H_numerical; + fd::finite_jacobian_tensor<4>(theta, f, H_numerical); + + CHECK(fd::compare_jacobian(H_analytic, H_numerical)); + if (!fd::compare_jacobian(H_analytic, H_numerical)) { + std::cout << "H_analytic:\n" << H_analytic << "\n\n"; + std::cout << "H_numerical:\n" << H_numerical << "\n\n"; + } + } +} + +TEST_CASE("SO(3) -> so(3)", "[rigid][pose]") +{ + double angle = GENERATE( + 0.0, 1e-8, 1e-6, ipc::PI / 4, ipc::PI / 2, ipc::PI * 0.75, + ipc::PI - 1e-4, ipc::PI - 1e-6, ipc::PI - 1e-8, ipc::PI, ipc::PI + 1e-8, + ipc::PI + 1e-6, ipc::PI + 1e-4, ipc::PI * 1.5, ipc::PI * 2); + Eigen::Vector3d axis = Eigen::Vector3d::Random().normalized(); + + Eigen::Vector3d theta_expected = angle * axis; + + Eigen::Matrix3d R = rotation_vector_to_matrix(theta_expected); + + Eigen::Vector3d theta = rotation_matrix_to_vector(R); + + Eigen::AngleAxisd angle_axis(R); + theta_expected = angle_axis.angle() * angle_axis.axis(); + + CHECK( + (theta.isApprox(theta_expected, 1e-4) + || theta.isApprox(angle * axis, 1e-4))); + + // std::cout << "input axis: " << axis.transpose() << "\n" + // << "input angle: " << angle << " (" << angle - ipc::PI << ")" + // << "\n" + // << "expected axis: " << angle_axis.axis().transpose() << "\n" + // << "expected angle: " << angle_axis.angle() << "\n" + // << "axis: " << theta.normalized().transpose() << "\n" + // << "angle: " << theta.norm() << "\n" + // << "\n" + // << std::endl; +} + +TEST_CASE("Benchmark SO(3) -> so(3)", "[!benchmark][rigid][pose]") +{ + Eigen::Matrix3d R = Eigen::Matrix3d::Random(); + Eigen::JacobiSVD svd( + R, Eigen::ComputeFullU | Eigen::ComputeFullV); + R = svd.matrixU() * svd.matrixV().transpose(); // closest rotation matrix + + BENCHMARK("Mine") { return rotation_matrix_to_vector(R); }; + + BENCHMARK("Eigen") + { + Eigen::AngleAxisd angle_axis(R); + return angle_axis.angle() * angle_axis.axis(); + }; +} + +TEST_CASE("Pose transform vertices Jacobian", "[rigid][pose][jacobian]") +{ + GENERATE(range(0, 10)); + const int DIM = GENERATE(2, 3); + + Pose p; + p.position = VectorMax3d::Random(DIM); + p.rotation = VectorMax3d::Random(DIM == 2 ? 1 : 3); + + constexpr int NUM_VERTICES = 2; + Eigen::MatrixXd V = Eigen::MatrixXd::Random(NUM_VERTICES, DIM); + + Eigen::MatrixXd dV_dx_analytic = p.transform_vertices_jacobian(V); + + auto f = [&](const Eigen::VectorXd& x) -> Eigen::VectorXd { + return Pose(x).transform_vertices(V).reshaped(); + }; + + Eigen::VectorXd x(DIM == 2 ? 3 : 6); + x << p.position, p.rotation; + + Eigen::MatrixXd dV_dx_numerical; + fd::finite_jacobian(x, f, dV_dx_numerical); + + CHECK(fd::compare_jacobian(dV_dx_analytic, dV_dx_numerical)); + if (!fd::compare_jacobian(dV_dx_analytic, dV_dx_numerical)) { + std::cout << "dV_dx_analytic:\n" << dV_dx_analytic << "\n\n"; + std::cout << "dV_dx_numerical:\n" << dV_dx_numerical << "\n\n"; + } + + // The single-vertex Jacobian matches that vertex's rows. + for (int i = 0; i < NUM_VERTICES; ++i) { + CHECK( + fd::compare_jacobian( + p.transform_vertex_jacobian(V.row(i).transpose()), + dV_dx_numerical.middleRows(i * DIM, DIM))); + } +} + +TEST_CASE("Pose transform vertices Hessian", "[rigid][pose][hessian]") +{ + // GENERATE(range(0, 10)); + const int DIM = GENERATE(2, 3); + + Pose p; + p.position = VectorMax3d::Random(DIM); + p.rotation = VectorMax3d::Random(DIM == 2 ? 1 : 3); + + constexpr int NUM_VERTICES = 2; + Eigen::MatrixXd V = Eigen::MatrixXd::Random(NUM_VERTICES, DIM); + + Eigen::MatrixXd d2V_dx2_analytic = p.transform_vertices_hessian(V); + + auto f = [&](const Eigen::VectorXd& x) -> Eigen::MatrixXd { + return Pose(x).transform_vertices_jacobian(V); + }; + + Eigen::VectorXd x(DIM == 2 ? 3 : 6); + x << p.position, p.rotation; + + Eigen::MatrixXd d2V_dx2_numerical; + fd::finite_jacobian_tensor<4>(x, f, d2V_dx2_numerical); + + CHECK(fd::compare_jacobian(d2V_dx2_analytic, d2V_dx2_numerical)); + if (!fd::compare_jacobian(d2V_dx2_analytic, d2V_dx2_numerical)) { + std::cout << "dV_dx_analytic:\n" << d2V_dx2_analytic << "\n\n"; + std::cout << "dV_dx_numerical:\n" << d2V_dx2_numerical << "\n\n"; + } + + // The single-vertex Hessian matches that vertex's rows. + for (int i = 0; i < NUM_VERTICES; ++i) { + CHECK( + fd::compare_jacobian( + p.transform_vertex_hessian(V.row(i).transpose()), + d2V_dx2_numerical.middleRows(i * DIM, DIM))); + } +} \ No newline at end of file diff --git a/tests/src/tests/dynamics/rigid/test_rigid_bodies.cpp b/tests/src/tests/dynamics/rigid/test_rigid_bodies.cpp new file mode 100644 index 000000000..16f1bed67 --- /dev/null +++ b/tests/src/tests/dynamics/rigid/test_rigid_bodies.cpp @@ -0,0 +1,143 @@ +// Test the mass utilities. + +#include +#include +#include +#include + +#include +#include +#include + +#include + +using namespace ipc; +using namespace ipc::rigid; + +TEST_CASE("RigidBodies::to_rigid_dof", "[rigid]") +{ + const int num_vertices_per_body = 10; + + std::vector initial_poses(2, Pose::identity(3)); + auto bodies = RigidBodies::build_from_meshes( + std::vector { + Eigen::MatrixXd::Random(num_vertices_per_body, 3), + Eigen::MatrixXd::Random(num_vertices_per_body, 3), + }, + std::vector(2), std::vector(2), + std::vector(2, 1.0), initial_poses); + + auto f = [&](const Eigen::VectorXd& x) -> double { + assert(x.size() == 12); + auto Va = bodies->body_vertices(0, Pose(x.head<6>())); + auto Vb = bodies->body_vertices(1, Pose(x.tail<6>())); + return (Va.transpose() * Vb).trace(); + }; + + Eigen::VectorXd x = Eigen::VectorXd::Random(12); + + // --- Gradient ------------------------------------------------------------ + + Eigen::VectorXd df_dV(bodies->ndof()); + df_dV.head(3 * num_vertices_per_body) = + bodies->body_vertices(1, Pose(x.tail<6>())).reshaped(); + df_dV.tail(3 * num_vertices_per_body) = + bodies->body_vertices(0, Pose(x.head<6>())).reshaped(); + + Eigen::VectorXd df_dx = + bodies->to_rigid_dof({ Pose(x.head<6>()), Pose(x.tail<6>()) }, df_dV); + + Eigen::VectorXd df_dx_fd; + fd::finite_gradient(x, f, df_dx_fd); + + CHECK(fd::compare_gradient(df_dx, df_dx_fd)); + if (!fd::compare_gradient(df_dx, df_dx_fd)) { + std::cout << "analytic:\n" << df_dx << "\n\n"; + std::cout << "numerical:\n" << df_dx_fd << "\n\n"; + } + + // --- Hessian ------------------------------------------------------------- + + Eigen::MatrixXd d2f_dV2 = + Eigen::MatrixXd::Zero(bodies->ndof(), bodies->ndof()); + d2f_dV2.topRightCorner(3 * num_vertices_per_body, 3 * num_vertices_per_body) + .setIdentity(); + d2f_dV2 + .bottomLeftCorner(3 * num_vertices_per_body, 3 * num_vertices_per_body) + .setIdentity(); + + Eigen::MatrixXd d2f_dx2 = bodies->to_rigid_dof( + { Pose(x.head<6>()), Pose(x.tail<6>()) }, df_dV, d2f_dV2.sparseView()); + + Eigen::MatrixXd d2f_dx2_fd; + fd::finite_hessian(x, f, d2f_dx2_fd); + + CHECK(fd::compare_hessian(d2f_dx2, d2f_dx2_fd)); + if (!fd::compare_hessian(d2f_dx2, d2f_dx2_fd)) { + std::cout << "analytic:\n" << d2f_dx2 << "\n\n"; + std::cout << "numerical:\n" << d2f_dx2_fd << "\n\n"; + } +} +TEST_CASE( + "Converting a plane keeps the other bodies' properties aligned", "[rigid]") +{ + Eigen::MatrixXd V_cube; + Eigen::MatrixXi E_cube, F_cube; + REQUIRE(tests::load_mesh("cube.ply", V_cube, E_cube, F_cube)); + + // A 2×2 square in the y = 0 plane, listed before the cubes. + Eigen::MatrixXd V_plane(4, 3); + V_plane << -1, 0, -1, 1, 0, -1, 1, 0, 1, -1, 0, 1; + Eigen::MatrixXi F_plane(2, 3); + F_plane << 0, 2, 1, 0, 3, 2; + Eigen::MatrixXi E_plane(5, 2); + E_plane << 0, 1, 1, 2, 2, 0, 2, 3, 3, 0; + + VectorMax6b slider(6); + slider << false, true, true, true, true, true; + + std::vector initial_poses(3, Pose::identity(3)); + initial_poses[1].position = Eigen::Vector3d(0, 1, 0); + initial_poses[2].position = Eigen::Vector3d(3, 1, 0); + + const auto bodies = RigidBodies::build_from_meshes( + { V_plane, V_cube, V_cube }, { E_plane, E_cube, E_cube }, + { F_plane, F_cube, F_cube }, { 1.0, 500.0, 2000.0 }, initial_poses, + /*convert_planes=*/true, { VectorMax6b(), VectorMax6b(), slider }, + { "ground", "light", "heavy" }); + + REQUIRE(bodies->planes.size() == 1); + REQUIRE(bodies->num_bodies() == 2); + REQUIRE(initial_poses.size() == 2); + + // Each cube keeps the density, name, DOF mask, and pose it was given. + CHECK((*bodies)[0].density() == Catch::Approx(500.0)); + CHECK((*bodies)[1].density() == Catch::Approx(2000.0)); + CHECK(bodies->body_name(0) == "light"); + CHECK(bodies->body_name(1) == "heavy"); + CHECK(!(*bodies)[0].is_dof_fixed().any()); + CHECK((*bodies)[1].is_dof_fixed() == slider); + CHECK(initial_poses[0].position.isApprox(Eigen::Vector3d(0, 1, 0))); + CHECK(initial_poses[1].position.isApprox(Eigen::Vector3d(3, 1, 0))); +} + +TEST_CASE("A copied RigidBodies still excludes same-body collisions", "[rigid]") +{ + Eigen::MatrixXd V; + Eigen::MatrixXi E, F; + REQUIRE(tests::load_mesh("cube.ply", V, E, F)); + + std::vector initial_poses(2, Pose::identity(3)); + initial_poses[1].position = Eigen::Vector3d(2, 0, 0); + auto bodies = RigidBodies::build_from_meshes( + { V, V }, { E, E }, { F, F }, { 1000.0, 1000.0 }, initial_poses); + + // The copy must not depend on the original, which we then destroy. + const auto copy = std::make_unique(*bodies); + bodies.reset(); + + const auto nv = copy->body_num_vertices(0); + CHECK(!copy->can_collide(0, 1)); // same body + CHECK(copy->can_collide(0, nv)); // different bodies + CHECK(!copy->can_collide(nv, nv + 1)); // same body +} diff --git a/tests/src/tests/dynamics/rigid/test_rigid_body.cpp b/tests/src/tests/dynamics/rigid/test_rigid_body.cpp new file mode 100644 index 000000000..f48e93933 --- /dev/null +++ b/tests/src/tests/dynamics/rigid/test_rigid_body.cpp @@ -0,0 +1,205 @@ +// Test the mass utilities. + +#include +#include +#include + +#include + +#include + +using namespace ipc; +using namespace ipc::rigid; + +TEST_CASE("Rigid body construction", "[rigid]") +{ + Eigen::MatrixXd V; + Eigen::MatrixXi E, F; + REQUIRE(tests::load_mesh("cube.ply", V, E, F)); + + const double L = 0.5, W = 1.0, H = 2.0; + V.col(0).array() *= L; + V.col(1).array() *= W; + V.col(2).array() *= H; + + const double density = GENERATE(1.0, 2.0, 3.0); + + Pose input_pose = Pose::identity(3); + Pose initial_pose = Pose::identity(3); + + SECTION("No modification") { } + SECTION("Input pose") + { + input_pose.position = Eigen::Vector3d(1.0, 2.0, 3.0); + input_pose.rotation = Eigen::Vector3d(0.1, 0.2, 0.3); + } + SECTION("Initial pose") + { + initial_pose.position = Eigen::Vector3d(1.0, 2.0, 3.0); + initial_pose.rotation = Eigen::Vector3d(0.1, 0.2, 0.3); + } + SECTION("Input and initial pose") + { + input_pose.position = Eigen::Vector3d(1.0, 2.0, 3.0); + input_pose.rotation = Eigen::Vector3d(0.1, 0.2, 0.3); + initial_pose.position = Eigen::Vector3d(4.0, 5.0, 6.0); + initial_pose.rotation = Eigen::Vector3d(0.4, 0.5, 0.6); + } + + Eigen::MatrixXd modified_V = input_pose.transform_vertices(V); + + const double m = density * L * W * H; // unit mass per voxel + const double Ixx = m * (W * W + H * H) / 12.0; + const double Iyy = m * (L * L + H * H) / 12.0; + const double Izz = m * (L * L + W * W) / 12.0; + Eigen::Vector3d I(Ixx, Iyy, Izz); + // Sort: + if (I(0) > I(1)) { + std::swap(I(0), I(1)); + } + if (I(1) > I(2)) { + std::swap(I(1), I(2)); + } + if (I(0) > I(1)) { + std::swap(I(0), I(1)); + } + + Pose modified_pose = initial_pose; + const RigidBody body(modified_V, E, F, density, modified_pose); + REQUIRE(modified_V.rows() == V.rows()); + REQUIRE(modified_V.cols() == V.cols()); + + { + CAPTURE(body.moment_of_inertia().transpose(), I.transpose()); + REQUIRE(body.mass() == Catch::Approx(m).margin(1e-8)); + REQUIRE(body.moment_of_inertia().isApprox(I)); + } + + // The modified position should be the initial position plus the input + // position + { + CAPTURE( + input_pose.position.transpose(), initial_pose.position.transpose(), + modified_pose.position.transpose()); + REQUIRE(modified_pose.position.isApprox( + (initial_pose * input_pose).position)); + } + + // The modified rotation should be the initial rotation times the input + // rotation + { + const Eigen::MatrixXd _V = modified_pose.transform_vertices(modified_V); + + const Eigen::MatrixXd expected_V = + (initial_pose * input_pose).transform_vertices(V); + + CHECK(_V.isApprox(expected_V, 1e-8)); + } +} +TEST_CASE( + "Changing a body's density matches building it at that density", "[rigid]") +{ + Eigen::MatrixXd V; + Eigen::MatrixXi E, F; + REQUIRE(tests::load_mesh("cube.ply", V, E, F)); + V.col(0).array() *= 2.0; // anisotropic so the inertia is nontrivial + + const double density = GENERATE(0.5, 7850.0); + + Eigen::MatrixXd V_default = V, V_expected = V; + Pose pose_default = Pose::identity(3), pose_expected = Pose::identity(3); + RigidBody body(V_default, E, F, /*density=*/1000.0, pose_default); + const RigidBody expected(V_expected, E, F, density, pose_expected); + + body.set_density(density); + + CHECK(body.density() == Catch::Approx(density)); + CHECK(body.volume() == Catch::Approx(expected.volume())); + CHECK(body.mass() == Catch::Approx(expected.mass())); + CHECK(body.moment_of_inertia().isApprox(expected.moment_of_inertia())); + CHECK(body.second_moment().diagonal().isApprox( + expected.second_moment().diagonal())); + // The geometry and principal frame do not depend on density. + CHECK(body.principal_rotation().isApprox(expected.principal_rotation())); + CHECK(pose_default.rotation.isApprox(pose_expected.rotation)); +} + +TEST_CASE( + "A body spinning about one world axis has that axis's inertia", "[rigid]") +{ + Eigen::MatrixXd V; + Eigen::MatrixXi E, F; + REQUIRE(tests::load_mesh("cube.ply", V, E, F)); + const double L = 0.5, W = 1.0, H = 2.0; + V.col(0).array() *= L; + V.col(1).array() *= W; + V.col(2).array() *= H; + + const int free_axis = GENERATE(0, 1, 2); + CAPTURE(free_axis); + + VectorMax6b is_dof_fixed = VectorMax6b::Zero(6); + is_dof_fixed.tail<3>().setOnes(); + is_dof_fixed(3 + free_axis) = false; + + const double density = 1000.0; + Pose pose = Pose::identity(3); + const RigidBody body(V, E, F, density, pose, is_dof_fixed); + + // The second moment ∫ρ x̄x̄ᵀ of the box, whose two entries orthogonal to + // the free axis sum to the moment of inertia about that axis. + const double m = density * L * W * H; + const Eigen::Vector3d second_moment = + m / 12.0 * Eigen::Vector3d(L * L, W * W, H * H); + + REQUIRE(body.second_moment().diagonal().size() == 3); + CHECK(body.second_moment().diagonal().isApprox(second_moment)); + const double I_free = second_moment.sum() - second_moment(free_axis); + CHECK( + body.moment_of_inertia()(free_axis) + == Catch::Approx(I_free).epsilon(1e-8)); +} + +TEST_CASE( + "A prescribed center of mass becomes the body's rotation center", "[rigid]") +{ + Eigen::MatrixXd V; + Eigen::MatrixXi E, F; + REQUIRE(tests::load_mesh("cube.ply", V, E, F)); + constexpr double L = 0.5, W = 1.0, H = 2.0; + V.col(0).array() *= L; + V.col(1).array() *= W; + V.col(2).array() *= H; + + // The box's own center of mass, one of its corners, and a point inside. + const Eigen::Vector3d center = GENERATE( + Eigen::Vector3d(0, 0, 0), Eigen::Vector3d(L / 2, W / 2, H / 2), + Eigen::Vector3d(-0.1, 0.3, -0.7)); + CAPTURE(center.transpose()); + + // We fix two rotations so the moments stay about the world axes. + VectorMax6b is_dof_fixed = VectorMax6b::Zero(6); + is_dof_fixed.segment<2>(3).setOnes(); + + const double density = 1000.0; + Eigen::MatrixXd rest_V = V; + Pose pose = Pose::identity(3); + const RigidBody body( + rest_V, E, F, density, pose, is_dof_fixed, VectorMax3d(center)); + + // The body sits where it was given, posed about the prescribed point. + CHECK((pose.position - center).norm() < 1e-12); + CHECK(pose.transform_vertices(rest_V).isApprox(V, 1e-12)); + + // The mass is unchanged, and the inertia is about the new center. + const double m = density * L * W * H; + CHECK(body.mass() == Catch::Approx(m)); + const Eigen::Vector3d I_center = + m / 12.0 * Eigen::Vector3d(W * W + H * H, L * L + H * H, L * L + W * W) + + m + * Eigen::Vector3d( + center.y() * center.y() + center.z() * center.z(), + center.x() * center.x() + center.z() * center.z(), + center.x() * center.x() + center.y() * center.y()); + CHECK(body.moment_of_inertia().isApprox(I_center)); +} diff --git a/tests/src/tests/dynamics/rigid/test_rigid_candidates.cpp b/tests/src/tests/dynamics/rigid/test_rigid_candidates.cpp new file mode 100644 index 000000000..8c608dda9 --- /dev/null +++ b/tests/src/tests/dynamics/rigid/test_rigid_candidates.cpp @@ -0,0 +1,577 @@ +#include +#include + +#include + +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include + +#include + +using namespace ipc; +using namespace ipc::rigid; + +namespace { + +/// A long thin rod along x (half-length 2) with distinct y/z extents so the +/// principal axes are uniquely determined. +void make_rod(Eigen::MatrixXd& V, Eigen::MatrixXi& E, Eigen::MatrixXi& F) +{ + tests::load_mesh("cube.ply", V, E, F); + V.col(0) *= 4.0; + V.col(1) *= 0.2; + V.col(2) *= 0.1; +} + +std::vector lerp_poses( + const std::vector& poses_t0, + const std::vector& poses_t1, + const double t) +{ + std::vector poses(poses_t0.size()); + for (size_t i = 0; i < poses.size(); i++) { + poses[i] = Pose( + (1 - t) * poses_t0[i].position + t * poses_t1[i].position, + (1 - t) * poses_t0[i].rotation + t * poses_t1[i].rotation); + } + return poses; +} + +/// Compose a world-frame rotation on top of a pose's rotation. +/// NOTE: build_from_meshes folds each body's principal-axes rotation R₀ into +/// the initial poses, so world-frame rotations must be composed with — not +/// assigned over — the post-build rotation. +VectorMax3d +compose_rotation(const Eigen::Matrix3d& world_rotation, const Pose& pose) +{ + return rotation_matrix_to_vector(world_rotation * pose.rotation_matrix()); +} + +/// Collect the inter-body element pairs that come within `radius` at any +/// sampled time. These are pairs the broad phase must report. +void sampled_close_pairs( + const RigidBodies& bodies, + const std::vector& poses_t0, + const std::vector& poses_t1, + const double radius, + std::set>& ee_pairs, + std::set>& fv_pairs, + const int n_samples = 21) +{ + const Eigen::MatrixXi& E = bodies.edges(); + const Eigen::MatrixXi& F = bodies.faces(); + + const auto edge_body = [&](index_t e) { + return bodies.vertex_to_body(E(e, 0)); + }; + const auto face_body = [&](index_t f) { + return bodies.vertex_to_body(F(f, 0)); + }; + + for (int s = 0; s <= n_samples; s++) { + const double t = s / double(n_samples); + const Eigen::MatrixXd V = + bodies.vertices(lerp_poses(poses_t0, poses_t1, t)); + + for (index_t ea = 0; ea < E.rows(); ea++) { + for (index_t eb = ea + 1; eb < E.rows(); eb++) { + if (edge_body(ea) == edge_body(eb)) { + continue; + } + const double d = edge_edge_distance( + V.row(E(ea, 0)), V.row(E(ea, 1)), V.row(E(eb, 0)), + V.row(E(eb, 1))); + if (d <= radius * radius) { + ee_pairs.emplace(std::min(ea, eb), std::max(ea, eb)); + } + } + } + + for (index_t f = 0; f < F.rows(); f++) { + for (index_t v = 0; v < V.rows(); v++) { + if (face_body(f) == bodies.vertex_to_body(v)) { + continue; + } + const double d = point_triangle_distance( + V.row(v), V.row(F(f, 0)), V.row(F(f, 1)), V.row(F(f, 2))); + if (d <= radius * radius) { + fv_pairs.emplace(f, v); + } + } + } + } +} + +/// Sampled first time at which any inter-body element pair comes within +/// `threshold`. A small positive threshold with fine sampling is required +/// because exactly-touching instants fall between samples, and once a vertex +/// penetrates the other body's interior all surface distances are positive +/// again. +double sampled_first_contact( + const RigidBodies& bodies, + const std::vector& poses_t0, + const std::vector& poses_t1, + const double threshold = 1e-3, + const int n_samples = 20'000) +{ + const Eigen::MatrixXi& E = bodies.edges(); + const Eigen::MatrixXi& F = bodies.faces(); + for (int s = 0; s <= n_samples; s++) { + const double t = s / double(n_samples); + const Eigen::MatrixXd V = + bodies.vertices(lerp_poses(poses_t0, poses_t1, t)); + for (index_t f = 0; f < F.rows(); f++) { + for (index_t v = 0; v < V.rows(); v++) { + if (bodies.vertex_to_body(F(f, 0)) + == bodies.vertex_to_body(v)) { + continue; + } + const double d = point_triangle_distance( + V.row(v), V.row(F(f, 0)), V.row(F(f, 1)), V.row(F(f, 2))); + if (d <= threshold * threshold) { + return t; + } + } + } + for (index_t ea = 0; ea < E.rows(); ea++) { + for (index_t eb = ea + 1; eb < E.rows(); eb++) { + if (bodies.vertex_to_body(E(ea, 0)) + == bodies.vertex_to_body(E(eb, 0))) { + continue; + } + const double d = edge_edge_distance( + V.row(E(ea, 0)), V.row(E(ea, 1)), V.row(E(eb, 0)), + V.row(E(eb, 1))); + if (d <= threshold * threshold) { + return t; + } + } + } + } + return 1.0; +} + +/// Build the spinning-rod-and-cube scene: a rod spinning half a revolution in +/// place with a small static cube placed on the rod tip's mid-sweep position. +/// The endpoint positions of the rod hug its initial axis, so linearized +/// vertex trajectories never approach the cube — only the mid-step sweep does. +std::shared_ptr make_spinning_rod_scene( + std::vector& poses_t0, std::vector& poses_t1) +{ + Eigen::MatrixXd V_rod, V_cube; + Eigen::MatrixXi E_rod, F_rod, E_cube, F_cube; + make_rod(V_rod, E_rod, F_rod); + tests::load_mesh("cube.ply", V_cube, E_cube, F_cube); + + // Stage 1: build the rod alone to learn its post-build pose (R₀ folded + // in) and locate the tip's mid-sweep position. + std::vector rod_poses(1, Pose::identity(3)); + auto rod_only = RigidBodies::build_from_meshes( + { V_rod }, { E_rod }, { F_rod }, { 1000.0 }, rod_poses); + + Pose rod_t1 = rod_poses[0]; + rod_t1.rotation = compose_rotation( + Eigen::AngleAxisd(ipc::PI, Eigen::Vector3d::UnitZ()).toRotationMatrix(), + rod_poses[0]); + + // Tip = rest vertex with the largest radius + index_t tip = 0; + rod_only->body_rest_positions(0).rowwise().norm().maxCoeff(&tip); + const VectorMax3d tip_mid = RigidTrajectory( + rod_only->body_rest_positions(0).row(tip).transpose(), rod_poses[0], + rod_t1)(0.5); + + // Stage 2: build the combined scene with the cube at the mid-sweep point, + // offset slightly outward so the endpoints are collision free. + std::vector initial_poses(2, Pose::identity(3)); + initial_poses[1].position = 1.2 * tip_mid; + + auto bodies = RigidBodies::build_from_meshes( + { V_rod, V_cube }, { E_rod, E_cube }, { F_rod, F_cube }, + { 1000.0, 1000.0 }, initial_poses); + + poses_t0 = initial_poses; + poses_t1 = initial_poses; + poses_t1[0] = rod_t1; // The rod's build is identical in both stages + + return bodies; +} + +} // namespace + +TEST_CASE("Rigid broad phase completeness", "[rigid][candidates]") +{ + // Two bodies with different vertex counts (exercises the body swap logic): + // a cube and a once-upsampled cube. + Eigen::MatrixXd V_cube; + Eigen::MatrixXi E_cube, F_cube; + tests::load_mesh("cube.ply", V_cube, E_cube, F_cube); + + Eigen::MatrixXd V_fine; + Eigen::MatrixXi F_fine, E_fine; + igl::upsample(V_cube, F_cube, V_fine, F_fine, 1); + igl::edges(F_fine, E_fine); + + std::vector initial_poses(2, Pose::identity(3)); + auto bodies = RigidBodies::build_from_meshes( + { V_fine, V_cube }, { E_fine, E_cube }, { F_fine, F_cube }, + { 1000.0, 1000.0 }, initial_poses); + + std::vector poses_t0 = initial_poses; + std::vector poses_t1 = initial_poses; + + SECTION("translation") + { + poses_t0[1].position = Eigen::Vector3d(2.5, 0, 0); + poses_t1[1].position = Eigen::Vector3d(1.05, 0, 0); + } + SECTION("rotation and translation") + { + poses_t0[1].position = Eigen::Vector3d(1.5, 1.5, 0); + poses_t1[1].position = Eigen::Vector3d(1.05, 0.25, 0); + poses_t1[1].rotation = compose_rotation( + Eigen::AngleAxisd(ipc::PI / 2, Eigen::Vector3d::UnitZ()) + .toRotationMatrix(), + poses_t0[1]); + poses_t1[0].rotation = compose_rotation( + Eigen::AngleAxisd(ipc::PI / 4, Eigen::Vector3d::UnitY()) + .toRotationMatrix(), + poses_t0[0]); + } + + const double inflation_radius = 0.1; + + RigidCandidates candidates; + candidates.build(*bodies, poses_t0, poses_t1, inflation_radius); + + std::set> ee_pairs, fv_pairs; + sampled_close_pairs( + *bodies, poses_t0, poses_t1, inflation_radius, ee_pairs, fv_pairs); + + REQUIRE((ee_pairs.size() > 0 || fv_pairs.size() > 0)); + + std::set> found_ee, found_fv; + for (const EdgeEdgeCandidate& c : candidates.ee_candidates) { + found_ee.emplace( + std::min(c.edge0_id, c.edge1_id), std::max(c.edge0_id, c.edge1_id)); + } + for (const FaceVertexCandidate& c : candidates.fv_candidates) { + found_fv.emplace(c.face_id, c.vertex_id); + } + + for (const auto& p : ee_pairs) { + CHECK(found_ee.count(p) == 1); + } + for (const auto& p : fv_pairs) { + CHECK(found_fv.count(p) == 1); + } +} + +TEST_CASE( + "Rigid broad phase catches rotational tunneling", "[rigid][candidates]") +{ + std::vector poses_t0, poses_t1; + auto bodies = make_spinning_rod_scene(poses_t0, poses_t1); + + const double inflation_radius = 0.01; + + // Sanity check the scene: the bodies are far apart at both endpoints but + // touch mid-sweep. + std::set> ee_pairs, fv_pairs; + sampled_close_pairs( + *bodies, poses_t0, poses_t1, inflation_radius, ee_pairs, fv_pairs, + /*n_samples=*/1); // only t=0 and t=1 + REQUIRE(ee_pairs.empty()); + REQUIRE(fv_pairs.empty()); + REQUIRE(sampled_first_contact(*bodies, poses_t0, poses_t1) < 1.0); + + RigidCandidates candidates; + candidates.build(*bodies, poses_t0, poses_t1, inflation_radius); + CHECK(candidates.size() > 0); + + // Completeness: all sampled close pairs are found. The rod tip sweeps + // quickly, so it is only near the cube for a short time window — sample + // finely enough to catch it. + sampled_close_pairs( + *bodies, poses_t0, poses_t1, inflation_radius, ee_pairs, fv_pairs, + /*n_samples=*/1001); + REQUIRE((ee_pairs.size() > 0 || fv_pairs.size() > 0)); + + std::set> found_ee, found_fv; + for (const EdgeEdgeCandidate& c : candidates.ee_candidates) { + found_ee.emplace( + std::min(c.edge0_id, c.edge1_id), std::max(c.edge0_id, c.edge1_id)); + } + for (const FaceVertexCandidate& c : candidates.fv_candidates) { + found_fv.emplace(c.face_id, c.vertex_id); + } + for (const auto& p : ee_pairs) { + CHECK(found_ee.count(p) == 1); + } + for (const auto& p : fv_pairs) { + CHECK(found_fv.count(p) == 1); + } +} + +TEST_CASE("Rigid narrow phase catches tunneling", "[rigid][candidates][ccd]") +{ + std::vector poses_t0, poses_t1; + auto bodies = make_spinning_rod_scene(poses_t0, poses_t1); + + RigidCandidates candidates; + candidates.build(*bodies, poses_t0, poses_t1, /*inflation_radius=*/0.01); + REQUIRE(candidates.size() > 0); + + const double first_contact = + sampled_first_contact(*bodies, poses_t0, poses_t1); + REQUIRE(first_contact < 1.0); + + // Linearized CCD misses the collision entirely: the endpoint positions of + // the rod hug its initial axis, so lerped vertex trajectories never + // approach the cube. + const double linear_alpha = + candidates.Candidates::compute_collision_free_stepsize( + *bodies, bodies->vertices(poses_t0), bodies->vertices(poses_t1)); + CHECK(linear_alpha == 1.0); + + // Rigid CCD stops before contact and near the true time of impact. + // (conservative_rescaling controls how early the CCD conservatively + // declares impact; 0.9 matches the toolkit's nonlinear CCD tests.) + NonlinearCCD ccd; + ccd.conservative_rescaling = 0.9; + const double alpha = candidates.compute_collision_free_stepsize( + *bodies, poses_t0, poses_t1, /*min_distance=*/0.0, ccd); + CHECK(alpha < 1.0); + CHECK(alpha <= first_contact); + CHECK(alpha == Catch::Approx(first_contact).margin(0.1)); + CHECK(!candidates.is_step_collision_free( + *bodies, poses_t0, poses_t1, /*min_distance=*/0.0, ccd)); + + // The step to the collision-free fraction must itself be collision free. + const std::vector poses_alpha = lerp_poses(poses_t0, poses_t1, alpha); + CHECK(candidates.is_step_collision_free( + *bodies, poses_t0, poses_alpha, /*min_distance=*/0.0, ccd)); +} + +TEST_CASE("Rigid plane-vertex candidates and CCD", "[rigid][candidates][ccd]") +{ + // A rod spinning half a revolution above a ground plane: the tip sweeps + // below the plane mid-rotation even though the endpoint poses are clear. + Eigen::MatrixXd V_rod; + Eigen::MatrixXi E_rod, F_rod; + make_rod(V_rod, E_rod, F_rod); + + std::vector initial_poses(1, Pose::identity(3)); + initial_poses[0].position = Eigen::Vector3d(0, 1.0, 0); + + auto bodies = RigidBodies::build_from_meshes( + { V_rod }, { E_rod }, { F_rod }, { 1000.0 }, initial_poses); + bodies->planes.emplace_back(Eigen::Vector3d(0, 1, 0), 0); // ground y = 0 + + std::vector poses_t0 = initial_poses; + std::vector poses_t1 = initial_poses; + poses_t1[0].rotation = compose_rotation( + Eigen::AngleAxisd(ipc::PI, Eigen::Vector3d::UnitZ()).toRotationMatrix(), + poses_t0[0]); + + // Sampled ground truth: first time any vertex dips below the plane + double first_contact = 1.0; + for (int s = 0; s <= 10'000; s++) { + const double t = s / 10'000.0; + const Eigen::MatrixXd V = + bodies->vertices(lerp_poses(poses_t0, poses_t1, t)); + if ((V.col(1).array() <= 0.0).any()) { + first_contact = t; + break; + } + } + REQUIRE(first_contact < 1.0); + + // Endpoints are clear of the plane + REQUIRE((bodies->vertices(poses_t0).col(1).array() > 0.0).all()); + REQUIRE((bodies->vertices(poses_t1).col(1).array() > 0.0).all()); + + RigidCandidates candidates; + candidates.build(*bodies, poses_t0, poses_t1, /*inflation_radius=*/0.01); + REQUIRE(candidates.size() > 0); + + NonlinearCCD ccd; + ccd.conservative_rescaling = 0.9; + const double alpha = candidates.compute_collision_free_stepsize( + *bodies, poses_t0, poses_t1, /*min_distance=*/0.0, ccd); + CHECK(alpha < 1.0); + CHECK(alpha <= first_contact); + CHECK(alpha == Catch::Approx(first_contact).margin(0.1)); + + // The full step sweeps the rod through the ground, while the clamped step + // leaves every vertex above it. + CHECK(!candidates.is_step_collision_free( + *bodies, poses_t0, poses_t1, /*min_distance=*/0.0, ccd)); + const std::vector poses_alpha = lerp_poses(poses_t0, poses_t1, alpha); + CHECK((bodies->vertices(poses_alpha).col(1).array() > 0.0).all()); +} + +TEST_CASE( + "Rigid broad phase with a reused broad-phase object", "[rigid][candidates]") +{ + // The simulator reuses a single broad-phase object for both the standard + // vertex-level candidate builds (barrier potential) and the rigid CCD + // build. Candidates::build sets broad_phase->can_vertices_collide to the + // mesh's *vertex*-level filter and leaves it set. The rigid body-level + // prefilter stores bodies as broad-phase "vertices", so applying the stale + // vertex filter to *body* indices silently drops body pairs (low body + // indices map to same-body vertex pairs, which the default filter + // rejects). Regression test: this exact scenario caused a bunny to tunnel + // through a bowl while every fresh-broad-phase test passed. + Eigen::MatrixXd V_cube; + Eigen::MatrixXi E_cube, F_cube; + tests::load_mesh("cube.ply", V_cube, E_cube, F_cube); + + std::vector initial_poses(2, Pose::identity(3)); + initial_poses[1].position = Eigen::Vector3d(1.2, 0, 0); + + auto bodies = RigidBodies::build_from_meshes( + { V_cube, V_cube }, { E_cube, E_cube }, { F_cube, F_cube }, + { 1000.0, 1000.0 }, initial_poses); + + std::vector poses_t0 = initial_poses; + std::vector poses_t1 = initial_poses; + poses_t1[1].position = Eigen::Vector3d(1.02, 0, 0); + + const double inflation_radius = 0.05; + + LBVH broad_phase; + + // Poison the filter exactly the way the simulator does: a standard + // vertex-level build leaves mesh.can_collide set on the broad phase. + Candidates linear_candidates; + linear_candidates.build( + *bodies, bodies->vertices(poses_t0), bodies->vertices(poses_t1), + inflation_radius, &broad_phase); + // The stale filter rejects vertices 0 and 1 (both in body 0), which are + // the ids the body-level prefilter would feed it for the body pair (0, 1). + REQUIRE(!broad_phase.can_vertices_collide(0, 1)); + + RigidCandidates candidates; + candidates.build( + *bodies, poses_t0, poses_t1, inflation_radius, &broad_phase); + CHECK(candidates.size() > 0); // With the bug: 0 body-body candidates + + // The caller's vertex-level filter must be restored afterwards. + CHECK(!broad_phase.can_vertices_collide(0, 1)); // same body + CHECK(broad_phase.can_vertices_collide(0, V_cube.rows())); // cross body +} + +TEST_CASE( + "Rigid broad phase honors vertex-level collision filter", + "[rigid][candidates]") +{ + // Two overlapping cubes whose collisions are disabled by a vertex-patch + // filter (the jointed-bodies use case): the rigid broad phase must apply + // the collision mesh's can_collide in its two-tree queries, otherwise CCD + // reports a false impact at t=0 and blocks all motion. + Eigen::MatrixXd V_cube; + Eigen::MatrixXi E_cube, F_cube; + tests::load_mesh("cube.ply", V_cube, E_cube, F_cube); + + std::vector initial_poses(2, Pose::identity(3)); + initial_poses[1].position = Eigen::Vector3d(0.75, 0, 0); // Overlapping + + auto bodies = RigidBodies::build_from_meshes( + { V_cube, V_cube }, { E_cube, E_cube }, { F_cube, F_cube }, + { 1000.0, 1000.0 }, initial_poses); + + std::vector poses_t0 = initial_poses; + std::vector poses_t1 = initial_poses; + poses_t1[1].position = Eigen::Vector3d(0.7, 0, 0); + + RigidCandidates candidates; + candidates.build(*bodies, poses_t0, poses_t1, /*inflation_radius=*/0.01); + CHECK(candidates.size() > 0); // Sanity: unfiltered pairs are found + + // Same patch for every vertex => every pair is blocked. + bodies->can_collide = make_vertex_patches_filter( + Eigen::VectorXi::Zero(bodies->num_vertices())); + + candidates.build(*bodies, poses_t0, poses_t1, /*inflation_radius=*/0.01); + CHECK(candidates.empty()); +} + +TEST_CASE( + "Rigid broad phase superset of linearized broad phase", + "[rigid][candidates]") +{ + // For small rotations, the rigid broad phase must find at least the + // candidates found by the linearized (vertex-trajectory) broad phase. + Eigen::MatrixXd V_cube; + Eigen::MatrixXi E_cube, F_cube; + tests::load_mesh("cube.ply", V_cube, E_cube, F_cube); + + std::vector initial_poses(2, Pose::identity(3)); + initial_poses[1].position = Eigen::Vector3d(1.2, 0, 0); + + auto bodies = RigidBodies::build_from_meshes( + { V_cube, V_cube }, { E_cube, E_cube }, { F_cube, F_cube }, + { 1000.0, 1000.0 }, initial_poses); + + std::vector poses_t0 = initial_poses; + std::vector poses_t1 = initial_poses; + poses_t1[1].position = Eigen::Vector3d(1.05, 0.05, 0); + poses_t1[1].rotation = compose_rotation( + Eigen::AngleAxisd(0.01, Eigen::Vector3d::UnitZ()).toRotationMatrix(), + poses_t0[1]); + + const double inflation_radius = 0.05; + + RigidCandidates rigid_candidates; + rigid_candidates.build(*bodies, poses_t0, poses_t1, inflation_radius); + + Candidates linear_candidates; + linear_candidates.build( + *bodies, bodies->vertices(poses_t0), bodies->vertices(poses_t1), + inflation_radius); + + std::set> found_ee, found_fv; + for (const EdgeEdgeCandidate& c : rigid_candidates.ee_candidates) { + found_ee.emplace( + std::min(c.edge0_id, c.edge1_id), std::max(c.edge0_id, c.edge1_id)); + } + for (const FaceVertexCandidate& c : rigid_candidates.fv_candidates) { + found_fv.emplace(c.face_id, c.vertex_id); + } + + const Eigen::MatrixXi& E = bodies->edges(); + const Eigen::MatrixXi& F = bodies->faces(); + + int num_interbody = 0; + for (const EdgeEdgeCandidate& c : linear_candidates.ee_candidates) { + if (bodies->vertex_to_body(E(c.edge0_id, 0)) + == bodies->vertex_to_body(E(c.edge1_id, 0))) { + continue; // Rigid candidates exclude intra-body pairs + } + num_interbody++; + CHECK( + found_ee.count( + { std::min(c.edge0_id, c.edge1_id), + std::max(c.edge0_id, c.edge1_id) }) + == 1); + } + for (const FaceVertexCandidate& c : linear_candidates.fv_candidates) { + if (bodies->vertex_to_body(F(c.face_id, 0)) + == bodies->vertex_to_body(c.vertex_id)) { + continue; + } + num_interbody++; + CHECK(found_fv.count({ c.face_id, c.vertex_id }) == 1); + } + CHECK(num_interbody > 0); +} diff --git a/tests/src/tests/dynamics/rigid/test_rigid_contact_hessian.cpp b/tests/src/tests/dynamics/rigid/test_rigid_contact_hessian.cpp new file mode 100644 index 000000000..93bfa7e6e --- /dev/null +++ b/tests/src/tests/dynamics/rigid/test_rigid_contact_hessian.cpp @@ -0,0 +1,93 @@ +#include + +#include + +#include +#include +#include +#include +#include + +#include + +using namespace ipc; +using namespace ipc::rigid; + +namespace { + +/// A 3×3 grid of isolated points in the y = 0 plane plus one point above it so +/// the inertia is nonsingular (all vertices are codimensional). +Eigen::MatrixXd point_grid() +{ + Eigen::MatrixXd V(10, 3); + int r = 0; + for (int i = -1; i <= 1; ++i) { + for (int j = -1; j <= 1; ++j) { + V.row(r++) = Eigen::RowVector3d(0.5 * i, 0, 0.5 * j); + } + } + V.row(r) = Eigen::RowVector3d(0, 0.3, 0); + return V; +} + +} // namespace + +TEST_CASE( + "Per-contact rigid Hessian is the exact Hessian without projection", + "[rigid][hessian]") +{ + constexpr double dhat = 1e-2; + + // Two identical point grids, one hovering just above the other and + // slightly rotated, so many point-point pairs sit inside dhat. + std::vector poses(2, Pose::identity(3)); + const Eigen::MatrixXd V_grid = point_grid(); + const Eigen::MatrixXi no_elements(0, 2), no_faces(0, 3); + auto bodies = RigidBodies::build_from_meshes( + { V_grid, V_grid }, { no_elements, no_elements }, + { no_faces, no_faces }, { 1000.0, 1000.0 }, poses); + + // build_from_meshes moves each body into its principal frame and stores + // the compensating transform in the pose, so we perturb that pose rather + // than overwrite it. + poses[0] = poses[1]; + poses[0].position += Eigen::Vector3d(0.001, 0.004, 0); + poses[0].rotation += Eigen::Vector3d(2e-3, -1e-3, 3e-3); + + const BarrierPotential barrier(dhat, /*stiffness=*/1.0); + // Use the rigid body-pair broad phase, as the simulator does: it is what + // produces the point-point candidates between codimensional vertices. + RigidCandidates candidates; + candidates.build(*bodies, poses, /*inflation_radius=*/dhat); + NormalCollisions collisions; + collisions.build(candidates, *bodies, bodies->vertices(poses), dhat); + REQUIRE(collisions.size() > 0); + + // Energy and gradient in rigid DOFs with the collision set held fixed. + const auto energy = [&](const Eigen::VectorXd& x) { + return barrier( + collisions, *bodies, bodies->vertices(Pose::to_poses(x, 3))); + }; + const auto gradient = [&](const Eigen::VectorXd& x) -> Eigen::VectorXd { + const std::vector p = Pose::to_poses(x, 3); + return bodies->to_rigid_dof( + p, barrier.gradient(collisions, *bodies, bodies->vertices(p))); + }; + + const Eigen::VectorXd x = Pose::from_poses(poses); + + Eigen::VectorXd fd_g; + fd::finite_gradient(x, energy, fd_g); + CHECK(fd::compare_gradient(gradient(x), fd_g)); + + const Eigen::MatrixXd H = per_contact_rigid_hessian( + *bodies, poses, collisions, barrier, bodies->vertices(poses), + /*gn_scale=*/1.0, /*geom_scale=*/1.0, PSDProjectionMethod::NONE); + Eigen::MatrixXd fd_H; + fd::finite_jacobian(x, gradient, fd_H); + + CHECK(fd::compare_hessian(H, fd_H, 1e-3)); + if (!fd::compare_hessian(H, fd_H, 1e-3)) { + std::cout << "analytic:\n" << H << "\n\nnumerical:\n" << fd_H << "\n"; + } +} diff --git a/tests/src/tests/dynamics/rigid/test_rigid_trajectory.cpp b/tests/src/tests/dynamics/rigid/test_rigid_trajectory.cpp new file mode 100644 index 000000000..283eca357 --- /dev/null +++ b/tests/src/tests/dynamics/rigid/test_rigid_trajectory.cpp @@ -0,0 +1,346 @@ +#include +#include + +#include +#include +#ifdef IPC_TOOLKIT_WITH_FILIB +#include +#include +#include +#endif + +using namespace ipc; +using namespace ipc::rigid; + +namespace { + +/// Densely sample max_t ‖x(t) − lerp(x(t0), x(t1))(t)‖ over [t0, t1]. +double sampled_max_deviation( + const RigidTrajectory& traj, + const double t0, + const double t1, + const int n = 1000) +{ + const VectorMax3d x_t0 = traj(t0); + const VectorMax3d x_t1 = traj(t1); + double max_d = 0; + for (int i = 0; i <= n; i++) { + const double s = i / double(n); + const double t = t0 + s * (t1 - t0); + max_d = std::max(max_d, (traj(t) - (x_t0 + s * (x_t1 - x_t0))).norm()); + } + return max_d; +} + +Pose random_pose(const int dim, const double max_angle) +{ + VectorMax3d position = VectorMax3d::Random(dim); + VectorMax3d rotation; + if (dim == 2) { + rotation = max_angle * VectorMax3d::Random(1); + } else { + rotation = VectorMax3d::Random(3).normalized() + * (max_angle * 0.5 * (Eigen::Vector2d::Random()(0) + 1)); + } + return Pose(position, rotation); +} + +} // namespace + +TEST_CASE("Rigid trajectory position", "[rigid][trajectory]") +{ + const int dim = GENERATE(2, 3); + for (int trial = 0; trial < 10; trial++) { + const Pose pose_t0 = random_pose(dim, 2 * ipc::PI); + const Pose pose_t1 = random_pose(dim, 2 * ipc::PI); + const VectorMax3d rest_position = VectorMax3d::Random(dim); + + const RigidTrajectory traj(rest_position, pose_t0, pose_t1); + + for (int i = 0; i <= 100; i++) { + const double t = i / 100.0; + const Pose pose_t( + (1 - t) * pose_t0.position + t * pose_t1.position, + (1 - t) * pose_t0.rotation + t * pose_t1.rotation); + const Eigen::MatrixXd expected = + pose_t.transform_vertices(rest_position.transpose()); + CHECK( + (traj(t) - expected.row(0).transpose()).norm() + == Catch::Approx(0).margin(1e-14)); + } + + // Endpoints are exact + CHECK( + (traj(0) + - (pose_t0.rotation_matrix() * rest_position + pose_t0.position)) + .norm() + == Catch::Approx(0).margin(1e-15)); + CHECK( + (traj(1) + - (pose_t1.rotation_matrix() * rest_position + pose_t1.position)) + .norm() + == Catch::Approx(0).margin(1e-15)); + } +} + +TEST_CASE( + "Rigid trajectory max_distance_from_linear is conservative", + "[rigid][trajectory]") +{ + const int dim = GENERATE(2, 3); + double max_angle; + SECTION("small rotation") { max_angle = 0.1; } + SECTION("moderate rotation") { max_angle = ipc::PI / 2; } + SECTION("large rotation") { max_angle = 2 * ipc::PI; } + + for (int trial = 0; trial < 10; trial++) { + const Pose pose_t0 = random_pose(dim, max_angle); + const Pose pose_t1 = random_pose(dim, max_angle); + const VectorMax3d rest_position = VectorMax3d::Random(dim); + + const RigidTrajectory traj(rest_position, pose_t0, pose_t1); + + // Check on the full interval and random sub-intervals + std::vector> intervals = { { 0, 1 } }; + for (int i = 0; i < 4; i++) { + const Eigen::Vector2d r = + (Eigen::Vector2d::Random() + Eigen::Vector2d::Ones()) / 2; + intervals.emplace_back(std::min(r(0), r(1)), std::max(r(0), r(1))); + } + + for (const auto& [t0, t1] : intervals) { + if (t1 - t0 < 1e-8) { + continue; + } + const double bound = traj.max_distance_from_linear(t0, t1); + const double sampled = sampled_max_deviation(traj, t0, t1); + CHECK(sampled <= bound * (1 + 1e-10) + 1e-12); + } + } +} + +TEST_CASE( + "Rigid trajectory pure translation has zero deviation", + "[rigid][trajectory]") +{ + const int dim = GENERATE(2, 3); + const VectorMax3d rotation = + dim == 2 ? VectorMax3d::Random(1) : VectorMax3d::Random(3); + const Pose pose_t0(VectorMax3d::Random(dim), rotation); + const Pose pose_t1(VectorMax3d::Random(dim), rotation); // same rotation + + const RigidTrajectory traj(VectorMax3d::Random(dim), pose_t0, pose_t1); + + CHECK(traj.max_distance_from_linear(0, 1) == 0.0); + CHECK(sampled_max_deviation(traj, 0, 1) == Catch::Approx(0).margin(1e-14)); +} + +TEST_CASE("Rigid trajectory bound tightness", "[rigid][trajectory]") +{ + // Fixed-axis rotation with the point perpendicular to the axis: the true + // deviation is the sagitta r(1 − cos(δ/2)); the bound is r δ²/8. Check the + // bound is not grossly over-conservative in this regime. + const double delta = GENERATE(0.1, 0.5, 1.0, ipc::PI / 2); + + const Pose pose_t0(Eigen::Vector3d::Zero(), Eigen::Vector3d::Zero()); + const Pose pose_t1(Eigen::Vector3d::Zero(), Eigen::Vector3d(0, 0, delta)); + const RigidTrajectory traj(Eigen::Vector3d(1, 0, 0), pose_t0, pose_t1); + + const double bound = traj.max_distance_from_linear(0, 1); + const double sampled = sampled_max_deviation(traj, 0, 1); + + CHECK(sampled <= bound); + CHECK(bound <= 2 * sampled); // tightness (ratio → 1 as δ → 0) +} + +// ============================================================================ +// Interval-arithmetic cross-check (tests only; production code is IA-free) +#ifdef IPC_TOOLKIT_WITH_FILIB + +namespace { + +class IntervalRigidTrajectory : public RigidTrajectory, + public IntervalNonlinearTrajectory { +public: + IntervalRigidTrajectory( + Eigen::ConstRef rest_position, + const Pose& pose_t0, + const Pose& pose_t1) + : RigidTrajectory(rest_position, pose_t0, pose_t1) + , m_rest_position(rest_position) + , m_pose_t0(pose_t0) + , m_pose_t1(pose_t1) + { + } + + using RigidTrajectory::operator(); + + VectorMax3I operator()(const filib::Interval& t) const override + { + const filib::Interval one_minus_t = filib::Interval(1.0) - t; + + const VectorMax3I p = + one_minus_t * m_pose_t0.position.template cast() + + t * m_pose_t1.position.template cast(); + const VectorMax3I theta = + one_minus_t * m_pose_t0.rotation.template cast() + + t * m_pose_t1.rotation.template cast(); + + const VectorMax3I x_rest = + m_rest_position.template cast(); + + if (theta.size() == 1) { + // 2D rotation + MatrixMax3I R(2, 2); + R(0, 0) = cos(theta(0)); + R(0, 1) = -sin(theta(0)); + R(1, 0) = sin(theta(0)); + R(1, 1) = cos(theta(0)); + return R * x_rest + p; + } + + // 3D: Rodrigues formula R = I + sinc(‖θ‖) K + ½ sinc²(‖θ‖/2) K² + const filib::Interval angle = norm(theta); + Matrix3I K; + K << filib::Interval(0.0), -theta(2), theta(1), // + theta(2), filib::Interval(0.0), -theta(0), // + -theta(1), theta(0), filib::Interval(0.0); + const filib::Interval s = sinc(angle); + const filib::Interval s_half = sinc(angle * filib::Interval(0.5)); + Matrix3I R = + s * K + (filib::Interval(0.5) * s_half * s_half) * (K * K).eval(); + R.diagonal().array() += filib::Interval(1.0); + return R * x_rest + p; + } + + // Expose the interval-based estimate for comparison + double interval_max_distance_from_linear(double t0, double t1) const + { + return IntervalNonlinearTrajectory::max_distance_from_linear(t0, t1); + } + + double + max_distance_from_linear(const double t0, const double t1) const override + { + return RigidTrajectory::max_distance_from_linear(t0, t1); + } + +private: + VectorMax3d m_rest_position; + Pose m_pose_t0, m_pose_t1; +}; + +} // namespace + +TEST_CASE("Rigid trajectory interval cross-check", "[rigid][trajectory][filib]") +{ + const int dim = GENERATE(2, 3); + const double max_angle = GENERATE(0.1, 1.0, ipc::PI); + + for (int trial = 0; trial < 5; trial++) { + const Pose pose_t0 = random_pose(dim, max_angle); + const Pose pose_t1 = random_pose(dim, max_angle); + const VectorMax3d rest_position = VectorMax3d::Random(dim); + + const IntervalRigidTrajectory traj(rest_position, pose_t0, pose_t1); + + // Interval evaluation contains the true position + for (int i = 0; i < 10; i++) { + const double t = i / 9.0; + const VectorMax3d x = traj(t); + const VectorMax3I x_I = traj(filib::Interval(t)); + for (int d = 0; d < dim; d++) { + CHECK(x(d) >= x_I(d).INF - 1e-12); + CHECK(x(d) <= x_I(d).SUP + 1e-12); + } + } + + // Both the analytic bound and the interval estimate dominate the + // sampled truth + const double sampled = sampled_max_deviation(traj, 0, 1); + const double analytic = traj.max_distance_from_linear(0, 1); + const double interval = traj.interval_max_distance_from_linear(0, 1); + CHECK(sampled <= analytic * (1 + 1e-10) + 1e-12); + CHECK(sampled <= interval * (1 + 1e-10) + 1e-12); + } +} + +#endif + +// ============================================================================ +// CCD with rigid trajectories (mirrors test_nonlinear_ccd.cpp scenarios) + +TEST_CASE("Rigid trajectory point-edge CCD", "[rigid][trajectory][ccd]") +{ + // Static point at (0, 0.5); edge from (-1,0) to (1,0) rotating by π. + const Pose identity_2d = Pose::identity(2); + const Pose rotated_2d( + Eigen::Vector2d::Zero(), VectorMax3d::Constant(1, ipc::PI)); + + const RigidTrajectory p(Eigen::Vector2d(0, 0.5), identity_2d, identity_2d); + const RigidTrajectory e0(Eigen::Vector2d(-1, 0), identity_2d, rotated_2d); + const RigidTrajectory e1(Eigen::Vector2d(1, 0), identity_2d, rotated_2d); + + NonlinearCCD ccd; + ccd.conservative_rescaling = 0.99; + + double toi; + const bool collision = ccd.point_edge_ccd(p, e0, e1, toi); + + CHECK(collision); + CHECK((0.49 <= toi && toi <= 0.5)); // conservative estimate +} + +TEST_CASE("Rigid trajectory edge-edge CCD", "[rigid][trajectory][ccd]") +{ + // Edge from (-1,0,0) to (1,0,0) rotating 2π about z; static edge at + // y = 0.5. First contact at 30° of rotation. + const Pose identity_3d = Pose::identity(3); + const Pose rotated_3d( + Eigen::Vector3d::Zero(), Eigen::Vector3d(0, 0, 2 * ipc::PI)); + + const RigidTrajectory ea0( + Eigen::Vector3d(-1, 0, 0), identity_3d, rotated_3d); + const RigidTrajectory ea1( + Eigen::Vector3d(1, 0, 0), identity_3d, rotated_3d); + const RigidTrajectory eb0( + Eigen::Vector3d(-1, 0.5, 0), identity_3d, identity_3d); + const RigidTrajectory eb1( + Eigen::Vector3d(1, 0.5, 0), identity_3d, identity_3d); + + NonlinearCCD ccd; + ccd.conservative_rescaling = 0.99; + + double toi; + const bool collision = ccd.edge_edge_ccd(ea0, ea1, eb0, eb1, toi); + + CHECK(collision); + CHECK(toi <= 30 / 360.0); + CHECK(toi == Catch::Approx(30 / 360.0).margin(1e-2)); +} + +TEST_CASE("Rigid trajectory point-triangle CCD", "[rigid][trajectory][ccd]") +{ + const double x = GENERATE(-0.1, 0.0, 0.1); + + const Pose identity_3d = Pose::identity(3); + const Pose rotated_3d( + Eigen::Vector3d::Zero(), Eigen::Vector3d(0, 0, ipc::PI)); + + const RigidTrajectory t0(Eigen::Vector3d(1, 0, 0), identity_3d, rotated_3d); + const RigidTrajectory t1(Eigen::Vector3d(x, 0, 1), identity_3d, rotated_3d); + const RigidTrajectory t2( + Eigen::Vector3d(x, 0, -1), identity_3d, rotated_3d); + const RigidTrajectory p( + Eigen::Vector3d(0, 0.5, 0), identity_3d, identity_3d); + + NonlinearCCD ccd; + ccd.conservative_rescaling = 0.99; + + double toi; + const bool collision = ccd.point_triangle_ccd(p, t0, t1, t2, toi); + + CHECK(collision); + CHECK(toi <= 0.5); + CHECK(toi == Catch::Approx(0.5).margin(1e-2)); +} diff --git a/tests/src/tests/dynamics/test_bdf.cpp b/tests/src/tests/dynamics/test_bdf.cpp new file mode 100644 index 000000000..0bf3bfd38 --- /dev/null +++ b/tests/src/tests/dynamics/test_bdf.cpp @@ -0,0 +1,142 @@ +#include +#include + +#include + +using namespace ipc; +using namespace ipc::dynamics; + +TEST_CASE("BDF coefficients", "[dynamics][bdf]") +{ + // The α coefficients of each order sum to 1 (consistency). + for (int order = 1; order <= 6; ++order) { + const std::vector& alpha = BDF::alphas(order - 1); + REQUIRE(alpha.size() == order); + double sum = 0; + for (const double a : alpha) { + sum += a; + } + CHECK(sum == Catch::Approx(1.0)); + CHECK(BDF::betas(order - 1) > 0.0); + } +} + +TEST_CASE("BDF order ramps up", "[dynamics][bdf]") +{ + const int target = GENERATE(1, 2, 3, 4, 5, 6); + + BDF bdf(target); + bdf.set_dt(0.01); + const int n = 3; // DOFs (single "body" worth is irrelevant here) + bdf.init( + Eigen::VectorXd::Zero(n), Eigen::VectorXd::Zero(n), + Eigen::VectorXd::Zero(n), /*num_bodies=*/1); + + CHECK(bdf.target_order() == target); + + // The effective order starts at 1 and increases by one per accepted step + // until it reaches the target (BDF startup). + CHECK(bdf.order() == 1); + for (int step = 1; step <= target + 2; ++step) { + bdf.update(Eigen::VectorXd::Zero(n)); + CHECK(bdf.order() == std::min(target, step + 1)); + } +} + +TEST_CASE("BDF is exact for constant velocity", "[dynamics][bdf]") +{ + // BDF-n reproduces polynomials of degree ≤ n exactly. For a + // constant-velocity trajectory x(t) = x0 + v0 t, once enough history has + // accumulated compute_velocity() must recover v0 and predicted_positions() + // must extrapolate exactly — for any order. + const int target = GENERATE(1, 2, 3, 4); + const int n = 5; + const double dt = 0.01; + + const Eigen::VectorXd x0 = Eigen::VectorXd::Random(n); + const Eigen::VectorXd v0 = Eigen::VectorXd::Random(n); + + BDF bdf(target); + bdf.set_dt(dt); + bdf.init(x0, v0, Eigen::VectorXd::Zero(n), /*num_bodies=*/1); + + Eigen::VectorXd x = x0; + // Warm up the history along the exact linear trajectory. + for (int step = 1; step <= target; ++step) { + x = x0 + step * dt * v0; + bdf.update(x); + } + + // Now the effective order equals the target; the scheme is exact. + REQUIRE(bdf.order() == target); + + const Eigen::VectorXd x_next = x + dt * v0; + CHECK((bdf.compute_velocity(x_next) - v0).norm() < 1e-10); + CHECK((bdf.predicted_positions() - x_next).norm() < 1e-10); + + // acceleration_scaling() = (β Δt)² + const double beta_dt = BDF::betas(target - 1) * dt; + CHECK(bdf.acceleration_scaling() == Catch::Approx(beta_dt * beta_dt)); + CHECK(bdf.velocity_scaling() == Catch::Approx(beta_dt)); +} + +TEST_CASE( + "BDF velocity derivatives match the velocity formula", "[dynamics][bdf]") +{ + const int order = GENERATE(1, 2, 3, 4); + CAPTURE(order); + const int n = 2; + const double dt = 0.01, delta = 1e-3; + + // Two integrators that share a history except for one past state, which + // we perturb by δ. The velocity formula is linear, so the change in the + // computed velocity is exactly dvdx(prev_ti)·δ. + std::vector history(order); + for (auto& x : history) { + x = Eigen::VectorXd::Random(n); + } + const auto make_bdf = [&](const int perturbed_step) { + BDF bdf(order); + bdf.set_dt(dt); + bdf.init( + Eigen::VectorXd::Zero(n), Eigen::VectorXd::Zero(n), + Eigen::VectorXd::Zero(n), /*num_bodies=*/1); + for (int step = 0; step < order; ++step) { + bdf.update( + step == perturbed_step + ? Eigen::VectorXd(history[step].array() + delta) + : history[step]); + } + return bdf; + }; + + const BDF reference = make_bdf(/*perturbed_step=*/-1); + REQUIRE(reference.order() == order); + const Eigen::VectorXd x = Eigen::VectorXd::Random(n); + + // prev_ti = 0 is the current state x. + const Eigen::VectorXd dv_current = + reference.compute_velocity(x.array() + delta) + - reference.compute_velocity(x); + CHECK(dv_current.isApproxToConstant(reference.dvdx(0) * delta, 1e-8)); + + // prev_ti = i ≥ 1 is the i-th most recent past state x_prev(i - 1). + for (int i = 1; i <= order; ++i) { + const BDF perturbed = make_bdf(/*perturbed_step=*/order - i); + const Eigen::VectorXd dv = + perturbed.compute_velocity(x) - reference.compute_velocity(x); + CAPTURE(i, dv.transpose(), reference.dvdx(i) * delta); + CHECK(dv.isApproxToConstant(reference.dvdx(i) * delta, 1e-8)); + } + + // States older than the formula's history have no influence. + CHECK(reference.dvdx(order + 1) == 0); +} + +TEST_CASE("BDF invalid order throws", "[dynamics][bdf]") +{ + CHECK_THROWS(BDF(0)); + CHECK_THROWS(BDF(7)); + CHECK_NOTHROW(BDF(1)); + CHECK_NOTHROW(BDF(6)); +} diff --git a/tests/src/tests/dynamics/test_body_potentials.cpp b/tests/src/tests/dynamics/test_body_potentials.cpp new file mode 100644 index 000000000..f0338bd2c --- /dev/null +++ b/tests/src/tests/dynamics/test_body_potentials.cpp @@ -0,0 +1,173 @@ +#include + +#include + +#include + +#include +#include +#include +#include +#include + +#include +#include + +using namespace ipc; +using namespace ipc::dynamics; + +namespace { +/// Embed a rigid state (6 DOF) into affine DOFs (12 DOF, column-major vec(A)). +Eigen::VectorXd rigid_to_affine_dof(Eigen::ConstRef x_rigid) +{ + const size_t num_bodies = x_rigid.size() / 6; + Eigen::VectorXd x(12 * num_bodies); + for (size_t i = 0; i < num_bodies; ++i) { + x.segment<3>(12 * i) = x_rigid.segment<3>(6 * i); + x.segment<9>(12 * i + 3) = + rigid::rotation_vector_to_matrix(x_rigid.segment<3>(6 * i + 3)) + .reshaped(); + } + return x; +} +} // namespace + +TEST_CASE( + "BodyPotentials gradient and hessian", + "[affine][body_potentials][gradient][hessian]") +{ + const bool rigid_cov = GENERATE(true, false); + CAPTURE(rigid_cov); + + Eigen::MatrixXd V; + Eigen::MatrixXi E, F; + REQUIRE(tests::load_mesh("cube.ply", V, E, F)); + // Anisotropic so the inertia tensor has distinct entries. + V.col(0) *= 2.0; + V.col(1) *= 0.5; + + std::vector initial_poses(2, rigid::Pose::identity(3)); + initial_poses[1].position = Eigen::Vector3d(3, 0, 0); + auto bodies = rigid::RigidBodies::build_from_meshes( + { V, V }, { E, E }, { F, F }, { 1000.0, 500.0 }, initial_poses); + + // Shared integrator with an affine-shaped state and nonzero velocity. + const Eigen::VectorXd x0 = rigid_to_affine_dof(Eigen::VectorXd::Random(12)); + const Eigen::VectorXd v0 = 0.1 * Eigen::VectorXd::Random(24); + const Eigen::VectorXd a0 = Eigen::VectorXd::Zero(24); + auto integrator = std::make_shared(/*order=*/1); + integrator->set_dt(0.01); + integrator->init(x0, v0, a0, /*num_bodies=*/2); + + std::shared_ptr to_affine; + if (rigid_cov) { + to_affine = std::make_shared(3, 2); + } else { + to_affine = std::make_shared(3, 2); + } + + BodyPotentials potential( + *bodies, integrator, to_affine, /*orthogonality_stiffness=*/1e3); + potential.set_gravity(Eigen::Vector3d(0, -9.81, 0)); + potential.update(*bodies); + + const double s = 1e-2; // arbitrary positive scaling + + // Evaluate at a perturbed state (reduced DOFs for rigid, affine for ABD). + Eigen::VectorXd x; + if (rigid_cov) { + x = 0.3 * Eigen::VectorXd::Random(12); // [p; θ] per body + } else { + x = rigid_to_affine_dof(0.3 * Eigen::VectorXd::Random(12)); + } + + const auto energy = [&](const Eigen::VectorXd& x_) { + return potential.energy(*bodies, x_, s); + }; + const auto gradient = [&](const Eigen::VectorXd& x_) { + return potential.gradient(*bodies, x_, s); + }; + + // --- Gradient ----------------------------------------------------------- + const Eigen::VectorXd g = gradient(x); + Eigen::VectorXd fd_g; + fd::finite_gradient(x, energy, fd_g); + CHECK(fd::compare_gradient(g, fd_g)); + if (!fd::compare_gradient(g, fd_g)) { + std::cout << "analytic:\n" << g.transpose() << "\n"; + std::cout << "numerical:\n" << fd_g.transpose() << "\n"; + } + + // --- Hessian (exact, no PSD projection) --------------------------------- + const Eigen::SparseMatrix H = + potential.hessian(*bodies, x, s, PSDProjectionMethod::NONE); + Eigen::MatrixXd fd_H; + fd::finite_jacobian(x, gradient, fd_H); + CHECK(fd::compare_hessian(Eigen::MatrixXd(H), fd_H, 1e-3)); + if (!fd::compare_hessian(Eigen::MatrixXd(H), fd_H, 1e-3)) { + std::cout << "analytic:\n" << Eigen::MatrixXd(H) << "\n\n"; + std::cout << "numerical:\n" << fd_H << "\n"; + } +} + +TEST_CASE( + "External torque applies the same world torque in any orientation", + "[affine][body_forces]") +{ + const int dim = GENERATE(2, 3); + CAPTURE(dim); + + Eigen::MatrixXd V; + Eigen::MatrixXi E, F; + if (dim == 3) { + REQUIRE(tests::load_mesh("cube.ply", V, E, F)); + } else { + V.resize(4, 2); + V << -1, -1, 1, -1, 1, 1, -1, 1; + E.resize(4, 2); + E << 0, 1, 1, 2, 2, 3, 3, 0; + } + + std::vector initial_poses(1, rigid::Pose::identity(dim)); + auto bodies = rigid::RigidBodies::build_from_meshes( + { V }, { E }, { F }, { 1000.0 }, initial_poses); + + const VectorMax3d torque = dim == 3 ? VectorMax3d(Eigen::Vector3d(2, -3, 5)) + : VectorMax3d::Constant(1, 4.0); + (*bodies)[0].set_external_force( + rigid::Pose(VectorMax3d::Zero(dim), torque)); + + // The frame dependence shows up only away from the identity, so we start + // from an arbitrary orientation. + const VectorMax3d theta0 = dim == 3 + ? VectorMax3d(Eigen::Vector3d(0.7, -2.1, 1.3)) + : VectorMax3d::Constant(1, 2.4); + const rigid::Pose pose0(VectorMax3d::Zero(dim), theta0); + const MatrixMax3d A0 = pose0.rotation_matrix(); + + affine::BodyForces forces; + forces.set_gravity(VectorMax3d::Zero(dim)); + forces.update(*bodies, { affine::Pose { VectorMax3d::Zero(dim), A0 } }); + + const auto energy = [&](const MatrixMax3d& A) { + Eigen::VectorXd y(dim + dim * dim); + y.head(dim).setZero(); + y.tail(dim * dim) = A.reshaped(); + return forces(*bodies, y); + }; + + // The generalized torque is −∂E/∂φ for a world rotation δφ applied on + // top of A₀, which must equal the applied world torque. + constexpr double EPS = 1e-6; + const int rot_ndof = dim == 3 ? 3 : 1; + for (int k = 0; k < rot_ndof; ++k) { + const VectorMax3d dphi = EPS * VectorMax3d::Unit(rot_ndof, k); + const MatrixMax3d A_plus = + rigid::Pose(VectorMax3d::Zero(dim), dphi).rotation_matrix() * A0; + const MatrixMax3d A_minus = + rigid::Pose(VectorMax3d::Zero(dim), -dphi).rotation_matrix() * A0; + const double generalized_torque = + -(energy(A_plus) - energy(A_minus)) / (2 * EPS); + CHECK(generalized_torque == Catch::Approx(torque(k)).epsilon(1e-6)); + } +} diff --git a/tests/src/tests/dynamics/test_to_affine.cpp b/tests/src/tests/dynamics/test_to_affine.cpp new file mode 100644 index 000000000..9bd130d39 --- /dev/null +++ b/tests/src/tests/dynamics/test_to_affine.cpp @@ -0,0 +1,97 @@ +#include + +#include + +#include + +#include +#include + +using namespace ipc; +using namespace ipc::dynamics; + +namespace { +std::shared_ptr +make_to_affine(const bool rigid_cov, const int dim, const size_t num_bodies) +{ + if (rigid_cov) { + return std::make_shared(dim, num_bodies); + } + return std::make_shared(dim, num_bodies); +} +} // namespace + +TEST_CASE("ToAffine chain rule", "[to_affine][gradient][hessian]") +{ + const int dim = GENERATE(2, 3); + const bool rigid_cov = GENERATE(true, false); + const size_t N = 2; + + CAPTURE(dim, rigid_cov); + + auto to_affine = make_to_affine(rigid_cov, dim, N); + const int rndof = to_affine->reduced_ndof(); + const int andof = to_affine->affine_ndof(); + + // Random reduced DOFs; entries in [-1, 1] keep rotations inside the log + // map. + const Eigen::VectorXd x = Eigen::VectorXd::Random(rndof * N); + + // A random block-diagonal SPD affine-space quadratic φ(y) = ½yᵀCy + b·y. + std::vector> C_triplets; + for (size_t i = 0; i < N; ++i) { + const Eigen::MatrixXd Mi = Eigen::MatrixXd::Random(andof, andof); + const Eigen::MatrixXd Ci = + Mi.transpose() * Mi + Eigen::MatrixXd::Identity(andof, andof); + for (int r = 0; r < andof; ++r) { + for (int c = 0; c < andof; ++c) { + C_triplets.emplace_back(i * andof + r, i * andof + c, Ci(r, c)); + } + } + } + Eigen::SparseMatrix C(andof * N, andof * N); + C.setFromTriplets(C_triplets.begin(), C_triplets.end()); + const Eigen::VectorXd b = Eigen::VectorXd::Random(andof * N); + + const auto energy = [&](const Eigen::VectorXd& x_) -> double { + const Eigen::VectorXd y = to_affine->to_affine(x_); + return 0.5 * y.dot(C * y) + b.dot(y); + }; + const auto grad_y = [&](const Eigen::VectorXd& y) -> Eigen::VectorXd { + return C * y + b; + }; + const auto analytic_grad = + [&](const Eigen::VectorXd& x_) -> Eigen::VectorXd { + return to_affine->apply_gradient(x_, grad_y(to_affine->to_affine(x_))); + }; + + // --- Round trip --------------------------------------------------------- + if (rigid_cov) { + CHECK( + (to_affine->from_affine(to_affine->to_affine(x)) - x).norm() + < 1e-9); + } else { + CHECK((to_affine->to_affine(x) - x).norm() == 0.0); + } + + // --- Gradient: (dy/dx)ᵀ ∇φ vs FD of φ∘to_affine ------------------------- + const Eigen::VectorXd g = analytic_grad(x); + Eigen::VectorXd fd_g; + fd::finite_gradient(x, energy, fd_g); + CHECK(fd::compare_gradient(g, fd_g)); + if (!fd::compare_gradient(g, fd_g)) { + std::cout << "analytic:\n" << g.transpose() << "\n"; + std::cout << "numerical:\n" << fd_g.transpose() << "\n"; + } + + // --- Hessian: apply_hessian vs FD of the analytic gradient -------------- + const Eigen::SparseMatrix H = to_affine->apply_hessian( + x, grad_y(to_affine->to_affine(x)), C, PSDProjectionMethod::NONE); + Eigen::MatrixXd fd_H; + fd::finite_jacobian(x, analytic_grad, fd_H); + CHECK(fd::compare_hessian(Eigen::MatrixXd(H), fd_H, 1e-3)); + if (!fd::compare_hessian(Eigen::MatrixXd(H), fd_H, 1e-3)) { + std::cout << "analytic:\n" << Eigen::MatrixXd(H) << "\n\n"; + std::cout << "numerical:\n" << fd_H << "\n"; + } +} diff --git a/tests/src/tests/friction/test_anisotropic_friction.cpp b/tests/src/tests/friction/test_anisotropic_friction.cpp index d713b50c9..d9f06a3b0 100644 --- a/tests/src/tests/friction/test_anisotropic_friction.cpp +++ b/tests/src/tests/friction/test_anisotropic_friction.cpp @@ -9,11 +9,11 @@ #include #include #include +#include #include #include #include -#include #include #include @@ -35,7 +35,7 @@ TEST_CASE("Anisotropic mu effective computation", "[friction][anisotropic][mu]") Eigen::Vector2d mu_k_aniso(mu_k0, mu_k1); // Test various directions - const double angle = GENERATE(range(0.0, 2.0 * igl::PI, igl::PI / 4.0)); + const double angle = GENERATE(range(0.0, 2.0 * ipc::PI, ipc::PI / 4.0)); Eigen::Vector2d tau_dir(std::cos(angle), std::sin(angle)); CAPTURE(mu_s_aniso, mu_k_aniso, tau_dir); @@ -124,7 +124,7 @@ TEST_CASE( Eigen::Vector2d mu_k_aniso_zero = Eigen::Vector2d::Zero(); // Test with various directions - const double angle = GENERATE(range(0.0, 2.0 * igl::PI, igl::PI / 4.0)); + const double angle = GENERATE(range(0.0, 2.0 * ipc::PI, ipc::PI / 4.0)); Eigen::Vector2d tau_dir(std::cos(angle), std::sin(angle)); // When anisotropic coefficients are zero, the function should handle it @@ -497,7 +497,7 @@ TEST_CASE("Anisotropic friction force", "[friction][anisotropic][force]") FrictionPotential friction_potential(1e-4); // Test with different velocity directions - const double angle = GENERATE(range(0.0, 2.0 * igl::PI, igl::PI / 4.0)); + const double angle = GENERATE(range(0.0, 2.0 * ipc::PI, ipc::PI / 4.0)); const double velocity_magnitude = GENERATE(0.01, 0.1); Eigen::MatrixXd velocities(2, 3); @@ -689,7 +689,7 @@ TEST_CASE( FrictionPotential friction_potential(1e-4); // Test various velocity directions - const double angle = GENERATE(range(0.0, 2.0 * igl::PI, igl::PI / 6.0)); + const double angle = GENERATE(range(0.0, 2.0 * ipc::PI, ipc::PI / 6.0)); const double velocity_magnitude = GENERATE(0.01, 0.1, 0.5); Eigen::MatrixXd velocities(2, 3); diff --git a/tests/src/tests/geometry/test_angle.cpp b/tests/src/tests/geometry/test_angle.cpp index 4c923f02c..9ff1a070d 100644 --- a/tests/src/tests/geometry/test_angle.cpp +++ b/tests/src/tests/geometry/test_angle.cpp @@ -5,15 +5,15 @@ #include #include +#include -#include #include #include using namespace ipc; -constexpr double deg2rad(double degrees) { return degrees * igl::PI / 180.0; } +constexpr double deg2rad(double degrees) { return degrees * ipc::PI / 180.0; } TEST_CASE("Dihedral angle and gradient", "[angle][dihedral]") { @@ -30,11 +30,11 @@ TEST_CASE("Dihedral angle and gradient", "[angle][dihedral]") CAPTURE(rot); Eigen::AngleAxisd rotation( - (igl::PI - rot) / 2.0, Eigen::Vector3d::UnitY()); + (ipc::PI - rot) / 2.0, Eigen::Vector3d::UnitY()); x2 = rotation * x2; x3 = rotation.inverse() * x3; - expected_angle = rot - igl::PI; + expected_angle = rot - ipc::PI; } SECTION("Case 1") diff --git a/tests/src/tests/io/CMakeLists.txt b/tests/src/tests/io/CMakeLists.txt new file mode 100644 index 000000000..cf6481834 --- /dev/null +++ b/tests/src/tests/io/CMakeLists.txt @@ -0,0 +1,14 @@ +set(SOURCES) + +if(IPC_TOOLKIT_WITH_GLTF) + list(APPEND SOURCES + test_read_gltf.cpp + test_write_gltf.cpp + ) +endif() + +target_sources(ipc_toolkit_tests PRIVATE ${SOURCES}) + +################################################################################ +# Subfolders +################################################################################ \ No newline at end of file diff --git a/tests/src/tests/io/test_read_gltf.cpp b/tests/src/tests/io/test_read_gltf.cpp new file mode 100644 index 000000000..aca45fe56 --- /dev/null +++ b/tests/src/tests/io/test_read_gltf.cpp @@ -0,0 +1,235 @@ +// Test the mass utilities. + +#include +#include + +#include +#include + +#include +#include +#include + +#include + +#include +#include + +using namespace ipc; +using namespace ipc::rigid; + +TEST_CASE("Read GLTF (5-cubes)", "[io][rigid]") +{ + std::shared_ptr bodies; + std::vector initial_poses; + + std::tie(bodies, initial_poses) = read_gltf( + (tests::DATA_DIR / "rigid-ipc/16-unit-tests/5-cubes.glb").string(), + true); + + REQUIRE(bodies != nullptr); + + CHECK(bodies->num_bodies() == 5); + CHECK(initial_poses.size() == 5); + CHECK(bodies->planes.size() == 1); + CAPTURE(bodies->planes[0].normal(), bodies->planes[0].offset()); + CHECK(bodies->planes[0].offset() == Catch::Approx(1)); + CHECK(bodies->planes[0].normal().isApprox(Eigen::Vector3d(0, 1, 0))); +} + +TEST_CASE("Read GLTF (incline plane)", "[io][rigid]") +{ + std::shared_ptr bodies; + std::vector initial_poses; + + std::tie(bodies, initial_poses) = read_gltf( + (tests::DATA_DIR + / "rigid-ipc/18-high-school-physics-friction-test-mu=0.5.glb") + .string(), + true); + + REQUIRE(bodies != nullptr); + + CHECK(bodies->num_bodies() == 1); + CHECK(initial_poses.size() == 1); + CHECK(bodies->planes.size() == 1); + CAPTURE(bodies->planes[0].normal(), bodies->planes[0].offset()); + CHECK(bodies->planes[0].offset() == Catch::Approx(0.50535359193774421)); + + const double theta = std::atan(0.5); + Eigen::Vector3d n(std::sin(theta), std::cos(theta), 0); + CAPTURE(theta, n); + CHECK(bodies->planes[0].normal().isApprox(n, 1e-6)); +} + +TEST_CASE("Read GLTF (card house)", "[io][rigid]") +{ + std::shared_ptr bodies; + std::vector initial_poses; + + std::tie(bodies, initial_poses) = read_gltf( + (tests::DATA_DIR / "rigid-ipc/10-codimensional-card-house.glb") + .string(), + true); + + REQUIRE(bodies != nullptr); + + CHECK(bodies->num_bodies() == 17); + CHECK(initial_poses.size() == bodies->num_bodies()); + CHECK(bodies->planes.size() == 1); + CAPTURE(bodies->planes[0].normal(), bodies->planes[0].offset()); + CHECK(bodies->planes[0].offset() == 0); + CHECK(bodies->planes[0].normal().isApprox(Eigen::Vector3d(0, 1, 0))); +} + +TEST_CASE("Read GLTF names, zero-scale nodes, and DOF masks", "[io][rigid]") +{ + Eigen::MatrixXd V; + Eigen::MatrixXi E, F; + tests::load_mesh("cube.ply", V, E, F); + + // Three named cubes in a row; the last two share a name. + std::vector initial_poses(3, Pose::identity(3)); + for (size_t i = 0; i < initial_poses.size(); i++) { + initial_poses[i].position = Eigen::Vector3d(3.0 * i, 0.0, 0.0); + } + auto bodies = RigidBodies::build_from_meshes( + { V, V, V }, { E, E, E }, { F, F, F }, { 1000.0, 1000.0, 1000.0 }, + initial_poses, /*convert_planes=*/false, /*is_dof_fixed=*/ {}, + { "Hidden", "Cube", "Cube" }); + + // Write an ASCII glTF, then scale the first node to zero. + const auto tmp = std::filesystem::temp_directory_path(); + const std::string base = (tmp / "ipctk_read_base.gltf").string(); + REQUIRE(write_gltf( + base, *bodies, std::list> { initial_poses }, 0.01, + /*embed_buffers=*/true, /*write_binary=*/false)); + nlohmann::json j; + { + std::ifstream f(base); + f >> j; + } + j["nodes"][0]["scale"] = { 0.0, 0.0, 0.0 }; + // write_gltf stores the unique body names, so we restore the duplicate. + j["nodes"][2]["name"] = "Cube"; + const std::string patched = (tmp / "ipctk_read_patched.gltf").string(); + { + std::ofstream f(patched); + f << j.dump(); + } + + VectorMax6b slider(6); + slider << false, true, true, true, true, true; + auto [bodies_in, poses_in] = read_gltf( + patched, /*convert_planes=*/false, + { { "Cube.1", slider }, { "NoSuchBody", slider } }); + REQUIRE(bodies_in != nullptr); + + // The zero-scale node is skipped. The others keep their node names, made + // unique, and masks are matched against those unique names. + REQUIRE(bodies_in->num_bodies() == 2); + CHECK(poses_in.size() == 2); + CHECK(bodies_in->body_name(0) == "Cube"); + CHECK(bodies_in->body_name(1) == "Cube.1"); + + // DOF masks are applied by name; unlisted bodies are free. + CHECK(!(*bodies_in)[0].is_dof_fixed().any()); + CHECK((*bodies_in)[1].is_dof_fixed() == slider); + + // Geometry only: every body gets the default density. + CHECK((*bodies_in)[0].density() == Catch::Approx(1000.0)); + + std::filesystem::remove(base); + std::filesystem::remove(patched); +} + +TEST_CASE( + "Read GLTF composes node hierarchies and merges primitives", "[io][rigid]") +{ + Eigen::MatrixXd V; + Eigen::MatrixXi E, F; + tests::load_mesh("cube.ply", V, E, F); + + std::vector initial_poses(1, Pose::identity(3)); + initial_poses[0].position = Eigen::Vector3d(1.0, 0.5, -2.0); + initial_poses[0].rotation = Eigen::Vector3d(0.3, -0.2, 0.4); + auto bodies = RigidBodies::build_from_meshes( + { V }, { E }, { F }, { 1000.0 }, initial_poses); + + const auto tmp = std::filesystem::temp_directory_path(); + const std::string base = (tmp / "ipctk_read_hierarchy_base.gltf").string(); + REQUIRE(write_gltf( + base, *bodies, std::list> { initial_poses }, 0.01, + /*embed_buffers=*/true, /*write_binary=*/false)); + nlohmann::json j; + { + std::ifstream f(base); + f >> j; + } + + // World-space vertices of the body as written. + auto [bodies_ref, poses_ref] = read_gltf(base); + REQUIRE(bodies_ref != nullptr); + const Eigen::MatrixXd V_ref = bodies_ref->vertices(poses_ref); + + const std::string patched = + (tmp / "ipctk_read_hierarchy_patched.gltf").string(); + const auto read_patched = [&]() { + { + std::ofstream f(patched); + f << j.dump(); + } + return read_gltf(patched); + }; + + SECTION("Child transforms are relative to their parent") + { + // Parent the body under a node that rotates, scales non-uniformly, + // and translates; the body's world geometry must follow. + const Eigen::Quaterniond q( + Eigen::AngleAxisd(0.7, Eigen::Vector3d(1, 2, 3).normalized())); + const Eigen::Vector3d s(1.0, 2.0, 0.5), t(0.0, 5.0, -1.0); + j["nodes"].push_back( + { + { "name", "Parent" }, + { "translation", { t.x(), t.y(), t.z() } }, + { "rotation", { q.x(), q.y(), q.z(), q.w() } }, + { "scale", { s.x(), s.y(), s.z() } }, + { "children", { 0 } }, + }); + j["scenes"][0]["nodes"] = { int(j["nodes"].size()) - 1 }; + + auto [bodies_in, poses_in] = read_patched(); + REQUIRE(bodies_in != nullptr); + REQUIRE(bodies_in->num_bodies() == 1); + + const Eigen::Matrix3d L = q.toRotationMatrix() * s.asDiagonal(); + const Eigen::MatrixXd expected = + (V_ref * L.transpose()).rowwise() + t.transpose(); + const Eigen::MatrixXd actual = bodies_in->vertices(poses_in); + CHECK(actual.isApprox(expected, 1e-5)); + } + + SECTION("Every primitive of a mesh contributes its own vertices") + { + auto& primitives = j["meshes"][0]["primitives"]; + primitives.push_back(primitives[0]); + + auto [bodies_in, poses_in] = read_patched(); + REQUIRE(bodies_in != nullptr); + REQUIRE(bodies_in->num_bodies() == 1); + + const Eigen::Index nv = V_ref.rows(); + REQUIRE(bodies_in->num_vertices() == 2 * nv); + REQUIRE(bodies_in->num_faces() == 2 * bodies_ref->num_faces()); + + // Each copy's faces index its own vertices. + const Eigen::MatrixXi& F_in = bodies_in->faces(); + const Eigen::Index nf = bodies_ref->num_faces(); + CHECK(F_in.topRows(nf).maxCoeff() < nv); + CHECK(F_in.bottomRows(nf).minCoeff() >= nv); + } + + std::filesystem::remove(base); + std::filesystem::remove(patched); +} diff --git a/tests/src/tests/io/test_write_gltf.cpp b/tests/src/tests/io/test_write_gltf.cpp new file mode 100644 index 000000000..f8f6d9c85 --- /dev/null +++ b/tests/src/tests/io/test_write_gltf.cpp @@ -0,0 +1,90 @@ +#include +#include + +#include + +#include +#include +#include + +using namespace ipc; +using namespace ipc::rigid; + +TEST_CASE("Write and read glTF round-trip", "[io][gltf]") +{ + Eigen::MatrixXd V; + Eigen::MatrixXi E, F; + tests::load_mesh("cube.ply", V, E, F); + + Eigen::MatrixXd V_box = V; + V_box.col(0) *= 2.0; // Anisotropic body (nontrivial R₀) + + // A codimensional rod (edges, no faces) has no triangles to write. + Eigen::MatrixXd V_rod(3, 3); + V_rod << 0, 0, 0, 1, 0.2, 0, 2, 0, 0.1; + Eigen::MatrixXi E_rod(2, 2); + E_rod << 0, 1, 1, 2; + + std::vector initial_poses(3, Pose::identity(3)); + initial_poses[0].position = Eigen::Vector3d(0, 1, 0); + initial_poses[1].position = Eigen::Vector3d(2, 0.5, 0); + initial_poses[2].position = Eigen::Vector3d(-2, 0.5, 0); + // A non-identity starting orientation must survive the round trip too. + initial_poses[0].rotation = Eigen::Vector3d(0.3, -0.2, 0.5); + + auto bodies = RigidBodies::build_from_meshes( + { V_box, V, V_rod }, { E, E, E_rod }, { F, F, Eigen::MatrixXi(0, 3) }, + { 1000.0, 500.0, 100.0 }, initial_poses, + /*convert_planes=*/false, /*is_dof_fixed=*/ {}, + { "Box", "Cube", "Rod" }); + + // Synthesize a short pose history (falling and rotating) + const double dt = 0.1; + std::list> pose_history; + for (int step = 0; step < 5; step++) { + std::vector poses = initial_poses; + for (size_t i = 0; i < poses.size(); i++) { + poses[i].position.y() -= 0.1 * step; + poses[i].rotation = rotation_matrix_to_vector( + Eigen::AngleAxisd(0.1 * step, Eigen::Vector3d::UnitZ()) + .toRotationMatrix() + * poses[i].rotation_matrix()); + } + pose_history.push_back(poses); + } + + const bool write_binary = GENERATE(true, false); + const std::string filename = + (std::filesystem::temp_directory_path() + / (write_binary ? "ipctk_round_trip.glb" : "ipctk_round_trip.gltf")) + .string(); + + REQUIRE(write_gltf( + filename, *bodies, pose_history, dt, /*embed_buffers=*/true, + write_binary)); + + // Round trip + auto [bodies_in, poses_in] = read_gltf(filename); + REQUIRE(bodies_in != nullptr); + + REQUIRE(bodies_in->num_bodies() == bodies->num_bodies()); + REQUIRE(poses_in.size() == bodies->num_bodies()); + + for (size_t i = 0; i < bodies->num_bodies(); i++) { + CHECK(bodies_in->body_num_vertices(i) == bodies->body_num_vertices(i)); + CHECK(bodies_in->body_num_edges(i) == bodies->body_num_edges(i)); + CHECK(bodies_in->body_num_faces(i) == bodies->body_num_faces(i)); + CHECK(bodies_in->body_name(i) == bodies->body_name(i)); + + // The world-space vertices at the initial pose must round-trip + // (through the R₀ baking on write and re-diagonalization on read). + // Vertex order is preserved by the writer. + const Eigen::MatrixXd V_out = + bodies->body_vertices(i, pose_history.front()[i]); + const Eigen::MatrixXd V_in = bodies_in->body_vertices(i, poses_in[i]); + REQUIRE(V_in.rows() == V_out.rows()); + CHECK((V_in - V_out).cwiseAbs().maxCoeff() < 1e-5); // float buffers + } + + std::filesystem::remove(filename); +} diff --git a/tests/src/tests/math/test_interval.cpp b/tests/src/tests/math/test_interval.cpp index 22f6d4f82..f6964348a 100644 --- a/tests/src/tests/math/test_interval.cpp +++ b/tests/src/tests/math/test_interval.cpp @@ -7,8 +7,7 @@ #include #include - -#include +#include using namespace ipc; @@ -41,10 +40,10 @@ TEST_CASE("Cosine interval arithmetic", "[interval]") double shift; SECTION("No shift") { shift = 0; } - SECTION("2π shift") { shift = 2 * igl::PI; } - SECTION("-2π shift") { shift = -2 * igl::PI; } - SECTION("100π shift") { shift = 100 * igl::PI; } - SECTION("-100π shift") { shift = -100 * igl::PI; } + SECTION("2π shift") { shift = 2 * ipc::PI; } + SECTION("-2π shift") { shift = -2 * ipc::PI; } + SECTION("100π shift") { shift = 100 * ipc::PI; } + SECTION("-100π shift") { shift = -100 * ipc::PI; } CAPTURE(shift); @@ -67,12 +66,12 @@ TEST_CASE("Sine interval arithmetic", "[interval]") { filib::Interval r; - double shift = igl::PI / 2; + double shift = ipc::PI / 2; SECTION("No shift") { shift += 0; } - SECTION("2π shift") { shift += 2 * igl::PI; } - SECTION("-2π shift") { shift += -2 * igl::PI; } - SECTION("100π shift") { shift += 100 * igl::PI; } - SECTION("-100π shift") { shift += -100 * igl::PI; } + SECTION("2π shift") { shift += 2 * ipc::PI; } + SECTION("-2π shift") { shift += -2 * ipc::PI; } + SECTION("100π shift") { shift += 100 * ipc::PI; } + SECTION("-100π shift") { shift += -100 * ipc::PI; } CAPTURE(shift); diff --git a/tests/src/tests/potential/test_adhesion_potentials.cpp b/tests/src/tests/potential/test_adhesion_potentials.cpp index 1d4b06282..c4f0a3de8 100644 --- a/tests/src/tests/potential/test_adhesion_potentials.cpp +++ b/tests/src/tests/potential/test_adhesion_potentials.cpp @@ -9,9 +9,9 @@ #include #include #include +#include #include -#include using namespace ipc; @@ -39,7 +39,7 @@ TEST_CASE("Normal adhesion potential", "[potential][adhesion]") if (rotated) { Eigen::Matrix3d R = Eigen::AngleAxis( - Eigen::AngleAxisd(igl::PI / 4, Eigen::Vector3d::UnitY())) + Eigen::AngleAxisd(ipc::PI / 4, Eigen::Vector3d::UnitY())) .toRotationMatrix(); vertices.bottomRows(n) = (vertices.bottomRows(n) * R.transpose()).eval(); diff --git a/tests/src/tests/potential/test_esp_potential.cpp b/tests/src/tests/potential/test_esp_potential.cpp index 46124e9ac..7e270ae45 100644 --- a/tests/src/tests/potential/test_esp_potential.cpp +++ b/tests/src/tests/potential/test_esp_potential.cpp @@ -11,7 +11,6 @@ #include #include -#include #include #include #include @@ -26,6 +25,7 @@ #include #include #include +#include #include @@ -268,8 +268,8 @@ inline void build_apex_to_apex_tets( Eigen::MatrixXi& E, Eigen::MatrixXi& F) { - const double c = std::cos(2 * igl::PI / 3), s = std::sin(2 * igl::PI / 3); - const double r = std::cos(igl::PI / 3), q = std::sin(igl::PI / 3); + const double c = std::cos(2 * ipc::PI / 3), s = std::sin(2 * ipc::PI / 3); + const double r = std::cos(ipc::PI / 3), q = std::sin(ipc::PI / 3); V.resize(8, 3); V << 0, 0, gap / 2, 1, 0, 1, c, s, 1, c, -s, 1, // 0, 0, -gap / 2, r, q, -1, -1, 0, -1, r, -q, -1; @@ -1828,8 +1828,8 @@ TEST_CASE( Eigen::MatrixXd V(n, 2); Eigen::MatrixXi E(n, 2); for (int i = 0; i < n; i++) { - V(i, 0) = std::cos(2 * igl::PI * i / n); - V(i, 1) = std::sin(2 * igl::PI * i / n); + V(i, 0) = std::cos(2 * ipc::PI * i / n); + V(i, 1) = std::sin(2 * ipc::PI * i / n); E(i, 0) = i; E(i, 1) = (i + 1) % n; } diff --git a/tests/src/tests/test_collision_mesh.cpp b/tests/src/tests/test_collision_mesh.cpp index eeff97f66..44008e7c5 100644 --- a/tests/src/tests/test_collision_mesh.cpp +++ b/tests/src/tests/test_collision_mesh.cpp @@ -6,8 +6,7 @@ #include #include #include - -#include +#include using namespace ipc; @@ -269,7 +268,7 @@ TEST_CASE("User-provided collision mesh areas", "[collision_mesh][areas]") SECTION("Custom areas become the collision weights") { - const double scale = 2 * igl::PI * 3.0; + const double scale = 2 * ipc::PI * 3.0; mesh.set_vertex_areas(scale * default_areas); mesh.set_edge_areas(scale * mesh.edge_areas()); diff --git a/tests/src/tests/utils/CMakeLists.txt b/tests/src/tests/utils/CMakeLists.txt index 972a0d5c3..d69549869 100644 --- a/tests/src/tests/utils/CMakeLists.txt +++ b/tests/src/tests/utils/CMakeLists.txt @@ -6,6 +6,7 @@ set(SOURCES test_vertex_matrix_view.cpp test_profiler.cpp test_simd_utils.cpp + test_sinc.cpp test_utils.cpp # Benchmarks diff --git a/tests/src/tests/utils/test_sinc.cpp b/tests/src/tests/utils/test_sinc.cpp new file mode 100644 index 000000000..e5c7717fc --- /dev/null +++ b/tests/src/tests/utils/test_sinc.cpp @@ -0,0 +1,151 @@ +#include +#include +#include + +#include + +#include + +// #include +#include +#include + +using namespace ipc; + +TEST_CASE("sinc(double)", "[sinc][double]") +{ + CHECK(sinc(0) == Catch::Approx(1)); + CHECK(sinc(1e-8) == Catch::Approx(1)); + CHECK(sinc(ipc::PI / 2) == Catch::Approx(2 / ipc::PI)); + CHECK(sinc(ipc::PI) == Catch::Approx(0).margin(1e-16)); +} + +TEST_CASE("sinc(Interval)", "[sinc][interval]") +{ + // All of these cases fall within the monotonic bound on sinc, so they are + // a tight bound (ignoring rounding). + filib::Interval x, expected_y; + + SECTION("y=[1, 1]") + { + SECTION("[0, 0]") { x = filib::Interval(0); }; + SECTION("[0, 1e-8]") { x = filib::Interval(0, 1e-8); }; + SECTION("[-1e-8, 0]") { x = filib::Interval(-1e-8, 0); }; + SECTION("[-1e-8, 1e-8]") { x = filib::Interval(-1e-8, 1e-8); }; + expected_y = filib::Interval(1); + } + SECTION("y=[0, 1]") + { + SECTION("[0, π]") { x = filib::Interval(0, ipc::PI); } + SECTION("[-π, 0]") { x = filib::Interval(-ipc::PI, 0); } + SECTION("[-π, π]") { x = filib::Interval(-ipc::PI, ipc::PI); } + expected_y = filib::Interval(0, 1); + } + SECTION("y=[2/pi, 1]") + { + const double PI_2 = ipc::PI / 2; + SECTION("[0, π/2]") { x = filib::Interval(0, PI_2); } + SECTION("[-π/2, 0]") { x = filib::Interval(-PI_2, 0); } + SECTION("[-π/2, π/2]") { x = filib::Interval(-PI_2, PI_2); } + expected_y = filib::Interval(2 / ipc::PI, 1); + } + + CAPTURE(x.INF, x.SUP); + filib::Interval y = sinc(x); + CHECK(y.INF == Catch::Approx(expected_y.INF).margin(1e-8)); + CHECK(y.SUP == Catch::Approx(expected_y.SUP).margin(1e-8)); +} + +TEST_CASE("Interval sinc with looser bounds", "[sinc][interval]") +{ + filib::Interval x, expected_y; + + SECTION("Non-monotonic") + { + const double a = GENERATE(-5, -1, 0); + x = filib::Interval(a, 5); + expected_y = filib::Interval(-0.217233628211221659, 1); + } + SECTION("Monotonic outside bounds") + { + x = filib::Interval(5, 7); + expected_y = filib::Interval(sinc(5), sinc(7)); + } + SECTION("Monotonic far outside bounds") + { + x = filib::Interval(21, 22); + expected_y = filib::Interval(sinc(22), sinc(21)); + } + + filib::Interval y = sinc(x); + CAPTURE(x.INF, x.SUP, y.INF, y.SUP, expected_y.INF, expected_y.SUP); + CHECK(in(expected_y, y)); + // Loosest bound + CHECK(in(y, filib::Interval(-0.23, 1))); + if (x.INF > 0) { + // Tighter bound if x.INF > 1 + CHECK(in(y, filib::Interval(-1 / x.INF, 1 / x.INF))); + } +} + +TEST_CASE("Interval sinc_norm_x", "[sinc][interval]") +{ + VectorMax3 x = VectorMax3::Zero(3); + filib::Interval expected_y; + + SECTION("Zero") + { + // x is already zero + expected_y = filib::Interval(1); + } + SECTION("Positive") + { + x(1) = filib::Interval(ipc::PI); + expected_y = filib::Interval(0); + } + SECTION("Negative") + { + x(1) = filib::Interval(-ipc::PI); + expected_y = filib::Interval(0); + } + SECTION("Mixed") + { + x(1) = filib::Interval(-ipc::PI, ipc::PI); + expected_y = filib::Interval(0, 1); + } + + filib::Interval y = sinc_norm_x(x); + CAPTURE(y.INF, y.SUP, expected_y.INF, expected_y.SUP); + CHECK(expected_y.INF == Catch::Approx(y.INF).margin(1e-8)); + CHECK(expected_y.SUP == Catch::Approx(y.SUP).margin(1e-8)); +} + +TEST_CASE("Grad sinc(||x||)", "[sinc][vector][diff]") +{ + double sign = GENERATE(-1, 1); + double val = GENERATE(0, 1e-8, ipc::PI / 2, ipc::PI, 5, 20, 100); + int index = GENERATE(0, 1, 2); + Eigen::Vector3d x = Eigen::Vector3d::Zero(); + x(index) = sign * val; + + Eigen::VectorXd fgrad(3); + fd::finite_gradient(x, sinc_norm_x, fgrad); + + Eigen::Vector3d grad = sinc_norm_x_grad(x); + CHECK(fd::compare_gradient(grad, fgrad)); +} + +TEST_CASE("Hess sinc(||x||)", "[sinc][vector][diff]") +{ + double sign = GENERATE(-1, 1); + double val = GENERATE(0, 1e-8, ipc::PI / 2, ipc::PI, 5, 20, 100); + int index = GENERATE(0, 1, 2); + Eigen::Vector3d x = Eigen::Vector3d::Zero(); + x(index) = sign * val; + + Eigen::MatrixXd fhess(3, 3); + fd::finite_hessian(x, sinc_norm_x, fhess); + + Eigen::Matrix3d hess = sinc_norm_x_hess(x); + CHECK(fd::compare_hessian(hess, fhess)); +} \ No newline at end of file diff --git a/tests/src/tests/utils/test_utils.cpp b/tests/src/tests/utils/test_utils.cpp index 101953c47..89ac2caf8 100644 --- a/tests/src/tests/utils/test_utils.cpp +++ b/tests/src/tests/utils/test_utils.cpp @@ -7,7 +7,7 @@ #include #include #include -#include +#include #include @@ -66,7 +66,7 @@ TEST_CASE("Project to PD", "[utils][project_to_pd]") CHECK(A_pd.isApprox(A)); } -TEST_CASE("Save OBJ of candidates", "[utils][save_obj]") +TEST_CASE("Write OBJ of candidates", "[utils][write_candidates_obj]") { Eigen::MatrixXd V(4, 3); V.row(0) << 0, 0, 0; @@ -81,21 +81,21 @@ TEST_CASE("Save OBJ of candidates", "[utils][save_obj]") SECTION("VertexVertexCandidate") { std::stringstream ss; - ipc::save_obj( + ipc::write_candidates_obj( ss, V, E, F, { { ipc::VertexVertexCandidate(0, 1) } }); CHECK(ss.str() == "o VV\nv 0 0 0\nv 1 0 0\n"); } SECTION("EdgeVertexCandidate") { std::stringstream ss; - ipc::save_obj( + ipc::write_candidates_obj( ss, V, E, F, { { ipc::EdgeVertexCandidate(0, 0) } }); CHECK(ss.str() == "o EV\nv 1 0 0\nv 0 1 0\nv 0 0 0\nl 1 2\n"); } SECTION("EdgeEdgeCandidate") { std::stringstream ss; - ipc::save_obj( + ipc::write_candidates_obj( ss, V, E, F, { { ipc::EdgeEdgeCandidate(0, 1) } }); CHECK( ss.str() @@ -104,7 +104,7 @@ TEST_CASE("Save OBJ of candidates", "[utils][save_obj]") SECTION("FaceVertexCandidate") { std::stringstream ss; - ipc::save_obj( + ipc::write_candidates_obj( ss, V, E, F, { { ipc::FaceVertexCandidate(0, 0) } }); CHECK( ss.str() == "o FV\nv 1 0 0\nv 0 1 0\nv 0 0 1\nv 0 0 0\nf 1 2 3\n"); @@ -112,7 +112,7 @@ TEST_CASE("Save OBJ of candidates", "[utils][save_obj]") SECTION("EdgeFaceCandidate") { std::stringstream ss; - ipc::save_obj( + ipc::write_candidates_obj( ss, V, E, F, { { ipc::EdgeFaceCandidate(0, 0) } }); CHECK( ss.str()