diff --git a/src/FDM/YASim/Airplane.cpp b/src/FDM/YASim/Airplane.cpp index 0385bfaf2..c6b5ae925 100644 --- a/src/FDM/YASim/Airplane.cpp +++ b/src/FDM/YASim/Airplane.cpp @@ -7,6 +7,7 @@ #include "Surface.hpp" #include "Rotorpart.hpp" #include "Thruster.hpp" +#include "Hitch.hpp" #include "Airplane.hpp" namespace yasim { @@ -38,11 +39,13 @@ Airplane::Airplane() _cruiseT = 0; _cruiseSpeed = 0; _cruiseWeight = 0; + _cruiseGlideAngle = 0; _approachP = 0; _approachT = 0; _approachSpeed = 0; _approachAoA = 0; _approachWeight = 0; + _approachGlideAngle = 0; _dragFactor = 1; _liftRatio = 1; @@ -182,16 +185,17 @@ void Airplane::updateGearState() } } -void Airplane::setApproach(float speed, float altitude, float aoa, float fuel) +void Airplane::setApproach(float speed, float altitude, float aoa, float fuel, float gla) { _approachSpeed = speed; _approachP = Atmosphere::getStdPressure(altitude); _approachT = Atmosphere::getStdTemperature(altitude); _approachAoA = aoa; _approachFuel = fuel; + _approachGlideAngle = gla; } -void Airplane::setCruise(float speed, float altitude, float fuel) +void Airplane::setCruise(float speed, float altitude, float fuel, float gla) { _cruiseSpeed = speed; _cruiseP = Atmosphere::getStdPressure(altitude); @@ -199,6 +203,7 @@ void Airplane::setCruise(float speed, float altitude, float fuel) _cruiseAoA = 0; _tailIncidence = 0; _cruiseFuel = fuel; + _cruiseGlideAngle = gla; } void Airplane::setElevatorControl(int control) @@ -323,6 +328,11 @@ void Airplane::addHook(Hook* hook) _model.addHook(hook); } +void Airplane::addHitch(Hitch* hitch) +{ + _model.addHitch(hitch); +} + void Airplane::addLaunchbar(Launchbar* launchbar) { _model.addLaunchbar(launchbar); @@ -417,7 +427,7 @@ int Airplane::getSolutionIterations() return _solutionIterations; } -void Airplane::setupState(float aoa, float speed, State* s) +void Airplane::setupState(float aoa, float speed, float gla, State* s) { float cosAoA = Math::cos(aoa); float sinAoA = Math::sin(aoa); @@ -425,7 +435,7 @@ void Airplane::setupState(float aoa, float speed, State* s) s->orient[3] = 0; s->orient[4] = 1; s->orient[5] = 0; s->orient[6] = -sinAoA; s->orient[7] = 0; s->orient[8] = cosAoA; - s->v[0] = speed; s->v[1] = 0; s->v[2] = 0; + s->v[0] = speed*Math::cos(gla); s->v[1] = -speed*Math::sin(gla); s->v[2] = 0; int i; for(i=0; i<3; i++) @@ -602,6 +612,7 @@ void Airplane::compileContactPoints() // I made these up g->setStaticFriction(0.6f); g->setDynamicFriction(0.5f); + g->setContactPoint(1); _model.addGear(g); } @@ -709,7 +720,8 @@ void Airplane::solveGear() g->getPosition(pos); Math::sub3(cg, pos, pos); gr->wgt = 1.0f/(0.5f+Math::sqrt(pos[0]*pos[0] + pos[1]*pos[1])); - total += gr->wgt; + if (!g->getIgnoreWhileSolving()) + total += gr->wgt; } // Renormalize so they sum to 1 @@ -731,7 +743,7 @@ void Airplane::solveGear() float e = energy * gr->wgt; float comp[3]; gr->gear->getCompression(comp); - float len = Math::mag3(comp); + float len = Math::mag3(comp)*(1+2*gr->gear->getInitialLoad()); // Energy in a spring: e = 0.5 * k * len^2 float k = 2 * e / (len*len); @@ -773,7 +785,7 @@ void Airplane::setupWeights(bool isApproach) void Airplane::runCruise() { - setupState(_cruiseAoA, _cruiseSpeed, &_cruiseState); + setupState(_cruiseAoA, _cruiseSpeed,_approachGlideAngle, &_cruiseState); _model.setState(&_cruiseState); _model.setAir(_cruiseP, _cruiseT, Atmosphere::calcStdDensity(_cruiseP, _cruiseT)); @@ -816,7 +828,7 @@ void Airplane::runCruise() void Airplane::runApproach() { - setupState(_approachAoA, _approachSpeed, &_approachState); + setupState(_approachAoA, _approachSpeed,_approachGlideAngle, &_approachState); _model.setState(&_approachState); _model.setAir(_approachP, _approachT, Atmosphere::calcStdDensity(_approachP, _approachT)); @@ -924,7 +936,7 @@ void Airplane::solve() runCruise(); _model.getThrust(tmp); - float thrust = tmp[0]; + float thrust = tmp[0] + _cruiseWeight * Math::sin(_cruiseGlideAngle) * 9.81; _model.getBody()->getAccel(tmp); Math::tmul33(_cruiseState.orient, tmp, tmp); @@ -1063,7 +1075,7 @@ void Airplane::solveHelicopter() applyDragFactor(Math::pow(15.7/1000, 1/SOLVE_TWEAK)); applyLiftRatio(Math::pow(104, 1/SOLVE_TWEAK)); } - setupState(0,0, &_cruiseState); + setupState(0,0,0, &_cruiseState); _model.setState(&_cruiseState); setupWeights(true); _controls.reset(); diff --git a/src/FDM/YASim/Airplane.hpp b/src/FDM/YASim/Airplane.hpp index 7ff4970bc..e0e53f4e5 100644 --- a/src/FDM/YASim/Airplane.hpp +++ b/src/FDM/YASim/Airplane.hpp @@ -13,6 +13,7 @@ class Gear; class Hook; class Launchbar; class Thruster; +class Hitch; class Airplane { public: @@ -45,12 +46,13 @@ public: void addLaunchbar(Launchbar* l); void addThruster(Thruster* t, float mass, float* cg); void addBallast(float* pos, float mass); + void addHitch(Hitch* h); int addWeight(float* pos, float size); void setWeight(int handle, float mass); - void setApproach(float speed, float altitude, float aoa, float fuel); - void setCruise(float speed, float altitude, float fuel); + void setApproach(float speed, float altitude, float aoa, float fuel, float gla); + void setCruise(float speed, float altitude, float fuel, float gla); void setElevatorControl(int control); void addApproachControl(int control, float val); @@ -61,6 +63,8 @@ public: int numGear(); Gear* getGear(int g); Hook* getHook(); + int numHitches() { return _hitches.size(); } + Hitch* getHitch(int h); Rotorgear* getRotorgear(); Launchbar* getLaunchbar(); @@ -88,7 +92,7 @@ public: float getApproachElevator() { return _approachElevator.val; } char* getFailureMsg(); - static void setupState(float aoa, float speed, State* s); // utility + static void setupState(float aoa, float speed, float gla, State* s); // utility private: struct Tank { float pos[3]; float cap; float fill; @@ -139,6 +143,7 @@ private: Vector _contacts; // non-gear ground contact points Vector _weights; Vector _surfs; // NON-wing Surfaces + Vector _hitches; //for airtow and winch Vector _solveWeights; @@ -149,6 +154,7 @@ private: float _cruiseSpeed; float _cruiseWeight; float _cruiseFuel; + float _cruiseGlideAngle; Vector _approachControls; State _approachState; @@ -158,6 +164,7 @@ private: float _approachAoA; float _approachWeight; float _approachFuel; + float _approachGlideAngle; int _solutionIterations; float _dragFactor; diff --git a/src/FDM/YASim/ControlMap.cpp b/src/FDM/YASim/ControlMap.cpp index 798000763..1b9de6834 100644 --- a/src/FDM/YASim/ControlMap.cpp +++ b/src/FDM/YASim/ControlMap.cpp @@ -10,6 +10,7 @@ #include "Rotor.hpp" #include "Math.hpp" #include "Propeller.hpp" +#include "Hitch.hpp" #include "ControlMap.hpp" namespace yasim { @@ -219,6 +220,10 @@ void ControlMap::applyControls(float dt) case WASTEGATE: ((PistonEngine*)((Thruster*)obj)->getEngine())->setWastegate(lval); break; + case WINCHRELSPEED: ((Hitch*)obj)->setWinchRelSpeed(lval); break; + case HITCHOPEN: ((Hitch*)obj)->setOpen(lval!=0); break; + case PLACEWINCH: ((Hitch*)obj)->setWinchPositionAuto(lval!=0); break; + case FINDAITOW: ((Hitch*)obj)->findBestAIObject(lval!=0); break; } } } @@ -233,6 +238,7 @@ float ControlMap::rangeMin(int type) case CYCLICELE: return -1; case CYCLICAIL: return -1; case COLLECTIVE: return -1; + case WINCHRELSPEED: return -1; case MAGNETOS: return 0; // [0:3] default: return 0; // [0:1] } diff --git a/src/FDM/YASim/ControlMap.hpp b/src/FDM/YASim/ControlMap.hpp index a48768f08..aadef7a21 100644 --- a/src/FDM/YASim/ControlMap.hpp +++ b/src/FDM/YASim/ControlMap.hpp @@ -16,7 +16,8 @@ public: BOOST, CASTERING, PROPPITCH, PROPFEATHER, COLLECTIVE, CYCLICAIL, CYCLICELE, ROTORENGINEON, ROTORBRAKE, - REVERSE_THRUST, WASTEGATE }; + REVERSE_THRUST, WASTEGATE, + WINCHRELSPEED, HITCHOPEN, PLACEWINCH, FINDAITOW}; enum { OPT_SPLIT = 0x01, OPT_INVERT = 0x02, diff --git a/src/FDM/YASim/FGFDM.cpp b/src/FDM/YASim/FGFDM.cpp index 625653df3..0544898fe 100644 --- a/src/FDM/YASim/FGFDM.cpp +++ b/src/FDM/YASim/FGFDM.cpp @@ -16,6 +16,7 @@ #include "TurbineEngine.hpp" #include "Rotor.hpp" #include "Rotorpart.hpp" +#include "Hitch.hpp" #include "FGFDM.hpp" @@ -158,12 +159,14 @@ void FGFDM::startElement(const char* name, const XMLAttributes &atts) float spd = attrf(a, "speed") * KTS2MPS; float alt = attrf(a, "alt", 0) * FT2M; float aoa = attrf(a, "aoa", 0) * DEG2RAD; - _airplane.setApproach(spd, alt, aoa, attrf(a, "fuel", 0.2)); + float gla = attrf(a, "glide-angle", 0) * DEG2RAD; + _airplane.setApproach(spd, alt, aoa, attrf(a, "fuel", 0.2),gla); _cruiseCurr = false; } else if(eq(name, "cruise")) { float spd = attrf(a, "speed") * KTS2MPS; float alt = attrf(a, "alt") * FT2M; - _airplane.setCruise(spd, alt, attrf(a, "fuel", 0.5)); + float gla = attrf(a, "glide-angle", 0) * DEG2RAD; + _airplane.setCruise(spd, alt, attrf(a, "fuel", 0.5),gla); _cruiseCurr = true; } else if(eq(name, "solve-weight")) { int idx = attri(a, "idx"); @@ -245,6 +248,46 @@ void FGFDM::startElement(const char* name, const XMLAttributes &atts) er->eng = j; er->prefix = dup(buf); _thrusters.add(er); + } else if(eq(name, "hitch")) { + Hitch* h = new Hitch(a->getValue("name")); + _currObj = h; + v[0] = attrf(a, "x"); + v[1] = attrf(a, "y"); + v[2] = attrf(a, "z"); + h->setPosition(v); + if(a->hasAttribute("force-is-calculated-by-other")) h->setForceIsCalculatedByOther(attrb(a,"force-is-calculated-by-other")); + _airplane.addHitch(h); + } else if(eq(name, "tow")) { + Hitch* h = (Hitch*)_currObj; + if(a->hasAttribute("length")) + h->setTowLength(attrf(a, "length")); + if(a->hasAttribute("elastic-constant")) + h->setTowElasticConstant(attrf(a, "elastic-constant")); + if(a->hasAttribute("break-force")) + h->setTowBreakForce(attrf(a, "break-force")); + if(a->hasAttribute("weight-per-meter")) + h->setTowWeightPerM(attrf(a, "weight-per-meter")); + if(a->hasAttribute("mp-auto-connect-period")) + h->setMpAutoConnectPeriod(attrf(a, "mp-auto-connect-period")); + } else if(eq(name, "winch")) { + Hitch* h = (Hitch*)_currObj; + double pos[3]; + pos[0] = attrd(a, "x",0); + pos[1] = attrd(a, "y",0); + pos[2] = attrd(a, "z",0); + h->setWinchPosition(pos); + if(a->hasAttribute("max-speed")) + h->setWinchMaxSpeed(attrf(a, "max-speed")); + if(a->hasAttribute("power")) + h->setWinchPower(attrf(a, "power") * 1000); + if(a->hasAttribute("max-force")) + h->setWinchMaxForce(attrf(a, "max-force")); + if(a->hasAttribute("initial-tow-length")) + h->setWinchInitialTowLength(attrf(a, "initial-tow-length")); + if(a->hasAttribute("max-tow-length")) + h->setWinchMaxTowLength(attrf(a, "max-tow-length")); + if(a->hasAttribute("min-tow-length")) + h->setWinchMinTowLength(attrf(a, "min-tow-length")); } else if(eq(name, "gear")) { Gear* g = new Gear(); _currObj = g; @@ -269,10 +312,17 @@ void FGFDM::startElement(const char* name, const XMLAttributes &atts) v[i] *= attrf(a, "compression", 1); g->setCompression(v); g->setBrake(attrf(a, "skid", 0)); + g->setInitialLoad(attrf(a, "initial-load", 0)); g->setStaticFriction(attrf(a, "sfric", 0.8)); g->setDynamicFriction(attrf(a, "dfric", 0.7)); g->setSpring(attrf(a, "spring", 1)); g->setDamping(attrf(a, "damp", 1)); + if(a->hasAttribute("on-water")) g->setOnWater(attrb(a,"on-water")); + if(a->hasAttribute("on-solid")) g->setOnSolid(attrb(a,"on-solid")); + if(a->hasAttribute("ignored-by-solver")) g->setIgnoreWhileSolving(attrb(a,"ignored-by-solver")); + g->setSpringFactorNotPlaning(attrf(a, "spring-factor-not-planing", 1)); + g->setSpeedPlaning(attrf(a, "speed-planing", 0) * KTS2MPS); + g->setReduceFrictionByExtension(attrf(a, "reduce-friction-by-extension", 0)); _airplane.addGear(g); } else if(eq(name, "hook")) { Hook* h = new Hook(); @@ -898,6 +948,11 @@ int FGFDM::parseOutput(const char* name) if(eq(name, "ROTORBRAKE")) return ControlMap::ROTORBRAKE; if(eq(name, "REVERSE_THRUST")) return ControlMap::REVERSE_THRUST; if(eq(name, "WASTEGATE")) return ControlMap::WASTEGATE; + if(eq(name, "WINCHRELSPEED")) return ControlMap::WINCHRELSPEED; + if(eq(name, "HITCHOPEN")) return ControlMap::HITCHOPEN; + if(eq(name, "PLACEWINCH")) return ControlMap::PLACEWINCH; + if(eq(name, "FINDAITOW")) return ControlMap::FINDAITOW; + SG_LOG(SG_FLIGHT,SG_ALERT,"Unrecognized control type '" << name << "' in YASim aircraft description."); exit(1); @@ -972,6 +1027,23 @@ float FGFDM::attrf(XMLAttributes* atts, char* attr, float def) else return (float)atof(val); } +double FGFDM::attrd(XMLAttributes* atts, char* attr) +{ + if(!atts->hasAttribute(attr)) { + SG_LOG(SG_FLIGHT,SG_ALERT,"Missing '" << attr << + "' in YASim aircraft description"); + exit(1); + } + return attrd(atts, attr, 0); +} + +double FGFDM::attrd(XMLAttributes* atts, char* attr, double def) +{ + const char* val = atts->getValue(attr); + if(val == 0) return def; + else return atof(val); +} + // ACK: the dreaded ambiguous string boolean. Remind me to shoot Maik // when I have a chance. :). Unless you have a parser that can check // symbol constants (we don't), this kind of coding is just a Bad diff --git a/src/FDM/YASim/FGFDM.hpp b/src/FDM/YASim/FGFDM.hpp index 12fc1b607..1df2c4872 100644 --- a/src/FDM/YASim/FGFDM.hpp +++ b/src/FDM/YASim/FGFDM.hpp @@ -53,6 +53,8 @@ private: int attri(XMLAttributes* atts, char* attr, int def); float attrf(XMLAttributes* atts, char* attr); float attrf(XMLAttributes* atts, char* attr, float def); + double attrd(XMLAttributes* atts, char* attr); + double attrd(XMLAttributes* atts, char* attr, double def); bool attrb(XMLAttributes* atts, char* attr); // The core Airplane object we manage. diff --git a/src/FDM/YASim/FGGround.cpp b/src/FDM/YASim/FGGround.cpp index e6af48cc4..d39235cf1 100644 --- a/src/FDM/YASim/FGGround.cpp +++ b/src/FDM/YASim/FGGround.cpp @@ -35,6 +35,23 @@ void FGGround::getGroundPlane(const double pos[3], for(int i=0; i<3; i++) vel[i] = dvel[i]; } +void FGGround::getGroundPlane(const double pos[3], + double plane[4], float vel[3], + int *type, const SGMaterial **material + ) +{ + // Return values for the callback. + double agl; + double cp[3], dvel[3]; + _iface->get_agl_m(_toff, pos, cp, plane, dvel, + type, material, &agl); + + // The plane below the actual contact point. + plane[3] = plane[0]*cp[0] + plane[1]*cp[1] + plane[2]*cp[2]; + + for(int i=0; i<3; i++) vel[i] = dvel[i]; +} + bool FGGround::caughtWire(const double pos[4][3]) { return _iface->caught_wire_m(_toff, pos); diff --git a/src/FDM/YASim/FGGround.hpp b/src/FDM/YASim/FGGround.hpp index 3567c0217..87f125aff 100644 --- a/src/FDM/YASim/FGGround.hpp +++ b/src/FDM/YASim/FGGround.hpp @@ -4,6 +4,7 @@ #include "Ground.hpp" class FGInterface; +class SGMaterial; namespace yasim { @@ -18,6 +19,16 @@ public: virtual void getGroundPlane(const double pos[3], double plane[4], float vel[3]); + virtual void getGroundPlane(const double pos[3], + double plane[4], float vel[3], + int *type, const SGMaterial **material);/* + double *frictionFactor, + double *rollingFriction, + double *loadCapacity, + double *loadResistance, + double *bumpiness, + bool *isSolid);*/ + virtual bool caughtWire(const double pos[4][3]); virtual bool getWire(double end[2][3], float vel[2][3]); diff --git a/src/FDM/YASim/Gear.cpp b/src/FDM/YASim/Gear.cpp index 1936c3594..3d34a7ed0 100644 --- a/src/FDM/YASim/Gear.cpp +++ b/src/FDM/YASim/Gear.cpp @@ -2,8 +2,13 @@ #include "BodyEnvironment.hpp" #include "RigidBody.hpp" +#include +#include #include "Gear.hpp" namespace yasim { +static const float YASIM_PI = 3.14159265358979323846; +static const float maxGroundBumpAmplitude=0.4; + //Amplitude can be positive and negative Gear::Gear() { @@ -16,9 +21,26 @@ Gear::Gear() _dfric = 0.7f; _brake = 0; _rot = 0; + _initialLoad = 0; _extension = 1; _castering = false; _frac = 0; + _ground_type = 0; + _ground_frictionFactor = 1; + _ground_rollingFriction = 0.02; + _ground_loadCapacity = 1e30; + _ground_loadResistance = 1e30; + _ground_isSolid = 1; + _ground_bumpiness = 0; + _onWater = 0; + _onSolid = 1; + _global_x = 0.0; + _global_y = 0.0; + _reduceFrictionByExtension = 0; + _spring_factor_not_planing = 1; + _speed_planing = 0; + _isContactPoint = 0; + _ignoreWhileSolving = 0; for(i=0; i<3; i++) _global_ground[i] = _global_vel[i] = 0; @@ -78,12 +100,99 @@ void Gear::setCastering(bool c) _castering = c; } -void Gear::setGlobalGround(double *global_ground, float* global_vel) +void Gear::setContactPoint(bool c) +{ + _isContactPoint=c; +} + +void Gear::setOnWater(bool c) +{ + _onWater = c; +} + +void Gear::setOnSolid(bool c) +{ + _onSolid = c; +} + +void Gear::setIgnoreWhileSolving(bool c) +{ + _ignoreWhileSolving = c; +} + +void Gear::setSpringFactorNotPlaning(float f) +{ + _spring_factor_not_planing = f; +} + +void Gear::setSpeedPlaning(float s) +{ + _speed_planing = s; +} + +void Gear::setReduceFrictionByExtension(float s) +{ + _reduceFrictionByExtension = s; +} + +void Gear::setInitialLoad(float l) +{ + _initialLoad = l; +} + +void Gear::setGlobalGround(double *global_ground, float* global_vel, + double globalX, double globalY, + int type, const SGMaterial *material) { int i; + double frictionFactor,rollingFriction,loadCapacity,loadResistance,bumpiness; + bool isSolid; + for(i=0; i<4; i++) _global_ground[i] = global_ground[i]; for(i=0; i<3; i++) _global_vel[i] = global_vel[i]; -} + + if (material) { + loadCapacity = (*material).get_load_resistence(); + frictionFactor =(*material).get_friction_factor(); + rollingFriction = (*material).get_rolling_friction(); + loadResistance = (*material).get_load_resistence(); + bumpiness = (*material).get_bumpiness(); + isSolid = (*material).get_solid(); + } else { + if (type == FGInterface::Solid) { + loadCapacity = DBL_MAX; + frictionFactor = 1.0; + rollingFriction = 0.02; + loadResistance = DBL_MAX; + bumpiness = 0.0; + isSolid = true; + } else if (type == FGInterface::Water) { + loadCapacity = DBL_MAX; + frictionFactor = 1.0; + rollingFriction = 2; + loadResistance = DBL_MAX; + bumpiness = 0.8; + isSolid = false; + } else { + loadCapacity = DBL_MAX; + frictionFactor = 0.9; + rollingFriction = 0.1; + loadResistance = DBL_MAX; + bumpiness = 0.2; + isSolid = true; + } + } + _ground_type = type; + _ground_frictionFactor = frictionFactor; + _ground_rollingFriction = rollingFriction; + _ground_loadCapacity = loadCapacity; + _ground_loadResistance = loadResistance; + _ground_bumpiness = bumpiness; + _ground_isSolid = isSolid; + _global_x = globalX; + _global_y = globalY; + + } void Gear::getPosition(float* out) { @@ -159,6 +268,31 @@ bool Gear::getCastering() return _castering; } +bool Gear::getGroundIsSolid() +{ + return _ground_isSolid; +} + +float Gear::getBumpAltitude() +{ + if (_ground_bumpiness<0.001) return 0.0; + double x = _global_x*0.1; + double y = _global_y*0.1; + x -= Math::floor(x); + y -= Math::floor(y); + x *= 2*YASIM_PI; + y *= 2*YASIM_PI; + //now x and y are in the range of 0..2pi + //we need a function, that is periodically on 2pi and gives some + //height. This is not very fast, but for a beginning. + //maybe this should be done by interpolating between some precalculated + //values + float h = Math::sin(x)+Math::sin(7*x)+Math::sin(8*x)+Math::sin(13*x); + h += Math::sin(2*y)+Math::sin(5*y)+Math::sin(9*y*x)+Math::sin(17*y); + + return h*(1/8.)*_ground_bumpiness*maxGroundBumpAmplitude; +} + void Gear::calcForce(RigidBody* body, State *s, float* v, float* rot) { // Init the return values @@ -169,6 +303,16 @@ void Gear::calcForce(RigidBody* body, State *s, float* v, float* rot) if(_extension < 1) return; + // Dont bother if we are in the "wrong" ground + if (!((_onWater&&!_ground_isSolid)||(_onSolid&&_ground_isSolid))) { + _wow = 0; + _frac = 0; + _compressDist = 0; + _rollSpeed = 0; + _casterAngle = 0; + return; + } + // The ground plane transformed to the local frame. float ground[4]; s->planeGlobalToLocal(_global_ground, ground); @@ -180,6 +324,12 @@ void Gear::calcForce(RigidBody* body, State *s, float* v, float* rot) // First off, make sure that the gear "tip" is below the ground. // If it's not, there's no force. float a = ground[3] - Math::dot3(_pos, ground); + float BumpAltitude=0; + if (a 0) { _wow = 0; @@ -196,7 +346,7 @@ void Gear::calcForce(RigidBody* body, State *s, float* v, float* rot) // above ground is negative. float tmp[3]; Math::add3(_cmpr, _pos, tmp); - float b = ground[3] - Math::dot3(tmp, ground); + float b = ground[3] - Math::dot3(tmp, ground)+BumpAltitude; // Calculate the point of ground _contact. _frac = a/(a-b); @@ -218,8 +368,25 @@ void Gear::calcForce(RigidBody* body, State *s, float* v, float* rot) // Finally, we can start adding up the forces. First the spring // compression. (note the clamping of _frac to 1): _frac = (_frac > 1) ? 1 : _frac; - float fmag = _frac*clen*_spring; + // Add the initial load to frac, but with continous transistion around 0 + float frac_with_initial_load; + if (_frac>0.2 || _initialLoad==0.0) + frac_with_initial_load = _frac+_initialLoad; + else + frac_with_initial_load = (_frac+_initialLoad) + *_frac*_frac*3*25-_frac*_frac*_frac*2*125; + + float fmag = frac_with_initial_load*clen*_spring; + if (_speed_planing>0) + { + float v = Math::mag3(cv); + if (v < _speed_planing) + { + float frac = v/_speed_planing; + fmag = fmag*_spring_factor_not_planing*(1-frac)+fmag*frac; + } + } // Then the damping. Use the only the velocity into the ground // (projection along "ground") projected along the compression // axis. So Vdamp = ground*(ground dot cv) dot cmpr @@ -284,11 +451,28 @@ void Gear::calcForce(RigidBody* body, State *s, float* v, float* rot) _rollSpeed = vsteer; _casterAngle = _rot; } - - float fsteer = _brake * calcFriction(wgt, vsteer); - float fskid = calcFriction(wgt, vskid); + float fsteer,fskid; + if(_ground_isSolid) + { + fsteer = (_brake * _ground_frictionFactor + +(1-_brake)*_ground_rollingFriction + )*calcFriction(wgt, vsteer); + fskid = calcFriction(wgt, vskid)*(_ground_frictionFactor); + } + else + { + fsteer = calcFrictionFluid(wgt, vsteer)*_ground_frictionFactor; + fskid = 10*calcFrictionFluid(wgt, vskid)*_ground_frictionFactor; + //factor 10: floats have different drag in x and y. + } if(vsteer > 0) fsteer = -fsteer; if(vskid > 0) fskid = -fskid; + + //reduce friction if wanted by _reduceFrictionByExtension + float factor = (1-_frac)*(1-_reduceFrictionByExtension)+_frac*1; + factor = Math::clamp(factor,0,1); + fsteer *= factor; + fskid *= factor; // Phoo! All done. Add it up and get out of here. Math::mul3(fsteer, steer, tmp); @@ -298,7 +482,7 @@ void Gear::calcForce(RigidBody* body, State *s, float* v, float* rot) Math::add3(tmp, _force, _force); } -float Gear::calcFriction(float wgt, float v) +float Gear::calcFriction(float wgt, float v) //used on solid ground { // How slow is stopped? 10 cm/second? const float STOP = 0.1f; @@ -308,5 +492,15 @@ float Gear::calcFriction(float wgt, float v) else return wgt * _dfric; } +float Gear::calcFrictionFluid(float wgt, float v) //used on fluid ground +{ + // How slow is stopped? 1 cm/second? + const float STOP = 0.01f; + const float iSTOP = 1.0f/STOP; + v = Math::abs(v); + if(v < STOP) return v*iSTOP * wgt * _sfric; + else return wgt * _dfric*v*v*0.01; + //*0.01: to get _dfric of the same size than _dfric on solid +} }; // namespace yasim diff --git a/src/FDM/YASim/Gear.hpp b/src/FDM/YASim/Gear.hpp index 435ca4973..ad08b6ea9 100644 --- a/src/FDM/YASim/Gear.hpp +++ b/src/FDM/YASim/Gear.hpp @@ -1,6 +1,8 @@ #ifndef _GEAR_HPP #define _GEAR_HPP +class SGMaterial; + namespace yasim { class RigidBody; @@ -39,8 +41,16 @@ public: void setRotation(float rotation); void setExtension(float extension); void setCastering(bool castering); - void setGlobalGround(double* global_ground, float* global_vel); - + void setOnWater(bool c); + void setOnSolid(bool c); + void setSpringFactorNotPlaning(float f); + void setSpeedPlaning(float s); + void setReduceFrictionByExtension(float s); + void setInitialLoad(float l); + void setIgnoreWhileSolving(bool c); + void setGlobalGround(double* global_ground, float* global_vel, + double globalX, double globalY, + int type, const SGMaterial *material); void getPosition(float* out); void getCompression(float* out); void getGlobalGround(double* global_ground); @@ -51,9 +61,12 @@ public: float getBrake(); float getRotation(); float getExtension(); + float getInitialLoad() {return _initialLoad; } bool getCastering(); float getCasterAngle() { return _casterAngle; } float getRollSpeed() { return _rollSpeed; } + float getBumpAltitude(); + bool getGroundIsSolid(); // Takes a velocity of the aircraft relative to ground, a rotation // vector, and a ground plane (all specified in local coordinates) @@ -67,9 +80,13 @@ public: float getWoW(); float getCompressFraction(); float getCompressDist() { return _compressDist; } + bool getSubmergable() {return (!_ground_isSolid)&&(!_isContactPoint); } + bool getIgnoreWhileSolving() {return _ignoreWhileSolving; } + void setContactPoint(bool c); private: float calcFriction(float wgt, float v); + float calcFrictionFluid(float wgt, float v); bool _castering; float _pos[3]; @@ -85,11 +102,29 @@ private: float _contact[3]; float _wow; float _frac; + float _initialLoad; float _compressDist; double _global_ground[4]; float _global_vel[3]; float _casterAngle; float _rollSpeed; + bool _isContactPoint; + bool _onWater; + bool _onSolid; + float _spring_factor_not_planing; + float _speed_planing; + float _reduceFrictionByExtension; + bool _ignoreWhileSolving; + + int _ground_type; + double _ground_frictionFactor; + double _ground_rollingFriction; + double _ground_loadCapacity; + double _ground_loadResistance; + double _ground_bumpiness; + bool _ground_isSolid; + double _global_x; + double _global_y; }; }; // namespace yasim diff --git a/src/FDM/YASim/Ground.cpp b/src/FDM/YASim/Ground.cpp index 3fc299d7f..5cafd00fe 100644 --- a/src/FDM/YASim/Ground.cpp +++ b/src/FDM/YASim/Ground.cpp @@ -1,5 +1,6 @@ #include "Glue.hpp" +#include #include "Ground.hpp" namespace yasim { @@ -28,6 +29,13 @@ void Ground::getGroundPlane(const double pos[3], vel[2] = 0.0; } +void Ground::getGroundPlane(const double pos[3], + double plane[4], float vel[3], + int *type, const SGMaterial **material) +{ + getGroundPlane(pos,plane,vel); +} + bool Ground::caughtWire(const double pos[4][3]) { return false; diff --git a/src/FDM/YASim/Ground.hpp b/src/FDM/YASim/Ground.hpp index eec8f997d..4fbcf7400 100644 --- a/src/FDM/YASim/Ground.hpp +++ b/src/FDM/YASim/Ground.hpp @@ -1,6 +1,7 @@ #ifndef _GROUND_HPP #define _GROUND_HPP +class SGMaterial; namespace yasim { class Ground { @@ -11,6 +12,10 @@ public: virtual void getGroundPlane(const double pos[3], double plane[4], float vel[3]); + virtual void getGroundPlane(const double pos[3], + double plane[4], float vel[3], + int *type, const SGMaterial **material); + virtual bool caughtWire(const double pos[4][3]); virtual bool getWire(double end[2][3], float vel[2][3]); diff --git a/src/FDM/YASim/Hitch.cpp b/src/FDM/YASim/Hitch.cpp new file mode 100755 index 000000000..72d66cf26 --- /dev/null +++ b/src/FDM/YASim/Hitch.cpp @@ -0,0 +1,778 @@ +#include "Math.hpp" +#include "BodyEnvironment.hpp" +#include "RigidBody.hpp" +#include
+#include + + + +#include "Hitch.hpp" +namespace yasim { +Hitch::Hitch(const char *name) +{ + int i; + strncpy(_name,name,128); + _name[127]=0; + for(i=0; i<3; i++) + _pos[i] = _force[i] = _winchPos[i] = _mp_lpos[i]=_towEndForce[i]=_mp_force[i]=0; + for(i=0; i<2; i++) + _global_ground[i] = 0; + _global_ground[2] = 1; + _global_ground[3] = -1e5; + _forceMagnitude=0; + _open=true; + _oldOpen=_open; + _towLength=60; + _towElasticConstant=1e5; + _towBrakeForce=100000; + _towWeightPerM=1; + _winchMaxSpeed=40; + _winchRelSpeed=0; + _winchInitialTowLength=1000; + _winchPower=100000; + _winchMaxForce=10000; + _winchActualForce=0; + _winchMaxTowLength=1000; + _winchMinTowLength=0; + _dist=0; + _towEndIsConnectedToProperty=false; + _towEndNode=0; + _nodeIsMultiplayer=false; + _nodeIsAiAircraft=false; + _forceIsCalculatedByMaster=false; + _nodeID=0; + //_ai_MP_callsign=0; + _height_above_ground=0; + _winch_height_above_ground=0; + _loPosFrac=0; + _lowest_tow_height=0; + _state=new State; + _displayed_len_lower_dist_message=false; + _last_wish=true; + _isSlave=false; + _mpAutoConnectPeriod=0; + _timeToNextAutoConnectTry=0; + _timeToNextReConnectTry=10; + _speed_in_tow_direction=0; + _mp_time_lag=1; + _mp_last_reported_dist=0; + _mp_last_reported_v=0; + _mp_is_slave=false; + _mp_open_last_state=false; + _timeLagCorrectedDist=0; + + //tie the properties + char text[128]; + sprintf(text,"/sim/hitches/%s", _name); + SGPropertyNode * node = fgGetNode(text, true); + node->tie("tow/length",SGRawValuePointer(&_towLength)); + node->tie("tow/elastic-constant",SGRawValuePointer(&_towElasticConstant)); + node->tie("tow/weight-per-m-kg-m",SGRawValuePointer(&_towWeightPerM)); + node->tie("tow/brake-force",SGRawValuePointer(&_towBrakeForce)); + node->tie("winch/max-speed-m-s",SGRawValuePointer(&_winchMaxSpeed)); + node->tie("winch/rel-speed",SGRawValuePointer(&_winchRelSpeed)); + node->tie("winch/initial-tow-length-m",SGRawValuePointer(&_winchInitialTowLength)); + node->tie("winch/min-tow-length-m",SGRawValuePointer(&_winchMinTowLength)); + node->tie("winch/max-tow-length-m",SGRawValuePointer(&_winchMaxTowLength)); + node->tie("winch/global-pos-x",SGRawValuePointer(&_winchPos[0])); + node->tie("winch/global-pos-y",SGRawValuePointer(&_winchPos[1])); + node->tie("winch/global-pos-z",SGRawValuePointer(&_winchPos[2])); + node->tie("winch/max-power",SGRawValuePointer(&_winchPower)); + node->tie("winch/max-force",SGRawValuePointer(&_winchMaxForce)); + node->tie("winch/actual-force",SGRawValuePointer(&_winchActualForce)); + node->tie("tow/end-force-x",SGRawValuePointer(&_reportTowEndForce[0])); + node->tie("tow/end-force-y",SGRawValuePointer(&_reportTowEndForce[1])); + node->tie("tow/end-force-z",SGRawValuePointer(&_reportTowEndForce[2])); + node->tie("force",SGRawValuePointer(&_forceMagnitude)); + node->tie("open",SGRawValuePointer(&_open)); + node->tie("force-is-calculated-by-other",SGRawValuePointer(&_forceIsCalculatedByMaster)); + node->tie("local-pos-x",SGRawValuePointer(&_pos[0])); + node->tie("local-pos-y",SGRawValuePointer(&_pos[1])); + node->tie("local-pos-z",SGRawValuePointer(&_pos[2])); + node->tie("tow/dist",SGRawValuePointer(&_dist)); + node->tie("tow/dist-time-lag-corrected",SGRawValuePointer(&_timeLagCorrectedDist)); + node->tie("tow/connected-to-property-node",SGRawValuePointer(&_towEndIsConnectedToProperty)); + node->tie("tow/connected-to-mp-node",SGRawValuePointer(&_nodeIsMultiplayer)); + node->tie("tow/connected-to-ai-node",SGRawValuePointer(&_nodeIsAiAircraft)); + node->tie("tow/connected-to-ai-or-mp-id",SGRawValuePointer(&_nodeID)); + node->tie("debug/hitch-height-above-ground",SGRawValuePointer(&_height_above_ground)); + node->tie("debug/tow-end-height-above-ground",SGRawValuePointer(&_winch_height_above_ground)); + node->tie("debug/tow-rel-lo-pos",SGRawValuePointer(&_loPosFrac)); + node->tie("debug/tow-lowest-pos-height",SGRawValuePointer(&_lowest_tow_height)); + node->tie("is-slave",SGRawValuePointer(&_isSlave)); + node->tie("speed-in-tow-direction",SGRawValuePointer(&_speed_in_tow_direction)); + node->tie("mp-auto-connect-period",SGRawValuePointer(&_mpAutoConnectPeriod)); + node->tie("mp-time-lag",SGRawValuePointer(&_mp_time_lag)); + node->setStringValue("tow/Node",""); + node->setStringValue("tow/connectedToAIorMP-callsign"); + node->setBoolValue("broken",false); +} + +Hitch::~Hitch() +{ + char text[128]; + sprintf(text,"/sim/hitches/%s", _name); + SGPropertyNode * node = fgGetNode(text, true); + node->untie("tow/length"); + node->untie("tow/elastic-constant"); + node->untie("tow/weight-per-m-kg-m"); + node->untie("tow/brake-force"); + node->untie("winch/max-speed-m-s"); + node->untie("winch/rel-speed"); + node->untie("winch/initial-tow-length-m"); + node->untie("winch/min-tow-length-m"); + node->untie("winch/max-tow-length-m"); + node->untie("winch/global-pos-x"); + node->untie("winch/global-pos-y"); + node->untie("winch/global-pos-z"); + node->untie("winch/max-power"); + node->untie("winch/max-force"); + node->untie("winch/actual-force"); + node->untie("tow/end-force-x"); + node->untie("tow/end-force-y"); + node->untie("tow/end-force-z"); + node->untie("force"); + node->untie("open"); + node->untie("force-is-calculated-by-other"); + node->untie("local-pos-x"); + node->untie("local-pos-y"); + node->untie("local-pos-z"); + node->untie("tow/dist"); + node->untie("tow/dist-time-lag-corrected"); + node->untie("tow/connected-to-property-node"); + node->untie("tow/connected-to-mp-node"); + node->untie("tow/connected-to-ai-node"); + node->untie("tow/connected-to-ai-or-mp-id"); + node->untie("debug/hitch-height-above-ground"); + node->untie("debug/tow-end-height-above-ground"); + node->untie("debug/tow-rel-lo-pos"); + node->untie("debug/tow-lowest-pos-height"); + node->untie("is-slave"); + node->untie("speed-in-tow-direction"); + node->untie("mp-auto-connect-period"); + node->untie("mp-time-lag"); + + delete _state; +} + +void Hitch::setPosition(float* position) +{ + int i; + for(i=0; i<3; i++) _pos[i] = position[i]; +} + +void Hitch::setTowLength(float length) +{ + _towLength = length; +} + +void Hitch::setOpen(bool isOpen) +{ + //test if we already processed this before + //without this test a binded property could + //try to close the Hitch every run + //it will close, if we are near the end + //e.g. if we are flying over the parked + //tow-aircraft.... + if (isOpen==_last_wish) + return; + _last_wish=isOpen; + _open=isOpen; +} + +void Hitch::setTowElasticConstant(float sc) +{ + _towElasticConstant=sc; +} + +void Hitch::setTowBreakForce(float bf) +{ + _towBrakeForce=bf; +} + +void Hitch::setWinchMaxForce(float f) +{ + _winchMaxForce=f; +} + +void Hitch::setTowWeightPerM(float rw) +{ + _towWeightPerM=rw; +} + +void Hitch::setWinchMaxSpeed(float mws) +{ + _winchMaxSpeed=mws; +} + +void Hitch::setWinchRelSpeed(float rws) +{ + _winchRelSpeed=rws; +} + +void Hitch::setWinchPosition(double *winchPosition)//in global coordinates! +{ + for (int i=0; i<3;i++) + _winchPos[i]=winchPosition[i]; +} + +void Hitch::setMpAutoConnectPeriod(float dt) +{ + _mpAutoConnectPeriod=dt; +} + +void Hitch::setForceIsCalculatedByOther(bool b) +{ + _forceIsCalculatedByMaster=b; +} + +const char *Hitch::getConnectedPropertyNode() const +{ + if (_towEndNode) + return _towEndNode->getDisplayName(); + else + return 0; +} + +void Hitch::setConnectedPropertyNode(const char *nodename) +{ + _towEndNode=fgGetNode(nodename,false); +} + +void Hitch::setWinchPositionAuto(bool doit) +{ + static bool lastState=false; + if(!_state) + return; + if (!doit) + { + lastState=false; + return; + } + if(lastState) + return; + lastState=true; + float lWinchPos[3]; + // The ground plane transformed to the local frame. + float ground[4]; + _state->planeGlobalToLocal(_global_ground, ground); + + float help[3]; + //find a normalized vector pointing forward parallel to the ground + help[0]=0; + help[1]=1; + help[2]=0; + Math::cross3(ground,help,help); + //multiplay by initial tow length; + //reduced by 1m to be able to close the + //hitch either if the glider slips backwards a bit + Math::mul3((_winchInitialTowLength-1.),help,help); + //add to the actual pos + Math::add3(_pos,help,lWinchPos); + //put it onto the ground plane + Math::mul3(ground[3],ground,help); + Math::add3(lWinchPos,help,lWinchPos); + + _state->posLocalToGlobal(lWinchPos,_winchPos); + _towLength=_winchInitialTowLength; + SG_LOG(SG_GENERAL, SG_ALERT,"Set the winch pos to "<<_winchPos[0]<<","<<_winchPos[1]<<","<<_winchPos[2]<setBoolValue("broken",false); + + //set the dist value (if not, the hitch would open in the next calcforce run + //float delta[3]; + //Math::sub3(lWinchPos,_pos,delta); + //_dist=Math::mag3(delta); + _dist=Math::mag3(lWinchPos); //use the aircraft center as reference for distance calculation + //this position is transferred to the MP-Aircraft. + //With this trick, both player in aerotow get the same length + +} + +void Hitch::findBestAIObject(bool doit,bool running_as_autoconnect) +{ + static bool lastState=false; + if(!_state) + return; + if (!doit) + { + lastState=false; + return; + } + if(lastState) + return; + lastState=true; + double gpos[3]; + _state->posLocalToGlobal(_pos,gpos); + double bestdist=_towLength*_towLength;//squared! + _towEndIsConnectedToProperty=false; + SGPropertyNode * ainode = fgGetNode("/ai/models",false); + if(!ainode) return; + char myCallsign[256]="***********"; + if (running_as_autoconnect) + { + //get own callsign + SGPropertyNode *cs=fgGetNode("/sim/multiplay/callsign",false); + if (cs) + { + strncpy(myCallsign,cs->getStringValue(),256); + myCallsign[255]=0; + } + //reset tow length for search radius. Lentgh will be later copied from master + _towLength=_winchInitialTowLength; + } + bool found=false; + vector nodes;// + for (int i=0;inChildren();i++) + { + SGPropertyNode * n=ainode->getChild(i); + _nodeIsMultiplayer = strncmp("multiplayer",n->getName(),11)==0; + _nodeIsAiAircraft = strncmp("aircraft",n->getName(),8)==0; + if (!(_nodeIsMultiplayer || _nodeIsAiAircraft)) + continue; + if (running_as_autoconnect) + { + if (!_nodeIsMultiplayer) + continue; + if(n->getBoolValue("sim/hitches/aerotow/open",true)) continue; + if(strncmp(myCallsign,n->getStringValue("sim/hitches/aerotow/tow/connectedToAIorMP-callsign"),255)!=0) + continue; + } + double pos[3]; + pos[0]=n->getDoubleValue("position/global-x",0); + pos[1]=n->getDoubleValue("position/global-y",0); + pos[2]=n->getDoubleValue("position/global-z",0); + double dist=0; + for (int j=0;j<3;j++) + dist+=(pos[j]-gpos[j])*(pos[j]-gpos[j]); + if (distsetStringValue("tow/Node",n->getPath()); + node->setStringValue("tow/Node",n->getDisplayName()); + _nodeID=n->getIntValue("id",0); + node->setStringValue("tow/connectedToAIorMP-callsign",n->getStringValue("callsign")); + _open=false; + found = true; + } + } + if (found) + { + char text[512]; + sprintf(text,"/sim/hitches/%s", _name); + SGPropertyNode * node = fgGetNode(text, true); + //if (!running_as_autoconnect) + SG_LOG(SG_GENERAL, SG_ALERT,"Found aircraft for aerotow: " + <getStringValue("tow/connectedToAIorMP-callsign") + <<", distance "<getStringValue("tow/Node")<posGlobalToLocal(_winchPos,lWinchPos); + Math::sub3(lWinchPos,_pos,delta); + //_dist=Math::mag3(delta); + _dist=Math::mag3(lWinchPos); //use the aircraft center as reference for distance calculation + //this position is transferred to the MP-Aircraft. + //With this trick, both player in aerotow get the same length + Math::unit3(delta,deltaN); + float lvel[3]; + s->velGlobalToLocal(s->v,lvel); + _speed_in_tow_direction=Math::dot3(lvel,deltaN); + if (_towEndIsConnectedToProperty && _nodeIsMultiplayer) + { + float mp_delta_dist_due_to_time_lag=0.5*_mp_time_lag*(-_mp_v+_speed_in_tow_direction); + _timeLagCorrectedDist=_dist+mp_delta_dist_due_to_time_lag; + if(_forceIsCalculatedByMaster && !_open) + { + s->velGlobalToLocal(_mp_force,_force); + return; + } + } + else + _timeLagCorrectedDist=_dist; + if (_open) + { + _force[0]=_force[1]=_force[2]=0; + return; + } + if(_dist>_towLength) + if(_towLength>1e-3) + _forceMagnitude=(_dist-_towLength)/_towLength*_towElasticConstant; + else + _forceMagnitude=2*_towBrakeForce; + else + _forceMagnitude=0; + if(_forceMagnitude>=_towBrakeForce) + { + _forceMagnitude=0; + _open=true; + char text[128]; + sprintf(text,"/sim/hitches/%s", _name); + SGPropertyNode * node = fgGetNode(text, true); + node->setBoolValue("broken",true); + _force[0]=_force[1]=_force[2]=0; + _towEndForce[0]=_towEndForce[1]=_towEndForce[2]=0; + _reportTowEndForce[0]=_reportTowEndForce[1]=_reportTowEndForce[2]=0; + return; + } + Math::mul3(_forceMagnitude,deltaN,_force); + Math::mul3(-1.,_force,_towEndForce); + _winchActualForce=_forceMagnitude; //missing: gravity on this end and friction + //Add the gravitiy of the rope. + //calculate some numbers: + float grav_force=_towWeightPerM*_towLength*9.81; + //the length of the gravity-expanded row: + float leng=_towLength+grav_force*_towLength/_towElasticConstant; + // 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); + + _height_above_ground = ground[3] - Math::dot3(_pos, ground); + + //the same for the winch-pos (the pos of the tow end) + _winch_height_above_ground = ground[3] - Math::dot3(lWinchPos, ground); + + //the frac of the grav force acting on _pos: + float grav_frac=0.5*(1+(_height_above_ground-_winch_height_above_ground)/leng); + grav_frac=Math::clamp(grav_frac,0,1); + float grav_frac_tow_end=1-grav_frac; + //reduce grav_frac, if the tow has ground contact. + if (_height_above_ground1e-3) + _loPosFrac=fa_/(fa_+fb_); + else + _loPosFrac=0.5; + //dist to tow-end parallel to ground + float ground_dist; + float help[3]; + //Math::cross3(delta,ground,help);//as long as we calculate the dist without _pos, od it with lWinchpos, the dist to our center.... + Math::cross3(lWinchPos,ground,help); + ground_dist=Math::mag3(help); + //height of lowest tow pos (relative to _pos) + _lowest_tow_height=_loPosFrac*Math::sqrt(Math::abs(stretchedTowLen*stretchedTowLen-ground_dist*ground_dist)); + if (_height_above_ground<_lowest_tow_height) + { + if (_height_above_ground>1e-3) + grav_frac*=_height_above_ground/_lowest_tow_height; + else + grav_frac=0; + } + if (_winch_height_above_ground<(_lowest_tow_height-_height_above_ground+_winch_height_above_ground)) + { + if (_winch_height_above_ground>1e-3) + grav_frac_tow_end*=_winch_height_above_ground/ + (_lowest_tow_height-_height_above_ground+_winch_height_above_ground); + else + grav_frac_tow_end=0; + } + } + else _lowest_tow_height=_loPosFrac=-1; //for debug output + float grav_force_v[3]; + Math::mul3(grav_frac*grav_force,ground,grav_force_v); + Math::add3(grav_force_v,_force,_force); + _forceMagnitude=Math::mag3(_force); + //the same for the tow end: + Math::mul3(grav_frac_tow_end*grav_force,ground,grav_force_v); + Math::add3(grav_force_v,_towEndForce,_towEndForce); + s->velLocalToGlobal(_towEndForce,_towEndForce); + + if(_forceMagnitude>=_towBrakeForce) + { + _forceMagnitude=0; + _open=true; + char text[128]; + sprintf(text,"/sim/hitches/%s", _name); + SGPropertyNode * node = fgGetNode(text, true); + node->setBoolValue("broken",true); + _force[0]=_force[1]=_force[2]=0; + _towEndForce[0]=_towEndForce[1]=_towEndForce[2]=0; + } + + +} + +// Computed values: total force +void Hitch::getForce(float* force, float* off) +{ + Math::set3(_force, force); + Math::set3(_pos, off); +} + +void Hitch::integrate (float dt) +{ + //check if hitch has opened or closed, if yes: message + if (_open !=_oldOpen) + { + if (_oldOpen) + { + if (_dist>_towLength*1.00001) + { + SG_LOG(SG_GENERAL, SG_ALERT,"Could not lock Hinch (tow length is insufficient) on hitch '"<<_name<<"' !"<setBoolValue("broken",false); + } + SG_LOG(SG_GENERAL, SG_ALERT,(_open?"Opened hitch (or broken tow) '":"Locked hitch '")<<_name<<"' !"<_mpAutoConnectPeriod) || (_timeToNextAutoConnectTry<0)) + { + _timeToNextAutoConnectTry=_mpAutoConnectPeriod; + //search for MP-Aircraft, which is towed with us + findBestAIObject(true,true); + } + } + //check, if tow end can be modified by property, if yes: update + if(_towEndIsConnectedToProperty) + { + char text[128]; + sprintf(text,"/sim/hitches/%s", _name); + SGPropertyNode * node = fgGetNode(text, true); + if (node) + { + //_towEndNode=fgGetNode(node->getStringValue("tow/Node"), false); + char towNode[256]; + strncpy(towNode,node->getStringValue("tow/Node"),256); + towNode[255]=0; + _towEndNode=fgGetNode("ai/models")->getNode(towNode, false); + //AI and multiplayer objects seem to change node? + //Check if we have the right one by callsign + if (_nodeIsMultiplayer || _nodeIsAiAircraft) + { + char MPcallsign[256]=""; + const char *MPc; + MPc=node->getStringValue("tow/connectedToAIorMP-callsign"); + if (MPc) + { + strncpy(MPcallsign,MPc,256); + MPcallsign[255]=0; + } + if (((_towEndNode)&&(strncmp(_towEndNode->getStringValue("callsign"),MPcallsign,255)!=0))||!_towEndNode) + { + _timeToNextReConnectTry-=dt; + if((_timeToNextReConnectTry<0)||(_timeToNextReConnectTry>10)) + { + _timeToNextReConnectTry=10; + SGPropertyNode * ainode = fgGetNode("/ai/models",false); + if(ainode) + { + for (int i=0;inChildren();i++) + { + SGPropertyNode * n=ainode->getChild(i); + if(_nodeIsMultiplayer?strncmp("multiplayer",n->getName(),11)==0:strncmp("aircraft",n->getName(),8)) + if (strcmp(n->getStringValue("callsign"),MPcallsign)==0)//found + { + _towEndNode=n; + //node->setStringValue("tow/Node",n->getPath()); + node->setStringValue("tow/Node",n->getDisplayName()); + } + } + } + } + } + } + if(_towEndNode) + { + _winchPos[0]=_towEndNode->getDoubleValue("position/global-x",_winchPos[0]); + _winchPos[1]=_towEndNode->getDoubleValue("position/global-y",_winchPos[1]); + _winchPos[2]=_towEndNode->getDoubleValue("position/global-z",_winchPos[2]); + _mp_lpos[0]=_towEndNode->getFloatValue("sim/hitches/aerotow/local-pos-x",0); + _mp_lpos[1]=_towEndNode->getFloatValue("sim/hitches/aerotow/local-pos-y",0); + _mp_lpos[2]=_towEndNode->getFloatValue("sim/hitches/aerotow/local-pos-z",0); + _mp_dist=_towEndNode->getFloatValue("sim/hitches/aerotow/tow/dist"); + _mp_v=_towEndNode->getFloatValue("sim/hitches/aerotow/speed-in-tow-direction"); + _mp_force[0]=_towEndNode->getFloatValue("sim/hitches/aerotow/tow/end-force-x",0); + _mp_force[1]=_towEndNode->getFloatValue("sim/hitches/aerotow/tow/end-force-y",0); + _mp_force[2]=_towEndNode->getFloatValue("sim/hitches/aerotow/tow/end-force-z",0); + + if(_isSlave) + { +#define gf(a,b) a=_towEndNode->getFloatValue(b,a) +#define gb(a,b) a=_towEndNode->getBoolValue(b,a) + gf(_towLength,"sim/hitches/aerotow/tow/length"); + gf(_towElasticConstant,"sim/hitches/aerotow/tow/elastic-constant"); + gf(_towWeightPerM,"sim/hitches/aerotow/tow/weight-per-m-kg-m"); + gf(_towBrakeForce,"sim/hitches/aerotow/brake-force"); + gb(_open,"sim/hitches/aerotow/open"); + gb(_mp_is_slave,"sim/hitches/aerotow/is-slave"); +#undef gf +#undef gb + if (_mp_is_slave) _isSlave=false; //someone should be master + } + else + { + //check if other has opened hitch, but is neccessary, that it was closed before + bool mp_open=_towEndNode->getBoolValue("sim/hitches/aerotow/open",_mp_open_last_state); + if (mp_open != _mp_open_last_state) //state has changed + { + _mp_open_last_state=mp_open; //store that value + if(!_open) + { + if(mp_open) + { + _open=true; + char text[512]; + sprintf(text,"/sim/hitches/%s", _name); + SGPropertyNode * node = fgGetNode(text, true); + node->getStringValue("tow/Node",""); + SG_LOG(SG_GENERAL, SG_ALERT,"'"<getStringValue("tow/Node","")<<"' has opened hitch!"<0.1) + { + float actual_time_lag_guess=(_mp_dist-_dist)/total_v; + //check, if it sounds ok + if((actual_time_lag_guess>0)&&(actual_time_lag_guess<5)) + { + float frac=abs_v*0.01; + if (frac>0.05) frac=0.05; + // if we are slow, the guess of the lag can be rather wrong. as faster we are + // the better the guess. Therefore frac is proportiona to the speed. Clamp it + // at 5m/s + _mp_time_lag=(1-frac)*_mp_time_lag+frac*actual_time_lag_guess; + } + } + } + } + } + } + //set the _reported_tow_end_force (smoothed) + //smooth it a bit and store it + float sc=10.*dt; //100ms + float tmp[3]; + Math::mul3(sc,_towEndForce,tmp); + Math::mul3(1.-sc,_reportTowEndForce,_reportTowEndForce); + Math::add3(tmp,_reportTowEndForce,_reportTowEndForce); + + if (_open) return; + if (_winchRelSpeed==0) return; + float factor=1,offset=0; + if (_winchActualForce>_winchMaxForce) + offset=-(_winchActualForce-_winchMaxForce)/_winchMaxForce*20; + if (_winchRelSpeed>0.01) // to avoit div by zero + { + float maxForcePowerLimit=_winchPower/(_winchRelSpeed*_winchMaxSpeed); + if (_winchActualForce>maxForcePowerLimit) + factor-=(_winchActualForce-maxForcePowerLimit)/maxForcePowerLimit; + } + _towLength-=dt*(factor*_winchRelSpeed+offset)*_winchMaxSpeed; + if (_towLength<=_winchMinTowLength) + { + if (_winchRelSpeed<0) + _winchRelSpeed=0; + _towLength=_winchMinTowLength; + return; + } + if (_towLength>=_winchMaxTowLength) + { + if (_winchRelSpeed<0) + _winchRelSpeed=0; + _towLength=_winchMaxTowLength; + return; + } +} + + + + +}; // namespace yasim \ No newline at end of file diff --git a/src/FDM/YASim/Hitch.hpp b/src/FDM/YASim/Hitch.hpp new file mode 100755 index 000000000..053680516 --- /dev/null +++ b/src/FDM/YASim/Hitch.hpp @@ -0,0 +1,112 @@ +#ifndef _HITCH_HPP +#define _HITCH_HPP +class SGPropertyNode; +namespace yasim { + +class Ground; +class RigidBody; +struct State; + +class Hitch { +public: + Hitch(const char *name); + ~Hitch(); + + // Externally set values + void setPosition(float* position); + void setOpen(bool isOpen); + //void setName(const char *text); + + void setTowLength(float length); + void setTowElasticConstant(float sc); + void setTowBreakForce(float bf); + void setTowWeightPerM(float rw); + void setWinchMaxSpeed(float mws); + void setWinchRelSpeed(float rws); + void setWinchPosition(double *winchPosition); //in global coordinates! + void setWinchPositionAuto(bool doit); + void findBestAIObject(bool doit,bool running_as_autoconnect=false); + void setWinchInitialTowLength(float length); + void setWinchPower(float power); + void setWinchMaxForce(float force); + void setWinchMaxTowLength(float length); + void setWinchMinTowLength(float length); + void setMpAutoConnectPeriod(float dt); + void setForceIsCalculatedByOther(bool b); + + void setGlobalGround(double *global_ground, float *global_vel); + + void getPosition(float* out); + float getTowLength(void); + + void calcForce(Ground *g_cb, RigidBody* body, State* s); + + // Computed values: total force + void getForce(float* force, float* off); + + void integrate (float dt); + + const char *getConnectedPropertyNode() const; + void setConnectedPropertyNode(const char *nodename); + +private: + float _pos[3]; + bool _open; + bool _oldOpen; + float _towLength; + float _towElasticConstant; + float _towBrakeForce; + float _towWeightPerM; + float _winchMaxSpeed; + float _winchRelSpeed; + float _winchInitialTowLength; + float _winchMaxTowLength; + float _winchMinTowLength; + float _winchPower; + float _winchMaxForce; + float _winchActualForce; + double _winchPos[3]; + float _force[3]; + float _towEndForce[3]; + float _reportTowEndForce[3]; + float _forceMagnitude; + double _global_ground[4]; + float _global_vel[3]; + char _name[256]; + State* _state; + float _dist; + float _timeLagCorrectedDist; + SGPropertyNode *_towEndNode; + const char *_towEndPropertyName; + bool _towEndIsConnectedToProperty; + bool _nodeIsMultiplayer; + bool _nodeIsAiAircraft; + bool _forceIsCalculatedByMaster; + int _nodeID; + //const char *_ai_MP_callsign; + bool _isSlave; + float _mpAutoConnectPeriod; + float _timeToNextAutoConnectTry; + float _timeToNextReConnectTry; + float _height_above_ground; + float _winch_height_above_ground; + float _loPosFrac; + float _lowest_tow_height; + float _speed_in_tow_direction; + float _mp_time_lag; + float _mp_last_reported_dist; + float _mp_last_reported_v; + float _mp_v; + float _mp_dist; + float _mp_lpos[3]; + float _mp_force[3]; + bool _mp_is_slave; + bool _mp_open_last_state; + + bool _displayed_len_lower_dist_message; + bool _last_wish; + +}; + +}; // namespace yasim +#endif // _HITCH_HPP diff --git a/src/FDM/YASim/Makefile.am b/src/FDM/YASim/Makefile.am index fb4afa076..aae98c48c 100644 --- a/src/FDM/YASim/Makefile.am +++ b/src/FDM/YASim/Makefile.am @@ -15,6 +15,7 @@ SHARED_SOURCE_FILES = \ Gear.cpp Gear.hpp \ Glue.cpp Glue.hpp \ Ground.cpp Ground.hpp \ + Hitch.cpp Hitch.hpp \ Hook.cpp Hook.hpp \ Launchbar.cpp Launchbar.hpp \ Integrator.cpp Integrator.hpp \ diff --git a/src/FDM/YASim/Model.cpp b/src/FDM/YASim/Model.cpp index 42479f95e..e328c6c5a 100644 --- a/src/FDM/YASim/Model.cpp +++ b/src/FDM/YASim/Model.cpp @@ -11,6 +11,7 @@ #include "Surface.hpp" #include "Rotor.hpp" #include "Rotorpart.hpp" +#include "Hitch.hpp" #include "Glue.hpp" #include "Ground.hpp" @@ -75,6 +76,9 @@ Model::~Model() delete _ground_cb; delete _hook; delete _launchbar; + for(int i=0; i<_hitches.size();i++) + delete (Hitch*)_hitches.get(i); + } void Model::getThrust(float* out) @@ -127,6 +131,11 @@ void Model::initIteration() _turb->offset(toff); } + for(i=0; i<_hitches.size(); i++) { + Hitch* h = (Hitch*)_hitches.get(i); + h->integrate(_integrator.getInterval()); + } + } @@ -252,6 +261,11 @@ void Model::addLaunchbar(Launchbar* launchbar) _launchbar = launchbar; } +int Model::addHitch(Hitch* hitch) +{ + return _hitches.add(hitch); +} + void Model::setGroundCallback(Ground* ground_cb) { delete _ground_cb; @@ -307,9 +321,34 @@ void Model::updateGround(State* s) // 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); + int type; + const SGMaterial* material; + _ground_cb->getGroundPlane(pt, global_ground, global_vel, + &type,&material); + static int h=0; + g->setGlobalGround(global_ground, global_vel, pt[0], pt[1], + type,material); } + + for(i=0; i<_hitches.size(); i++) { + Hitch* h = (Hitch*)_hitches.get(i); + + // Get the point of interest + float pos[3]; + h->getPosition(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); + h->setGlobalGround(global_ground, global_vel); + } + for(i=0; i<_rotorgear.getRotors()->size(); i++) { Rotor* r = (Rotor*)_rotorgear.getRotors()->get(i); r->findGroundEffectAltitude(_ground_cb,s); @@ -468,8 +507,16 @@ void Model::calcForces(State* s) _body.addForce(contactlb, forcelb); _body.addForce(contacthb, forcehb); } -} +// The hitches + for(i=0; i<_hitches.size(); i++) { + float force[3], contact[3]; + Hitch* h = (Hitch*)_hitches.get(i); + h->calcForce(_ground_cb,&_body, s); + h->getForce(force, contact); + _body.addForce(contact, force); + } +} void Model::newState(State* s) { _s = s; @@ -480,23 +527,26 @@ void Model::newState(State* s) 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); + if (!g->getSubmergable()) + { + // 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); + // 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 - if(dist < min) - min = dist; + // Find the lowest one + if(dist < min) + min = dist; + } } _agl = min; if(_agl < -1) // Allow for some integration slop diff --git a/src/FDM/YASim/Model.hpp b/src/FDM/YASim/Model.hpp index b0a81fb86..7c31b8ac8 100644 --- a/src/FDM/YASim/Model.hpp +++ b/src/FDM/YASim/Model.hpp @@ -19,6 +19,7 @@ class Gear; class Ground; class Hook; class Launchbar; +class Hitch; class Model : public BodyEnvironment { public: @@ -49,6 +50,7 @@ public: Rotorgear* getRotorgear(void); Gear* getGear(int handle); Hook* getHook(void); + int addHitch(Hitch* hitch); Launchbar* getLaunchbar(void); // Semi-private methods for use by the Airplane solver. @@ -92,6 +94,7 @@ private: Vector _gears; Hook* _hook; Launchbar* _launchbar; + Vector _hitches; float _groundEffectSpan; float _groundEffect; diff --git a/src/FDM/YASim/YASim.cxx b/src/FDM/YASim/YASim.cxx index 165a4b4aa..12c499c42 100644 --- a/src/FDM/YASim/YASim.cxx +++ b/src/FDM/YASim/YASim.cxx @@ -477,6 +477,7 @@ void YASim::copyFromYASim() node->setFloatValue("compression-m", g->getCompressDist()); node->setFloatValue("caster-angle-deg", g->getCasterAngle() * RAD2DEG); node->setFloatValue("rollspeed-ms", g->getRollSpeed()); + node->setBoolValue("ground-is-solid", g->getGroundIsSolid()!=0); } Hook* h = airplane->getHook(); diff --git a/src/FDM/YASim/yasim-test.cpp b/src/FDM/YASim/yasim-test.cpp index ebda62476..e63a9125a 100644 --- a/src/FDM/YASim/yasim-test.cpp +++ b/src/FDM/YASim/yasim-test.cpp @@ -46,7 +46,7 @@ void yasim_graph(Airplane* a, float alt, float kts) for(int deg=-179; deg<=179; deg++) { float aoa = deg * DEG2RAD; - Airplane::setupState(aoa, kts * KTS2MPS, &s); + Airplane::setupState(aoa, kts * KTS2MPS, 0 ,&s); m->getBody()->reset(); m->initIteration(); m->calcForces(&s);