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]);
28 for(int i=0; i<3; i++) {
29 if(i != 0) printf(" ");
30 printf("%6.2f %6.2f %6.2f\n",
31 tmp.orient[3*i+0], tmp.orient[3*i+1], tmp.orient[3*i+2]);
33 printf("v: %6.2f %6.2f %6.2f\n", tmp.v[0], tmp.v[1], tmp.v[2]);
34 printf("acc: %6.2f %6.2f %6.2f\n", tmp.acc[0], tmp.acc[1], tmp.acc[2]);
35 printf("rot: %6.2f %6.2f %6.2f\n", tmp.rot[0], tmp.rot[1], tmp.rot[2]);
36 printf("rac: %6.2f %6.2f %6.2f\n", tmp.racc[0], tmp.racc[1], tmp.racc[2]);
41 for(int i=0; i<3; i++) _wind[i] = 0;
43 _integrator.setBody(&_body);
44 _integrator.setEnvironment(this);
46 // Default value of 30 Hz
47 _integrator.setInterval(1.0/30.0);
52 // FIXME: who owns these things? Need a policy
55 void Model::getThrust(float* out)
58 out[0] = out[1] = out[2] = 0;
59 for(int i=0; i<_thrusters.size(); i++) {
60 Thruster* t = (Thruster*)_thrusters.get(i);
62 Math::add3(tmp, out, out);
66 void Model::initIteration()
68 // Precompute torque and angular momentum for the thrusters
69 for(int i=0; i<3; i++)
70 _gyro[i] = _torque[i] = 0;
71 for(int i=0; i<_thrusters.size(); i++) {
72 Thruster* t = (Thruster*)_thrusters.get(i);
74 // Get the wind velocity at the thruster location
77 localWind(pos, _s, v);
81 t->integrate(_integrator.getInterval());
84 Math::add3(v, _torque, _torque);
87 Math::add3(v, _gyro, _gyro);
94 _body.recalc(); // FIXME: amortize this, somehow
95 _integrator.calcNewInterval();
98 State* Model::getState()
103 void Model::setState(State* s)
105 _integrator.setState(s);
106 _s = _integrator.getState();
109 RigidBody* Model::getBody()
114 Integrator* Model::getIntegrator()
119 Surface* Model::getSurface(int handle)
121 return (Surface*)_surfaces.get(handle);
124 int Model::addThruster(Thruster* t)
126 return _thrusters.add(t);
129 int Model::numThrusters()
131 return _thrusters.size();
134 Thruster* Model::getThruster(int handle)
136 return (Thruster*)_thrusters.get(handle);
139 void Model::setThruster(int handle, Thruster* t)
141 _thrusters.set(handle, t);
144 int Model::addSurface(Surface* surf)
146 return _surfaces.add(surf);
149 int Model::addGear(Gear* gear)
151 return _gears.add(gear);
154 void Model::setGroundEffect(float* pos, float span, float mul)
156 Math::set3(pos, _wingCenter);
157 _groundEffectSpan = span;
161 // The first three elements are a unit vector pointing from the global
162 // origin to the plane, the final element is the distance from the
163 // origin (the radius of the earth, in most implementations). So
164 // (v dot _ground)-_ground[3] gives the distance AGL.
165 void Model::setGroundPlane(double* planeNormal, double fromOrigin)
167 for(int i=0; i<3; i++) _ground[i] = planeNormal[i];
168 _ground[3] = fromOrigin;
171 void Model::setAir(float pressure, float temp)
175 _rho = Atmosphere::calcDensity(pressure, temp);
178 void Model::setWind(float* wind)
180 Math::set3(wind, _wind);
183 void Model::calcForces(State* s)
185 // Add in the pre-computed stuff. These values aren't part of the
186 // Runge-Kutta integration (they don't depend on position or
187 // velocity), and are therefore constant across the four calls to
188 // calcForces. They get computed before we begin the integration
190 _body.setGyro(_gyro);
191 _body.addTorque(_torque);
192 for(int i=0; i<_thrusters.size(); i++) {
193 Thruster* t = (Thruster*)_thrusters.get(i);
194 float thrust[3], pos[3];
195 t->getThrust(thrust);
197 _body.addForce(pos, thrust);
200 // Gravity, convert to a force, then to local coordinates
202 Glue::geodUp(s->pos, grav);
203 Math::mul3(-9.8 * _body.getTotalMass(), grav, grav);
204 Math::vmul33(s->orient, grav, grav);
205 _body.addForce(grav);
207 // Do each surface, remembering that the local velocity at each
208 // point is different due to rotation.
210 faero[0] = faero[1] = faero[2] = 0;
211 for(int i=0; i<_surfaces.size(); i++) {
212 Surface* sf = (Surface*)_surfaces.get(i);
214 // Vsurf = wind - velocity + (rot cross (cg - pos))
216 sf->getPosition(pos);
217 localWind(pos, s, vs);
219 float force[3], torque[3];
220 sf->calcForce(vs, _rho, force, torque);
221 Math::add3(faero, force, faero);
222 _body.addForce(pos, force);
223 _body.addTorque(torque);
226 // Get a ground plane in local coordinates. The first three
227 // elements are the normal vector, the final one is the distance
228 // from the local origin along that vector to the ground plane
229 // (negative for objects "above" the ground)
231 ground[3] = localGround(s, ground);
233 // Account for ground effect by multiplying the vertical force
234 // component by an amount linear with the fraction of the wingspan
236 float dist = ground[4] - Math::dot3(ground, _wingCenter);
237 if(dist > 0 && dist < _groundEffectSpan) {
238 float fz = Math::dot3(faero, ground);
239 Math::mul3(fz * _groundEffect * dist/_groundEffectSpan,
241 _body.addForce(faero);
244 // Convert the velocity and rotation vectors to local coordinates
245 float lrot[3], lv[3];
246 Math::vmul33(s->orient, s->rot, lrot);
247 Math::vmul33(s->orient, s->v, lv);
250 for(int i=0; i<_gears.size(); i++) {
251 float force[3], contact[3];
252 Gear* g = (Gear*)_gears.get(i);
253 g->calcForce(&_body, lv, lrot, ground);
254 g->getForce(force, contact);
255 _body.addForce(contact, force);
259 void Model::newState(State* s)
265 // Some simple collision detection
266 float ground[4], pos[3], cmpr[3];
267 ground[3] = localGround(s, ground);
268 for(int i=0; i<_gears.size(); i++) {
269 Gear* g = (Gear*)_gears.get(i);
271 g->getCompression(cmpr);
272 Math::add3(cmpr, pos, pos);
273 float dist = ground[3] - Math::dot3(pos, ground);
275 printf("CRASH: gear %d\n", i);
281 // Returns a unit "down" vector for the ground in out, and the
282 // distance from the local origin to the ground as the return value.
283 // So for a given position V, "dist - (V dot out)" will be the height
285 float Model::localGround(State* s, float* out)
287 // Get the ground's "down" vector, this can be in floats, because
288 // we don't need positioning accuracy. The direction has plenty
289 // of accuracy after truncation.
290 out[0] = -(float)_ground[0];
291 out[1] = -(float)_ground[1];
292 out[2] = -(float)_ground[2];
293 Math::vmul33(s->orient, out, out);
295 // The distance from the ground to the Aircraft's origin:
296 double dist = (s->pos[0]*_ground[0]
297 + s->pos[1]*_ground[1]
298 + s->pos[2]*_ground[2] - _ground[3]);
303 // Calculates the airflow direction at the given point and for the
304 // specified aircraft velocity.
305 void Model::localWind(float* pos, State* s, float* out)
307 // Most of the input is in global coordinates. Fix that.
308 float lwind[3], lrot[3], lv[3];
309 Math::vmul33(s->orient, _wind, lwind);
310 Math::vmul33(s->orient, s->rot, lrot);
311 Math::vmul33(s->orient, s->v, lv);
313 _body.pointVelocity(pos, lrot, out); // rotational velocity
314 Math::mul3(-1, out, out); // (negated)
315 Math::add3(lwind, out, out); // + wind
316 Math::sub3(out, lv, out); // - velocity
319 }; // namespace yasim