Added logic to properly handle movement and teleport impact on collis…

…ion hands

addons/godot-xr-tools/desktop-support/movement_desktop_flight.gd

@@ -139,7 +139,7 @@ func physics_movement(delta: float, player_body: XRToolsPlayerBody, disabled: bo
 
 	# If exclusive then perform the exclusive move-and-slide
 	if exclusive:
-		player_body.velocity = player_body.move_body(flight_velocity)
+		player_body.velocity = player_body.move_player(flight_velocity)
 		return true
 
 	# Update velocity and return for additional effects

addons/godot-xr-tools/functions/movement_flight.gd

@@ -186,7 +186,7 @@ func physics_movement(delta: float, player_body: XRToolsPlayerBody, disabled: bo
 
 	# If exclusive then perform the exclusive move-and-slide
 	if exclusive:
-		player_body.velocity = player_body.move_body(flight_velocity)
+		player_body.velocity = player_body.move_player(flight_velocity)
 		return true
 
 	# Update velocity and return for additional effects

addons/godot-xr-tools/functions/movement_glide.gd

@@ -190,7 +190,7 @@ func physics_movement(delta: float, player_body: XRToolsPlayerBody, disabled: bo
 
 	# Perform the glide
 	var glide_velocity := horizontal_velocity + vertical_velocity * player_body.up_gravity
-	player_body.velocity = player_body.move_body(glide_velocity)
+	player_body.velocity = player_body.move_player(glide_velocity)
 
 	# Report exclusive motion performed (to bypass gravity)
 	return true

addons/godot-xr-tools/functions/movement_grapple.gd

@@ -191,7 +191,7 @@ func physics_movement(delta: float, player_body: XRToolsPlayerBody, disabled: bo
 	player_body.velocity *= 1.0 - friction * delta
 
 	# Perform exclusive movement as we have dealt with gravity
-	player_body.velocity = player_body.move_body(player_body.velocity)
+	player_body.velocity = player_body.move_player(player_body.velocity)
 	return true
 
 

addons/godot-xr-tools/functions/movement_turn.gd

@@ -83,6 +83,7 @@ func physics_movement(delta: float, player_body: XRToolsPlayerBody, _disabled: b
 	# Turn one step in the requested direction
 	if step_turn_delay != 0.0:
 		_turn_step = step_turn_delay
+
 	player_body.rotate_player(deg_to_rad(step_turn_angle) * sign(left_right))
 
 

addons/godot-xr-tools/functions/movement_world_grab.gd

@@ -139,7 +139,7 @@ func physics_movement(delta: float, player_body: XRToolsPlayerBody, disabled: bo
 
 	# Move the player by the offset
 	var old_position := player_body.global_position
-	player_body.move_body(-offset / delta)
+	player_body.move_player(-offset / delta)
 	player_body.velocity = Vector3.ZERO
 	#player_body.move_and_collide(-offset)
 

addons/godot-xr-tools/hands/collision_hand.gd

@@ -75,7 +75,7 @@ const TELEPORT_DISTANCE := 1.0
 
 ## Force we exert on a picked up object when hand is at maximum distance
 ## before letting go.
-@export_range(1.0, 1000.0, 0.1, "suffix:N") var max_pickup_force : float = 400.0
+@export_range(1.0, 1000.0, 0.1, "suffix:N") var max_pickup_force : float = 300.0
 
 
 # Controller to target (if no target overrides)
@@ -168,6 +168,12 @@ func _ready():
 	process_physics_priority = -90
 	sync_to_physics = false
 
+	# Connect to player body signals (if applicable)
+	var player_body = XRToolsPlayerBody.find_instance(self)
+	if player_body:
+		player_body.player_moved.connect(_on_player_moved)
+		player_body.player_teleported.connect(_on_player_teleported)
+
 	# Populate nodes
 	_controller = XRTools.find_xr_ancestor(self, "*", "XRController3D")
 
@@ -262,12 +268,14 @@ func _move_to_target(delta):
 
 	# Handle too far from target
 	if global_position.distance_to(_target.global_position) > TELEPORT_DISTANCE:
+		print("max distance reached")
 		max_distance_reached.emit()
 
 		global_transform = _target.global_transform
 		return
 
 	# Orient the hand
+	# TODO: should check for collisions along this rotation
 	global_transform.basis = _target.global_transform.basis
 
 	# Adjust target position if we're holding something
@@ -307,6 +315,35 @@ func _move_to_target(delta):
 	force_update_transform()
 
 
+# If our player moved, attempt to move our hand but ignoring weight.
+func _on_player_moved(delta_transform : Transform3D):
+	if mode == CollisionHandMode.DISABLED:
+		return
+
+	if mode == CollisionHandMode.TELEPORT:
+		_on_player_teleported(delta_transform)
+		return
+
+	var target : Transform3D = delta_transform * global_transform
+
+	# Rotate
+	# TODO: should check for collisions along this rotation
+	global_basis = target.basis
+
+	# And attempt to move
+	move_and_slide(target.origin - global_position)
+	force_update_transform()
+
+
+# If our player teleported, just move.
+func _on_player_teleported(delta_transform : Transform3D):
+	if mode == CollisionHandMode.DISABLED:
+		return
+
+	global_transform = delta_transform * global_transform
+	force_update_transform()
+
+
 # This function inserts a target override into the overrides list by priority
 # order.
 func _insert_target_override(target : Node3D, priority : int) -> void:

addons/godot-xr-tools/hands/scenes/collision/collision_hand.tscn

@@ -4,6 +4,7 @@
 
 [node name="XRToolsCollisionHand" type="AnimatableBody3D"]
 collision_layer = 131072
-collision_mask = 327711
+collision_mask = 262175
 sync_to_physics = false
 script = ExtResource("1_vdcct")
+max_pickup_force = 400.0

addons/godot-xr-tools/hands/scenes/collision/collision_hand_left.tscn

@@ -5,7 +5,7 @@
 
 [node name="CollisionHandLeft" type="AnimatableBody3D"]
 collision_layer = 131072
-collision_mask = 327711
+collision_mask = 262175
 sync_to_physics = false
 script = ExtResource("1_t5acd")
 

addons/godot-xr-tools/hands/scenes/collision/collision_hand_right.tscn

@@ -5,7 +5,7 @@
 
 [node name="CollisionHandRight" type="AnimatableBody3D"]
 collision_layer = 131072
-collision_mask = 327711
+collision_mask = 262175
 sync_to_physics = false
 script = ExtResource("1_so3hf")
 

addons/godot-xr-tools/objects/force_body/force_body.gd

@@ -97,3 +97,7 @@ func move_and_slide(move : Vector3) -> ForceBodyCollision:
 
 	# Return the last collision data
 	return ret
+
+
+func _ready():
+	process_physics_priority = -90

addons/godot-xr-tools/objects/grab_points/grab.gd

@@ -56,6 +56,7 @@ func _init(
 	controller = p_grabber.controller
 	hand = p_grabber.hand
 	collision_hand = p_grabber.collision_hand
+	player_body = p_grabber.player_body
 
 	# Set the point
 	what = p_what

addons/godot-xr-tools/objects/grab_points/grab_driver.gd

@@ -30,6 +30,8 @@ var lerp_duration : float = 1.0
 ## Lerp time
 var lerp_time : float = 0.0
 
+## Player body
+var player_body : XRToolsPlayerBody
 
 # Called every frame. 'delta' is the elapsed time since the previous frame.
 func _physics_process(delta : float) -> void:
@@ -89,6 +91,7 @@ func _physics_process(delta : float) -> void:
 			else:
 				# Lerp completed
 				state = GrabState.SNAP
+				_update_player_body()
 				_update_weight()
 				if primary: primary.set_arrived()
 				if secondary: secondary.set_arrived()
@@ -113,6 +116,7 @@ func add_grab(p_grab : Grab) -> void:
 
 	# If snapped then report arrived at the new grab
 	if state == GrabState.SNAP:
+		_update_player_body()
 		_update_weight()
 		p_grab.set_arrived()
 
@@ -141,6 +145,7 @@ func remove_grab(p_grab : Grab) -> void:
 		secondary = null
 
 	if state == GrabState.SNAP:
+		_update_player_body()
 		_update_weight()
 
 
@@ -209,6 +214,7 @@ static func create_snap(
 	p_target.get_parent().add_child(driver)
 	driver.remote_path = driver.get_path_to(p_target)
 
+	driver._update_player_body()
 	driver._update_weight()
 
 	# Return the driver
@@ -223,6 +229,47 @@ static func _vote(a : float, b : float) -> float:
 	return b / (a + b)
 
 
+# Check player_body
+func _update_player_body():
+	var new_player_body : XRToolsPlayerBody
+
+	if primary and primary.player_body:
+		new_player_body = primary.player_body
+	elif secondary and secondary.player_body:
+		new_player_body = secondary.player_body
+
+	# Already set to this one?
+	if player_body == new_player_body:
+		return
+
+	# Out with the old
+	if player_body:
+		player_body.player_moved.disconnect(_on_player_moved)
+		player_body.player_teleported.disconnect(_on_player_teleported)
+
+	# in with the new
+	player_body = new_player_body
+	if player_body:
+		player_body.player_moved.connect(_on_player_moved)
+		player_body.player_teleported.connect(_on_player_teleported)
+
+
+# Our player has moved, move our grab driver by the same amount
+func _on_player_moved(delta_transform : Transform3D):
+	global_transform = delta_transform * global_transform
+	force_update_transform()
+	if is_instance_valid(target):
+		target.force_update_transform()
+
+
+# Our player has teleported, move our grab driver by the same amount
+func _on_player_teleported(delta_transform : Transform3D):
+	global_transform = delta_transform * global_transform
+	force_update_transform()
+	if is_instance_valid(target):
+		target.force_update_transform()
+
+
 # Update the weight on collision hands
 func _update_weight():
 	if primary:

addons/godot-xr-tools/objects/grab_points/grabber.gd

@@ -22,11 +22,15 @@ var hand : XRToolsHand
 ## Collision hand associated with the grabber
 var collision_hand : XRToolsCollisionHand
 
+## Player body (if applicable)
+var player_body : XRToolsPlayerBody
 
 ## Initialize the grabber
 func _init(p_by : Node3D) -> void:
 	by = p_by
 	pickup = p_by as XRToolsFunctionPickup
 	controller = pickup.get_controller() if pickup else null
-	hand = XRToolsHand.find_instance(controller)
-	collision_hand = XRToolsCollisionHand.find_instance(controller)
+	if controller:
+		hand = XRToolsHand.find_instance(controller)
+		collision_hand = XRToolsCollisionHand.find_instance(controller)
+		player_body = XRToolsPlayerBody.find_instance(controller)