]> git.mxchange.org Git - flightgear.git/blob - src/AIModel/AIShip.cxx
Add a lower-bound type navaid lookup, and the ability to specify navaid type in the...
[flightgear.git] / src / AIModel / AIShip.cxx
1 // FGAIShip - FGAIBase-derived class creates an AI ship
2 //
3 // Written by David Culp, started October 2003.
4 // - davidculp2@comcast.net
5 //
6 // This program is free software; you can redistribute it and/or
7 // modify it under the terms of the GNU General Public License as
8 // published by the Free Software Foundation; either version 2 of the
9 // License, or (at your option) any later version.
10 //
11 // This program is distributed in the hope that it will be useful, but
12 // WITHOUT ANY WARRANTY; without even the implied warranty of
13 // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
14 // General Public License for more details.
15 //
16 // You should have received a copy of the GNU General Public License
17 // along with this program; if not, write to the Free Software
18 // Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
19
20 #ifdef HAVE_CONFIG_H
21 #  include <config.h>
22 #endif
23
24 #include <simgear/math/point3d.hxx>
25 #include <math.h>
26
27 #include "AIShip.hxx"
28
29
30 FGAIShip::FGAIShip(FGAIManager* mgr) {
31    manager = mgr;
32    _type_str = "ship";
33    _otype = otShip;
34
35 }
36
37 FGAIShip::~FGAIShip() {
38 }
39
40
41 bool FGAIShip::init() {
42    
43    hdg_lock = false;
44    rudder = 0.0;
45    no_roll = false;
46     
47    rudder_constant = 0.5;
48    roll_constant = 0.001;
49    speed_constant = 0.05;
50    hdg_constant = 0.01;
51    
52    return FGAIBase::init();
53 }
54
55 void FGAIShip::bind() {
56     FGAIBase::bind();
57
58     props->tie("surface-positions/rudder-pos-deg",
59                 SGRawValuePointer<float>(&rudder));
60     props->tie("controls/heading-lock",
61                 SGRawValuePointer<bool>(&hdg_lock));
62     props->tie("controls/tgt-speed-kts",
63                 SGRawValuePointer<double>(&tgt_speed));
64     props->tie("controls/tgt-heading-degs",
65                 SGRawValuePointer<double>(&tgt_heading)); 
66     props->tie("controls/constants/rudder",
67                 SGRawValuePointer<double>(&rudder_constant));
68     props->tie("controls/constants/roll",
69                 SGRawValuePointer<double>(&roll_constant));
70     props->tie("controls/constants/rudder",
71                 SGRawValuePointer<double>(&rudder_constant));
72     props->tie("controls/constants/speed",
73                 SGRawValuePointer<double>(&speed_constant)); 
74
75     props->setStringValue("name", name.c_str());
76 }
77
78 void FGAIShip::unbind() {
79     FGAIBase::unbind();
80     props->untie("surface-positions/rudder-pos-deg");
81     props->untie("controls/heading-lock");
82     props->untie("controls/tgt-speed-kts");
83     props->untie("controls/tgt-heading-degs");
84     props->untie("controls/constants/roll");
85     props->untie("controls/constants/rudder");
86     props->untie("controls/constants/speed");           
87
88 }
89
90 void FGAIShip::update(double dt) {
91
92    FGAIBase::update(dt);
93    Run(dt);
94    Transform();
95 }
96
97
98
99 void FGAIShip::Run(double dt) {
100
101    if (fp) ProcessFlightPlan(dt);
102
103    double sp_turn_radius_ft;
104    double rd_turn_radius_ft;
105    double speed_north_deg_sec;
106    double speed_east_deg_sec;
107    double dist_covered_ft;
108    double alpha;
109    double rudder_limit;
110    double raw_roll;   
111
112    // adjust speed
113    double speed_diff = tgt_speed - speed;
114    if (fabs(speed_diff) > 0.1) {
115      if (speed_diff > 0.0) speed += speed_constant * dt;
116      if (speed_diff < 0.0) speed -= speed_constant * dt;
117    } 
118    
119    // convert speed to degrees per second
120    speed_north_deg_sec = cos( hdg / SGD_RADIANS_TO_DEGREES )
121                           * speed * 1.686 / ft_per_deg_lat;
122    speed_east_deg_sec  = sin( hdg / SGD_RADIANS_TO_DEGREES )
123                           * speed * 1.686 / ft_per_deg_lon;
124
125    // set new position
126    pos.setlat( pos.lat() + speed_north_deg_sec * dt);
127    pos.setlon( pos.lon() + speed_east_deg_sec * dt); 
128
129    
130    // adjust heading based on current rudder angle
131    if (rudder <= -0.25 || rudder >= 0.25)  {
132    /*  turn_radius_ft = 0.088362 * speed * speed
133                        / tan( fabs(rudder) / SG_RADIANS_TO_DEGREES );
134      turn_circum_ft = SGD_2PI * turn_radius_ft;
135      dist_covered_ft = speed * 1.686 * dt; 
136      alpha = dist_covered_ft / turn_circum_ft * 360.0;*/
137      
138      if (turn_radius_ft <= 0) turn_radius_ft = 0; // don't allow nonsense values
139      if (rudder > 45) rudder = 45;
140      if (rudder < -45) rudder = -45;
141
142 // adjust turn radius for speed. The equation is very approximate.
143      sp_turn_radius_ft = 10 * pow ((speed - 15),2) + turn_radius_ft;
144 //     cout << " speed turn radius " << sp_turn_radius_ft ; 
145
146 // adjust turn radius for rudder angle. The equation is even more approximate.     
147      float a = 19;
148      float b = -0.2485;
149      float c = 0.543;
150      
151      rd_turn_radius_ft = (a * exp(b * fabs(rudder)) + c) * sp_turn_radius_ft;
152      
153 //     cout <<" rudder turn radius " << rd_turn_radius_ft << endl;
154           
155 // calculate the angle, alpha, subtended by the arc traversed in time dt        
156      alpha = ((speed * 1.686 * dt)/rd_turn_radius_ft) * SG_RADIANS_TO_DEGREES;
157
158    
159 // make sure that alpha is applied in the right direction   
160      hdg += alpha * sign( rudder );
161      if ( hdg > 360.0 ) hdg -= 360.0;
162      if ( hdg < 0.0) hdg += 360.0;
163
164 //adjust roll for rudder angle and speed. Another bit of voodoo    
165      raw_roll =  -0.0166667 * speed * rudder;
166    }
167    else
168    {
169 // rudder angle is 0  
170      raw_roll = 0;
171 //     cout << " roll "<< roll << endl;
172    }
173
174     //low pass filter
175      roll = (raw_roll * roll_constant) + (roll * (1 - roll_constant));
176          
177      /*cout  << " rudder: " << rudder << " raw roll: "<< raw_roll<<" roll: " << roll ;
178      cout  << " hdg: " << hdg << endl ;*/
179
180    // adjust target rudder angle if heading lock engaged
181    if (hdg_lock) {
182      double rudder_sense = 0.0;
183      double diff = fabs(hdg - tgt_heading);
184      if (diff > 180) diff = fabs(diff - 360);
185      double sum = hdg + diff;
186      if (sum > 360.0) sum -= 360.0;
187      if (fabs(sum - tgt_heading) < 1.0) { 
188        rudder_sense = 1.0;
189      } else {
190        rudder_sense = -1.0;
191      } 
192      if (diff < 15){ 
193          tgt_rudder = diff * rudder_sense;
194          }
195          else
196          {
197          tgt_rudder = 45 * rudder_sense;
198      }     
199    }
200
201    // adjust rudder angle
202     double rudder_diff = tgt_rudder - rudder;
203     // set the rudder limit by speed
204     if (speed <= 40 ){
205        rudder_limit = (-0.825 * speed) + 35;
206     }else{
207        rudder_limit = 2;
208    }
209
210     if (fabs(rudder_diff) > 0.1) {
211         if (rudder_diff > 0.0){
212             rudder += rudder_constant * dt;
213             if (rudder > rudder_limit) rudder = rudder_limit;// apply the rudder limit
214         } else if (rudder_diff < 0.0){
215             rudder -= rudder_constant * dt;
216             if (rudder < -rudder_limit) rudder = -rudder_limit;
217         }
218 }
219
220
221
222 }//end function
223
224
225 void FGAIShip::AccelTo(double speed) {
226    tgt_speed = speed;
227 }
228
229 void FGAIShip::PitchTo(double angle) {
230    tgt_pitch = angle;
231 }
232
233
234 void FGAIShip::RollTo(double angle) {
235    tgt_roll = angle;
236 }
237
238
239 void FGAIShip::YawTo(double angle) {
240 }
241
242
243 void FGAIShip::ClimbTo(double altitude) {
244 }
245
246
247 void FGAIShip::TurnTo(double heading) {
248    tgt_heading = heading;
249    hdg_lock = true;
250 }
251
252
253 double FGAIShip::sign(double x) {
254
255   if ( x < 0.0 ) { return -1.0; }
256   else { return 1.0; }
257 }
258
259 void FGAIShip::setFlightPlan(FGAIFlightPlan* f) {
260   fp = f;
261 }
262
263 void FGAIShip::setName(const string& n) {
264   name = n;
265 }
266
267 void FGAIShip::ProcessFlightPlan(double dt) {
268   // not implemented yet
269 }
270
271 void FGAIShip::setRudder(float r) {
272   rudder = r;
273 }
274
275 void FGAIShip::setRoll(double rl) {
276    roll = rl;
277 }