Add weighted logic to collision hands

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

@@ -388,6 +388,11 @@ func drop_object() -> void:
 		_velocity_averager.linear_velocity() * impulse_factor,
 		_velocity_averager.angular_velocity())
 	picked_up_object = null
+
+	if _collision_hand:
+		# Reset the held weight
+		_collision_hand.set_held_weight(0.0)
+
 	emit_signal("has_dropped")
 
 

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

@@ -9,6 +9,8 @@ extends XRToolsForceBody
 ## its ancestor [XRController3D], and can act as a container for hand models
 ## and pickup functions.
 
+# We reached our teleport distance
+signal max_distance_reached
 
 ## Modes for collision hand
 enum CollisionHandMode {
@@ -39,7 +41,6 @@ const ORIENT_DISPLACEMENT := 0.05
 # Distance to teleport hands
 const TELEPORT_DISTANCE := 1.0
 
-
 ## Controls the hand collision mode
 @export var mode : CollisionHandMode = CollisionHandMode.COLLIDE
 
@@ -68,6 +69,15 @@ const TELEPORT_DISTANCE := 1.0
 
 		notify_property_list_changed()
 
+
+## Minimum force we can exert on a picked up object
+@export_range(1.0, 1000.0, 0.1, "suffix:N") var min_pickup_force : float = 15.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
+
+
 # Controller to target (if no target overrides)
 var _controller : XRController3D
 
@@ -81,6 +91,11 @@ var _target : Node3D
 var _palm_collision_shape : CollisionShape3D
 var _digit_collision_shapes : Dictionary
 
+# The weight held by this hand
+var _held_weight : float = 0.0
+
+# Movement on last frame
+var _last_movement : Vector3 = Vector3()
 
 ## Target-override class
 class TargetOverride:
@@ -96,6 +111,11 @@ class TargetOverride:
 		priority = p
 
 
+# Update the weight attributed to this hand (updated from pickable system).
+func set_held_weight(new_weight):
+	_held_weight = new_weight
+
+
 # Add support for is_xr_class on XRTools classes
 func is_xr_class(name : String) -> bool:
 	return name == "XRToolsCollisionHand"
@@ -148,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")
 
@@ -156,14 +182,17 @@ func _ready():
 
 
 # Handle physics processing
-func _physics_process(_delta):
+func _physics_process(delta):
 	# Do not process if in the editor
 	if Engine.is_editor_hint():
 		return
 
+	var current_position = global_position
+
 	# Move to the current target
-	_move_to_target()
+	_move_to_target(delta)
 
+	_last_movement = global_position - current_position
 
 ## This function adds a target override. The collision hand will attempt to
 ## move to the highest priority target, or the [XRController3D] if no override
@@ -227,7 +256,7 @@ static func find_right(node : Node) -> XRToolsCollisionHand:
 
 
 # This function moves the collision hand to the target node.
-func _move_to_target():
+func _move_to_target(delta):
 	# Handle DISABLED or no target
 	if mode == CollisionHandMode.DISABLED or not _target:
 		return
@@ -239,12 +268,78 @@ func _move_to_target():
 
 	# 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 then move
+	# Orient the hand
 	global_transform.basis = _target.global_transform.basis
-	move_and_slide(_target.global_position - global_position)
+
+	# Adjust target position if we're holding something
+	var target_movement : Vector3 = _target.global_position - global_position
+	if _held_weight > 0.0:
+		var gravity_state := PhysicsServer3D.body_get_direct_state(get_rid())
+		var gravity = gravity_state.total_gravity * delta
+
+		# Calculate the movement of our held object if we weren't holding it
+		var base_movement : Vector3 = _last_movement * 0.2 + gravity
+
+		# How much movement is left until we reach our target
+		var remaining_movement = target_movement - base_movement
+
+		# The below is an approximation as we're not taking the logarithmic
+		# nature of force acceleration into account for simplicitiy.
+
+		# Distance over time gives our needed acceleration which
+		# gives us the force needed on the object to move it to our
+		# target destination.
+		# But dividing and then multiplying over delta and mass is wasteful.
+		var needed_distance = remaining_movement.length()
+
+		# Force we can exert on the object
+		var force = min_pickup_force + \
+			(target_movement.length() * (max_pickup_force-min_pickup_force) / TELEPORT_DISTANCE)
+
+		# How much can we move our object?
+		var possible_distance = delta * force / _held_weight
+		if possible_distance < needed_distance:
+			# We can't make our distance? adjust our movement!
+			remaining_movement *= (possible_distance / needed_distance)
+			target_movement = base_movement + remaining_movement
+
+	# And move
+	move_and_slide(target_movement)
+	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()
 
 

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

@@ -78,6 +78,7 @@ func _init(
 
 	# Apply collision exceptions
 	if collision_hand:
+		collision_hand.max_distance_reached.connect(_on_max_distance_reached)
 		_add_collision_exceptions(what)
 
 
@@ -154,6 +155,11 @@ func release() -> void:
 	what.released.emit(what, by)
 
 
+# Hand has moved too far away from object, can no longer hold on to it.
+func _on_max_distance_reached() -> void:
+	pickup.drop_object()
+
+
 # Set hand-pose overrides
 func _set_hand_pose() -> void:
 	# Skip if not hand
@@ -192,7 +198,6 @@ func _add_collision_exceptions(from : Node):
 
 	# If this is a physics body, add an exception
 	if from is PhysicsBody3D:
-		print_debug("Add collision exception for ", from.name)
 		# Make sure we don't collide with what we're holding
 		_collision_exceptions.push_back(from)
 		collision_hand.add_collision_exception_with(from)

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

@@ -89,6 +89,7 @@ func _physics_process(delta : float) -> void:
 			else:
 				# Lerp completed
 				state = GrabState.SNAP
+				_update_weight()
 				if primary: primary.set_arrived()
 				if secondary: secondary.set_arrived()
 
@@ -112,6 +113,7 @@ func add_grab(p_grab : Grab) -> void:
 
 	# If snapped then report arrived at the new grab
 	if state == GrabState.SNAP:
+		_update_weight()
 		p_grab.set_arrived()
 
 
@@ -138,6 +140,9 @@ func remove_grab(p_grab : Grab) -> void:
 		print_verbose("%s> %s (secondary) released" % [target.name, p_grab.by.name])
 		secondary = null
 
+	if state == GrabState.SNAP:
+		_update_weight()
+
 
 # Discard the driver
 func discard():
@@ -204,6 +209,8 @@ static func create_snap(
 	p_target.get_parent().add_child(driver)
 	driver.remote_path = driver.get_path_to(p_target)
 
+	driver._update_weight()
+
 	# Return the driver
 	return driver
 
@@ -214,3 +221,17 @@ static func _vote(a : float, b : float) -> float:
 		return 0.0
 
 	return b / (a + b)
+
+
+# Update the weight on collision hands
+func _update_weight():
+	if primary:
+		var weight : float = target.mass
+		if secondary:
+			# Each hand carries half the weight
+			weight = weight / 2.0
+			if secondary.collision_hand:
+				secondary.collision_hand.set_held_weight(weight)
+
+		if primary.collision_hand:
+			primary.collision_hand.set_held_weight(weight)

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()

assets/3dmodelscc0/models/scenes/sniper_rifle.tscn

@@ -112,6 +112,7 @@ radius = 0.001
 
 [node name="SniperRifle" instance=ExtResource("1_lpv7q")]
 collision_mask = 7
+mass = 4.0
 script = ExtResource("2_bdnea")
 second_hand_grab = 2