484 lines
20 KiB
GDScript
484 lines
20 KiB
GDScript
# zero_g_move_controller.gd
|
|
extends Node
|
|
class_name ZeroGMovementComponent
|
|
|
|
## References
|
|
var pawn: CharacterPawn3D
|
|
var camera_pivot: Node3D
|
|
|
|
## State & Parameters
|
|
var current_grip: GripArea3D = null # Use GripArea3D type hint
|
|
var nearby_grips: Array[GripArea3D] = []
|
|
|
|
# --- Reach Parameters ---
|
|
@export var reach_speed: float = 10.0 # Speed pawn moves towards grip
|
|
@export var reach_orient_speed: float = 10.0 # Speed pawn orients to grip
|
|
|
|
# --- Grip damping parameters ---
|
|
@export var gripping_linear_damping: float = 50.0 # How quickly velocity stops
|
|
@export var gripping_linear_kd: float = 2 * sqrt(gripping_linear_damping) # How quickly velocity stops
|
|
@export var gripping_angular_damping: float = 5.0 # How quickly spin stops
|
|
@export var gripping_orient_speed: float = 2.0 # How quickly pawn rotates to face grip
|
|
|
|
# --- Climbing parameters ---
|
|
@export var climb_speed: float = 2.0
|
|
@export var grip_handover_distance: float = 1 # How close to next grip to initiate handover
|
|
@export var climb_acceleration: float = 10.0 # How quickly pawn reaches climb_speed
|
|
@export var climb_angle_threshold_deg: float = 120.0 # How wide the forward cone is
|
|
@export var release_past_grip_threshold: float = 0.4 # How far past the grip origin before releasing
|
|
var next_grip_target: GripArea3D = null # The grip we are trying to transition to
|
|
|
|
# --- Launch Parameters ---
|
|
|
|
# --- Seeking Climb State ---
|
|
var _seeking_climb_input: Vector2 = Vector2.ZERO # The move_input held when seeking started
|
|
|
|
@export var launch_charge_rate: float = 1.5
|
|
@export var max_launch_speed: float = 4.0
|
|
var launch_direction: Vector3 = Vector3.ZERO
|
|
var launch_charge: float = 0.0
|
|
|
|
# Enum for internal state
|
|
enum MovementState {
|
|
IDLE,
|
|
REACHING,
|
|
GRIPPING,
|
|
CLIMBING,
|
|
SEEKING_CLIMB,
|
|
CHARGING_LAUNCH
|
|
}
|
|
var current_state: MovementState = MovementState.IDLE:
|
|
set(new_state):
|
|
if new_state == current_state: return
|
|
_on_exit_state(current_state) # Call exit logic for old state
|
|
current_state = new_state
|
|
_on_enter_state(current_state) # Call enter logic for new state
|
|
|
|
func _ready():
|
|
pawn = get_parent() as CharacterPawn3D
|
|
if not pawn: printerr("ZeroGMovementComponent must be child of CharacterPawn3D")
|
|
camera_pivot = pawn.get_node_or_null("CameraPivot")
|
|
if not camera_pivot: printerr("ZeroGMovementComponent couldn't find CameraPivot")
|
|
|
|
# --- Standardized Movement API ---
|
|
|
|
## Called by Pawn when relevant state is active (e.g., GRABBING_GRIP, REACHING_MOVE)
|
|
func process_movement(delta: float, move_input: Vector2, vertical_input: float, roll_input: float, reach_input: PlayerController3D.KeyInput, release_input: PlayerController3D.KeyInput):
|
|
if not is_instance_valid(pawn): return
|
|
|
|
_update_state(
|
|
delta,
|
|
move_input,
|
|
reach_input,
|
|
release_input
|
|
)
|
|
|
|
match current_state:
|
|
MovementState.IDLE:
|
|
_process_idle(delta, move_input, vertical_input, roll_input, release_input)
|
|
MovementState.REACHING:
|
|
_process_reaching(delta)
|
|
MovementState.GRIPPING:
|
|
_apply_grip_physics(delta, move_input, roll_input)
|
|
MovementState.CLIMBING:
|
|
_apply_climb_physics(delta, move_input)
|
|
MovementState.SEEKING_CLIMB:
|
|
_process_seeking_climb(delta, move_input)
|
|
MovementState.CHARGING_LAUNCH:
|
|
_handle_launch_charge(delta)
|
|
|
|
|
|
# === STATE MACHINE ===
|
|
func _on_enter_state(state : MovementState):
|
|
print("ZeroGMovementComponent activated for state: ", MovementState.keys()[state])
|
|
# TODO: Use forces to match velocity to grip
|
|
# if state == MovementState.GRIPPING:
|
|
# pawn.velocity = Vector3.ZERO
|
|
# pawn.angular_velocity = Vector3.ZERO
|
|
# else: # e.g., REACHING_MOVE?
|
|
# state = MovementState.IDLE # Or SEARCHING?
|
|
|
|
func _on_exit_state(state: MovementState):
|
|
print("ZeroGMovementComponent deactivated for state: ", MovementState.keys()[state])
|
|
pass
|
|
|
|
# Ensure grip is released if state changes unexpectedly
|
|
#if state == MovementState.GRIPPING:
|
|
#_release_current_grip()
|
|
|
|
func _update_state(
|
|
_delta: float,
|
|
move_input: Vector2,
|
|
reach_input: PlayerController3D.KeyInput,
|
|
release_input: PlayerController3D.KeyInput,
|
|
):
|
|
match current_state:
|
|
MovementState.IDLE:
|
|
# Already handled initiating reach in process_movement
|
|
if reach_input.pressed or reach_input.held:
|
|
current_state = MovementState.REACHING
|
|
MovementState.REACHING:
|
|
# TODO: If reach animation completes/hand near target -> GRIPPING
|
|
# If interact released during reach -> CANCEL -> IDLE
|
|
if _seeking_climb_input != Vector2.ZERO:
|
|
# We are in a "seek-climb-reach" chain. Cancel if move input stops.
|
|
if move_input == Vector2.ZERO:
|
|
# This will transition state to IDLE
|
|
_cancel_reach()
|
|
elif not (reach_input.pressed or reach_input.held):
|
|
# This was a normal reach initiated by click. Cancel if click is released.
|
|
_cancel_reach()
|
|
|
|
MovementState.GRIPPING:
|
|
# print("ZeroGMovementComponent: Gripping State Active")
|
|
if release_input.pressed or release_input.held or not is_instance_valid(current_grip):
|
|
_release_current_grip(move_input)
|
|
return
|
|
|
|
# Check for launch charge *before* checking for climb, as it's a more specific action.
|
|
if (reach_input.pressed or reach_input.held) and move_input != Vector2.ZERO:
|
|
_start_charge(move_input)
|
|
return
|
|
elif move_input != Vector2.ZERO:
|
|
_start_climb(move_input) # This is overshadowed by the above check.
|
|
MovementState.CLIMBING:
|
|
if reach_input.pressed or reach_input.held:
|
|
_start_charge(move_input)
|
|
return
|
|
if release_input.pressed or release_input.held or not is_instance_valid(current_grip):
|
|
_stop_climb(true) # Release grip and stop
|
|
return
|
|
if move_input == Vector2.ZERO: # Player stopped giving input
|
|
_stop_climb(false) # Stop moving, return to GRIPPING
|
|
return
|
|
# Continue climbing logic (finding next grip) happens in _process_climbing
|
|
MovementState.CHARGING_LAUNCH:
|
|
if not (reach_input.pressed or reach_input.held):
|
|
_execute_launch(move_input)
|
|
elif move_input == Vector2.ZERO: # Cancel charge while holding interact
|
|
current_state = MovementState.GRIPPING
|
|
print("ZeroGMovementComponent: Cancelled Launch Charge")
|
|
|
|
|
|
# === MOVEMENT PROCESSING ===
|
|
func _process_idle(delta: float, move_input: Vector2, vertical_input: float, roll_input: float, release_input: PlayerController3D.KeyInput):
|
|
# State is IDLE (free-floating).
|
|
# Check for EVA suit usage.
|
|
var has_movement_input = (move_input != Vector2.ZERO or vertical_input != 0.0 or roll_input != 0.0)
|
|
if has_movement_input and is_instance_valid(pawn.eva_suit_component):
|
|
# Use EVA suit
|
|
pawn.eva_suit_component.apply_thrusters(pawn, delta, move_input, vertical_input, roll_input)
|
|
|
|
# Check for body orientation (if applicable)
|
|
if release_input.held and is_instance_valid(pawn.eva_suit_component):
|
|
pawn.eva_suit_component._orient_pawn(delta) # Use suit's orient
|
|
|
|
func _process_reaching(_delta: float):
|
|
# TODO: Drive IK target towards current_grip.get_grip_transform().origin
|
|
# TODO: Monitor distance / animation state
|
|
# For now, we just instantly grip.
|
|
if _seeking_climb_input != Vector2.ZERO:
|
|
_attempt_grip(next_grip_target) # Complete the seek-reach
|
|
else:
|
|
_attempt_grip(_find_best_grip())
|
|
|
|
func _apply_grip_physics(delta: float, _move_input: Vector2, roll_input: float):
|
|
if not is_instance_valid(pawn) or not is_instance_valid(current_grip):
|
|
_release_current_grip(); return
|
|
|
|
# TODO: Later, replace step 2 and 3 with IK driving the hand bone to the target_transform.origin,
|
|
# while the physics/orientation logic stops the main body's momentum.
|
|
|
|
# --- 1. Calculate Target Transform (Same as before) ---
|
|
var grip_base_transform = current_grip.global_transform
|
|
var target_direction = grip_base_transform.basis.z.normalized()
|
|
var hold_distance = _get_hold_distance()
|
|
var target_position = grip_base_transform.origin + target_direction * hold_distance
|
|
|
|
var target_basis = _choose_grip_orientation(grip_base_transform.basis)
|
|
|
|
# --- 2. Apply Linear Force (PD Controller) ---
|
|
var error_pos = target_position - pawn.global_position
|
|
# Simple P-controller for velocity (acts as a spring)
|
|
|
|
# We get the force from the PD controller and apply it as acceleration.
|
|
var force = MotionUtils.calculate_pd_position_force(
|
|
target_position,
|
|
pawn.global_position,
|
|
pawn.linear_velocity, # Use linear_velocity (from RigidBody3D)
|
|
gripping_linear_damping, # Kp
|
|
gripping_linear_damping # Kd
|
|
)
|
|
|
|
# Simple D-controller (damping)
|
|
# target_velocity_pos -= pawn.linear_velocity * gripping_angular_damping # 'angular_damping' here acts as Kd
|
|
# TODO: Add less force the smaller error_pos is to stop ocillating around target pos
|
|
pawn.apply_central_force((force / pawn.mass) * delta)
|
|
|
|
# --- 3. Apply Angular Force (PD Controller) ---
|
|
if not is_zero_approx(roll_input):
|
|
# Manual Roll Input (applies torque)
|
|
var roll_torque_global = pawn.global_transform.basis.z * (-roll_input) * gripping_orient_speed # Use global Z
|
|
pawn.apply_torque(roll_torque_global * delta)
|
|
else:
|
|
# Auto-Orient (PD Controller)
|
|
_apply_orientation_torque(target_basis, delta)
|
|
|
|
func _apply_climb_physics(delta: float, move_input: Vector2):
|
|
if not is_instance_valid(pawn) or not is_instance_valid(current_grip):
|
|
_stop_climb(true); return
|
|
|
|
# 1. Calculate Climb Direction: For climbing we interpret W as up from the pawns perspective instead of forward
|
|
var climb_direction = move_input.y * pawn.global_basis.y + move_input.x * pawn.global_basis.x
|
|
climb_direction = climb_direction.normalized()
|
|
|
|
# 2. Find Next Grip
|
|
next_grip_target = _find_best_grip(climb_direction, INF, climb_angle_threshold_deg)
|
|
|
|
# 3. Check for Handover: This should be more eager to mark a new grip as current than below check is to release when climbing past
|
|
var performed_handover = _attempt_grip(next_grip_target)
|
|
|
|
# 4. Check for Release Past Grip (if no handover)
|
|
if not performed_handover:
|
|
var current_grip_pos = current_grip.global_position
|
|
var vector_from_grip_to_pawn = pawn.global_position - current_grip_pos
|
|
var distance_along_climb_dir = vector_from_grip_to_pawn.dot(climb_direction)
|
|
if distance_along_climb_dir > release_past_grip_threshold: # Release threshold
|
|
_release_current_grip(move_input)
|
|
return # State changed to IDLE
|
|
|
|
# 5. Apply Movement Force
|
|
var target_velocity = climb_direction * climb_speed
|
|
var error_vel = target_velocity - pawn.linear_velocity
|
|
var force = error_vel * climb_acceleration # Kp = climb_acceleration
|
|
pawn.apply_central_force(force * delta)
|
|
|
|
# 6. Apply Angular Force (Auto-Orient to current grip)
|
|
var grip_base_transform = current_grip.global_transform
|
|
var target_basis = _choose_grip_orientation(grip_base_transform.basis)
|
|
_apply_orientation_torque(target_basis, delta)
|
|
|
|
|
|
func _process_seeking_climb(_delta: float, move_input: Vector2):
|
|
# If the player's input has changed from what initiated the seek, cancel it.
|
|
if not move_input.is_equal_approx(_seeking_climb_input):
|
|
var target_grip = _find_best_grip()
|
|
_seeking_climb_input = Vector2.ZERO # Reset for next time
|
|
if _attempt_grip(target_grip):
|
|
# Successfully found and grabbed a grip. The state is now GRIPPING.
|
|
print("Seeking Climb ended, gripped new target.")
|
|
else:
|
|
current_state = MovementState.IDLE
|
|
# No grip found. Transition to IDLE.
|
|
print("Seeking Climb ended, no grip found. Reverting to IDLE.")
|
|
|
|
# --- Grip Helpers
|
|
|
|
## The single, authoritative function for grabbing a grip.
|
|
func _attempt_grip(target_grip: GripArea3D) -> bool:
|
|
if not is_instance_valid(target_grip):
|
|
return false
|
|
|
|
if target_grip.grab(pawn):
|
|
# Successfully grabbed the new grip.
|
|
var old_grip = current_grip
|
|
if is_instance_valid(old_grip) and old_grip != target_grip:
|
|
old_grip.release(pawn)
|
|
|
|
current_grip = target_grip
|
|
next_grip_target = null
|
|
_seeking_climb_input = Vector2.ZERO
|
|
|
|
# If we weren't already climbing, transition to GRIPPING state.
|
|
if current_state != MovementState.CLIMBING:
|
|
current_state = MovementState.GRIPPING
|
|
|
|
print("Successfully gripped: ", current_grip.get_parent().name)
|
|
return true
|
|
else:
|
|
# Failed to grab the new grip.
|
|
print("Failed to grip: ", target_grip.get_parent().name, " (likely occupied).")
|
|
if current_state == MovementState.CLIMBING:
|
|
_stop_climb(false) # Stop climbing, return to gripping previous one
|
|
return false
|
|
|
|
# --- Grip Orientation Helper ---
|
|
func _choose_grip_orientation(grip_basis: Basis) -> Basis:
|
|
# 1. Define the two possible target orientations based on the grip.
|
|
# Both will look away from the grip's surface (-Z).
|
|
var look_at_dir = -grip_basis.z.normalized()
|
|
var target_basis_up = Basis.looking_at(look_at_dir, grip_basis.y.normalized()).orthonormalized()
|
|
var target_basis_down = Basis.looking_at(look_at_dir, -grip_basis.y.normalized()).orthonormalized()
|
|
|
|
# 2. Get the pawn's current orientation.
|
|
var current_basis = pawn.global_basis
|
|
|
|
# 3. Compare which target orientation is "closer" to the current one.
|
|
# We can do this by finding the angle of rotation needed to get from current to each target.
|
|
# The quaternion dot product is related to the angle between orientations. A larger absolute dot product means a smaller angle.
|
|
var dot_up = current_basis.get_rotation_quaternion().dot(target_basis_up.get_rotation_quaternion())
|
|
var dot_down = current_basis.get_rotation_quaternion().dot(target_basis_down.get_rotation_quaternion())
|
|
|
|
# We choose the basis that results in a larger absolute dot product (smaller rotational distance).
|
|
return target_basis_up if abs(dot_up) >= abs(dot_down) else target_basis_down
|
|
|
|
# --- Grip Selection Logic ---
|
|
# Finds the best grip based on direction, distance, and angle constraints
|
|
func _find_best_grip(direction := Vector3.ZERO, max_distance_sq := INF, angle_threshold_deg := 120.0) -> GripArea3D:
|
|
var best_grip: GripArea3D = null
|
|
var min_dist_sq = max_distance_sq # Start checking against max allowed distance
|
|
|
|
var use_direction_filter = direction != Vector3.ZERO
|
|
var max_allowed_angle_rad = 0.0 # Initialize
|
|
if use_direction_filter:
|
|
# Calculate the maximum allowed angle deviation from the center direction
|
|
max_allowed_angle_rad = deg_to_rad(angle_threshold_deg) / 2.0
|
|
|
|
# Iterate through all grips detected by the pawn
|
|
for grip in nearby_grips:
|
|
# Basic validity checks
|
|
if not is_instance_valid(grip) or grip == current_grip or not grip.can_grab(pawn):
|
|
continue
|
|
|
|
var grip_pos = grip.global_position
|
|
# Use direction_to which automatically normalizes
|
|
var dir_to_grip = pawn.global_position.direction_to(grip_pos)
|
|
var dist_sq = pawn.global_position.distance_squared_to(grip_pos)
|
|
|
|
# Check distance first
|
|
if dist_sq >= min_dist_sq: # Use >= because we update min_dist_sq later
|
|
continue
|
|
|
|
# If using direction filter, check angle constraint
|
|
if use_direction_filter:
|
|
# Ensure the direction vector we compare against is normalized
|
|
var normalized_direction = direction.normalized()
|
|
# Calculate the dot product
|
|
var dot = dir_to_grip.dot(normalized_direction)
|
|
# Clamp dot product to handle potential floating-point errors outside [-1, 1]
|
|
dot = clamp(dot, -1.0, 1.0)
|
|
# Calculate the actual angle between the vectors in radians
|
|
var angle_rad = acos(dot)
|
|
|
|
# Check if the calculated angle exceeds the maximum allowed deviation
|
|
if angle_rad > max_allowed_angle_rad:
|
|
# print("Grip ", grip.get_parent().name, " outside cone. Angle: ", rad_to_deg(angle_rad), " > ", rad_to_deg(max_allowed_angle_rad))
|
|
continue # Skip this grip if it's outside the cone
|
|
|
|
# If it passes all filters and is closer than the previous best:
|
|
min_dist_sq = dist_sq
|
|
best_grip = grip
|
|
|
|
# if is_instance_valid(best_grip):
|
|
# print("Best grip found: ", best_grip.get_parent().name, " at distance squared: ", min_dist_sq)
|
|
|
|
return best_grip
|
|
|
|
# --- Reaching Helpers ---
|
|
func _get_hold_distance() -> float:
|
|
# Use the pawn.grip_detector.position.length() method if you prefer that:
|
|
if is_instance_valid(pawn) and is_instance_valid(pawn.grip_detector):
|
|
return pawn.grip_detector.position.length()
|
|
else:
|
|
return 0.5 # Fallback distance if detector isn't set up right
|
|
|
|
func _release_current_grip(move_input: Vector2 = Vector2.ZERO):
|
|
if is_instance_valid(current_grip):
|
|
current_grip.release(pawn)
|
|
current_grip = null
|
|
|
|
# If we were climbing and are still holding a climb input, start seeking.
|
|
if move_input != Vector2.ZERO:
|
|
current_state = MovementState.SEEKING_CLIMB
|
|
_seeking_climb_input = move_input # Store the input that started the seek
|
|
# print("ZeroGMovementComponent: Released grip, now SEEKING_CLIMB.")
|
|
else:
|
|
current_state = MovementState.IDLE
|
|
# print("ZeroGMovementComponent: Released grip, now IDLE.")
|
|
|
|
|
|
func _cancel_reach():
|
|
# TODO: Logic to stop IK/animation if reach is cancelled mid-way
|
|
_release_current_grip(Vector2.ZERO) # Ensure grip reference is cleared
|
|
print("ZeroGMovementComponent: Reach cancelled.")
|
|
|
|
# --- Climbing Helpers ---
|
|
func _start_climb(move_input: Vector2):
|
|
if not is_instance_valid(current_grip): return
|
|
current_state = MovementState.CLIMBING
|
|
|
|
# Calculate initial climb direction based on input relative to camera/grip
|
|
var pawn_up = pawn.global_basis.y
|
|
var pawn_right = pawn.global_basis.x
|
|
|
|
print("ZeroGMoveController: Started Climbing in direction: ", (pawn_up * move_input.y + pawn_right * move_input.x).normalized())
|
|
|
|
func _stop_climb(release_grip: bool):
|
|
# print("ZeroGMoveController: Stopping Climb. Release Grip: ", release_grip)
|
|
# TODO: Implement using forces
|
|
# pawn.velocity = pawn.velocity.lerp(Vector3.ZERO, 0.5) # Apply some braking
|
|
next_grip_target = null
|
|
if release_grip:
|
|
_release_current_grip() # Transitions to IDLE
|
|
else:
|
|
current_state = MovementState.GRIPPING # Go back to stationary gripping
|
|
|
|
func _apply_orientation_torque(target_basis: Basis, delta: float):
|
|
var torque = MotionUtils.calculate_pd_rotation_torque(
|
|
target_basis,
|
|
pawn.global_basis,
|
|
pawn.angular_velocity, # Use angular_velocity (from RigidBody3D)
|
|
gripping_orient_speed, # Kp
|
|
gripping_orient_speed # Kd
|
|
)
|
|
|
|
pawn.apply_torque(torque * delta)
|
|
|
|
# --- Launch helpers ---
|
|
func _start_charge(move_input: Vector2):
|
|
if not is_instance_valid(current_grip): return # Safety check
|
|
current_state = MovementState.CHARGING_LAUNCH
|
|
launch_charge = 0.0
|
|
|
|
# Calculate launch direction based on input and push-off normal
|
|
# The direction is based on the pawn's current orientation, not the camera or grip.
|
|
# This makes it feel like you're pushing off in a direction relative to your body.
|
|
var pawn_up = pawn.global_basis.y
|
|
var pawn_right = pawn.global_basis.x
|
|
launch_direction = (pawn_up * move_input.y + pawn_right * move_input.x).normalized()
|
|
|
|
print("ZeroGMovementComponent: Charging Launch")
|
|
|
|
|
|
func _handle_launch_charge(delta: float):
|
|
launch_charge = min(launch_charge + launch_charge_rate * delta, max_launch_speed)
|
|
|
|
func _execute_launch(move_input: Vector2):
|
|
if not is_instance_valid(current_grip): return # Safety check
|
|
|
|
launch_charge = 0.0
|
|
_release_current_grip(move_input) # Release AFTER calculating direction
|
|
pawn.apply_impulse(launch_direction * launch_charge)
|
|
|
|
# Instead of going to IDLE, go to SEEKING_CLIMB to find the next grip.
|
|
# The move_input that started the launch is what we'll use for the seek direction.
|
|
# _seeking_climb_input = (pawn.global_basis.y.dot(launch_direction) * Vector2.UP) + (pawn.global_basis.x.dot(launch_direction) * Vector2.RIGHT)
|
|
# current_state = MovementState.SEEKING_CLIMB
|
|
print("ZeroGMovementComponent: Launched with speed ", pawn.linear_velocity.length(), " and now SEEKING_CLIMB")
|
|
|
|
|
|
# --- Signal Handlers ---
|
|
func on_grip_area_entered(area: Area3D):
|
|
if area is GripArea3D: # Check if the entered area is actually a GripArea3D node
|
|
var grip = area as GripArea3D
|
|
if not grip in nearby_grips:
|
|
nearby_grips.append(grip)
|
|
# print("Detected nearby grip: ", grip.get_parent().name if grip.get_parent() else "UNKNOWN") # Print parent name for context
|
|
|
|
func on_grip_area_exited(area: Area3D):
|
|
if area is GripArea3D:
|
|
var grip = area as GripArea3D
|
|
if grip in nearby_grips:
|
|
nearby_grips.erase(grip)
|
|
# print("Grip out of range: ", grip.get_parent().name if grip.get_parent() else "UNKNOWN")
|