Updated pickupable script

Scenes/Main.tscn

@@ -10,6 +10,7 @@
 [ext_resource type="PackedScene" uid="uid://cyn6cmirqiiya" path="res://Scenes/SpatialAudioPlayer.tscn" id="8_bmxlf"]
 [ext_resource type="AudioStream" uid="uid://dkc00xclmtkbo" path="res://Scenes/Buy Something!.mp3" id="8_vy5pj"]
 [ext_resource type="PackedScene" uid="uid://dknge5384ijcx" path="res://Scenes/SimpleKey.tscn" id="10_bmxlf"]
+[ext_resource type="Script" uid="uid://b143brnl7gf4f" path="res://Scripts/Pickupable.gd" id="11_4kpch"]
 
 [sub_resource type="BoxMesh" id="BoxMesh_1blfk"]
 size = Vector3(50, 0.1, 50)
@@ -21,6 +22,13 @@ size = Vector3(50, 0.1, 50)
 
 [sub_resource type="BoxMesh" id="BoxMesh_fos0i"]
 
+[sub_resource type="SphereMesh" id="SphereMesh_1blfk"]
+
+[sub_resource type="StandardMaterial3D" id="StandardMaterial3D_va8t4"]
+albedo_color = Color(1, 0, 0, 1)
+
+[sub_resource type="SphereShape3D" id="SphereShape3D_1blfk"]
+
 [node name="Main" type="Node3D" unique_id=856459222]
 
 [node name="StaticBody3D" type="StaticBody3D" parent="." unique_id=268564877]
@@ -117,7 +125,28 @@ maxRaycastDistance = 10.0
 
 [node name="SimplePickupable4" parent="." unique_id=1419866814 instance=ExtResource("10_bmxlf")]
 transform = Transform3D(1, 0, 0, 0, 1, 0, 0, 0, 1, 0, 1.0351448, 3.2192059)
-bodyMass = 5.0
+
+[node name="SimplePickupable5" type="Node3D" parent="." unique_id=1116791131 node_paths=PackedStringArray("body")]
+transform = Transform3D(1, 0, 0, 0, 1, 0, 0, 0, 1, -1.0500765, 1.2124488, 6.335041)
+script = ExtResource("11_4kpch")
+body = NodePath("")
+bodyMass = 100.0
+metadata/_custom_type_script = "uid://b143brnl7gf4f"
+
+[node name="HeavyCube" type="RigidBody3D" parent="." unique_id=51671641 node_paths=PackedStringArray("body")]
+transform = Transform3D(1, 0, 0, 0, 1, 0, 0, 0, 1, 0, 2.310133, 6.0430264)
+collision_layer = 9
+collision_mask = 13
+script = ExtResource("11_4kpch")
+body = NodePath(".")
+bodyMass = 100.0
+
+[node name="MeshInstance3D" type="MeshInstance3D" parent="HeavyCube" unique_id=269584364]
+mesh = SubResource("SphereMesh_1blfk")
+surface_material_override/0 = SubResource("StandardMaterial3D_va8t4")
+
+[node name="CollisionShape3D" type="CollisionShape3D" parent="HeavyCube" unique_id=843818180]
+shape = SubResource("SphereShape3D_1blfk")
 
 [connection signal="pressed" from="SimplePressButton" to="Block" method="toggleMove"]
 [connection signal="held" from="SimpleHoldButton" to="Block" method="holdMove"]

Scripts/Pickupable.gd

@@ -3,11 +3,14 @@ class_name Pickupable
 
 @export var springStrength: float = 150.0
 @export var springDamping: float = 50.0
+@export var rotateStrength: float = 10.0
+@export var rotateDamping: float = 2.0
 
-@export var rotateStrength: float = 5.0
+@export var maxLiftForce: float = 600.0
-@export var rotateDamping: float = 5.0
+@export var maxTorque: float = 40.0
+@export var maxThrowImpulse: float = 100.0
 
-@export var throwForce: float = 10.0
+@export var throwSpeed: float = 10.0
 @export var maxDistance: float = 4.0
 @export var body: RigidBody3D
 @export var snapAnchor: Marker3D
@@ -18,9 +21,9 @@ var dragging: bool = false
 var rotating: bool = false
 var playerRef
 
-var grabLocal: Vector3
 var holdDistance: float
 var rotationOffset: Basis
+var localCenterOffset: Vector3
 
 func _ready():
 	if body:
