#include "Surface.hpp"
#include "Rotorpart.hpp"
#include "Thruster.hpp"
+#include "Hitch.hpp"
#include "Airplane.hpp"
namespace yasim {
_cruiseT = 0;
_cruiseSpeed = 0;
_cruiseWeight = 0;
+ _cruiseGlideAngle = 0;
_approachP = 0;
_approachT = 0;
_approachSpeed = 0;
_approachAoA = 0;
_approachWeight = 0;
+ _approachGlideAngle = 0;
_dragFactor = 1;
_liftRatio = 1;
}
}
-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);
_cruiseAoA = 0;
_tailIncidence = 0;
_cruiseFuel = fuel;
+ _cruiseGlideAngle = gla;
}
void Airplane::setElevatorControl(int control)
_model.addHook(hook);
}
+void Airplane::addHitch(Hitch* hitch)
+{
+ _model.addHitch(hitch);
+}
+
void Airplane::addLaunchbar(Launchbar* launchbar)
{
_model.addLaunchbar(launchbar);
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);
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++)
// I made these up
g->setStaticFriction(0.6f);
g->setDynamicFriction(0.5f);
+ g->setContactPoint(1);
_model.addGear(g);
}
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
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);
void Airplane::runCruise()
{
- setupState(_cruiseAoA, _cruiseSpeed, &_cruiseState);
+ setupState(_cruiseAoA, _cruiseSpeed,_approachGlideAngle, &_cruiseState);
_model.setState(&_cruiseState);
_model.setAir(_cruiseP, _cruiseT,
Atmosphere::calcStdDensity(_cruiseP, _cruiseT));
void Airplane::runApproach()
{
- setupState(_approachAoA, _approachSpeed, &_approachState);
+ setupState(_approachAoA, _approachSpeed,_approachGlideAngle, &_approachState);
_model.setState(&_approachState);
_model.setAir(_approachP, _approachT,
Atmosphere::calcStdDensity(_approachP, _approachT));
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);
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();
class Hook;
class Launchbar;
class Thruster;
+class Hitch;
class Airplane {
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);
int numGear();
Gear* getGear(int g);
Hook* getHook();
+ int numHitches() { return _hitches.size(); }
+ Hitch* getHitch(int h);
Rotorgear* getRotorgear();
Launchbar* getLaunchbar();
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;
Vector _contacts; // non-gear ground contact points
Vector _weights;
Vector _surfs; // NON-wing Surfaces
+ Vector _hitches; //for airtow and winch
Vector _solveWeights;
float _cruiseSpeed;
float _cruiseWeight;
float _cruiseFuel;
+ float _cruiseGlideAngle;
Vector _approachControls;
State _approachState;
float _approachAoA;
float _approachWeight;
float _approachFuel;
+ float _approachGlideAngle;
int _solutionIterations;
float _dragFactor;
#include "Rotor.hpp"
#include "Math.hpp"
#include "Propeller.hpp"
+#include "Hitch.hpp"
#include "ControlMap.hpp"
namespace yasim {
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;
}
}
}
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]
}
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,
#include "TurbineEngine.hpp"
#include "Rotor.hpp"
#include "Rotorpart.hpp"
+#include "Hitch.hpp"
#include "FGFDM.hpp"
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");
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;
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();
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);
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
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.
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);
#include "Ground.hpp"
class FGInterface;
+class SGMaterial;
namespace yasim {
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]);
#include "BodyEnvironment.hpp"
#include "RigidBody.hpp"
+#include <simgear/scene/material/mat.hxx>
+#include <FDM/flight.hxx>
#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()
{
_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;
_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)
{
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
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);
// 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<maxGroundBumpAmplitude)
+ {
+ BumpAltitude=getBumpAltitude();
+ a+=BumpAltitude;
+ }
_compressDist = -a;
if(a > 0) {
_wow = 0;
// 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);
// 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
_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);
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;
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
#ifndef _GEAR_HPP
#define _GEAR_HPP
+class SGMaterial;
+
namespace yasim {
class RigidBody;
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);
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)
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];
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
#include "Glue.hpp"
+#include <simgear/scene/material/mat.hxx>
#include "Ground.hpp"
namespace yasim {
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;
#ifndef _GROUND_HPP
#define _GROUND_HPP
+class SGMaterial;
namespace yasim {
class Ground {
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]);
--- /dev/null
+#include "Math.hpp"
+#include "BodyEnvironment.hpp"
+#include "RigidBody.hpp"
+#include <Main/fg_props.hxx>
+#include <string.h>
+
+
+
+#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<float>(&_towLength));
+ node->tie("tow/elastic-constant",SGRawValuePointer<float>(&_towElasticConstant));
+ node->tie("tow/weight-per-m-kg-m",SGRawValuePointer<float>(&_towWeightPerM));
+ node->tie("tow/brake-force",SGRawValuePointer<float>(&_towBrakeForce));
+ node->tie("winch/max-speed-m-s",SGRawValuePointer<float>(&_winchMaxSpeed));
+ node->tie("winch/rel-speed",SGRawValuePointer<float>(&_winchRelSpeed));
+ node->tie("winch/initial-tow-length-m",SGRawValuePointer<float>(&_winchInitialTowLength));
+ node->tie("winch/min-tow-length-m",SGRawValuePointer<float>(&_winchMinTowLength));
+ node->tie("winch/max-tow-length-m",SGRawValuePointer<float>(&_winchMaxTowLength));
+ node->tie("winch/global-pos-x",SGRawValuePointer<double>(&_winchPos[0]));
+ node->tie("winch/global-pos-y",SGRawValuePointer<double>(&_winchPos[1]));
+ node->tie("winch/global-pos-z",SGRawValuePointer<double>(&_winchPos[2]));
+ node->tie("winch/max-power",SGRawValuePointer<float>(&_winchPower));
+ node->tie("winch/max-force",SGRawValuePointer<float>(&_winchMaxForce));
+ node->tie("winch/actual-force",SGRawValuePointer<float>(&_winchActualForce));
+ node->tie("tow/end-force-x",SGRawValuePointer<float>(&_reportTowEndForce[0]));
+ node->tie("tow/end-force-y",SGRawValuePointer<float>(&_reportTowEndForce[1]));
+ node->tie("tow/end-force-z",SGRawValuePointer<float>(&_reportTowEndForce[2]));
+ node->tie("force",SGRawValuePointer<float>(&_forceMagnitude));
+ node->tie("open",SGRawValuePointer<bool>(&_open));
+ node->tie("force-is-calculated-by-other",SGRawValuePointer<bool>(&_forceIsCalculatedByMaster));
+ node->tie("local-pos-x",SGRawValuePointer<float>(&_pos[0]));
+ node->tie("local-pos-y",SGRawValuePointer<float>(&_pos[1]));
+ node->tie("local-pos-z",SGRawValuePointer<float>(&_pos[2]));
+ node->tie("tow/dist",SGRawValuePointer<float>(&_dist));
+ node->tie("tow/dist-time-lag-corrected",SGRawValuePointer<float>(&_timeLagCorrectedDist));
+ node->tie("tow/connected-to-property-node",SGRawValuePointer<bool>(&_towEndIsConnectedToProperty));
+ node->tie("tow/connected-to-mp-node",SGRawValuePointer<bool>(&_nodeIsMultiplayer));
+ node->tie("tow/connected-to-ai-node",SGRawValuePointer<bool>(&_nodeIsAiAircraft));
+ node->tie("tow/connected-to-ai-or-mp-id",SGRawValuePointer<int>(&_nodeID));
+ node->tie("debug/hitch-height-above-ground",SGRawValuePointer<float>(&_height_above_ground));
+ node->tie("debug/tow-end-height-above-ground",SGRawValuePointer<float>(&_winch_height_above_ground));
+ node->tie("debug/tow-rel-lo-pos",SGRawValuePointer<float>(&_loPosFrac));
+ node->tie("debug/tow-lowest-pos-height",SGRawValuePointer<float>(&_lowest_tow_height));
+ node->tie("is-slave",SGRawValuePointer<bool>(&_isSlave));
+ node->tie("speed-in-tow-direction",SGRawValuePointer<float>(&_speed_in_tow_direction));
+ node->tie("mp-auto-connect-period",SGRawValuePointer<float>(&_mpAutoConnectPeriod));
+ node->tie("mp-time-lag",SGRawValuePointer<float>(&_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]<<endl);
+ _open=false;
+
+ char text[512];
+ sprintf(text,"/sim/hitches/%s", _name);
+ SGPropertyNode * node = fgGetNode(text, true);
+ node->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 <SGPropertyNode_ptr> nodes;//<SGPropertyNode_ptr>
+ for (int i=0;i<ainode->nChildren();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 (dist<bestdist)
+ {
+ bestdist=dist;
+ _towEndNode=n;
+ _towEndIsConnectedToProperty=true;
+ char text[512];
+ sprintf(text,"/sim/hitches/%s", _name);
+ SGPropertyNode * node = fgGetNode(text, true);
+ //node->setStringValue("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: "
+ <<node->getStringValue("tow/connectedToAIorMP-callsign")
+ <<", distance "<<Math::sqrt(bestdist)<<"m at "
+ <<node->getStringValue("tow/Node")<<endl);
+ if (running_as_autoconnect)
+ _isSlave=true;
+ //set the dist value to some value below the tow lentgh (if not, the hitch
+ //would open in the next calc force run
+ _dist=_towLength*0.5;
+ _mp_open_last_state=true;
+ }
+ else
+ if (!running_as_autoconnect)
+ SG_LOG(SG_GENERAL, SG_ALERT,"Found no aircraft for aerotow!"<<endl);
+}
+
+void Hitch::setWinchInitialTowLength(float length)
+{
+ _winchInitialTowLength=length;
+}
+
+void Hitch::setWinchPower(float power)
+{
+ _winchPower=power;
+}
+
+void Hitch::setWinchMaxTowLength(float length)
+{
+ _winchMaxTowLength=length;
+}
+
+void Hitch::setWinchMinTowLength(float length)
+{
+ _winchMinTowLength=length;
+}
+
+void Hitch::setGlobalGround(double *global_ground, float *global_vel)
+{
+ int i;
+ for(i=0; i<4; i++) _global_ground[i] = global_ground[i];
+ for(i=0; i<3; i++) _global_vel[i] = global_vel[i];
+}
+
+void Hitch::getPosition(float* out)
+{
+ int i;
+ for(i=0; i<3; i++) out[i] = _pos[i];
+}
+
+float Hitch::getTowLength(void)
+{
+ return _towLength;
+}
+
+void Hitch::calcForce(Ground *g_cb, RigidBody* body, State* s)
+{
+ float lWinchPos[3],delta[3],deltaN[3];
+ *_state=*s;
+ s->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_ground<leng) //if not, the tow can not be on ground
+ {
+ float fa[3],fb[3],fg[3];
+ //the grav force an the hitch position:
+ Math::mul3(-grav_frac*grav_force,ground,fg);
+ //the total force on hitch postion:
+ Math::add3(fg,_force,fa);
+ //the grav force an the tow end position:
+ Math::mul3(-(1-grav_frac)*grav_force,ground,fg);
+ //the total force on tow end postion:
+ //note: sub: _force on tow-end is negative of force on hitch postion
+ Math::sub3(fg,_force,fb);
+ float fa_=Math::mag3(fa);
+ float fb_=Math::mag3(fb);
+ float stretchedTowLen;
+ stretchedTowLen=_towLength*(1.+(fa_+fb_)/(2*_towElasticConstant));
+ //the relative position of the lowest postion of the tow:
+ if ((fa_+fb_)>1e-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<<"' !"<<endl);
+ _open=true;
+ return;
+ }
+ char text[512];
+ sprintf(text,"/sim/hitches/%s", _name);
+ SGPropertyNode * node = fgGetNode(text, true);
+ node->setBoolValue("broken",false);
+ }
+ SG_LOG(SG_GENERAL, SG_ALERT,(_open?"Opened hitch (or broken tow) '":"Locked hitch '")<<_name<<"' !"<<endl);
+ _oldOpen=_open;
+ }
+
+ //check, if tow end should be searched in all MP-aircrafts
+ if(_open && _mpAutoConnectPeriod)
+ {
+ _isSlave=false;
+ _timeToNextAutoConnectTry-=dt;
+ if ((_timeToNextAutoConnectTry>_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;i<ainode->nChildren();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,"'"<<node->getStringValue("tow/Node","")<<"' has opened hitch!"<<endl);
+ }
+ }
+ }
+ }
+ //try to calculate the time lag
+ if ((_mp_last_reported_dist!=_mp_dist)||(_mp_last_reported_v!=_mp_v)) //new data;
+ {
+ _mp_last_reported_dist=_mp_dist;
+ _mp_last_reported_v=_mp_v;
+ float total_v=-_mp_v+_speed_in_tow_direction;//mp has opposite tow direction
+ float abs_v=Math::abs(total_v);
+ if (abs_v>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
--- /dev/null
+#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
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 \
#include "Surface.hpp"
#include "Rotor.hpp"
#include "Rotorpart.hpp"
+#include "Hitch.hpp"
#include "Glue.hpp"
#include "Ground.hpp"
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)
_turb->offset(toff);
}
+ for(i=0; i<_hitches.size(); i++) {
+ Hitch* h = (Hitch*)_hitches.get(i);
+ h->integrate(_integrator.getInterval());
+ }
+
}
_launchbar = launchbar;
}
+int Model::addHitch(Hitch* hitch)
+{
+ return _hitches.add(hitch);
+}
+
void Model::setGroundCallback(Ground* ground_cb)
{
delete _ground_cb;
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];
+ 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);
- g->setGlobalGround(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);
_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;
for(i=0; i<_gears.size(); i++) {
Gear* g = (Gear*)_gears.get(i);
- // Get the point of ground contact
- float pos[3], cmpr[3];
- g->getPosition(pos);
- g->getCompression(cmpr);
- Math::mul3(g->getCompressFraction(), cmpr, cmpr);
- Math::add3(cmpr, pos, pos);
-
- // The plane transformed to local coordinates.
- double global_ground[4];
- g->getGlobalGround(global_ground);
- float ground[4];
- s->planeGlobalToLocal(global_ground, ground);
- float dist = ground[3] - Math::dot3(pos, ground);
-
- // Find the lowest one
- if(dist < min)
- min = dist;
+ 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);
+
+ // Find the lowest one
+ if(dist < min)
+ min = dist;
+ }
}
_agl = min;
if(_agl < -1) // Allow for some integration slop
class Ground;
class Hook;
class Launchbar;
+class Hitch;
class Model : public BodyEnvironment {
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.
Vector _gears;
Hook* _hook;
Launchbar* _launchbar;
+ Vector _hitches;
float _groundEffectSpan;
float _groundEffect;
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();
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);
projects / flightgear.git / commitdiff