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.0/30.0);
54 // FIXME: who owns these things? Need a policy
57 void Model::getThrust(float* out)
60 out[0] = out[1] = out[2] = 0;
62 for(i=0; i<_thrusters.size(); i++) {
63 Thruster* t = (Thruster*)_thrusters.get(i);
65 Math::add3(tmp, out, out);
69 void Model::initIteration()
71 // Precompute torque and angular momentum for the thrusters
74 _gyro[i] = _torque[i] = 0;
75 for(i=0; i<_thrusters.size(); i++) {
76 Thruster* t = (Thruster*)_thrusters.get(i);
78 // Get the wind velocity at the thruster location
81 localWind(pos, _s, v);
84 t->setAir(_pressure, _temp);
85 t->integrate(_integrator.getInterval());
88 Math::add3(v, _torque, _torque);
91 Math::add3(v, _gyro, _gyro);
98 _body.recalc(); // FIXME: amortize this, somehow
99 _integrator.calcNewInterval();
102 State* Model::getState()
107 void Model::setState(State* s)
109 _integrator.setState(s);
110 _s = _integrator.getState();
113 RigidBody* Model::getBody()
118 Integrator* Model::getIntegrator()
123 Surface* Model::getSurface(int handle)
125 return (Surface*)_surfaces.get(handle);
128 int Model::addThruster(Thruster* t)
130 return _thrusters.add(t);
133 int Model::numThrusters()
135 return _thrusters.size();
138 Thruster* Model::getThruster(int handle)
140 return (Thruster*)_thrusters.get(handle);
143 void Model::setThruster(int handle, Thruster* t)
145 _thrusters.set(handle, t);
148 int Model::addSurface(Surface* surf)
150 return _surfaces.add(surf);
153 int Model::addGear(Gear* gear)
155 return _gears.add(gear);
158 void Model::setGroundEffect(float* pos, float span, float mul)
160 Math::set3(pos, _wingCenter);
161 _groundEffectSpan = span;
165 // The first three elements are a unit vector pointing from the global
166 // origin to the plane, the final element is the distance from the
167 // origin (the radius of the earth, in most implementations). So
168 // (v dot _ground)-_ground[3] gives the distance AGL.
169 void Model::setGroundPlane(double* planeNormal, double fromOrigin)
172 for(i=0; i<3; i++) _ground[i] = planeNormal[i];
173 _ground[3] = fromOrigin;
176 void Model::setAir(float pressure, float temp)
178 _pressure = pressure;
180 _rho = Atmosphere::calcDensity(pressure, temp);
183 void Model::setWind(float* wind)
185 Math::set3(wind, _wind);
188 void Model::calcForces(State* s)
190 // Add in the pre-computed stuff. These values aren't part of the
191 // Runge-Kutta integration (they don't depend on position or
192 // velocity), and are therefore constant across the four calls to
193 // calcForces. They get computed before we begin the integration
195 _body.setGyro(_gyro);
196 _body.addTorque(_torque);
198 for(i=0; i<_thrusters.size(); i++) {
199 Thruster* t = (Thruster*)_thrusters.get(i);
200 float thrust[3], pos[3];
201 t->getThrust(thrust);
203 _body.addForce(pos, thrust);
206 // Gravity, convert to a force, then to local coordinates
208 Glue::geodUp(s->pos, grav);
209 Math::mul3(-9.8 * _body.getTotalMass(), grav, grav);
210 Math::vmul33(s->orient, grav, grav);
211 _body.addForce(grav);
213 // Do each surface, remembering that the local velocity at each
214 // point is different due to rotation.
216 faero[0] = faero[1] = faero[2] = 0;
217 for(i=0; i<_surfaces.size(); i++) {
218 Surface* sf = (Surface*)_surfaces.get(i);
220 // Vsurf = wind - velocity + (rot cross (cg - pos))
222 sf->getPosition(pos);
223 localWind(pos, s, vs);
225 float force[3], torque[3];
226 sf->calcForce(vs, _rho, force, torque);
227 Math::add3(faero, force, faero);
228 _body.addForce(pos, force);
229 _body.addTorque(torque);
232 // Get a ground plane in local coordinates. The first three
233 // elements are the normal vector, the final one is the distance
234 // from the local origin along that vector to the ground plane
235 // (negative for objects "above" the ground)
237 ground[3] = localGround(s, ground);
239 // Account for ground effect by multiplying the vertical force
240 // component by an amount linear with the fraction of the wingspan
242 float dist = ground[4] - Math::dot3(ground, _wingCenter);
243 if(dist > 0 && dist < _groundEffectSpan) {
244 float fz = Math::dot3(faero, ground);
245 Math::mul3(fz * _groundEffect * dist/_groundEffectSpan,
247 _body.addForce(faero);
250 // Convert the velocity and rotation vectors to local coordinates
251 float lrot[3], lv[3];
252 Math::vmul33(s->orient, s->rot, lrot);
253 Math::vmul33(s->orient, s->v, lv);
256 for(i=0; i<_gears.size(); i++) {
257 float force[3], contact[3];
258 Gear* g = (Gear*)_gears.get(i);
259 g->calcForce(&_body, lv, lrot, ground);
260 g->getForce(force, contact);
261 _body.addForce(contact, force);
265 void Model::newState(State* s)
271 // Some simple collision detection
272 float ground[4], pos[3], cmpr[3];
273 ground[3] = localGround(s, ground);
275 for(i=0; i<_gears.size(); i++) {
276 Gear* g = (Gear*)_gears.get(i);
278 g->getCompression(cmpr);
279 Math::add3(cmpr, pos, pos);
280 float dist = ground[3] - Math::dot3(pos, ground);
282 printf("CRASH: gear %d\n", i);
288 // Returns a unit "down" vector for the ground in out, and the
289 // distance from the local origin to the ground as the return value.
290 // So for a given position V, "dist - (V dot out)" will be the height
292 float Model::localGround(State* s, float* out)
294 // Get the ground's "down" vector, this can be in floats, because
295 // we don't need positioning accuracy. The direction has plenty
296 // of accuracy after truncation.
297 out[0] = -(float)_ground[0];
298 out[1] = -(float)_ground[1];
299 out[2] = -(float)_ground[2];
300 Math::vmul33(s->orient, out, out);
302 // The distance from the ground to the Aircraft's origin:
303 double dist = (s->pos[0]*_ground[0]
304 + s->pos[1]*_ground[1]
305 + s->pos[2]*_ground[2] - _ground[3]);
310 // Calculates the airflow direction at the given point and for the
311 // specified aircraft velocity.
312 void Model::localWind(float* pos, State* s, float* out)
314 // Most of the input is in global coordinates. Fix that.
315 float lwind[3], lrot[3], lv[3];
316 Math::vmul33(s->orient, _wind, lwind);
317 Math::vmul33(s->orient, s->rot, lrot);
318 Math::vmul33(s->orient, s->v, lv);
320 _body.pointVelocity(pos, lrot, out); // rotational velocity
321 Math::mul3(-1, out, out); // (negated)
322 Math::add3(lwind, out, out); // + wind
323 Math::sub3(out, lv, out); // - velocity
326 }; // namespace yasim