# Torque & Turning

Discussion in 'Physics' started by yingpar, Jan 21, 2016.

1. ### yingpar

Joined:
Sep 25, 2014
Posts:
10
Alright, I've been stressing over this a bit and I thought I'd ask around and see if someone else had a solution.

I have a Rigidbody with a given mass, torque, and angular drag. I want to give this object a set of angles and have it rotate to face those angles. So far I have no problem telling the object "this is where you need to go," and having it apply torque in the right directions, but if I do that until I reach the target angles, it will go past them as it decelerates. I'd like to calculate the exact point at which it should stop applying torque in order to come to a stop at the correct rotation.

Thoughts?

2. ### Billy4184

Joined:
Jul 7, 2014
Posts:
6,013
You will need to use a PID controller. Let me know if you have any specific questions about it.

3. ### Harpoon

Joined:
Aug 26, 2014
Posts:
20
I'm working on the same thing for my AI right now, so I think I can help with some ideas.

If you know how much torque is available and moment of inertia of your object you can calculate how much angular acceleration it will generate based on this formula:

a = T/I

We also know, that formula for distance in accelerated movement is: v=at, so applying this to curcular motion gives us: w = at so:

t = w/a

where w = angular speed (which we know from rigidbody.angularvelocity)

Now, that we know how much time will it take to stop rotation we can calculate how many angles will it take:

angles = (at^2)/2

or in single equation:

angles = ((T/I)*(w/(T/I))^2)/2

So, you can apply torque in direction of turn if angles (in radians) to turn are greater than angles. If not, apply opposite torque until angular velocity is 0.

This is how it should work in theory.

Deleted User likes this.
4. ### yingpar

Joined:
Sep 25, 2014
Posts:
10
Oh wow, thank you - that's exactly what I was looking for! Do you account for angular drag? Or do you just not use it in your project?

5. ### yingpar

Joined:
Sep 25, 2014
Posts:
10
I've got it partially working for one axis. Here's my test code:

Code (CSharp):
1.
2. using UnityEngine;
3.
4. [RequireComponent(typeof(Rigidbody))]
5. [RequireComponent(typeof(Collider))]
6. public class Spinny : MonoBehaviour
7. {
8.      Rigidbody rb;
9.      float inertia, torque;
10.      float timeToStop;
11.
12.      public float AngularAcceleration = 6.28f;
13.
14.      void Awake()
15.      {
16.           rb = GetComponent<Rigidbody>();
17.      }
18.
19.      void Start()
20.      {
21.           inertia = rb.inertiaTensor.z;
22.           torque = inertia * AngularAcceleration;
23.      }
24.
25.      void Update()
26.      {
27.           if (Input.GetKey(KeyCode.LeftArrow)) {
29.           } else if (Input.GetKey(KeyCode.A)) {
30.                timeToStop = rb.angularVelocity.z / AngularAcceleration;
31.           }
32.
33.           if (timeToStop > 0f) {
35.                timeToStop -= Time.deltaTime;
36.           }
37.      }
38. }
39.
The next problem seems to be dealing with the tiny bit of leftover momentum. Is there a way to do that without just setting angularVelocity to 0?

6. ### Harpoon

Joined:
Aug 26, 2014
Posts:
20
Well, looking at your script you don't really need to calculate torque if you want constant deceleration. You just need to use ForceMode.Acceleration.

And, if'm honest I'd use a slightly a different approach to ensure that your object stops:

Code (CSharp):
1. using UnityEngine;
2. [RequireComponent(typeof(Rigidbody))]
3. [RequireComponent(typeof(Collider))]
4. public class Spinny : MonoBehaviour
5. {
6.   Rigidbody rb;
7.     float drag;
8.     bool stop;
9.   public float AngularAcceleration = 6.28f;
10.   void Awake()
11.   {
12.   rb = GetComponent<Rigidbody>();
13.   }
14.   void Start()
15.   {
16.       drag = rb.angularDrag;
17.   }
18.   void FixedUpdate()
19.   {
20.   if (Input.GetKey(KeyCode.LeftArrow)) {
22.   } else if (Input.GetKey(KeyCode.A)) {
23.         stop = true;
24.   }
25.   if (rb.angularVelocity.z>0) {
26.         if (stop){
27.           rb.AddTorque(Vector3.back * Mathf.Min(AngularAcceleration, rb.angularVelocity.z), ForceMode.Acceleration);
28.         }
29.   }else{
30.         stop = false;
31.       }
32.   }
33. }

7. ### yingpar

