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

…ion hands

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

@@ -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,6 +268,7 @@ 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
@@ -307,6 +314,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
+	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/player/player_body.gd

@@ -24,11 +24,14 @@ extends CharacterBody3D
 signal player_jumped()
 
 ## Signal emitted when the player teleports
-signal player_teleported()
+signal player_teleported(delta_transform)
 
 ## Signal emitted when the player bounces
 signal player_bounced(collider, magnitude)
 
+## Signal emitted when the player has moved (excluding teleport)
+## This only captures movement handled by the player body logic.
+signal player_moved(delta_transform)
 
 ## Enumeration indicating when ground control can be used
 enum GroundControl {
@@ -262,6 +265,9 @@ func _physics_process(delta: float):
 		set_physics_process(false)
 		return
 
+	# Remember where we are now
+	var current_transform : Transform3D = global_transform
+
 	# Calculate the players "up" direction and plane
 	up_player = origin_node.global_transform.basis.y
 
@@ -325,11 +331,18 @@ func _physics_process(delta: float):
 	# Orient the player towards (potentially modified) gravity
 	slew_up(-gravity.normalized(), 5.0 * delta)
 
+	# If we moved our player, emit signal
+	var delta_transform : Transform3D = global_transform * current_transform.inverse()
+	if delta_transform.origin.length() > 0.001:
+		player_moved.emit(delta_transform)
 
-## Teleport the player body
+## Teleport the player body.
+## This moves the player without checking for collisions.
 func teleport(target : Transform3D) -> void:
+	var inv_global_transform : Transform3D = global_transform.inverse()
+
 	# Get the player-to-origin transform
-	var player_to_origin = global_transform.inverse() * origin_node.global_transform
+	var player_to_origin : Transform3D = inv_global_transform * origin_node.global_transform
 
 	# Set the player
 	global_transform = target
@@ -338,7 +351,7 @@ func teleport(target : Transform3D) -> void:
 	origin_node.global_transform = target * player_to_origin
 
 	# Report the player teleported
-	player_teleported.emit()
+	player_teleported.emit(target * inv_global_transform)
 
 
 ## Request a jump
@@ -410,6 +423,8 @@ func move_body(p_velocity: Vector3) -> Vector3:
 
 ## This method rotates the player by rotating the [XROrigin3D] around the camera.
 func rotate_player(angle: float):
+	var inv_global_transform : Transform3D = global_transform.inverse()
+
 	var t1 := Transform3D()
 	var t2 := Transform3D()
 	var rot := Transform3D()
@@ -419,6 +434,8 @@ func rotate_player(angle: float):
 	rot = rot.rotated(Vector3.DOWN, angle)
 	origin_node.transform = (origin_node.transform * t2 * rot * t1).orthonormalized()
 
+	player_moved.emit(global_transform * inv_global_transform)
+
 ## This method slews the players up vector by rotating the [ARVROrigin] around
 ## the players feet.
 func slew_up(up: Vector3, slew: float) -> void:

addons/godot-xr-tools/player/poke/poke_body.gd

@@ -25,6 +25,18 @@ func is_xr_class(name : String) -> bool:
 	return name == "XRToolsPokeBody" or super(name)
 
 
+func _ready():
+	# Do not initialise if in the editor
+	if Engine.is_editor_hint():
+		return
+
+	# 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)
+
+
 # Try moving to the parent Poke node
 func _physics_process(_delta):
 	# Do not process if in the editor
@@ -52,3 +64,22 @@ func _physics_process(_delta):
 	# Report when we start touching a new object
 	if _contact and _contact != old_contact:
 		body_contact_start.emit(_contact)
+
+
+# If our player moved, attempt to move our hand but ignoring weight.
+func _on_player_moved(delta_transform : Transform3D):
+	var target : Transform3D = delta_transform * global_transform
+
+	# Rotate
+	global_basis = target.basis
+
+	# And attempt to move (we'll detect contact change in physics process.
+	move_and_slide(target.origin - global_position)
+
+	force_update_transform()
+
+
+# If our player teleported, just move.
+func _on_player_teleported(delta_transform : Transform3D):
+	global_transform = delta_transform * global_transform
+	force_update_transform()

scenes/pickable_demo/objects/grab_ball.tscn

@@ -32,6 +32,7 @@ albedo_color = Color(1, 1, 0, 1)
 
 [node name="GrabBall" instance=ExtResource("1")]
 collision_mask = 720903
+mass = 10.0
 physics_material_override = SubResource("PhysicsMaterial_bv5s3")
 ranged_grab_method = 2
 second_hand_grab = 2

scenes/pickable_demo/pickable_demo.tscn

@@ -1,4 +1,4 @@
-[gd_scene load_steps=46 format=3 uid="uid://0c76wodjd7rm"]
+[gd_scene load_steps=47 format=3 uid="uid://0c76wodjd7rm"]
 
 [ext_resource type="PackedScene" uid="uid://qbmx03iibuuu" path="res://addons/godot-xr-tools/staging/scene_base.tscn" id="1"]
 [ext_resource type="Script" path="res://scenes/demo_scene_base.gd" id="2_kjksy"]
@@ -12,6 +12,7 @@
 [ext_resource type="PackedScene" uid="uid://b4kad2kuba1yn" path="res://addons/godot-xr-tools/hands/scenes/lowpoly/left_hand_low.tscn" id="7_ywaf6"]
 [ext_resource type="PackedScene" uid="uid://b4ysuy43poobf" path="res://addons/godot-xr-tools/functions/function_pickup.tscn" id="8"]
 [ext_resource type="PackedScene" uid="uid://diyu06cw06syv" path="res://addons/godot-xr-tools/player/player_body.tscn" id="9"]
+[ext_resource type="PackedScene" uid="uid://fiul51tsyoop" path="res://addons/godot-xr-tools/functions/function_teleport.tscn" id="9_31mn7"]
 [ext_resource type="PackedScene" uid="uid://raeeicvvindd" path="res://addons/godot-xr-tools/hands/scenes/highpoly/right_hand.tscn" id="9_v8epv"]
 [ext_resource type="PackedScene" uid="uid://b6bk2pj8vbj28" path="res://addons/godot-xr-tools/functions/movement_turn.tscn" id="10"]
 [ext_resource type="PackedScene" uid="uid://1mb16xioom74" path="res://scenes/pickable_demo/objects/belt_snap_zone.tscn" id="10_5odnk"]
@@ -34,65 +35,65 @@
 [ext_resource type="PackedScene" uid="uid://bmjemjgtnpkpo" path="res://assets/3dmodelscc0/models/scenes/sniper_rifle.tscn" id="25_xgu4l"]
 [ext_resource type="PackedScene" uid="uid://deuxld12hxsq0" path="res://scenes/pickable_demo/objects/picatinny_scope.tscn" id="26_x40vw"]
 
-[sub_resource type="AnimationNodeAnimation" id="AnimationNodeAnimation_lmsx6"]
+[sub_resource type="AnimationNodeAnimation" id="AnimationNodeAnimation_2qrqe"]
 animation = &"Grip"
 
-[sub_resource type="AnimationNodeAnimation" id="AnimationNodeAnimation_bwyq3"]
+[sub_resource type="AnimationNodeAnimation" id="AnimationNodeAnimation_lqxtc"]
 animation = &"Grip"
 
-[sub_resource type="AnimationNodeBlend2" id="AnimationNodeBlend2_o7t7c"]
+[sub_resource type="AnimationNodeBlend2" id="AnimationNodeBlend2_b6i4m"]
 filter_enabled = true
 filters = ["Armature/Skeleton3D:Little_Distal_L", "Armature/Skeleton3D:Little_Intermediate_L", "Armature/Skeleton3D:Little_Metacarpal_L", "Armature/Skeleton3D:Little_Proximal_L", "Armature/Skeleton3D:Middle_Distal_L", "Armature/Skeleton3D:Middle_Intermediate_L", "Armature/Skeleton3D:Middle_Metacarpal_L", "Armature/Skeleton3D:Middle_Proximal_L", "Armature/Skeleton3D:Ring_Distal_L", "Armature/Skeleton3D:Ring_Intermediate_L", "Armature/Skeleton3D:Ring_Metacarpal_L", "Armature/Skeleton3D:Ring_Proximal_L", "Armature/Skeleton3D:Thumb_Distal_L", "Armature/Skeleton3D:Thumb_Metacarpal_L", "Armature/Skeleton3D:Thumb_Proximal_L", "Armature/Skeleton:Little_Distal_L", "Armature/Skeleton:Little_Intermediate_L", "Armature/Skeleton:Little_Proximal_L", "Armature/Skeleton:Middle_Distal_L", "Armature/Skeleton:Middle_Intermediate_L", "Armature/Skeleton:Middle_Proximal_L", "Armature/Skeleton:Ring_Distal_L", "Armature/Skeleton:Ring_Intermediate_L", "Armature/Skeleton:Ring_Proximal_L", "Armature/Skeleton:Thumb_Distal_L", "Armature/Skeleton:Thumb_Proximal_L"]
 
-[sub_resource type="AnimationNodeAnimation" id="AnimationNodeAnimation_jfy5w"]
+[sub_resource type="AnimationNodeAnimation" id="AnimationNodeAnimation_2yx2b"]
 animation = &"Grip 5"
 
-[sub_resource type="AnimationNodeBlend2" id="AnimationNodeBlend2_4tpt7"]
+[sub_resource type="AnimationNodeBlend2" id="AnimationNodeBlend2_sl1ss"]
 filter_enabled = true
 filters = ["Armature/Skeleton3D:Index_Distal_L", "Armature/Skeleton3D:Index_Intermediate_L", "Armature/Skeleton3D:Index_Metacarpal_L", "Armature/Skeleton3D:Index_Proximal_L", "Armature/Skeleton:Index_Distal_L", "Armature/Skeleton:Index_Intermediate_L", "Armature/Skeleton:Index_Proximal_L"]
 
-[sub_resource type="AnimationNodeBlendTree" id="AnimationNodeBlendTree_p6s08"]
+[sub_resource type="AnimationNodeBlendTree" id="AnimationNodeBlendTree_d375t"]
 graph_offset = Vector2(-536, 11)
-nodes/ClosedHand1/node = SubResource("AnimationNodeAnimation_lmsx6")
+nodes/ClosedHand1/node = SubResource("AnimationNodeAnimation_2qrqe")
 nodes/ClosedHand1/position = Vector2(-600, 300)
-nodes/ClosedHand2/node = SubResource("AnimationNodeAnimation_bwyq3")
+nodes/ClosedHand2/node = SubResource("AnimationNodeAnimation_lqxtc")
 nodes/ClosedHand2/position = Vector2(-360, 300)
-nodes/Grip/node = SubResource("AnimationNodeBlend2_o7t7c")
+nodes/Grip/node = SubResource("AnimationNodeBlend2_b6i4m")
 nodes/Grip/position = Vector2(0, 20)
-nodes/OpenHand/node = SubResource("AnimationNodeAnimation_jfy5w")
+nodes/OpenHand/node = SubResource("AnimationNodeAnimation_2yx2b")
 nodes/OpenHand/position = Vector2(-600, 100)
-nodes/Trigger/node = SubResource("AnimationNodeBlend2_4tpt7")
+nodes/Trigger/node = SubResource("AnimationNodeBlend2_sl1ss")
 nodes/Trigger/position = Vector2(-360, 20)
 node_connections = [&"Grip", 0, &"Trigger", &"Grip", 1, &"ClosedHand2", &"Trigger", 0, &"OpenHand", &"Trigger", 1, &"ClosedHand1", &"output", 0, &"Grip"]
 
-[sub_resource type="AnimationNodeAnimation" id="AnimationNodeAnimation_wbiva"]
+[sub_resource type="AnimationNodeAnimation" id="AnimationNodeAnimation_4a3ao"]
 animation = &"Grip"
 
-[sub_resource type="AnimationNodeAnimation" id="AnimationNodeAnimation_efvb2"]
+[sub_resource type="AnimationNodeAnimation" id="AnimationNodeAnimation_x0eae"]
 animation = &"Grip"
 
-[sub_resource type="AnimationNodeBlend2" id="AnimationNodeBlend2_v66ik"]
+[sub_resource type="AnimationNodeBlend2" id="AnimationNodeBlend2_hskkh"]
 filter_enabled = true
 filters = ["Armature/Skeleton3D:Little_Distal_R", "Armature/Skeleton3D:Little_Intermediate_R", "Armature/Skeleton3D:Little_Metacarpal_R", "Armature/Skeleton3D:Little_Proximal_R", "Armature/Skeleton3D:Middle_Distal_R", "Armature/Skeleton3D:Middle_Intermediate_R", "Armature/Skeleton3D:Middle_Metacarpal_R", "Armature/Skeleton3D:Middle_Proximal_R", "Armature/Skeleton3D:Ring_Distal_R", "Armature/Skeleton3D:Ring_Intermediate_R", "Armature/Skeleton3D:Ring_Metacarpal_R", "Armature/Skeleton3D:Ring_Proximal_R", "Armature/Skeleton3D:Thumb_Distal_R", "Armature/Skeleton3D:Thumb_Metacarpal_R", "Armature/Skeleton3D:Thumb_Proximal_R", "Armature/Skeleton:Little_Distal_R", "Armature/Skeleton:Little_Intermediate_R", "Armature/Skeleton:Little_Proximal_R", "Armature/Skeleton:Middle_Distal_R", "Armature/Skeleton:Middle_Intermediate_R", "Armature/Skeleton:Middle_Proximal_R", "Armature/Skeleton:Ring_Distal_R", "Armature/Skeleton:Ring_Intermediate_R", "Armature/Skeleton:Ring_Proximal_R", "Armature/Skeleton:Thumb_Distal_R", "Armature/Skeleton:Thumb_Proximal_R"]
 
-[sub_resource type="AnimationNodeAnimation" id="AnimationNodeAnimation_qt4i3"]
+[sub_resource type="AnimationNodeAnimation" id="AnimationNodeAnimation_5aax7"]
 animation = &"Grip 5"
 
-[sub_resource type="AnimationNodeBlend2" id="AnimationNodeBlend2_3osea"]
+[sub_resource type="AnimationNodeBlend2" id="AnimationNodeBlend2_wpsy4"]
 filter_enabled = true
 filters = ["Armature/Skeleton3D:Index_Distal_R", "Armature/Skeleton3D:Index_Intermediate_R", "Armature/Skeleton3D:Index_Metacarpal_R", "Armature/Skeleton3D:Index_Proximal_R", "Armature/Skeleton:Index_Distal_R", "Armature/Skeleton:Index_Intermediate_R", "Armature/Skeleton:Index_Proximal_R"]
 
-[sub_resource type="AnimationNodeBlendTree" id="AnimationNodeBlendTree_6sler"]
+[sub_resource type="AnimationNodeBlendTree" id="AnimationNodeBlendTree_3oisq"]
 graph_offset = Vector2(-552.664, 107.301)
-nodes/ClosedHand1/node = SubResource("AnimationNodeAnimation_wbiva")
+nodes/ClosedHand1/node = SubResource("AnimationNodeAnimation_4a3ao")
 nodes/ClosedHand1/position = Vector2(-600, 300)
-nodes/ClosedHand2/node = SubResource("AnimationNodeAnimation_efvb2")
+nodes/ClosedHand2/node = SubResource("AnimationNodeAnimation_x0eae")
 nodes/ClosedHand2/position = Vector2(-360, 300)
-nodes/Grip/node = SubResource("AnimationNodeBlend2_v66ik")
+nodes/Grip/node = SubResource("AnimationNodeBlend2_hskkh")
 nodes/Grip/position = Vector2(0, 40)
-nodes/OpenHand/node = SubResource("AnimationNodeAnimation_qt4i3")
+nodes/OpenHand/node = SubResource("AnimationNodeAnimation_5aax7")
 nodes/OpenHand/position = Vector2(-600, 100)
-nodes/Trigger/node = SubResource("AnimationNodeBlend2_3osea")
+nodes/Trigger/node = SubResource("AnimationNodeBlend2_wpsy4")
 nodes/Trigger/position = Vector2(-360, 40)
 node_connections = [&"Grip", 0, &"Trigger", &"Grip", 1, &"ClosedHand2", &"Trigger", 0, &"OpenHand", &"Trigger", 1, &"ClosedHand1", &"output", 0, &"Grip"]
 
@@ -101,7 +102,6 @@ script = ExtResource("2_kjksy")
 
 [node name="XRToolsCollisionHand" parent="XROrigin3D/LeftHand" index="0" node_paths=PackedStringArray("hand_skeleton") instance=ExtResource("3_m7tr4")]
 hand_skeleton = NodePath("LeftHand/Hand_Nails_low_L/Armature/Skeleton3D")
-min_pickup_force = 15.0
 
 [node name="LeftHand" parent="XROrigin3D/LeftHand/XRToolsCollisionHand" index="0" instance=ExtResource("7_ywaf6")]
 
@@ -138,20 +138,22 @@ push_bodies = false
 
 [node name="AnimationTree" parent="XROrigin3D/LeftHand/XRToolsCollisionHand/LeftHand" index="1"]
 root_node = NodePath("../Hand_Nails_low_L")
-tree_root = SubResource("AnimationNodeBlendTree_p6s08")
+tree_root = SubResource("AnimationNodeBlendTree_d375t")
 
 [node name="MovementDirect" parent="XROrigin3D/LeftHand/XRToolsCollisionHand" index="1" instance=ExtResource("7")]
 strafe = true
+enabled = false
 
 [node name="FunctionPickup" parent="XROrigin3D/LeftHand/XRToolsCollisionHand" index="2" instance=ExtResource("8")]
 grab_distance = 0.1
 ranged_angle = 10.0
 
 [node name="ControlPadLocationLeft" parent="XROrigin3D/LeftHand/XRToolsCollisionHand" index="3" instance=ExtResource("7_fdgf8")]
 
+[node name="FunctionTeleport" parent="XROrigin3D/LeftHand/XRToolsCollisionHand" index="4" instance=ExtResource("9_31mn7")]
+
 [node name="XRToolsCollisionHand" parent="XROrigin3D/RightHand" index="0" node_paths=PackedStringArray("hand_skeleton") instance=ExtResource("3_m7tr4")]
 hand_skeleton = NodePath("RightHand/Hand_Nails_R/Armature/Skeleton3D")
-min_pickup_force = 15.0
 
 [node name="RightHand" parent="XROrigin3D/RightHand/XRToolsCollisionHand" index="0" instance=ExtResource("9_v8epv")]
 
@@ -188,7 +190,7 @@ push_bodies = false
 
 [node name="AnimationTree" parent="XROrigin3D/RightHand/XRToolsCollisionHand/RightHand" index="1"]
 root_node = NodePath("../Hand_Nails_R")
-tree_root = SubResource("AnimationNodeBlendTree_6sler")
+tree_root = SubResource("AnimationNodeBlendTree_3oisq")
 
 [node name="MovementDirect" parent="XROrigin3D/RightHand/XRToolsCollisionHand" index="1" instance=ExtResource("7")]