class_name TwoBoneIK extends RefCounted
## Shared two-bone IK solver for procedural animation.
## Extracted from ProceduralAnimator for reuse by creature systems.
## Two-bone IK: rotate upper and lower bones so the tip reaches target.
## target_pos is in skeleton-local space (NOT world space).
static func solve(skeleton: Skeleton3D, upper_idx: int, lower_idx: int,
tip_idx: int, target_pos: Vector3) -> void:
var upper_global: Transform3D = skeleton.get_bone_global_pose(upper_idx)
var lower_global: Transform3D = skeleton.get_bone_global_pose(lower_idx)
var tip_global: Transform3D = skeleton.get_bone_global_pose(tip_idx)
var upper_pos: Vector3 = upper_global.origin
var lower_pos: Vector3 = lower_global.origin
var tip_pos: Vector3 = tip_global.origin
var upper_len: float = upper_pos.distance_to(lower_pos)
var lower_len: float = lower_pos.distance_to(tip_pos)
var target_dist: float = upper_pos.distance_to(target_pos)
# Clamp target to reachable range
var max_reach: float = upper_len + lower_len - 0.01
if target_dist > max_reach:
target_pos = upper_pos + (target_pos - upper_pos).normalized() * max_reach
target_dist = max_reach
# Rotate upper bone to aim at target
var current_aim: Vector3 = (tip_pos - upper_pos).normalized()
var aim_dir: Vector3 = (target_pos - upper_pos).normalized()
if current_aim.length_squared() > 0.001 and aim_dir.length_squared() > 0.001:
var dot: float = current_aim.dot(aim_dir)
if dot < 0.9999:
var rot: Quaternion
if dot < -0.9999:
var fallback_axis: Vector3 = Vector3.UP.cross(current_aim)
if fallback_axis.length_squared() < 0.001:
fallback_axis = Vector3.RIGHT.cross(current_aim)
rot = Quaternion(fallback_axis.normalized(), PI)
else:
rot = Quaternion(current_aim, aim_dir)
# Convert world rotation to bone-local
var parent_idx: int = skeleton.get_bone_parent(upper_idx)
var upper_pose_rot: Quaternion = skeleton.get_bone_pose_rotation(upper_idx)
if parent_idx >= 0:
var parent_basis: Basis = skeleton.get_bone_global_pose(parent_idx).basis
var local_rot: Quaternion = (parent_basis.inverse() * Basis(rot) * parent_basis).get_rotation_quaternion()
skeleton.set_bone_pose_rotation(upper_idx, local_rot * upper_pose_rot)
else:
skeleton.set_bone_pose_rotation(upper_idx, rot * upper_pose_rot)
# Recompute after upper rotation
lower_global = skeleton.get_bone_global_pose(lower_idx)
tip_global = skeleton.get_bone_global_pose(tip_idx)
tip_pos = tip_global.origin
# Rotate lower bone to close remaining gap
var lower_current: Vector3 = (tip_pos - lower_global.origin).normalized()
var lower_aim: Vector3 = (target_pos - lower_global.origin).normalized()
if lower_current.length_squared() > 0.001 and lower_aim.length_squared() > 0.001:
var dot2: float = lower_current.dot(lower_aim)
if dot2 < 0.9999:
var rot2: Quaternion
if dot2 < -0.9999:
var fallback2: Vector3 = Vector3.UP.cross(lower_current)
if fallback2.length_squared() < 0.001:
fallback2 = Vector3.RIGHT.cross(lower_current)
rot2 = Quaternion(fallback2.normalized(), PI)
else:
rot2 = Quaternion(lower_current, lower_aim)
var parent_idx2: int = skeleton.get_bone_parent(lower_idx)
var lower_pose_rot: Quaternion = skeleton.get_bone_pose_rotation(lower_idx)
if parent_idx2 >= 0:
var parent_basis2: Basis = skeleton.get_bone_global_pose(parent_idx2).basis
var local_rot2: Quaternion = (parent_basis2.inverse() * Basis(rot2) * parent_basis2).get_rotation_quaternion()
skeleton.set_bone_pose_rotation(lower_idx, local_rot2 * lower_pose_rot)
else:
skeleton.set_bone_pose_rotation(lower_idx, rot2 * lower_pose_rot)