Autopilot for Physics-Based Flight system

Hi guys,

I'm making a flight game for iOS, and I'm trying to implement an autopilot feature. What I have is a set of waypoints for the player, and when the autopilot is enabled, the player's bird gets the nearest waypoint and is supposed to orient the player towards it, then fly to it ignoring user inputs. It's then supposed to fly to another waypoint at random, and keep doing so until the autopilot is turned off. Here is the code:

Code (csharp):

1.  
2.  
3. #pragma strict
4.  
5. var rollSpeed : float = 0.0005; //rolling rotation speed
6.  
7. var pitchSpeed : float = 75.0; //vertical rotation speed
8.  
9. var yawSpeed : float = 1000.0; //horizontal rotation speed
10.  
11. var accelerate : float = 600; //main engine thrust
12.  
13. var thrust : float = 1;
14.  
15. var groundHit : RaycastHit;
16.  
17. var maxThrust : float = 5;
18.  
19. var rollDragFactor : float = 5;
20.  
21. var liftFactor : float = 0.3;
22.  
23. ...
24.  
25. var throttlePad : Joystick;
26.  
27. ...
28.  
29. var autopilotButton : GUITexture;
30.  
31. ...
32.  
33. var autopilotEnabled : boolean = false;
34.  
35. private var oldRoll : float = 0;
36.  
37. private var oldVelocity : float;
38.    
39. private var speed : float = 0.0;
40.  
41. private var lift : float = 0;
42.  
43. private var sensitivity : float = 1.0;
44.  
45. ...
46.  
47. private var gyroBool : boolean;
48.  
49. private var gyro : Gyroscope;
50.  
51. private var rotFix : Quaternion;
52.  
53. private var gravityValue : float;
54.  
55. private var waypoints : GameObject[];
56.  
57. private var autoDestination : Vector3;
58.  
59.  
60. // Use this for initialization
61.  
62. function Start ()
63. {
64.  
65.     gravityValue = Mathf.Abs(Physics.gravity.y);
66.     oldVelocity = rigidbody.velocity.sqrMagnitude;
67.    
68.     ...
69.    
70.     //Setup controls
71.    
72.      gyroBool = SystemInfo.supportsGyroscope;
73.      
74.      if (gyroBool)
75.      {
76.  
77.         gyro = Input.gyro;
78.  
79.         gyro.enabled = true;
80.  
81.         if (Screen.orientation == ScreenOrientation.LandscapeRight)
82.         {
83.             rotFix = Quaternion(0,0,0.7071,0.7071);
84.         }
85.     }
86.     else
87.     {
88.         // Add accelerometer controls
89.         print("NO GYRO");
90.  
91.     }
92.    
93.     sensitivity = PlayerPrefs.GetFloat("Sensitivity", 1.0f);
94.    
95.     ...
96.    
97.     // Setup for autopilot system
98.    
99.     GetWaypoints();
100. }
101.  
102. ...
103.  
104. function GetWaypoints () {
105.    
106.     waypoints = GameObject.FindGameObjectsWithTag("Pigeon Navpoint");
107.    
108. }
109.  
110. function GetNearestTaggedObject() : GameObject {
111.     // and finally the actual process for finding the nearest object:
112.  
113.     var nearestDistanceSqr = Mathf.Infinity;
114.     var nearestObj : GameObject = null;
115.  
116.     // loop through each tagged object, remembering nearest one found
117.     for (var obj : GameObject in waypoints) {
118.  
119.         var objectPos = obj.transform.position;
120.         var distanceSqr = (objectPos - transform.position).sqrMagnitude;
121.  
122.         if (distanceSqr < nearestDistanceSqr) {
123.             nearestObj = obj;
124.             nearestDistanceSqr = distanceSqr;
125.         }
126.     }
127.  
128.     return nearestObj;
129. }
130.  
131. function FixedUpdate()
132. {
133.     animation.CrossFade("Flying");
134.  
135.     Physics.Raycast( rigidbody.position - Vector3.up, -Vector3.up, groundHit );
136.    
137.     UpdateOrientation();
138.     ...
139.    
140.     var tapCount = Input.touchCount;
141.    
142.     for ( var i = 0 ; i < tapCount ; i++ ) {
143.    
144.         var touch = Input.GetTouch(i);
145.  
146.         ...
147.  
148.         if(touch.phase == TouchPhase.Began  autopilotButton.HitTest(touch.position))
149.         {
150.             ToggleAutopilot();
151.         }
152.     }
153. }
154.    
155. function UpdateOrientation()
156. {
157.  
158.         var roll : float = 0;
159.         var pitch : float = 0;
160.         var yaw : float = 0;
161.        
162.         var dir : Vector3 = Vector3.zero;
163.        
164.         //Get inputs
165.        
166.         if (isControllable)
167.         {
168.             if (!autopilotEnabled)
169.             {
170.                 // we assume that the device is held parallel to the ground
171.                 // and the Home button is in the right hand
172.            
173.                 // remap the device acceleration axis to game coordinates:
174.                 //  1) XY plane of the device is mapped onto XZ plane
175.                 //  2) rotated 90 degrees around Y axis
176.            
177.                 if (gyroBool)
178.                 {
179.                     dir.x = gyro.rotationRateUnbiased.y; // Pitch
180.                     dir.z = gyro.rotationRateUnbiased.x; // Yaw
181.                     dir.y = -gyro.rotationRateUnbiased.z; // Roll
182.                
183.                     //dir.x = 0; // Pitch
184.                     //dir.z = 0; // Yaw
185.                     //dir.y = 0; // Roll
186.                 }
187.                 else
188.                 {
189.                     dir.x = -Input.acceleration.y; // Pitch
190.                     dir.z = Input.acceleration.x; // Yaw
191.                     dir.y = Input.acceleration.z; // Roll
192.                 }
193.             }
194.             else
195.             {
196.                 dir.x = 0; // Pitch
197.                 dir.z = 0; // Yaw
198.                 dir.y = 0; // Roll
199.                
200.                 // Orient towards the the autopilot target
201.                 //rigidbody.rotation = Quaternion.LookRotation(autoDestination * Time.deltaTime);
202.                 rigidbody.rotation = Quaternion.LookRotation(autoDestination * Time.deltaTime);
203.             }
204.            
205.             // clamp acceleration vector to the unit sphere
206.             if (dir.sqrMagnitude > 1)
207.                 dir.Normalize();
208.  
209.             // Make it move at a consistent rate per time step...
210.             dir *= Time.deltaTime;
211.            
212.             roll = dir.y * (Time.deltaTime * rollSpeed) * sensitivity;
213.             pitch = dir.x * (Time.deltaTime * pitchSpeed) * sensitivity;
214.             yaw = dir.z * (Time.deltaTime * yawSpeed) * sensitivity;
215.            
216.             thrust += (throttlePad.position.y) * (Time.deltaTime * accelerate);
217.            
218.             if (Mathf.Abs(thrust) > maxThrust)
219.             {
220.                 if (thrust > 0)
221.                     thrust = maxThrust;
222.             }
223.             else if (thrust < 0)
224.                     thrust = 0;
225.            
226.             animation["Flying"].speed = (rigidbody.velocity.magnitude / 10);
227.            
228.             //Rotate the pigeon on its axes
229.             rigidbody.AddRelativeTorque(-pitch, yaw, -roll);
230.         }
231.         else
232.         {
233.             thrust = (Time.deltaTime * accelerate);
234.             lift = rigidbody.velocity.x * liftFactor;
235.         }
236.        
237.         lift = Mathf.Abs(rigidbody.velocity.x * liftFactor);
238.        
239.         if (lift > gravityValue * 1)
240.         {
241.             lift = gravityValue * 1;
242.         }
243.        
244.         oldRoll = roll;
245.        
246.         // Move forwards
247.         rigidbody.AddRelativeForce(0,lift, thrust);
248.  
249. ...
250.  
251. }
252.  
253. ...
254.  
255. function ToggleAutopilot()
256. {
257.     if (!autopilotEnabled)
258.         autopilotEnabled = true;   
259.     else
260.     {
261.         autopilotEnabled = false;
262.         GetNearestTaggedObject();
263.     }
264. }
265.  
266. ...
267.  
268.  

