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::xyz2geoc(double* xyz, double* lat, double* lon, double* alt)
43 double x=xyz[0], y=xyz[1], z=xyz[2];
45 // Cylindrical radius from the polar axis
46 double rcyl = Math::sqrt(x*x + y*y);
48 // In geocentric coordinates, these are just the angles in
50 *lon = Math::atan2(y, x);
51 *lat = Math::atan2(z, rcyl);
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);
60 *alt = len * (1-frac);
63 void Glue::geoc2xyz(double lat, double lon, double alt, double* out)
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);
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);
87 double Glue::geod2geocLat(double lat)
89 double r = Math::cos(lat)*STRETCH*STRETCH;
90 double z = Math::sin(lat);
91 return Math::atan2(z, r);
94 double Glue::geoc2geodLat(double lat)
96 double r = Math::cos(lat)*SQUASH*SQUASH;
97 double z = Math::sin(lat);
98 return Math::atan2(z, r);
101 void Glue::xyz2geod(double* xyz, double* lat, double* lon, double* alt)
103 xyz2geoc(xyz, lat, lon, alt);
104 *lat = geoc2geodLat(*lat);
107 void Glue::geod2xyz(double lat, double lon, double alt, double* out)
109 lat = geod2geocLat(lat);
110 geoc2xyz(lat, lon, alt, out);
113 void Glue::xyz2nedMat(double lat, double lon, float* out)
115 // Shorthand for our output vectors:
116 float *north = out, *east = out+3, *down = out+6;
118 float slat = Math::sin(lat);
119 float clat = Math::cos(lat);
120 float slon = Math::sin(lon);
121 float clon = Math::cos(lon);
123 north[0] = -clon * slat;
124 north[1] = -slon * slat;
131 down[0] = -clon * clat;
132 down[1] = -slon * clat;
136 void Glue::euler2orient(float roll, float pitch, float hdg, float* out)
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.
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;
153 out[4] = out[8] = -1;
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;
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;
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;
180 Math::trans33(out, out);
183 void Glue::orient2euler(float* o, float* roll, float* pitch, float* hdg)
185 // The airplane's "pointing" direction in NED coordinates is vx,
186 // and it's y (left-right) axis is vy.
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];
191 // The heading is simply the rotation of the projection onto the
193 *hdg = Math::atan2(vx[1], vx[0]);
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);
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
210 uz[0] = 0; uz[1] = 0; uz[2] = 1;
211 Math::cross3(uz, ux, uy);
213 Math::cross3(ux, uy, uz);
215 float py = -Math::dot3(vy, uy);
216 float pz = -Math::dot3(vy, uz);
217 *roll = Math::atan2(pz, py);
220 void Glue::geodUp(double* pos, float* out)
222 double lat, lon, alt;
223 xyz2geod(pos, &lat, &lon, &alt);
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;
234 }; // namespace yasim