Updated pickupable script

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 rotateDamping: float = 5.0
+@export var maxLiftForce: float = 600.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
-
-		rotationOffset = Basis(Vector3.UP, deg_to_rad(-delta.x * 0.35)) * rotationOffset
-		rotationOffset = Basis(Vector3.RIGHT, deg_to_rad(delta.y * 0.35)) * rotationOffset
+		var previewTargetBasis = holdPoint.global_basis * rotationOffset
+		var previewCurrentQuat = body.global_basis.get_rotation_quaternion()
+		var previewTargetQuat = previewTargetBasis.get_rotation_quaternion()
+		
+		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)