@@ -30,14 +33,16 @@ func beginInteract(player, hitPosition: Vector3):
 	playerRef = player
 	dragging = true
 
-	grabLocal = body.to_local(hitPosition)
 	var holdPoint = player.hand
 	holdDistance = holdPoint.global_position.distance_to(hitPosition)
 	rotationOffset = holdPoint.global_basis.inverse() * body.global_basis
+	localCenterOffset = body.global_basis.inverse() * (body.global_position - hitPosition)
 
 func updateInteract(player):
 	if !dragging:
 		return
+		
+	body.sleeping = false
 
 	# Toggle rotation mode
 	if Input.is_action_just_pressed("rotate"):
@@ -55,33 +60,55 @@ func updateInteract(player):
 		endInteract(player)
 		return
 
-	# Target position
-	var targetPosition = holdPoint.global_position + (-holdPoint.global_basis.z * holdDistance)
-	var offset = targetPosition - body.global_position
-
-	# Linear spring
-	body.apply_force(offset * springStrength - body.linear_velocity * springDamping, body.to_global(grabLocal) - body.global_position)
-
 	if rotating:
-		var delta = player.interactionMouseDelta
+		var previewTargetBasis = holdPoint.global_basis * rotationOffset
-
+		var previewCurrentQuat = body.global_basis.get_rotation_quaternion()
-		rotationOffset = Basis(Vector3.UP, deg_to_rad(-delta.x * 0.35)) * rotationOffset
+		var previewTargetQuat = previewTargetBasis.get_rotation_quaternion()
-		rotationOffset = Basis(Vector3.RIGHT, deg_to_rad(delta.y * 0.35)) * rotationOffset
+		
+		var previewDiff = previewTargetQuat * previewCurrentQuat.inverse()
+		if previewDiff.w < 0.0:
+			previewDiff = -previewDiff
+			
+		if previewDiff.get_angle() < 1.0: 
+			var delta = player.interactionMouseDelta
+			var weightPenalty = 1.0 / (1.0 + (body.mass * 0.1))
+			
+			var rot_x = deg_to_rad(-delta.x * 0.35 * weightPenalty)
+			var rot_y = deg_to_rad(-delta.y * 0.35 * weightPenalty)
+			
+			rotationOffset = Basis(Vector3.UP, rot_x) * rotationOffset
+			rotationOffset = Basis(Vector3.RIGHT, rot_y) * rotationOffset
 
 	# Angular spring
 	var targetBasis = holdPoint.global_basis * rotationOffset
-
 	var currentQuat = body.global_basis.get_rotation_quaternion()
 	var targetQuat = targetBasis.get_rotation_quaternion()
 	var diff = targetQuat * currentQuat.inverse()
 
-	if diff.dot(Quaternion.IDENTITY) < 0:
+	if diff.w < 0.0:
 		diff = -diff
 
 	var angle = diff.get_angle()
-
 	if angle > 0.001:
-		body.apply_torque(diff.get_axis() * angle * rotateStrength - body.angular_velocity * rotateDamping)
+		var angularAcceleration = (diff.get_axis() * angle * rotateStrength * 10.0) - (body.angular_velocity * rotateDamping * 10.0)
+		var invI = body.get_inverse_inertia_tensor()
+		var torque = Vector3.ZERO
+		
+		if abs(invI.determinant()) > 0.00001:
+			var I = invI.inverse()
+			torque = I * angularAcceleration
+		else:
+			torque = angularAcceleration * body.mass * 0.01 
+		torque = torque.limit_length(maxTorque)
+		body.apply_torque(torque)
+
+	var targetGrabPos = holdPoint.global_position + (-holdPoint.global_basis.z * holdDistance)
+	var targetCenterPos = targetGrabPos + (targetBasis * localCenterOffset)
+	var posError = targetCenterPos - body.global_position
+	var force = (posError * springStrength) - (body.linear_velocity * springDamping)
+	force *= body.mass 
+	force = force.limit_length(maxLiftForce)
+	body.apply_central_force(force)
 
 func endInteract(player):
 	dragging = false
@@ -94,4 +121,7 @@ func throw():
 		return
 
 	var direction = -playerRef.camera.global_basis.z
-	body.apply_impulse(direction * throwForce)
+	var throwImpulse = direction * throwSpeed * body.mass
+	throwImpulse = throwImpulse.limit_length(maxThrowImpulse)
+	body.apply_impulse(throwImpulse)
+	endInteract(playerRef)