Add weighted logic to collision hands (#695)

* Add weighted logic to collision hands

* Added logic to properly handle movement and teleport impact on collision 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/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/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

@@ -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 = 300.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,77 @@ 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
-	global_transform.basis = _target.global_transform.basis
-	move_and_slide(_target.global_position - global_position)
+	# Orient the hand
+	rotate_and_collide(_target.global_basis)
+
+	# 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
+	rotate_and_collide(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/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,86 @@ func move_and_slide(move : Vector3) -> ForceBodyCollision:
 
 	# Return the last collision data
 	return ret
+
+
+## Attempts to rotate our object until it collides
+func rotate_and_collide( \
+	target_global_basis : Basis, \
+	step_angle : float = deg_to_rad(5.0) \
+	) -> ForceBodyCollision:
+	# Make sure this is off or weird shit happens...
+	sync_to_physics = false
+
+	var ret : ForceBodyCollision = null
+
+	var space = PhysicsServer3D.body_get_space(get_rid())
+	var direct_state = PhysicsServer3D.space_get_direct_state(space)
+
+	# We don't seem to have a rotational movement query for collisions,
+	# so best we can do is to rotate in steps and test
+	var from_quat : Quaternion = Quaternion(global_basis)
+	var to_quat : Quaternion = Quaternion(target_global_basis)
+	var angle : float = from_quat.angle_to(to_quat)
+	var steps : float = ceil(angle / step_angle)
+
+	# Convert collision exceptions to a RID array
+	var exception_rids : Array[RID]
+	for collision_exception in get_collision_exceptions():
+		exception_rids.push_back(collision_exception.get_rid())
+
+	# Prevent collisions with ourselves
+	exception_rids.push_back(get_rid())
+
+	# Find our shape ids
+	var shape_rids : Array[RID]
+	for node in get_children(true):
+		if node is CollisionShape3D:
+			var col_shape : CollisionShape3D = node
+			if not col_shape.disabled:
+				shape_rids.push_back(col_shape.shape.get_rid())
+
+	# Our physics query
+	var query : PhysicsShapeQueryParameters3D = PhysicsShapeQueryParameters3D.new()
+	query.collide_with_areas = false
+	query.collide_with_bodies = true
+	query.collision_mask = collision_mask
+	query.exclude = exception_rids
+
+	# Check our collisions
+	var step : float = 0.0
+	var new_quat : Quaternion = from_quat
+	var t = global_transform
+	while step < steps and not ret:
+		step += 1.0
+
+		var test_quat : Quaternion = from_quat.slerp(to_quat, step / steps)
+		t.basis = Basis(test_quat)
+		query.transform = t
+
+		for rid in shape_rids:
+			query.shape_rid = rid
+			var collision = direct_state.get_rest_info(query)
+			if not collision.is_empty():
+				ret = ForceBodyCollision.new()
+				ret.collider = instance_from_id(collision["collider_id"])
+				ret.position = collision["point"]
+				ret.normal = collision["normal"]
+
+				# TODO May need to see about applying a rotational force
+				# if pushbodies is true
+
+				break
+
+		if not ret:
+			# No collision, we can rotate this far!
+			new_quat = test_quat
+
+	# Update our rotation to our last successful rotation
+	global_basis = Basis(new_quat)
+
+	# Return the last collision data
+	return ret
+
+
+func _ready():
+	process_physics_priority = -90

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)