Joined:
Sep 25, 2014
Posts:
10
I should have specified that that was just an experiment to make sure I was following along with the mechanics! It's all a little brain-bending for me so I'm trying to go one step at a time. The ultimate goal is still to have something that you can set to a specific rotation and have it figure out how to get there, rather than having to press an arrow key.

I have another update that I'll post in a bit.

8. ### yingpar

Joined:
Sep 25, 2014
Posts:
10
Alright. Here's my latest attempt. The formula to figure out how long it should take to bring the object to a halt works fine, but the second part, to find how many radians it will take, seems off. The error grows exponentially the faster the object spins. (Of interest: Unity seems to have a built-in speed limit of 7 rad/s.)

Code follows:
Code (CSharp):
1. using UnityEngine;
2.
3. [RequireComponent(typeof(Rigidbody))]
4. [RequireComponent(typeof(Collider))]
5. public class Spinny : MonoBehaviour
6. {
7.     Rigidbody rb;
8.     float inertia, torque;
9.
10.     float timeToStop, stopTimer;
11.     float anglesToStop, lastRotation, stopAngles;
12.     bool stopping;
13.
14.     public float AngularAcceleration = 3.14f;
15.
16.     void Awake()
17.     {
18.         rb = GetComponent<Rigidbody>();
19.     }
20.
21.     void Start()
22.     {
23.         inertia = rb.inertiaTensor.z;
24.         Debug.Log("Inertia: " + inertia.ToString("F2") + " kg*m².");
25.         torque = inertia * AngularAcceleration;
26.         Debug.Log("Torque: " + torque.ToString("F2") + " N*m.");
27.         stopping = false;
28.     }
29.
30.     void FixedUpdate()
31.     {
32.         if (!stopping && Input.GetKey(KeyCode.LeftArrow)) {
34.         }
35.         if (stopping && timeToStop > 0f) {
37.
38.             timeToStop -= Time.deltaTime;
39.             stopTimer += Time.deltaTime;
40.
41.             stopAngles += Mathf.Abs((transform.eulerAngles.z * Mathf.Deg2Rad) - lastRotation);
42.         } else if (stopping && timeToStop <= 0f) {
43.             rb.angularVelocity = Vector3.zero;
44.             Debug.Log("Stopped. It took " + stopTimer.ToString("F2") + " s and " + stopAngles.ToString("F2") + " rad.");
45.
46.             if (!Mathf.Approximately(0f, rb.angularVelocity.z)) {
47.                 Debug.Log("Oops! Leftover Angular Velocity: " + rb.angularVelocity.z + " rad/s.");
48.             }
49.
50.             stopping = false;
51.         }
52.
53.         lastRotation = transform.eulerAngles.z * Mathf.Deg2Rad;
54.     }
55.
56.     void Update()
57.     {
58.         if (Input.GetKeyDown(KeyCode.A)) {
59.             Debug.Log("Stopping!");
60.             Debug.Log("Current Angular Velocity: " + rb.angularVelocity.z.ToString("F2") + " rad/s.");
61.
62.             timeToStop = rb.angularVelocity.z / AngularAcceleration;
63.             Debug.Log("Estimated Time to Stop: " + timeToStop.ToString("F2") + " s.");
64.             stopTimer = 0f;
65.
66.             anglesToStop = Mathf.Pow(AngularAcceleration * timeToStop, 2f) / 2f;
67.             Debug.Log("Estimated Angles to Stop: " + anglesToStop.ToString("F2") + " rad.");
68.             stopAngles = 0f;
69.
70.             stopping = true;
71.         }
72.     }
73. }
74.

Edit: In plain english, what this is supposed to do (so far) is figure out for how many radians it will have to apply reverse torque in order to bring it to a stop. Once that works, the next step would be to use that calculation to determine when to call for a stop in order to reach a specific rotation. Does that make sense?

Last edited: Jan 26, 2016
9. ### Billy4184

Joined:
Jul 7, 2014
Posts:
6,013
You say that you want to know when to 'stop applying torque', but what about applying a braking torque as you approach the target rotation? This simple approach is used in PID controllers which are designed for exactly this situation.

So your object has an angular velocity which gives it momentum that takes it past the target rotation. If you try to solve this simply by stopping the application of torque at the appropriate time, you won't be able to rotate quickly as you will have to wait for the momentum to die down, which could take a long time. What you need to do is apply a braking/damping force.

First of all, the P in PID stands for Proportional, and this is what you probably have already implemented, a torque that is applied proportional to how far away the object is from the desired rotation.

The D in PID is what you're looking for, it stands for Derivative (rate-of-change, i.e. velocity) and it refers to a torque that is applied to counter the current velocity of the object, in this case the angular velocity. It basically amounts to a damping force and is extremely simple to implement.

