6 static const double EQURAD = 6378137; // equatorial radius
7 static const double STRETCH = 1.003352810665; // equ./polar radius
9 // Derived from the above
10 static const double SQUASH = 0.99665839311; // 1/STRETCH
11 static const double POLRAD = 6356823.77346; // EQURAD*SQUASH
12 static const double iPOLRAD = 1.57311266701e-07; // 1/POLRAD
14 void Glue::calcAlphaBeta(State* s, float* alpha, float* beta)
16 // Convert the velocity to the aircraft frame.
18 Math::vmul33(s->orient, s->v, v);
20 // By convention, positive alpha is an up pitch, and a positive
21 // beta is yawed to the right.
22 *alpha = -Math::atan2(v[2], v[0]);
23 *beta = Math::atan2(v[1], v[0]);
26 void Glue::calcEulerRates(State* s, float* roll, float* pitch, float* hdg)
28 // This one is easy, the projection of the rotation vector around
32 *hdg = -Math::dot3(up, s->rot); // negate for "NED" conventions
34 // A bit harder: the X component of the rotation vector expressed
35 // in airframe coordinates.
37 Math::vmul33(s->orient, s->rot, lr);
40 // Hardest: the component of rotation along the direction formed
41 // by the cross product of (and thus perpendicular to) the
42 // aircraft's forward vector (i.e. the first three elements of the
43 // orientation matrix) and the "up" axis.
45 Math::cross3(s->orient, up, pitchAxis);
46 Math::unit3(pitchAxis, pitchAxis);
47 *pitch = Math::dot3(pitchAxis, s->rot);
50 void Glue::xyz2geoc(double* xyz, double* lat, double* lon, double* alt)
52 double x=xyz[0], y=xyz[1], z=xyz[2];
54 // Cylindrical radius from the polar axis
55 double rcyl = Math::sqrt(x*x + y*y);
57 // In geocentric coordinates, these are just the angles in
59 *lon = Math::atan2(y, x);
60 *lat = Math::atan2(z, rcyl);
62 // To get XYZ coordinate of "ground", we "squash" the cylindric
63 // radius into a coordinate system where the earth is a sphere,
64 // find the fraction of the xyz vector that is above ground.
65 double rsquash = SQUASH * rcyl;
66 double frac = POLRAD/Math::sqrt(rsquash*rsquash + z*z);
67 double len = Math::sqrt(x*x + y*y + z*z);
69 *alt = len * (1-frac);
72 void Glue::geoc2xyz(double lat, double lon, double alt, double* out)
74 // Generate a unit vector
75 double rcyl = Math::cos(lat);
76 double x = rcyl*Math::cos(lon);
77 double y = rcyl*Math::sin(lon);
78 double z = Math::sin(lat);
80 // Convert to "squashed" space, renormalize the unit vector,
81 // multiply by the polar radius, and back convert to get us the
82 // point of intersection of the unit vector with the surface.
83 // Then just add the altitude.
84 double rtmp = rcyl*SQUASH;
85 double renorm = POLRAD/Math::sqrt(rtmp*rtmp + z*z);
86 double ztmp = z*renorm;
87 rtmp *= renorm*STRETCH;
88 double len = Math::sqrt(rtmp*rtmp + ztmp*ztmp);
96 double Glue::geod2geocLat(double lat)
98 double r = Math::cos(lat)*STRETCH*STRETCH;
99 double z = Math::sin(lat);
100 return Math::atan2(z, r);
103 double Glue::geoc2geodLat(double lat)
105 double r = Math::cos(lat)*SQUASH*SQUASH;
106 double z = Math::sin(lat);
107 return Math::atan2(z, r);
110 void Glue::xyz2geod(double* xyz, double* lat, double* lon, double* alt)
112 xyz2geoc(xyz, lat, lon, alt);
113 *lat = geoc2geodLat(*lat);
116 void Glue::geod2xyz(double lat, double lon, double alt, double* out)
118 lat = geod2geocLat(lat);
119 geoc2xyz(lat, lon, alt, out);
122 void Glue::xyz2nedMat(double lat, double lon, float* out)
124 // Shorthand for our output vectors:
125 float *north = out, *east = out+3, *down = out+6;
127 float slat = Math::sin(lat);
128 float clat = Math::cos(lat);
129 float slon = Math::sin(lon);
130 float clon = Math::cos(lon);
132 north[0] = -clon * slat;
133 north[1] = -slon * slat;
140 down[0] = -clon * clat;
141 down[1] = -slon * clat;
145 void Glue::euler2orient(float roll, float pitch, float hdg, float* out)
147 // To translate a point in aircraft space to the output "NED"
148 // frame, first negate the Y and Z axes (ugh), then roll around
149 // the X axis, then pitch around Y, then point to the correct
150 // heading about Z. Expressed as a matrix multiplication, then,
151 // the transformation from aircraft to local is HPRN. And our
152 // desired output is the inverse (i.e. transpose) of that. Since
153 // all rotations are 2D, they have a simpler form than a generic
154 // rotation and are done out longhand below for efficiency.
156 // Init to the identity matrix
160 out[3*i+j] = (i==j) ? 1 : 0;
163 out[4] = out[8] = -1;
165 float s = Math::sin(roll);
166 float c = Math::cos(roll);
168 for(col=0; col<3; col++) {
169 float y=out[col+3], z=out[col+6];
170 out[col+3] = c*y - s*z;
171 out[col+6] = s*y + c*z;
174 s = Math::sin(pitch);
175 c = Math::cos(pitch);
176 for(col=0; col<3; col++) {
177 float x=out[col], z=out[col+6];
178 out[col] = c*x + s*z;
179 out[col+6] = c*z - s*x;
184 for(col=0; col<3; col++) {
185 float x=out[col], y=out[col+3];
186 out[col] = c*x - s*y;
187 out[col+3] = s*x + c*y;
191 Math::trans33(out, out);
194 void Glue::orient2euler(float* o, float* roll, float* pitch, float* hdg)
196 // The airplane's "pointing" direction in NED coordinates is vx,
197 // and it's y (left-right) axis is vy.
199 vx[0]=o[0], vx[1]=o[1], vx[2]=o[2];
200 vy[0]=o[3], vy[1]=o[4], vy[2]=o[5];
202 // The heading is simply the rotation of the projection onto the
204 *hdg = Math::atan2(vx[1], vx[0]);
206 // The pitch is the angle between the XY plane and vx, remember
207 // that rotations toward positive Z are _negative_
208 float projmag = Math::sqrt(vx[0]*vx[0]+vx[1]*vx[1]);
209 *pitch = -Math::atan2(vx[2], projmag);
211 // Roll is a bit harder. Construct an "unrolled" orientation,
212 // where the X axis is the same as the "rolled" one, and the Y
213 // axis is parallel to the XY plane. These two can give you an
214 // "unrolled" Z axis as their cross product. Now, take the "vy"
215 // axis, which points out the left wing. The projections of this
216 // along the "unrolled" YZ plane will give you the roll angle via
221 uz[0] = 0; uz[1] = 0; uz[2] = 1;
222 Math::cross3(uz, ux, uy);
224 Math::cross3(ux, uy, uz);
226 float py = -Math::dot3(vy, uy);
227 float pz = -Math::dot3(vy, uz);
228 *roll = Math::atan2(pz, py);
231 void Glue::geodUp(double* pos, float* out)
233 double lat, lon, alt;
234 xyz2geod(pos, &lat, &lon, &alt);
236 float slat = Math::sin(lat);
237 float clat = Math::cos(lat);
238 float slon = Math::sin(lon);
239 float clon = Math::cos(lon);
240 out[0] = clon * clat;
241 out[1] = slon * clat;
245 }; // namespace yasim