5 void Glue::calcAlphaBeta(State* s, float* alpha, float* beta)
7 // Convert the velocity to the aircraft frame.
9 Math::vmul33(s->orient, s->v, v);
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]);
17 void Glue::calcEulerRates(State* s, float* roll, float* pitch, float* hdg)
19 // This one is easy, the projection of the rotation vector around
23 *hdg = -Math::dot3(up, s->rot); // negate for "NED" conventions
25 // A bit harder: the X component of the rotation vector expressed
26 // in airframe coordinates.
28 Math::vmul33(s->orient, s->rot, lr);
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.
36 Math::cross3(s->orient, up, pitchAxis);
37 Math::unit3(pitchAxis, pitchAxis);
38 *pitch = Math::dot3(pitchAxis, s->rot);
41 void Glue::xyz2nedMat(double lat, double lon, float* out)
43 // Shorthand for our output vectors:
44 float *north = out, *east = out+3, *down = out+6;
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);
51 north[0] = -clon * slat;
52 north[1] = -slon * slat;
59 down[0] = -clon * clat;
60 down[1] = -slon * clat;
64 void Glue::euler2orient(float roll, float pitch, float hdg, float* out)
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.
75 // Init to the identity matrix
79 out[3*i+j] = (i==j) ? 1.0f : 0.0f;
84 float s = Math::sin(roll);
85 float c = Math::cos(roll);
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;
95 for(col=0; col<3; col++) {
96 float x=out[col], z=out[col+6];
98 out[col+6] = c*z - s*x;
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;
110 Math::trans33(out, out);
113 void Glue::orient2euler(float* o, float* roll, float* pitch, float* hdg)
115 // The airplane's "pointing" direction in NED coordinates is vx,
116 // and it's y (left-right) axis is vy.
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];
121 // The heading is simply the rotation of the projection onto the
123 *hdg = Math::atan2(vx[1], vx[0]);
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);
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
140 uz[0] = 0; uz[1] = 0; uz[2] = 1;
141 Math::cross3(uz, ux, uy);
143 Math::cross3(ux, uy, uz);
145 float py = -Math::dot3(vy, uy);
146 float pz = -Math::dot3(vy, uz);
147 *roll = Math::atan2(pz, py);
150 void Glue::geodUp(double lat, double lon, float* up)
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));
158 // FIXME: Hardcoded WGS84 numbers...
159 void Glue::geodUp(double* pos, float* up)
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);
172 }; // namespace yasim