All you have to do is get the angular velocity of your object, and apply a torque in the opposite direction to that velocity that is proportional to that velocity. For example:

Code (CSharp):
1.
2.
3. public float dampingCoefficient;
4.
5. // Get the local angular velocity
6. Vector3 localAngularVelocity = myTransform.InverseTransformDirection(myRigidbody.angularVelocity);
7.
8. // Calculate damping torques based on that angular velocity and apply it in the opposite direction (negative)
9. Vector3 dampingTorque = localAngularVelocity * -dampingCoefficient;
10.
12.
13.
Then you just tweak the damping coefficient until you're happy with the effect.

10. ### yingpar

Joined:
Sep 25, 2014
Posts:
10
I meant "apply stopping torque." Physics!

Your method will definitely bring the body to a stop, but I'm not sure how it can account for doing so at a specific rotation, which was the problem I was trying to solve.

I. e., how can I know when to start braking so that my car ends up exactly at the stop line without going over given an arbitrary distance, momentum, and stopping force?

Last edited: Jan 26, 2016
11. ### Billy4184

Joined:
Jul 7, 2014
Posts:
6,013
If you want to bring your object to a stop with mathematical precision, it is probably a bit more complicated. However If you use the correct coefficient with the method I showed you, oscillation is virtually non-existent (and certainly not visible at all).

Can I you give a little more context to your issue? Is it a vehicle of some kind?

12. ### yingpar

Joined:
Sep 25, 2014
Posts:
10
Sure. Movement and the related calculations have always been a pain for me, so I'm trying to go back and figure them out a step at a time.

At the moment it's an asteroids-style ship floating around in frictionless space. Although I have key controls in my code at the moment, I'd like to just be able to tell the ship "turn to face this direction" and have it turn to face that direction. In order to make this arrangement as extensible as possible, I'd also like to be able to define the ship's mass and maximum torque and have it work backwards from there by itself.

Does that make sense?

13. ### Billy4184

Joined:
Jul 7, 2014
Posts:
6,013
I've written quite a few spaceship controllers, I can tell you! You can see the WIP of what I'm working on at the moment in my sig. In working on the AI, I tried a lot of different approaches to the control system - all of them physics-based using forces.

I'm not sure if your game is 3D but I will assume so.

