Hi,
A long time ago I started working on a different implementation for
YASim static friction. With help from Csaba and Mathias Fröhlich the
patch worked but I never finished polishing it to submit a final
version. I finally got back to it and, although it is working on my
tests, I am not sure it follows the architecture design properly. I
think it could use some more testing also, as I couldn't test most of
the aircraft.
If you guys could test it and/or give any tips on how not to break any
interface design it would be appreciated.
I have a question also, is it possible to configure how much brake is
applied when I press the brake button? For now when you brake, the wheel
will lock and it will be hard to control the plane. I think it would be
necessary to limit the amount of brakes applied when using a key. It is
probably going to be more realistic when using a breaking pedal right
now though.
Regards,
Diogo
diff --git a/src/FDM/YASim/BodyEnvironment.hpp b/src/FDM/YASim/BodyEnvironment.hpp
index 0ff096a..d9b1719 100644
--- a/src/FDM/YASim/BodyEnvironment.hpp
+++ b/src/FDM/YASim/BodyEnvironment.hpp
@@ -19,6 +19,7 @@ struct State {
float rot[3]; // rotational velocity
float acc[3]; // acceleration
float racc[3]; // rotational acceleration
+ double dt; // Time offset
// Simple initialization
State() {
@@ -29,6 +30,7 @@ struct State {
for(j=0; j<3; j++)
orient[3*i+j] = i==j ? 1.0f : 0.0f;
}
+ dt = 0.0;
}
void posLocalToGlobal(float* lpos, double *gpos) {
diff --git a/src/FDM/YASim/FGGround.cpp b/src/FDM/YASim/FGGround.cpp
index a2f787b..f15ef19 100644
--- a/src/FDM/YASim/FGGround.cpp
+++ b/src/FDM/YASim/FGGround.cpp
@@ -22,13 +22,13 @@ FGGround::~FGGround()
}
void FGGround::getGroundPlane(const double pos[3],
- double plane[4], float vel[3])
+ double plane[4], float vel[3],
+ unsigned &body)
{
// Return values for the callback.
double cp[3], dvel[3], dangvel[3];
const simgear::BVHMaterial* material;
- simgear::BVHNode::Id id;
- _iface->get_agl_m(_toff, pos, 2, cp, plane, dvel, dangvel, material, id);
+ _iface->get_agl_m(_toff, pos, 2, cp, plane, dvel, dangvel, material, body);
// The plane below the actual contact point.
plane[3] = plane[0]*cp[0] + plane[1]*cp[1] + plane[2]*cp[2];
@@ -38,12 +38,12 @@ void FGGround::getGroundPlane(const double pos[3],
void FGGround::getGroundPlane(const double pos[3],
double plane[4], float vel[3],
- const simgear::BVHMaterial **material)
+ const simgear::BVHMaterial **material,
+ unsigned &body)
{
// Return values for the callback.
double cp[3], dvel[3], dangvel[3];
- simgear::BVHNode::Id id;
- _iface->get_agl_m(_toff, pos, 2, cp, plane, dvel, dangvel, *material, id);
+ _iface->get_agl_m(_toff, pos, 2, cp, plane, dvel, dangvel, *material, body);
// The plane below the actual contact point.
plane[3] = plane[0]*cp[0] + plane[1]*cp[1] + plane[2]*cp[2];
@@ -51,6 +51,15 @@ void FGGround::getGroundPlane(const double pos[3],
for(int i=0; i<3; i++) vel[i] = dvel[i];
}
+bool FGGround::getBody(double t, double bodyToWorld[16], double linearVel[3],
+ double angularVel[3], unsigned &body)
+{
+ if (!_iface->get_body_m(_toff + t , body, bodyToWorld, linearVel, angularVel))
+ return false;
+
+ return true;
+}
+
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 f3cdadb..1869acf 100644
--- a/src/FDM/YASim/FGGround.hpp
+++ b/src/FDM/YASim/FGGround.hpp
@@ -19,11 +19,16 @@ public:
virtual ~FGGround();
virtual void getGroundPlane(const double pos[3],
- double plane[4], float vel[3]);
+ double plane[4], float vel[3],
+ unsigned &body);
virtual void getGroundPlane(const double pos[3],
double plane[4], float vel[3],
- const simgear::BVHMaterial **material);
+ const simgear::BVHMaterial **material,
+ unsigned &body);
+
+ virtual bool getBody(double t, double bodyToWorld[16], double linearVel[3],
+ double angularVel[3], unsigned &id);
virtual bool caughtWire(const double pos[4][3]);
diff --git a/src/FDM/YASim/Gear.cpp b/src/FDM/YASim/Gear.cpp
index 6276c44..13faa96 100644
--- a/src/FDM/YASim/Gear.cpp
+++ b/src/FDM/YASim/Gear.cpp
@@ -5,6 +5,7 @@
#include "Math.hpp"
#include "BodyEnvironment.hpp"
+#include "Ground.hpp"
#include "RigidBody.hpp"
#include <cfloat>
@@ -13,6 +14,7 @@
#include "Gear.hpp"
namespace yasim {
static const float YASIM_PI = 3.14159265358979323846;
+static const float DEG2RAD = YASIM_PI / 180.0;
static const float maxGroundBumpAmplitude=0.4;
//Amplitude can be positive and negative
@@ -20,11 +22,12 @@ Gear::Gear()
{
int i;
for(i=0; i<3; i++)
- _pos[i] = _cmpr[i] = 0;
+ _pos[i] = _cmpr[i] = _stuck[i] = 0;
_spring = 1;
_damp = 0;
_sfric = 0.8f;
_dfric = 0.7f;
+ _fric_spring = 0.005f; // Spring length = 5 mm
_brake = 0;
_rot = 0;
_initialLoad = 0;
@@ -51,6 +54,13 @@ Gear::Gear()
_global_ground[i] = _global_vel[i] = 0;
_global_ground[2] = 1;
_global_ground[3] = -1e3;
+
+ for(int y=0; y<3; y++)
+ {
+ _ground_trans[y] = 0.0;
+ for(int x=0; x<3; x++)
+ _ground_rot[y*3+x] = x == y ? 1.0 : 0.0;
+ }
}
void Gear::setPosition(float* position)
@@ -147,7 +157,7 @@ void Gear::setInitialLoad(float l)
void Gear::setGlobalGround(double *global_ground, float* global_vel,
double globalX, double globalY,
- const simgear::BVHMaterial *material)
+ const simgear::BVHMaterial *material, unsigned body)
{
int i;
double frictionFactor,rollingFriction,loadCapacity,loadResistance,bumpiness;
@@ -180,8 +190,8 @@ void Gear::setGlobalGround(double *global_ground, float* global_vel,
_ground_isSolid = isSolid;
_global_x = globalX;
_global_y = globalY;
-
- }
+ _body_id = body;
+}
void Gear::getPosition(float* out)
{
@@ -282,7 +292,7 @@ float Gear::getBumpAltitude()
return h*(1/8.)*_ground_bumpiness*maxGroundBumpAmplitude;
}
-void Gear::calcForce(RigidBody* body, State *s, float* v, float* rot)
+void Gear::calcForce(Ground *g_cb, RigidBody* body, State *s, float* v, float* rot)
{
// Init the return values
int i;
@@ -446,56 +456,254 @@ void Gear::calcForce(RigidBody* body, State *s, float* v, float* rot)
_rollSpeed = vsteer;
_casterAngle = _rot;
}
- float fsteer,fskid;
+
+ float ffric[3];
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.
+ float stuckf[3];
+ double stuckd[3], lVel[3], aVel[3];
+ double body_xform[16];
+ g_cb->getBody(s->dt, body_xform, lVel, aVel, _body_id);
+ _ground_trans[0] = body_xform[12];
+ _ground_trans[1] = body_xform[13];
+ _ground_trans[2] = body_xform[14];
+ for (i = 0; i < 3; i++)
+ for (int j = 0; j < 3; j++)
+ _ground_rot[i*3+j] = body_xform[i*4+j];
+
+ // Convert platform coordinates to global
+ Math::tmul33(_ground_rot, _stuck, stuckd);
+ stuckd[0] += _ground_trans[0];
+ stuckd[1] += _ground_trans[1];
+ stuckd[2] += _ground_trans[2];
+
+ s->posGlobalToLocal(stuckd, stuckf);
+ calcFriction(stuckf, cv, steer, skid, wgt, ffric);
}
- 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;
+ else calcFrictionFluid(cv, steer, skid, wgt, ffric);
// Phoo! All done. Add it up and get out of here.
- Math::mul3(fsteer, steer, tmp);
- Math::add3(tmp, _force, _force);
+ Math::add3(ffric, _force, _force);
+}
+
+void Gear::updateStuckPoint(State* s)
+{
+ float stuck[3];
+ double stuckd[3];
+ // Convert platform coordinates to global
+ Math::tmul33(_ground_rot, _stuck, stuckd);
+ stuckd[0] += _ground_trans[0];
+ stuckd[1] += _ground_trans[1];
+ stuckd[2] += _ground_trans[2];
+ // Global to aircraft coordinates
+ s->posGlobalToLocal(stuckd, stuck);
+ // The ground plane transformed to the local frame.
+ float ground[4];
+ s->planeGlobalToLocal(_global_ground, ground);
+ float gup[3]; // "up" unit vector from the ground
+ Math::set3(ground, gup);
+ Math::mul3(-1, gup, gup);
+
+ float xhat[] = {1,0,0};
+ float skid[3], steer[3];
+ Math::cross3(gup, xhat, skid); // up cross xhat =~ skid
+ Math::unit3(skid, skid); // == skid
+ Math::cross3(skid, gup, steer); // skid cross up == steer
+
+ if(_rot != 0) {
+ // Correct for a rotation
+ float srot = Math::sin(_rot);
+ float crot = Math::cos(_rot);
+ float tx = steer[0];
+ float ty = steer[1];
+ steer[0] = crot*tx + srot*ty;
+ steer[1] = -srot*tx + crot*ty;
+
+ tx = skid[0];
+ ty = skid[1];
+
+ skid[0] = crot*tx + srot*ty;
+ skid[1] = -srot*tx + crot*ty;
+ }
+
+ if (_rolling)
+ {
+ // Calculate the gear's movement
+ float tmp[3];
+ Math::sub3(_contact, stuck, tmp);
+ // Get the movement in the skid and steer directions
+ float dskid, dsteer;
+ dskid = Math::dot3(tmp, skid);
+ dsteer = Math::dot3(tmp, steer);
+ // The movement is not always exactly on the steer axis
+ // so we should allow some empirical "slip" because of
+ // the wheel flexibility (max 5 degrees)
+ // FIXME: This angle should depend on the tire type/condition
+ // Is 5 degrees a good value?
+ float rad = (dskid / _fric_spring) * 5.0 * DEG2RAD;
+ dskid -= Math::abs(dsteer) * Math::tan(rad);
+
+ Math::mul3(dskid, skid, tmp);
+ Math::sub3(_contact, tmp, stuck);
+ }
+ else if (_slipping) Math::set3(_contact, stuck);
- Math::mul3(fskid, skid, tmp);
- Math::add3(tmp, _force, _force);
+ if (_rolling || _slipping)
+ {
+ s->posLocalToGlobal(stuck, stuckd);
+ // Convert back to body coordinates
+ stuckd[0] -= _ground_trans[0];
+ stuckd[1] -= _ground_trans[1];
+ stuckd[2] -= _ground_trans[2];
+ Math::vmul33(_ground_rot, stuckd, _stuck);
+ }
}
-float Gear::calcFriction(float wgt, float v) //used on solid ground
+//used on solid ground
+void Gear::calcFriction(float *stuck, float *cv, float *steer, float *skid
+ , float wgt, float *force)
{
- // How slow is stopped? 10 cm/second?
- const float STOP = 0.1f;
- const float iSTOP = 1.0f/STOP;
- v = Math::abs(v);
- if(v < STOP) return v*iSTOP * wgt * _sfric;
- else return wgt * _dfric;
+ // Calculate the gear's movement
+ float dgear[3];
+ Math::sub3(_contact, stuck, dgear);
+
+ // Get the movement in the steer and skid directions
+ float dsteer, dskid;
+ dsteer = Math::dot3(dgear, steer);
+ dskid = Math::dot3(dgear, skid);
+
+ // Get the velocities in the steer and skid directions
+ float vsteer, vskid;
+ vsteer = Math::dot3(cv, steer);
+ vskid = Math::dot3(cv, skid);
+
+ // If rolling and slipping are false, the gear is stopped
+ _rolling = false;
+ _slipping = false;
+ // Detect if it is slipping
+ if (Math::sqrt(dskid*dskid + dsteer*dsteer*(_brake/_sfric)) > _fric_spring)
+ {
+/*
+ // Turn on our ABS ;)
+ // FIXME: This is off because we don't want ABS on helicopters
+ // as their "wheels" should be locked all the time
+ if (Math::abs(dskid) < _fric_spring)
+ {
+ float bl;
+ bl = _sfric - _sfric * (Math::abs(dskid) / _fric_spring);
+ if (_brake > bl) _brake = bl;
+ }
+ else
+*/
+ _slipping = true;
+ }
+
+
+ float fric, fspring, fdamper, brake, tmp[3];
+ if (!_slipping)
+ {
+ // Calculate the steer force.
+ // Brake is limited between 0 and 1, wheel lock on 1.
+ brake = _brake > _sfric ? 1 : _brake/_sfric;
+ fspring = Math::abs((dsteer / _fric_spring) * wgt * _sfric);
+ // Set _rolling so the stuck point is updated
+ if ((Math::abs(dsteer) > _fric_spring) || (fspring > brake * wgt * _sfric))
+ {
+ _rolling = true;
+ fric = _ground_rollingFriction * wgt * _sfric; // Rolling
+ fric += _brake * wgt * _sfric * _ground_frictionFactor; // Brake
+ if (vsteer > 0) fric = -fric;
+ }
+ else // Stopped
+ {
+ fdamper = Math::abs(vsteer * wgt * _sfric);
+ fdamper *= ((dsteer * vsteer) > 0) ? 1 : -1;
+ fric = fspring + fdamper;
+ fric *= brake * _ground_frictionFactor
+ + (1 - brake) * _ground_rollingFriction;
+ if (dsteer > 0) fric = -fric;
+ }
+ Math::mul3(fric, steer, force);
+
+ // Calculate the skid force.
+ fspring = Math::abs((dskid / _fric_spring) * wgt * _sfric);
+ fdamper = Math::abs(vskid * wgt * _sfric);
+ fdamper *= ((dskid * vskid) > 0) ? 1 : -1;
+ fric = _ground_frictionFactor * (fspring + fdamper);
+ if (dskid > 0) fric = -fric;
+
+ Math::mul3(fric, skid, tmp);
+ Math::add3(force, tmp, force);
+
+ // The damper can add a lot of force,
+ // if it is to big, then it will slip
+ if (Math::mag3(force) > wgt * _sfric * _ground_frictionFactor)
+ {
+ _slipping = true;
+ _rolling = false;
+ }
+ }
+ if (_slipping)
+ {
+ // Get the direction of movement
+ float dir[3];
+ Math::unit3(dgear, dir);
+
+ // Calculate the steer force.
+ // brake is limited between 0 and 1, wheel lock on 1
+ brake = _brake > _dfric ? 1 : _brake/_dfric;
+ fric = wgt * _dfric * Math::abs(Math::dot3(dir, steer));
+ fric *= _ground_rollingFriction * (1 - brake)
+ + _ground_frictionFactor * brake;
+ if (vsteer > 0) fric = -fric;
+ Math::mul3(fric, steer, force);
+
+ // Calculate the skid force.
+ fric = wgt * _dfric * _ground_frictionFactor;
+ // Multiply by 1 when no brake, else reduce the turning component
+ fric *= (1 - brake) + Math::abs(Math::dot3(dir, skid)) * brake;
+ if (vskid > 0) fric = -fric;
+
+ Math::mul3(fric, skid, tmp);
+ Math::add3(force, tmp, force);
+ }
+
+
+ //reduce friction if wanted by _reduceFrictionByExtension
+ float factor = (1-_frac)*(1-_reduceFrictionByExtension)+_frac*1;
+ factor = Math::clamp(factor,0,1);
+ Math::mul3(factor, force, force);
}
-float Gear::calcFrictionFluid(float wgt, float v) //used on fluid ground
+//used on fluid ground
+void Gear::calcFrictionFluid(float *cv, float *steer, float *skid, float wgt, float *force)
{
// 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;
+ float vsteer, vskid;
+ vsteer = Math::dot3(cv, steer);
+ vskid = Math::dot3(cv, skid);
+
+ float tmp[3];
+ float fric;
+ // Calculate the steer force
+ float v = Math::abs(vsteer);
+ if(v < STOP) fric = v*iSTOP * wgt * _sfric;
+ else fric = wgt * _dfric*v*v*0.01;
+ //*0.01: to get _dfric of the same size than _dfric on solid
+ if (v > 0) fric = -fric;
+ Math::mul3(_ground_frictionFactor * fric, steer, force);
+
+ // Calculate the skid force
+ v = Math::abs(vskid);
+ if(v < STOP) fric = v*iSTOP * wgt * _sfric;
+ else fric = wgt * _dfric*v*v*0.01;
//*0.01: to get _dfric of the same size than _dfric on solid
+ if (v > 0) fric = -fric;
+ Math::mul3(10 * _ground_frictionFactor * fric, skid, tmp);
+ Math::add3(force, tmp, force);
+ //factor 10: floats have different drag in x and y.
}
}; // namespace yasim
diff --git a/src/FDM/YASim/Gear.hpp b/src/FDM/YASim/Gear.hpp
index 8d8cb35..82953a1 100644
--- a/src/FDM/YASim/Gear.hpp
+++ b/src/FDM/YASim/Gear.hpp
@@ -7,6 +7,7 @@ class BVHMaterial;
namespace yasim {
+class Ground;
class RigidBody;
struct State;
@@ -52,7 +53,7 @@ public:
void setIgnoreWhileSolving(bool c);
void setGlobalGround(double* global_ground, float* global_vel,
double globalX, double globalY,
- const simgear::BVHMaterial *material);
+ const simgear::BVHMaterial *material, unsigned body);
void getPosition(float* out);
void getCompression(float* out);
void getGlobalGround(double* global_ground);
@@ -75,7 +76,7 @@ public:
// vector, and a ground plane (all specified in local coordinates)
// and make a force and point of application (i.e. ground contact)
// available via getForce().
- void calcForce(RigidBody* body, State* s, float* v, float* rot);
+ void calcForce(Ground *g_cb, RigidBody* body, State* s, float* v, float* rot);
// Computed values: total force, weight-on-wheels (force normal to
// ground) and compression fraction.
@@ -87,12 +88,19 @@ public:
bool getIgnoreWhileSolving() {return _ignoreWhileSolving; }
void setContactPoint(bool c);
+ // Update the stuck point after all integrator's iterations
+ void updateStuckPoint(State* s);
+
private:
- float calcFriction(float wgt, float v);
- float calcFrictionFluid(float wgt, float v);
+ void calcFriction(float *gpos, float *cv, float *steer, float *skid, float wgt, float *force);
+ void calcFrictionFluid(float *cv, float *steer, float *skid, float wgt, float *force);
+ float _fric_spring;
bool _castering;
+ bool _rolling;
+ bool _slipping;
float _pos[3];
+ double _stuck[3];
float _cmpr[3];
float _spring;
float _damp;
@@ -127,6 +135,9 @@ private:
bool _ground_isSolid;
double _global_x;
double _global_y;
+ unsigned _body_id;
+ double _ground_rot[9];
+ double _ground_trans[3];
};
}; // namespace yasim
diff --git a/src/FDM/YASim/Ground.cpp b/src/FDM/YASim/Ground.cpp
index 955319e..9447cb2 100644
--- a/src/FDM/YASim/Ground.cpp
+++ b/src/FDM/YASim/Ground.cpp
@@ -17,7 +17,8 @@ Ground::~Ground()
}
void Ground::getGroundPlane(const double pos[3],
- double plane[4], float vel[3])
+ double plane[4], float vel[3],
+ unsigned &body)
{
// ground. Calculate a cartesian coordinate for the ground under
// us, find the (geodetic) up vector normal to the ground, then
@@ -31,13 +32,23 @@ void Ground::getGroundPlane(const double pos[3],
vel[0] = 0.0;
vel[1] = 0.0;
vel[2] = 0.0;
+
+ body = 0;
}
void Ground::getGroundPlane(const double pos[3],
double plane[4], float vel[3],
- const simgear::BVHMaterial **material)
+ const simgear::BVHMaterial **material,
+ unsigned &body)
+{
+ getGroundPlane(pos,plane,vel,body);
+}
+
+bool Ground::getBody(double t, double bodyToWorld[16], double linearVel[3],
+ double angularVel[3], unsigned &body)
{
- getGroundPlane(pos,plane,vel);
+ return getBody(t, bodyToWorld, linearVel,
+ angularVel, body);
}
bool Ground::caughtWire(const double pos[4][3])
diff --git a/src/FDM/YASim/Ground.hpp b/src/FDM/YASim/Ground.hpp
index 436c6e7..79ea3b6 100644
--- a/src/FDM/YASim/Ground.hpp
+++ b/src/FDM/YASim/Ground.hpp
@@ -12,11 +12,16 @@ public:
virtual ~Ground();
virtual void getGroundPlane(const double pos[3],
- double plane[4], float vel[3]);
+ double plane[4], float vel[3],
+ unsigned &body);
virtual void getGroundPlane(const double pos[3],
double plane[4], float vel[3],
- const simgear::BVHMaterial **material);
+ const simgear::BVHMaterial **material,
+ unsigned &body);
+
+ virtual bool getBody(double t, double bodyToWorld[16], double linearVel[3],
+ double angularVel[3], unsigned &id);
virtual bool caughtWire(const double pos[4][3]);
diff --git a/src/FDM/YASim/Integrator.cpp b/src/FDM/YASim/Integrator.cpp
index efb080c..76fcbc0 100644
--- a/src/FDM/YASim/Integrator.cpp
+++ b/src/FDM/YASim/Integrator.cpp
@@ -160,25 +160,26 @@ void Integrator::calcNewInterval()
// extract the accelerations, and convert to vectors in the
// global frame.
//
- _body->reset();
+ _body->reset();
- // FIXME. Copying into a state object is clumsy! The
- // per-coordinate arrays should be changed into a single array
- // of State objects. Ick.
- State stmp;
- for(j=0; j<3; j++) {
- stmp.pos[j] = pos[i][j];
- stmp.v[j] = vel[i][j];
- stmp.rot[j] = rot[i][j];
- }
- for(j=0; j<9; j++)
- stmp.orient[j] = ori[i][j];
+ // FIXME. Copying into a state object is clumsy! The
+ // per-coordinate arrays should be changed into a single array
+ // of State objects. Ick.
+ State stmp;
+ for(j=0; j<3; j++) {
+ stmp.pos[j] = pos[i][j];
+ stmp.v[j] = vel[i][j];
+ stmp.rot[j] = rot[i][j];
+ }
+ for(j=0; j<9; j++)
+ stmp.orient[j] = ori[i][j];
+ stmp.dt = dt - _dt;
_env->calcForces(&stmp);
_body->getAccel(acc[i]);
_body->getAngularAccel(rac[i]);
- l2gVector(_s.orient, acc[i], acc[i]);
- l2gVector(_s.orient, rac[i], rac[i]);
+ l2gVector(_s.orient, acc[i], acc[i]);
+ l2gVector(_s.orient, rac[i], rac[i]);
//
// Save the resulting derivatives for the next iteration
diff --git a/src/FDM/YASim/Math.hpp b/src/FDM/YASim/Math.hpp
index ab1f39b..0201708 100644
--- a/src/FDM/YASim/Math.hpp
+++ b/src/FDM/YASim/Math.hpp
@@ -94,6 +94,12 @@ public:
// 3 4 5
// 6 7 8
+ static inline void set33(float* a, float* b)
+ {
+ for (int i=0; i < 9; i++)
+ a[i] = b[i];
+ }
+
// Multiply two matrices
static void mmul33(float* a, float* b, float* out) {
float tmp[9];
@@ -121,6 +127,22 @@ public:
out[2] = x*m[6] + y*m[7] + z*m[8];
}
+ static inline void vmul33(float* m, double* v, double* out)
+ {
+ double x = v[0], y = v[1], z = v[2];
+ out[0] = x*m[0] + y*m[1] + z*m[2];
+ out[1] = x*m[3] + y*m[4] + z*m[5];
+ out[2] = x*m[6] + y*m[7] + z*m[8];
+ }
+
+ static inline void vmul33(double* m, double* v, double* out)
+ {
+ double x = v[0], y = v[1], z = v[2];
+ out[0] = x*m[0] + y*m[1] + z*m[2];
+ out[1] = x*m[3] + y*m[4] + z*m[5];
+ out[2] = x*m[6] + y*m[7] + z*m[8];
+ }
+
// Multiply the vector by the matrix transpose. Or pre-multiply the
// matrix by v as a row vector. Same thing.
static inline void tmul33(float* m, float* v, float* out) {
@@ -130,6 +152,22 @@ public:
out[2] = x*m[2] + y*m[5] + z*m[8];
}
+ static inline void tmul33(float* m, double* v, double* out)
+ {
+ double x = v[0], y = v[1], z = v[2];
+ out[0] = x*m[0] + y*m[3] + z*m[6];
+ out[1] = x*m[1] + y*m[4] + z*m[7];
+ out[2] = x*m[2] + y*m[5] + z*m[8];
+ }
+
+ static inline void tmul33(double* m, double* v, double* out)
+ {
+ double x = v[0], y = v[1], z = v[2];
+ out[0] = x*m[0] + y*m[3] + z*m[6];
+ out[1] = x*m[1] + y*m[4] + z*m[7];
+ out[2] = x*m[2] + y*m[5] + z*m[8];
+ }
+
// Invert matrix
static void invert33(float* m, float* out) {
// Compute the inverse as the adjoint matrix times 1/(det M).
diff --git a/src/FDM/YASim/Model.cpp b/src/FDM/YASim/Model.cpp
index 8f33bdb..fdb3bc0 100644
--- a/src/FDM/YASim/Model.cpp
+++ b/src/FDM/YASim/Model.cpp
@@ -304,7 +304,8 @@ void Model::setWind(float* wind)
void Model::updateGround(State* s)
{
float dummy[3];
- _ground_cb->getGroundPlane(s->pos, _global_ground, dummy);
+ unsigned body;
+ _ground_cb->getGroundPlane(s->pos, _global_ground, dummy, body);
int i;
// The landing gear
@@ -327,8 +328,8 @@ void Model::updateGround(State* s)
double global_ground[4];
float global_vel[3];
const simgear::BVHMaterial* material;
- _ground_cb->getGroundPlane(pt, global_ground, global_vel, &material);
- g->setGlobalGround(global_ground, global_vel, pt[0], pt[1], material);
+ _ground_cb->getGroundPlane(pt, global_ground, global_vel, &material, body);
+ g->setGlobalGround(global_ground, global_vel, pt[0], pt[1], material, body);
}
for(i=0; i<_hitches.size(); i++) {
@@ -346,7 +347,7 @@ 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);
+ _ground_cb->getGroundPlane(pt, global_ground, global_vel, body);
h->setGlobalGround(global_ground, global_vel);
}
@@ -360,7 +361,7 @@ void Model::updateGround(State* s)
double pt[3];
_hook->getTipGlobalPosition(s, pt);
double global_ground[4];
- _ground_cb->getGroundPlane(pt, global_ground, dummy);
+ _ground_cb->getGroundPlane(pt, global_ground, dummy, body);
_hook->setGlobalGround(global_ground);
}
@@ -369,7 +370,7 @@ void Model::updateGround(State* s)
double pt[3];
_launchbar->getTipGlobalPosition(s, pt);
double global_ground[4];
- _ground_cb->getGroundPlane(pt, global_ground, dummy);
+ _ground_cb->getGroundPlane(pt, global_ground, dummy, body);
_launchbar->setGlobalGround(global_ground);
}
}
@@ -487,7 +488,7 @@ void Model::calcForces(State* s)
float force[3], contact[3];
Gear* g = (Gear*)_gears.get(i);
- g->calcForce(&_body, s, lv, lrot);
+ g->calcForce(_ground_cb, &_body, s, lv, lrot);
g->getForce(force, contact);
_body.addForce(contact, force);
}
@@ -530,24 +531,25 @@ void Model::newState(State* s)
if (!g->getSubmergable())
{
- // Get the point of ground contact
+ // 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);
+ 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);
+ 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;
}
+ g->updateStuckPoint(s);
}
_agl = min;
if(_agl < -1) // Allow for some integration slop
diff --git a/src/FDM/YASim/Rotor.cpp b/src/FDM/YASim/Rotor.cpp
index ab1775a..38e36b1 100644
--- a/src/FDM/YASim/Rotor.cpp
+++ b/src/FDM/YASim/Rotor.cpp
@@ -854,7 +854,8 @@ void Rotor::testForRotorGroundContact(Ground * ground_cb,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);
+ unsigned body;
+ ground_cb->getGroundPlane(pt, global_ground, global_vel, body);
// find h, the distance to the ground
// The ground plane transformed to the local frame.
float ground[4];
@@ -902,7 +903,8 @@ float Rotor::findGroundEffectAltitude(Ground * ground_cb,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);
+ unsigned body;
+ ground_cb->getGroundPlane(pt, global_ground, global_vel, body);
// find h, the distance to the ground
// The ground plane transformed to the local frame.
float ground[4];
@@ -941,7 +943,8 @@ float Rotor::findGroundEffectAltitude(Ground * ground_cb,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);
+ unsigned body;
+ ground_cb->getGroundPlane(pt, global_ground, global_vel, body);
// find h, the distance to the ground
// The ground plane transformed to the local frame.
float ground[4];
diff --git a/src/FDM/flight.cxx b/src/FDM/flight.cxx
index c65a009..44f0f28 100644
--- a/src/FDM/flight.cxx
+++ b/src/FDM/flight.cxx
@@ -681,14 +681,16 @@ FGInterface::get_body_m(double t, simgear::BVHNode::Id id,
{
SGMatrixd _bodyToWorld;
SGVec3d _linearVel, _angularVel;
- if (!ground_cache.get_body(t, _bodyToWorld, _linearVel, _angularVel, id))
- return false;
+ bool _foundID = ground_cache.get_body(t, _bodyToWorld, _linearVel, _angularVel, id);
assign(linearVel, _linearVel);
assign(angularVel, _angularVel);
for (unsigned i = 0; i < 16; ++i)
bodyToWorld[i] = _bodyToWorld.data()[i];
+ if (!_foundID)
+ return false;
+
return true;
}
diff --git a/src/FDM/groundcache.cxx b/src/FDM/groundcache.cxx
index 0b34115..0844d78 100644
--- a/src/FDM/groundcache.cxx
+++ b/src/FDM/groundcache.cxx
@@ -449,6 +449,7 @@ public:
_bodyToWorld(SGMatrixd::unit()),
_linearVelocity(0, 0, 0),
_angularVelocity(0, 0, 0),
+ _foundId(false),
_time(t)
{ }
@@ -537,13 +538,14 @@ FGGroundCache::get_body(double t, SGMatrixd& bodyToWorld, SGVec3d& linearVel,
t += cache_time_offset;
BodyFinder bodyFinder(id, t);
_localBvhTree->accept(bodyFinder);
- if (bodyFinder.empty())
- return false;
bodyToWorld = bodyFinder.getBodyToWorld();
linearVel = bodyFinder.getLinearVelocity();
angularVel = bodyFinder.getAngularVelocity();
+ if (bodyFinder.empty())
+ return false;
+
return true;
}
------------------------------------------------------------------------------
Precog is a next-generation analytics platform capable of advanced
analytics on semi-structured data. The platform includes APIs for building
apps and a phenomenal toolset for data science. Developers can use
our toolset for easy data analysis & visualization. Get a free account!
http://www2.precog.com/precogplatform/slashdotnewsletter
_______________________________________________
Flightgear-devel mailing list
Flightgear-devel@lists.sourceforge.net
https://lists.sourceforge.net/lists/listinfo/flightgear-devel
