460 lines
19 KiB
GDScript
460 lines
19 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 = 5.0 # 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 ---
|
|
@export var launch_charge_rate: float = 20.0
|
|
@export var max_launch_speed: float = 15.0
|
|
var launch_direction: Vector3 = Vector3.ZERO
|
|
var launch_charge: float = 0.0
|
|
|
|
# Enum for internal state
|
|
enum MovementState {
|
|
IDLE,
|
|
REACHING,
|
|
GRIPPING,
|
|
CLIMBING,
|
|
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:
|
|
# State is IDLE (free-floating).
|
|
# Check for EVA suit usage.
|
|
var is_moving = (move_input != Vector2.ZERO or vertical_input != 0.0 or roll_input != 0.0)
|
|
if is_moving 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
|
|
MovementState.REACHING:
|
|
_process_reaching(delta)
|
|
MovementState.GRIPPING:
|
|
_apply_grip_physics(delta, move_input, roll_input)
|
|
MovementState.CLIMBING:
|
|
_apply_climb_physics(delta, move_input)
|
|
MovementState.CHARGING_LAUNCH:
|
|
_handle_launch_charge(delta)
|
|
|
|
|
|
## Called by Pawn for collision (optional, might not be needed if grabbing stops movement)
|
|
func handle_collision(collision: KinematicCollision3D, collision_energy_loss: float):
|
|
# Basic bounce if somehow colliding while using this controller
|
|
var surface_normal = collision.get_normal()
|
|
pawn.velocity = pawn.velocity.bounce(surface_normal)
|
|
pawn.velocity *= (1.0 - collision_energy_loss * 0.5)
|
|
|
|
# === STATE MACHINE
|
|
func _on_enter_state(state : MovementState):
|
|
# print("ZeroGMovementComponent activated for state: ", MovementState.keys()[state])
|
|
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])
|
|
|
|
# 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
|
|
# print("ZeroGMovementComponent: Reaching State Active")
|
|
if not (reach_input.pressed or reach_input.held):
|
|
_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()
|
|
# Pawn's main state machine will handle transition out
|
|
if move_input != Vector2.ZERO:
|
|
_start_climb(move_input)
|
|
pass
|
|
MovementState.CLIMBING:
|
|
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
|
|
|
|
# elif move_input != Vector2.ZERO:
|
|
# _start_charge(move_input) # Start charging launch
|
|
# MovementState.CHARGING_LAUNCH:
|
|
# if reaching_released:
|
|
# _execute_launch()
|
|
# # Pawn's main state machine handles transition out
|
|
# elif move_input == Vector2.ZERO: # Cancel charge while holding interact
|
|
# state = MovementState.GRIPPING
|
|
# print("ZeroGMovementComponent: Cancelled Launch Charge")
|
|
|
|
|
|
# === MOVEMENT PROCESSING ===
|
|
func _process_reaching(_delta: float):
|
|
_try_initiate_reach()
|
|
# TODO: Drive IK target towards current_grip.get_grip_transform().origin
|
|
# TODO: Monitor distance / animation state
|
|
# When close enough: state = MovementState.GRIPPING
|
|
pass
|
|
|
|
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 grip_up_vector = grip_base_transform.basis.y.normalized()
|
|
var grip_down_vector = -grip_base_transform.basis.y.normalized()
|
|
var pawn_up_vector = pawn.global_transform.basis.y
|
|
var dot_up = pawn_up_vector.dot(grip_up_vector)
|
|
var dot_down = pawn_up_vector.dot(grip_down_vector)
|
|
var chosen_orientation_up_vector = grip_up_vector if dot_up >= dot_down else grip_down_vector
|
|
var target_basis = Basis.looking_at(-target_direction, chosen_orientation_up_vector).orthonormalized()
|
|
|
|
# --- 2. Apply Linear Force (PD Controller) ---
|
|
var error_pos = target_position - pawn.global_position
|
|
# Simple P-controller for velocity (acts as a spring)
|
|
var target_velocity_pos = error_pos * gripping_linear_damping # 'damping' here acts as Kp
|
|
# Simple D-controller (damping)
|
|
target_velocity_pos -= pawn.velocity * gripping_angular_damping # 'angular_damping' here acts as Kd
|
|
# Apply force via acceleration
|
|
pawn.velocity = pawn.velocity.lerp(target_velocity_pos, delta * 10.0) # Smoothly apply correction
|
|
|
|
# --- 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.add_torque(roll_torque_global, delta)
|
|
else:
|
|
# Auto-Orient (PD Controller)
|
|
var current_quat = pawn.global_transform.basis.get_rotation_quaternion()
|
|
var target_quat = target_basis.get_rotation_quaternion()
|
|
var error_quat = target_quat * current_quat.inverse()
|
|
var error_angle = error_quat.get_angle()
|
|
var error_axis = error_quat.get_axis()
|
|
|
|
# Proportional torque (spring)
|
|
var torque_proportional = error_axis.normalized() * error_angle * gripping_orient_speed # 'speed' acts as Kp
|
|
# Derivative torque (damping)
|
|
var torque_derivative = -pawn.angular_velocity * gripping_angular_damping # 'damping' acts as Kd
|
|
|
|
var total_torque_global = (torque_proportional + torque_derivative)
|
|
pawn.add_torque(total_torque_global, 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 = _perform_grip_handover()
|
|
|
|
# 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()
|
|
return # State changed to IDLE
|
|
|
|
# 5. Apply Movement Force
|
|
var target_velocity = climb_direction * climb_speed
|
|
pawn.velocity = pawn.velocity.lerp(target_velocity, delta * climb_acceleration)
|
|
|
|
# 6. Apply Angular Force (Auto-Orient to current grip)
|
|
var grip_base_transform = current_grip.global_transform
|
|
var target_direction = grip_base_transform.basis.z.normalized()
|
|
var grip_up_vector = grip_base_transform.basis.y.normalized()
|
|
var grip_down_vector = -grip_base_transform.basis.y.normalized()
|
|
var pawn_up_vector = pawn.global_transform.basis.y
|
|
var dot_up = pawn_up_vector.dot(grip_up_vector)
|
|
var dot_down = pawn_up_vector.dot(grip_down_vector)
|
|
var chosen_orientation_up_vector = grip_up_vector if dot_up >= dot_down else grip_down_vector
|
|
var target_basis = Basis.looking_at(-target_direction, chosen_orientation_up_vector).orthonormalized()
|
|
|
|
var current_quat = pawn.global_transform.basis.get_rotation_quaternion()
|
|
var target_quat = target_basis.get_rotation_quaternion()
|
|
var error_quat = target_quat * current_quat.inverse()
|
|
var error_angle = error_quat.get_angle()
|
|
var error_axis = error_quat.get_axis()
|
|
|
|
var torque_proportional = error_axis.normalized() * error_angle * gripping_orient_speed
|
|
var torque_derivative = -pawn.angular_velocity * gripping_angular_damping
|
|
var total_torque_global = (torque_proportional + torque_derivative)
|
|
pawn.add_torque(total_torque_global, delta)
|
|
|
|
# --- Grip Helpers
|
|
# Attempts to find and grab the best available grip within range
|
|
func _try_initiate_reach():
|
|
var closest_grip: GripArea3D = _find_best_grip()
|
|
|
|
if is_instance_valid(closest_grip):
|
|
current_grip = closest_grip
|
|
current_state = MovementState.GRIPPING # Set internal state
|
|
print("ZeroGMovementComponent: Initiated grab on ", current_grip.get_parent().name)
|
|
# else:
|
|
# print("ZeroGMovementComponent: Grab failed (grip occupied?)")
|
|
else:
|
|
print("ZeroGMovementComponent: No available grips in range to initiate reach.")
|
|
# TODO: Initiate generic surface grab?
|
|
|
|
# --- Grip Orientation Helper ---
|
|
func _choose_grip_orientation(grip_basis: Basis) -> Basis:
|
|
var grip_up_vector = grip_basis.y.normalized()
|
|
var grip_down_vector = -grip_basis.y.normalized()
|
|
var pawn_up_vector = pawn.global_transform.basis.y
|
|
|
|
var dot_up = pawn_up_vector.dot(grip_up_vector)
|
|
var dot_down = pawn_up_vector.dot(grip_down_vector)
|
|
|
|
var chosen_orientation_up_vector = grip_up_vector if dot_up >= dot_down else grip_down_vector
|
|
return Basis.looking_at(-grip_basis.z.normalized(), chosen_orientation_up_vector).orthonormalized()
|
|
|
|
# --- 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
|
|
|
|
# --- Helper for Hold Distance ---
|
|
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():
|
|
if is_instance_valid(current_grip):
|
|
current_grip.release(pawn)
|
|
current_grip = null
|
|
current_state = MovementState.IDLE
|
|
print("ZeroGMovementComponent: Released grip and returned to FLOATING state.")
|
|
|
|
|
|
func _cancel_reach():
|
|
# TODO: Logic to stop IK/animation if reach is cancelled mid-way
|
|
_release_current_grip() # Ensure grip reference is cleared
|
|
current_state = MovementState.IDLE
|
|
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)
|
|
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 _perform_grip_handover() -> bool:
|
|
if not is_instance_valid(next_grip_target): return false
|
|
|
|
print("Attempting handover to: ", next_grip_target.get_parent().name)
|
|
if next_grip_target.grab(pawn):
|
|
# Successfully grabbed the next one
|
|
if is_instance_valid(current_grip):
|
|
current_grip.release(pawn) # Release the old one
|
|
current_grip = next_grip_target # Update current grip reference
|
|
next_grip_target = null # Clear the target
|
|
print("Handover successful. New grip: ", current_grip.get_parent().name)
|
|
# Stay in CLIMBING state, velocity continues
|
|
return true # Indicate success
|
|
else:
|
|
# Failed to grab next grip (e.g., became occupied)
|
|
print("Handover failed - couldn't grab next grip.")
|
|
_stop_climb(false) # Stop climbing, return to gripping previous one
|
|
return false # Indicate failure
|
|
|
|
|
|
# --- Launch helpers ---
|
|
func _start_charge(move_input: Vector2):
|
|
if not is_instance_valid(current_grip): return
|
|
current_state = MovementState.CHARGING_LAUNCH
|
|
launch_charge = 0.0
|
|
|
|
# Calculate launch direction based on input and push-off normal
|
|
var push_dir_local = (Vector3.FORWARD * -move_input.y + Vector3.RIGHT * move_input.x).normalized()
|
|
var push_normal = current_grip.get_push_off_normal()
|
|
# Basis oriented away from surface, using pawn's current up as reference
|
|
var _surface_basis = Basis.looking_at(push_normal, pawn.global_transform.basis.y).orthonormalized()
|
|
|
|
var input_influence = 0.5 # Blend between pushing straight off and sliding along input dir
|
|
var input_dir_world = pawn.camera_pivot.global_transform.basis * push_dir_local # Convert input dir relative to camera/head
|
|
var push_dir_along_surface = input_dir_world.slide(push_normal).normalized()
|
|
|
|
launch_direction = (push_normal * (1.0 - input_influence) + push_dir_along_surface * input_influence).normalized()
|
|
print("ZeroGMovementComponent: Charging Launch")
|
|
|
|
|
|
func _handle_launch_charge(delta: float):
|
|
launch_charge = min(launch_charge + launch_charge_rate * delta, max_launch_speed)
|
|
pawn.velocity = Vector3.ZERO
|
|
pawn.angular_velocity = Vector3.ZERO
|
|
|
|
|
|
func _execute_launch():
|
|
if not is_instance_valid(current_grip): return # Safety check
|
|
_release_current_grip() # Release AFTER calculating direction
|
|
pawn.velocity = launch_direction * launch_charge # Apply launch velocity to pawn
|
|
launch_charge = 0.0
|
|
current_state = MovementState.IDLE
|
|
print("ZeroGMovementComponent: Launched with speed ", pawn.velocity.length())
|
|
|
|
|
|
# --- Signal Handlers ---
|
|
func on_grip_area_entered(area: Area3D):
|
|
print("Area detected")
|
|
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")
|