src/Main/ttest.cxx

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