#include "Glue.hpp"
namespace yasim {
-void Glue::calcAlphaBeta(State* s, float* alpha, float* beta)
+void Glue::calcAlphaBeta(State* s, float* wind, float* alpha, float* beta)
{
// Convert the velocity to the aircraft frame.
float v[3];
- Math::vmul33(s->orient, s->v, v);
+ Math::sub3(s->v, wind, v);
+ Math::vmul33(s->orient, v, v);
// By convention, positive alpha is an up pitch, and a positive
// beta is yawed to the right.
*pitch = Math::dot3(pitchAxis, s->rot);
}
-void Glue::xyz2geoc(double* xyz, double* lat, double* lon, double* alt)
-{
- double x=xyz[0], y=xyz[1], z=xyz[2];
-
- // Cylindrical radius from the polar axis
- double rcyl = Math::sqrt(x*x + y*y);
-
- // In geocentric coordinates, these are just the angles in
- // cartesian space.
- *lon = Math::atan2(y, x);
- *lat = Math::atan2(z, rcyl);
-
- // To get XYZ coordinate of "ground", we "squash" the cylindric
- // radius into a coordinate system where the earth is a sphere,
- // find the fraction of the xyz vector that is above ground.
- double rsquash = SQUASH * rcyl;
- double frac = POLRAD/Math::sqrt(rsquash*rsquash + z*z);
- double len = Math::sqrt(x*x + y*y + z*z);
-
- *alt = len * (1-frac);
-}
-
-void Glue::geoc2xyz(double lat, double lon, double alt, double* out)
-{
- // Generate a unit vector
- double rcyl = Math::cos(lat);
- double x = rcyl*Math::cos(lon);
- double y = rcyl*Math::sin(lon);
- double z = Math::sin(lat);
-
- // Convert to "squashed" space, renormalize the unit vector,
- // multiply by the polar radius, and back convert to get us the
- // point of intersection of the unit vector with the surface.
- // Then just add the altitude.
- double rtmp = rcyl*SQUASH;
- double renorm = POLRAD/Math::sqrt(rtmp*rtmp + z*z);
- double ztmp = z*renorm;
- rtmp *= renorm*STRETCH;
- double len = Math::sqrt(rtmp*rtmp + ztmp*ztmp);
- len += alt;
-
- out[0] = x*len;
- out[1] = y*len;
- out[2] = z*len;
-}
-
-double Glue::geod2geocLat(double lat)
-{
- double r = Math::cos(lat)*STRETCH*STRETCH;
- double z = Math::sin(lat);
- return Math::atan2(z, r);
-}
-
-double Glue::geoc2geodLat(double lat)
-{
- double r = Math::cos(lat)*SQUASH*SQUASH;
- double z = Math::sin(lat);
- return Math::atan2(z, r);
-}
-
-void Glue::xyz2geod(double* xyz, double* lat, double* lon, double* alt)
-{
- xyz2geoc(xyz, lat, lon, alt);
- *lat = geoc2geodLat(*lat);
-}
-
-void Glue::geod2xyz(double lat, double lon, double alt, double* out)
-{
- lat = geod2geocLat(lat);
- geoc2xyz(lat, lon, alt, out);
-}
-
void Glue::xyz2nedMat(double lat, double lon, float* out)
{
// Shorthand for our output vectors:
float *north = out, *east = out+3, *down = out+6;
- float slat = Math::sin(lat);
- float clat = Math::cos(lat);
- float slon = Math::sin(lon);
- float clon = Math::cos(lon);
+ float slat = (float) Math::sin(lat);
+ float clat = (float)Math::cos(lat);
+ float slon = (float)Math::sin(lon);
+ float clon = (float)Math::cos(lon);
north[0] = -clon * slat;
north[1] = -slon * slat;
// rotation and are done out longhand below for efficiency.
// Init to the identity matrix
- for(int i=0; i<3; i++)
- for(int j=0; j<3; j++)
- out[3*i+j] = (i==j) ? 1 : 0;
+ int i, j;
+ for(i=0; i<3; i++)
+ for(j=0; j<3; j++)
+ out[3*i+j] = (i==j) ? 1.0f : 0.0f;
// Negate Y and Z
out[4] = out[8] = -1;
float s = Math::sin(roll);
float c = Math::cos(roll);
- for(int col=0; col<3; col++) {
+ int col;
+ for(col=0; col<3; col++) {
float y=out[col+3], z=out[col+6];
out[col+3] = c*y - s*z;
out[col+6] = s*y + c*z;
s = Math::sin(pitch);
c = Math::cos(pitch);
- for(int col=0; col<3; col++) {
+ for(col=0; col<3; col++) {
float x=out[col], z=out[col+6];
out[col] = c*x + s*z;
out[col+6] = c*z - s*x;
s = Math::sin(hdg);
c = Math::cos(hdg);
- for(int col=0; col<3; col++) {
+ for(col=0; col<3; col++) {
float x=out[col], y=out[col+3];
out[col] = c*x - s*y;
out[col+3] = s*x + c*y;
*roll = Math::atan2(pz, py);
}
-void Glue::geodUp(double* pos, float* out)
+void Glue::geodUp(double lat, double lon, float* up)
+{
+ double coslat = Math::cos(lat);
+ up[0] = (float)(Math::cos(lon) * coslat);
+ up[1] = (float)(Math::sin(lon) * coslat);
+ up[2] = (float)(Math::sin(lat));
+}
+
+// FIXME: Hardcoded WGS84 numbers...
+void Glue::geodUp(double* pos, float* up)
{
- double lat, lon, alt;
- xyz2geod(pos, &lat, &lon, &alt);
-
- float slat = Math::sin(lat);
- float clat = Math::cos(lat);
- float slon = Math::sin(lon);
- float clon = Math::cos(lon);
- out[0] = clon * clat;
- out[1] = slon * clat;
- out[2] = slat;
+ const double SQUASH = 0.9966471893352525192801545;
+ const double STRETCH = 1.0033640898209764189003079;
+ float x = (float)(pos[0] * SQUASH);
+ float y = (float)(pos[1] * SQUASH);
+ float z = (float)(pos[2] * STRETCH);
+ float norm = 1/Math::sqrt(x*x + y*y + z*z);
+ up[0] = x * norm;
+ up[1] = y * norm;
+ up[2] = z * norm;
}
}; // namespace yasim
projects / flightgear.git / blobdiff