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