src/FDM/YASim/Glue.cpp

   1 #include "Math.hpp"
   2 #include "Glue.hpp"
   3 namespace yasim {
   4
   5 // WGS84 numbers
   6 static const double EQURAD = 6378137;         // equatorial radius
   7 static const double STRETCH = 1.003352810665; // equ./polar radius
   8
   9 // Derived from the above
  10 static const double SQUASH = 0.99665839311;      // 1/STRETCH
  11 static const double POLRAD = 6356823.77346;      // EQURAD*SQUASH
  12 static const double iPOLRAD = 1.57311266701e-07; // 1/POLRAD
  13
  14 void Glue::calcAlphaBeta(State* s, float* alpha, float* beta)
  15 {
  16     // Convert the velocity to the aircraft frame.
  17     float v[3];
  18     Math::vmul33(s->orient, s->v, v);
  19
  20     // By convention, positive alpha is an up pitch, and a positive
  21     // beta is yawed to the right.
  22     *alpha = -Math::atan2(v[2], v[0]);
  23     *beta = Math::atan2(v[1], v[0]);
  24 }
  25
  26 void Glue::calcEulerRates(State* s, float* roll, float* pitch, float* hdg)
  27 {
  28     // This one is easy, the projection of the rotation vector around
  29     // the "up" axis.
  30     float up[3];
  31     geodUp(s->pos, up);
  32     *hdg = -Math::dot3(up, s->rot); // negate for "NED" conventions
  33
  34     // A bit harder: the X component of the rotation vector expressed
  35     // in airframe coordinates.
  36     float lr[3];
  37     Math::vmul33(s->orient, s->rot, lr);
  38     *roll = lr[0];
  39
  40     // Hardest: the component of rotation along the direction formed
  41     // by the cross product of (and thus perpendicular to) the
  42     // aircraft's forward vector (i.e. the first three elements of the
  43     // orientation matrix) and the "up" axis.
  44     float pitchAxis[3];
  45     Math::cross3(s->orient, up, pitchAxis);
  46     Math::unit3(pitchAxis, pitchAxis);
  47     *pitch = Math::dot3(pitchAxis, s->rot);
  48 }
  49
  50 void Glue::xyz2geoc(double* xyz, double* lat, double* lon, double* alt)
  51 {
  52     double x=xyz[0], y=xyz[1], z=xyz[2];
  53
  54     // Cylindrical radius from the polar axis
  55     double rcyl = Math::sqrt(x*x + y*y);
  56
  57     // In geocentric coordinates, these are just the angles in
  58     // cartesian space.
  59     *lon = Math::atan2(y, x);
  60     *lat = Math::atan2(z, rcyl);
  61
  62     // To get XYZ coordinate of "ground", we "squash" the cylindric
  63     // radius into a coordinate system where the earth is a sphere,
  64     // find the fraction of the xyz vector that is above ground.
  65     double rsquash = SQUASH * rcyl;
  66     double frac = POLRAD/Math::sqrt(rsquash*rsquash + z*z);
  67     double len = Math::sqrt(x*x + y*y + z*z);
  68
  69     *alt = len * (1-frac);
  70 }
  71
  72 void Glue::geoc2xyz(double lat, double lon, double alt, double* out)
  73 {
  74     // Generate a unit vector
  75     double rcyl = Math::cos(lat);
  76     double x = rcyl*Math::cos(lon);
  77     double y = rcyl*Math::sin(lon);
  78     double z = Math::sin(lat);
  79
  80     // Convert to "squashed" space, renormalize the unit vector,
  81     // multiply by the polar radius, and back convert to get us the
  82     // point of intersection of the unit vector with the surface.
  83     // Then just add the altitude.
  84     double rtmp = rcyl*SQUASH;
  85     double renorm = POLRAD/Math::sqrt(rtmp*rtmp + z*z);
  86     double ztmp = z*renorm;
  87     rtmp *= renorm*STRETCH;
  88     double len = Math::sqrt(rtmp*rtmp + ztmp*ztmp);
  89     len += alt;
  90
  91     out[0] = x*len;
  92     out[1] = y*len;
  93     out[2] = z*len;
  94 }
  95
  96 double Glue::geod2geocLat(double lat)
  97 {
  98     double r = Math::cos(lat)*STRETCH*STRETCH;
  99     double z = Math::sin(lat);
 100     return Math::atan2(z, r);
 101 }
 102
 103 double Glue::geoc2geodLat(double lat)
 104 {
 105     double r = Math::cos(lat)*SQUASH*SQUASH;
 106     double z = Math::sin(lat);
 107     return Math::atan2(z, r);
 108 }
 109
 110 void Glue::xyz2geod(double* xyz, double* lat, double* lon, double* alt)
 111 {
 112     xyz2geoc(xyz, lat, lon, alt);
 113     *lat = geoc2geodLat(*lat);
 114 }
 115
 116 void Glue::geod2xyz(double lat, double lon, double alt, double* out)
 117 {
 118     lat = geod2geocLat(lat);
 119     geoc2xyz(lat, lon, alt, out);
 120 }
 121
 122 void Glue::xyz2nedMat(double lat, double lon, float* out)
 123 {
 124     // Shorthand for our output vectors:
 125     float *north = out, *east = out+3, *down = out+6;
 126
 127     float slat = (float) Math::sin(lat);
 128     float clat = (float)Math::cos(lat);
 129     float slon = (float)Math::sin(lon);
 130     float clon = (float)Math::cos(lon);
 131
 132     north[0] = -clon * slat;
 133     north[1] = -slon * slat;
 134     north[2] = clat;
 135
 136     east[0] = -slon;
 137     east[1] = clon;
 138     east[2] = 0;
 139
 140     down[0] = -clon * clat;
 141     down[1] = -slon * clat;
 142     down[2] = -slat;
 143 }
 144
 145 void Glue::euler2orient(float roll, float pitch, float hdg, float* out)
 146 {
 147     // To translate a point in aircraft space to the output "NED"
 148     // frame, first negate the Y and Z axes (ugh), then roll around
 149     // the X axis, then pitch around Y, then point to the correct
 150     // heading about Z.  Expressed as a matrix multiplication, then,
 151     // the transformation from aircraft to local is HPRN.  And our
 152     // desired output is the inverse (i.e. transpose) of that.  Since
 153     // all rotations are 2D, they have a simpler form than a generic
 154     // rotation and are done out longhand below for efficiency.
 155
 156     // Init to the identity matrix
 157     int i, j;
 158     for(i=0; i<3; i++)
 159         for(j=0; j<3; j++)
 160             out[3*i+j] = (i==j) ? 1.0f : 0.0f;
 161
 162     // Negate Y and Z
 163     out[4] = out[8] = -1;
 164
 165     float s = Math::sin(roll);
 166     float c = Math::cos(roll);
 167     int col;
 168     for(col=0; col<3; col++) {
 169         float y=out[col+3], z=out[col+6];
 170         out[col+3] = c*y - s*z;
 171         out[col+6] = s*y + c*z;
 172     }
 173
 174     s = Math::sin(pitch);
 175     c = Math::cos(pitch);
 176     for(col=0; col<3; col++) {
 177         float x=out[col], z=out[col+6];
 178         out[col]   = c*x + s*z;
 179         out[col+6] = c*z - s*x;
 180     }
 181
 182     s = Math::sin(hdg);
 183     c = Math::cos(hdg);
 184     for(col=0; col<3; col++) {
 185         float x=out[col], y=out[col+3];
 186         out[col]   = c*x - s*y;
 187         out[col+3] = s*x + c*y;
 188     }
 189
 190     // Invert:
 191     Math::trans33(out, out);
 192 }
 193
 194 void Glue::orient2euler(float* o, float* roll, float* pitch, float* hdg)
 195 {
 196     // The airplane's "pointing" direction in NED coordinates is vx,
 197     // and it's y (left-right) axis is vy.
 198     float vx[3], vy[3];
 199     vx[0]=o[0], vx[1]=o[1], vx[2]=o[2];
 200     vy[0]=o[3], vy[1]=o[4], vy[2]=o[5];
 201
 202     // The heading is simply the rotation of the projection onto the
 203     // XY plane
 204     *hdg = Math::atan2(vx[1], vx[0]);
 205
 206     // The pitch is the angle between the XY plane and vx, remember
 207     // that rotations toward positive Z are _negative_
 208     float projmag = Math::sqrt(vx[0]*vx[0]+vx[1]*vx[1]);
 209     *pitch = -Math::atan2(vx[2], projmag);
 210
 211     // Roll is a bit harder.  Construct an "unrolled" orientation,
 212     // where the X axis is the same as the "rolled" one, and the Y
 213     // axis is parallel to the XY plane.  These two can give you an
 214     // "unrolled" Z axis as their cross product.  Now, take the "vy"
 215     // axis, which points out the left wing.  The projections of this
 216     // along the "unrolled" YZ plane will give you the roll angle via
 217     // atan().
 218     float* ux = vx;
 219     float  uy[3], uz[3];
 220
 221     uz[0] = 0; uz[1] = 0; uz[2] = 1;
 222     Math::cross3(uz, ux, uy);
 223     Math::unit3(uy, uy);
 224     Math::cross3(ux, uy, uz);
 225
 226     float py = -Math::dot3(vy, uy);
 227     float pz = -Math::dot3(vy, uz);
 228     *roll = Math::atan2(pz, py);
 229 }
 230
 231 void Glue::geodUp(double* pos, float* out)
 232 {
 233     double lat, lon, alt;
 234     xyz2geod(pos, &lat, &lon, &alt);
 235
 236     float slat = (float)Math::sin(lat);
 237     float clat = (float)Math::cos(lat);
 238     float slon = (float)Math::sin(lon);
 239     float clon = (float)Math::cos(lon);
 240     out[0] = clon * clat;
 241     out[1] = slon * clat;
 242     out[2] = slat;
 243 }
 244
 245 }; // namespace yasim
 246