@tool
class_name LegPhysics extends RefCounted
## Verlet-integrated body secondary physics for grapple dangle.
## Simulates spine chain (hip→spine→chest→neck) and both leg chains
## (thigh→shin→foot) with distance constraints and gravity.
## During grapple: everything goes loose — spine trails, legs dangle.
## During normal movement: snaps to animated pose (invisible).
# Bone names (Rigify DEF- convention)
const LEG_BONES := {
"hip_l": "DEF-thigh.L",
"knee_l": "DEF-shin.L",
"ankle_l": "DEF-foot.L",
"hip_r": "DEF-thigh.R",
"knee_r": "DEF-shin.R",
"ankle_r": "DEF-foot.R",
}
const ARM_BONES := {
"shoulder_l": "DEF-upper_arm.L",
"elbow_l": "DEF-forearm.L",
"wrist_l": "DEF-hand.L",
}
const SPINE_BONES: Array[String] = [
"DEF-spine", # 0: hip (root — pinned to shoulder/hand anchor)
"DEF-spine.001", # 1: spine
"DEF-spine.003", # 2: chest
"DEF-spine.005", # 3: neck
]
# Physics parameters
const LEG_DRAG: float = 0.88 ## Velocity damping per frame
const SPINE_DRAG: float = 0.85 ## Spine is slightly stiffer than legs
const STIFFNESS_DANGLE: float = 0.97 ## Grapple dangle — very loose
const SPINE_STIFFNESS: float = 0.92 ## Spine trails but not as loose as legs
const CONSTRAINT_STRENGTH: float = 0.9 ## Distance constraint solver strength
const GRAVITY_STRENGTH: float = 9.8 ## Gravity pull on dangling body
# Per-leg state: [left, right], each has [hip, knee, ankle]
var _leg_points: Array = [[], []]
var _leg_old_points: Array = [[], []]
var _leg_upper_length: Array = [0.0, 0.0]
var _leg_lower_length: Array = [0.0, 0.0]
# Spine state: 4 particles [hip, spine, chest, neck]
var _spine_points: PackedVector3Array = PackedVector3Array()
var _spine_old_points: PackedVector3Array = PackedVector3Array()
var _spine_lengths: Array[float] = [] # rest lengths between consecutive bones
var _spine_initialized: bool = false
var _legs_initialized: bool = false
# Left arm state (trail behind during grapple — must run after spine Verlet)
var _arm_upper_length: float = 0.3
var _arm_lower_length: float = 0.25
var _arm_initialized: bool = false
# Context (set by animator)
var character_velocity: Vector3 = Vector3.ZERO
var local_up: Vector3 = Vector3.UP
var is_grappling: bool = false
var grapple_tension_dir: Vector3 = Vector3.ZERO
var grapple_anchor_dir: Vector3 = Vector3.UP
var _turbulence_time: float = 0.0 ## Phase for flag-like oscillation
func _init_legs(skeleton: Skeleton3D, gx: Transform3D) -> void:
for side in range(2):
var suffix: String = "_l" if side == 0 else "_r"
var hip_idx: int = skeleton.find_bone(LEG_BONES["hip" + suffix])
var knee_idx: int = skeleton.find_bone(LEG_BONES["knee" + suffix])
var ankle_idx: int = skeleton.find_bone(LEG_BONES["ankle" + suffix])
if hip_idx < 0 or knee_idx < 0 or ankle_idx < 0:
continue
var h: Vector3 = (gx * skeleton.get_bone_global_pose(hip_idx)).origin
var k: Vector3 = (gx * skeleton.get_bone_global_pose(knee_idx)).origin
var a: Vector3 = (gx * skeleton.get_bone_global_pose(ankle_idx)).origin
_leg_points[side] = [h, k, a]
_leg_old_points[side] = [h, k, a]
_leg_upper_length[side] = h.distance_to(k)
_leg_lower_length[side] = k.distance_to(a)
_legs_initialized = true
func _init_spine(skeleton: Skeleton3D, gx: Transform3D) -> void:
_spine_points.resize(SPINE_BONES.size())
_spine_old_points.resize(SPINE_BONES.size())
_spine_lengths.clear()
for i in range(SPINE_BONES.size()):
var idx: int = skeleton.find_bone(SPINE_BONES[i])
if idx < 0:
return # Missing bone — abort
var pos: Vector3 = (gx * skeleton.get_bone_global_pose(idx)).origin
_spine_points[i] = pos
_spine_old_points[i] = pos
for i in range(SPINE_BONES.size() - 1):
_spine_lengths.append(_spine_points[i].distance_to(_spine_points[i + 1]))
_spine_initialized = true
func apply(skeleton: Skeleton3D, delta: float) -> void:
if skeleton == null:
return
var gx: Transform3D = skeleton.global_transform
if not _legs_initialized:
_init_legs(skeleton, gx)
if not _spine_initialized:
_init_spine(skeleton, gx)
if not _legs_initialized or not _spine_initialized:
return
var frames: float = delta * 60.0
if frames < 0.001:
return
if is_grappling:
_turbulence_time += delta * (3.0 + character_velocity.length() * 0.5)
_apply_spine(skeleton, gx, delta, frames)
_apply_legs(skeleton, gx, delta, frames)
# Left arm MUST run after spine — spine Verlet rotates the chest,
# which is the arm's parent. Writing arm bones before spine is final
# causes the parent rotation to move the arm out of position.
_apply_trail_arm(skeleton, gx, delta)
func _apply_spine(skeleton: Skeleton3D, gx: Transform3D, delta: float, frames: float) -> void:
# Read current animated bone positions
var anim_pos: PackedVector3Array = PackedVector3Array()
anim_pos.resize(SPINE_BONES.size())
var bone_indices: Array[int] = []
for i in range(SPINE_BONES.size()):
var idx: int = skeleton.find_bone(SPINE_BONES[i])
bone_indices.append(idx)
if idx >= 0:
anim_pos[i] = (gx * skeleton.get_bone_global_pose(idx)).origin
# Update rest lengths from current pose
for i in range(SPINE_BONES.size() - 1):
var d: float = anim_pos[i].distance_to(anim_pos[i + 1])
if d > 0.01:
_spine_lengths[i] = d
if not is_grappling:
# Snap to animated pose
for i in range(SPINE_BONES.size()):
_spine_points[i] = anim_pos[i]
_spine_old_points[i] = anim_pos[i]
return
# The spine hangs from the top (neck/chest end, near the grapple hand).
# Pin the LAST bone (neck) since the hand is up near the anchor.
# The hip dangles below.
var pin_idx: int = SPINE_BONES.size() - 1 # neck is pinned
# Save old
for i in range(SPINE_BONES.size()):
_spine_old_points[i] = _spine_points[i]
# Pin top to animated position (near grapple hand)
_spine_points[pin_idx] = anim_pos[pin_idx]
# Verlet integration for free bones (all except pinned neck)
var grav_dir: Vector3 = -local_up
var full_vel_offset: Vector3 = character_velocity * delta
for i in range(pin_idx): # hip, spine, chest — not neck
var vel: Vector3 = _spine_points[i] - _spine_old_points[i]
var relative_vel: Vector3 = vel - full_vel_offset
var damped_vel: Vector3 = relative_vel * pow(SPINE_DRAG, frames) + full_vel_offset
_spine_points[i] += damped_vel
# Gravity
_spine_points[i] += grav_dir * GRAVITY_STRENGTH * delta * delta
# Flag turbulence — traveling wave from neck down to hips
var turb_right: Vector3 = local_up.cross(character_velocity.normalized()).normalized() if character_velocity.length() > 0.5 else Vector3.RIGHT
var turb_fwd: Vector3 = turb_right.cross(local_up).normalized()
var speed_amp: float = clampf(character_velocity.length() / 10.0, 0.15, 1.0)
for i in range(pin_idx):
# Distance from pin (0 = hip = farthest from pin, largest amplitude)
var chain_dist: float = float(pin_idx - i) / float(pin_idx)
var amplitude: float = 0.025 * chain_dist * speed_amp
var phase: float = _turbulence_time + float(i) * 2.2
_spine_points[i] += turb_right * sin(phase * 5.3) * amplitude
_spine_points[i] += turb_fwd * sin(phase * 3.7 + 0.8) * amplitude * 0.5
# Spring toward animated pose (weak)
var spring_factor: float = 1.0 - pow(SPINE_STIFFNESS, frames)
for i in range(pin_idx):
_spine_points[i] += (anim_pos[i] - _spine_points[i]) * spring_factor
# Distance constraints — iterate from pinned end downward
for _iter in range(4):
# Re-pin top
_spine_points[pin_idx] = anim_pos[pin_idx]
# Solve from top to bottom (neck→chest→spine→hip)
for i in range(SPINE_BONES.size() - 2, -1, -1):
var a: Vector3 = _spine_points[i + 1] # upper (toward neck)
var b: Vector3 = _spine_points[i] # lower (toward hip)
var ab: Vector3 = b - a
var dist: float = ab.length()
if dist < 0.0001:
continue
var error: float = dist - _spine_lengths[i]
var correction: Vector3 = ab.normalized() * error
if i + 1 == pin_idx:
# Upper is pinned — full correction to lower
_spine_points[i] -= correction
else:
_spine_points[i] -= correction * 0.5
_spine_points[i + 1] += correction * 0.5
# Write back to skeleton — orient each bone toward the next
for i in range(SPINE_BONES.size() - 1):
if bone_indices[i] >= 0:
_orient_bone_toward(skeleton, gx, bone_indices[i],
_spine_points[i], _spine_points[i + 1])
func _apply_legs(skeleton: Skeleton3D, gx: Transform3D, delta: float, frames: float) -> void:
for side in range(2):
if _leg_points[side].size() != 3:
continue
var suffix: String = "_l" if side == 0 else "_r"
var hip_idx: int = skeleton.find_bone(LEG_BONES["hip" + suffix])
var knee_idx: int = skeleton.find_bone(LEG_BONES["knee" + suffix])
var ankle_idx: int = skeleton.find_bone(LEG_BONES["ankle" + suffix])
if hip_idx < 0 or knee_idx < 0 or ankle_idx < 0:
continue
var hip_pos: Vector3 = (gx * skeleton.get_bone_global_pose(hip_idx)).origin
var knee_pos: Vector3 = (gx * skeleton.get_bone_global_pose(knee_idx)).origin
var ankle_pos: Vector3 = (gx * skeleton.get_bone_global_pose(ankle_idx)).origin
# Update rest lengths
_leg_upper_length[side] = hip_pos.distance_to(knee_pos)
_leg_lower_length[side] = knee_pos.distance_to(ankle_pos)
if not is_grappling:
_leg_points[side] = [hip_pos, knee_pos, ankle_pos]
_leg_old_points[side] = [hip_pos, knee_pos, ankle_pos]
continue
# Save old positions
_leg_old_points[side] = [_leg_points[side][0], _leg_points[side][1], _leg_points[side][2]]
# Hip joint tracks the Verlet spine hip position (dangles from spine)
if _spine_points.size() > 0:
_leg_points[side][0] = _spine_points[0] # Spine hip drives leg hips
else:
_leg_points[side][0] = hip_pos
# Verlet integration for knee and ankle
var full_vel_offset: Vector3 = character_velocity * delta
var grav_dir: Vector3 = -local_up
for i in range(1, 3):
var vel: Vector3 = _leg_points[side][i] - _leg_old_points[side][i]
var relative_vel: Vector3 = vel - full_vel_offset
var damped_vel: Vector3 = relative_vel * pow(LEG_DRAG, frames) + full_vel_offset
_leg_points[side][i] += damped_vel
# Gravity
_leg_points[side][i] += grav_dir * GRAVITY_STRENGTH * delta * delta
# Flag turbulence on legs — each leg gets a different phase offset
var turb_right: Vector3 = local_up.cross(character_velocity.normalized()).normalized() if character_velocity.length() > 0.5 else Vector3.RIGHT
var turb_fwd: Vector3 = turb_right.cross(local_up).normalized()
var speed_amp: float = clampf(character_velocity.length() / 10.0, 0.15, 1.0)
var leg_phase_offset: float = float(side) * 1.5 # Left/right legs out of phase
for i in range(1, 3):
var chain_dist: float = float(i) * 0.5 # knee=0.5, ankle=1.0
var amplitude: float = 0.03 * chain_dist * speed_amp
var phase: float = _turbulence_time + leg_phase_offset + float(i) * 1.9
_leg_points[side][i] += turb_right * sin(phase * 4.1) * amplitude
_leg_points[side][i] += turb_fwd * sin(phase * 2.9 + 1.3) * amplitude * 0.7
# Spring toward animated pose (very weak during dangle)
var spring_factor: float = 1.0 - pow(STIFFNESS_DANGLE, frames)
_leg_points[side][1] += (knee_pos - _leg_points[side][1]) * spring_factor
_leg_points[side][2] += (ankle_pos - _leg_points[side][2]) * spring_factor
# Distance constraints
for _iter in range(3):
# Pin hip to spine
if _spine_points.size() > 0:
_leg_points[side][0] = _spine_points[0]
# Upper leg: hip → knee
var h_to_k: Vector3 = _leg_points[side][1] - _leg_points[side][0]
if h_to_k.length_squared() > 0.0001:
var target_k: Vector3 = _leg_points[side][0] + h_to_k.normalized() * _leg_upper_length[side]
_leg_points[side][1] += (target_k - _leg_points[side][1]) * CONSTRAINT_STRENGTH
# Lower leg: knee → ankle
var k_to_a: Vector3 = _leg_points[side][2] - _leg_points[side][1]
if k_to_a.length_squared() > 0.0001:
var target_a: Vector3 = _leg_points[side][1] + k_to_a.normalized() * _leg_lower_length[side]
_leg_points[side][2] += (target_a - _leg_points[side][2]) * CONSTRAINT_STRENGTH
# Write back to skeleton
_orient_bone_toward(skeleton, gx, hip_idx, _leg_points[side][0], _leg_points[side][1])
_orient_bone_toward(skeleton, gx, knee_idx, _leg_points[side][1], _leg_points[side][2])
func _apply_trail_arm(skeleton: Skeleton3D, gx: Transform3D, delta: float) -> void:
if not is_grappling:
return
var shoulder_idx: int = skeleton.find_bone(ARM_BONES["shoulder_l"])
var elbow_idx: int = skeleton.find_bone(ARM_BONES["elbow_l"])
var wrist_idx: int = skeleton.find_bone(ARM_BONES["wrist_l"])
if shoulder_idx < 0 or elbow_idx < 0 or wrist_idx < 0:
return
# Read current world positions (AFTER spine Verlet has rotated the chest)
var shoulder: Vector3 = (gx * skeleton.get_bone_global_pose(shoulder_idx)).origin
var elbow: Vector3 = (gx * skeleton.get_bone_global_pose(elbow_idx)).origin
var wrist: Vector3 = (gx * skeleton.get_bone_global_pose(wrist_idx)).origin
if not _arm_initialized:
_arm_upper_length = shoulder.distance_to(elbow)
_arm_lower_length = elbow.distance_to(wrist)
_arm_initialized = true
# Trail direction: opposite of velocity (arm streams behind movement)
var trail_dir: Vector3
if character_velocity.length() > 1.0:
trail_dir = -character_velocity.normalized()
elif grapple_anchor_dir.length_squared() > 0.01:
trail_dir = -grapple_anchor_dir.normalized()
else:
trail_dir = -local_up
trail_dir = trail_dir.slerp(-local_up, 0.05).normalized()
# Turbulence
var turb_right: Vector3 = trail_dir.cross(local_up).normalized()
if turb_right.length_squared() < 0.01:
turb_right = trail_dir.cross(Vector3.RIGHT).normalized()
var turb_fwd: Vector3 = turb_right.cross(trail_dir).normalized()
var speed_amp: float = clampf(character_velocity.length() / 8.0, 0.2, 1.0)
var elbow_phase: float = _turbulence_time * 4.7
var elbow_turb: Vector3 = turb_right * sin(elbow_phase) * 0.06 * speed_amp + turb_fwd * sin(elbow_phase * 0.7 + 1.0) * 0.04 * speed_amp
var elbow_target: Vector3 = shoulder + (trail_dir + elbow_turb).normalized() * _arm_upper_length
var wrist_phase: float = _turbulence_time * 4.7 + 1.8
var wrist_turb: Vector3 = turb_right * sin(wrist_phase) * 0.10 * speed_amp + turb_fwd * sin(wrist_phase * 0.7 + 1.0) * 0.07 * speed_amp
var wrist_dir: Vector3 = (trail_dir + wrist_turb).normalized()
var wrist_target: Vector3 = elbow_target + wrist_dir * _arm_lower_length
_orient_bone_toward(skeleton, gx, shoulder_idx, shoulder, elbow_target)
_orient_bone_toward(skeleton, gx, elbow_idx, elbow_target, wrist_target)
func _orient_bone_toward(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
var bone_global: Transform3D = gx * skeleton.get_bone_global_pose(bone_idx)
var current_dir: Vector3 = bone_global.basis.y.normalized() # Rigify: Y = bone direction
var dot: float = current_dir.dot(target_dir)
if dot > 0.9999:
return
var axis: Vector3
var angle: float
if dot < -0.9999:
axis = Vector3.UP.cross(current_dir)
if axis.length_squared() < 0.001:
axis = Vector3.RIGHT.cross(current_dir)
axis = axis.normalized()
angle = PI
else:
axis = current_dir.cross(target_dir).normalized()
angle = current_dir.angle_to(target_dir)
var rotate: Quaternion = Quaternion(axis, angle)
var parent_idx: int = skeleton.get_bone_parent(bone_idx)
if parent_idx < 0:
var current_rot: Quaternion = skeleton.get_bone_pose_rotation(bone_idx)
skeleton.set_bone_pose_rotation(bone_idx, rotate * current_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)