src/FDM/YASim/Glue.cpp

   1 #include "Math.hpp"
   2 #include "Glue.hpp"
   3 namespace yasim {
   4
   5 void Glue::calcAlphaBeta(State* s, float* wind, float* alpha, float* beta)
   6 {
   7     // Convert the velocity to the aircraft frame.
   8     float v[3];
   9     Math::sub3(s->v, wind, v);
  10     Math::vmul33(s->orient, v, v);
  11
  12     // By convention, positive alpha is an up pitch, and a positive
  13     // beta is yawed to the right.
  14     *alpha = -Math::atan2(v[2], v[0]);
  15     *beta = Math::atan2(v[1], v[0]);
  16 }
  17
  18 void Glue::calcEulerRates(State* s, float* roll, float* pitch, float* hdg)
  19 {
  20     // This one is easy, the projection of the rotation vector around
  21     // the "up" axis.
  22     float up[3];
  23     geodUp(s->pos, up);
  24     *hdg = -Math::dot3(up, s->rot); // negate for "NED" conventions
  25
  26     // A bit harder: the X component of the rotation vector expressed
  27     // in airframe coordinates.
  28     float lr[3];
  29     Math::vmul33(s->orient, s->rot, lr);
  30     *roll = lr[0];
  31
  32     // Hardest: the component of rotation along the direction formed
  33     // by the cross product of (and thus perpendicular to) the
  34     // aircraft's forward vector (i.e. the first three elements of the
  35     // orientation matrix) and the "up" axis.
  36     float pitchAxis[3];
  37     Math::cross3(s->orient, up, pitchAxis);
  38     Math::unit3(pitchAxis, pitchAxis);
  39     *pitch = Math::dot3(pitchAxis, s->rot);
  40 }
  41
  42 void Glue::xyz2nedMat(double lat, double lon, float* out)
  43 {
  44     // Shorthand for our output vectors:
  45     float *north = out, *east = out+3, *down = out+6;
  46
  47     float slat = (float) Math::sin(lat);
  48     float clat = (float)Math::cos(lat);
  49     float slon = (float)Math::sin(lon);
  50     float clon = (float)Math::cos(lon);
  51
  52     north[0] = -clon * slat;
  53     north[1] = -slon * slat;
  54     north[2] = clat;
  55
  56     east[0] = -slon;
  57     east[1] = clon;
  58     east[2] = 0;
  59
  60     down[0] = -clon * clat;
  61     down[1] = -slon * clat;
  62     down[2] = -slat;
  63 }
  64
  65 void Glue::euler2orient(float roll, float pitch, float hdg, float* out)
  66 {
  67     // To translate a point in aircraft space to the output "NED"
  68     // frame, first negate the Y and Z axes (ugh), then roll around
  69     // the X axis, then pitch around Y, then point to the correct
  70     // heading about Z.  Expressed as a matrix multiplication, then,
  71     // the transformation from aircraft to local is HPRN.  And our
  72     // desired output is the inverse (i.e. transpose) of that.  Since
  73     // all rotations are 2D, they have a simpler form than a generic
  74     // rotation and are done out longhand below for efficiency.
  75
  76     // Init to the identity matrix
  77     int i, j;
  78     for(i=0; i<3; i++)
  79         for(j=0; j<3; j++)
  80             out[3*i+j] = (i==j) ? 1.0f : 0.0f;
  81
  82     // Negate Y and Z
  83     out[4] = out[8] = -1;
  84
  85     float s = Math::sin(roll);
  86     float c = Math::cos(roll);
  87     int col;
  88     for(col=0; col<3; col++) {
  89         float y=out[col+3], z=out[col+6];
  90         out[col+3] = c*y - s*z;
  91         out[col+6] = s*y + c*z;
  92     }
  93
  94     s = Math::sin(pitch);
  95     c = Math::cos(pitch);
  96     for(col=0; col<3; col++) {
  97         float x=out[col], z=out[col+6];
  98         out[col]   = c*x + s*z;
  99         out[col+6] = c*z - s*x;
 100     }
 101
 102     s = Math::sin(hdg);
 103     c = Math::cos(hdg);
 104     for(col=0; col<3; col++) {
 105         float x=out[col], y=out[col+3];
 106         out[col]   = c*x - s*y;
 107         out[col+3] = s*x + c*y;
 108     }
 109
 110     // Invert:
 111     Math::trans33(out, out);
 112 }
 113
 114 void Glue::orient2euler(float* o, float* roll, float* pitch, float* hdg)
 115 {
 116     // The airplane's "pointing" direction in NED coordinates is vx,
 117     // and it's y (left-right) axis is vy.
 118     float vx[3], vy[3];
 119     vx[0]=o[0], vx[1]=o[1], vx[2]=o[2];
 120     vy[0]=o[3], vy[1]=o[4], vy[2]=o[5];
 121
 122     // The heading is simply the rotation of the projection onto the
 123     // XY plane
 124     *hdg = Math::atan2(vx[1], vx[0]);
 125
 126     // The pitch is the angle between the XY plane and vx, remember
 127     // that rotations toward positive Z are _negative_
 128     float projmag = Math::sqrt(vx[0]*vx[0]+vx[1]*vx[1]);
 129     *pitch = -Math::atan2(vx[2], projmag);
 130
 131     // Roll is a bit harder.  Construct an "unrolled" orientation,
 132     // where the X axis is the same as the "rolled" one, and the Y
 133     // axis is parallel to the XY plane.  These two can give you an
 134     // "unrolled" Z axis as their cross product.  Now, take the "vy"
 135     // axis, which points out the left wing.  The projections of this
 136     // along the "unrolled" YZ plane will give you the roll angle via
 137     // atan().
 138     float* ux = vx;
 139     float  uy[3], uz[3];
 140
 141     uz[0] = 0; uz[1] = 0; uz[2] = 1;
 142     Math::cross3(uz, ux, uy);
 143     Math::unit3(uy, uy);
 144     Math::cross3(ux, uy, uz);
 145
 146     float py = -Math::dot3(vy, uy);
 147     float pz = -Math::dot3(vy, uz);
 148     *roll = Math::atan2(pz, py);
 149 }
 150
 151 void Glue::geodUp(double lat, double lon, float* up)
 152 {
 153     double coslat = Math::cos(lat);
 154     up[0] = (float)(Math::cos(lon) * coslat);
 155     up[1] = (float)(Math::sin(lon) * coslat);
 156     up[2] = (float)(Math::sin(lat));
 157 }
 158
 159 // FIXME: Hardcoded WGS84 numbers...
 160 void Glue::geodUp(double* pos, float* up)
 161 {
 162     const double SQUASH  = 0.9966471893352525192801545;
 163     const double STRETCH = 1.0033640898209764189003079;
 164     float x = (float)(pos[0] * SQUASH);
 165     float y = (float)(pos[1] * SQUASH);
 166     float z = (float)(pos[2] * STRETCH);
 167     float norm = 1/Math::sqrt(x*x + y*y + z*z);
 168     up[0] = x * norm;
 169     up[1] = y * norm;
 170     up[2] = z * norm;
 171 }
 172
 173 }; // namespace yasim
 174