-#include <stdio.h>
-
#include "Atmosphere.hpp"
#include "Thruster.hpp"
#include "Math.hpp"
#include "PistonEngine.hpp"
#include "Gear.hpp"
#include "Surface.hpp"
+#include "Rotor.hpp"
+#include "Rotorpart.hpp"
+#include "Rotorblade.hpp"
#include "Glue.hpp"
#include "Model.hpp"
namespace yasim {
+#if 0
void printState(State* s)
{
State tmp = *s;
printf("rot: %6.2f %6.2f %6.2f\n", tmp.rot[0], tmp.rot[1], tmp.rot[2]);
printf("rac: %6.2f %6.2f %6.2f\n", tmp.racc[0], tmp.racc[1], tmp.racc[2]);
}
+#endif
Model::Model()
{
_integrator.setEnvironment(this);
// Default value of 30 Hz
- _integrator.setInterval(1.0/30.0);
+ _integrator.setInterval(1.0f/30.0f);
+
+ _agl = 0;
+ _crashed = false;
+ _turb = 0;
}
Model::~Model()
int i;
for(i=0; i<3; i++)
_gyro[i] = _torque[i] = 0;
+
+ // Need a local altitude for the wind calculation
+ float dummy[3];
+ float alt = Math::abs(localGround(_s, dummy));
+
for(i=0; i<_thrusters.size(); i++) {
Thruster* t = (Thruster*)_thrusters.get(i);
// Get the wind velocity at the thruster location
float pos[3], v[3];
t->getPosition(pos);
- localWind(pos, _s, v);
+ localWind(pos, _s, v, alt);
t->setWind(v);
- t->setAir(_pressure, _temp);
+ t->setAir(_pressure, _temp, _rho);
t->integrate(_integrator.getInterval());
t->getTorque(v);
t->getGyro(v);
Math::add3(v, _gyro, _gyro);
}
+
+ // Displace the turbulence coordinates according to the local wind.
+ if(_turb) {
+ float toff[3];
+ Math::mul3(_integrator.getInterval(), _wind, toff);
+ _turb->offset(toff);
+ }
+}
+
+// FIXME: This method looks to me like it's doing *integration*, not
+// initialization. Integration code should ideally go into
+// calcForces. Only very "unstiff" problems can be solved well like
+// this (see the engine code for an example); I don't know if rotor
+// dynamics qualify or not.
+// -Andy
+void Model::initRotorIteration()
+{
+ int i;
+ float dt = _integrator.getInterval();
+ float lrot[3];
+ Math::vmul33(_s->orient, _s->rot, lrot);
+ Math::mul3(dt,lrot,lrot);
+ for(i=0; i<_rotors.size(); i++) {
+ Rotor* r = (Rotor*)_rotors.get(i);
+ r->inititeration(dt);
+ }
+ for(i=0; i<_rotorparts.size(); i++) {
+ Rotorpart* rp = (Rotorpart*)_rotorparts.get(i);
+ rp->inititeration(dt,lrot);
+ }
+ for(i=0; i<_rotorblades.size(); i++) {
+ Rotorblade* rp = (Rotorblade*)_rotorblades.get(i);
+ rp->inititeration(dt,lrot);
+ }
}
void Model::iterate()
{
initIteration();
+ initRotorIteration();
_body.recalc(); // FIXME: amortize this, somehow
_integrator.calcNewInterval();
}
+bool Model::isCrashed()
+{
+ return _crashed;
+}
+
+void Model::setCrashed(bool crashed)
+{
+ _crashed = crashed;
+}
+
+float Model::getAGL()
+{
+ return _agl;
+}
+
State* Model::getState()
{
return _s;
return (Surface*)_surfaces.get(handle);
}
+Rotorpart* Model::getRotorpart(int handle)
+{
+ return (Rotorpart*)_rotorparts.get(handle);
+}
+Rotorblade* Model::getRotorblade(int handle)
+{
+ return (Rotorblade*)_rotorblades.get(handle);
+}
+Rotor* Model::getRotor(int handle)
+{
+ return (Rotor*)_rotors.get(handle);
+}
+
int Model::addThruster(Thruster* t)
{
return _thrusters.add(t);
return _surfaces.add(surf);
}
+int Model::addRotorpart(Rotorpart* rpart)
+{
+ return _rotorparts.add(rpart);
+}
+int Model::addRotorblade(Rotorblade* rblade)
+{
+ return _rotorblades.add(rblade);
+}
+int Model::addRotor(Rotor* r)
+{
+ return _rotors.add(r);
+}
+
int Model::addGear(Gear* gear)
{
return _gears.add(gear);
// (v dot _ground)-_ground[3] gives the distance AGL.
void Model::setGroundPlane(double* planeNormal, double fromOrigin)
{
- int i;
- for(i=0; i<3; i++) _ground[i] = planeNormal[i];
+ for(int i=0; i<3; i++) _ground[i] = planeNormal[i];
_ground[3] = fromOrigin;
}
-void Model::setAir(float pressure, float temp)
+void Model::setAir(float pressure, float temp, float density)
{
_pressure = pressure;
_temp = temp;
- _rho = Atmosphere::calcDensity(pressure, temp);
+ _rho = density;
}
void Model::setWind(float* wind)
_body.addForce(pos, thrust);
}
+ // Get a ground plane in local coordinates. The first three
+ // elements are the normal vector, the final one is the distance
+ // from the local origin along that vector to the ground plane
+ // (negative for objects "above" the ground)
+ float ground[4];
+ ground[3] = localGround(s, ground);
+ float alt = Math::abs(ground[3]);
+
// Gravity, convert to a force, then to local coordinates
float grav[3];
Glue::geodUp(s->pos, grav);
- Math::mul3(-9.8 * _body.getTotalMass(), grav, grav);
+ Math::mul3(-9.8f * _body.getTotalMass(), grav, grav);
Math::vmul33(s->orient, grav, grav);
_body.addForce(grav);
// Vsurf = wind - velocity + (rot cross (cg - pos))
float vs[3], pos[3];
sf->getPosition(pos);
- localWind(pos, s, vs);
+ localWind(pos, s, vs, alt);
float force[3], torque[3];
sf->calcForce(vs, _rho, force, torque);
Math::add3(faero, force, faero);
+
_body.addForce(pos, force);
_body.addTorque(torque);
}
+ for(i=0; i<_rotorparts.size(); i++) {
+ Rotorpart* sf = (Rotorpart*)_rotorparts.get(i);
- // Get a ground plane in local coordinates. The first three
- // elements are the normal vector, the final one is the distance
- // from the local origin along that vector to the ground plane
- // (negative for objects "above" the ground)
- float ground[4];
- ground[3] = localGround(s, ground);
+ // Vsurf = wind - velocity + (rot cross (cg - pos))
+ float vs[3], pos[3];
+ sf->getPosition(pos);
+ localWind(pos, s, vs, alt);
+
+ float force[3], torque[3];
+ sf->calcForce(vs, _rho, force, torque);
+ //Math::add3(faero, force, faero);
+
+ sf->getPositionForceAttac(pos);
+
+ _body.addForce(pos, force);
+ _body.addTorque(torque);
+ }
+ for(i=0; i<_rotorblades.size(); i++) {
+ Rotorblade* sf = (Rotorblade*)_rotorblades.get(i);
+
+ // Vsurf = wind - velocity + (rot cross (cg - pos))
+ float vs[3], pos[3];
+ sf->getPosition(pos);
+ localWind(pos, s, vs, alt);
+
+ float force[3], torque[3];
+ sf->calcForce(vs, _rho, force, torque);
+ //Math::add3(faero, force, faero);
+
+ sf->getPositionForceAttac(pos);
+
+ _body.addForce(pos, force);
+ _body.addTorque(torque);
+ }
// Account for ground effect by multiplying the vertical force
// component by an amount linear with the fraction of the wingspan
// above the ground.
- float dist = ground[4] - Math::dot3(ground, _wingCenter);
+ float dist = ground[3] - Math::dot3(ground, _wingCenter);
if(dist > 0 && dist < _groundEffectSpan) {
float fz = Math::dot3(faero, ground);
- Math::mul3(fz * _groundEffect * dist/_groundEffectSpan,
- ground, faero);
+ fz *= (_groundEffectSpan - dist) / _groundEffectSpan;
+ fz *= _groundEffect;
+ Math::mul3(fz, ground, faero);
_body.addForce(faero);
}
{
_s = s;
- //printState(s);
-
// Some simple collision detection
+ float min = 1e8;
float ground[4], pos[3], cmpr[3];
ground[3] = localGround(s, ground);
int i;
for(i=0; i<_gears.size(); i++) {
Gear* g = (Gear*)_gears.get(i);
+
+ // Get the point of ground contact
g->getPosition(pos);
g->getCompression(cmpr);
+ Math::mul3(g->getCompressFraction(), cmpr, cmpr);
Math::add3(cmpr, pos, pos);
float dist = ground[3] - Math::dot3(pos, ground);
- if(dist < 0) {
- printf("CRASH: gear %d\n", i);
- *(int*)0=0;
- }
+
+ // Find the lowest one
+ if(dist < min)
+ min = dist;
}
+ _agl = min;
+ if(_agl < -1) // Allow for some integration slop
+ _crashed = true;
}
// Returns a unit "down" vector for the ground in out, and the
// Calculates the airflow direction at the given point and for the
// specified aircraft velocity.
-void Model::localWind(float* pos, State* s, float* out)
+void Model::localWind(float* pos, State* s, float* out, float alt)
{
- // Most of the input is in global coordinates. Fix that.
- float lwind[3], lrot[3], lv[3];
- Math::vmul33(s->orient, _wind, lwind);
+ float tmp[3], lwind[3], lrot[3], lv[3];
+
+ // Get a global coordinate for our local position, and calculate
+ // turbulence.
+ if(_turb) {
+ double gpos[3]; float up[3];
+ Math::tmul33(s->orient, pos, tmp);
+ for(int i=0; i<3; i++) {
+ gpos[i] = s->pos[i] + tmp[i];
+ up[i] = _ground[i];
+ }
+ _turb->getTurbulence(gpos, alt, up, lwind);
+ Math::add3(_wind, lwind, lwind);
+ } else {
+ Math::set3(_wind, lwind);
+ }
+
+ // Convert to local coordinates
+ Math::vmul33(s->orient, lwind, lwind);
Math::vmul33(s->orient, s->rot, lrot);
Math::vmul33(s->orient, s->v, lv);
projects / flightgear.git / blobdiff