5 void Glue::calcAlphaBeta(State* s, float* wind, float* alpha, float* beta)
7 // Convert the velocity to the aircraft frame.
9 Math::sub3(s->v, wind, v);
10 Math::vmul33(s->orient, v, v);
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]);
18 void Glue::calcEulerRates(State* s, float* roll, float* pitch, float* hdg)
20 // This one is easy, the projection of the rotation vector around
24 *hdg = -Math::dot3(up, s->rot); // negate for "NED" conventions
26 // A bit harder: the X component of the rotation vector expressed
27 // in airframe coordinates.
29 Math::vmul33(s->orient, s->rot, lr);
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.
37 Math::cross3(s->orient, up, pitchAxis);
38 Math::unit3(pitchAxis, pitchAxis);
39 *pitch = Math::dot3(pitchAxis, s->rot);
42 void Glue::xyz2nedMat(double lat, double lon, float* out)
44 // Shorthand for our output vectors:
45 float *north = out, *east = out+3, *down = out+6;
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);
52 north[0] = -clon * slat;
53 north[1] = -slon * slat;
60 down[0] = -clon * clat;
61 down[1] = -slon * clat;
65 void Glue::euler2orient(float roll, float pitch, float hdg, float* out)
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.
76 // Init to the identity matrix
80 out[3*i+j] = (i==j) ? 1.0f : 0.0f;
85 float s = Math::sin(roll);
86 float c = Math::cos(roll);
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;
96 for(col=0; col<3; col++) {
97 float x=out[col], z=out[col+6];
99 out[col+6] = c*z - s*x;
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;
111 Math::trans33(out, out);
114 void Glue::orient2euler(float* o, float* roll, float* pitch, float* hdg)
116 // The airplane's "pointing" direction in NED coordinates is vx,
117 // and it's y (left-right) axis is vy.
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];
122 // The heading is simply the rotation of the projection onto the
124 *hdg = Math::atan2(vx[1], vx[0]);
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);
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
141 uz[0] = 0; uz[1] = 0; uz[2] = 1;
142 Math::cross3(uz, ux, uy);
144 Math::cross3(ux, uy, uz);
146 float py = -Math::dot3(vy, uy);
147 float pz = -Math::dot3(vy, uz);
148 *roll = Math::atan2(pz, py);
151 void Glue::geodUp(double lat, double lon, float* up)
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));
159 // FIXME: Hardcoded WGS84 numbers...
160 void Glue::geodUp(double* pos, float* up)
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);
173 }; // namespace yasim