Merge pull request #8179 from golfinq/rigidbody-workaround-fix

Fixed code in article "Using RigidBody"

Author: mhilbrunner

tutorials/physics/rigid_body.rst

@@ -29,21 +29,23 @@ The "look at" method
 --------------------
 
 As described above, using the Node3D's ``look_at()`` method can't be used each frame to follow a target.
-Here is a custom ``look_at()`` method that will work reliably with rigid bodies:
+Here is a custom ``look_at()`` method called ``look_follow()`` that will work with rigid bodies:
 
 .. tabs::
  .. code-tab:: gdscript GDScript
 
     extends RigidBody3D
 
-    func look_follow(state, current_transform, target_position):
-        var up_dir = Vector3(0, 1, 0)
-        var cur_dir = current_transform.basis * Vector3(0, 0, 1)
-        var target_dir = current_transform.origin.direction_to(target_position)
-        var rotation_angle = acos(cur_dir.x) - acos(target_dir.x)
-
-        state.angular_velocity = up_dir * (rotation_angle / state.step)
+    var speed: float = 0.1
 
+    func look_follow(state: PhysicsDirectBodyState3D, current_transform: Transform3D, target_position: Vector3) -> void:
+        var forward_local_axis: Vector3 = Vector3(1, 0, 0)
+        var forward_dir: Vector3 = (current_transform.basis * forward_local_axis).normalized()
+        var target_dir: Vector3 = (target_position - current_transform.origin).normalized()
+        var local_speed: float = clampf(speed, 0, acos(forward_dir.dot(target_dir)))
+        if forward_dir.dot(target_dir) > 1e-4:
+            state.angular_velocity = local_speed * forward_dir.cross(target_dir) / state.step
+    
     func _integrate_forces(state):
         var target_position = $my_target_node3d_node.global_transform.origin
         look_follow(state, global_transform, target_position)
@@ -54,24 +56,30 @@ Here is a custom ``look_at()`` method that will work reliably with rigid bodies:
 
     public partial class MyRigidBody3D : RigidBody3D
     {
-        private void LookFollow(PhysicsDirectBodyState state, Transform3D currentTransform, Vector3 targetPosition)
+        private float _speed = 0.1f;
+        private void LookFollow(PhysicsDirectBodyState3D state, Transform3D currentTransform, Vector3 targetPosition)
         {
-            var upDir = new Vector3(0, 1, 0);
-            var curDir = currentTransform.Basis * new Vector3(0, 0, 1);
-            var targetDir = currentTransform.Origin.DirectionTo(targetPosition);
-            var rotationAngle = Mathf.Acos(curDir.X) - Mathf.Acos(targetDir.X);
-
-            state.SetAngularVelocity(upDir * (rotationAngle / state.GetStep()));
+            Vector3 forwardLocalAxis = new Vector3(1, 0, 0);
+            Vector3 forwardDir = (currentTransform.Basis * forwardLocalAxis).Normalized();
+            Vector3 targetDir = (targetPosition - currentTransform.Origin).Normalized();
+            float localSpeed = Mathf.Clamp(_speed, 0.0f, Mathf.Acos(forwardDir.Dot(targetDir)));
+            if (forwardDir.Dot(targetDir) > 1e-4)
+            {
+                state.AngularVelocity = forwardDir.Cross(targetDir) * localSpeed / state.Step;
+            }
         }
 
-        public override void _IntegrateForces(PhysicsDirectBodyState state)
+        public override void _IntegrateForces(PhysicsDirectBodyState3D state)
         {
-            var targetPosition = GetNode<Node3D>("MyTargetNode3DNode").GetGlobalTransform().Origin;
-            LookFollow(state, GetGlobalTransform(), targetPosition);
+            Vector3 targetPosition = GetNode<Node3D>("MyTargetNode3DNode").GlobalTransform.Origin;
+            LookFollow(state, GlobalTransform, targetPosition);
         }
     }
 
 
-This method uses the rigid body's ``angular_velocity`` property to rotate the body. It first calculates the difference between the current and desired angle and then adds the velocity needed to rotate by that amount in one frame's time.
-
-.. note:: This script will not work with rigid bodies in *character mode* because then, the body's rotation is locked. In that case, you would have to rotate the attached mesh node instead using the standard Node3D methods.
+This method uses the rigid body's ``angular_velocity`` property to rotate the body. 
+The axis to rotate around is given by the cross product between the current forward direction and the direction one wants to look in. 
+The ``clamp`` is a simple method used to prevent the amount of rotation from going past the direction which is wanted to be looked in, 
+as the total amount of rotation needed is given by the arccosine of the dot product. 
+This method can be used with ``axis_lock_angular_*`` as well. If more precise control is needed, solutions such as ones relying on :ref:`class_Quaternion` may be required, 
+as discussed in :ref:`doc_using_transforms`.