feat(solver): graph decomposition for cluster-by-cluster solving (phase 3)
Add a Python decomposition layer using NetworkX that partitions the constraint graph into biconnected components (rigid clusters), orders them via a block-cut tree, and solves each cluster independently. Articulation-point bodies propagate as boundary conditions between clusters. New module kindred_solver/decompose.py: - DOF table mapping BaseJointKind to residual counts - Constraint graph construction (nx.MultiGraph) - Biconnected component detection + articulation points - Block-cut tree solve ordering (root-first from grounded cluster) - Cluster-by-cluster solver with boundary body fix/unfix cycling - Pebble game integration for per-cluster rigidity classification Changes to existing modules: - params.py: add unfix() for boundary body cycling - solver.py: extract _monolithic_solve(), add decomposition branch for assemblies with >= 8 free bodies Performance: for k clusters of ~n/k params each, total cost drops from O(n^3) to O(n^3/k^2). 220 tests passing (up from 207).
This commit is contained in:
@@ -32,12 +32,16 @@ from .constraints import (
|
||||
TangentConstraint,
|
||||
UniversalConstraint,
|
||||
)
|
||||
from .decompose import decompose, solve_decomposed
|
||||
from .dof import count_dof
|
||||
from .entities import RigidBody
|
||||
from .newton import newton_solve
|
||||
from .params import ParamTable
|
||||
from .prepass import single_equation_pass, substitution_pass
|
||||
|
||||
# Assemblies with fewer free bodies than this use the monolithic path.
|
||||
_DECOMPOSE_THRESHOLD = 8
|
||||
|
||||
# All BaseJointKind values this solver can handle
|
||||
_SUPPORTED = {
|
||||
# Phase 1
|
||||
@@ -95,11 +99,12 @@ class KindredSolver(kcsolve.IKCSolver):
|
||||
)
|
||||
bodies[part.id] = body
|
||||
|
||||
# 2. Build constraint residuals
|
||||
# 2. Build constraint residuals (track index mapping for decomposition)
|
||||
all_residuals = []
|
||||
constraint_objs = []
|
||||
constraint_indices = [] # parallel to constraint_objs: index in ctx.constraints
|
||||
|
||||
for c in ctx.constraints:
|
||||
for idx, c in enumerate(ctx.constraints):
|
||||
if not c.activated:
|
||||
continue
|
||||
body_i = bodies.get(c.part_i)
|
||||
@@ -123,6 +128,7 @@ class KindredSolver(kcsolve.IKCSolver):
|
||||
if obj is None:
|
||||
continue
|
||||
constraint_objs.append(obj)
|
||||
constraint_indices.append(idx)
|
||||
all_residuals.extend(obj.residuals())
|
||||
|
||||
# 3. Add quaternion normalization residuals for non-grounded bodies
|
||||
@@ -132,26 +138,31 @@ class KindredSolver(kcsolve.IKCSolver):
|
||||
all_residuals.append(body.quat_norm_residual())
|
||||
quat_groups.append(body.quat_param_names())
|
||||
|
||||
# 4. Pre-passes
|
||||
# 4. Pre-passes on full system
|
||||
all_residuals = substitution_pass(all_residuals, params)
|
||||
all_residuals = single_equation_pass(all_residuals, params)
|
||||
|
||||
# 5. Newton-Raphson (with BFGS fallback)
|
||||
converged = newton_solve(
|
||||
all_residuals,
|
||||
params,
|
||||
quat_groups=quat_groups,
|
||||
max_iter=100,
|
||||
tol=1e-10,
|
||||
)
|
||||
if not converged:
|
||||
converged = bfgs_solve(
|
||||
all_residuals,
|
||||
params,
|
||||
quat_groups=quat_groups,
|
||||
max_iter=200,
|
||||
tol=1e-10,
|
||||
)
|
||||
# 5. Solve (decomposed for large assemblies, monolithic for small)
|
||||
n_free_bodies = sum(1 for b in bodies.values() if not b.grounded)
|
||||
if n_free_bodies >= _DECOMPOSE_THRESHOLD:
|
||||
grounded_ids = {pid for pid, b in bodies.items() if b.grounded}
|
||||
clusters = decompose(ctx.constraints, grounded_ids)
|
||||
if len(clusters) > 1:
|
||||
converged = solve_decomposed(
|
||||
clusters,
|
||||
bodies,
|
||||
constraint_objs,
|
||||
constraint_indices,
|
||||
params,
|
||||
)
|
||||
else:
|
||||
converged = _monolithic_solve(
|
||||
all_residuals,
|
||||
params,
|
||||
quat_groups,
|
||||
)
|
||||
else:
|
||||
converged = _monolithic_solve(all_residuals, params, quat_groups)
|
||||
|
||||
# 6. DOF
|
||||
dof = count_dof(all_residuals, params)
|
||||
@@ -182,6 +193,26 @@ class KindredSolver(kcsolve.IKCSolver):
|
||||
return True
|
||||
|
||||
|
||||
def _monolithic_solve(all_residuals, params, quat_groups):
|
||||
"""Newton-Raphson solve with BFGS fallback on the full system."""
|
||||
converged = newton_solve(
|
||||
all_residuals,
|
||||
params,
|
||||
quat_groups=quat_groups,
|
||||
max_iter=100,
|
||||
tol=1e-10,
|
||||
)
|
||||
if not converged:
|
||||
converged = bfgs_solve(
|
||||
all_residuals,
|
||||
params,
|
||||
quat_groups=quat_groups,
|
||||
max_iter=200,
|
||||
tol=1e-10,
|
||||
)
|
||||
return converged
|
||||
|
||||
|
||||
def _build_constraint(
|
||||
kind,
|
||||
body_i,
|
||||
|
||||
Reference in New Issue
Block a user