However, even though I've tried both transform.LookAt() (even though transform and rigidbody functions should never be mixed) and Quaternion.LookRotation(), I have the same basic problem: when I tap the autopilot button, the player changes orientation in a choppy way, shifting in just one frame. Moreover, the autopilot button is twitchy because if you leave your finger on it, it keeps switching autopilotEnabled between true and false. How should the birds seeking behavior be implemented in a smooth manner, and how should the autopilot button be changed to only register a touch if a finger is lifted from it instead of when a finger is put on it?

MachCUBED

 

Smooth transitions in position and orientation can be handled through interpolation (i.e. Lerp(), Slerp(), or roll-your-own interpolation code). There are billions of preexisting examples and sample code available.

If you need to couple force and torque into a control scheme, a PID controller can do the trick (I sound like a broken record this week).

Your button problem is easily solved by keeping track of when the button is pressed. Create a boolean variable that is set to true when the autopilot button is pressed, and only switch the autopilot on or off if the button isn't already pressed.

Code (csharp):

1.  
2. bool autopilotButtonIsPressed = false;
3. bool autopilotEnabled = false;
4. .
5. .
6. .
7. if (user is pressing the autopilot button) {
8.    if (autoPilotButtonIsPressed == false) {
9.       autopilotButtonIsPressed = true;
10.       if (autopilotEnabled == false)
11.          autopilotEnabled = true;
12.       else
13.          autopilotEnabled = false;
14.    }
15. } else {
16.    autopilotButtonIsPressed = false;
17. }
18.  

 

