fly/flightgear
1
0
Fork 0
Code Activity

Bertrand Coconnier:

Browse source 
Additionally to the original fix (problem with the Tec2b matrix initialization) I have modified the code for the "STRUCTURE" contacts in order not to generate NaNs when the z direction of the body frame is normal to the ground normal. Now there should no longer be any NaN generated by the landing gear code.
This commit is contained in:
ehofman 2009-08-31 07:06:26 +00:00 committed by Tim Moore
parent 294a7b675a
commit 98b5e7e6fe
2 changed files with 45 additions and 13 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

44
src/FDM/JSBSim/models/FGLGear.cpp
View file

@ -392,19 +392,37 @@ FGColumnVector3& FGLGear::Force(void)
void FGLGear::ComputeGroundCoordSys(void)
{
// Compute the rolling direction projected on the ground
// It consists in finding a vector 'r' such that 'r' lies in the plane (w,z) and r.n = 0 (scalar
// product) where:
// 'n' is the normal to the ground,
// (x,y,z) are the directions defined in the body coord system
// and 'w' is 'x' rotated by the steering angle (SteerAngle) in the plane (x,y).
// r = u * w + v * z and r.n = 0 => v/u = -w.n/z.n = a
// We also want u**2+v**2=1 and u > 0 (i.e. r orientated in the same 'direction' than w)
// after some arithmetic, one finds that :
double a = -(vGroundNormal(eX)*cos(SteerAngle)+vGroundNormal(eY)*sin(SteerAngle)) / vGroundNormal(eZ);
double u = 1. / sqrt(1. + a*a);
double v = a * u;
FGColumnVector3 vRollingGroundVec = FGColumnVector3(u * cos(SteerAngle), u * sin(SteerAngle), v);
FGColumnVector3 vRollingGroundVec;
if (eContactType == ctBOGEY) {
// Compute the rolling direction projected on the ground
// It consists in finding a vector 'r' such that 'r' lies in the plane (w,z) and r.n = 0 (scalar
// product) where:
// 'n' is the normal to the ground,
// (x,y,z) are the directions defined in the body coord system
// and 'w' is 'x' rotated by the steering angle (SteerAngle) in the plane (x,y).
// r = u * w + v * z and r.n = 0 => v/u = -w.n/z.n = a
// We also want u**2+v**2=1 and u > 0 (i.e. r orientated in the same 'direction' than w)
// after some arithmetic, one finds that :
double a = -(vGroundNormal(eX)*cos(SteerAngle)+vGroundNormal(eY)*sin(SteerAngle)) / vGroundNormal(eZ);
double u = 1. / sqrt(1. + a*a);
double v = a * u;
vRollingGroundVec = FGColumnVector3(u * cos(SteerAngle), u * sin(SteerAngle), v);
}
else {
// Here the only significant direction is the normal to the ground "vGroundNormal". Since there is
// no wheel the 2 other vectors of the orthonormal basis are not meaningful and are only used to
// create the transformation matrix Tg2b. So we are building vRollingGroundVec as an arbitrary
// vector normal to vGroundNormal
if (fabs(vGroundNormal(eX)) > 0.)
vRollingGroundVec = FGColumnVector3(-vGroundNormal(eZ)/vGroundNormal(eX), 0., 1.);
else if (fabs(vGroundNormal(eY)) > 0.)
vRollingGroundVec = FGColumnVector3(0., -vGroundNormal(eZ)/vGroundNormal(eY), 1.);
else
vRollingGroundVec = FGColumnVector3(1., 0., -vGroundNormal(eX)/vGroundNormal(eZ));
vRollingGroundVec.Normalize();
}
// The sliping direction is the cross product multiplication of the ground normal and rolling
// directions

14
src/FDM/JSBSim/models/FGPropagate.cpp
View file

@ -189,6 +189,20 @@ void FGPropagate::SetInitialState(const FGInitialCondition *FGIC)
// Recompute the LocalTerrainRadius.
RecomputeLocalTerrainRadius();
// These local copies of the transformation matrices are for use for
// initial conditions only.
Tl2b = GetTl2b(); // local to body frame transform
Tb2l = Tl2b.Transposed(); // body to local frame transform
Tl2ec = GetTl2ec(); // local to ECEF transform
Tec2l = Tl2ec.Transposed(); // ECEF to local frame transform
Tec2b = Tl2b * Tec2l; // ECEF to body frame transform
Tb2ec = Tec2b.Transposed(); // body to ECEF frame tranform
Ti2ec = GetTi2ec(); // ECI to ECEF transform
Tec2i = Ti2ec.Transposed(); // ECEF to ECI frame transform
Ti2b = Tec2b*Ti2ec; // ECI to body frame transform
Tb2i = Ti2b.Transposed(); // body to ECI frame transform
}
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%