Em Seg, 2009-02-23 às 17:58 -0300, Diogo Kastrup escreveu:
> The carrier sailing out from underneath is strange, it looks like the
> groundcache can't finding a intersection with the ground for these
> aircrafts and it is using a fallback value. Can someone check if this
> happens without my patch also? I will try to find out why this is
> happening.
Yep, the groundcache can't find a intersection. I think this problem
occurs without my patch also. To fix it just change groundcache.cxx
around line 312 from this:
if (d*d < reference_vehicle_radius*dot(n, n)) {
to this:
if (d*d < reference_vehicle_radius*reference_vehicle_radius*dot(n, n)) {
I am sending the diff again with the fix for this and the (untested) fix
for the zero-speed problem, just in case someone want to play with it.
I am still trying to figure out why the s76c is shaking. This happens
with other helicopters also, like the R22.
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 23:14:47 -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 23:14:48 -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 23:14:49 -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.
@@ -304,7 +309,7 @@
// Only check if the triangle is in the cache sphere if the plane
// containing the triangle is near enough
double d = dot(n, v[0] - localCacheReference);
- if (d*d < reference_vehicle_radius*dot(n, n)) {
+ if (d*d < reference_vehicle_radius*reference_vehicle_radius*dot(n, n)) {
// Check if the sphere around the vehicle intersects the sphere
// around that triangle. If so, put that triangle into the cache.
double r2 = boundRadius + reference_vehicle_radius;
@@ -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,
@@ -535,7 +598,7 @@
contact = SGVec3d(0, 0, 0);
normal = SGVec3d(0, 0, 0);
- // Time difference to th reference time.
+ // Time difference to the reference time.
t -= cache_ref_time;
// The double valued point we start to search for intersection.
@@ -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 23:14:49 -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 23:14:49 -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 23:14:49 -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 23:14:49 -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 23:14:50 -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);
+
+ // 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)
+ {
+ // Get the direction of movement
+ float dir[3];
+ Math::unit3(cv, 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
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 23:14:50 -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 23:14:50 -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 23:14:50 -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 23:14:50 -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 23:14:50 -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 23:14:50 -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 23:14:51 -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
