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"
@@ -156,14 +176,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 +250,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 +262,48 @@ func _move_to_target():
 
 	# Handle too far from target
 	if global_position.distance_to(_target.global_position) > TELEPORT_DISTANCE:
+		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()
 
 

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)

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
 

assets/meshes/picatinny_rail/picatinny_30.glb.import

@@ -17,7 +17,6 @@ nodes/root_type="Node3D"
 nodes/root_name="Scene Root"
 nodes/apply_root_scale=true
 nodes/root_scale=1.0
-nodes/import_as_skeleton_bones=false
 meshes/ensure_tangents=true
 meshes/generate_lods=true
 meshes/create_shadow_meshes=true
@@ -29,7 +28,6 @@ animation/import=false
 animation/fps=30
 animation/trimming=false
 animation/remove_immutable_tracks=true
-animation/import_rest_as_RESET=false
 import_script/path=""
 _subresources={}
 gltf/naming_version=0

scenes/pickable_demo/objects/grab_ball.tscn

@@ -1,11 +1,14 @@
-[gd_scene load_steps=10 format=3 uid="uid://clta811tt773x"]
+[gd_scene load_steps=11 format=3 uid="uid://clta811tt773x"]
 
 [ext_resource type="PackedScene" uid="uid://c8l60rnugru40" path="res://addons/godot-xr-tools/objects/pickable.tscn" id="1"]
 [ext_resource type="Material" path="res://assets/wahooney.itch.io/brown_grid_triplanar.tres" id="2"]
 [ext_resource type="PackedScene" uid="uid://c25yxb0vt53vc" path="res://addons/godot-xr-tools/objects/grab_points/grab_point_hand_left.tscn" id="2_qhto0"]
 [ext_resource type="Script" path="res://addons/godot-xr-tools/objects/highlight/highlight_visible.gd" id="3"]
 [ext_resource type="PackedScene" uid="uid://ctw7nbntd5pcj" path="res://addons/godot-xr-tools/objects/grab_points/grab_point_hand_right.tscn" id="3_0q4lf"]
 
+[sub_resource type="PhysicsMaterial" id="PhysicsMaterial_bv5s3"]
+bounce = 0.8
+
 [sub_resource type="SphereShape3D" id="2"]
 margin = 0.01
 radius = 0.2
@@ -29,6 +32,7 @@ albedo_color = Color(1, 1, 0, 1)
 
 [node name="GrabBall" instance=ExtResource("1")]
 collision_mask = 720903
+physics_material_override = SubResource("PhysicsMaterial_bv5s3")
 ranged_grab_method = 2
 second_hand_grab = 2
 

scenes/pickable_demo/objects/hammer.tscn

@@ -28,6 +28,7 @@ size = Vector3(0.1, 0.1, 0.2)
 
 [node name="Hammer" instance=ExtResource("1")]
 collision_mask = 7
+mass = 2.0
 ranged_grab_method = 0
 second_hand_grab = 2
 

scenes/pickable_demo/objects/picatinny_scope.tscn

@@ -45,6 +45,7 @@ blend_shape_mode = 0
 shadow_mesh = SubResource("ArrayMesh_mmp6s")
 
 [node name="PicatinnyScope" groups=["Picatinny"] instance=ExtResource("1_1xi8m")]
+mass = 0.5
 
 [node name="CollisionShape3D" parent="." index="0"]
 transform = Transform3D(-1, 0, 8.74228e-08, 0, 1, 0, -8.74228e-08, 0, -1, 0.0028, 0.0208471, -0.0617341)

scenes/pickable_demo/objects/teacup.tscn

@@ -57,6 +57,7 @@ open_pose = ExtResource("7_m23hc")
 closed_pose = ExtResource("7_m23hc")
 
 [node name="Teacup" groups=["Teacup"] instance=ExtResource("1_q232w")]
+mass = 0.2
 second_hand_grab = 1
 
 [node name="Teacup" type="MeshInstance3D" parent="." index="0"]