@tool
class_name ChainSolver extends RefCounted
## Overgrowth-style IK chain solvers.
## Takes key_transforms (7 intermediate transforms) and writes final bone poses.
##
## Based on DrawLeg (line 12279), DrawBody (line 13529), DrawHead (line 13580),
## DrawArms (line 12441) from aschar.as.
##
## Key concept: RotateBoneToMatchVec — given a bone, rotate it so that the
## vector from bone_start to bone_end matches a target direction.
## Solve leg chain: hip → knee → ankle → foot → toe.
## FABRIK-like: rotate initial chain toward IK target, scale to reach,
## then iteratively constrain bone lengths (2 iterations).
##
## hip_transform: key_transforms[kHipKey]
## foot_transform: key_transforms[kLeftLegKey] or [kRightLegKey]
static func solve_leg(skeleton: Skeleton3D, gx: Transform3D,
hip_transform: Transform3D, foot_transform: Transform3D,
hip_bone: String, thigh_bone: String, shin_bone: String,
foot_bone: String, toe_bone: String) -> void:
var hip_idx: int = skeleton.find_bone(hip_bone)
var thigh_idx: int = skeleton.find_bone(thigh_bone)
var shin_idx: int = skeleton.find_bone(shin_bone)
var foot_idx: int = skeleton.find_bone(foot_bone)
var toe_idx: int = skeleton.find_bone(toe_bone)
if hip_idx < 0 or thigh_idx < 0 or shin_idx < 0 or foot_idx < 0:
return
# Get current joint world positions from posed skeleton
var hip_pos: Vector3 = (gx * skeleton.get_bone_global_pose(hip_idx)).origin
var knee_pos: Vector3 = (gx * skeleton.get_bone_global_pose(thigh_idx)).origin
var ankle_pos: Vector3 = (gx * skeleton.get_bone_global_pose(shin_idx)).origin
var foot_base: Vector3 = (gx * skeleton.get_bone_global_pose(foot_idx)).origin
var foot_tip: Vector3 = foot_base
if toe_idx >= 0:
foot_tip = (gx * skeleton.get_bone_global_pose(toe_idx)).origin
# Bone lengths from current pose
var upper_leg_length: float = hip_pos.distance_to(knee_pos)
var lower_leg_length: float = knee_pos.distance_to(ankle_pos)
var upper_foot_length: float = ankle_pos.distance_to(foot_base)
# IK targets from key_transforms
var target_hip: Vector3 = hip_transform.origin
var target_foot: Vector3 = foot_transform.origin
# Save old state for rotation computation
var old_hip: Vector3 = hip_pos
var old_length: float = foot_base.distance_to(old_hip)
if old_length < 0.001:
return
# Step 1: Rotate entire chain to point toward IK target
var old_dir: Vector3 = (foot_base - old_hip).normalized()
var new_dir: Vector3 = (target_foot - target_hip).normalized()
var rotate: Quaternion = _rotation_between_vectors(old_dir, new_dir)
knee_pos = rotate * (knee_pos - old_hip) + target_hip
ankle_pos = rotate * (ankle_pos - old_hip) + target_hip
foot_base = rotate * (foot_base - old_hip) + target_hip
# Step 2: Scale to reach target distance
var new_length: float = target_foot.distance_to(target_hip)
var weight: float = new_length / old_length if old_length > 0.001 else 1.0
knee_pos = target_hip.lerp(knee_pos, weight)
ankle_pos = target_hip.lerp(ankle_pos, weight)
foot_base = target_hip.lerp(foot_base, weight)
# Step 3: FABRIK-like constraint iterations (Overgrowth uses 2)
for i in range(2):
# Constrain upper leg from hip
knee_pos = target_hip + (knee_pos - target_hip).normalized() * upper_leg_length
# Constrain foot bone from foot_base
ankle_pos = foot_base + (ankle_pos - foot_base).normalized() * upper_foot_length
# Constrain lower leg between knee and ankle
var knee_ankle_dir: Vector3 = (knee_pos - ankle_pos).normalized()
var mid: Vector3 = (knee_pos + ankle_pos) * 0.5
ankle_pos = mid - knee_ankle_dir * 0.5 * lower_leg_length
knee_pos = mid + knee_ankle_dir * 0.5 * lower_leg_length
# Step 4: Apply foot transform directly (sets foot orientation)
_set_bone_world_transform(skeleton, gx, foot_idx, foot_transform)
# Step 5: RotateBoneToMatchVec for each bone in chain
# Foot: foot_base → foot_tip
if toe_idx >= 0:
foot_tip = foot_transform * _get_bone_tail_offset(skeleton, toe_idx)
_rotate_bone_to_match_vec(skeleton, gx, foot_idx, foot_base, foot_tip)
# Shin: ankle → foot_base
_rotate_bone_to_match_vec(skeleton, gx, shin_idx, ankle_pos, foot_base)
# Thigh: knee → ankle
_rotate_bone_to_match_vec(skeleton, gx, thigh_idx, knee_pos, ankle_pos)
# Hip: hip → knee
_rotate_bone_to_match_vec(skeleton, gx, hip_idx, target_hip, knee_pos)
## Solve torso chain: hip → abdomen → chest.
## Sets chest and hip from key_transforms, interpolates abdomen at 50%.
##
## hip_transform: key_transforms[kHipKey]
## chest_transform: key_transforms[kChestKey]
static func solve_body(skeleton: Skeleton3D, gx: Transform3D,
hip_transform: Transform3D, chest_transform: Transform3D,
hip_bone: String, spine_bone: String, chest_bone: String) -> void:
var hip_idx: int = skeleton.find_bone(hip_bone)
var spine_idx: int = skeleton.find_bone(spine_bone)
var chest_idx: int = skeleton.find_bone(chest_bone)
if hip_idx < 0 or spine_idx < 0 or chest_idx < 0:
return
# Key points from transforms
var collarbone: Vector3 = chest_transform * _get_bone_tail_offset(skeleton, chest_idx)
var ribs: Vector3 = chest_transform.origin
var stomach: Vector3 = hip_transform * _get_bone_tail_offset(skeleton, spine_idx)
var hips: Vector3 = hip_transform.origin
# Set chest bone from chest_transform
_set_bone_world_transform(skeleton, gx, chest_idx, chest_transform)
# Abdomen is 50% blend between hip and chest (Overgrowth line 13559)
var abdomen_transform: Transform3D = hip_transform.interpolate_with(chest_transform, 0.5)
_set_bone_world_transform(skeleton, gx, spine_idx, abdomen_transform)
# RotateBoneToMatchVec for spine chain
_rotate_bone_to_match_vec(skeleton, gx, chest_idx, ribs, collarbone)
_rotate_bone_to_match_vec(skeleton, gx, spine_idx, stomach, ribs)
_rotate_bone_to_match_vec(skeleton, gx, hip_idx, hips, stomach)
## Solve head chain: neck → head.
## Applies breathing offset, rotates neck and head bones.
##
## chest_transform: key_transforms[kChestKey]
## head_transform: key_transforms[kHeadKey]
static func solve_head(skeleton: Skeleton3D, gx: Transform3D,
chest_transform: Transform3D, head_transform: Transform3D,
neck_bone: String, head_bone: String, breath_amount: float = 0.0) -> void:
var neck_idx: int = skeleton.find_bone(neck_bone)
var head_idx: int = skeleton.find_bone(head_bone)
if neck_idx < 0 or head_idx < 0:
return
# Key points
var crown: Vector3 = head_transform * _get_bone_tail_offset(skeleton, head_idx)
var skull: Vector3 = head_transform.origin
var neck: Vector3 = chest_transform * _get_bone_tail_offset(skeleton, neck_idx)
# Breathing offset (Overgrowth line 13607)
if absf(breath_amount) > 0.001:
var breathe_dir: Vector3 = chest_transform.basis * Vector3(0, -0.3, 1).normalized()
skull += breathe_dir * breath_amount * 0.005
crown += breathe_dir * breath_amount * 0.002
# Set head from head_transform
_set_bone_world_transform(skeleton, gx, head_idx, head_transform)
# RotateBoneToMatchVec
_rotate_bone_to_match_vec(skeleton, gx, neck_idx, neck, skull)
_rotate_bone_to_match_vec(skeleton, gx, head_idx, skull, crown)
## Solve arm chain: shoulder → elbow → wrist → hand.
## Same FABRIK approach as legs: rotate, scale, constrain.
##
## chest_transform: key_transforms[kChestKey]
## hand_transform: key_transforms[kLeftArmKey] or [kRightArmKey]
static func solve_arm(skeleton: Skeleton3D, gx: Transform3D,
chest_transform: Transform3D, hand_transform: Transform3D,
shoulder_bone: String, upper_arm_bone: String,
forearm_bone: String, hand_bone: String) -> void:
var shoulder_idx: int = skeleton.find_bone(shoulder_bone)
var upper_arm_idx: int = skeleton.find_bone(upper_arm_bone)
var forearm_idx: int = skeleton.find_bone(forearm_bone)
var hand_idx: int = skeleton.find_bone(hand_bone)
if shoulder_idx < 0 or upper_arm_idx < 0 or forearm_idx < 0 or hand_idx < 0:
return
# Current joint positions
var shoulder_pos: Vector3 = (gx * skeleton.get_bone_global_pose(shoulder_idx)).origin
var elbow_pos: Vector3 = (gx * skeleton.get_bone_global_pose(upper_arm_idx)).origin
var wrist_pos: Vector3 = (gx * skeleton.get_bone_global_pose(forearm_idx)).origin
var _hand_pos: Vector3 = (gx * skeleton.get_bone_global_pose(hand_idx)).origin
# Bone lengths
var upper_arm_length: float = shoulder_pos.distance_to(elbow_pos)
var lower_arm_length: float = elbow_pos.distance_to(wrist_pos)
# Chest-relative transform
var old_shoulder: Vector3 = shoulder_pos
var old_wrist: Vector3 = wrist_pos
# New positions from key transforms
var new_wrist: Vector3 = hand_transform.origin
# Shoulder moves with chest
var chest_bone_idx: int = skeleton.find_bone("DEF-spine.003")
if chest_bone_idx >= 0:
var old_chest: Transform3D = gx * skeleton.get_bone_global_pose(chest_bone_idx)
var rel_mat: Transform3D = chest_transform * old_chest.affine_inverse()
shoulder_pos = rel_mat * shoulder_pos
# Rotate arm toward new target
var old_dir: Vector3 = old_wrist - old_shoulder
var new_dir: Vector3 = new_wrist - shoulder_pos
if old_dir.length_squared() > 0.0001 and new_dir.length_squared() > 0.0001:
var rotate: Quaternion = _rotation_between_vectors(old_dir.normalized(), new_dir.normalized())
elbow_pos = shoulder_pos + rotate * (elbow_pos - old_shoulder)
# Scale to reach
var old_length: float = old_dir.length()
var new_length: float = new_dir.length()
if old_length > 0.001:
elbow_pos = shoulder_pos + (elbow_pos - shoulder_pos) * (new_length / old_length)
# FABRIK constraint iterations
for i in range(2):
var offset: Vector3 = Vector3.ZERO
offset += (shoulder_pos + (elbow_pos - shoulder_pos).normalized() * upper_arm_length) - elbow_pos
offset += (new_wrist + (elbow_pos - new_wrist).normalized() * lower_arm_length) - elbow_pos
elbow_pos += offset
# Write bones
_rotate_bone_to_match_vec(skeleton, gx, shoulder_idx, shoulder_pos, elbow_pos)
_rotate_bone_to_match_vec(skeleton, gx, upper_arm_idx, elbow_pos, new_wrist)
_set_bone_world_transform(skeleton, gx, hand_idx, hand_transform)
## DrawLeg for THIS rig (Rigify DEF bones, twist segments between joints).
##
## solve_leg above reads the joints one bone too high -- `knee` from the THIGH's
## origin (the hip socket), `ankle` from the shin's (the knee), `hip` from
## DEF-spine (the pelvis centre) -- and finishes by rotating the PELVIS toward the
## "knee", once per leg. That is why the chain solvers were switched off. Measured
## on the rig (2026-10-01): DEF-thigh origin = hip socket, DEF-shin origin = knee,
## DEF-foot origin = ankle, +Y = down the bone, with .001 twist segments between.
##
## Two-bone analytic solve: hip socket H (wherever the pelvis now puts it), target
## ankle T (foot IK's answer, captured before the pelvis moved), lengths from the
## pose, and the knee kept bending the way it already bends (the pole is the
## current knee's offset from the hip-ankle line). The thigh and shin are turned by
## their ACTUAL joint-to-joint vectors, not an assumed bone axis; the foot then
## gets foot IK's orientation back.
static func solve_leg_rigify(skeleton: Skeleton3D, gx: Transform3D,
thigh_bone: String, shin_bone: String, foot_bone: String,
target_foot: Transform3D) -> void:
var thigh: int = skeleton.find_bone(thigh_bone)
var shin: int = skeleton.find_bone(shin_bone)
var foot: int = skeleton.find_bone(foot_bone)
if thigh < 0 or shin < 0 or foot < 0:
return
var H: Vector3 = (gx * skeleton.get_bone_global_pose(thigh)).origin
var K: Vector3 = (gx * skeleton.get_bone_global_pose(shin)).origin
var A: Vector3 = (gx * skeleton.get_bone_global_pose(foot)).origin
var T: Vector3 = target_foot.origin
var l1: float = H.distance_to(K)
var l2: float = K.distance_to(A)
if l1 < 1e-4 or l2 < 1e-4:
return
var to_t: Vector3 = T - H
var d: float = to_t.length()
if d < 1e-4:
return
var dir: Vector3 = to_t / d
if d > l1 + l2 - 1e-4:
clamped += 1
d = clampf(d, absf(l1 - l2) + 1e-4, l1 + l2 - 1e-4)
# Pole: where the knee points now, off the hip-ankle line.
var ha: Vector3 = A - H
var pole: Vector3 = (K - H) - ha.normalized() * (K - H).dot(ha.normalized()) if ha.length_squared() > 1e-8 else Vector3.ZERO
pole -= dir * pole.dot(dir)
if pole.length_squared() < 1e-8:
pole = gx.basis.z - dir * gx.basis.z.dot(dir) # the rig faces +Z
pole = pole.normalized()
var a: float = (l1 * l1 + d * d - l2 * l2) / (2.0 * d)
var h: float = sqrt(maxf(l1 * l1 - a * a, 0.0))
var K2: Vector3 = H + dir * a + pole * h
var T2: Vector3 = H + dir * d
_turn_bone(skeleton, gx, thigh, K - H, K2 - H)
var A_now: Vector3 = (gx * skeleton.get_bone_global_pose(foot)).origin
var K_now: Vector3 = (gx * skeleton.get_bone_global_pose(shin)).origin
_turn_bone(skeleton, gx, shin, A_now - K_now, T2 - K_now)
# Foot IK's orientation, unchanged by the leg turning above it.
var par: int = skeleton.get_bone_parent(foot)
var pb: Basis = (gx * skeleton.get_bone_global_pose(par)).basis if par >= 0 else gx.basis
skeleton.set_bone_pose_rotation(foot, (pb.inverse() * target_foot.basis).get_rotation_quaternion())
## Overgrowth's DoHipIK, for what the sway adds: how far the pelvis must come DOWN
## (along `up`) for this leg's ankle to be within reach of `target_foot`. The idle
## legs are nearly straight (hip socket to ankle ~= thigh + shin), so a sway of a
## few mm up or out put the ankle out of reach and the clamped solve dragged the
## foot (gate_stand_wobble: 348 out-of-reach solves, feet 4-8 mm). Solves
## |T - (H - up*drop)| = reach exactly; 0 when already in reach.
static func hip_drop_for_reach(skeleton: Skeleton3D, gx: Transform3D,
thigh_bone: String, shin_bone: String, foot_bone: String,
target_foot: Transform3D, up: Vector3) -> float:
var thigh: int = skeleton.find_bone(thigh_bone)
var shin: int = skeleton.find_bone(shin_bone)
var foot: int = skeleton.find_bone(foot_bone)
if thigh < 0 or shin < 0 or foot < 0:
return 0.0
var H: Vector3 = (gx * skeleton.get_bone_global_pose(thigh)).origin
var K: Vector3 = (gx * skeleton.get_bone_global_pose(shin)).origin
var A: Vector3 = (gx * skeleton.get_bone_global_pose(foot)).origin
var reach: float = H.distance_to(K) + K.distance_to(A) - 0.002
var v: Vector3 = target_foot.origin - H
if v.length() <= reach:
return 0.0
var vu: float = v.dot(up)
var disc: float = vu * vu - v.length_squared() + reach * reach
if disc < 0.0:
return 0.0 # too far SIDEWAYS: lowering cannot fix it
return maxf(0.0, -vu - sqrt(disc))
## Rotate a bone (in world space) by the rotation taking `from` onto `to`.
static func _turn_bone(skeleton: Skeleton3D, gx: Transform3D, bone: int,
from: Vector3, to: Vector3) -> void:
if from.length_squared() < 1e-10 or to.length_squared() < 1e-10:
return
# The EXACT arc, NOT _rotation_between_vectors: that returns IDENTITY whenever
# dot > 0.9999, i.e. it silently skips any turn under ~0.81 degrees -- up to
# 4.4 mm at the end of a 0.31 m shin, and the sway's corrections are exactly
# that small (measured: knee 1.0 mm and ankle 3.4 mm off the solved points).
var f: Vector3 = from.normalized()
var t: Vector3 = to.normalized()
if f.dot(t) < -0.9999:
return # a 180 degree flip is not a leg correction
var q := Quaternion(f, t)
var par: int = skeleton.get_bone_parent(bone)
var pb: Basis = (gx * skeleton.get_bone_global_pose(par)).basis if par >= 0 else gx.basis
var local: Quaternion = (pb.inverse() * Basis(q) * pb).get_rotation_quaternion()
skeleton.set_bone_pose_rotation(bone, local * skeleton.get_bone_pose_rotation(bone))
## DIAGNOSTIC: solves whose target ankle was out of reach (leg too short).
static var clamped: int = 0
# ---- Helpers ----
## Quaternion that rotates vector a to vector b.
static func _rotation_between_vectors(a: Vector3, b: Vector3) -> Quaternion:
var dot: float = a.dot(b)
if dot > 0.9999:
return Quaternion.IDENTITY
if dot < -0.9999:
# 180 degree rotation around any perpendicular axis
var perp: Vector3 = Vector3.UP.cross(a)
if perp.length_squared() < 0.001:
perp = Vector3.RIGHT.cross(a)
return Quaternion(perp.normalized(), PI)
var axis: Vector3 = a.cross(b).normalized()
var angle: float = a.angle_to(b)
return Quaternion(axis, angle)
## RotateBoneToMatchVec: rotate a bone so that it points from start to end.
## This is the core operation in Overgrowth's chain solvers.
static func _rotate_bone_to_match_vec(skeleton: Skeleton3D, gx: Transform3D,
bone_idx: int, start: Vector3, end: Vector3) -> void:
var target_dir: Vector3 = (end - start).normalized()
if target_dir.length_squared() < 0.001:
return
# Get current bone direction (from bone origin to its tail in world space)
var bone_global: Transform3D = gx * skeleton.get_bone_global_pose(bone_idx)
var current_dir: Vector3 = bone_global.basis.y.normalized() # Rigify: Y axis = bone direction
# Compute rotation to align current direction with target
var rotate: Quaternion = _rotation_between_vectors(current_dir, target_dir)
if rotate.is_equal_approx(Quaternion.IDENTITY):
return
# Apply rotation in bone's parent space
var parent_idx: int = skeleton.get_bone_parent(bone_idx)
if parent_idx < 0:
var root_rot: Quaternion = skeleton.get_bone_pose_rotation(bone_idx)
skeleton.set_bone_pose_rotation(bone_idx, rotate * root_rot)
return
var parent_global: Transform3D = gx * skeleton.get_bone_global_pose(parent_idx)
var local_rot: Quaternion = (parent_global.basis.inverse() * Basis(rotate) * parent_global.basis).get_rotation_quaternion()
var current_rot: Quaternion = skeleton.get_bone_pose_rotation(bone_idx)
skeleton.set_bone_pose_rotation(bone_idx, local_rot * current_rot)
# Also update position to match start point
var current_pos: Vector3 = (gx * skeleton.get_bone_global_pose(bone_idx)).origin
var pos_offset: Vector3 = start - current_pos
if pos_offset.length_squared() > 0.0001:
var local_offset: Vector3 = gx.basis.inverse() * pos_offset
var bone_pos: Vector3 = skeleton.get_bone_pose_position(bone_idx)
skeleton.set_bone_pose_position(bone_idx, bone_pos + local_offset)
## Set a bone's world-space transform by converting to local pose space.
static func _set_bone_world_transform(skeleton: Skeleton3D, gx: Transform3D,
bone_idx: int, world_transform: Transform3D) -> void:
# Convert world transform to skeleton-local
var skel_local: Transform3D = gx.affine_inverse() * world_transform
# Convert to parent-relative
var parent_idx: int = skeleton.get_bone_parent(bone_idx)
var pose_transform: Transform3D
if parent_idx >= 0:
var parent_global: Transform3D = skeleton.get_bone_global_pose(parent_idx)
pose_transform = parent_global.affine_inverse() * skel_local
else:
pose_transform = skel_local
skeleton.set_bone_pose_position(bone_idx, pose_transform.origin)
skeleton.set_bone_pose_rotation(bone_idx, pose_transform.basis.get_rotation_quaternion())
## Get a bone's tail offset (position of child bone relative to this bone).
## Approximated from rest pose child position.
static func _get_bone_tail_offset(skeleton: Skeleton3D, bone_idx: int) -> Vector3:
# Find first child bone
for i in range(skeleton.get_bone_count()):
if skeleton.get_bone_parent(i) == bone_idx:
return skeleton.get_bone_rest(i).origin
# No children — use Y axis scaled by a default bone length
var rest: Transform3D = skeleton.get_bone_rest(bone_idx)
return rest.basis.y * 0.1