From 53f09ff6a571113640cc9add52e1ca449e3d1b73 Mon Sep 17 00:00:00 2001 From: mfranz Date: Wed, 17 Jan 2007 20:42:39 +0000 Subject: [PATCH] Maik JUSTUS: (OK'ed by Andy) """ - ground properties (e.g. feel bumpiness and the reduced friction of grass or go swimming with the beaver) - initial load for yasim gears (to get rid of the jitter the beaver has on ground) - glider/winch/aerotow (do winch start with YASim glider or do aerotow over the net) I will place a how-to on the wiki soon, here very short: use the sgs233y (or the bocian if you have AJ (up ot now) non-GPL bocian) winch start: Ctrl-w for placing the winch, hold w to winch, press Shift-w to release the tow aerotow: Place the glider within 60m to a MP-aircraft, press Ctrl-t to tow to this aircraft. If the MP-aircraft is the J3 and the patch is installed on both sides, the J3 feels the forces, too. The J3-pilot has to taxi very slow up to the moment, the glider starts moving. Increase the throttle gently. Don't lift the J3 early, wait for the glider being lifted, lift gently. """ --- src/FDM/YASim/Airplane.cpp | 32 +- src/FDM/YASim/Airplane.hpp | 13 +- src/FDM/YASim/ControlMap.cpp | 6 + src/FDM/YASim/ControlMap.hpp | 3 +- src/FDM/YASim/FGFDM.cpp | 76 +++- src/FDM/YASim/FGFDM.hpp | 2 + src/FDM/YASim/FGGround.cpp | 17 + src/FDM/YASim/FGGround.hpp | 11 + src/FDM/YASim/Gear.cpp | 210 +++++++++- src/FDM/YASim/Gear.hpp | 39 +- src/FDM/YASim/Ground.cpp | 8 + src/FDM/YASim/Ground.hpp | 5 + src/FDM/YASim/Hitch.cpp | 778 +++++++++++++++++++++++++++++++++++ src/FDM/YASim/Hitch.hpp | 112 +++++ src/FDM/YASim/Makefile.am | 1 + src/FDM/YASim/Model.cpp | 86 +++- src/FDM/YASim/Model.hpp | 3 + src/FDM/YASim/YASim.cxx | 1 + src/FDM/YASim/yasim-test.cpp | 2 +- 19 files changed, 1360 insertions(+), 45 deletions(-) create mode 100755 src/FDM/YASim/Hitch.cpp create mode 100755 src/FDM/YASim/Hitch.hpp 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);