Hello,
I finally got this working with the carrier, here is the patch. Please
let me know what you guys think about it.
Features:
- No more sliding when should be stopped.
- better behavior when cornering
- better braking behavior *
Problems:
- More information about the brake system is needed to simulate the
wheel locks better. This requires changes to the aircraft definition
format. For now we may need to slow down the reaction to the brake
button so we can stop planes easier without sliding. Brakes mapped to an
axis should work much better.
- More information about the tire in general could help improving the
simulation.
I may be a little busy and offline this week as I am moving to another
city, but if you guys decide to commit this patch and some changes are
required, just let me know and I will work on it ASAP.
Regards,
Diogo
Index: src/FDM/flight.cxx
===================================================================
RCS file: /var/cvs/FlightGear-0.9/source/src/FDM/flight.cxx,v
retrieving revision 1.35
diff -u -r1.35 flight.cxx
--- src/FDM/flight.cxx 9 Nov 2007 05:39:13 -0000 1.35
+++ src/FDM/flight.cxx 23 Feb 2009 02:16:31 -0000
@@ -705,6 +705,15 @@
return dist*SG_METER_TO_FEET;
}
+void
+FGInterface::get_platform(double t, double pos[3], float *orient)
+{
+ SGVec3d _pos;
+ ground_cache.get_platform(t, _pos, orient);
+ if (pos)
+ assign(pos, _pos);
+}
+
// Legacy interface just kept because of JSBSim
bool
FGInterface::get_agl_m(double t, const double pt[3],
Index: src/FDM/flight.hxx
===================================================================
RCS file: /var/cvs/FlightGear-0.9/source/src/FDM/flight.hxx,v
retrieving revision 1.18
diff -u -r1.18 flight.hxx
--- src/FDM/flight.hxx 27 Jul 2008 16:25:14 -0000 1.18
+++ src/FDM/flight.hxx 23 Feb 2009 02:16:31 -0000
@@ -629,6 +629,9 @@
double end[2][3], double vel[2][3]);
double get_cat_ft(double t, const double pt[3],
double end[2][3], double vel[2][3]);
+
+ // Return the position and orientation of the platform
+ void get_platform(double t, double pos[3], float *orient);
// Return the altitude above ground below the wgs84 point pt
@@ -642,8 +645,8 @@
int *type, double *loadCapacity,
double *frictionFactor, double *agl);
bool get_agl_m(double t, const double pt[3],
- double contact[3], double normal[3], double vel[3],
- int *type, const SGMaterial **material,double *agl);
+ double contact[3], double normal[3], double vel[3],
+ int *type, const SGMaterial **material,double *agl);
bool get_agl_ft(double t, const double pt[3],
double contact[3], double normal[3], double vel[3],
int *type, double *loadCapacity,
Index: src/FDM/groundcache.cxx
===================================================================
RCS file: /var/cvs/FlightGear-0.9/source/src/FDM/groundcache.cxx,v
retrieving revision 1.33
diff -u -r1.33 groundcache.cxx
--- src/FDM/groundcache.cxx 15 Feb 2009 00:56:22 -0000 1.33
+++ src/FDM/groundcache.cxx 23 Feb 2009 02:16:32 -0000
@@ -158,6 +158,7 @@
_material(0),
cache_ref_time(0.0),
wire_id(0),
+ platform_id(0),
reference_wgs84_point(SGVec3d(0, 0, 0)),
reference_vehicle_radius(0.0),
down(0.0, 0.0, 0.0),
@@ -198,6 +199,8 @@
gp.vel = SGVec3d(0.0, 0.0, 0.0);
gp.rot = SGVec3d(0.0, 0.0, 0.0);
gp.pivot = SGVec3d(0.0, 0.0, 0.0);
+ gp.platform_id = 0;
+ gp.platform = NULL;
gp.material = 0;
backfaceCulling = false;
// XXX state set might be higher up in scene graph
@@ -235,6 +238,8 @@
break;
default:
gp.type = FGInterface::Solid;
+ gp.platform_id = ud->carrier->getID();
+ gp.platform = ud->carrier;
break;
}
// Copy the velocity from the carrier class.
@@ -323,6 +328,10 @@
t.sphere.setRadius(boundRadius);
t.gp = gp;
triangles.push_back(t);
+ if (gp.platform_id
+ && (platforms.empty()
+ || dynamic_cast<class FGAIBase*>(platforms.back())->getID() != gp.platform_id))
+ platforms.push_back(gp.platform);
}
}
// In case the cache is empty, we still provide agl computations.
@@ -414,6 +423,7 @@
triangles.resize(0);
catapults.resize(0);
wires.resize(0);
+ platforms.resize(0);
// Store the parameters we used to build up that cache.
reference_wgs84_point = pt;
@@ -460,6 +470,7 @@
<< ", # triangles = " << triangles.size()
<< ", # wires = " << wires.size()
<< ", # catapults = " << catapults.size()
+ << ", # platforms = " << platforms.size()
<< ", ground_radius = " << ground_radius );
// If the ground radius is still below 5e6 meters, then we do not yet have
@@ -520,6 +531,58 @@
return sqrt(dist);
}
+void
+FGGroundCache::get_platform(double t, SGVec3d& pos, float *orient)
+{
+ size_t sz = platforms.size();
+ if (sz == 0)
+ {
+ pos = SGVec3d(0,0,0);
+ for (int y = 0; y < 3; y++)
+ for (int x = 0; x < 3; x++)
+ orient[y*3+x] = x == y ? 1.0 : 0.0;
+ return;
+ }
+
+ for (size_t i = 0; i < sz; ++i) {
+ FGAIBase* platform = dynamic_cast<class FGAIBase*>(platforms[i]);
+ if (platform->getID() == platform_id)
+ {
+ SGMatrixf orient4;
+ double hdg, pitch, roll, speed, lat, lon;
+ hdg = platform->_getHeading();
+ pitch = platform->_getPitch();
+ roll = platform->_getRoll();
+ speed = platform->_getSpeed();
+ lat = platform->_getLatitude();
+ lon = platform->_getLongitude();
+
+ // Copied from AICarrier.cxx
+ // Transform that one to the horizontal local coordinate system.
+ SGQuatd ec2hl = SGQuatd::fromLonLatDeg(lon, lat);
+ // The orientation of the platform wrt the horizontal local frame
+ SGQuatd hl2body = SGQuatd::fromYawPitchRollDeg(hdg, pitch, roll);
+ // and postrotate the orientation of the AIModel wrt the horizontal
+ // local frame
+ SGQuatd ec2body = ec2hl*hl2body;
+ // Compute the velocity in m/s in the earth centered coordinate system axis
+ double v_north = 0.51444444*speed*cos(hdg * SGD_DEGREES_TO_RADIANS);
+ double v_east = 0.51444444*speed*sin(hdg * SGD_DEGREES_TO_RADIANS);
+ SGVec3d vel_wrt_earth = ec2hl.backTransform(SGVec3d(v_north, v_east, 0));
+
+ orient4.set(hl2body);
+ for (int y = 0; y < 3; y++)
+ for (int x = 0; x < 3; x++)
+ orient[y*3+x] = orient4[y*4+x];
+
+ // Time difference to the reference time.
+ t -= cache_ref_time;
+
+ pos = platform->getCartPos() + t * vel_wrt_earth;
+ }
+ }
+}
+
bool
FGGroundCache::get_agl(double t, const SGVec3d& dpt, double max_altoff,
SGVec3d& contact, SGVec3d& normal, SGVec3d& vel,
@@ -578,6 +641,7 @@
*agl = dot(down, contact - dpt);
if (material)
*material = triangles[i].gp.material;
+ platform_id = triangles[i].gp.platform_id;
}
}
}
Index: src/FDM/groundcache.hxx
===================================================================
RCS file: /var/cvs/FlightGear-0.9/source/src/FDM/groundcache.hxx,v
retrieving revision 1.11
diff -u -r1.11 groundcache.hxx
--- src/FDM/groundcache.hxx 13 Feb 2009 09:21:35 -0000 1.11
+++ src/FDM/groundcache.hxx 23 Feb 2009 02:16:32 -0000
@@ -62,6 +62,9 @@
// pt in wgs84 coordinates.
double get_cat(double t, const SGVec3d& pt,
SGVec3d end[2], SGVec3d vel[2]);
+
+ // Get the platform position and orientation
+ void get_platform(double t, SGVec3d& pos, float *orient);
// Return the altitude above ground below the wgs84 point pt
@@ -101,6 +104,9 @@
SGVec3d vel;
SGVec3d rot;
SGVec3d pivot;
+ // the platform owning the triangle
+ int platform_id;
+ osg::Referenced* platform;
// the simgear material reference, contains friction coeficients ...
const SGMaterial* material;
};
@@ -134,11 +140,14 @@
double cache_ref_time;
// The wire identifier to track.
int wire_id;
+ // The platform represented by the groundcache
+ int platform_id;
// Containers which hold all the essential information about this cache.
std::vector<Triangle> triangles;
std::vector<Catapult> catapults;
std::vector<Wire> wires;
+ std::vector<osg::Referenced*> platforms;
// The point and radius where the cache is built around.
// That are the arguments that were given to prepare_ground_cache.
Index: src/FDM/YASim/BodyEnvironment.hpp
===================================================================
RCS file: /var/cvs/FlightGear-0.9/source/src/FDM/YASim/BodyEnvironment.hpp,v
retrieving revision 1.4
diff -u -r1.4 BodyEnvironment.hpp
--- src/FDM/YASim/BodyEnvironment.hpp 27 Dec 2008 16:08:21 -0000 1.4
+++ src/FDM/YASim/BodyEnvironment.hpp 23 Feb 2009 02:16:33 -0000
@@ -19,6 +19,7 @@
float rot[3]; // rotational velocity
float acc[3]; // acceleration
float racc[3]; // rotational acceleration
+ float dt; // Time offset
// Simple initialization
State() {
@@ -29,6 +30,7 @@
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) {
Index: src/FDM/YASim/FGGround.cpp
===================================================================
RCS file: /var/cvs/FlightGear-0.9/source/src/FDM/YASim/FGGround.cpp,v
retrieving revision 1.3
diff -u -r1.3 FGGround.cpp
--- src/FDM/YASim/FGGround.cpp 17 Jan 2007 20:42:39 -0000 1.3
+++ src/FDM/YASim/FGGround.cpp 23 Feb 2009 02:16:33 -0000
@@ -83,6 +83,11 @@
return dist;
}
+void FGGround::getPlatform(float dt, double pos[3], float *orient)
+{
+ _iface->get_platform(_toff + dt, pos, orient);
+}
+
void FGGround::setTimeOffset(double toff)
{
_toff = toff;
Index: src/FDM/YASim/FGGround.hpp
===================================================================
RCS file: /var/cvs/FlightGear-0.9/source/src/FDM/YASim/FGGround.hpp,v
retrieving revision 1.2
diff -u -r1.2 FGGround.hpp
--- src/FDM/YASim/FGGround.hpp 17 Jan 2007 20:42:39 -0000 1.2
+++ src/FDM/YASim/FGGround.hpp 23 Feb 2009 02:16:33 -0000
@@ -38,6 +38,8 @@
virtual float getCatapult(const double pos[3],
double end[2][3], float vel[2][3]);
+ virtual void getPlatform(float dt, double pos[3], float *orient);
+
void setTimeOffset(double toff);
private:
Index: src/FDM/YASim/Gear.cpp
===================================================================
RCS file: /var/cvs/FlightGear-0.9/source/src/FDM/YASim/Gear.cpp,v
retrieving revision 1.9
diff -u -r1.9 Gear.cpp
--- src/FDM/YASim/Gear.cpp 13 May 2007 11:58:36 -0000 1.9
+++ src/FDM/YASim/Gear.cpp 23 Feb 2009 02:16:33 -0000
@@ -1,5 +1,6 @@
#include "Math.hpp"
#include "BodyEnvironment.hpp"
+#include "Ground.hpp"
#include "RigidBody.hpp"
#include <simgear/scene/material/mat.hxx>
@@ -7,6 +8,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
@@ -14,11 +16,12 @@
{
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 = 0.5 cm
_brake = 0;
_rot = 0;
_initialLoad = 0;
@@ -46,6 +49,13 @@
_global_ground[i] = _global_vel[i] = 0;
_global_ground[2] = 1;
_global_ground[3] = -1e3;
+
+ for(int y=0; y<3; y++)
+ {
+ _ground_pivot[y] = 0.0;
+ for(int x=0; x<3; x++)
+ _ground_orient[y*3+x] = x == y ? 1.0 : 0.0;
+ }
}
void Gear::setPosition(float* position)
@@ -191,8 +201,7 @@
_ground_isSolid = isSolid;
_global_x = globalX;
_global_y = globalY;
-
- }
+}
void Gear::getPosition(float* out)
{
@@ -293,7 +302,7 @@
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;
@@ -451,56 +460,247 @@
_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];
+ g_cb->getPlatform(s->dt, _ground_pivot, _ground_orient);
+
+ // Convert platform coordinates to global
+ Math::tmul33(_ground_orient, _stuck, stuckd);
+ stuckd[0] += _ground_pivot[0];
+ stuckd[1] += _ground_pivot[1];
+ stuckd[2] += _ground_pivot[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::mul3(fskid, skid, tmp);
- Math::add3(tmp, _force, _force);
+ Math::add3(ffric, _force, _force);
}
-float Gear::calcFriction(float wgt, float v) //used on solid ground
+void Gear::updateStuckPoint(State* s)
{
- // 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;
+ // Some values must be recalculated here because the
+ // integrator takes averaged values from the iterations
+ float stuck[3];
+ double stuckd[3];
+ // Convert platform coordinates to global
+ Math::tmul33(_ground_orient, _stuck, stuckd);
+ stuckd[0] += _ground_pivot[0];
+ stuckd[1] += _ground_pivot[1];
+ stuckd[2] += _ground_pivot[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);
+
+ if (_rolling || _slipping)
+ {
+ s->posLocalToGlobal(stuck, stuckd);
+ // Convert back to platform coordinates
+ stuckd[0] -= _ground_pivot[0];
+ stuckd[1] -= _ground_pivot[1];
+ stuckd[2] -= _ground_pivot[2];
+ Math::vmul33(_ground_orient, stuckd, _stuck);
+ }
+}
+
+//used on solid ground
+void Gear::calcFriction(float *stuck, float *cv,float *steer, float *skid
+ , float wgt, float *force)
+{
+ // 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);
+
+ // Get the direction of movement
+ float dir[3];
+ Math::unit3(cv, dir);
+
+ // If rolling and slipping are false, the gear is stopped
+ _rolling = false;
+ _slipping = false;
+ // Here we 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)
+ {
+ // 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
Index: src/FDM/YASim/Gear.hpp
===================================================================
RCS file: /var/cvs/FlightGear-0.9/source/src/FDM/YASim/Gear.hpp,v
retrieving revision 1.7
diff -u -r1.7 Gear.hpp
--- src/FDM/YASim/Gear.hpp 18 Jan 2007 21:46:28 -0000 1.7
+++ src/FDM/YASim/Gear.hpp 23 Feb 2009 02:16:33 -0000
@@ -5,6 +5,7 @@
namespace yasim {
+class Ground;
class RigidBody;
struct State;
@@ -73,7 +74,7 @@
// 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.
@@ -85,12 +86,19 @@
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;
@@ -126,6 +134,8 @@
bool _ground_isSolid;
double _global_x;
double _global_y;
+ float _ground_orient[9];
+ double _ground_pivot[3];
};
}; // namespace yasim
Index: src/FDM/YASim/Ground.cpp
===================================================================
RCS file: /var/cvs/FlightGear-0.9/source/src/FDM/YASim/Ground.cpp,v
retrieving revision 1.2
diff -u -r1.2 Ground.cpp
--- src/FDM/YASim/Ground.cpp 17 Jan 2007 20:42:39 -0000 1.2
+++ src/FDM/YASim/Ground.cpp 23 Feb 2009 02:16:33 -0000
@@ -30,8 +30,8 @@
}
void Ground::getGroundPlane(const double pos[3],
- double plane[4], float vel[3],
- int *type, const SGMaterial **material)
+ double plane[4], float vel[3],
+ int *type, const SGMaterial **material)
{
getGroundPlane(pos,plane,vel);
}
@@ -56,5 +56,15 @@
return 1e10;
}
+void Ground::getPlatform(float dt, double pos[3], float *orient)
+{
+ for (int y = 0; y < 3; y++)
+ {
+ pos[y] = 0.0;
+ for (int x = 0; x < 3; x++)
+ orient[y*3+x] = x == y ? 1.0 : 0.0;
+ }
+}
+
}; // namespace yasim
Index: src/FDM/YASim/Ground.hpp
===================================================================
RCS file: /var/cvs/FlightGear-0.9/source/src/FDM/YASim/Ground.hpp,v
retrieving revision 1.2
diff -u -r1.2 Ground.hpp
--- src/FDM/YASim/Ground.hpp 17 Jan 2007 20:42:39 -0000 1.2
+++ src/FDM/YASim/Ground.hpp 23 Feb 2009 02:16:33 -0000
@@ -13,8 +13,8 @@
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 plane[4], float vel[3],
+ int *type, const SGMaterial **material);
virtual bool caughtWire(const double pos[4][3]);
@@ -24,6 +24,8 @@
virtual float getCatapult(const double pos[3],
double end[2][3], float vel[2][3]);
+
+ virtual void getPlatform(float dt, double pos[3], float *orient);
};
}; // namespace yasim
Index: src/FDM/YASim/Integrator.cpp
===================================================================
RCS file: /var/cvs/FlightGear-0.9/source/src/FDM/YASim/Integrator.cpp,v
retrieving revision 1.2
diff -u -r1.2 Integrator.cpp
--- src/FDM/YASim/Integrator.cpp 7 Oct 2004 20:34:56 -0000 1.2
+++ src/FDM/YASim/Integrator.cpp 23 Feb 2009 02:16:34 -0000
@@ -173,12 +173,13 @@
}
for(j=0; j<9; j++)
stmp.orient[j] = ori[i][j];
- _env->calcForces(&stmp);
+ stmp.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]);
+ _body->getAccel(acc[i]);
+ _body->getAngularAccel(rac[i]);
+ l2gVector(_s.orient, acc[i], acc[i]);
+ l2gVector(_s.orient, rac[i], rac[i]);
//
// Save the resulting derivatives for the next iteration
Index: src/FDM/YASim/Math.cpp
===================================================================
RCS file: /var/cvs/FlightGear-0.9/source/src/FDM/YASim/Math.cpp,v
retrieving revision 1.5
diff -u -r1.5 Math.cpp
--- src/FDM/YASim/Math.cpp 14 Sep 2006 18:18:33 -0000 1.5
+++ src/FDM/YASim/Math.cpp 23 Feb 2009 02:16:34 -0000
@@ -163,6 +163,12 @@
out[2] = ax*by - bx*ay;
}
+void Math::set33(float* a, float* b)
+{
+ for (int i=0; i < 9; i++)
+ a[i] = b[i];
+}
+
void Math::mmul33(float* a, float* b, float* out)
{
float tmp[9];
@@ -191,6 +197,14 @@
out[2] = x*m[6] + y*m[7] + z*m[8];
}
+void Math::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];
+}
+
void Math::tmul33(float* m, float* v, float* out)
{
float x = v[0], y = v[1], z = v[2];
@@ -199,6 +213,14 @@
out[2] = x*m[2] + y*m[5] + z*m[8];
}
+void Math::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];
+}
+
void Math::invert33(float* m, float* out)
{
// Compute the inverse as the adjoint matrix times 1/(det M).
Index: src/FDM/YASim/Math.hpp
===================================================================
RCS file: /var/cvs/FlightGear-0.9/source/src/FDM/YASim/Math.hpp,v
retrieving revision 1.5
diff -u -r1.5 Math.hpp
--- src/FDM/YASim/Math.hpp 14 Sep 2006 18:18:33 -0000 1.5
+++ src/FDM/YASim/Math.hpp 23 Feb 2009 02:16:34 -0000
@@ -51,15 +51,19 @@
// 3 4 5
// 6 7 8
+ // Copy matrix
+ static void set33(float* a, float* b);
// Multiply two matrices
static void mmul33(float* a, float* b, float* out);
// Multiply by vector
static void vmul33(float* m, float* v, float* out);
+ static void vmul33(float* m, double* v, double* out);
// Multiply the vector by the matrix transpose. Or pre-multiply the
// matrix by v as a row vector. Same thing.
static void tmul33(float* m, float* v, float* out);
+ static void tmul33(float* m, double* v, double* out);
// Invert matrix
static void invert33(float* m, float* out);
Index: src/FDM/YASim/Model.cpp
===================================================================
RCS file: /var/cvs/FlightGear-0.9/source/src/FDM/YASim/Model.cpp,v
retrieving revision 1.14
diff -u -r1.14 Model.cpp
--- src/FDM/YASim/Model.cpp 17 Jan 2007 20:42:39 -0000 1.14
+++ src/FDM/YASim/Model.cpp 23 Feb 2009 02:16:34 -0000
@@ -324,10 +324,10 @@
int type;
const SGMaterial* material;
_ground_cb->getGroundPlane(pt, global_ground, global_vel,
- &type,&material);
+ &type, &material);
static int h=0;
g->setGlobalGround(global_ground, global_vel, pt[0], pt[1],
- type,material);
+ type, material);
}
for(i=0; i<_hitches.size(); i++) {
@@ -486,7 +486,7 @@
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);
}
@@ -529,24 +529,25 @@
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
------------------------------------------------------------------------------
Open Source Business Conference (OSBC), March 24-25, 2009, San Francisco, CA
-OSBC tackles the biggest issue in open source: Open Sourcing the Enterprise
-Strategies to boost innovation and cut costs with open source participation
-Receive a $600 discount off the registration fee with the source code: SFAD
http://p.sf.net/sfu/XcvMzF8H
_______________________________________________
Flightgear-devel mailing list
Flightgear-devel@lists.sourceforge.net
https://lists.sourceforge.net/lists/listinfo/flightgear-devel