To avoid gimbal lock (I assume you've run into it already) you need to set up a hierarchy for the different axes. E.g., for a turret-style movement you might yaw until your nose is near the target before pitching toward it.

The key thing to remember with gimbal lock is that all the problems occur when some of your rotations are ~90 degrees or more from the target. That is where euler angles can start becoming difficult to deal with. So you should only apply torques for a particular axis when the rotations on the other axes that are higher up in the hierarchy are well below 90 degrees from their target.

For example, my missile controller yaws (rotates on the y axis) toward the target, and only when it is well below 90 degrees away from its yaw target (lets say around 75 degrees) I begin to implement pitch. This, together with the damping method I showed you above, makes it possible to rotate toward the target from any starting rotation, and decelerates smoothly as it approaches the target, and doesn't overshoot.

Let me know if you have any questions about it, I hope I understood your issue correctly.

14. ### Harpoon

Joined:
Aug 26, 2014
Posts:
20
Hi again.

First, if it's in frictionless environment I'd disable all drag. Makes it more believable and easier to calculate. What I did was creating two separate scripts so that the ship can be controlled by both AI and the player.
First script is just a ship controller, which is something like this:

Code (CSharp):
1. public float yaw; // other scripts (like AI) access this variable to send command to the ship
2. public float maxTorque;
3. float yawRotationVelocity;
4. float MaxRotVel;
5.
6. void FixedUpdate()
7. {
8.     yawRotationVelocity = transform.InverseTranformDirection(rb.angularVelocity.y); //ship's current rotation speed relative in local yaw axis
9.     float targetRotation = 0;
10.
11.     if (yaw!=0)
12.     {
13.         targetRotation = yaw * MaxRotVel;
14.     }else
15.     {
16.         targetRotation = 0;
17.     }
18.
19.     float difference = targetRotation - yawRotationVelocity;
20.     float yawTorque = Mathf.Min(Mathf.Abs(difference),maxTorque) * Mathf.Sign(difference); //if difference > maxtorque apply max torque else apply difference
21.
23. }
24.
Second is an AI script in which I use method from my first post to calculate when to stop turning, so the last line in the code should be something like

Code (CSharp):
1. if (Mathf.Abs(anglesToStop)<Mathf.Abs(anglesToTarget))
2. {
3.     ShipController.yaw = Mathf.Sign(anglesToTarget); //you can modify this part to make the turn more smooth like yaw = anglesToTarget/90f so that it will apply full torque if target is perpendicular
4. }else
5. {
6.     ShipController.yaw = 0;
7. }
There might be slight oscillations but if your code is right the AI script will correct itself.

Also please keep in mind that I don't have access to Unity ATM so there might be some errors in code but the principle is ok and it works.

15. ### yingpar

Joined:
Sep 25, 2014
Posts:
10
If anyone is still interested, I seem to have finally reached a solution (at least for myself!). This is for 2-dimensional rotation.

Ship object:
Code (CSharp):
1. using UnityEngine;
2.
3. [RequireComponent(typeof(Rigidbody))]
4. [RequireComponent(typeof(Collider))]
5. public class ShipMovement : MonoBehaviour
6. {
7.     Rigidbody rb;
9.     float angularAcceleration, turnSpeed;
10.
11.     public float MaxTorque = 7.5f;
12.     public float StopThreshold = .5f;
13.
14.     Vector3 tVec;
15.
16.     void Awake()
17.     {
18.         rb = GetComponent<Rigidbody>();
19.     }
20.
21.     void Start()
22.     {
23.         Debug.Log("Inertia: " + rb.inertiaTensor.z.ToString("F2") + " kg*m²");
24.         Debug.Log("Max. Torque: " + MaxTorque + " N*m");
25.         angularAcceleration = MaxTorque / rb.inertiaTensor.z * Mathf.Rad2Deg;
26.         Debug.Log("Max. Angular Acceleration: " + angularAcceleration.ToString("F2") + "°/s².");
27.
28.         tVec = Vector3.zero;
29.     }
30.
31.     void FixedUpdate()
32.     {
33.         turnSpeed = rb.angularVelocity.z * Mathf.Rad2Deg;
34.
35.         if (tVec != Vector3.zero) {
36.             tVec += rb.velocity * Time.fixedDeltaTime;
37.             Debug.DrawLine(transform.position, tVec);
38.             var dRot = Quaternion.FromToRotation(transform.up * -1f, (tVec - transform.position));
39.             var difference = 360f - dRot.eulerAngles.z - 180f;
40.
41.             var stopDistance = (angularAcceleration * Mathf.Pow(turnSpeed / angularAcceleration, 2f)) / 2f;
42.             var torqueDirection = -1f * Mathf.Sign(difference);
43.             if (stopDistance >= Mathf.Abs(difference)) {
44.                 torqueDirection *= -1f;
45.             }
46.             if (Mathf.Abs(difference) > StopThreshold) {
47.                 rb.AddTorque(Vector3.forward * torqueDirection * MaxTorque);
48.             } else {
49.                 rb.angularVelocity = Vector3.zero;
50.                 tVec = Vector3.zero;
51.             }
52.         }
53.     }
54.
55.     public void SetTargetVector(Vector3 newTarget)
56.     {
57.         tVec = newTarget;
58.     }
59. }
60.

Ship Controller object, child of Ship object:
Code (CSharp):
1. using UnityEngine;
2.
3. [RequireComponent(typeof(Collider))]
4. public class PlayerShipMovementController : MonoBehaviour
5. {
6.     GameObject ship;
7.     Vector3 mousePos;
8.     bool isDragging;
9.
10.     void Awake()
11.     {
12.         ship = transform.parent.gameObject;
13.         if (ship == null || ship.GetComponent<ShipMovement>() == null) {
14.             Debug.LogError("Player Ship Movement Controller has no Parent!");
15.             Debug.Break();
16.         }
17.     }
18.
19.     void Start()
20.     {
21.         isDragging = false;
22.     }
23.
24.     void Update()
25.     {
26.         mousePos = Camera.main.ScreenToWorldPoint(Input.mousePosition);
27.         mousePos.z = ship.transform.position.z;
28.
29.         if (!isDragging) {
30.             if (Input.GetMouseButtonDown(0)) {
31.                 var ray = Camera.main.ScreenPointToRay(Input.mousePosition);
32.                 var rayHit = new RaycastHit();
33.                 if (Physics.Raycast(ray, out rayHit, 15f) && rayHit.collider.gameObject == gameObject) {
34.                     isDragging = true;
35.                 }
36.             }
37.         } else if (Input.GetMouseButton(0)) {
38.             Debug.DrawLine(ship.transform.position, mousePos);
39.         } else {
40.             ship.SendMessage("SetTargetVector", mousePos);
41.             isDragging = false;
42.         }
43.     }
44. }
45.

Last edited: Jan 31, 2016
felres likes this.