extends Node3D
## Do both feet reach the SLOPE, at every FACING?
##
## A SCENE, NOT A `--script` GATE, AND THAT IS THE WHOLE POINT.
##
## The first version ran as a SceneTree script with the player on
## PROCESS_MODE_DISABLED, to stop _physics_process fighting the state it set. A
## disabled node is treated as PAUSED, and `move_and_slide()` on a paused body
## RETURNS WITHOUT MOVING. Measured: y stayed at 0.1500 and velocity at -4.0 for
## forty consecutive frames, zero slide collisions. The player never landed,
## is_grounded was false, FootIK returned at its first line — and the gate
## printed a full 32-row table of the ANIMATION POSE and called it foot
## placement. It even showed a yaw-periodic left/right asymmetry, which was
## stance geometry projected onto a slope and nothing to do with the IK.
##
## Run as a scene and the ordinary SceneTree does its job: physics moves the
## body, _process_local runs, and the engine drives the skeleton modifiers itself
## WITH its per-frame bone-pose reset — which also removes the pose-accumulation
## trap that manual _process_modification() calls forced earlier gates to work
## around.
##
## FACING IS SWEPT because every foot bug found so far has been facing-dependent,
## and a gate at one facing cannot see any of them.
##
## THE REFERENCE IS FLAT GROUND, measured by this gate: the foot BONE is not at
## the sole, and its height above the ground is a rig fact rather than a round
## number.
##
## Run:
## "$HYPER_TPS_GODOT" --headless res://scenes/test/foot_slope_gate.tscn
const SLOPES := [0.0, 15.0, 25.0, 35.0]
const YAWS := [0.0, 90.0]
const SETTLE := 45
const FOOT_TOL := 0.06 ## height error against the flat reference, metres
const ASYM_TOL := 0.08 ## left/right disagreement on a uniform slope
var _plan: Array = []
var _idx := 0
var _frames := 0
var _player: Node = null
var _ground: StaticBody3D = null
var _ref: Array = []
var _rows: Array = []
var _rig_failures := 0
var _measure_now := false
## Bone reads MUST happen inside Skeleton3D.skeleton_updated. Godot 4.6 restores
## the pre-modifier pose afterwards, so get_bone_global_pose() in _process
## returns the ANIMATION pose — proven with a two-bone skeleton and a modifier
## writing a known +0.25: _process read 1.0, the signal read 1.25
## (scenes/test/modifier_read_probe.tscn). That is why this gate produced
## byte-identical tables with foot IK switched OFF.
var _skel: Node = null
var _cap_foot := [Vector3.ZERO, Vector3.ZERO]
var _cap_stretch := 0.0
var _cap_hip := Vector3.ZERO
## Pelvis and chest orientation, so the trunk counter can be judged. Which axis
## is "up" on these bones is a rig fact, so both are compared against their own
## FLAT-ground orientation rather than against world up.
var _cap_pelvis_up := Vector3.UP
var _cap_chest_up := Vector3.UP
var _ref_pelvis_up := Vector3.UP
var _ref_chest_up := Vector3.UP
## The foot's own "up" axis, per foot. Which axis of a DEF-foot bone points out
## of the sole is a rig fact, so it is CALIBRATED on flat ground rather than
## assumed: whatever it is there, it should have rotated onto the slope normal by
## the same amount the ground did.
var _cap_up := [Vector3.UP, Vector3.UP]
var _ref_up := [Vector3.UP, Vector3.UP]
var _cap_ok := false
func _ready() -> void:
# After the player, so the facing this gate holds is the last word each frame.
process_priority = 100
for s in SLOPES:
for y in YAWS:
_plan.append({"slope": s, "yaw": y})
if s == 0.0:
break # the flat reference does not depend on facing
_build()
func _physics_process(_d: float) -> void:
if _player == null or _measure_now:
return
# Hold the authored facing. The controller turns the model toward velocity;
# this gate is asking what happens AT a given facing.
var cfg: Dictionary = _plan[_idx]
var m: Node3D = _player.get_node("Model")
m.global_transform.basis = Basis(Vector3.UP, deg_to_rad(float(cfg["yaw"])))
# --no-lean: the CONTROL. pelvis-dh has a large baseline that varies with
# slope and facing (it is pelvis vs FOOT midpoint), so the absolute number
# does not isolate the lean. The difference between these two runs does.
var fik0: Variant = null
var an0: Node = _find(_player, "ProceduralAnimator")
if an0 != null:
fik0 = an0.get("_foot_ik")
if fik0 != null:
for arg in OS.get_cmdline_user_args():
if arg == "--no-lean":
fik0.slope_lean = 0.0
if _skel == null:
_skel = _find_skel(_player)
if _skel != null and _skel.has_signal("skeleton_updated"):
_skel.skeleton_updated.connect(_on_skeleton_updated)
_frames += 1
if _frames >= SETTLE:
_measure_now = true
## The only place the modified pose is visible.
func _on_skeleton_updated() -> void:
if _skel == null:
return
var names := ["DEF-foot.L", "DEF-foot.R"]
for i in range(2):
var bi: int = _skel.find_bone(names[i])
if bi >= 0:
var bx: Transform3D = _skel.global_transform * _skel.get_bone_global_pose(bi)
_cap_foot[i] = bx.origin
_cap_up[i] = bx.basis.y.normalized()
# Leg-bone stretch: what distinguishes TRANSLATING a foot from SOLVING a leg.
var chain := [["DEF-thigh.L", "DEF-shin.L", "DEF-foot.L"],
["DEF-thigh.R", "DEF-shin.R", "DEF-foot.R"]]
var worst := 0.0
for i in range(2):
var bi := [_skel.find_bone(chain[i][0]), _skel.find_bone(chain[i][1]), _skel.find_bone(chain[i][2])]
if bi[0] < 0 or bi[1] < 0 or bi[2] < 0:
continue
for k in range(2):
var a: Vector3 = _skel.get_bone_global_pose(bi[k]).origin
var b: Vector3 = _skel.get_bone_global_pose(bi[k + 1]).origin
var ra: Vector3 = _skel.get_bone_global_rest(bi[k]).origin
var rb: Vector3 = _skel.get_bone_global_rest(bi[k + 1]).origin
var st: float = absf(a.distance_to(b) - ra.distance_to(rb))
if st > worst:
worst = st
# Where the pelvis actually sits, relative to the midpoint of the feet, in the
# DOWNHILL direction. Negative means it has leaned into the hill.
var hb: int = _skel.find_bone("DEF-spine")
if hb >= 0:
_cap_hip = (_skel.global_transform * _skel.get_bone_global_pose(hb)).origin
var pb: int = _skel.find_bone("DEF-spine")
if pb >= 0:
_cap_pelvis_up = (_skel.global_transform * _skel.get_bone_global_pose(pb)).basis.y.normalized()
var cb: int = _skel.find_bone("DEF-spine.003")
if cb >= 0:
_cap_chest_up = (_skel.global_transform * _skel.get_bone_global_pose(cb)).basis.y.normalized()
_cap_stretch = worst
_cap_ok = true
## Measured in _process, from what the signal captured this frame.
func _process(_d: float) -> void:
if not _measure_now:
return
_measure_now = false
_measure(_plan[_idx])
_teardown()
_idx += 1
if _idx >= _plan.size():
_report()
get_tree().quit(0 if _rows.size() > 0 and _rig_failures == 0 else 1)
return
_build()
func _build() -> void:
var cfg: Dictionary = _plan[_idx]
var s: float = tan(deg_to_rad(float(cfg["slope"])))
_ground = StaticBody3D.new()
var cs := CollisionShape3D.new()
var f := PackedVector3Array()
var i := 0
while i < 40:
var j := 0
while j < 40:
var x0 := float(i) * 1.0 - 20.0
var z0 := float(j) * 1.0 - 20.0
var x1 := x0 + 1.0
var z1 := z0 + 1.0
var y0 := x0 * s
var y1 := x1 * s
# CW, so the face normal points UP. [[feedback_godot-cw-winding]]
f.push_back(Vector3(x0, y0, z0)); f.push_back(Vector3(x1, y1, z1)); f.push_back(Vector3(x0, y0, z1))
f.push_back(Vector3(x0, y0, z0)); f.push_back(Vector3(x1, y1, z0)); f.push_back(Vector3(x1, y1, z1))
j += 1
i += 1
var tri := ConcavePolygonShape3D.new()
tri.set_faces(f)
cs.shape = tri
_ground.add_child(cs)
add_child(_ground)
var scene: PackedScene = load("res://scenes/player/player_procedural.tscn")
_player = scene.instantiate()
add_child(_player)
_player.global_position = Vector3(0.0, 0.6, 0.0)
_skel = null
_cap_ok = false
# INSTRUMENT CHECK. If the table is identical with foot IK switched OFF, then
# this gate is reading the pre-modifier pose and every number in it is the
# animation, not the IK.
for a in OS.get_cmdline_user_args():
if a == "--no-foot-ik":
_player.enable_foot_ik = false
_frames = 0
func _above_plane(p: Vector3, slope_deg: float) -> float:
var s: float = tan(deg_to_rad(slope_deg))
return (p.y - s * p.x) * cos(deg_to_rad(slope_deg))
func _measure(cfg: Dictionary) -> void:
var anim: Node = _find(_player, "ProceduralAnimator")
var skel: Node = _find_skel(_player)
if anim == null or skel == null:
print(" RIG FAILED: no animator/skeleton")
_rig_failures += 1
return
var fik: Variant = anim.get("_foot_ik")
var grounded: bool = _player.is_on_floor()
var any_hit := false
if fik != null:
var g: Array = fik.get("_ground_hit")
any_hit = bool(g[0]) or bool(g[1])
if not grounded or not any_hit:
# REFUSE TO MEASURE rather than report the animation pose as foot placement.
print(" RIG FAILED slope %.0f yaw %.0f: on_floor=%s hits=%s y=%.3f"
% [float(cfg["slope"]), float(cfg["yaw"]), str(grounded),
str(fik.get("_ground_hit")) if fik != null else "no FootIK",
_player.global_position.y])
_rig_failures += 1
return
var worst_stretch: float = _cap_stretch
var slope: float = float(cfg["slope"])
# WHAT THE TARGET IS MADE OF. The ankle target is
# foot_pos + local_up * (_offsets[i] - _hip_offset.y)
# and _offsets[i] is clamp((ground_point - foot_pos).dot(up) + GROUND_OFFSET).
# Printing the parts is the only way to see which one is wrong.
if false and fik != null:
print(" [target] offsets=%s hip_y=%.4f hit=%s foot=%s ground=%s n=%s"
% [str(fik.get("_offsets")), float((fik.get("_hip_offset") as Vector3).y),
str(fik.get("_ground_hit")),
str(fik.get("debug_foot_positions")),
str(fik.get("debug_ground_points")),
str(fik.get("debug_ground_normals"))])
if not _cap_ok:
print(" RIG FAILED slope %.0f yaw %.0f: skeleton_updated never fired"
% [slope, float(cfg["yaw"])])
_rig_failures += 1
return
var h := [_above_plane(_cap_foot[0], slope), _above_plane(_cap_foot[1], slope)]
if slope == 0.0:
_ref = [h[0], h[1]]
_ref_up = [_cap_up[0], _cap_up[1]]
_ref_pelvis_up = _cap_pelvis_up
_ref_chest_up = _cap_chest_up
print("flat reference: L %.4f R %.4f (foot bone height above the ground)"
% [_ref[0], _ref[1]])
return
if _ref.is_empty():
print(" RIG FAILED: no flat reference — the flat row did not measure.")
_rig_failures += 1
return
# CONFORM: has the foot rotated onto the slope the way it sits on the flat?
# 0 means it matches the surface as well as it matched flat ground; a value
# near the slope angle means the foot stayed horizontal and is not conforming
# at all.
var sn: Vector3 = Vector3(-tan(deg_to_rad(slope)), 1.0, 0.0).normalized()
# Downhill is the horizontal part of the surface normal.
var dh: Vector3 = (sn - Vector3.UP * sn.dot(Vector3.UP))
var lean_dh: float = 0.0
if dh.length_squared() > 1e-6:
var mid_f: Vector3 = ((_cap_foot[0] as Vector3) + (_cap_foot[1] as Vector3)) * 0.5
lean_dh = (_cap_hip - mid_f).dot(dh.normalized())
var conform := [0.0, 0.0]
for i in range(2):
var want: float = rad_to_deg((_ref_up[i] as Vector3).angle_to(Vector3.UP))
var got: float = rad_to_deg((_cap_up[i] as Vector3).angle_to(sn))
conform[i] = got - want
_rows.append({"slope": slope, "yaw": float(cfg["yaw"]), "cf": conform, "lean": lean_dh,
"pelvis_tilt": rad_to_deg(_cap_pelvis_up.angle_to(_ref_pelvis_up)),
"chest_tilt": rad_to_deg(_cap_chest_up.angle_to(_ref_chest_up)),
"el": h[0] - float(_ref[0]), "er": h[1] - float(_ref[1]),
"asym": absf((h[0] - float(_ref[0])) - (h[1] - float(_ref[1]))),
"stretch": worst_stretch})
func _teardown() -> void:
if _player != null:
_player.queue_free()
_player = null
if _ground != null:
_ground.queue_free()
_ground = null
func _report() -> void:
print("")
print("slope yaw L err R err L-R asym conform L/R pelvis-dh pelvisTilt chestTilt")
print("-------------------------------------------------------------------------")
var bad := 0
for r in _rows:
var ok: bool = absf(r["el"]) <= FOOT_TOL and absf(r["er"]) <= FOOT_TOL \
and r["asym"] <= ASYM_TOL
if not ok:
bad += 1
print("%5.0f %4.0f %+8.4f %+8.4f %7.4f %+6.1f %+6.1f %+8.4f %s"
% [r["slope"], r["yaw"], r["el"], r["er"], r["asym"],
r["cf"][0], r["cf"][1], r["lean"], "ok" if ok else "<< FAIL"])
print(" tilt vs flat: pelvis %5.1f deg, chest %5.1f deg"
% [r["pelvis_tilt"], r["chest_tilt"]])
print("")
var max_stretch := 0.0
for r in _rows:
if r["stretch"] > max_stretch:
max_stretch = r["stretch"]
print("tolerances: |err| <= %.2f m vs the FLAT reference, asym <= %.2f m"
% [FOOT_TOL, ASYM_TOL])
print("worst leg-bone stretch vs the rest pose: %.5f m" % max_stretch)
if _rig_failures > 0:
print("RIG FAILED in %d configurations — those measured nothing." % _rig_failures)
if bad == 0 and _rig_failures == 0:
print("PASS both feet reach the slope at every facing")
elif bad > 0:
print("FAIL %d of %d configurations" % [bad, _rows.size()])
func _find(n: Node, nm: String) -> Node:
if n.name == nm:
return n
for c in n.get_children():
var f: Node = _find(c, nm)
if f != null:
return f
return null
func _find_skel(n: Node) -> Node:
if n.get_class() == "Skeleton3D":
return n
for c in n.get_children():
var f: Node = _find_skel(c)
if f != null:
return f
return null