src/Main/ttest.cxx

   1 // This should be ported to plib/sg before you try to compile it.  If
   2 // you do the port please send it to me. :-)
   3
   4 #include <Math/mat3.h>
   5 #include <Include/fg_constants.h>
   6
   7 main() {
   8     MAT3mat R_Phi, R_Theta, R_Psi, R_Lat, R_Lon, T_view;
   9     MAT3mat TMP, AIRCRAFT, WORLD, EYE_TO_WORLD, WORLD_TO_EYE;
  10     MAT3hvec vec, eye, vec1;
  11
  12     double FG_Phi = 0.00;
  13     double FG_Theta = 0.00;
  14     double FG_Psi = 0.00;
  15     // double FG_Latitude = 33.3528917 * SGD_DEGREES_TO_RADIANS;
  16     double FG_Latitude = 0.0;
  17     // double FG_Longitude = -110.6642444 * SGD_DEGREES_TO_RADIANS;
  18     double FG_Longitude = 90.0 * SGD_DEGREES_TO_RADIANS;
  19     // double view_pos[] = {2936.3222, 1736.9243, 3689.5359, 1.0};
  20     double view_pos[] = {0.0, 0.0, 0.0, 1.0};
  21
  22     // Roll Matrix
  23     MAT3_SET_HVEC(vec, 0.0, 0.0, -1.0, 1.0);
  24     MAT3rotate(R_Phi, vec, FG_Phi);
  25     printf("Roll matrix (Phi)\n");
  26     MAT3print(R_Phi, stdout);
  27
  28     // Pitch Matrix
  29     MAT3_SET_HVEC(vec, 1.0, 0.0, 0.0, 1.0);
  30     MAT3rotate(R_Theta, vec, FG_Theta);
  31     printf("\nPitch matrix (Theta)\n");
  32     MAT3print(R_Theta, stdout);
  33
  34     // Yaw Matrix
  35     MAT3_SET_HVEC(vec, 0.0, -1.0, 0.0, 1.0);
  36     MAT3rotate(R_Psi, vec, SGD_PI + FG_Psi);
  37     printf("\nYaw matrix (Psi)\n");
  38     MAT3print(R_Psi, stdout);
  39
  40     // Latitude
  41     MAT3_SET_HVEC(vec, 1.0, 0.0, 0.0, 1.0);
  42     // R_Lat = rotate about X axis
  43     MAT3rotate(R_Lat, vec, FG_Latitude);
  44     printf("\nLatitude matrix\n");
  45     MAT3print(R_Lat, stdout);
  46
  47     // Longitude
  48     MAT3_SET_HVEC(vec, 0.0, 0.0, 1.0, 1.0);
  49     // R_Lon = rotate about Z axis
  50     MAT3rotate(R_Lon, vec, FG_Longitude - SGD_PI_2 );
  51     printf("\nLongitude matrix\n");
  52     MAT3print(R_Lon, stdout);
  53
  54     // View position in scenery centered coordinates
  55     MAT3translate(T_view, view_pos);
  56     printf("\nTranslation matrix\n");
  57     MAT3print(T_view, stdout);
  58
  59     // aircraft roll/pitch/yaw
  60     MAT3mult(TMP, R_Phi, R_Theta);
  61     MAT3mult(AIRCRAFT, TMP, R_Psi);
  62     printf("\naircraft roll pitch yaw\n");
  63     MAT3print(AIRCRAFT, stdout);
  64
  65     // lon/lat
  66     MAT3mult(WORLD, R_Lat, R_Lon);
  67     printf("\nworld\n");
  68     MAT3print(WORLD, stdout);
  69
  70     MAT3mult(EYE_TO_WORLD, AIRCRAFT, WORLD);
  71     MAT3mult(EYE_TO_WORLD, EYE_TO_WORLD, T_view);
  72     printf("\nEye to world\n");
  73     MAT3print(EYE_TO_WORLD, stdout);
  74
  75     MAT3invert(WORLD_TO_EYE, EYE_TO_WORLD);
  76     printf("\nWorld to eye\n");
  77     MAT3print(WORLD_TO_EYE, stdout);
  78
  79     MAT3_SET_HVEC(eye, 0.0, 0.0, 0.0, 1.0);
  80     MAT3mult_vec(vec, eye, EYE_TO_WORLD);
  81     printf("\neye -> world = %.2f %.2f %.2f\n", vec[0], vec[1], vec[2]);
  82
  83     MAT3_SET_HVEC(vec1, 0.0, 6378138.12, 0.0, 1.0);
  84     MAT3mult_vec(vec, vec1, EYE_TO_WORLD);
  85     printf( "\n+y (eye) -> +y (world) = %.2f %.2f %.2f\n",
  86             vec[0], vec[1], vec[2]);
  87
  88     MAT3mult_vec(vec1, vec, WORLD_TO_EYE);
  89     printf( "\n+y (world) -> +y (eye) = %.2f %.2f %.2f\n",
  90             vec1[0], vec1[1], vec1[2]);
  91 }
  92
  93
  94
  95
  96