ryan
db4ff8d939
- Moved the rig scene from `res://master_rig.tscn` to `res://scenes/stickman_rig.tscn`, preserving its UID. - Split the `StickmanRig` controller into three components: `StickmanLocomotion`, `StickmanRagdoll`, and `StickmanActionRunner`. - Updated the architecture to ensure `StickmanRig` acts as the public facade for the new components. - Created a new test suite `test_phase8_rig_modularization.gd` to verify the integrity of the relocated rig and its components. - Added checks for scene loading, component resolution, action queue delegation, and state authority.
764 lines
35 KiB
GDScript
764 lines
35 KiB
GDScript
class_name StickmanLocomotion
|
|
extends Node
|
|
## Owns navigation, walking/gait, terrain following, animated falls, and landings.
|
|
## StickmanRig schedules this component and exposes its public facade.
|
|
|
|
enum TerrainMotion { DISABLED, AIRBORNE, GROUNDED, LANDING_IMPACT }
|
|
|
|
const TERRAIN_PROBE_UP: float = 96.0
|
|
const TERRAIN_PROBE_DOWN: float = 700.0
|
|
const TERRAIN_NORMAL_MIN_Y: float = -0.15
|
|
const TERRAIN_SUPPORT_RANGE: float = 160.0
|
|
const TERRAIN_MAX_FOOT_SURFACE_DROP: float = 72.0
|
|
const TERRAIN_FOOT_GUARD_HEIGHT: float = 24.0
|
|
const TERRAIN_CENTER_PROBE_OFFSETS: Array[float] = [-24.0, 0.0, 24.0]
|
|
const TERRAIN_FOOT_PROBE_OFFSETS: Array[float] = [-4.0, 0.0, 4.0]
|
|
const TERRAIN_GRAVITY: float = 1400.0
|
|
const TERRAIN_MAX_FALL_SPEED: float = 1800.0
|
|
const RECOVERY_PROBE_START_OFFSET: float = 50.0
|
|
const RECOVERY_PROBE_END_OFFSET: float = 1500.0
|
|
const RECOVERY_PROBE_TOLERANCE: float = 200.0
|
|
const RECOVERY_PROBE_X_OFFSETS: Array[float] = [0.0, 25.0, -25.0, 50.0, -50.0, 100.0, -100.0]
|
|
const AIR_DRAG_X: float = 0.75
|
|
const AIR_POSE_START_SPEED: float = 240.0
|
|
const AIR_POSE_FULL_SPEED: float = 1200.0
|
|
const AIR_POSE_RESPONSE: float = 10.0
|
|
const AIR_HAND_FORWARD: float = 220.0
|
|
const AIR_HAND_FORWARD_START_SPEED: float = 80.0
|
|
const AIR_HAND_FORWARD_FULL_SPEED: float = 520.0
|
|
const AIR_HAND_LIFT: float = 680.0
|
|
const AIR_HAND_FLAIL_AMPLITUDE: Vector2 = Vector2(20.0, 16.0)
|
|
const AIR_FOOT_FLAIL_AMPLITUDE: Vector2 = Vector2(14.0, 10.0)
|
|
const AIR_FLAIL_HZ: float = 12.0
|
|
const HARD_LANDING_MIN_FALL_HEIGHT: float = 80.0
|
|
const HARD_LANDING_MAX_FALL_HEIGHT: float = 900.0
|
|
const LANDING_COMPRESSION_MIN: float = 32.0
|
|
const LANDING_COMPRESSION_MAX: float = 160.0
|
|
const LANDING_HAND_DROP_MIN: float = 270.0
|
|
const LANDING_HAND_DROP_MAX: float = 520.0
|
|
const LANDING_COMPRESS_DURATION: float = 0.08
|
|
const LANDING_RECOVER_DURATION: float = 0.22
|
|
const PELVIS_MAX_OFFSET: float = 240.0
|
|
const PELVIS_RESPONSE: float = 10.0
|
|
const FOOT_OFFSET: Vector2 = Vector2(0.0, -385.0)
|
|
const NAV_AGENT_LOCAL_POS: Vector2 = Vector2(0.0, 385.0)
|
|
const ARRIVE_DISTANCE: float = 8.0
|
|
const NAV_PATH_DESIRED_DISTANCE: float = 8.0
|
|
const NAV_TARGET_DESIRED_DISTANCE: float = 12.0
|
|
const LATCH_PROBE_MAX_FRAMES: int = 6
|
|
const WALK_CYCLE_DURATION: float = 0.8
|
|
|
|
const HEAD_COLLISION_RADIUS: float = 90.0
|
|
const TORSO_COLLISION_RADIUS: float = 20.0
|
|
const ARM_COLLISION_RADIUS: float = 12.0
|
|
const LEG_COLLISION_RADIUS: float = 14.0
|
|
const HEAD_COLLISION_FALLBACK_CENTER: Vector2 = Vector2(0.0, -72.0)
|
|
const COLLISION_LAYER: int = 1
|
|
const COLLISION_PARTS: Dictionary = {
|
|
"head": {"visual": "Head", "bone": "Torso/Head", "shape": "circle", "radius": HEAD_COLLISION_RADIUS},
|
|
"torso": {"visual": "Body", "bone": "Torso", "shape": "capsule", "radius": TORSO_COLLISION_RADIUS},
|
|
"left_upper_arm": {"visual": "LeftUpperArm", "bone": "Torso/LeftUpperArm", "shape": "capsule", "radius": ARM_COLLISION_RADIUS},
|
|
"left_lower_arm": {"visual": "LeftLowerArm", "bone": "Torso/LeftUpperArm/LeftLowerArm", "shape": "capsule", "radius": ARM_COLLISION_RADIUS},
|
|
"right_upper_arm": {"visual": "RightUpperArm", "bone": "Torso/RightUpperArm", "shape": "capsule", "radius": ARM_COLLISION_RADIUS},
|
|
"right_lower_arm": {"visual": "RightLowerArm", "bone": "Torso/RightUpperArm/RightLowerArm", "shape": "capsule", "radius": ARM_COLLISION_RADIUS},
|
|
"left_upper_leg": {"visual": "LeftUpperLeg", "bone": "Torso/LeftUpperLeg", "shape": "capsule", "radius": LEG_COLLISION_RADIUS},
|
|
"left_lower_leg": {"visual": "LeftLowerLeg", "bone": "Torso/LeftUpperLeg/LeftLowerLeg", "shape": "capsule", "radius": LEG_COLLISION_RADIUS},
|
|
"right_upper_leg": {"visual": "RightUpperLeg", "bone": "Torso/RightUpperLeg", "shape": "capsule", "radius": LEG_COLLISION_RADIUS},
|
|
"right_lower_leg": {"visual": "RightLowerLeg", "bone": "Torso/RightUpperLeg/RightLowerLeg", "shape": "capsule", "radius": LEG_COLLISION_RADIUS},
|
|
}
|
|
const REACTION_BASE_ANGLE: float = 0.35
|
|
const REACTION_REFERENCE_SPEED: float = 400.0
|
|
const REACTION_MAX_ANGLE: float = 0.6
|
|
const REACTION_LOCAL_X_EPSILON: float = 0.05
|
|
const REACTION_TARGETS: Dictionary = {
|
|
"head": [{"joint": "Head", "weight": 1.0, "decay": 0.50}],
|
|
"torso": [{"joint": "Torso", "weight": 0.5, "decay": 0.40}],
|
|
"left_upper_arm": [{"joint": "LeftUpperArm", "weight": 1.0, "decay": 0.40}],
|
|
"left_lower_arm": [{"joint": "LeftLowerArm", "weight": 1.0, "decay": 0.35}, {"joint": "LeftUpperArm", "weight": 0.4, "decay": 0.25}],
|
|
"right_upper_arm": [{"joint": "RightUpperArm", "weight": 1.0, "decay": 0.40}],
|
|
"right_lower_arm": [{"joint": "RightLowerArm", "weight": 1.0, "decay": 0.35}, {"joint": "RightUpperArm", "weight": 0.4, "decay": 0.25}],
|
|
"left_upper_leg": [{"joint": "LeftUpperLeg", "weight": 1.0, "decay": 0.40}],
|
|
"left_lower_leg": [{"joint": "LeftLowerLeg", "weight": 1.0, "decay": 0.35}, {"joint": "LeftUpperLeg", "weight": 0.4, "decay": 0.25}],
|
|
"right_upper_leg": [{"joint": "RightUpperLeg", "weight": 1.0, "decay": 0.40}],
|
|
"right_lower_leg": [{"joint": "RightLowerLeg", "weight": 1.0, "decay": 0.35}, {"joint": "RightUpperLeg", "weight": 0.4, "decay": 0.25}],
|
|
}
|
|
|
|
signal arrived(target: Vector2)
|
|
|
|
var _rig: StickmanRig = null
|
|
var foot_contacts: Dictionary = {"Left_Leg": {"valid": false, "locked": false}, "Right_Leg": {"valid": false, "locked": false}}
|
|
var terrain_pelvis_offset: float = 0.0
|
|
var terrain_air_velocity: Vector2 = Vector2.ZERO
|
|
var terrain_motion: TerrainMotion = TerrainMotion.DISABLED
|
|
var terrain_following_enabled: bool = false
|
|
var _air_time: float = 0.0
|
|
var air_pose_weight: float = 0.0
|
|
var air_pose_base: Dictionary = {}
|
|
var _air_entry_root_y: float = 0.0
|
|
var air_fall_distance: float = 0.0
|
|
var _landing_elapsed: float = 0.0
|
|
var landing_compression: float = 0.0
|
|
var _landing_hand_drop: float = 0.0
|
|
var _landing_torso_base: Vector2 = Vector2.ZERO
|
|
var _landing_hand_start: Dictionary = {}
|
|
var _landing_contacts: Dictionary = {}
|
|
var _terrain_paused_walk_animation: bool = false
|
|
var _nav_agent: NavigationAgent2D = null
|
|
var _nav_anchor: Node2D = null
|
|
var _walking: bool = false
|
|
var _walk_target_feet: Vector2 = Vector2.ZERO
|
|
var _walk_speed_current: float = 300.0
|
|
var _walk_mode: String = "nav"
|
|
var _walk_mode_latched: bool = false
|
|
var _walk_latch_probe_frames: int = 0
|
|
var _walk_settle_frames: int = 0
|
|
var _walk_done: bool = false
|
|
var _leg_base_markers: Dictionary = {}
|
|
var _last_grounded_walk_velocity_x: float = 0.0
|
|
var collision_bodies: Dictionary = {}
|
|
var collision_lengths: Dictionary = {}
|
|
var _collision_visuals: Dictionary = {}
|
|
var reaction_bones: Dictionary = {}
|
|
var impact_reactions: Dictionary = {}
|
|
|
|
func configure(rig: StickmanRig) -> void:
|
|
_rig = rig
|
|
|
|
for leg_name: String in ["Left_Leg", "Right_Leg"]:
|
|
var base: Marker2D = _rig.get_node_or_null(NodePath("AnimationTargets/%s_Base" % leg_name)) as Marker2D
|
|
if base == null:
|
|
push_warning("StickmanLocomotion: missing AnimationTargets/%s_Base." % leg_name)
|
|
else:
|
|
_leg_base_markers[leg_name] = base
|
|
_nav_anchor = Node2D.new()
|
|
_nav_anchor.name = "NavigationAgentAnchor"
|
|
_nav_anchor.position = NAV_AGENT_LOCAL_POS
|
|
_rig.add_child(_nav_anchor)
|
|
_nav_agent = NavigationAgent2D.new()
|
|
_nav_agent.name = "NavigationAgent2D"
|
|
_nav_agent.path_desired_distance = NAV_PATH_DESIRED_DISTANCE
|
|
_nav_agent.target_desired_distance = NAV_TARGET_DESIRED_DISTANCE
|
|
_nav_agent.path_max_distance = 100.0
|
|
_nav_agent.max_speed = _rig.walk_speed
|
|
_nav_agent.avoidance_enabled = false
|
|
_nav_anchor.add_child(_nav_agent)
|
|
|
|
|
|
func setup_collision_bodies() -> void:
|
|
collision_bodies.clear()
|
|
collision_lengths.clear()
|
|
_collision_visuals.clear()
|
|
reaction_bones.clear()
|
|
if _rig == null or _rig._body_container == null or _rig._skeleton == null:
|
|
return
|
|
for key: String in COLLISION_PARTS:
|
|
var definition: Dictionary = COLLISION_PARTS[key]
|
|
var bone: Bone2D = _rig._skeleton.get_node_or_null(NodePath(str(definition["bone"]))) as Bone2D
|
|
if bone == null or not is_instance_valid(bone):
|
|
push_warning("StickmanLocomotion: missing reaction bone '%s'." % str(definition["bone"]))
|
|
continue
|
|
var joint_name: String = "Torso" if str(definition["visual"]) == "Body" else str(definition["visual"])
|
|
reaction_bones[joint_name] = bone
|
|
var visual: Node2D = _rig._body_container.get_node_or_null(NodePath(str(definition["visual"]))) as Node2D
|
|
if visual == null:
|
|
push_warning("StickmanLocomotion: missing collision visual for '%s'." % key)
|
|
continue
|
|
var proxy := AnimatableBody2D.new()
|
|
proxy.name = "CollisionBody"
|
|
proxy.collision_layer = COLLISION_LAYER
|
|
proxy.collision_mask = COLLISION_LAYER
|
|
proxy.sync_to_physics = false
|
|
proxy.set_meta("stickman_rig", _rig)
|
|
proxy.set_meta("part_name", key)
|
|
var shape_node := CollisionShape2D.new()
|
|
shape_node.name = "CollisionShape2D"
|
|
if str(definition["shape"]) == "circle":
|
|
var circle := CircleShape2D.new()
|
|
var bounds: Dictionary = mounted_visual_bounds_local(visual)
|
|
if bool(bounds["valid"]):
|
|
var rect: Rect2 = bounds["rect"] as Rect2
|
|
shape_node.position = rect.get_center()
|
|
circle.radius = maxf(float(definition["radius"]), maxf(rect.size.x, rect.size.y) * 0.5)
|
|
else:
|
|
shape_node.position = HEAD_COLLISION_FALLBACK_CENTER
|
|
circle.radius = float(definition["radius"])
|
|
shape_node.shape = circle
|
|
else:
|
|
var length: float = absf(_rig._head_bone.position.y) if key == "torso" and _rig._head_bone != null else bone.length
|
|
collision_lengths[key] = length
|
|
var capsule := CapsuleShape2D.new()
|
|
capsule.height = length
|
|
capsule.radius = float(definition["radius"])
|
|
shape_node.shape = capsule
|
|
shape_node.position = Vector2(0.0, length * 0.5)
|
|
proxy.add_child(shape_node)
|
|
visual.add_child(proxy)
|
|
proxy.top_level = true
|
|
proxy.global_transform = visual.global_transform
|
|
collision_bodies[key] = proxy
|
|
_collision_visuals[key] = visual
|
|
|
|
|
|
func sync_collision_bodies() -> void:
|
|
for key: String in collision_bodies:
|
|
var proxy: AnimatableBody2D = collision_bodies.get(key) as AnimatableBody2D
|
|
var visual: Node2D = _collision_visuals.get(key) as Node2D
|
|
if proxy != null and visual != null and is_instance_valid(proxy) and is_instance_valid(visual):
|
|
proxy.global_transform = visual.global_transform
|
|
|
|
|
|
func mounted_visual_bounds_local(part: Node2D) -> Dictionary:
|
|
var points: Array[Vector2] = []
|
|
var nodes: Array[Node] = [part]
|
|
while not nodes.is_empty():
|
|
var child: Node = nodes.pop_back()
|
|
if child is Line2D:
|
|
for point: Vector2 in (child as Line2D).points:
|
|
points.append(part.to_local((child as Node2D).to_global(point)))
|
|
elif child is Polygon2D:
|
|
for point: Vector2 in (child as Polygon2D).polygon:
|
|
points.append(part.to_local((child as Node2D).to_global(point)))
|
|
for grandchild: Node in child.get_children():
|
|
nodes.append(grandchild)
|
|
if points.is_empty():
|
|
return {"valid": false, "rect": Rect2()}
|
|
var rect := Rect2(points[0], Vector2.ZERO)
|
|
for point: Vector2 in points:
|
|
rect = rect.expand(point)
|
|
return {"valid": true, "rect": rect}
|
|
|
|
|
|
func set_collision_bodies_enabled(enabled: bool) -> void:
|
|
var layer: int = COLLISION_LAYER if enabled else 0
|
|
for proxy: AnimatableBody2D in collision_bodies.values():
|
|
if proxy != null and is_instance_valid(proxy):
|
|
proxy.collision_layer = layer
|
|
proxy.collision_mask = layer
|
|
|
|
|
|
func apply_collision_reaction(part_name: String, impact_direction: Vector2, impact_speed: float, impact_position: Vector2 = Vector2.INF) -> void:
|
|
if _rig == null or _rig.state != StickmanRig.RigState.ANIMATED or impact_direction.is_zero_approx() or impact_speed <= 0.0:
|
|
return
|
|
for target: Dictionary in REACTION_TARGETS.get(part_name, []):
|
|
var joint: String = str(target["joint"])
|
|
var bone: Bone2D = reaction_bones.get(joint) as Bone2D
|
|
if bone == null or not is_instance_valid(bone):
|
|
continue
|
|
var direction: float = collision_contact_direction(part_name, impact_position, impact_direction)
|
|
if is_zero_approx(direction):
|
|
var local_dir: Vector2 = bone.global_transform.affine_inverse().basis_xform(impact_direction).normalized()
|
|
direction = signf(local_dir.x) if absf(local_dir.x) > REACTION_LOCAL_X_EPSILON else 0.0
|
|
if is_zero_approx(direction): direction = -1.0 if joint.begins_with("Left") else 1.0
|
|
var angle: float = direction * minf(REACTION_MAX_ANGLE, REACTION_BASE_ANGLE * impact_speed / REACTION_REFERENCE_SPEED) * float(target["weight"])
|
|
var old: Dictionary = impact_reactions.get(joint, {})
|
|
if old.is_empty():
|
|
impact_reactions[joint] = {"angle": angle, "strength": 1.0, "decay": float(target["decay"]), "base_rotation": bone.rotation}
|
|
else:
|
|
if absf(angle) >= absf(float(old.get("angle", 0.0))):
|
|
old["angle"] = angle
|
|
old["decay"] = float(target["decay"])
|
|
old["strength"] = 1.0
|
|
impact_reactions[joint] = old
|
|
|
|
|
|
func collision_contact_direction(part_name: String, impact_position: Vector2, impact_direction: Vector2) -> float:
|
|
if not impact_position.is_finite(): return 0.0
|
|
var proxy: AnimatableBody2D = collision_bodies.get(part_name) as AnimatableBody2D
|
|
if proxy == null or not is_instance_valid(proxy): return 0.0
|
|
var shape_node: CollisionShape2D = proxy.get_node_or_null(NodePath("CollisionShape2D")) as CollisionShape2D
|
|
if shape_node == null: return 0.0
|
|
var side: float = proxy.to_local(impact_position).x - shape_node.position.x
|
|
if absf(side) <= REACTION_LOCAL_X_EPSILON: return 0.0
|
|
var local_force: Vector2 = proxy.global_transform.affine_inverse().basis_xform(impact_direction).normalized()
|
|
return signf(side * local_force.y) if absf(local_force.y) > REACTION_LOCAL_X_EPSILON else signf(side)
|
|
|
|
|
|
func clear_impact_reactions(restore_bones: bool = false) -> void:
|
|
if restore_bones:
|
|
for joint: String in impact_reactions:
|
|
var reaction: Dictionary = impact_reactions[joint]
|
|
var bone: Bone2D = reaction_bones.get(joint) as Bone2D
|
|
if bone != null and is_instance_valid(bone) and reaction.has("base_rotation"):
|
|
bone.rotation = float(reaction["base_rotation"])
|
|
impact_reactions.clear()
|
|
|
|
|
|
func apply_impact_reactions(delta: float) -> void:
|
|
if _rig == null or _rig.state != StickmanRig.RigState.ANIMATED:
|
|
clear_impact_reactions()
|
|
return
|
|
for joint: String in impact_reactions.keys().duplicate():
|
|
var reaction: Dictionary = impact_reactions[joint]
|
|
var bone: Bone2D = reaction_bones.get(joint) as Bone2D
|
|
if bone == null or not is_instance_valid(bone):
|
|
impact_reactions.erase(joint)
|
|
continue
|
|
var base_rotation: float = float(reaction.get("base_rotation", bone.rotation))
|
|
bone.rotation = base_rotation + float(reaction["angle"]) * float(reaction["strength"])
|
|
reaction["strength"] = float(reaction["strength"]) - delta / maxf(float(reaction["decay"]), 0.001)
|
|
if float(reaction["strength"]) <= 0.0:
|
|
bone.rotation = base_rotation
|
|
impact_reactions.erase(joint)
|
|
else:
|
|
impact_reactions[joint] = reaction
|
|
|
|
func tick_movement(delta: float, _rig_state: int) -> void:
|
|
if _rig == null:
|
|
return
|
|
terrain_pelvis_offset = 0.0
|
|
if terrain_following_enabled and terrain_motion == TerrainMotion.GROUNDED:
|
|
_last_grounded_walk_velocity_x = 0.0
|
|
_update_walking(delta)
|
|
_update_terrain_following(delta)
|
|
_settle_walk_markers()
|
|
|
|
func tick_post_skeleton(delta: float, _rig_state: int) -> void:
|
|
if _rig != null:
|
|
apply_impact_reactions(delta)
|
|
sync_collision_bodies()
|
|
|
|
func walk_to(target: Vector2, speed: float = -1.0) -> bool:
|
|
if _rig == null or _rig.state != StickmanRig.RigState.ANIMATED:
|
|
push_warning("StickmanRig: walk_to ignored; not ANIMATED.")
|
|
return false
|
|
if _rig._is_action_queue_blocked():
|
|
push_warning("StickmanRig: walk_to ignored during landing impact.")
|
|
return false
|
|
if _nav_agent == null or _nav_anchor == null:
|
|
push_warning("StickmanLocomotion: walk_to ignored; navigation is unavailable.")
|
|
return false
|
|
_walk_target_feet = target
|
|
_walk_speed_current = speed if speed > 0.0 else _rig.walk_speed
|
|
_nav_agent.max_speed = _walk_speed_current
|
|
_nav_agent.target_position = target
|
|
var dx: float = target.x - _rig.global_position.x
|
|
if dx < -0.5:
|
|
_rig.set_facing_profile(StickmanRig.FacingProfile.LEFT)
|
|
elif dx > 0.5:
|
|
_rig.set_facing_profile(StickmanRig.FacingProfile.RIGHT)
|
|
_rig._set_limb_mods_enabled(true)
|
|
if _rig._anim_player != null and is_instance_valid(_rig._anim_player) and _rig._anim_player.has_animation("walk_right"):
|
|
_rig._anim_player.play("walk_right")
|
|
_walking = true
|
|
_walk_done = false
|
|
_walk_mode = "nav"
|
|
_walk_mode_latched = false
|
|
_walk_latch_probe_frames = 0
|
|
_walk_settle_frames = 0
|
|
return true
|
|
|
|
func cancel_walk(restore_idle_pose: bool = true) -> void:
|
|
if _rig == null:
|
|
return
|
|
var current_limb_rotations: Dictionary = _rig._capture_limb_rotations()
|
|
if _rig._anim_player != null and is_instance_valid(_rig._anim_player):
|
|
_rig._anim_player.stop(true)
|
|
if _nav_agent != null and _nav_anchor != null:
|
|
_nav_agent.target_position = _nav_anchor.global_position
|
|
_walking = false
|
|
_walk_done = false
|
|
_walk_mode_latched = false
|
|
_walk_latch_probe_frames = 0
|
|
_walk_settle_frames = 0
|
|
if restore_idle_pose:
|
|
_rig._apply_idle_limb_solver_mode(current_limb_rotations)
|
|
|
|
func is_walking() -> bool:
|
|
return _walking
|
|
|
|
func get_foot_offset() -> Vector2:
|
|
return FOOT_OFFSET
|
|
|
|
func consume_walk_completed() -> bool:
|
|
if not _walk_done:
|
|
return false
|
|
_walk_done = false
|
|
return true
|
|
|
|
func _update_walking(delta: float) -> void:
|
|
if not _walking:
|
|
return
|
|
if _rig.state != StickmanRig.RigState.ANIMATED:
|
|
cancel_walk(false)
|
|
return
|
|
if blocks_walking() or _nav_agent == null:
|
|
return
|
|
if NavigationServer2D.map_get_iteration_id(_nav_agent.get_navigation_map()) == 0:
|
|
return
|
|
if not _walk_mode_latched:
|
|
if _walk_latch_probe_frames < LATCH_PROBE_MAX_FRAMES:
|
|
_walk_latch_probe_frames += 1
|
|
_nav_agent.get_next_path_position()
|
|
if _nav_agent.is_target_reachable():
|
|
_walk_mode_latched = true
|
|
_walk_mode = "nav"
|
|
else:
|
|
return
|
|
else:
|
|
_walk_mode_latched = true
|
|
_walk_mode = "direct"
|
|
var next_feet: Vector2 = _nav_agent.get_next_path_position()
|
|
var final_root: Vector2 = _walk_target_feet + FOOT_OFFSET
|
|
var dist_to_final: float = absf(_rig.global_position.x - final_root.x)
|
|
var root_target: Vector2
|
|
if _walk_mode == "nav":
|
|
if _nav_agent.is_navigation_finished() and dist_to_final <= 2.0 * ARRIVE_DISTANCE:
|
|
_rig.global_position.x = final_root.x
|
|
_finish_walk()
|
|
return
|
|
# A freshly synchronized agent may yield its own feet as the first
|
|
# waypoint. Treat that zero-progress waypoint as the final segment.
|
|
var next_progress: float = next_feet.x - _rig.global_position.x
|
|
var remaining_progress: float = final_root.x - _rig.global_position.x
|
|
if dist_to_final <= 2.0 * ARRIVE_DISTANCE or remaining_progress * next_progress <= 0.0:
|
|
root_target = Vector2(final_root.x, _rig.global_position.y)
|
|
else:
|
|
root_target = Vector2(next_feet.x, _rig.global_position.y)
|
|
else:
|
|
root_target = Vector2(final_root.x, _rig.global_position.y)
|
|
var before_x: float = _rig.global_position.x
|
|
_rig.global_position = _rig.global_position.move_toward(root_target, _walk_speed_current * delta)
|
|
_last_grounded_walk_velocity_x = (_rig.global_position.x - before_x) / delta if delta > 0.0 else 0.0
|
|
if dist_to_final <= ARRIVE_DISTANCE or absf(_rig.global_position.x - final_root.x) <= ARRIVE_DISTANCE:
|
|
_rig.global_position.x = final_root.x
|
|
_finish_walk()
|
|
|
|
func _settle_walk_markers() -> void:
|
|
if _walk_settle_frames > 0:
|
|
_walk_settle_frames -= 1
|
|
_rig._restore_standing_markers()
|
|
|
|
func _finish_walk() -> void:
|
|
var final_limb_rotations: Dictionary = _rig._capture_limb_rotations()
|
|
if _rig._anim_player != null and is_instance_valid(_rig._anim_player):
|
|
_rig._anim_player.stop(true)
|
|
_walking = false
|
|
_rig._apply_idle_limb_solver_mode(final_limb_rotations)
|
|
_walk_settle_frames = 0
|
|
_walk_done = true
|
|
arrived.emit(_walk_target_feet)
|
|
|
|
func begin_terrain_fall() -> void:
|
|
if _rig == null:
|
|
return
|
|
terrain_following_enabled = true
|
|
terrain_pelvis_offset = 0.0
|
|
_terrain_paused_walk_animation = false
|
|
_enter_airborne(0.0)
|
|
_reset_foot_contacts()
|
|
|
|
func set_terrain_following_enabled(enabled: bool) -> void:
|
|
if enabled:
|
|
if terrain_motion == TerrainMotion.DISABLED:
|
|
begin_terrain_fall()
|
|
return
|
|
terrain_following_enabled = false
|
|
_cancel_landing_impact(true)
|
|
_restore_air_pose()
|
|
terrain_pelvis_offset = 0.0
|
|
terrain_air_velocity = Vector2.ZERO
|
|
terrain_motion = TerrainMotion.DISABLED
|
|
_terrain_paused_walk_animation = false
|
|
_reset_foot_contacts()
|
|
for leg_name: String in ["Left_Leg", "Right_Leg"]:
|
|
var base: Marker2D = _leg_base_markers.get(leg_name) as Marker2D
|
|
var target: Marker2D = _rig._get_ik_marker(leg_name)
|
|
if base != null and target != null:
|
|
target.position = base.position
|
|
|
|
func prepare_for_ragdoll() -> void:
|
|
if terrain_motion != TerrainMotion.DISABLED:
|
|
set_terrain_following_enabled(false)
|
|
|
|
func reset_for_ragdoll_handoff() -> void:
|
|
_cancel_landing_impact(false)
|
|
air_pose_base.clear()
|
|
air_pose_weight = 0.0
|
|
_air_time = 0.0
|
|
_air_entry_root_y = 0.0
|
|
air_fall_distance = 0.0
|
|
terrain_motion = TerrainMotion.DISABLED
|
|
terrain_air_velocity = Vector2.ZERO
|
|
|
|
func is_action_blocked() -> bool:
|
|
return terrain_motion == TerrainMotion.LANDING_IMPACT
|
|
|
|
func blocks_walking() -> bool:
|
|
return terrain_following_enabled and terrain_motion != TerrainMotion.GROUNDED
|
|
|
|
func _reset_foot_contacts() -> void:
|
|
for leg_name: String in foot_contacts:
|
|
foot_contacts[leg_name] = {"valid": false, "locked": false}
|
|
|
|
func _probe_terrain_below(world_point: Vector2, probe_down: float = TERRAIN_PROBE_DOWN) -> Dictionary:
|
|
var space_state: PhysicsDirectSpaceState2D = _rig.get_world_2d().direct_space_state
|
|
if space_state == null:
|
|
return {"valid": false}
|
|
var query := PhysicsRayQueryParameters2D.create(world_point + Vector2.UP * TERRAIN_PROBE_UP, world_point + Vector2.DOWN * probe_down)
|
|
query.collision_mask = 1
|
|
query.collide_with_areas = false
|
|
query.exclude = _rig.get_collision_body_rids()
|
|
var hit: Dictionary = space_state.intersect_ray(query)
|
|
if hit.is_empty() or not (hit.get("collider") is TerrainBlock):
|
|
return {"valid": false}
|
|
var normal: Vector2 = hit.get("normal", Vector2.ZERO)
|
|
if normal.y > TERRAIN_NORMAL_MIN_Y:
|
|
return {"valid": false}
|
|
return {"valid": true, "world_pos": hit.get("position", world_point), "normal": normal}
|
|
|
|
func _probe_terrain_fan(world_point: Vector2, offsets: Array[float]) -> Dictionary:
|
|
var best: Dictionary = {"valid": false}
|
|
for x_offset: float in offsets:
|
|
var hit: Dictionary = _probe_terrain_below(world_point + Vector2(x_offset, 0.0))
|
|
if bool(hit.get("valid", false)) and (not bool(best.get("valid", false)) or (hit["world_pos"] as Vector2).y < (best["world_pos"] as Vector2).y):
|
|
best = hit
|
|
return best
|
|
|
|
func probe_ground_y(world_x: float, top_y: float, bottom_y: float, exclude: Array[RID]) -> float:
|
|
var best_y := bottom_y
|
|
var best_distance := INF
|
|
if _rig == null:
|
|
return best_y
|
|
var space_state: PhysicsDirectSpaceState2D = _rig.get_world_2d().direct_space_state
|
|
if space_state == null:
|
|
return best_y
|
|
for offset: float in RECOVERY_PROBE_X_OFFSETS:
|
|
var query := PhysicsRayQueryParameters2D.create(
|
|
Vector2(world_x + offset, top_y - RECOVERY_PROBE_START_OFFSET),
|
|
Vector2(world_x + offset, bottom_y + RECOVERY_PROBE_END_OFFSET))
|
|
query.collision_mask = 1
|
|
query.collide_with_areas = false
|
|
query.collide_with_bodies = true
|
|
query.exclude = exclude
|
|
var hit := space_state.intersect_ray(query)
|
|
if hit.is_empty():
|
|
continue
|
|
var hit_position: Vector2 = hit.get("position", Vector2.ZERO)
|
|
if hit_position.y < top_y - RECOVERY_PROBE_TOLERANCE or hit_position.y > bottom_y + RECOVERY_PROBE_TOLERANCE:
|
|
continue
|
|
var distance := absf(hit_position.y - bottom_y)
|
|
if distance < best_distance:
|
|
best_distance = distance
|
|
best_y = hit_position.y
|
|
return best_y
|
|
|
|
func _stance_weight(leg_name: String) -> float:
|
|
if not _walking or _rig._anim_player == null:
|
|
return 1.0
|
|
var phase: float = fmod(_rig._anim_player.get_current_animation_position(), WALK_CYCLE_DURATION)
|
|
var right_weight: float = 0.5 + 0.5 * cos(TAU * phase / WALK_CYCLE_DURATION)
|
|
return right_weight if leg_name == "Right_Leg" else 1.0 - right_weight
|
|
|
|
func _update_terrain_following(delta: float) -> void:
|
|
if not terrain_following_enabled or _rig.state != StickmanRig.RigState.ANIMATED or _leg_base_markers.size() != 2:
|
|
terrain_pelvis_offset = 0.0
|
|
return
|
|
_rig._set_limb_mods_enabled(true)
|
|
var center_world: Vector2 = _rig.to_global(NAV_AGENT_LOCAL_POS)
|
|
var center_probe: Dictionary = _probe_terrain_fan(center_world, TERRAIN_CENTER_PROBE_OFFSETS)
|
|
var center_delta: float = (center_probe["world_pos"] as Vector2).y - center_world.y if bool(center_probe.get("valid", false)) else INF
|
|
var center_supported: bool = bool(center_probe.get("valid", false)) and absf(center_delta) <= TERRAIN_SUPPORT_RANGE
|
|
var center_surface_y: float = (center_probe["world_pos"] as Vector2).y if bool(center_probe.get("valid", false)) else INF
|
|
var solved: Dictionary = {}
|
|
for leg_name: String in ["Left_Leg", "Right_Leg"]:
|
|
var base: Marker2D = _leg_base_markers[leg_name] as Marker2D
|
|
var target: Marker2D = _rig._get_ik_marker(leg_name)
|
|
if base == null or target == null:
|
|
continue
|
|
var base_world: Vector2 = base.global_position
|
|
var probe: Dictionary = _probe_terrain_fan(base_world, TERRAIN_FOOT_PROBE_OFFSETS)
|
|
var probe_delta: float = (probe["world_pos"] as Vector2).y - base_world.y if bool(probe.get("valid", false)) else INF
|
|
var within_reach: bool = absf(probe_delta) <= PELVIS_MAX_OFFSET
|
|
if within_reach and center_supported:
|
|
within_reach = (probe["world_pos"] as Vector2).y <= center_surface_y + TERRAIN_MAX_FOOT_SURFACE_DROP
|
|
var stance: float = _stance_weight(leg_name)
|
|
var contact: Dictionary = foot_contacts[leg_name]
|
|
if stance >= 0.5 and bool(probe.get("valid", false)) and within_reach:
|
|
if not bool(contact.get("locked", false)):
|
|
contact = {"valid": true, "locked": true, "world_pos": probe["world_pos"], "normal": probe["normal"]}
|
|
elif (contact.get("world_pos", base_world) as Vector2).distance_to(probe["world_pos"]) > 2.0:
|
|
contact["world_pos"] = probe["world_pos"]
|
|
elif stance < 0.5 or not within_reach:
|
|
contact = {"valid": bool(probe.get("valid", false)), "locked": false}
|
|
foot_contacts[leg_name] = contact
|
|
var floor_world: Vector2 = contact.get("world_pos", base_world) as Vector2 if bool(contact.get("locked", false)) else probe.get("world_pos", base_world) as Vector2
|
|
solved[leg_name] = {"base_marker": base, "floor": floor_world, "surface": probe.get("world_pos", base_world) if within_reach else base_world, "surface_valid": bool(probe.get("valid", false)) and within_reach, "weight": stance, "target": target}
|
|
if terrain_motion == TerrainMotion.LANDING_IMPACT:
|
|
_update_landing_impact(delta)
|
|
return
|
|
if terrain_motion == TerrainMotion.AIRBORNE:
|
|
_update_airborne(delta, solved)
|
|
return
|
|
if not center_supported:
|
|
_enter_airborne(_last_grounded_walk_velocity_x)
|
|
_update_airborne(delta, solved)
|
|
return
|
|
terrain_pelvis_offset = lerpf(0.0, clampf(center_delta, -PELVIS_MAX_OFFSET, PELVIS_MAX_OFFSET), 1.0 - exp(-PELVIS_RESPONSE * delta))
|
|
terrain_motion = TerrainMotion.GROUNDED
|
|
terrain_air_velocity = Vector2.ZERO
|
|
_resume_walk_animation()
|
|
_rig.global_position.y += terrain_pelvis_offset
|
|
for leg_name: String in solved:
|
|
var item: Dictionary = solved[leg_name]
|
|
var base_after: Vector2 = (item["base_marker"] as Marker2D).global_position
|
|
var weight: float = float(item["weight"])
|
|
var surface: Vector2 = item["surface"] as Vector2
|
|
if bool(item["surface_valid"]):
|
|
weight = maxf(weight, smoothstep(0.0, 1.0, 1.0 - clampf((surface.y - base_after.y) / TERRAIN_FOOT_GUARD_HEIGHT, 0.0, 1.0)))
|
|
var wanted: Vector2 = base_after
|
|
wanted.y = lerpf(base_after.y, (item["floor"] as Vector2).y, weight)
|
|
if bool(item["surface_valid"]) and wanted.y > surface.y:
|
|
wanted.y = surface.y
|
|
(item["target"] as Marker2D).global_position = wanted
|
|
|
|
func _enter_airborne(horizontal_velocity: float) -> void:
|
|
if terrain_motion != TerrainMotion.AIRBORNE:
|
|
air_pose_base.clear()
|
|
for marker_name: String in ["Left_Hand", "Right_Hand", "Left_Leg", "Right_Leg"]:
|
|
var marker: Marker2D = _rig._get_ik_marker(marker_name)
|
|
if marker != null:
|
|
air_pose_base[marker_name] = marker.position
|
|
_air_time = 0.0
|
|
air_pose_weight = 0.0
|
|
_air_entry_root_y = _rig.global_position.y
|
|
air_fall_distance = 0.0
|
|
terrain_motion = TerrainMotion.AIRBORNE
|
|
terrain_air_velocity = Vector2(horizontal_velocity, 0.0)
|
|
if _walking and _rig._anim_player != null and _rig._anim_player.is_playing():
|
|
_rig._anim_player.pause()
|
|
_terrain_paused_walk_animation = true
|
|
_reset_foot_contacts()
|
|
|
|
func _update_airborne(delta: float, solved: Dictionary) -> void:
|
|
terrain_air_velocity.x *= exp(-AIR_DRAG_X * delta)
|
|
terrain_air_velocity.y = minf(terrain_air_velocity.y + TERRAIN_GRAVITY * delta, TERRAIN_MAX_FALL_SPEED)
|
|
_rig.global_position += _sweep_airborne_horizontal_motion(Vector2(terrain_air_velocity.x * delta, 0.0))
|
|
_rig.global_position.y += terrain_air_velocity.y * delta
|
|
air_fall_distance = maxf(air_fall_distance, _rig.global_position.y - _air_entry_root_y)
|
|
_air_time += delta
|
|
_apply_air_pose(delta)
|
|
var center_world: Vector2 = _rig.to_global(NAV_AGENT_LOCAL_POS)
|
|
var center_probe: Dictionary = _probe_terrain_fan(center_world, TERRAIN_CENTER_PROBE_OFFSETS)
|
|
var landing_delta: float = (center_probe["world_pos"] as Vector2).y - center_world.y if bool(center_probe.get("valid", false)) else INF
|
|
if bool(center_probe.get("valid", false)) and terrain_air_velocity.y >= 0.0 and landing_delta >= -TERRAIN_SUPPORT_RANGE and landing_delta <= TERRAIN_SUPPORT_RANGE + terrain_air_velocity.y * delta:
|
|
_rig.global_position.y += landing_delta
|
|
var speed: float = maxf(terrain_air_velocity.y, 0.0)
|
|
terrain_air_velocity = Vector2.ZERO
|
|
if air_fall_distance >= HARD_LANDING_MIN_FALL_HEIGHT:
|
|
_begin_landing(speed, center_probe, solved)
|
|
else:
|
|
_restore_air_pose()
|
|
terrain_motion = TerrainMotion.GROUNDED
|
|
_resume_walk_animation()
|
|
|
|
func _apply_air_pose(delta: float) -> void:
|
|
if air_pose_base.is_empty():
|
|
return
|
|
var speed: float = maxf(terrain_air_velocity.y, 0.0)
|
|
air_pose_weight = lerpf(air_pose_weight, smoothstep(AIR_POSE_START_SPEED, AIR_POSE_FULL_SPEED, speed), 1.0 - exp(-AIR_POSE_RESPONSE * delta))
|
|
var forward_weight: float = smoothstep(AIR_HAND_FORWARD_START_SPEED, AIR_HAND_FORWARD_FULL_SPEED, speed)
|
|
var phase: float = TAU * AIR_FLAIL_HZ * _air_time
|
|
for marker_name: String in air_pose_base:
|
|
var marker: Marker2D = _rig._get_ik_marker(marker_name)
|
|
if marker == null:
|
|
continue
|
|
var base: Vector2 = air_pose_base[marker_name]
|
|
var local_phase: float = phase + (PI if marker_name.begins_with("Right") else 0.0)
|
|
var offset: Vector2
|
|
if marker_name.ends_with("Hand"):
|
|
offset = Vector2(AIR_HAND_FORWARD * forward_weight, -AIR_HAND_LIFT * air_pose_weight) + Vector2(sin(local_phase) * AIR_HAND_FLAIL_AMPLITUDE.x, cos(local_phase) * AIR_HAND_FLAIL_AMPLITUDE.y) * air_pose_weight
|
|
marker.position = base + offset
|
|
else:
|
|
offset = Vector2(sin(local_phase) * AIR_FOOT_FLAIL_AMPLITUDE.x, cos(local_phase) * AIR_FOOT_FLAIL_AMPLITUDE.y)
|
|
marker.position = base + offset * air_pose_weight
|
|
|
|
func _sweep_airborne_horizontal_motion(motion: Vector2) -> Vector2:
|
|
if is_zero_approx(motion.x):
|
|
return motion
|
|
var space_state: PhysicsDirectSpaceState2D = _rig.get_world_2d().direct_space_state
|
|
if space_state == null:
|
|
return motion
|
|
var safe_fraction: float = 1.0
|
|
var excluded: Array[RID] = []
|
|
for own_proxy: AnimatableBody2D in collision_bodies.values():
|
|
if own_proxy != null and is_instance_valid(own_proxy):
|
|
var own_rid: RID = own_proxy.get_rid()
|
|
if own_rid.is_valid():
|
|
excluded.append(own_rid)
|
|
for proxy: AnimatableBody2D in collision_bodies.values():
|
|
if proxy == null or not is_instance_valid(proxy):
|
|
continue
|
|
var shape_node: CollisionShape2D = proxy.get_node_or_null(NodePath("CollisionShape2D")) as CollisionShape2D
|
|
if shape_node == null or shape_node.shape == null:
|
|
continue
|
|
var query := PhysicsShapeQueryParameters2D.new()
|
|
query.shape = shape_node.shape
|
|
query.transform = shape_node.global_transform
|
|
query.motion = motion
|
|
query.collision_mask = COLLISION_LAYER
|
|
query.exclude = excluded
|
|
var cast: PackedFloat32Array = space_state.cast_motion(query)
|
|
if cast.size() >= 1:
|
|
safe_fraction = minf(safe_fraction, cast[0])
|
|
if safe_fraction < 0.9999:
|
|
terrain_air_velocity.x = 0.0
|
|
return motion * safe_fraction
|
|
|
|
func _begin_landing(_impact_speed: float, center_probe: Dictionary, _solved: Dictionary) -> void:
|
|
terrain_motion = TerrainMotion.LANDING_IMPACT
|
|
_landing_elapsed = 0.0
|
|
var weight: float = clampf(inverse_lerp(HARD_LANDING_MIN_FALL_HEIGHT, HARD_LANDING_MAX_FALL_HEIGHT, air_fall_distance), 0.0, 1.0)
|
|
landing_compression = lerpf(LANDING_COMPRESSION_MIN, LANDING_COMPRESSION_MAX, weight)
|
|
_landing_hand_drop = lerpf(LANDING_HAND_DROP_MIN, LANDING_HAND_DROP_MAX, weight)
|
|
var torso: Marker2D = _rig._get_ik_marker("Torso")
|
|
if torso != null: _landing_torso_base = torso.position
|
|
_landing_hand_start.clear(); _landing_contacts.clear()
|
|
for hand_name: String in ["Left_Hand", "Right_Hand"]:
|
|
var hand: Marker2D = _rig._get_ik_marker(hand_name)
|
|
if hand != null: _landing_hand_start[hand_name] = hand.position
|
|
var center_y: float = (center_probe.get("world_pos", _rig.global_position) as Vector2).y
|
|
for leg_name: String in ["Left_Leg", "Right_Leg"]:
|
|
var target: Marker2D = _rig._get_ik_marker(leg_name)
|
|
if target == null: continue
|
|
var base: Marker2D = _leg_base_markers.get(leg_name) as Marker2D
|
|
var probe: Dictionary = _probe_terrain_fan(base.global_position if base != null else target.global_position, TERRAIN_FOOT_PROBE_OFFSETS)
|
|
var contact: Vector2 = probe["world_pos"] as Vector2 if bool(probe.get("valid", false)) else Vector2(target.global_position.x, center_y)
|
|
_landing_contacts[leg_name] = contact; target.global_position = contact
|
|
|
|
func _update_landing_impact(delta: float) -> void:
|
|
_landing_elapsed += delta
|
|
var total: float = LANDING_COMPRESS_DURATION + LANDING_RECOVER_DURATION
|
|
var offset: float = landing_compression * (1.0 - pow(1.0 - clampf(_landing_elapsed / LANDING_COMPRESS_DURATION, 0.0, 1.0), 2.0)) if _landing_elapsed <= LANDING_COMPRESS_DURATION else landing_compression * cos(clampf((_landing_elapsed - LANDING_COMPRESS_DURATION) / LANDING_RECOVER_DURATION, 0.0, 1.0) * PI * 0.5)
|
|
var torso: Marker2D = _rig._get_ik_marker("Torso")
|
|
if torso != null: torso.position = _landing_torso_base + Vector2.DOWN * offset
|
|
for leg_name: String in _landing_contacts:
|
|
var target: Marker2D = _rig._get_ik_marker(leg_name)
|
|
if target != null: target.global_position = _landing_contacts[leg_name]
|
|
for hand_name: String in _landing_hand_start:
|
|
var hand: Marker2D = _rig._get_ik_marker(hand_name)
|
|
if hand != null: hand.position = (_landing_hand_start[hand_name] as Vector2).lerp(air_pose_base.get(hand_name, _landing_hand_start[hand_name]), clampf(_landing_elapsed / total, 0.0, 1.0)) + Vector2.DOWN * _landing_hand_drop * offset / maxf(landing_compression, 0.0001)
|
|
if _landing_elapsed >= total: _finish_landing_impact()
|
|
|
|
func _finish_landing_impact() -> void:
|
|
var torso: Marker2D = _rig._get_ik_marker("Torso")
|
|
if torso != null: torso.position = _landing_torso_base
|
|
_restore_air_pose(); _landing_contacts.clear(); _landing_hand_start.clear(); _landing_elapsed = 0.0; landing_compression = 0.0; _landing_hand_drop = 0.0; terrain_motion = TerrainMotion.GROUNDED
|
|
_resume_walk_animation()
|
|
|
|
func _cancel_landing_impact(restore_pose: bool) -> void:
|
|
if terrain_motion == TerrainMotion.LANDING_IMPACT and restore_pose:
|
|
var torso: Marker2D = _rig._get_ik_marker("Torso")
|
|
if torso != null: torso.position = _landing_torso_base
|
|
_landing_contacts.clear(); _landing_hand_start.clear(); _landing_elapsed = 0.0; landing_compression = 0.0; _landing_hand_drop = 0.0
|
|
|
|
func _restore_air_pose() -> void:
|
|
for marker_name: String in air_pose_base:
|
|
var marker: Marker2D = _rig._get_ik_marker(marker_name)
|
|
if marker != null: marker.position = air_pose_base[marker_name]
|
|
air_pose_base.clear(); air_pose_weight = 0.0; _air_time = 0.0; _air_entry_root_y = 0.0; air_fall_distance = 0.0
|
|
|
|
func _resume_walk_animation() -> void:
|
|
if _terrain_paused_walk_animation and _walking and _rig._anim_player != null:
|
|
_rig._anim_player.play()
|
|
_terrain_paused_walk_animation = false
|