From: ehofman Date: Thu, 17 Feb 2005 10:26:14 +0000 (+0000) Subject: Mathias Frohlich: Add carrier capabilities for YASim aircraft. X-Git-Url: https://git.mxchange.org/?a=commitdiff_plain;h=c2e688244d6db9aa42a3f71a00f9e70fbd1f8060;p=flightgear.git Mathias Frohlich: Add carrier capabilities for YASim aircraft. --- diff --git a/src/FDM/YASim/Airplane.cpp b/src/FDM/YASim/Airplane.cpp index d5f49c59b..df9dfbe60 100644 --- a/src/FDM/YASim/Airplane.cpp +++ b/src/FDM/YASim/Airplane.cpp @@ -137,6 +137,16 @@ Gear* Airplane::getGear(int g) return ((GearRec*)_gears.get(g))->gear; } +Hook* Airplane::getHook() +{ + return _model.getHook(); +} + +Launchbar* Airplane::getLaunchbar() +{ + return _model.getLaunchbar(); +} + void Airplane::updateGearState() { for(int i=0; i<_gears.size(); i++) { @@ -285,6 +295,16 @@ void Airplane::addGear(Gear* gear) _gears.add(g); } +void Airplane::addHook(Hook* hook) +{ + _model.addHook(hook); +} + +void Airplane::addLaunchbar(Launchbar* launchbar) +{ + _model.addLaunchbar(launchbar); +} + void Airplane::addThruster(Thruster* thruster, float mass, float* cg) { ThrustRec* t = new ThrustRec(); @@ -595,10 +615,6 @@ void Airplane::compileContactPoints() void Airplane::compile() { - double ground[3]; - ground[0] = 0; ground[1] = 0; ground[2] = 1; - _model.setGroundPlane(ground, -100000); - RigidBody* body = _model.getBody(); int firstMass = body->numMasses(); diff --git a/src/FDM/YASim/Airplane.hpp b/src/FDM/YASim/Airplane.hpp index 9131fa683..8c46c46f5 100644 --- a/src/FDM/YASim/Airplane.hpp +++ b/src/FDM/YASim/Airplane.hpp @@ -10,6 +10,8 @@ namespace yasim { class Gear; +class Hook; +class Launchbar; class Thruster; class Airplane { @@ -42,6 +44,8 @@ public: float taper=1, float mid=0.5); int addTank(float* pos, float cap, float fuelDensity); void addGear(Gear* g); + void addHook(Hook* h); + void addLaunchbar(Launchbar* l); void addThruster(Thruster* t, float mass, float* cg); void addBallast(float* pos, float mass); @@ -59,6 +63,8 @@ public: int numGear(); Gear* getGear(int g); + Hook* getHook(); + Launchbar* getLaunchbar(); int numThrusters() { return _thrusters.size(); } Thruster* getThruster(int n) { diff --git a/src/FDM/YASim/BodyEnvironment.hpp b/src/FDM/YASim/BodyEnvironment.hpp index ca8258431..69b4a3e4e 100644 --- a/src/FDM/YASim/BodyEnvironment.hpp +++ b/src/FDM/YASim/BodyEnvironment.hpp @@ -2,6 +2,7 @@ #define _BODYENVIRONMENT_HPP #include "RigidBody.hpp" +#include "Math.hpp" namespace yasim { @@ -29,6 +30,39 @@ struct State { orient[3*i+j] = i==j ? 1.0f : 0.0f; } } + + void posLocalToGlobal(float* lpos, double *gpos) { + float tmp[3]; + Math::tmul33(orient, lpos, tmp); + gpos[0] = tmp[0] + pos[0]; + gpos[1] = tmp[1] + pos[1]; + gpos[2] = tmp[2] + pos[2]; + } + void posGlobalToLocal(double* gpos, float *lpos) { + lpos[0] = gpos[0] - pos[0]; + lpos[1] = gpos[1] - pos[1]; + lpos[2] = gpos[2] - pos[2]; + Math::vmul33(orient, lpos, lpos); + } + void velLocalToGlobal(float* lvel, float *gvel) { + Math::tmul33(orient, lvel, gvel); + } + void velGlobalToLocal(float* gvel, float *lvel) { + Math::vmul33(orient, gvel, lvel); + } + + void planeGlobalToLocal(double* gplane, float *lplane) { + // First the normal vector transformed to local coordinates. + lplane[0] = -gplane[0]; + lplane[1] = -gplane[1]; + lplane[2] = -gplane[2]; + Math::vmul33(orient, lplane, lplane); + + // Then the distance from the plane to the Aircraft's origin. + lplane[3] = (float)(pos[0]*gplane[0] + pos[1]*gplane[1] + + pos[2]*gplane[2] - gplane[3]); + } + }; // diff --git a/src/FDM/YASim/ControlMap.cpp b/src/FDM/YASim/ControlMap.cpp index 87bf3b80d..cd7174cc5 100644 --- a/src/FDM/YASim/ControlMap.cpp +++ b/src/FDM/YASim/ControlMap.cpp @@ -4,6 +4,8 @@ #include "PistonEngine.hpp" #include "TurbineEngine.hpp" #include "Gear.hpp" +#include "Hook.hpp" +#include "Launchbar.hpp" #include "Wing.hpp" #include "Rotor.hpp" #include "Math.hpp" @@ -198,6 +200,8 @@ void ControlMap::applyControls(float dt) case BRAKE: ((Gear*)obj)->setBrake(lval); break; case STEER: ((Gear*)obj)->setRotation(lval); break; case EXTEND: ((Gear*)obj)->setExtension(lval); break; + case HEXTEND: ((Hook*)obj)->setExtension(lval); break; + case LEXTEND: ((Launchbar*)obj)->setExtension(lval); break; case CASTERING:((Gear*)obj)->setCastering(lval != 0); break; case SLAT: ((Wing*)obj)->setSlat(lval); break; case FLAP0: ((Wing*)obj)->setFlap0(lval, rval); break; diff --git a/src/FDM/YASim/FGFDM.cpp b/src/FDM/YASim/FGFDM.cpp index cc332d34d..12049ea41 100644 --- a/src/FDM/YASim/FGFDM.cpp +++ b/src/FDM/YASim/FGFDM.cpp @@ -7,6 +7,8 @@ #include "Jet.hpp" #include "SimpleJet.hpp" #include "Gear.hpp" +#include "Hook.hpp" +#include "Launchbar.hpp" #include "Atmosphere.hpp" #include "PropEngine.hpp" #include "Propeller.hpp" @@ -46,6 +48,8 @@ static const float NM2FTLB = (1/(LBS2N*FT2M)); FGFDM::FGFDM() { + _vehicle_radius = 0.0f; + _nextEngine = 0; // Map /controls/flight/elevator to the approach elevator control. This @@ -231,6 +235,9 @@ void FGFDM::startElement(const char* name, const XMLAttributes &atts) v[1] = attrf(a, "y"); v[2] = attrf(a, "z"); g->setPosition(v); + float nrm = Math::mag3(v); + if (_vehicle_radius < nrm) + _vehicle_radius = nrm; v[0] = 0; v[1] = 0; v[2] = attrf(a, "compression", 1); @@ -242,7 +249,36 @@ void FGFDM::startElement(const char* name, const XMLAttributes &atts) g->setDamping(attrf(a, "damp", 1)); _airplane.addGear(g); } else if(eq(name, "hook")) { + Hook* h = new Hook(); + _currObj = h; + v[0] = attrf(a, "x"); + v[1] = attrf(a, "y"); + v[2] = attrf(a, "z"); + h->setPosition(v); + float length = attrf(a, "length", 1.0); + h->setLength(length); + float nrm = length+Math::mag3(v); + if (_vehicle_radius < nrm) + _vehicle_radius = nrm; + h->setDownAngle(attrf(a, "down-angle", 70) * DEG2RAD); + h->setUpAngle(attrf(a, "up-angle", 0) * DEG2RAD); + _airplane.addHook(h); } else if(eq(name, "launchbar")) { + Launchbar* l = new Launchbar(); + _currObj = l; + v[0] = attrf(a, "x"); + v[1] = attrf(a, "y"); + v[2] = attrf(a, "z"); + l->setLaunchbarMount(v); + v[0] = attrf(a, "holdback-x", v[0]); + v[1] = attrf(a, "holdback-y", v[1]); + v[2] = attrf(a, "holdback-z", v[2]); + l->setHoldbackMount(v); + float length = attrf(a, "length", 1.0); + l->setLength(length); + l->setDownAngle(attrf(a, "down-angle", 30) * DEG2RAD); + l->setUpAngle(attrf(a, "up-angle", -30) * DEG2RAD); + _airplane.addLaunchbar(l); } else if(eq(name, "fuselage")) { float b[3]; v[0] = attrf(a, "ax"); diff --git a/src/FDM/YASim/FGFDM.hpp b/src/FDM/YASim/FGFDM.hpp index e6503581e..12fc1b607 100644 --- a/src/FDM/YASim/FGFDM.hpp +++ b/src/FDM/YASim/FGFDM.hpp @@ -27,6 +27,8 @@ public: // XML parsing callback from XMLVisitor virtual void startElement(const char* name, const XMLAttributes &atts); + float getVehicleRadius(void) const { return _vehicle_radius; } + private: struct AxisRec { char* name; int handle; }; struct EngRec { char* prefix; Thruster* eng; }; @@ -72,6 +74,9 @@ private: // Output properties for the ControlMap Vector _controlProps; + // Radius of the vehicle, for intersection testing. + float _vehicle_radius; + // Parsing temporaries void* _currObj; bool _cruiseCurr; diff --git a/src/FDM/YASim/Gear.cpp b/src/FDM/YASim/Gear.cpp index 51492dcfa..d56a519a0 100644 --- a/src/FDM/YASim/Gear.cpp +++ b/src/FDM/YASim/Gear.cpp @@ -1,4 +1,5 @@ #include "Math.hpp" +#include "BodyEnvironment.hpp" #include "RigidBody.hpp" #include "Gear.hpp" @@ -17,6 +18,12 @@ Gear::Gear() _rot = 0; _extension = 1; _castering = false; + _frac = 0; + + for(i=0; i<3; i++) + _global_ground[i] = _global_vel[i] = 0; + _global_ground[2] = 1; + _global_ground[3] = -1e3; } void Gear::setPosition(float* position) @@ -71,6 +78,13 @@ void Gear::setCastering(bool c) _castering = c; } +void Gear::setGlobalGround(double *global_ground, float* global_vel) +{ + int i; + for(i=0; i<4; i++) _global_ground[i] = global_ground[i]; + for(i=0; i<3; i++) _global_vel[i] = global_vel[i]; +} + void Gear::getPosition(float* out) { int i; @@ -83,6 +97,12 @@ void Gear::getCompression(float* out) for(i=0; i<3; i++) out[i] = _cmpr[i]; } +void Gear::getGlobalGround(double* global_ground) +{ + int i; + for(i=0; i<4; i++) global_ground[i] = _global_ground[i]; +} + float Gear::getSpring() { return _spring; @@ -139,7 +159,7 @@ bool Gear::getCastering() return _castering; } -void Gear::calcForce(RigidBody* body, float* v, float* rot, float* ground) +void Gear::calcForce(RigidBody* body, State *s, float* v, float* rot) { // Init the return values int i; @@ -149,7 +169,13 @@ void Gear::calcForce(RigidBody* body, float* v, float* rot, float* ground) if(_extension < 1) return; - float tmp[3]; + // The ground plane transformed to the local frame. + float ground[4]; + s->planeGlobalToLocal(_global_ground, ground); + + // The velocity of the contact patch transformed to local coordinates. + float glvel[3]; + s->velGlobalToLocal(_global_vel, glvel); // First off, make sure that the gear "tip" is below the ground. // If it's not, there's no force. @@ -164,6 +190,7 @@ void Gear::calcForce(RigidBody* body, float* v, float* rot, float* ground) // distance from the base to ground. We can get the fraction // (0-1) of compression from a/(a-b). Note the minus sign -- stuff // above ground is negative. + float tmp[3]; Math::add3(_cmpr, _pos, tmp); float b = ground[3] - Math::dot3(tmp, ground); @@ -182,6 +209,7 @@ void Gear::calcForce(RigidBody* body, float* v, float* rot, float* ground) float cv[3]; body->pointVelocity(_contact, rot, cv); Math::add3(cv, v, cv); + Math::sub3(cv, glvel, cv); // Finally, we can start adding up the forces. First the spring // compression. (note the clamping of _frac to 1): @@ -226,11 +254,18 @@ void Gear::calcForce(RigidBody* body, float* v, float* rot, float* ground) Math::cross3(skid, gup, steer); // skid cross up == steer if(_rot != 0) { - // Correct for a (small) rotation - Math::mul3(_rot, steer, tmp); - Math::add3(tmp, skid, skid); - Math::unit3(skid, skid); - Math::cross3(skid, gup, steer); + // Correct for a rotation + float srot = Math::sin(_rot); + float crot = Math::cos(_rot); + float tx = steer[0]; + float ty = steer[1]; + steer[0] = crot*tx + srot*ty; + steer[1] = -srot*tx + crot*ty; + + tx = skid[0]; + ty = skid[1]; + skid[0] = crot*tx + srot*ty; + skid[1] = -srot*tx + crot*ty; } float vsteer = Math::dot3(cv, steer); diff --git a/src/FDM/YASim/Gear.hpp b/src/FDM/YASim/Gear.hpp index f4170ea66..dc9a4ed35 100644 --- a/src/FDM/YASim/Gear.hpp +++ b/src/FDM/YASim/Gear.hpp @@ -4,6 +4,7 @@ namespace yasim { class RigidBody; +class State; // A landing gear has the following parameters: // @@ -38,9 +39,11 @@ public: void setRotation(float rotation); void setExtension(float extension); void setCastering(bool castering); + void setGlobalGround(double* global_ground, float* global_vel); void getPosition(float* out); void getCompression(float* out); + void getGlobalGround(double* global_ground); float getSpring(); float getDamping(); float getStaticFriction(); @@ -54,7 +57,7 @@ public: // vector, and a ground plane (all specified in local coordinates) // and make a force and point of application (i.e. ground contact) // available via getForce(). - void calcForce(RigidBody* body, float* v, float* rot, float* ground); + void calcForce(RigidBody* body, State* s, float* v, float* rot); // Computed values: total force, weight-on-wheels (force normal to // ground) and compression fraction. @@ -79,6 +82,8 @@ private: float _contact[3]; float _wow; float _frac; + double _global_ground[4]; + float _global_vel[3]; }; }; // namespace yasim diff --git a/src/FDM/YASim/Makefile.am b/src/FDM/YASim/Makefile.am index dd949c08f..f4cbd4e6b 100644 --- a/src/FDM/YASim/Makefile.am +++ b/src/FDM/YASim/Makefile.am @@ -14,6 +14,9 @@ SHARED_SOURCE_FILES = \ FGFDM.cpp FGFDM.hpp \ Gear.cpp Gear.hpp \ Glue.cpp Glue.hpp \ + Ground.cpp Ground.hpp \ + Hook.cpp Hook.hpp \ + Launchbar.cpp Launchbar.hpp \ Integrator.cpp Integrator.hpp \ Jet.cpp Jet.hpp \ Math.cpp Math.hpp \ @@ -36,7 +39,7 @@ SHARED_SOURCE_FILES = \ noinst_LIBRARIES = libYASim.a -libYASim_a_SOURCES = YASim.cxx YASim.hxx $(SHARED_SOURCE_FILES) +libYASim_a_SOURCES = YASim.cxx YASim.hxx FGGround.cpp FGGround.hpp $(SHARED_SOURCE_FILES) bin_PROGRAMS = yasim noinst_PROGRAMS = proptest diff --git a/src/FDM/YASim/Model.cpp b/src/FDM/YASim/Model.cpp index 24c2940c0..643b88fe1 100644 --- a/src/FDM/YASim/Model.cpp +++ b/src/FDM/YASim/Model.cpp @@ -6,11 +6,14 @@ #include "Propeller.hpp" #include "PistonEngine.hpp" #include "Gear.hpp" +#include "Hook.hpp" +#include "Launchbar.hpp" #include "Surface.hpp" #include "Rotor.hpp" #include "Rotorpart.hpp" #include "Rotorblade.hpp" #include "Glue.hpp" +#include "Ground.hpp" #include "Model.hpp" namespace yasim { @@ -54,11 +57,17 @@ Model::Model() _agl = 0; _crashed = false; _turb = 0; + _ground_cb = new Ground(); + _hook = 0; + _launchbar = 0; } Model::~Model() { // FIXME: who owns these things? Need a policy + delete _ground_cb; + delete _hook; + delete _launchbar; } void Model::getThrust(float* out) @@ -81,8 +90,9 @@ void Model::initIteration() _gyro[i] = _torque[i] = 0; // Need a local altitude for the wind calculation - float dummy[3]; - float alt = Math::abs(localGround(_s, dummy)); + float lground[4]; + _s->planeGlobalToLocal(_global_ground, lground); + float alt = Math::abs(lground[3]); for(i=0; i<_thrusters.size(); i++) { Thruster* t = (Thruster*)_thrusters.get(i); @@ -109,6 +119,8 @@ void Model::initIteration() Math::mul3(_integrator.getInterval(), _wind, toff); _turb->offset(toff); } + + } // FIXME: This method looks to me like it's doing *integration*, not @@ -205,6 +217,16 @@ int Model::addThruster(Thruster* t) return _thrusters.add(t); } +Hook* Model::getHook(void) +{ + return _hook; +} + +Launchbar* Model::getLaunchbar(void) +{ + return _launchbar; +} + int Model::numThrusters() { return _thrusters.size(); @@ -243,6 +265,27 @@ int Model::addGear(Gear* gear) return _gears.add(gear); } +void Model::addHook(Hook* hook) +{ + _hook = hook; +} + +void Model::addLaunchbar(Launchbar* launchbar) +{ + _launchbar = launchbar; +} + +void Model::setGroundCallback(Ground* ground_cb) +{ + delete _ground_cb; + _ground_cb = ground_cb; +} + +Ground* Model::getGroundCallback(void) +{ + return _ground_cb; +} + void Model::setGroundEffect(float* pos, float span, float mul) { Math::set3(pos, _wingCenter); @@ -250,16 +293,6 @@ void Model::setGroundEffect(float* pos, float span, float mul) _groundEffect = mul; } -// The first three elements are a unit vector pointing from the global -// origin to the plane, the final element is the distance from the -// origin (the radius of the earth, in most implementations). So -// (v dot _ground)-_ground[3] gives the distance AGL. -void Model::setGroundPlane(double* planeNormal, double fromOrigin) -{ - for(int i=0; i<3; i++) _ground[i] = planeNormal[i]; - _ground[3] = fromOrigin; -} - void Model::setAir(float pressure, float temp, float density) { _pressure = pressure; @@ -272,6 +305,54 @@ void Model::setWind(float* wind) Math::set3(wind, _wind); } +void Model::updateGround(State* s) +{ + float dummy[3]; + _ground_cb->getGroundPlane(s->pos, _global_ground, dummy); + + int i; + // The landing gear + for(i=0; i<_gears.size(); i++) { + Gear* g = (Gear*)_gears.get(i); + + // Get the point of ground contact + float pos[3], cmpr[3]; + g->getPosition(pos); + g->getCompression(cmpr); + + Math::mul3(g->getCompressFraction(), cmpr, cmpr); + Math::add3(cmpr, pos, pos); + // Transform the local coordinates of the contact point to + // global coordinates. + double pt[3]; + s->posLocalToGlobal(pos, pt); + + // Ask for the ground plane in the global coordinate system + double global_ground[4]; + float global_vel[3]; + _ground_cb->getGroundPlane(pt, global_ground, global_vel); + g->setGlobalGround(global_ground, global_vel); + } + + // The arrester hook + if(_hook) { + double pt[3]; + _hook->getTipGlobalPosition(s, pt); + double global_ground[4]; + _ground_cb->getGroundPlane(pt, global_ground, dummy); + _hook->setGlobalGround(global_ground); + } + + // The launchbar/holdback + if(_launchbar) { + double pt[3]; + _launchbar->getTipGlobalPosition(s, pt); + double global_ground[4]; + _ground_cb->getGroundPlane(pt, global_ground, dummy); + _launchbar->setGlobalGround(global_ground); + } +} + void Model::calcForces(State* s) { // Add in the pre-computed stuff. These values aren't part of the @@ -295,7 +376,7 @@ void Model::calcForces(State* s) // 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); + s->planeGlobalToLocal(_global_ground, ground); float alt = Math::abs(ground[3]); // Gravity, convert to a force, then to local coordinates @@ -380,10 +461,29 @@ void Model::calcForces(State* s) for(i=0; i<_gears.size(); i++) { float force[3], contact[3]; Gear* g = (Gear*)_gears.get(i); - g->calcForce(&_body, lv, lrot, ground); + + g->calcForce(&_body, s, lv, lrot); g->getForce(force, contact); _body.addForce(contact, force); } + + // The arrester hook + if(_hook) { + float v[3], rot[3], glvel[3], ground[3]; + _hook->calcForce(_ground_cb, &_body, s, lv, lrot); + float force[3], contact[3]; + _hook->getForce(force, contact); + _body.addForce(contact, force); + } + + // The launchbar/holdback + if(_launchbar) { + float v[3], rot[3], glvel[3], ground[3]; + _launchbar->calcForce(_ground_cb, &_body, s, lv, lrot); + float force[3], contact[3]; + _launchbar->getForce(force, contact); + _body.addForce(contact, force); + } } void Model::newState(State* s) @@ -392,17 +492,22 @@ void Model::newState(State* 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 + float pos[3], cmpr[3]; g->getPosition(pos); g->getCompression(cmpr); Math::mul3(g->getCompressFraction(), cmpr, cmpr); Math::add3(cmpr, pos, pos); + + // The plane transformed to local coordinates. + double global_ground[4]; + g->getGlobalGround(global_ground); + float ground[4]; + s->planeGlobalToLocal(global_ground, ground); float dist = ground[3] - Math::dot3(pos, ground); // Find the lowest one @@ -414,28 +519,6 @@ void Model::newState(State* s) _crashed = true; } -// Returns a unit "down" vector for the ground in out, and the -// distance from the local origin to the ground as the return value. -// So for a given position V, "dist - (V dot out)" will be the height -// AGL. -float Model::localGround(State* s, float* out) -{ - // Get the ground's "down" vector, this can be in floats, because - // we don't need positioning accuracy. The direction has plenty - // of accuracy after truncation. - out[0] = -(float)_ground[0]; - out[1] = -(float)_ground[1]; - out[2] = -(float)_ground[2]; - Math::vmul33(s->orient, out, out); - - // The distance from the ground to the Aircraft's origin: - double dist = (s->pos[0]*_ground[0] - + s->pos[1]*_ground[1] - + s->pos[2]*_ground[2] - _ground[3]); - - return (float)dist; -} - // Calculates the airflow direction at the given point and for the // specified aircraft velocity. void Model::localWind(float* pos, State* s, float* out, float alt) @@ -449,8 +532,8 @@ void Model::localWind(float* pos, State* s, float* out, float alt) Math::tmul33(s->orient, pos, tmp); for(int i=0; i<3; i++) { gpos[i] = s->pos[i] + tmp[i]; - up[i] = _ground[i]; } + Glue::geodUp(gpos, up); _turb->getTurbulence(gpos, alt, up, lwind); Math::add3(_wind, lwind, lwind); } else { diff --git a/src/FDM/YASim/Model.hpp b/src/FDM/YASim/Model.hpp index a27af5c05..64f98ba26 100644 --- a/src/FDM/YASim/Model.hpp +++ b/src/FDM/YASim/Model.hpp @@ -17,6 +17,9 @@ class Rotorpart; class Rotorblade; class Rotor; class Gear; +class Ground; +class Hook; +class Launchbar; class Model : public BodyEnvironment { public: @@ -44,11 +47,15 @@ public: int addRotorblade(Rotorblade* rblade); int addRotor(Rotor* rotor); int addGear(Gear* gear); + void addHook(Hook* hook); + void addLaunchbar(Launchbar* launchbar); Surface* getSurface(int handle); Rotorpart* getRotorpart(int handle); Rotorblade* getRotorblade(int handle); Rotor* getRotor(int handle); Gear* getGear(int handle); + Hook* getHook(void); + Launchbar* getLaunchbar(void); // Semi-private methods for use by the Airplane solver. int numThrusters(); @@ -57,14 +64,18 @@ public: void initIteration(); void getThrust(float* out); + void setGroundCallback(Ground* ground_cb); + Ground* getGroundCallback(void); + // // Per-iteration settables // - void setGroundPlane(double* planeNormal, double fromOrigin); void setGroundEffect(float* pos, float span, float mul); void setWind(float* wind); void setAir(float pressure, float temp, float density); + void updateGround(State* s); + // BodyEnvironment callbacks virtual void calcForces(State* s); virtual void newState(State* s); @@ -73,7 +84,6 @@ private: void initRotorIteration(); void calcGearForce(Gear* g, float* v, float* rot, float* ground); float gearFriction(float wgt, float v, Gear* g); - float localGround(State* s, float* out); void localWind(float* pos, State* s, float* out, float alt); Integrator _integrator; @@ -87,12 +97,15 @@ private: Vector _rotorblades; Vector _rotors; Vector _gears; + Hook* _hook; + Launchbar* _launchbar; float _groundEffectSpan; float _groundEffect; float _wingCenter[3]; - double _ground[4]; + Ground* _ground_cb; + double _global_ground[4]; float _pressure; float _temp; float _rho; diff --git a/src/FDM/YASim/YASim.cxx b/src/FDM/YASim/YASim.cxx index 9f7594b2e..d6c2236b4 100644 --- a/src/FDM/YASim/YASim.cxx +++ b/src/FDM/YASim/YASim.cxx @@ -16,6 +16,9 @@ #include "Integrator.hpp" #include "Glue.hpp" #include "Gear.hpp" +#include "Hook.hpp" +#include "Launchbar.hpp" +#include "FGGround.hpp" #include "PropEngine.hpp" #include "PistonEngine.hpp" @@ -43,6 +46,7 @@ YASim::YASim(double dt) _dt = dt; + _fdm->getAirplane()->getModel()->setGroundCallback( new FGGround(this) ); _fdm->getAirplane()->getModel()->getIntegrator()->setInterval(_dt); } @@ -185,12 +189,32 @@ void YASim::update(double dt) return; } + // ground. Calculate a cartesian coordinate for the ground under + // us, find the (geodetic) up vector normal to the ground, then + // use that to find the final (radius) term of the plane equation. + float v[3] = { get_uBody()*FT2M, get_vBody()*FT2M, get_wBody()*FT2M }; + float lat = get_Latitude(); float lon = get_Longitude(); + double xyz[3]; + sgGeodToCart(lat, lon, 0.0, xyz); + // build the environment cache. + float vr = _fdm->getVehicleRadius(); + vr += 2.0*dt*Math::mag3(v); + prepare_ground_cache_m( 0.0, xyz, vr ); + + // Track time increments. + FGGround* gr + = (FGGround*)_fdm->getAirplane()->getModel()->getGroundCallback(); + int i; for(i=0; isetTimeOffset(iterations*_dt); copyToYASim(false); _fdm->iterate(_dt); copyFromYASim(); } + + // Reset the time increment. + gr->setTimeOffset(0.0); } void YASim::copyToYASim(bool copyState) @@ -209,10 +233,6 @@ void YASim::copyToYASim(bool copyState) wind[1] = get_V_east_airmass() * FT2M * -1.0; wind[2] = get_V_down_airmass() * FT2M * -1.0; - // Get ground elevation - double ground = fgGetDouble("/position/ground-elev-m"); - // cout << "YASIM: ground = " << ground << endl; - float pressure = fgGetFloat("/environment/pressure-inhg") * INHG2PA; float temp = fgGetFloat("/environment/temperature-degc") + 273.15; float dens = fgGetFloat("/environment/density-slugft3") @@ -279,19 +299,16 @@ void YASim::copyToYASim(bool copyState) Math::tmul33(xyz2ned, wind, wind); model->setWind(wind); - // ground. Calculate a cartesian coordinate for the ground under - // us, find the (geodetic) up vector normal to the ground, then - // use that to find the final (radius) term of the plane equation. - double xyz[3], gplane[3]; float up[3]; - sgGeodToCart(lat, lon, ground, xyz); - Glue::geodUp(lat, lon, up); // FIXME, needless reverse computation... - int i; - for(i=0; i<3; i++) gplane[i] = up[i]; - double rad = gplane[0]*xyz[0] + gplane[1]*xyz[1] + gplane[2]*xyz[2]; - model->setGroundPlane(gplane, rad); - // air model->setAir(pressure, temp, dens); + + // Query a ground plane for each gear/hook/launchbar and + // write that value into the corresponding class. + _fdm->getAirplane()->getModel()->updateGround(&s); + + Launchbar* l = model->getLaunchbar(); + if (l) + l->setLaunchCmd(0.0setBoolValue("wow", g->getCompressFraction() != 0); node->setFloatValue("compression-norm", g->getCompressFraction()); } + + Hook* h = airplane->getHook(); + if(h) { + SGPropertyNode * node = fgGetNode("gear/tailhook", 0, true); + node->setFloatValue("position-norm", h->getCompressFraction()); + } + + Launchbar* l = airplane->getLaunchbar(); + if(l) { + SGPropertyNode * node = fgGetNode("gear/launchbar", 0, true); + node->setFloatValue("position-norm", l->getCompressFraction()); + } } diff --git a/src/FDM/groundcache.cxx b/src/FDM/groundcache.cxx index c6f5cf869..6ade97acc 100644 --- a/src/FDM/groundcache.cxx +++ b/src/FDM/groundcache.cxx @@ -143,14 +143,16 @@ FGGroundCache::addAndFlattenLeaf(GLenum ty, ssgLeaf *l, ssgIndexArray *ia, { // Extract data from the leaf which is just copied. ssgVertexArray *va = ((ssgVtxTable *)l)->getVertices(); + ssgNormalArray *na = ((ssgVtxTable *)l)->getNormals(); // Create a new leaf. - ssgVtxArray *vtxa = new ssgVtxArray( ty, va, 0, 0, 0, ia ); + ssgVtxArray *vtxa = new ssgVtxArray( ty, va, na, 0, 0, ia ); // Clones data ... vtxa->removeUnusedVertices(); // Apply transform. We won't store transforms in our cache. vtxa->transform( xform ); // Check for magic texture names object names and such ... vtxa->setUserData( extractGroundProperty( l ) ); + vtxa->setCullFace( l->getCullFace() ); // Finally append to cache. cache_root.addKid((ssgEntity*)vtxa); } @@ -528,10 +530,18 @@ FGGroundCache::get_agl(double t, const double dpt[3], sgVec3 isecpoint; if ( sgIsectInfLinePlane( isecpoint, pt, dir, plane ) && sgPointInTriangle3( isecpoint, tri ) ) { + // Only accept surfaces with the normal pointing upwards. + // For double sided surfaces flip the normal in this case. + float dirDot = sgScalarProductVec3(plane, dir); + if ( dirDot >= 0 && va->getCullFace() == 1 ) { + sgScaleVec4( plane, -1 ); + dirDot = -dirDot; + } + // Check for the closest intersection point. // FIXME: is this the right one? double newSqdist = sgDistanceSquaredVec3( isecpoint, pt ); - if ( newSqdist < sqdist ) { + if ( newSqdist < sqdist && dirDot < 0 ) { sqdist = newSqdist; ret = true; // Save the new potential intersection point.