@tool
class_name FootIK extends RefCounted
## Overgrowth-style foot IK with swept-sphere collision, toe alignment,
## hip compensation (position + rotation), and per-foot ground normal.
##
## Based on DoFootIK() (line 15137) + DoHipIK() (line 15281) from aschar.as.
## Uses GetSweptSphereCollision(pos+(0,0.2,0), pos+(0,-0.6,0), 0.05)
## instead of thin raycasts for robust ground detection.
# Bone names (Rigify DEF- convention)
const LEFT_FOOT := "DEF-foot.L"
const RIGHT_FOOT := "DEF-foot.R"
const LEFT_TOE := "DEF-toe.L"
const RIGHT_TOE := "DEF-toe.R"
const LEFT_SHIN := "DEF-shin.L"
const RIGHT_SHIN := "DEF-shin.R"
const LEFT_THIGH := "DEF-thigh.L"
const RIGHT_THIGH := "DEF-thigh.R"
const HIPS := "DEF-spine"
# Swept-sphere config (Overgrowth: GetSweptSphereCollision line 15211)
const SPHERE_RADIUS: float = 0.05 ## Sweep sphere radius
const SWEEP_UP: float = 0.2 ## Start 0.2 above foot
const SWEEP_DOWN: float = 0.6 ## End 0.6 below foot
## aschar.as:15216 subtracts 0.01 from the ground height. GROUND_OFFSET was +0.02
## and was standing in for this; it is a different constant.
const GROUND_BIAS: float = 0.01
const GROUND_OFFSET: float = 0.02 ## still used by the toe sweep
const TOE_SWEEP_UP: float = 0.1
const TOE_SWEEP_DOWN: float = 0.3
# Balancing probes (Overgrowth uses 8 directions, line 15186)
const BALANCE_PROBE_COUNT: int = 8
const BALANCE_PROBE_RADIUS: float = 0.4
const BALANCE_PROBE_SPHERE: float = 0.05
# Smoothing (Overgrowth: pow(0.8, num_frames))
const OFFSET_SMOOTH_BASE: float = 0.2 ## Lower = smoother
const ROTATION_SMOOTH_BASE: float = 0.2
const HIP_OFFSET_SMOOTH_BASE: float = 0.05
const HIP_ROTATE_SMOOTH_BASE: float = 0.05
## THE TRUNK STAYS NEARER VERTICAL THAN THE PELVIS. Third of the three postural
## constraints on a slope: soles in contact, mass over the base of support, and
## head and trunk near vertical -- the vestibular system fights hard for a level
## head, so the compensation is spent in the spine.
##
## Not 1.0: the shoulders stay MORE level than the pelvis, not perfectly level.
## Real lateral spinal compensation is partial, and a fully cancelled trunk on a
## tilted pelvis reads as a mannequin.
##
## Applied as the inverse of _hip_rotation, which DoHipIK already produces as a
## WORLD-space quaternion -- so this needs no assumption about which way the
## rig's bone axes point, which is the trap that made the first wall-run
## counter-roll roll the head 115 degrees on a vertical body.
var trunk_counter: float = 0.6
## COM OVER THE BASE OF SUPPORT. Standing on a slope, gravity acts vertically but
## the support polygon is tilted, so a body centred between the feet has its mass
## sitting DOWNHILL of them. Real posture answers that by shifting into the hill:
## across a slope you lean uphill, facing uphill you lean forward, facing downhill
## you sit back. Without it a character reads as standing on flat ground that
## happens to be tilted -- every joint below the pelvis adapting and everything
## above it riding along.
##
## Expressed as a fraction of the horizontal distance the slope displaces you.
## 0 restores the old behaviour.
##
## This works because _hip_offset is a full displacement and the foot targets
## already subtract it, so the pelvis moves and the legs hold the feet where they
## were planted -- which is the whole point.
var slope_lean: float = 0.35
# Limits
## 0.4 was the ceiling on a TRANSLATED foot and became the binding constraint on
## a slope. The leg is SOLVED now and TwoBoneIK clamps to the limb's own reach,
## so this only has to be loose enough not to be the limit.
const MAX_FOOT_OFFSET: float = 0.9 ## Max ankle-target travel
const MAX_HIP_ROTATE: float = 0.2 ## Max hip tilt angle (radians)
# Per-foot state
var _offsets: Array = [0.0, 0.0] # [left, right]
var _rotations: Array = [Quaternion.IDENTITY, Quaternion.IDENTITY]
var _toe_rotations: Array = [Quaternion.IDENTITY, Quaternion.IDENTITY]
var _ground_hit: Array = [false, false]
## Whether the IK is DRIVING this foot at all. A foot with no contact is still
## driven while the swing clearance is lifting it, so this is a superset of
## _ground_hit -- and it, not _ground_hit, gates the leg solve.
var _ik_active: Array = [false, false]
## How much each foot is in SWING rather than stance, taken from the animated lift.
var _swing: Array = [0.0, 0.0]
## Footfall detection (on _swing): a foot is LIFTED above TOUCH_LIFTED, and touches
## down when it falls back under TOUCH_PLANTED while the body is grounded; then
## on_touchdown(foot) is called (0 left, 1 right).
## 0.12, not 0.3: measured at half input the walk cycle's LEFT foot only reaches a swing
## of 0.17-0.19 (the right 0.40; at full run 0.84 / 1.0) — 0.3 missed every left step.
const TOUCH_LIFTED := 0.12
const TOUCH_PLANTED := 0.05
var _lifted: Array = [false, false]
var on_touchdown: Callable = Callable()
# Debug visualization data (read externally to draw markers)
var debug_foot_positions: Array = [Vector3.ZERO, Vector3.ZERO] # [left, right] bone world pos
var debug_ground_points: Array = [Vector3.ZERO, Vector3.ZERO] # [left, right] sweep hit world pos
var debug_ground_normals: Array = [Vector3.UP, Vector3.UP] # [left, right] surface normal at hit
var debug_sweep_from: Array = [Vector3.ZERO, Vector3.ZERO] # sweep start
var debug_sweep_to: Array = [Vector3.ZERO, Vector3.ZERO] # sweep end
# Hip state
var _hip_offset: Vector3 = Vector3.ZERO
var _hip_rotation: Quaternion = Quaternion.IDENTITY
## SKI STANCE, 0 = normal, 1 = fully committed. Driven from the controller's
## ski_holding through the animator. Holding the ski key already drops traction to
## nothing; before this, that was the WHOLE feature -- the character slid with the
## pose of someone out for a jog, which is why skiing did not read on screen.
##
## Two postural changes, and they are the two a skier actually makes. The base of
## support widens, because the disturbing force is now sideways and the base has
## to span it; and the centre of mass drops over that wider base. The lean into a
## turn is NOT here -- the acceleration tilt already produces it (measured 21.4
## deg turning against 18.8 straight) and a second mechanism on the same quantity
## is how the wall-run facing got broken twice.
## How much of the planting term is applied. Lowered on a wall so the locomotion
## cycle is not flattened into the surface. 1.0 = the ground behaviour.
var plant_weight: float = 1.0
var ski_amount: float = 0.0
## Half-width added per foot, so the stance widens by twice this.
var ski_stance_width: float = 0.09
## THE CROUCH IS NOT IMPLEMENTED, deliberately. Dropping the pelvis by adding to
## target_hip_off reaches _hip_offset intact -- probed at -0.05 to -0.07 m -- and
## then arrives at the measured pelvis about fifty times smaller: 0.10 m of crouch
## moved it 0.0010 m, and 0.50 m moved it 0.0030 m. Something downstream is
## cancelling almost all of it and I have not found what. ski_stance_gate asserts
## the crouch and stays RED on it rather than carry code whose effect cannot be
## shown.
## GAIT. On a slope of angle t and stride L the ground rises L*tan(t) over one
## stride, so the swing foot has to clear ground coming UP to meet it. Without
## that, a climb reads as a walk on tilted ground.
##
## WHICH FOOT IS SWINGING COMES FROM THE ANIMATION, not from a cycle-phase guess:
## the pose has already lifted it. That is what Overgrowth's anim_pos.y is for.
var swing_clearance: float = 0.35
## FADE THE IK AS THE FOOT RISES (aschar.as:15242, unported until now):
## rotate_weight = max(0, 1 - (anim_pos.y - 0.05) * 4)
## A foot the animation has picked up must not be dragged back to the ground.
## Without this the planting fights every step and no swing height survives it.
var swing_fade: float = 4.0
## Clearance before any of it engages, so a standing character is untouched.
const SWING_DEADZONE: float = 0.05
# Leg length tracking (Overgrowth: target_leg_length, line 14145)
var _target_leg_lengths: Array = [0.0, 0.0]
## Rest height of each foot bone above the root, so "lifted" is measured against
## the rig's own standing pose rather than a guessed constant.
var _rest_foot_height: Array = [0.0, 0.0]
var _leg_lengths_cached: bool = false
## RID of the character body to exclude from raycasts.
## Set this from the player controller: foot_ik.exclude_rid = get_rid()
var exclude_rid: RID = RID()
## Apply foot IK. Call after pose is written to skeleton bones.
## horizontal_speed is used to reduce IK influence at high speeds.
## local_up overrides Vector3.UP for spherical gravity (default: world UP).
func apply(skeleton: Skeleton3D, delta: float, is_grounded: bool,
horizontal_speed: float = 0.0, local_up: Vector3 = Vector3.UP,
surface_normal: Vector3 = Vector3.UP) -> void:
if skeleton == null:
return
# Fade out when airborne
if not is_grounded:
var fade: float = 1.0 - pow(0.1, delta)
for i in range(2):
_offsets[i] = lerpf(_offsets[i], 0.0, fade)
_rotations[i] = _rotations[i].slerp(Quaternion.IDENTITY, fade)
_toe_rotations[i] = _toe_rotations[i].slerp(Quaternion.IDENTITY, fade)
_ground_hit[i] = false
_ik_active[i] = false
_hip_offset = _hip_offset.lerp(Vector3.ZERO, fade)
_hip_rotation = _hip_rotation.slerp(Quaternion.IDENTITY, fade)
return
var space: PhysicsDirectSpaceState3D = skeleton.get_world_3d().direct_space_state
if space == null:
return
var gx: Transform3D = skeleton.global_transform
# Cache leg lengths from rest pose on first run
if not _leg_lengths_cached:
_cache_leg_lengths(skeleton, gx)
# Reduce IK weight at high speeds (Overgrowth: weight *= 1.0 - roll_ik_fade)
var ik_weight: float = clampf(1.0 - horizontal_speed * 0.03, 0.3, 1.0)
# THE SLOPE COMES FROM THE BODY, NOT FROM THE SWING FOOT.
# The clearance term below asks "how fast is the ground rising to meet this
# foot", and the swing foot has no contact BY DEFINITION -- that is the whole
# point of a swing. Taking the slope from each foot's own sweep meant the term
# could only apply while the foot was still low enough not to need it, and it
# vanished entirely above SWEEP_DOWN (0.6 m), which is the peak of the swing.
# The body's contact normal knows the slope the whole time.
#
# surface_normal is the controller's smoothed ground_normal; its component
# perpendicular to local_up has length sin(slope).
var body_tilt: Vector3 = surface_normal - local_up * surface_normal.dot(local_up)
var body_slope_sin: float = clampf(body_tilt.length(), 0.0, 1.0)
var root_pos: Vector3 = gx.origin
## The BODY's up, which is what the animation lifts a foot along. Distinct from
## local_up, which is where the ground sweep points.
var body_up: Vector3 = gx.basis.y.normalized()
# ---- Process each foot ----
var foot_bones: Array = [LEFT_FOOT, RIGHT_FOOT]
var toe_bones: Array = [LEFT_TOE, RIGHT_TOE]
var raw_offsets: Array = [0.0, 0.0]
var raw_rotations: Array = [Quaternion.IDENTITY, Quaternion.IDENTITY]
var raw_toe_rotations: Array = [Quaternion.IDENTITY, Quaternion.IDENTITY]
for i in range(2):
var foot_idx: int = skeleton.find_bone(foot_bones[i])
if foot_idx < 0:
continue
var foot_global: Transform3D = gx * skeleton.get_bone_global_pose(foot_idx)
var foot_pos: Vector3 = foot_global.origin
debug_foot_positions[i] = foot_pos
# Swept-sphere ground detection (Overgrowth: GetSweptSphereCollision)
# Sweep a small sphere from above the foot to below it
debug_sweep_from[i] = foot_pos + local_up * SWEEP_UP
debug_sweep_to[i] = foot_pos - local_up * SWEEP_DOWN
var sweep_result: Dictionary = _swept_sphere_cast(space, foot_pos,
local_up * SWEEP_UP, -local_up * SWEEP_DOWN, SPHERE_RADIUS)
# How far the ANIMATION has picked this foot up, against the rig's own
# standing pose. Zero while planted, so a standing character is unaffected
# by everything below. Computed from the pose alone, so it survives a sweep
# that found nothing -- which is exactly the case the clearance is for.
# MEASURED ALONG THE BODY'S UP, NOT ALONG THE SWEEP DIRECTION.
# local_up is where the sweep points -- world up on the ground, the WALL
# NORMAL during a wall run. The animation lifts the foot along the BODY's
# vertical, which on a wall is perpendicular to that normal, so anim_h barely
# moved, `lift` stayed clamped at 0 and `swing` never left 0.000. Planting
# then ran at full weight on BOTH feet for the whole run.
#
# MEASURED in wall_run_gate: swing=[0.000, 0.000], hit=[true,true],
# offs=[-0.199,-0.199] constant -- both feet glued flat to the wall, and the
# foot travelling 0.13 m along the wall over a half cycle against a stride of
# 1.5-2 m. That is the reported "no leg movement": the run cycle is playing
# and FootIK is flattening it against the wall.
#
# _rest_foot_height is get_bone_global_rest().origin.y -- the rest foot along
# the SKELETON's own Y -- so gx.basis.y is the axis it was always meant to be
# compared against. On the ground the two coincide and nothing changes.
var anim_h: float = (foot_pos - root_pos).dot(body_up)
# THE DEAD ZONE IS PART OF THE FORMULA. Overgrowth fades on
# (anim_pos.y - 0.05), not on anim_pos.y, so nothing happens until the
# foot is clear by 5 cm. Without it an idle pose that sits a centimetre or
# two off the rest pose starts fading the planting on a STANDING
# character -- measured as a common-mode shift of the whole stance.
var lift: float = maxf(anim_h - float(_rest_foot_height[i]) - SWING_DEADZONE, 0.0)
var swing: float = clampf(lift * swing_fade, 0.0, 1.0)
_swing[i] = swing
# TOUCHDOWN: the swing coming back down — the footfall the footstep sound
# follows (gate_footstep_rhythm). Hysteresis, so a foot hovering at the
# threshold is one step, not a chatter of them.
if swing > TOUCH_LIFTED:
_lifted[i] = true
elif _lifted[i] and swing < TOUCH_PLANTED:
_lifted[i] = false
if is_grounded and on_touchdown.is_valid():
on_touchdown.call(i)
# On a slope the swing foot must clear ground that is RISING to meet it.
# Proportional to sin(slope) -- the steeper the hill, the more the ground
# has come up by the time the foot lands.
var clear: float = swing * swing_clearance * body_slope_sin
if sweep_result.is_empty():
# No contact under this foot. That is the NORMAL state at the top of a
# stride, not an error -- keep lifting it, and keep driving the leg, or
# the clearance is computed and then thrown away.
_ground_hit[i] = false
_ik_active[i] = clear > 0.0
raw_offsets[i] = clampf(clear, -MAX_FOOT_OFFSET, MAX_FOOT_OFFSET)
continue
_ground_hit[i] = true
_ik_active[i] = true
var ground_point: Vector3 = sweep_result["point"]
var ground_normal: Vector3 = sweep_result["normal"]
debug_ground_points[i] = ground_point
debug_ground_normals[i] = ground_normal
# ---- Foot height offset (aschar.as:15211-15229) ----
#
# THE PORT DROPPED A TERM AND IT IS THE WHOLE BUG. Overgrowth computes
#
# ground_height = sphere_col.position.y - 0.01
# new_pos.y = pos.y + (ground_height - pos.y)
# + anim_pos.y - 0.05 <-- MISSING HERE
# + foot_info_height[j]
#
# `anim_pos.y` is the ANIMATED foot height (GetUnmodifiedIKTransform), i.e.
# where the animation put the foot above the character root. Without it we
# drove the foot BONE onto the contact point -- but the bone origin is the
# ANKLE, which legitimately stands ~0.098 m above the sole. Measured on flat
# ground: foot bone at y=0.0977, contact at y=0.000, so the old formula asked
# for -0.0777 where the right answer is about -0.01. On a 35 degree slope that
# error pushed the uphill foot +0.108 m into the air.
#
# `anim_pos.y` CANCELS: the foot we measure IS at the animated position, so
# target - current = ground_height - sphere_radius, everything relative to the
# character root. No rig constant, nothing to calibrate.
#
# And `sphere_col.position` is the SPHERE CENTRE at contact, not the point on
# the collider -- our sweep returns the latter, so add the radius back along
# the surface normal.
var sphere_centre: Vector3 = ground_point + ground_normal * SPHERE_RADIUS
var ground_h: float = (sphere_centre - root_pos).dot(local_up) - GROUND_BIAS
# Planting fades out as the foot rises: the swing foot is the animation's to
# place, not the ground's.
var plant: float = (ground_h - SPHERE_RADIUS) * ik_weight * (1.0 - swing) * plant_weight
# WHEN THIS FOOT HAS REAL CONTACT, ITS OWN NORMAL BEATS THE BODY'S.
# The body normal is what makes the clearance survive a foot with no
# contact, but it is the controller's `ground_normal`, and that is smoothed
# 0.9/0.1 from an initial Vector3.UP -- MEASURED to take ~75 physics frames
# (1.25 s) to converge on a 35 degree slope, under-reading the whole way.
# After that it is exact: 0 wrong frames out of 276. So it is not unreliable
# in play, it is unreliable for the first second -- enough to cost 0.019 m of
# peak in a gate that starts sampling at frame 40, inside the warm-up.
# Contact if we have it, body normal only when we do not: the contact normal
# needs no warm-up and is the more local fact anyway.
var surf_tilt: Vector3 = ground_normal - local_up * ground_normal.dot(local_up)
var clear_here: float = swing * swing_clearance * clampf(surf_tilt.length(), 0.0, 1.0)
raw_offsets[i] = clampf(plant + clear_here, -MAX_FOOT_OFFSET, MAX_FOOT_OFFSET)
# Foot rotation to ground normal
if ground_normal.dot(local_up) < 0.999:
var axis: Vector3 = local_up.cross(ground_normal).normalized()
var angle: float = local_up.angle_to(ground_normal)
# Fades with the same signal: a foot in the air has no surface to conform
# to (aschar.as:15242's rotate_weight).
raw_rotations[i] = Quaternion(axis, angle * ik_weight * (1.0 - _swing[i]))
# ---- Toe swept-sphere ----
var toe_idx: int = skeleton.find_bone(toe_bones[i])
if toe_idx >= 0:
var toe_global: Transform3D = gx * skeleton.get_bone_global_pose(toe_idx)
var toe_pos: Vector3 = toe_global.origin
var toe_result: Dictionary = _swept_sphere_cast(space, toe_pos,
local_up * TOE_SWEEP_UP, -local_up * TOE_SWEEP_DOWN, SPHERE_RADIUS)
if not toe_result.is_empty():
var toe_normal: Vector3 = toe_result["normal"]
if toe_normal.dot(local_up) < 0.999:
var t_axis: Vector3 = local_up.cross(toe_normal).normalized()
var t_angle: float = local_up.angle_to(toe_normal)
raw_toe_rotations[i] = Quaternion(t_axis, t_angle * ik_weight * 0.7)
# ---- Smooth all values (Overgrowth: pow(0.8, num_frames)) ----
var frames: float = delta * 60.0
var pos_smooth: float = pow(OFFSET_SMOOTH_BASE, frames)
var rot_smooth: float = pow(ROTATION_SMOOTH_BASE, frames)
for i in range(2):
_offsets[i] = lerpf(raw_offsets[i], _offsets[i], pos_smooth)
_rotations[i] = raw_rotations[i].slerp(_rotations[i], rot_smooth)
_toe_rotations[i] = raw_toe_rotations[i].slerp(_toe_rotations[i], rot_smooth)
# ---- Hip IK: SOLVE the pelvis (Overgrowth DoHipIK, aschar.as:15281) ----
#
# Ours dropped the pelvis by `min(offsets) * 0.8` and rolled it about
# Vector3.FORWARD -- a scalar and a hardcoded WORLD axis. Overgrowth solves it:
# each HIP JOINT is pulled to target_leg_length from its own foot, twice, and
# the pair is then re-seated as a rigid segment keeping its original width and
# horizontal placement. That is what lets BOTH feet reach on a slope; a scalar
# drop cannot, which is why the downhill foot stayed 0.106 m short.
#
# The rotation axis comes from the hip line itself (cross of the original and
# solved hip vectors), not a world axis -- so it is correct at every facing.
# And the tilt limit FADES WITH SPEED (max(0, 0.2 - flat_speed * 0.2)): a
# pelvis that keeps tilting while you run reads as a limp.
var thigh_bones2: Array = [LEFT_THIGH, RIGHT_THIGH]
var target_hip_off: Vector3 = Vector3.ZERO
var target_hip_rot: Quaternion = Quaternion.IDENTITY
var hip_p: Array = [Vector3.ZERO, Vector3.ZERO]
var orig_hip: Array = [Vector3.ZERO, Vector3.ZERO]
var foot_target: Array = [Vector3.ZERO, Vector3.ZERO]
var hip_ok: bool = true
for i in range(2):
var t_i: int = skeleton.find_bone(thigh_bones2[i])
var f_i: int = skeleton.find_bone(foot_bones[i])
if t_i < 0 or f_i < 0 or _target_leg_lengths[i] <= 0.0:
hip_ok = false
break
hip_p[i] = (gx * skeleton.get_bone_global_pose(t_i)).origin
orig_hip[i] = hip_p[i]
foot_target[i] = (gx * skeleton.get_bone_global_pose(f_i)).origin + local_up * _offsets[i]
if hip_ok:
var orig_mid: Vector3 = (orig_hip[0] + orig_hip[1]) * 0.5
for _iter in range(2):
var hip_dist: float = hip_p[0].distance_to(hip_p[1])
for i in range(2):
var d: Vector3 = hip_p[i] - foot_target[i]
if d.length_squared() < 1e-8:
d = local_up
hip_p[i] = foot_target[i] + d.normalized() * float(_target_leg_lengths[i])
# Keep the ORIGINAL horizontal placement, take the SOLVED height. The
# source writes vec3(orig_mid.x, mid.y, orig_mid.z); this is that,
# expressed about local_up so it survives spherical gravity.
var mid: Vector3 = (hip_p[0] + hip_p[1]) * 0.5
mid = orig_mid - local_up * orig_mid.dot(local_up) + local_up * mid.dot(local_up)
var dir: Vector3 = (hip_p[1] - hip_p[0]) + (orig_hip[1] - orig_hip[0]) * 3.0
if dir.length_squared() < 1e-8:
dir = orig_hip[1] - orig_hip[0]
dir = dir.normalized()
hip_p[0] = mid - dir * hip_dist * 0.5
hip_p[1] = mid + dir * hip_dist * 0.5
target_hip_off = (hip_p[0] + hip_p[1]) * 0.5 - orig_mid
# ---- Lean into the hill ----
# The mean contact normal tilts away from vertical TOWARD the downhill
# side, so its horizontal part IS the downhill direction and its length is
# sin(slope). Shift the pelvis the other way, proportional to how far the
# feet are separated along the slope -- a steeper slope, or a wider
# stance, needs more.
if slope_lean > 0.0 and _ground_hit[0] and _ground_hit[1]:
var mean_n: Vector3 = (debug_ground_normals[0] + debug_ground_normals[1])
if mean_n.length_squared() > 1e-6:
mean_n = mean_n.normalized()
var downhill: Vector3 = mean_n - local_up * mean_n.dot(local_up)
var slope_sin: float = downhill.length()
if slope_sin > 0.001:
downhill = downhill.normalized()
# How far the stance actually spans down the slope: the
# height difference between the feet is what has to be
# balanced over, not the angle in the abstract.
var drop: float = absf((foot_target[0] - foot_target[1]).dot(local_up))
target_hip_off -= downhill * (drop * 0.5 + slope_sin * 0.25) * slope_lean
var ov: Vector3 = (orig_hip[1] - orig_hip[0])
var nv: Vector3 = (hip_p[1] - hip_p[0])
if ov.length_squared() > 1e-8 and nv.length_squared() > 1e-8:
ov = ov.normalized()
nv = nv.normalized()
var axis: Vector3 = ov.cross(nv)
if axis.length_squared() > 1e-10:
axis = axis.normalized()
var right_vec: Vector3 = ov.cross(axis)
var angle: float = atan2(-nv.dot(right_vec), nv.dot(ov))
var max_rot: float = maxf(0.0, MAX_HIP_ROTATE - horizontal_speed * 0.2)
target_hip_rot = Quaternion(axis, clampf(angle, -max_rot, max_rot) * ik_weight)
# Smooth hip
var hip_pos_smooth: float = pow(HIP_OFFSET_SMOOTH_BASE, frames)
var hip_rot_smooth: float = pow(HIP_ROTATE_SMOOTH_BASE, frames)
_hip_offset = target_hip_off.lerp(_hip_offset, hip_pos_smooth)
_hip_rotation = target_hip_rot.slerp(_hip_rotation, hip_rot_smooth)
# ---- Cross-foot compensation (Overgrowth line 15264) ----
# If only one foot hits ground, shift the other foot by that offset
for i in range(2):
var other: int = 1 - i
# ...but NOT one the swing clearance is driving. Copying the stance foot's
# planting offset onto a lifting foot is exactly the clearance being thrown
# away, one line after it was computed.
if _ground_hit[i] and not _ground_hit[other] and not _ik_active[other]:
_offsets[other] = _offsets[i]
# ---- Apply hip ----
var hip_idx: int = skeleton.find_bone(HIPS)
if hip_idx >= 0:
var hip_pos: Vector3 = skeleton.get_bone_pose_position(hip_idx)
# The solve produces a full displacement, not just a height.
var local_hip_offset: Vector3 = gx.basis.inverse() * _hip_offset
skeleton.set_bone_pose_position(hip_idx, hip_pos + local_hip_offset)
# Hip rotation
if not _hip_rotation.is_equal_approx(Quaternion.IDENTITY):
var hip_rot: Quaternion = skeleton.get_bone_pose_rotation(hip_idx)
var parent_idx: int = skeleton.get_bone_parent(hip_idx)
if parent_idx >= 0:
var parent_basis: Basis = (gx * skeleton.get_bone_global_pose(parent_idx)).basis
var local_hip_rot: Quaternion = (parent_basis.inverse() * Basis(_hip_rotation) * parent_basis).get_rotation_quaternion()
skeleton.set_bone_pose_rotation(hip_idx, local_hip_rot * hip_rot)
# ---- Trunk: counter the pelvis roll, distributed up the spine ----
# Distributed rather than hinged at one joint, for the same reason the wall-run
# counter-roll is: a single joint taking the whole angle is a broken back.
# Weights sum to 1, so the head receives the full counter through the chain.
if trunk_counter > 0.0 and not _hip_rotation.is_equal_approx(Quaternion.IDENTITY):
var counter: Quaternion = _hip_rotation.inverse()
var spine_chain: Array = ["DEF-spine.002", "DEF-spine.003",
"DEF-spine.005", "DEF-spine.006"]
var spine_w: Array = [0.30, 0.30, 0.20, 0.20]
for i in range(spine_chain.size()):
var q: Quaternion = Quaternion.IDENTITY.slerp(counter,
trunk_counter * float(spine_w[i]))
if not q.is_equal_approx(Quaternion.IDENTITY):
_apply_bone_rotation(skeleton, gx, spine_chain[i], q)
# ---- SOLVE each leg to REACH the ground ----
#
# This used to TRANSLATE the foot bone and then nudge the shin by 15% of the
# same offset to "keep the knee stable". Three things wrong with that: nothing
# made the ankle position correspond to a knee and hip that could reach it
# (measured leg-bone stretch up to 0.245 m at 45 degrees), the translation was
# converted through the SKELETON's basis rather than each bone's PARENT, and
# MAX_FOOT_OFFSET capped how far a foot could move at all.
#
# TwoBoneIK is the solver wall climbing already drives for its grips. It works
# in SKELETON-LOCAL space and rotates from the CURRENT aim toward the target,
# so it is a delta on the animated pose -- the knee keeps the bend direction
# the pose library gave it, which is why the absence of a pole vector is
# tolerable for a correction this size.
var thigh_bones: Array = [LEFT_THIGH, RIGHT_THIGH]
var gx_inv: Transform3D = gx.affine_inverse()
for i in range(2):
# _ik_active, NOT _ground_hit: a foot the clearance is lifting has no
# contact and still needs its leg solved to the lifted target.
if not _ik_active[i]:
continue
var thigh_idx: int = skeleton.find_bone(thigh_bones[i])
var shin_idx2: int = skeleton.find_bone(LEFT_SHIN if i == 0 else RIGHT_SHIN)
var foot_idx2: int = skeleton.find_bone(foot_bones[i])
if thigh_idx < 0 or shin_idx2 < 0 or foot_idx2 < 0:
continue
# Re-read the foot AFTER the hip moved: the leg reaches from wherever the
# pelvis ended up, so a target computed before the hip drop is stale.
var foot_now: Vector3 = (gx * skeleton.get_bone_global_pose(foot_idx2)).origin
# foot_now already carries the hip displacement, so remove it from the
# remaining travel rather than double-counting it.
# SKI STANCE WIDTH, along the line BETWEEN the feet rather than along any
# rig or model axis. The lateral direction is the one thing the two feet
# already agree on, so this needs no assumption about which way the rig's
# bones point -- the assumption that rolled the head 115 degrees the first
# time the counter-roll was written.
var widen: Vector3 = Vector3.ZERO
if ski_amount > 0.001:
var across: Vector3 = debug_foot_positions[0] - debug_foot_positions[1]
across -= local_up * across.dot(local_up)
if across.length_squared() > 1e-6:
var sign_i: float = 1.0 if i == 0 else -1.0
widen = across.normalized() * (ski_stance_width * ski_amount * sign_i)
var target_world: Vector3 = foot_now + local_up * _offsets[i] - _hip_offset + widen
TwoBoneIK.solve(skeleton, thigh_idx, shin_idx2, foot_idx2, gx_inv * target_world)
# ---- Foot and toe conform, AFTER the leg has been solved ----
# The solve rotates thigh and shin, moving the foot's parent frame, so a
# conform rotation applied before it would be expressed in a frame that no
# longer exists.
for i in range(2):
if not _ground_hit[i]:
continue
if not _rotations[i].is_equal_approx(Quaternion.IDENTITY):
_apply_bone_rotation(skeleton, gx, foot_bones[i], _rotations[i])
if not _toe_rotations[i].is_equal_approx(Quaternion.IDENTITY):
_apply_bone_rotation(skeleton, gx, toe_bones[i], _toe_rotations[i])
func _apply_bone_offset(skeleton: Skeleton3D, gx: Transform3D,
bone_name: String, offset: float, rotation: Quaternion,
local_up: Vector3 = Vector3.UP) -> void:
var idx: int = skeleton.find_bone(bone_name)
if idx < 0:
return
# Position
var local_offset: Vector3 = gx.basis.inverse() * (local_up * offset)
var pos: Vector3 = skeleton.get_bone_pose_position(idx)
skeleton.set_bone_pose_position(idx, pos + local_offset)
# Rotation
if not rotation.is_equal_approx(Quaternion.IDENTITY):
_apply_bone_rotation(skeleton, gx, bone_name, rotation)
func _apply_bone_rotation(skeleton: Skeleton3D, gx: Transform3D,
bone_name: String, rotation: Quaternion) -> void:
var idx: int = skeleton.find_bone(bone_name)
if idx < 0:
return
var parent_idx: int = skeleton.get_bone_parent(idx)
if parent_idx < 0:
return
var parent_basis: Basis = (gx * skeleton.get_bone_global_pose(parent_idx)).basis
var local_rot: Quaternion = (parent_basis.inverse() * Basis(rotation) * parent_basis).get_rotation_quaternion()
var current: Quaternion = skeleton.get_bone_pose_rotation(idx)
skeleton.set_bone_pose_rotation(idx, local_rot * current)
func _cache_leg_lengths(skeleton: Skeleton3D, gx: Transform3D) -> void:
var foot_bones: Array = [LEFT_FOOT, RIGHT_FOOT]
# THIGH to foot, not spine to foot. DoHipIK constrains each HIP JOINT to
# target_leg_length from its own foot, and the hip joints are the two thigh
# roots -- the spine bone is a single point between them and cannot express a
# per-leg constraint. This value was previously measured from the spine and
# then never read at all.
var thigh_bones: Array = [LEFT_THIGH, RIGHT_THIGH]
for i in range(2):
var t_idx: int = skeleton.find_bone(thigh_bones[i])
var foot_idx: int = skeleton.find_bone(foot_bones[i])
if t_idx >= 0 and foot_idx >= 0:
var t_pos: Vector3 = (gx * skeleton.get_bone_global_pose(t_idx)).origin
var f_pos: Vector3 = (gx * skeleton.get_bone_global_pose(foot_idx)).origin
_target_leg_lengths[i] = t_pos.distance_to(f_pos)
# From the REST pose, which is the rig standing on flat ground.
_rest_foot_height[i] = skeleton.get_bone_global_rest(foot_idx).origin.y
_leg_lengths_cached = true
## Swept-sphere collision: sweep a sphere from origin+up_offset to origin+down_offset.
## Delegates to SphereSweep shared utility.
func _swept_sphere_cast(space: PhysicsDirectSpaceState3D, origin: Vector3,
up_offset: Vector3, down_offset: Vector3, radius: float) -> Dictionary:
var from: Vector3 = origin + up_offset
var to: Vector3 = origin + down_offset
var excludes: Array[RID] = []
if exclude_rid.is_valid():
excludes.append(exclude_rid)
return SphereSweep.sweep(space, from, to, radius, excludes)