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 fb8995342..55f406e8a 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/coverage.yml b/.github/workflows/coverage.yml index f4239d455..2882c129a 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' @@ -56,6 +57,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 \ -DCMAKE_BUILD_TYPE=Release \ diff --git a/.gitignore b/.gitignore index 893ede1b6..557ff691d 100644 --- a/.gitignore +++ b/.gitignore @@ -676,4 +676,6 @@ CMakeGraphVizOptions.cmake CLAUDE.md .claude -graphify-out \ No newline at end of file +graphify-out +__cmake_systeminformation +simulator_test.glb diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml index a66ed7c58..2450f2973 100644 --- a/.pre-commit-config.yaml +++ b/.pre-commit-config.yaml @@ -7,10 +7,8 @@ repos: rev: v21.1.2 hooks: - id: clang-format - name: clang-format - description: "Use clang-format to format C/C++ code" - entry: clang-format args: + - -i - --style=file - --verbose files: '\.(c|cc|cpp|h|hpp|tpp|cxx|hh|inl|ipp)$' diff --git a/CMakeLists.txt b/CMakeLists.txt index 5d0cc3932..8bf0e5887 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -65,13 +65,34 @@ project(IPCToolkit VERSION "1.6.0") if(IPC_TOOLKIT_TOPLEVEL_PROJECT) - option(IPC_TOOLKIT_BUILD_TESTS "Build unit-tests" ON) - option(IPC_TOOLKIT_BUILD_PYTHON "Build Python bindings" OFF) + # Generate a compile_commands.json file + set(CMAKE_EXPORT_COMPILE_COMMANDS ${CLO_IPC_TOPLEVEL_PROJECT}) + + # If we are on a Mac, explicitly add the sysroot to the compiler flags + if(CMAKE_EXPORT_COMPILE_COMMANDS AND APPLE) + # CMake usually finds the sysroot automatically, but if it didn't, fetch it + if(NOT CMAKE_OSX_SYSROOT) + execute_process(COMMAND xcrun --show-sdk-path + OUTPUT_VARIABLE CMAKE_OSX_SYSROOT + OUTPUT_STRIP_TRAILING_WHITESPACE) + endif() + + # Force the -isysroot flag into the compile commands for clang-tidy + add_compile_options("-isysroot" "${CMAKE_OSX_SYSROOT}") + endif() +endif() + +if(IPC_TOOLKIT_TOPLEVEL_PROJECT) + option(IPC_TOOLKIT_BUILD_TESTS "Build unit-tests" ON) + option(IPC_TOOLKIT_BUILD_PYTHON "Build Python bindings" OFF) + option(IPC_TOOLKIT_BUILD_DEMO "Build the demo simulator" ON) else() - # If this is not the top-level project, we don't want to build tests or Python - # bindings. This is useful for projects that use IPC Toolkit as a submodule. - set(IPC_TOOLKIT_BUILD_TESTS OFF CACHE BOOL "Build unit-tests" FORCE) - set(IPC_TOOLKIT_BUILD_PYTHON OFF CACHE BOOL "Build Python bindings" FORCE) + # If this is not the top-level project, we don't want to build tests, Python + # bindings, or the demo simulator. This is useful for projects that use IPC + # Toolkit as a submodule. + set(IPC_TOOLKIT_BUILD_TESTS OFF CACHE BOOL "Build unit-tests" FORCE) + set(IPC_TOOLKIT_BUILD_PYTHON OFF CACHE BOOL "Build Python bindings" FORCE) + set(IPC_TOOLKIT_BUILD_DEMO OFF CACHE BOOL "Build the demo simulator" FORCE) endif() option(IPC_TOOLKIT_WITH_CUDA "Enable CUDA CCD" OFF) @@ -80,6 +101,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) @@ -214,6 +236,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 @@ -287,6 +315,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 ################################################################################ @@ -313,6 +351,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/README.md b/README.md index 66dbd94df..c9e059dc8 100644 --- a/README.md +++ b/README.md @@ -24,7 +24,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/ipc_toolkit/ipc_toolkit_tests_data.cmake b/cmake/ipc_toolkit/ipc_toolkit_tests_data.cmake index 9c53bc956..751672636 100644 --- a/cmake/ipc_toolkit/ipc_toolkit_tests_data.cmake +++ b/cmake/ipc_toolkit/ipc_toolkit_tests_data.cmake @@ -30,7 +30,7 @@ else() SOURCE_DIR ${IPC_TOOLKIT_TESTS_DATA_DIR} GIT_REPOSITORY https://github.com/ipc-sim/ipc-toolkit-tests-data.git - GIT_TAG c7eba549d9a80d15569a013c473f0aff104ac44a + GIT_TAG 5f455c16f461e56d68e9241fa038bd20d95bce0a CONFIGURE_COMMAND "" BUILD_COMMAND "" diff --git a/cmake/ipc_toolkit/ipc_toolkit_use_colors.cmake b/cmake/ipc_toolkit/ipc_toolkit_use_colors.cmake index d3d6b3695..5ee73479e 100644 --- a/cmake/ipc_toolkit/ipc_toolkit_use_colors.cmake +++ b/cmake/ipc_toolkit/ipc_toolkit_use_colors.cmake @@ -13,17 +13,6 @@ include_guard(GLOBAL) # options if("${CMAKE_CXX_COMPILER_ID}" STREQUAL "MSVC") - # Reduce the warning level of external files to the selected value (W1 - only major). - # Requires Visual Studio 2017 version 15.7 - # https://blogs.msdn.microsoft.com/vcblog/2017/12/13/broken-warnings-theory/ - - # There is an issue in using these flags in earlier versions of MSVC: - # https://developercommunity.visualstudio.com/content/problem/220812/experimentalexternal-generates-a-lot-of-c4193-warn.html - if(MSVC_VERSION GREATER 1920) - add_compile_options(/experimental:external) - add_compile_options(/external:W1) - endif() - # When building in parallel, MSVC sometimes fails with the following error: # > fatal error C1090: PDB API call failed, error code '23' # To avoid this problem, we force PDB write to be synchronous with /FS. diff --git a/cmake/ipc_toolkit/ipc_toolkit_warnings.cmake b/cmake/ipc_toolkit/ipc_toolkit_warnings.cmake index fb32358e1..e2910aaa3 100644 --- a/cmake/ipc_toolkit/ipc_toolkit_warnings.cmake +++ b/cmake/ipc_toolkit/ipc_toolkit_warnings.cmake @@ -10,8 +10,22 @@ endif() # Flags above don't make sense for MSVC if(MSVC) set(IPC_TOOLKIT_WARNING_FLAGS - /Wall # Display all warnings - /MP # Multi-processor compilation + /W4 # High warning level (MSVC equivalent of -Wall -Wextra; /Wall is + # unusably noisy, enabling off-by-default warnings like C4711/C4710/ + # C4514/C4820/C5045) + /MP # Multi-processor compilation + # Silence /W4 warnings that have no counterpart in our GCC/Clang flag set + # (-Wall -Wextra -Wpedantic, with -Wconversion intentionally off), so the + # MSVC output matches the other compilers: + /wd4250 # 'inherits via dominance' (benign; from virtual inheritance) + /wd4244 # conversion, possible loss of data (GCC: -Wconversion, off) + /wd4267 # size_t -> smaller conversion (GCC: -Wconversion, off) + /wd4201 # nonstandard nameless struct/union (a GCC/Clang extension) + /wd4068 # unknown pragma (cross-compiler #pragma gcc/clang directives) + /wd4100 # unreferenced formal parameter. GCC's -Wunused-parameter only + # fires here because the Windows CI builds Release: asserts (and the + # params they use) are compiled out, and the collision-stencil + # virtual overrides legitimately ignore some params. ) else() set(IPC_TOOLKIT_WARNING_FLAGS diff --git a/cmake/recipes/polysolve.cmake b/cmake/recipes/polysolve.cmake new file mode 100644 index 000000000..ebe77efb9 --- /dev/null +++ b/cmake/recipes/polysolve.cmake @@ -0,0 +1,37 @@ +# PolySolve (https://github.com/polyfem/polysolve) +# License: MIT +if(TARGET polysolve::polysolve) + return() +endif() + +message(STATUS "Third-party: creating target 'polysolve::polysolve'") + +# 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.) +set(POLYSOLVE_WITH_ACCELERATE OFF CACHE BOOL "Enable Apple Accelerate" FORCE) +set(POLYSOLVE_WITH_AMGCL OFF CACHE BOOL "Use AMGCL" FORCE) +set(POLYSOLVE_WITH_CHOLMOD OFF CACHE BOOL "Enable Cholmod library" FORCE) +set(POLYSOLVE_WITH_HYPRE OFF CACHE BOOL "Enable hypre" FORCE) +set(POLYSOLVE_WITH_MKL OFF CACHE BOOL "Enable MKL library" FORCE) +set(POLYSOLVE_WITH_PARDISO OFF CACHE BOOL "Enable Pardiso library" FORCE) +set(POLYSOLVE_WITH_SPECTRA OFF CACHE BOOL "Enable Spectra library" FORCE) +set(POLYSOLVE_WITH_SPQR OFF CACHE BOOL "Enable SPQR library" FORCE) +set(POLYSOLVE_WITH_SUPERLU OFF CACHE BOOL "Enable SuperLU library" FORCE) +set(POLYSOLVE_WITH_TESTS OFF CACHE BOOL "Build unit-tests" FORCE) +set(POLYSOLVE_WITH_UMFPACK OFF CACHE BOOL "Enable UmfPack library" FORCE) + +# 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("gh:polyfem/polysolve#1b01e6cec13c6813ae48846d1f98654bbbf1402b") + +# 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..3f0b9771d --- /dev/null +++ b/demo/CMakeLists.txt @@ -0,0 +1,43 @@ +################################################################################ +# 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) + +# 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..f5be3be4a --- /dev/null +++ b/demo/README.md @@ -0,0 +1,254 @@ +# 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 `1e-2 × bbox_diagonal` (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..04423999d --- /dev/null +++ b/demo/python/simulator.cpp @@ -0,0 +1,246 @@ +#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, "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("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( + "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( + "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 " + "(reset each step).") + .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); +} 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..a69d8183f --- /dev/null +++ b/demo/src/kinematics.cpp @@ -0,0 +1,286 @@ +#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. + NonlinearCCD m_ccd; + }; + + /// @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..51ac38747 --- /dev/null +++ b/demo/src/simulator.cpp @@ -0,0 +1,1141 @@ +#include "simulator.hpp" + +#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 + +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; + + std::vector + zero_velocities(const size_t num_bodies, const int dim) + { + return std::vector(num_bodies, rigid::Pose::Identity(dim)); + } + + /// 2D skew "matrix" generator: ω S with S = [[0, -1], [1, 0]]. + inline Eigen::Matrix2d skew_2d(const double omega) + { + Eigen::Matrix2d S; // NOLINT + S << 0, -omega, omega, 0; + return S; + } +} // 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); + m_normal_collisions->set_collision_set_type( + NormalCollisions::CollisionSetType::IMPROVED_MAX_APPROX); + 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)"); + } + } + 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; + if (m_settings.velocity_conv_tol > 0 + && !solver_params.contains("x_delta_tol")) { + solver_params["x_delta_tol"] = + m_settings.velocity_conv_tol * m_bbox_diagonal * dt; + } + + // 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; + + // Zero the velocity components of fixed DOFs. + rigid::Pose velocity = velocities[i]; + const auto& fixed = (*m_bodies)[i].is_dof_fixed(); + 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; + } + } + + 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) = + (skew_2d(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); + + // Barrier: chain the vertex-space Hessian to the DOFs (Jᵀ H J plus the + // second-order term for the rigid parameterization, which needs the + // vertex-space gradient). + const Eigen::MatrixXd V = m_kinematics->world_vertices(x); + 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, + m_project_to_psd ? PSDProjectionMethod::CLAMP + : PSDProjectionMethod::NONE); + + hess = hess + + s * m_kinematics->map_vertex_hessian(x, barrier_grad, barrier_hess); + + 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); pass the pre-scaled vertex-space + // gradient/Hessian so map_vertex_hessian applies both pieces. + const Eigen::MatrixXd velocities = friction_velocities(x); + const Eigen::VectorXd friction_grad = s + * m_friction_potential->gradient( + *m_tangential_collisions, *m_bodies, velocities); + const Eigen::SparseMatrix friction_hess = + (s / m_time_integrator->velocity_scaling()) + * m_friction_potential->hessian( + *m_tangential_collisions, *m_bodies, velocities, + m_project_to_psd ? PSDProjectionMethod::CLAMP + : PSDProjectionMethod::NONE); + hess = hess + + m_kinematics->map_vertex_hessian(x, friction_grad, friction_hess); + } + + 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->U().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 with the pinned rows/columns replaced by + // identity so the direction solve never moves them. + const Eigen::MatrixXd& U = m_joints->U(); + Eigen::MatrixXd H = U.transpose() * (hess * U); + const int m = int(m_joints->num_constraints()); + H.topRows(m).setZero(); + H.leftCols(m).setZero(); + H.diagonal().head(m).setOnes(); + for (const int i : m_pinned_dofs) { + H.row(i).setZero(); + H.col(i).setZero(); + H(i, i) = 1.0; + } + hess = H.sparseView(); + } else if (!m_pinned_dofs.empty()) { + // Zero the pinned rows/columns and put ones on their diagonal so the + // direction solve never moves them (S H S + (I − S) with a diagonal + // selector S, staying sparse). + Eigen::SparseMatrix S(hess.rows(), hess.cols()); // NOLINT + std::vector> triplets; + triplets.reserve(hess.rows()); + std::vector pinned(hess.rows(), false); + for (const int i : m_pinned_dofs) { + pinned[i] = true; + } + 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 (const int i : m_pinned_dofs) { + 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. + return m_kinematics->max_step_size( + to_full(v0), to_full(v1), + INFLATION_RADIUS_MULTIPLIER * m_barrier_potential->dhat(), + m_broad_phase.get()); +} + +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(); + } + if (m_solver_v.size() != delta_v.size()) { + return delta_v.norm(); // fallback: no base point available + } + // Maximum world-space vertex displacement of the proposed step. + return (m_kinematics->world_vertices(to_full(m_solver_v + delta_v)) + - m_kinematics->world_vertices(to_full(m_solver_v))) + .lpNorm(); +} + +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); + + if (kappa != m_barrier_potential->stiffness()) { + logger().debug( + "updated barrier stiffness: κ={:g} min_distance={:g}", kappa, + 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); + + double average_mass = 0; + 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_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, + 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); + 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 = 1e-2 * m_bbox_diagonal; + m_last_momentum_balance = -1; + initialize_friction_step(); + update_friction_collisions(x); + + // 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; + + // 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; + + const bool 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; + } + + // Re-seed the barrier stiffness for the next solve (the original + // Rigid IPC solver re-initializes κ at the start of every solve). + 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); + + if (update_candidates) { + m_kinematics->update_candidates( + x, INFLATION_RADIUS_MULTIPLIER * m_barrier_potential->dhat(), + m_broad_phase.get()); + } + + m_normal_collisions->build( + m_kinematics->candidates(), *m_bodies, V, m_barrier_potential->dhat()); +} + +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); +} + +// ---------------------------------------------------------------------------- +// 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 int dim = m_bodies->dim(); + const double h = m_time_integrator->dt(); + const Eigen::VectorXd& v_prev = m_time_integrator->v_prev(0); + const int state_ndof = + int(m_time_integrator->pos_ndof() + m_time_integrator->rot_ndof()); + + 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(); + + // The prescribed velocity: linear in .position, angular ω in + // .rotation. The driver applies the script-or-integrate policy; the + // ω extraction from the integrator state layout lives here. + rigid::Pose velocity; + velocity.position = v_prev.segment(state_ndof * i, dim); + if (dim == 2) { + // 2D (both models): ω from Q̇ = ω S Q ⇒ ω = (Q̇ Qᵀ)(1, 0). + const Eigen::Matrix2d Q = pose.rotation; + const Eigen::Matrix2d Qdot = + v_prev.segment<4>(state_ndof * i + 2).reshaped(2, 2); + velocity.rotation = + VectorMax3d::Constant(1, (Qdot * Q.transpose())(1, 0)); + } else { + // 3D: [ω]× = Q̇ Qᵀ (projected to antisymmetric for robustness). + const Eigen::Matrix3d Q = pose.rotation; + const Eigen::Matrix3d Qdot = + v_prev.segment<9>(state_ndof * i + 3).reshaped(3, 3); + const Eigen::Matrix3d omega_hat = 0.5 + * (Qdot * Q.transpose() - (Qdot * Q.transpose()).transpose()); + velocity.rotation = Eigen::Vector3d( + omega_hat(2, 1), omega_hat(0, 2), omega_hat(1, 0)); + } + + 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); +} + +bool Simulator::al_active() const +{ + return m_body_potentials->augmented_lagrangian_active(); +} + +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) { + rigid_poses[i].position = affine_poses[i].position; + rigid_poses[i].rotation = affine_poses[i].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..06a00aaf6 --- /dev/null +++ b/demo/src/simulator.hpp @@ -0,0 +1,652 @@ +#pragma once + +#include "kinematic_driver.hpp" +#include "kinematics.hpp" + +#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 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 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 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‖ ≤ 1e-2·bbox_diagonal (up to + /// max_outer_iterations). + int friction_iterations = 1; + /// @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 + /// (reset each step) [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. + bool use_area_weighting = true; + /// @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). Set ≤ 0 to disable. + 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). + nlohmann::json solver_params = { + { "solver", + { { { "type", "ProjectedNewton" }, + { "residual_tolerance", 1e100 } }, + { { "type", "RegularizedProjectedNewton" }, + { "residual_tolerance", 1e100 } } } }, + { "allow_non_grad_convergence", true }, + { "max_iterations", 100 }, + { "grad_norm_tol", 1e-6 }, + { "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). + 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 + { + return m_settings.friction_coefficient > 0 + && m_settings.friction_iterations != 0; + } + + /// @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 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 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/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 ca73632f7..19de6b91c 100755 --- a/python/CMakeLists.txt +++ b/python/CMakeLists.txt @@ -9,6 +9,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) @@ -41,4 +52,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/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..823a983f6 --- /dev/null +++ b/python/examples/joints.py @@ -0,0 +1,228 @@ +"""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. + +# 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..46d7fce5b --- /dev/null +++ b/python/examples/rigid.py @@ -0,0 +1,154 @@ +import pathlib +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) + +if len(sys.argv) > 1: + bodies, initial_poses = ipctk.read_gltf(sys.argv[1], True) +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))] + +# for pose in initial_poses: +# print(pose) + +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, +) + +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", + ) + +settings = ipctk.demo.Simulator.Settings() +settings.body_dynamics = ipctk.demo.Simulator.BodyDynamics.RIGID +settings.bdf_order = 2 + +sim = ipctk.demo.Simulator( + bodies=bodies, + initial_poses=initial_poses, + dt=1 / 60.0, + settings=settings, +) + +playing = False + + +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 + 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() + imgui.SameLine() + if imgui.Button("Save"): + ipctk.write_gltf( + "output.glb", + bodies, + sim.rigid_pose_history, + 1 / 60.0, + ) + + +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 d20970c2e..1bb6bf569 100644 --- a/python/src/bindings.cpp +++ b/python/src/bindings.cpp @@ -1,6 +1,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; @@ -126,4 +131,16 @@ PYBIND11_MODULE(ipctk, m) // root 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 } 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 be82f62cc..bf0be8f7c 100644 --- a/python/src/candidates/candidates.cpp +++ b/python/src/candidates/candidates.cpp @@ -262,7 +262,7 @@ 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_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/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..6ade12cba --- /dev/null +++ b/python/src/dynamics/rigid/bodies.cpp @@ -0,0 +1,153 @@ +#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&>(), + "vertices"_a, "edges"_a, "faces"_a, "density"_a, "initial_pose"_a, + "is_dof_fixed"_a = VectorMax6b()) + .def_property_readonly("mass", &RigidBody::mass) + .def_property_readonly( + "moment_of_inertia", &RigidBody::moment_of_inertia) + .def_property_readonly("J", &RigidBody::J) + .def_property_readonly("R0", &RigidBody::R0) + .def_property_readonly("external_force", &RigidBody::external_force) + .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()) + .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[]), + "index"_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. + + 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. + + Returns: + A pair containing the RigidBodies object and a vector of initial poses for each body. + )ipc_Qu8mg5v7", + "filename"_a, "convert_planes"_a = false); +#endif +} diff --git a/src/ipc/CMakeLists.txt b/src/ipc/CMakeLists.txt index fd8b5be2f..c0f65ec99 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 fd622a3df..89f3a4483 100644 --- a/src/ipc/broad_phase/lbvh.cpp +++ b/src/ipc/broad_phase/lbvh.cpp @@ -796,6 +796,112 @@ void LBVH::detect_face_face_candidates( std::bind(&LBVH::can_faces_collide, this, _1, _2), candidates); } +// ============================================================================ +// Two-tree (inter-BVH) queries. +// +// The other BVH is used as the source (its leaves 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. + +void LBVH::detect_vertex_vertex_candidates( + const LBVH& other, + const std::function& can_collide, + std::vector& candidates) const +{ + if (!has_vertices() || !other.has_vertices()) { + return; + } + + IPC_TOOLKIT_PROFILE_BLOCK( + "LBVH::detect_vertex_vertex_candidates(two-tree)"); + + detect_candidates( + other.vertex_bvh, vertex_bvh, /*rightmost_leaves=*/ {}, can_collide, + candidates); +} + +void LBVH::detect_edge_vertex_candidates( + const LBVH& other, + const std::function& can_collide, + std::vector& candidates) const +{ + if (!has_edges() || !other.has_vertices()) { + return; + } + + IPC_TOOLKIT_PROFILE_BLOCK("LBVH::detect_edge_vertex_candidates(two-tree)"); + + detect_candidates( + other.vertex_bvh, edge_bvh, /*rightmost_leaves=*/ {}, can_collide, + candidates); +} + +void LBVH::detect_vertex_edge_candidates( + const LBVH& other, + const std::function& can_collide, + std::vector& candidates) const +{ + if (!has_vertices() || !other.has_edges()) { + return; + } + + IPC_TOOLKIT_PROFILE_BLOCK("LBVH::detect_vertex_edge_candidates(two-tree)"); + + detect_candidates( + other.edge_bvh, vertex_bvh, /*rightmost_leaves=*/ {}, can_collide, + candidates); +} + +void LBVH::detect_edge_edge_candidates( + const LBVH& other, + const std::function& can_collide, + std::vector& candidates) const +{ + if (!has_edges() || !other.has_edges()) { + return; + } + + IPC_TOOLKIT_PROFILE_BLOCK("LBVH::detect_edge_edge_candidates(two-tree)"); + + detect_candidates( + other.edge_bvh, edge_bvh, /*rightmost_leaves=*/ {}, can_collide, + candidates); +} + +void LBVH::detect_face_vertex_candidates( + const LBVH& other, + const std::function& can_collide, + std::vector& candidates) const +{ + if (!has_faces() || !other.has_vertices()) { + return; + } + + IPC_TOOLKIT_PROFILE_BLOCK("LBVH::detect_face_vertex_candidates(two-tree)"); + + detect_candidates( + other.vertex_bvh, face_bvh, /*rightmost_leaves=*/ {}, can_collide, + candidates); +} + +void LBVH::detect_vertex_face_candidates( + const LBVH& other, + const std::function& can_collide, + std::vector& candidates) const +{ + if (!has_vertices() || !other.has_faces()) { + return; + } + + IPC_TOOLKIT_PROFILE_BLOCK("LBVH::detect_vertex_face_candidates(two-tree)"); + + detect_candidates( + other.face_bvh, 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 64f37de77..c11d0c701 100644 --- a/src/ipc/broad_phase/lbvh.hpp +++ b/src/ipc/broad_phase/lbvh.hpp @@ -144,6 +144,66 @@ class LBVH : public BroadPhase { const Nodes& edge_nodes() const { return edge_bvh; } const Nodes& face_nodes() const { return face_bvh; } + // ------------------------------------------------------------------------- + // Two-tree (inter-BVH) queries between this LBVH and another LBVH. + // Candidate ids are each BVH'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 BVH's vertices. + /// @param[in] other The other LBVH 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 LBVH& other, + const std::function& can_collide, + std::vector& candidates) const; + + /// @brief Find candidate collisions between this BVH's edges and another BVH's vertices. + /// @param[in] other The other LBVH 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 LBVH& other, + const std::function& can_collide, + std::vector& candidates) const; + + /// @brief Find candidate collisions between this BVH's vertices and another BVH's edges. + /// @param[in] other The other LBVH 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 LBVH& other, + const std::function& can_collide, + std::vector& candidates) const; + + /// @brief Find candidate collisions between this BVH's edges and another BVH's edges. + /// @param[in] other The other LBVH 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 LBVH& other, + const std::function& can_collide, + std::vector& candidates) const; + + /// @brief Find candidate collisions between this BVH's faces and another BVH's vertices. + /// @param[in] other The other LBVH 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 LBVH& other, + const std::function& can_collide, + std::vector& candidates) const; + + /// @brief Find candidate collisions between this BVH's vertices and another BVH's faces. + /// @param[in] other The other LBVH 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 LBVH& 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. diff --git a/src/ipc/candidates/candidates.cpp b/src/ipc/candidates/candidates.cpp index 6d8bc9685..9f4a59f98 100644 --- a/src/ipc/candidates/candidates.cpp +++ b/src/ipc/candidates/candidates.cpp @@ -8,9 +8,9 @@ #include #include #include +#include #include #include -#include #include #include @@ -696,7 +696,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, @@ -707,13 +707,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 a52ae6bb0..d35ba33b5 100644 --- a/src/ipc/candidates/candidates.hpp +++ b/src/ipc/candidates/candidates.hpp @@ -224,13 +224,13 @@ 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, 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/nonlinear_ccd.cpp b/src/ipc/ccd/nonlinear_ccd.cpp index 23246e668..4486011ef 100644 --- a/src/ipc/ccd/nonlinear_ccd.cpp +++ b/src/ipc/ccd/nonlinear_ccd.cpp @@ -172,9 +172,8 @@ 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) { diff --git a/src/ipc/collision_mesh.hpp b/src/ipc/collision_mesh.hpp index d335c7de5..38576115c 100644 --- a/src/ipc/collision_mesh.hpp +++ b/src/ipc/collision_mesh.hpp @@ -13,6 +13,9 @@ class CollisionMesh { /// Collision Mesh objects are immutable, so use the other constructors. CollisionMesh() = default; + /// @brief Virtual destructor for the Collision Mesh class. + virtual ~CollisionMesh() = default; + /// @brief Construct a new Collision Mesh object directly from the collision mesh vertices. /// @param rest_positions The vertices of the collision mesh at rest (|V| × dim). /// @param edges The edges of the collision mesh (|E| × 2). @@ -65,9 +68,6 @@ class CollisionMesh { /// @brief Initialize vertex and edge areas. void init_area_jacobians(); - /// @brief Destroy the Collision Mesh object - ~CollisionMesh() = default; - /// @brief Get the number of vertices in the collision mesh. size_t num_vertices() const { return m_vertex_to_full_vertex.size(); } @@ -156,7 +156,7 @@ class CollisionMesh { /// @brief Compute the vertex positions from the positions of the full mesh. /// @param full_positions The vertex positions of the full mesh (|FV| × dim). /// @return The vertex positions of the collision mesh (|V| × dim). - Eigen::MatrixXd + virtual Eigen::MatrixXd vertices(Eigen::ConstRef 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 3c5c84e75..9a35bb6dd 100644 --- a/src/ipc/config.hpp.in +++ b/src/ipc/config.hpp.in @@ -19,6 +19,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 // #define IPC_TOOLKIT_DEBUG_AUTODIFF 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..e3895fa4f --- /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::J(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..2544c9495 --- /dev/null +++ b/src/ipc/dynamics/affine/augmented_lagrangian.cpp @@ -0,0 +1,272 @@ +#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.R0().rows(); + Eigen::VectorXd w(dim * dim); + for (int j = 0; j < dim; ++j) { + w.segment(dim * j, dim).array() = body.J().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) +{ + assert(targets.size() == bodies.num_bodies()); + 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_target_rotations.resize(targets.size()); + m_lambda.resize(targets.size()); + m_lambda_A.resize(targets.size()); + m_num_kinematic = 0; + for (size_t i = 0; i < targets.size(); ++i) { + m_lambda[i] = VectorMax3d::Zero(m_dim); + m_lambda_A[i] = Eigen::VectorXd::Zero(m_dim * m_dim); + // 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_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; +} + +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) +{ + 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 { + 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; + m_lambda[i] -= m_kappa_q * std::sqrt(bodies[i].mass()) + * (x.segment(ndof * i, m_dim) - m_targets[i].position); + } + } + 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 { + for (size_t i = 0; i < bodies.num_bodies(); ++i) { + if (bodies[i].type() != rigid::RigidBody::Type::KINEMATIC) { + continue; + } + // λ_A ← λ_A − κ_A W^{1/2} (a − â) + const int ndof = m_dim + m_dim * m_dim; + m_lambda_A[i] -= m_kappa_A + * angular_weights(bodies[i]) + .array() + .sqrt() + .matrix() + .asDiagonal() + * (x.segment(ndof * i + m_dim, m_dim * m_dim) + - Eigen::VectorXd(m_target_rotations[i].reshaped())); + } + } + logger().debug( + "augmented lagrangian (angular): η_A={:g} κ_A={:g} satisfied={}", + eta_A, m_kappa_A, m_angular_satisfied); + } +} + +// ---- 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() + - std::sqrt(bodies[i].mass()) * 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 = angular_weights(bodies[i]); + energy += 0.5 * m_kappa_A * da.dot(w.cwiseProduct(da)) + - m_lambda_A[i].dot(w.array().sqrt().matrix().cwiseProduct(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 + - std::sqrt(bodies[i].mass()) * 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 = angular_weights(bodies[i]); + grad.segment(ndof * i + m_dim, m_dim * m_dim) = + m_kappa_A * w.cwiseProduct(da) + - w.array().sqrt().matrix().cwiseProduct(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 = angular_weights(bodies[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..539be7c10 --- /dev/null +++ b/src/ipc/dynamics/affine/augmented_lagrangian.hpp @@ -0,0 +1,116 @@ +#pragma once + +#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 +/// - \sqrt{m}\, \lambda^\top (q - \hat{q}) +/// + \tfrac{\kappa_Q}{2} (a - \hat{a})^\top W (a - \hat{a}) +/// - \lambda_A^\top W^{1/2} (a - \hat{a}), +/// \f] +/// where â = vec(R(θ̂)) embeds the rigid target rotation. Fully quadratic +/// with constant PSD Hessian. Same method-of-multipliers schedule as the +/// rigid AL. Pure potential: the simulator applies the (βΔt)² scaling. +class AugmentedLagrangian { +public: + struct Params { + double initial_penalty = 1e3; + double max_penalty = 1e8; + double satisfied_progress = 0.999; + double stall_progress = 0.99; + }; + + AugmentedLagrangian() = default; + + explicit AugmentedLagrangian(const Params& params) : m_params(params) { } + + /// @brief Start a new step: capture the start state and targets, 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). + void init( + const rigid::RigidBodies& bodies, + Eigen::ConstRef x0, + const std::vector& targets); + + /// @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; + + 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; + + double m_kappa_q = 1e3; + double m_kappa_A = 1e3; // NOLINT(readability-identifier-naming) + + /// @brief Per-body multipliers (λ linear ∈ R^dim, λ_A angular ∈ R^dim²). + std::vector m_lambda; + std::vector m_lambda_A; // NOLINT + + bool m_linear_satisfied = false; + bool m_angular_satisfied = false; +}; + +} // 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..028100e3b --- /dev/null +++ b/src/ipc/dynamics/affine/body_forces.cpp @@ -0,0 +1,43 @@ +#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()) { + // Transform the world-space torque into body space. + const auto& A = poses[i].rotation; + if (dim == 3) { + const Eigen::Matrix3d tau = + A.transpose() * cross_product_matrix(torque); + m_wrench.segment<9>(ndof * i + 3) = -tau.reshaped(); + } else { + // 2D: the skew generator is S = [[0, -1], [1, 0]]. + Eigen::Matrix2d skew; // NOLINT + skew << 0, -torque(0), torque(0), 0; + const Eigen::Matrix2d tau = A.transpose() * skew; + m_wrench.segment<4>(ndof * i + 2) = -tau.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..aff85f94a --- /dev/null +++ b/src/ipc/dynamics/affine/body_forces.hpp @@ -0,0 +1,58 @@ +#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(τ)] (τ transformed as in the rigid BodyForces, +/// column-major). +/// +/// 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..38f001d99 --- /dev/null +++ b/src/ipc/dynamics/affine/inertial_term.cpp @@ -0,0 +1,73 @@ +#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; + 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].J().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), so refresh the + // zero blocks of non-DYNAMIC bodies. + build_mass_matrix(bodies); +} + +// ---- 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..5b449fce7 --- /dev/null +++ b/src/ipc/dynamics/affine/inertial_term.hpp @@ -0,0 +1,60 @@ +#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); + + 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; +}; + +} // 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..a6a4da690 --- /dev/null +++ b/src/ipc/dynamics/affine/joints.cpp @@ -0,0 +1,254 @@ +#include "joints.hpp" + +#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); +} + +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) +{ + assert(!m_finalized); + assert(body_a != body_b); + 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) +{ + assert(!m_finalized); + 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) +{ + assert(!m_finalized); + 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) +{ + assert(!m_finalized); + 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()); + assert(m < n); + + // Assemble C (m×n) and s + Eigen::MatrixXd C(m, n); + Eigen::VectorXd s(m); + for (int i = 0; i < m; i++) { + C.row(i) = m_rows[i]; + 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(n); + 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(n - 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]); + } + } + + // V_perm = [C̃; 0 I] (upper triangular) and U_perm = V_perm⁻¹ + Eigen::MatrixXd V_perm = Eigen::MatrixXd::Identity(n, n); + V_perm.topRows(m) = C; + + const Eigen::MatrixXd U_perm = V_perm.triangularView().solve( + Eigen::MatrixXd::Identity(n, n)); + + // Fold the DOF permutation in: x_perm[i] = x[perm[i]] + m_u.setZero(n, n); + m_v.setZero(n, n); + for (int i = 0; i < n; i++) { + m_u.row(perm[i]) = U_perm.row(i); + m_v.col(perm[i]) = V_perm.col(i); + } + + m_s = s; + + m_perm_inv.resize(n); + for (int i = 0; i < n; i++) { + m_perm_inv[perm[i]] = i; + } + + m_finalized = true; +} + +bool JointConstraints::is_body_constrained(const size_t body) const +{ + 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..b4587554a --- /dev/null +++ b/src/ipc/dynamics/affine/joints.hpp @@ -0,0 +1,125 @@ +#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. +/// +/// 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 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). + const Eigen::MatrixXd& U() const { return m_u; } // NOLINT + + /// @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 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::MatrixXd m_u; ///< x = Uz (permutation folded in) + Eigen::MatrixXd 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..eb20a672c --- /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.R0().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.R0().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.R0().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..85c417677 --- /dev/null +++ b/src/ipc/dynamics/affine/pose.cpp @@ -0,0 +1,64 @@ +#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) { + // For 2D, set the rotation matrix directly + rotation.resize(2, 2); + rotation << std::cos(theta(0)), -std::sin(theta(0)), // + std::sin(theta(0)), std::cos(theta(0)); + } else { + // For 3D, convert the rotation vector to a rotation matrix + rotation = rigid::rotation_vector_to_matrix(theta); + } +} + +Eigen::SparseMatrix +Pose::J(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..1188978f1 --- /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 J(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 + J(Eigen::ConstRef rest_positions); // NOLINT +}; + +} // 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..f5716015e --- /dev/null +++ b/src/ipc/dynamics/body_potentials.cpp @@ -0,0 +1,121 @@ +#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); + } + if (include_al && m_al.active()) { + g += scaling * 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 += scaling * 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 += scaling * 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) +{ + m_al.init(bodies, m_to_affine->to_affine(x0), targets); +} + +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..136b9b895 --- /dev/null +++ b/src/ipc/dynamics/body_potentials.hpp @@ -0,0 +1,138 @@ +#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 rest × 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². +/// +/// 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) ---- + + void init_augmented_lagrangian( + const rigid::RigidBodies& bodies, + Eigen::ConstRef x0, + const std::vector& targets); + + 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..49e23e949 --- /dev/null +++ b/src/ipc/dynamics/rigid/CMakeLists.txt @@ -0,0 +1,21 @@ +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_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..8b8738d56 --- /dev/null +++ b/src/ipc/dynamics/rigid/mass.cpp @@ -0,0 +1,279 @@ +#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( + const Eigen::MatrixXd& vertices, + const Eigen::MatrixXi& 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( + const Eigen::MatrixXd& vertices, + const Eigen::MatrixXi& 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( + const Eigen::MatrixXd& vertices, + const Eigen::MatrixXi& 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. + compute_mass_properties_lumped( + vertices, facets.cols() == 2 ? facets : 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( + const Eigen::MatrixXd& vertices, + const Eigen::MatrixXi& 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 + igl::massmatrix( + vertices, 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..3c84ccb3e --- /dev/null +++ b/src/ipc/dynamics/rigid/mass.hpp @@ -0,0 +1,38 @@ +#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 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( + const Eigen::MatrixXd& vertices, + const Eigen::MatrixXi& 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( + const Eigen::MatrixXd& vertices, + const Eigen::MatrixXi& 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..96c597a9e --- /dev/null +++ b/src/ipc/dynamics/rigid/pose.cpp @@ -0,0 +1,268 @@ +#include "pose.hpp" + +#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 >= M_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 >= M_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; +} + +} // 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..a195a8a48 --- /dev/null +++ b/src/ipc/dynamics/rigid/pose.hpp @@ -0,0 +1,262 @@ +#pragma once + +#include + +namespace ipc::rigid { + +/// @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) // NOLINT(readability-identifier-naming) + { + 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) { + // For 2D, set the rotation matrix directly + MatrixMax3d R(2, 2); + R << std::cos(rotation(0)), -std::sin(rotation(0)), + std::sin(rotation(0)), std::cos(rotation(0)); + return R; + } 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 * EIGEN_PI); + + // 4. Canonicalize PI: Ensure -π < angle <= π + // This prevents the representation from jumping between PI and -PI + if (angle <= -EIGEN_PI) { + angle = EIGEN_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 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..6cadac823 --- /dev/null +++ b/src/ipc/dynamics/rigid/rigid_bodies.cpp @@ -0,0 +1,350 @@ +#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) +{ + 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()); + + 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()); + } + + 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::shared_ptr bodies = std::make_shared( + concat_rest_positions, concat_edges, concat_faces, body_vertex_starts, + body_edge_starts, body_face_starts, densities, initial_poses, + nonplane_is_dof_fixed); + + bodies->planes = std::move(planes); + + 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) + : 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); + + 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]); + 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); + this->can_collide = [this](size_t vi, size_t vj) { + assert(body_vertex_starts.size() > 0); + return this->vertex_to_body(vi) != this->vertex_to_body(vj); + }; + assert(!this->can_collide(0, 0)); +} + +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..1068e178d --- /dev/null +++ b/src/ipc/dynamics/rigid/rigid_bodies.hpp @@ -0,0 +1,259 @@ +#pragma once + +#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). + 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 = {}); + + /// @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). + /// @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 = {}); + + /// @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 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. + /// @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 x 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()); + + // 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; + } + +private: + /// @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; +}; + +} // 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..2653c33b1 --- /dev/null +++ b/src/ipc/dynamics/rigid/rigid_body.cpp @@ -0,0 +1,187 @@ +#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 + compute_J(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) +{ + 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 center_of_mass; + MatrixMax3d inertia_tensor; + compute_mass_properties( + vertices, (dim == 2 || faces.size() == 0) ? edges : faces, density, + m_mass, center_of_mass, inertia_tensor); + m_volume = m_mass / density; + + // 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_R0 = Eigen::Matrix3d::Identity(); + } 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_R0 = solver.eigenvectors(); + + // Ensure that we have an orientation preserving transform + if (m_R0.determinant() < 0.0) { + m_R0.col(0) *= -1.0; + } + assert(m_R0.isUnitary(1e-9)); + + // Remove the initial rotation from the rest vertices + vertices = vertices * m_R0; + + // Store the initial rotation in the pose (R = RᵢR₀) + Eigen::AngleAxisd r = Eigen::AngleAxisd(Eigen::Matrix3d(m_R0)); + centering_pose.rotation = r.angle() * r.axis(); + + // Compute the diagonal second moment in the principal frame from the + // classical inertia tensor. + m_J = compute_J(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_R0 = R0; + + // Remove the initial rotation from the rest vertices + vertices = vertices * m_R0; + + // 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_J = 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_J.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..c4337f2fc --- /dev/null +++ b/src/ipc/dynamics/rigid/rigid_body.hpp @@ -0,0 +1,126 @@ +#pragma once + +#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. + RigidBody( + Eigen::Ref vertices, + Eigen::ConstRef edges, + Eigen::ConstRef faces, + const double density, + Pose& initial_pose, + const VectorMax6b& is_dof_fixed = VectorMax6b()); + + // ---- Getters ------------------------------------------------------------ + double volume() const { return m_volume; } + double mass() const { return m_mass; } + double density() const { return m_mass / m_volume; } + 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). + const auto& J() const { return m_J; } // NOLINT + const MatrixMax3d& R0() const { return m_R0; } // NOLINT + const Pose& external_force() const { return m_external_force; } + std::shared_ptr bvh() const { return m_bvh; } + double bounding_radius() const { return m_bounding_radius; } + + Type type() const { return m_type; } + 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 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; + + // NOLINTNEXTLINE(readability-identifier-naming) + Eigen::DiagonalMatrix m_J; + + /// @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_R0; // NOLINT(readability-identifier-naming) + + /// @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..e4086f4ab --- /dev/null +++ b/src/ipc/dynamics/rigid/rigid_candidates.cpp @@ -0,0 +1,553 @@ +#include "rigid_candidates.hpp" + +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include + +#include + +namespace ipc::rigid { + +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(); + + // 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); + 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). Save the caller's + // filter, use an accept-all filter for the body-level pass, and restore + // it afterwards. + const CollisionFilter caller_filter = 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. + std::vector body_candidates; + broad_phase->detect_vertex_vertex_candidates(body_candidates); + broad_phase->can_vertices_collide = caller_filter; + + // 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 + + // 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 uninflated, 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)); + for (size_t v = 0; v < body_j_vertex_boxes.size(); ++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 a temporary BVH 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. + // TODO: We should skip this BVH build and directly pass over the boxes + // to the two-tree traversal, but the current LBVH interface does not + // support that. + LBVH body_j_bvh; + body_j_bvh.build( + body_j_vertex_boxes, bodies.body_edges(body_j), + bodies.body_faces(body_j), dim); + + // 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_bvh, + [&](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_bvh, + [&](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_bvh, + [&](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_bvh, + [&](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_bvh, + [&](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_bvh, + [&](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_bvh, + [&](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_bvh, + [&](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..97ffb072d --- /dev/null +++ b/src/ipc/dynamics/rigid/rigid_candidates.hpp @@ -0,0 +1,125 @@ +#pragma once + +#include +#include +#include + +namespace ipc::rigid { + +/// @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 = NonlinearCCD()) 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 = NonlinearCCD()) 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 = NonlinearCCD()) 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_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..98b405886 --- /dev/null +++ b/src/ipc/dynamics/time_integration/bdf.cpp @@ -0,0 +1,111 @@ +#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(); + } + if (int(prev_ti) >= order()) { + return 0; + } + return -alphas(order() - 1)[prev_ti] / 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..9b47ff366 --- /dev/null +++ b/src/ipc/dynamics/time_integration/bdf.hpp @@ -0,0 +1,91 @@ +#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$, + /// \f$\partial v/\partial x^{t-i} = -\alpha_i/(\beta\Delta t)\f$. + 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..a097bafe6 --- /dev/null +++ b/src/ipc/dynamics/time_integration/time_integrator.hpp @@ -0,0 +1,160 @@ +#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 the i-th previous + /// positions (0 → current). + 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..7be83a22e --- /dev/null +++ b/src/ipc/dynamics/to_affine.cpp @@ -0,0 +1,203 @@ +#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 + H.topLeftCorner(d, d) = Ha.topLeftCorner(d, d); + // pos-rot and rot-pos (zero for our separable terms, general here) + 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θ² + MatrixMax3d H_rot = dQ.transpose() * Ha.block(d, d, d * d, d * d) * dQ; + H_rot += (d2Q.transpose() * g_affine.segment(i * andof + d, d * d)) + .reshaped(rot, rot); + // Project only the rotation block (position block is PSD by + // construction) + H.block(d, d, rot, rot) = project_to_psd(H_rot, project_hessian_to_psd); + + blocks[i] = H; + } + 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/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..2dd225235 --- /dev/null +++ b/src/ipc/io/read_gltf.cpp @@ -0,0 +1,216 @@ +#include "read_gltf.hpp" + +#include +#include + +#include +#include +#include + +namespace ipc::rigid { + +namespace { + + void extract_mesh_data( + const tinygltf::Model& model, + const tinygltf::Primitive& primitive, + Eigen::MatrixXd& V, + Eigen::MatrixXi& F) + { + // --- 1. Extract Vertices (POSITION) --- + // Look for the "POSITION" attribute in the mesh primitive + const tinygltf::Accessor& vAccessor = + model.accessors[primitive.attributes.at("POSITION")]; + const tinygltf::BufferView& vBufferView = + model.bufferViews[vAccessor.bufferView]; + const tinygltf::Buffer& vBuffer = model.buffers[vBufferView.buffer]; + + // Resize Eigen matrix: rows = num vertices, cols = 3 (x, y, z) + const size_t vOffset = V.rows(); + V.conservativeResize(V.rows() + vAccessor.count, 3); + + // Pointer to the start of the position data + const float* vData = reinterpret_cast( + &vBuffer.data[vBufferView.byteOffset + vAccessor.byteOffset]); + + // Calculate stride (usually 3 * sizeof(float), but check for + // interleaved data) + int vStride = vAccessor.ByteStride(vBufferView) / sizeof(float); + + for (size_t i = 0; i < vAccessor.count; ++i) { + V(vOffset + i, 0) = static_cast(vData[i * vStride + 0]); + V(vOffset + i, 1) = static_cast(vData[i * vStride + 1]); + V(vOffset + i, 2) = static_cast(vData[i * vStride + 2]); + } + + // --- 2. Extract Faces (INDICES) --- + const tinygltf::Accessor& iAccessor = + model.accessors[primitive.indices]; + const tinygltf::BufferView& iBufferView = + model.bufferViews[iAccessor.bufferView]; + const tinygltf::Buffer& iBuffer = model.buffers[iBufferView.buffer]; + + // glTF supports 3 vertices per triangle. MatrixXi: rows = num faces, + // cols = 3 + const size_t fOffset = F.rows(); + F.conservativeResize(F.rows() + iAccessor.count / 3, 3); + + const unsigned char* iDataPtr = + &iBuffer.data[iBufferView.byteOffset + iAccessor.byteOffset]; + + // Faces can be stored as scalar types: unsigned short, unsigned int, or + // unsigned byte + for (size_t i = 0; i < iAccessor.count / 3; ++i) { + for (int j = 0; j < 3; ++j) { + size_t idx = i * 3 + j; + if (iAccessor.componentType + == TINYGLTF_COMPONENT_TYPE_UNSIGNED_INT) { + F(fOffset + i, j) = static_cast( + reinterpret_cast(iDataPtr)[idx]); + } else if ( + iAccessor.componentType + == TINYGLTF_COMPONENT_TYPE_UNSIGNED_SHORT) { + F(fOffset + i, j) = static_cast( + reinterpret_cast(iDataPtr)[idx]); + } else if ( + iAccessor.componentType + == TINYGLTF_COMPONENT_TYPE_UNSIGNED_BYTE) { + F(fOffset + i, j) = static_cast( + reinterpret_cast(iDataPtr)[idx]); + } + } + } + } + + void process_node( + const tinygltf::Model& model, + const tinygltf::Node& node, + std::vector& rest_positions, + std::vector& edges, + std::vector& faces, + std::vector& densities, + std::vector& initial_poses) + { + // 1. Check if this node is a visual 'body' (has a mesh) + if (node.mesh > -1) { + const tinygltf::Mesh& mesh = model.meshes[node.mesh]; + logger().info("Found Body: {} (Mesh: {})", node.name, mesh.name); + rest_positions.emplace_back(); + faces.emplace_back(); + for (const auto& primitive : mesh.primitives) { + extract_mesh_data( + model, primitive, rest_positions.back(), faces.back()); + } + edges.emplace_back(); + igl::edges(faces.back(), edges.back()); + + // 2. Extract Transform (Position, Rotation, Scale) + // glTF uses column-major matrices or TRS (Translation, Rotation, + // Scale) + initial_poses.push_back(Pose::Identity(3)); // Default pose + if (node.translation.size() == 3) { + logger().info( + " - Position: [{}, {}, {}]", node.translation[0], + node.translation[1], node.translation[2]); + initial_poses.back().position << node.translation[0], + node.translation[1], node.translation[2]; + } + + if (node.scale.size() == 3) { + logger().info( + " - Scale: [{}, {}, {}]", node.scale[0], node.scale[1], + node.scale[2]); + + rest_positions.back() = rest_positions.back().array().rowwise() + * Eigen::Vector3d( + node.scale[0], node.scale[1], node.scale[2]) + .transpose() + .array(); + } + + if (node.rotation.size() == 4) { + logger().info( + " - Rotation (quaternion): [{}, {}, {}, {}]", + node.rotation[0], node.rotation[1], node.rotation[2], + node.rotation[3]); + // NOTE: glTF uses (x, y, z, w) + Eigen::AngleAxisd aa( + Eigen::Quaterniond( + node.rotation[3], node.rotation[0], node.rotation[1], + node.rotation[2])); + initial_poses.back().rotation = aa.angle() * aa.axis(); + } + + // 3. TODO: Add Physics Extension Loading Here + // This is where you will check node.extensions for + // "OMI_physics_body" + densities.emplace_back(1e3); // Default density + } + + // 4. Recursively process children (important for nested objects/stacks) + for (int childIdx : node.children) { + process_node( + model, model.nodes[childIdx], rest_positions, edges, faces, + densities, initial_poses); + } + } + +} // namespace + +std::pair, std::vector> +read_gltf(const std::string& filename, const bool convert_planes) +{ + 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, {} }; + } + + // Process the default scene + int sceneToLoad = model.defaultScene > -1 ? model.defaultScene : 0; + const tinygltf::Scene& scene = model.scenes[sceneToLoad]; + + logger().info( + "Loading scene: {} with {} root nodes.", scene.name, + scene.nodes.size()); + + std::vector rest_positions; + std::vector edges; + std::vector faces; + std::vector densities; + std::vector initial_poses; + + for (int nodeIdx : scene.nodes) { + process_node( + model, model.nodes[nodeIdx], rest_positions, edges, faces, + densities, initial_poses); + } + + return { RigidBodies::build_from_meshes( + rest_positions, edges, faces, densities, initial_poses, + convert_planes), + 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..7a4a2a543 --- /dev/null +++ b/src/ipc/io/read_gltf.hpp @@ -0,0 +1,20 @@ +#pragma once + +#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. +/// @param filename The input glTF filename. +/// @param convert_planes If true, any mesh with exactly two coplanar faces will be converted to a plane. +/// @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); + +} // 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/utils/save_obj.hpp b/src/ipc/io/write_candidates_obj.hpp similarity index 87% rename from src/ipc/utils/save_obj.hpp rename to src/ipc/io/write_candidates_obj.hpp index 819b323f2..dc8adfad6 100644 --- a/src/ipc/utils/save_obj.hpp +++ b/src/ipc/io/write_candidates_obj.hpp @@ -9,7 +9,7 @@ namespace ipc { template -void save_obj( +void write_candidates_obj( std::ostream& out, Eigen::ConstRef V, Eigen::ConstRef E, @@ -18,7 +18,7 @@ void save_obj( const int v_offset = 0); template -bool save_obj( +bool write_candidates_obj( const std::string& filename, Eigen::ConstRef V, Eigen::ConstRef E, @@ -29,7 +29,7 @@ bool save_obj( if (!obj.is_open()) { return false; } - save_obj(obj, V, E, F, candidates); + write_candidates_obj(obj, V, E, F, candidates); return true; } diff --git a/src/ipc/io/write_gltf.cpp b/src/ipc/io/write_gltf.cpp new file mode 100644 index 000000000..bae90304a --- /dev/null +++ b/src/ipc/io/write_gltf.cpp @@ -0,0 +1,384 @@ +#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(); + + 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); + node.name = body_name; + node.translation = { { poses.front()[i].position.x(), + poses.front()[i].position.y(), + poses.front()[i].position.z() } }; + // NOTE: Use identity for the first frame by applying the initial + // rotation to the vertices and removing the rotation from further + // rotations. + // Eigen::Quaternion q = poses[0][i].construct_quaternion(); + Eigen::Quaternion q = Eigen::Quaternion::Identity(); + 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 = TINYGLTF_MODE_TRIANGLES; + + 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].R0().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 = 3 * bodies.body_num_faces(i); + 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) * bodies.body_faces(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].R0().transpose(); + for (int r = 0; r < V.rows(); r++) { + for (int c = 0; c < V.cols(); c++) { + Float v = V(r, c); + std::memcpy(&byte_data[byte_i], &v, sizeof(Float)); + byte_i += sizeof(Float); + } + } + + Eigen::MatrixXi F = bodies.body_faces(i); + for (int r = 0; r < F.rows(); r++) { + for (int c = 0; c < F.cols(); c++) { + unsigned int fij = F(r, c); + std::memcpy(&byte_data[byte_i], &fij, sizeof(unsigned int)); + byte_i += sizeof(unsigned int); + } + } + } + for (int i = 0; i < num_steps; i++) { + Float t = 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++) { + Float pd = p[d]; + std::memcpy(&byte_data[byte_i], &pd, sizeof(Float)); + byte_i += sizeof(Float); + } + } + + const Eigen::Quaternion q0(Eigen::Matrix3d(bodies[i].R0())); + 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++) { + Float qd = 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].R0().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()); + } + } + double diag = (scene_max - scene_min).norm(); + double half_size = std::max(1.0, diag * 2.0); + + 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 b08bec9c8..d9606eb5b 100644 --- a/src/ipc/math/CMakeLists.txt +++ b/src/ipc/math/CMakeLists.txt @@ -5,6 +5,8 @@ set(SOURCES math.hpp math.tpp morton.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/math.cpp b/src/ipc/math/math.cpp index 175ba657b..7fc678bb7 100644 --- a/src/ipc/math/math.cpp +++ b/src/ipc/math/math.cpp @@ -4,9 +4,44 @@ #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 ComputeFullUV = 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 +55,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 64ded490f..c83b34d50 100644 --- a/src/ipc/math/math.hpp +++ b/src/ipc/math/math.hpp @@ -158,6 +158,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..04a5e45f3 --- /dev/null +++ b/src/ipc/math/sinc.cpp @@ -0,0 +1,148 @@ +#include "sinc.hpp" + +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 = sqrt(TAYLOR_0_BOUND); + const double TAYLOR_N_BOUND = sqrt(TAYLOR_2_BOUND); + + // 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 (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); + } + + // Compute sinc'(x) / x + inline double dsinc_over_x(double x) + { + static const double eps = 1e-4; + + double x2 = x * x; + if (abs(x) > eps) { + return (x * cos(x) - 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) + { + static const double eps = 0.1; + + double x2 = x * x; + double x4 = x2 * x2; + if (abs(x) > eps) { + return ((3 - x2) * sin(x) - 3 * x * 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 (abs(x) >= TAYLOR_N_BOUND) { + return sin(x) / x; + } + + // approximation by taylor series in x at 0 up to order 1 + double result = 1; + + if (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 (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; +} + +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); +} + +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..bdb61b1e6 --- /dev/null +++ b/src/ipc/math/sinc.hpp @@ -0,0 +1,44 @@ +#pragma once + +#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 f0086228e..a39d3153f 100644 --- a/src/ipc/utils/CMakeLists.txt +++ b/src/ipc/utils/CMakeLists.txt @@ -1,4 +1,5 @@ set(SOURCES + autodiff_types.hpp autodiff_types.hpp default_init_allocator.hpp eigen_ext.hpp @@ -11,8 +12,6 @@ set(SOURCES merge_thread_local.hpp profiler.cpp profiler.hpp - save_obj.cpp - save_obj.hpp unordered_map_and_set.cpp unordered_map_and_set.hpp vertex_to_min_edge.cpp diff --git a/src/ipc/utils/logger.cpp b/src/ipc/utils/logger.cpp index 331fa945a..7001f0f3e 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/tests/CMakeLists.txt b/tests/CMakeLists.txt index b534803f8..4266ebc99 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 6ac22663d..7daeec769 100644 --- a/tests/src/tests/CMakeLists.txt +++ b/tests/src/tests/CMakeLists.txt @@ -29,9 +29,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/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..7ac6a58cf --- /dev/null +++ b/tests/src/tests/demo/test_2d_simulator.cpp @@ -0,0 +1,323 @@ +// 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 * igl::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; + + // 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; + settings.velocity_conv_tol = 1e-3; + + // 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..b2dccf42c --- /dev/null +++ b/tests/src/tests/demo/test_affine_simulator.cpp @@ -0,0 +1,180 @@ +#include +#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..ef2400fae --- /dev/null +++ b/tests/src/tests/demo/test_codim.cpp @@ -0,0 +1,222 @@ +// 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 + +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); + for (int v = 0; v < 10; ++v) { + // Strictly separated (squared distance > 0); the conservative CCD + // rescaling leaves only a small gap. + CHECK(point_point_distance(V.row(v), V.row(10 + v)) > 0); + } + + // The discrete (distance) overload also produces the vv candidates. + RigidCandidates discrete; + discrete.build(*bodies, poses_alpha, /*inflation_radius=*/1e-2); + 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); + + auto bodies = RigidBodies::build_from_meshes( + { V_cloud, V_cloud }, { E_cloud, E_cloud }, { F_cloud, F_cloud }, + { 1000.0, 1000.0 }, initial_poses); + (*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..89534fb37 --- /dev/null +++ b/tests/src/tests/demo/test_friction_simulator.cpp @@ -0,0 +1,291 @@ +// Tests for the velocity-based lagged friction in the demo simulator. + +#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 * igl::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); +} + +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..834396715 --- /dev/null +++ b/tests/src/tests/demo/test_joints.cpp @@ -0,0 +1,230 @@ +#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; + +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); + + std::vector initial_poses(2, rigid::Pose::Identity(3)); + initial_poses[1].position = Eigen::Vector3d(1, 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); + 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 = 24; + 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(2); + for (int i = 0; i < 2; 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("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..3e0850779 --- /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, igl::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..91332db9a --- /dev/null +++ b/tests/src/tests/demo/test_static_kinematic.cpp @@ -0,0 +1,343 @@ +// 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 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); +} 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..cd3f55204 --- /dev/null +++ b/tests/src/tests/dynamics/affine/test_augmented_lagrangian.cpp @@ -0,0 +1,154 @@ +// 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)); +} + +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 + + // 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..778bd6606 --- /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..7b0c3e2c2 --- /dev/null +++ b/tests/src/tests/dynamics/rigid/CMakeLists.txt @@ -0,0 +1,15 @@ +set(SOURCES + test_mass.cpp + test_pose.cpp + test_rigid_bodies.cpp + test_rigid_body.cpp + test_rigid_candidates.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..333acfd24 --- /dev/null +++ b/tests/src/tests/dynamics/rigid/test_pose.cpp @@ -0,0 +1,266 @@ +#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 * igl::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 * igl::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 * igl::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, igl::PI / 4, igl::PI / 2, igl::PI * 0.75, + igl::PI - 1e-4, igl::PI - 1e-6, igl::PI - 1e-8, igl::PI, igl::PI + 1e-8, + igl::PI + 1e-6, igl::PI + 1e-4, igl::PI * 1.5, igl::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 - igl::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"; + } +} + +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"; + } +} \ 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..1b66716cd --- /dev/null +++ b/tests/src/tests/dynamics/rigid/test_rigid_bodies.cpp @@ -0,0 +1,80 @@ +// 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"; + } +} \ No newline at end of file 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..4d3505627 --- /dev/null +++ b/tests/src/tests/dynamics/rigid/test_rigid_body.cpp @@ -0,0 +1,98 @@ +// 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)); + } +} \ No newline at end of file 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..c055a5509 --- /dev/null +++ b/tests/src/tests/dynamics/rigid/test_rigid_candidates.cpp @@ -0,0 +1,575 @@ +#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(igl::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(igl::PI / 2, Eigen::Vector3d::UnitZ()) + .toRotationMatrix(), + poses_t0[1]); + poses_t1[0].rotation = compose_rotation( + Eigen::AngleAxisd(igl::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(igl::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)); + + 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)); + 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_trajectory.cpp b/tests/src/tests/dynamics/rigid/test_rigid_trajectory.cpp new file mode 100644 index 000000000..82d367603 --- /dev/null +++ b/tests/src/tests/dynamics/rigid/test_rigid_trajectory.cpp @@ -0,0 +1,345 @@ +#include +#include + +#include +#include +#ifdef IPC_TOOLKIT_WITH_FILIB +#include +#include +#endif + +#include + +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 * igl::PI); + const Pose pose_t1 = random_pose(dim, 2 * igl::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 = igl::PI / 2; } + SECTION("large rotation") { max_angle = 2 * igl::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, igl::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, igl::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, igl::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.9; + + 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 * igl::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); + + double toi; + const bool collision = + NonlinearCCD().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, igl::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.9; + + 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..07162de76 --- /dev/null +++ b/tests/src/tests/dynamics/test_bdf.cpp @@ -0,0 +1,89 @@ +#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 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..175700cc9 --- /dev/null +++ b/tests/src/tests/dynamics/test_body_potentials.cpp @@ -0,0 +1,110 @@ +#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"; + } +} 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/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..5b2a2530d --- /dev/null +++ b/tests/src/tests/io/test_read_gltf.cpp @@ -0,0 +1,76 @@ +// Test the mass utilities. + +#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))); +} \ No newline at end of file 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..cddb7156b --- /dev/null +++ b/tests/src/tests/io/test_write_gltf.cpp @@ -0,0 +1,78 @@ +#include +#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₀) + + std::vector initial_poses(2, Pose::Identity(3)); + initial_poses[0].position = Eigen::Vector3d(0, 1, 0); + initial_poses[1].position = Eigen::Vector3d(2, 0.5, 0); + + auto bodies = RigidBodies::build_from_meshes( + { V_box, V }, { E, E }, { F, F }, { 1000.0, 500.0 }, initial_poses); + + // 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_faces(i) == bodies->body_num_faces(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/utils/CMakeLists.txt b/tests/src/tests/utils/CMakeLists.txt index 8acd17887..bbd4b333c 100644 --- a/tests/src/tests/utils/CMakeLists.txt +++ b/tests/src/tests/utils/CMakeLists.txt @@ -3,6 +3,7 @@ set(SOURCES test_local_to_global.cpp test_matrixcache.cpp test_profiler.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..1aa284a3e --- /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(igl::PI / 2) == Catch::Approx(2 / igl::PI)); + CHECK(sinc(igl::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, igl::PI); } + SECTION("[-π, 0]") { x = filib::Interval(-igl::PI, 0); } + SECTION("[-π, π]") { x = filib::Interval(-igl::PI, igl::PI); } + expected_y = filib::Interval(0, 1); + } + SECTION("y=[2/pi, 1]") + { + const double PI_2 = igl::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 / igl::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(igl::PI); + expected_y = filib::Interval(0); + } + SECTION("Negative") + { + x(1) = filib::Interval(-igl::PI); + expected_y = filib::Interval(0); + } + SECTION("Mixed") + { + x(1) = filib::Interval(-igl::PI, igl::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, igl::PI / 2, igl::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, igl::PI / 2, igl::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()