Thanks for the help Brian, the autopilot button now works smoothly. There is still a bit of a question mark over the actual autopilot though. Here is the new code for turning the autopilot on and off:

Code (csharp):

1.  
2. function ToggleAutopilot()
3. {
4.     if (!autopilotButtonIsPressed)
5.     {
6.         autopilotButtonIsPressed = true;
7.         if (!autopilotEnabled)
8.             autopilotEnabled = true;
9.         else
10.         {
11.             autopilotEnabled = false;
12.             GetNearestTaggedObject();
13.         }
14.     }
15.     else
16.         autopilotButtonIsPressed = false;
17. }
18.  

The above code completely solves the problem of having the autopilot flick on and off at every frame. Now, it just stays on once you tap it, and stays off when you tap it again.

I've also updated the movement code for when the autopilot is on:

Code (csharp):

1.  
2. dir.x = 0; // Pitch
3. dir.z = 0; // Yaw
4. dir.y = 0; // Roll
5.                
6. // Orient towards the the autopilot target
7. var targetQuaternion : Quaternion = Quaternion.LookRotation(autoDestination);
8. rigidbody.rotation = Quaternion.Slerp(rigidbody.rotation, targetQuaternion, Time.deltaTime);
9.  

The above code rotates the player's bird nice and smoothly. But instead of turning towards the target, the bird changes its rotation to be that of the target.

I then read this thread and tried changing the code after setting the dir components to 0:

Code (csharp):

1.  
2. // Orient towards the the autopilot target
3.                
4. var playerVec : Vector3 = rigidbody.rotation * Vector3.forward;
5.                
6. var directionAngle : float = Vector3.Angle(playerVec, autoDestination);
7.                
8. var targetQuaternion : Quaternion = Quaternion.LookRotation(autoDestination * directionAngle);
9.                
10. rigidbody.rotation = Quaternion.Slerp(rigidbody.rotation, targetQuaternion, Time.deltaTime);
11.  

The above code merely rotates the bird 90 degrees or so relative to the orientation of the target. What needs to be done in order to orient the bird towards a target and have it be continuously updated as the bird moves?

EDIT: Problem fixed, use the following code:

Code (csharp):

1.  
2. // Orient towards the the autopilot target
3.                
4.                 var playerVec : Vector3 = rigidbody.rotation * Vector3.forward;
5.                
6.                 var directionAngle : float = Vector3.Angle(playerVec, autoDestination);
7.                
8.                 var heading = autoDestination - rigidbody.position;
9.                
10.                 var targetQuaternion : Quaternion = Quaternion.LookRotation(heading);
11.                
12.                 rigidbody.rotation = Quaternion.Slerp(rigidbody.rotation, targetQuaternion, Time.deltaTime);
13.  

MachCUBED