src/FDM/YASim/Glue.cpp

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