3 #include "Atmosphere.hpp"
4 #include "Thruster.hpp"
6 #include "RigidBody.hpp"
7 #include "Integrator.hpp"
8 #include "Propeller.hpp"
9 #include "PistonEngine.hpp"
11 #include "Surface.hpp"
17 void printState(State* s)
20 Math::vmul33(tmp.orient, tmp.v, tmp.v);
21 Math::vmul33(tmp.orient, tmp.acc, tmp.acc);
22 Math::vmul33(tmp.orient, tmp.rot, tmp.rot);
23 Math::vmul33(tmp.orient, tmp.racc, tmp.racc);
25 printf("\nNEW STATE (LOCAL COORDS)\n");
26 printf("pos: %10.2f %10.2f %10.2f\n", tmp.pos[0], tmp.pos[1], tmp.pos[2]);
30 if(i != 0) printf(" ");
31 printf("%6.2f %6.2f %6.2f\n",
32 tmp.orient[3*i+0], tmp.orient[3*i+1], tmp.orient[3*i+2]);
34 printf("v: %6.2f %6.2f %6.2f\n", tmp.v[0], tmp.v[1], tmp.v[2]);
35 printf("acc: %6.2f %6.2f %6.2f\n", tmp.acc[0], tmp.acc[1], tmp.acc[2]);
36 printf("rot: %6.2f %6.2f %6.2f\n", tmp.rot[0], tmp.rot[1], tmp.rot[2]);
37 printf("rac: %6.2f %6.2f %6.2f\n", tmp.racc[0], tmp.racc[1], tmp.racc[2]);
43 for(i=0; i<3; i++) _wind[i] = 0;
45 _integrator.setBody(&_body);
46 _integrator.setEnvironment(this);
48 // Default value of 30 Hz
49 _integrator.setInterval(1.0f/30.0f);
57 // FIXME: who owns these things? Need a policy
60 void Model::getThrust(float* out)
63 out[0] = out[1] = out[2] = 0;
65 for(i=0; i<_thrusters.size(); i++) {
66 Thruster* t = (Thruster*)_thrusters.get(i);
68 Math::add3(tmp, out, out);
72 void Model::initIteration()
74 // Precompute torque and angular momentum for the thrusters
77 _gyro[i] = _torque[i] = 0;
78 for(i=0; i<_thrusters.size(); i++) {
79 Thruster* t = (Thruster*)_thrusters.get(i);
81 // Get the wind velocity at the thruster location
84 localWind(pos, _s, v);
87 t->setAir(_pressure, _temp, _rho);
88 t->integrate(_integrator.getInterval());
91 Math::add3(v, _torque, _torque);
94 Math::add3(v, _gyro, _gyro);
101 _body.recalc(); // FIXME: amortize this, somehow
102 _integrator.calcNewInterval();
105 bool Model::isCrashed()
110 void Model::setCrashed(bool crashed)
115 float Model::getAGL()
120 State* Model::getState()
125 void Model::setState(State* s)
127 _integrator.setState(s);
128 _s = _integrator.getState();
131 RigidBody* Model::getBody()
136 Integrator* Model::getIntegrator()
141 Surface* Model::getSurface(int handle)
143 return (Surface*)_surfaces.get(handle);
146 int Model::addThruster(Thruster* t)
148 return _thrusters.add(t);
151 int Model::numThrusters()
153 return _thrusters.size();
156 Thruster* Model::getThruster(int handle)
158 return (Thruster*)_thrusters.get(handle);
161 void Model::setThruster(int handle, Thruster* t)
163 _thrusters.set(handle, t);
166 int Model::addSurface(Surface* surf)
168 return _surfaces.add(surf);
171 int Model::addGear(Gear* gear)
173 return _gears.add(gear);
176 void Model::setGroundEffect(float* pos, float span, float mul)
178 Math::set3(pos, _wingCenter);
179 _groundEffectSpan = span;
183 // The first three elements are a unit vector pointing from the global
184 // origin to the plane, the final element is the distance from the
185 // origin (the radius of the earth, in most implementations). So
186 // (v dot _ground)-_ground[3] gives the distance AGL.
187 void Model::setGroundPlane(double* planeNormal, double fromOrigin)
190 for(i=0; i<3; i++) _ground[i] = planeNormal[i];
191 _ground[3] = fromOrigin;
194 void Model::setAir(float pressure, float temp, float density)
196 _pressure = pressure;
201 void Model::setWind(float* wind)
203 Math::set3(wind, _wind);
206 void Model::calcForces(State* s)
208 // Add in the pre-computed stuff. These values aren't part of the
209 // Runge-Kutta integration (they don't depend on position or
210 // velocity), and are therefore constant across the four calls to
211 // calcForces. They get computed before we begin the integration
213 _body.setGyro(_gyro);
214 _body.addTorque(_torque);
216 for(i=0; i<_thrusters.size(); i++) {
217 Thruster* t = (Thruster*)_thrusters.get(i);
218 float thrust[3], pos[3];
219 t->getThrust(thrust);
221 _body.addForce(pos, thrust);
224 // Gravity, convert to a force, then to local coordinates
226 Glue::geodUp(s->pos, grav);
227 Math::mul3(-9.8f * _body.getTotalMass(), grav, grav);
228 Math::vmul33(s->orient, grav, grav);
229 _body.addForce(grav);
231 // Do each surface, remembering that the local velocity at each
232 // point is different due to rotation.
234 faero[0] = faero[1] = faero[2] = 0;
235 for(i=0; i<_surfaces.size(); i++) {
236 Surface* sf = (Surface*)_surfaces.get(i);
238 // Vsurf = wind - velocity + (rot cross (cg - pos))
240 sf->getPosition(pos);
241 localWind(pos, s, vs);
243 float force[3], torque[3];
244 sf->calcForce(vs, _rho, force, torque);
245 Math::add3(faero, force, faero);
246 _body.addForce(pos, force);
247 _body.addTorque(torque);
250 // Get a ground plane in local coordinates. The first three
251 // elements are the normal vector, the final one is the distance
252 // from the local origin along that vector to the ground plane
253 // (negative for objects "above" the ground)
255 ground[3] = localGround(s, ground);
257 // Account for ground effect by multiplying the vertical force
258 // component by an amount linear with the fraction of the wingspan
260 float dist = ground[3] - Math::dot3(ground, _wingCenter);
261 if(dist > 0 && dist < _groundEffectSpan) {
262 float fz = Math::dot3(faero, ground);
263 Math::mul3(fz * _groundEffect * dist/_groundEffectSpan,
265 _body.addForce(faero);
268 // Convert the velocity and rotation vectors to local coordinates
269 float lrot[3], lv[3];
270 Math::vmul33(s->orient, s->rot, lrot);
271 Math::vmul33(s->orient, s->v, lv);
274 for(i=0; i<_gears.size(); i++) {
275 float force[3], contact[3];
276 Gear* g = (Gear*)_gears.get(i);
277 g->calcForce(&_body, lv, lrot, ground);
278 g->getForce(force, contact);
279 _body.addForce(contact, force);
283 void Model::newState(State* s)
289 // Some simple collision detection
291 float ground[4], pos[3], cmpr[3];
292 ground[3] = localGround(s, ground);
294 for(i=0; i<_gears.size(); i++) {
295 Gear* g = (Gear*)_gears.get(i);
297 // Get the point of ground contact
299 g->getCompression(cmpr);
300 Math::mul3(g->getCompressFraction(), cmpr, cmpr);
301 Math::add3(cmpr, pos, pos);
302 float dist = ground[3] - Math::dot3(pos, ground);
304 // Find the lowest one
309 if(_agl < -1) // Allow for some integration slop
313 // Returns a unit "down" vector for the ground in out, and the
314 // distance from the local origin to the ground as the return value.
315 // So for a given position V, "dist - (V dot out)" will be the height
317 float Model::localGround(State* s, float* out)
319 // Get the ground's "down" vector, this can be in floats, because
320 // we don't need positioning accuracy. The direction has plenty
321 // of accuracy after truncation.
322 out[0] = -(float)_ground[0];
323 out[1] = -(float)_ground[1];
324 out[2] = -(float)_ground[2];
325 Math::vmul33(s->orient, out, out);
327 // The distance from the ground to the Aircraft's origin:
328 double dist = (s->pos[0]*_ground[0]
329 + s->pos[1]*_ground[1]
330 + s->pos[2]*_ground[2] - _ground[3]);
335 // Calculates the airflow direction at the given point and for the
336 // specified aircraft velocity.
337 void Model::localWind(float* pos, State* s, float* out)
339 // Most of the input is in global coordinates. Fix that.
340 float lwind[3], lrot[3], lv[3];
341 Math::vmul33(s->orient, _wind, lwind);
342 Math::vmul33(s->orient, s->rot, lrot);
343 Math::vmul33(s->orient, s->v, lv);
345 _body.pointVelocity(pos, lrot, out); // rotational velocity
346 Math::mul3(-1, out, out); // (negated)
347 Math::add3(lwind, out, out); // + wind
348 Math::sub3(out, lv, out); // - velocity
351 }; // namespace yasim