SHARE
TWEET

Untitled

a guest Jun 27th, 2015 65 Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
  1. // X = Roll  = Bank
  2. // Y = Pitch = Attitude
  3. // Z = Yaw   = Heading
  4.  
  5. enum AxisType {
  6.     X,Y,Z
  7. };
  8.  
  9. static QVector3D toVector3D(AxisType a) {
  10.     if (a == X) {
  11.         return QVector3D(1,0,0);
  12.     } else if (a == Y) {
  13.         return QVector3D(0,1,0);
  14.     } else {
  15.         return QVector3D(0,0,1);
  16.     }
  17. }
  18. static AxisType nextCircular(AxisType a) {
  19.     if (a == X) {
  20.         return Y;
  21.     } else if (a == Y) {
  22.         return Z;
  23.     } else {
  24.         return X;
  25.     }
  26. }
  27. static qreal angle3D(const QVector3D &a, const QVector3D &b) {
  28.     qreal angle = qAcos(QVector3D::dotProduct(a, b) / double(a.length() * b.length()));
  29.     if (isnan(angle))
  30.         return 0;
  31.     return angle;
  32. }
  33.  
  34. struct EulerOrder {
  35.     AxisType axisOrder[3];
  36.     EulerOrder(AxisType first, AxisType second, AxisType third) {
  37.         axisOrder[0] = first;
  38.         axisOrder[1] = second;
  39.         axisOrder[2] = third;
  40.     }
  41.     QList<QVector3D> orderedAxis() const {
  42.         return QList<QVector3D>() << toVector3D(axisOrder[0]) << toVector3D(axisOrder[1]) << toVector3D(axisOrder[2]);
  43.     }
  44.     AxisType getAxisType(int index) const {
  45.         if ((index > 2) || (index < 0)) {
  46.             qDebug() << "EulerOrder[index] called with an invalid index";
  47.         }
  48.  
  49.         return axisOrder[index];
  50.     }
  51.     bool isCircular() const {
  52.         return nextCircular(axisOrder[0]) == axisOrder[1];
  53.     }
  54.     bool isRepeating() const {
  55.         return axisOrder[0] == axisOrder[2];
  56.     }
  57. };
  58.  
  59. class EulerAngleDecomposer {
  60.     struct IndexData {
  61.         // for all of these indices:
  62.         // 0 = X
  63.         // 1 = Y
  64.         // 2 = Z
  65.  
  66.         AxisType m_i1; // zero based index of first euler rotation
  67.         AxisType m_i1n; // next circular index following i1
  68.         AxisType m_i1nn; // next circular index following i1n
  69.  
  70.         AxisType m_i2; // zero based index of second euler rotation
  71.         AxisType m_i2n; // next circular index following i2
  72.         AxisType m_i2nn; // next circular index following i2n
  73.  
  74.         AxisType m_i3; // zero based index of third euler rotation
  75.         AxisType m_i3n; // next circular index following i3
  76.         AxisType m_i3nn; // next circular index following i3n
  77.  
  78.         // m_unitAxis[0] = first euler rotation axis
  79.         // m_unitAxis[1] = second euler rotation axis
  80.         // m_unitAxis[2] = third euler rotation axis
  81.         QList<QVector3D> m_unitAxis;
  82.  
  83.         // create from a EulerOrder
  84.         IndexData(const EulerOrder &order) {
  85.             m_i1 = order.getAxisType(0);
  86.             m_i2 = order.getAxisType(1);
  87.             m_i3 = order.getAxisType(2);
  88.  
  89.             // now populate m_ixn, ans ixnn's
  90.             m_i1n = nextCircular(m_i1);
  91.             m_i1nn = nextCircular(m_i1n);
  92.  
  93.             m_i2n = nextCircular(m_i2);
  94.             m_i2nn = nextCircular(m_i2n);
  95.  
  96.             m_i3n = nextCircular(m_i3);
  97.             m_i3nn = nextCircular(m_i3n);
  98.  
  99.             m_unitAxis = order.orderedAxis();
  100.         }
  101.  
  102.         QVector3D V1() { return m_unitAxis[0]; } // first axis of rotation
  103.         QVector3D V2() { return m_unitAxis[1]; } // second axis of rotation
  104.         QVector3D V3() { return m_unitAxis[2]; } // third axis of rotation
  105.         QVector3D V3n()  { return toVector3D(m_i3n); }
  106.         // next axis after V3 (circular)
  107.         // a little table:
  108.         // V3()     -->     V3n()
  109.         // X        -->     Y
  110.         // Y        -->     Z
  111.         // Z        -->     X
  112.  
  113.         AxisType i1()   { return m_i1; } // first rotation axis
  114.         AxisType i1n()  { return m_i1n; } // next circular axis folowing i1(). not to be confused with the second axis of rotation (i2)
  115.         AxisType i1nn() { return m_i1nn; } // next circular axis following i1n(). not to be confused with the third axis of rotation (i3)
  116.     };
  117. public:
  118.     static QVector3D DecomposeFromQuaternion(const QQuaternion &q, const EulerOrder &order) {
  119.         IndexData d(order);
  120.  
  121.         QVector3D v3Rot = q.rotatedVector(d.V3()).normalized(); // q->GetRotatedVector(d.V3()).unit();
  122.  
  123.         // recall:
  124.         // i1;      // zero based index of first euler rotation
  125.         // i1n;     // next circular index following i1
  126.         // i1nn;    // next circular index following i1n
  127.  
  128.         qreal theta1 = 0;
  129.         qreal theta2 = 0;
  130.         qreal theta3 = 0;
  131.  
  132.         if (order.isRepeating()) {
  133.             if (order.isCircular()) {
  134.                 // circular, repeating
  135.                 theta1 = qAtan2(v3Rot[d.i1n()], -v3Rot[d.i1nn()]);
  136.                 theta2 = -qAcos(v3Rot[d.i1()]);
  137.             } else {
  138.                 // non circular, repeating
  139.                 theta1 = qAtan2(v3Rot[d.i1nn()], v3Rot[d.i1n()]);
  140.                 theta2 = qAcos(v3Rot[d.i1()]);
  141.             }
  142.  
  143.             // By convention, repeating sequences restrict theta2 to 0-->180
  144.             if (theta2 < 0) {
  145.                 // need to resolve the ambiguity restrict theta2 to 0 --> 180
  146.                 //theta2 = theta2.negate();
  147.                 theta2 = -theta2;
  148.                 //theta1 = theta1 - pi;
  149.             }
  150.  
  151.             // special case where theta2 is zero, which is somewhat nonsense for a repeating sequence
  152.             // in this case, put all the entire angle into theta3
  153.             if (theta2 == 0 || theta2 == M_PI) {
  154.                 theta1 = 0;
  155.             }
  156.         } else {
  157.             if (order.isCircular()) {
  158.                 // circular, non-repeating
  159.                 theta1 = qAtan2(-v3Rot[d.i1n()], v3Rot[d.i1nn()]);
  160.                 theta2 = -qAsin(-v3Rot[d.i1()]);
  161.             } else {
  162.                 // non circular, non repeating
  163.                 theta1 = qAtan2(v3Rot[d.i1nn()], v3Rot[d.i1n()]);
  164.                 theta2 = -qAsin(v3Rot[d.i1()]);
  165.             }
  166.         }
  167.  
  168.         // Create the Q12 quaternion using the first two axis and angles
  169.         QQuaternion Q1 = QQuaternion::fromAxisAndAngle(d.V1(), qRadiansToDegrees(theta1));
  170.         QQuaternion Q2 = QQuaternion::fromAxisAndAngle(d.V2(), qRadiansToDegrees(theta2));
  171.  
  172.         QQuaternion Q12 = Q1 * Q2;
  173.  
  174.         // get the next circular vector after V3
  175.         QVector3D V3n = d.V3n();
  176.  
  177.         // rotate V3n through Q12 and q
  178.         QVector3D V3n12 = Q12.rotatedVector(V3n).normalized();
  179.         QVector3D V3nG = q.rotatedVector(V3n).normalized();
  180.  
  181.         // get the angle between them - theta3
  182.         theta3 = angle3D(V3n12, V3nG);
  183.  
  184.         // use a cross product to determine the direction of the angle
  185.         QVector3D Vc = QVector3D::crossProduct(V3n12, V3nG).normalized();
  186.  
  187.         qreal sign = QVector3D::dotProduct(Vc, v3Rot) > 0 ? 1.0 : -1.0;
  188.  
  189.         theta3 = sign * qAbs(theta3);
  190.  
  191.         return QVector3D(qRadiansToDegrees(theta1), qRadiansToDegrees(theta2), qRadiansToDegrees(theta3));
  192.     }
  193. };
  194.  
  195. QQuaternion log(const QQuaternion &q) {
  196.     qreal a = qAcos(q.scalar());
  197.     qreal sina = qSin(a);
  198.     QQuaternion ret;
  199.  
  200.     ret.setScalar(0);
  201.     if (sina > 0) {
  202.         ret.setX(a * q.x() / sina);
  203.         ret.setY(a * q.y() / sina);
  204.         ret.setZ(a * q.z() / sina);
  205.     } else {
  206.         ret.setX(0);
  207.         ret.setY(0);
  208.         ret.setZ(0);
  209.     }
  210.     return ret;
  211. }
  212.  
  213. QQuaternion exp(const QQuaternion &q) {
  214.     qreal a = q.vector().length();
  215.     qreal sina = qSin(a);
  216.     qreal cosa = qCos(a);
  217.     QQuaternion ret;
  218.  
  219.     ret.setScalar(cosa);
  220.     if (a > 0) {
  221.         ret.setX(sina * q.x() / a);
  222.         ret.setY(sina * q.y() / a);
  223.         ret.setZ(sina * q.z() / a);
  224.     } else {
  225.         ret.setX(0);
  226.         ret.setY(0);
  227.         ret.setZ(0);
  228.     }
  229.     return ret;
  230. }
  231.  
  232. bool closeEnough(const float& a, const float& b, const float& epsilon = std::numeric_limits<float>::epsilon()) {
  233.     return (epsilon > std::abs(a - b));
  234. }
  235.  
  236. qreal clampF(qreal f) {
  237.     return qBound<qreal>(-1.0, f, 1.0);
  238. }
  239.  
  240. QVector3D eulerAngles(const QMatrix3x3 &R, float prevX) {
  241.     //check for gimbal lock
  242.     if (closeEnough(clampF(R(0, 2)), -1.0f)) {
  243.         float x = prevX; //gimbal lock, value of x doesn't matter
  244.         float y = M_PI / 2;
  245.         float z = x + qAtan2(clampF(R(1, 0)), clampF(R(2, 0)));
  246.         return { x, y, z };
  247.     } else if (closeEnough(clampF(R(0, 2)), 1.0f)) {
  248.         float x = prevX;
  249.         float y = -M_PI / 2;
  250.         float z = -x + qAtan2(clampF(-R(1, 0)), clampF(-R(2, 0)));
  251.         return { x, y, z };
  252.     } else { //two solutions exist
  253.         float x1 = -qAsin(clampF(R(0, 2)));
  254.         float x2 = M_PI - x1;
  255.  
  256.         qDebug() << prevX << x1 << x2;
  257.         if (qAbs(x1 - prevX) > qAbs(x2 - prevX)) {
  258.             x1 = x2;
  259.         }
  260.  
  261.         qreal cx1 = qCos(x1);
  262.         qreal cx2 = qCos(x2);
  263.  
  264.         float y1 = qAtan2(clampF(R(1, 2)) / cx1, clampF(R(2, 2)) / cx1);
  265.         float y2 = qAtan2(clampF(R(1, 2)) / cx2, clampF(R(2, 2)) / cx2);
  266.  
  267.         float z1 = qAtan2(clampF(R(0, 1)) / cx1, clampF(R(0, 0)) / cx1);
  268.         float z2 = qAtan2(clampF(R(0, 1)) / cx2, clampF(R(0, 0)) / cx2);
  269.  
  270.         //return { qMin(x1, x2), qMin(y1, y2), qMin(z1, z2) };
  271.  
  272.         //choose one solution to return
  273.         //for example the "shortest" rotation
  274.         if ((std::abs(x1) + std::abs(y1) + std::abs(z1)) <= (std::abs(x2) + std::abs(y2) + std::abs(z2))) {
  275.             return { x1, y1, z1 };
  276.         } else {
  277.             return { x2, y2, z2 };
  278.         }
  279.     }
  280. }
  281. QVector3D eulerAngles(const QMatrix3x3 &R) {
  282.     // Assuming the angles are in radians.
  283.     if (R(1, 0) > 0.998) { // singularity at north pole
  284.         return { atan2(clampF(R(0, 2)), clampF(R(2, 2))),
  285.                  M_PI/2,
  286.                  0
  287.         };
  288.     }
  289.     if (R(1, 0) < -0.998) { // singularity at south pole
  290.         return { atan2(clampF(R(0, 2)), clampF(R(2, 2))),
  291.                  -M_PI / 2,
  292.                  0
  293.         };
  294.     }
  295.     return { atan2(clampF(-R(2, 0)), clampF(R(0, 0))),
  296.              atan2(clampF(-R(1, 2)), clampF(R(1, 1))),
  297.              asin(clampF(R(1, 0))) };
  298. }
  299.  
  300. QVector3D eulerAngles(const QMatrix3x3 &R) {
  301.     float Psi, Theta, Phi;
  302.  
  303.     if (fabs(R(2, 2)) > .999999999) {
  304.         Theta = (clampF(R(2, 2)) > 0) ? 0 : M_PI;
  305.         Psi = 0;
  306.         if (fabs(clampF(R(0, 0))) > .999999999)
  307.         Phi = (clampF(R(0, 0)) > 0) ? 0 : M_PI;
  308.         else Phi = (clampF(R(1, 0)) > 0) ? acos(clampF(R(0, 0))) : - acos(clampF(R(0, 0)));
  309.     } else {
  310.         Theta = acos(clampF(R(2, 2)));
  311.         double st = sin(Theta);
  312.         double si = clampF(R(0, 2)) / st;
  313.         double co = - clampF(R(1, 2)) / st;
  314.         if (fabs(co) > .999999999)
  315.             Psi = (co > 0) ? 0 : M_PI;
  316.         else
  317.             Psi = (si > 0) ? acos(co) : - acos(co);
  318.         si = clampF(R(2, 0)) / st;
  319.         co = clampF(R(2, 1)) / st;
  320.         if (fabs(co) > .999999999)
  321.             Phi = (co > 0) ? 0 : M_PI;
  322.         else
  323.             Phi = (si > 0) ? acos(co) : - acos(co);
  324.     }
  325.     return { Theta, Phi, Psi };
  326. }
  327.  
  328. QVector3D eulerAngles(const QMatrix3x3 &R) {
  329.     QVector3D ret;
  330.     float x(0), y(0), z(0);
  331.  
  332.     const int order = 3;
  333.     switch (order) {
  334.     case 1: { // XYZ
  335.           y = qAsin(R(0, 2));
  336.           if (y < M_PI_2) {
  337.              if(y > -M_PI_2) {
  338.                 x = qAtan2(-R(1,2), R(2,2));
  339.                 z = qAtan2(-R(0,1), R(0,0));
  340.              } else { // Non-unique (x - z constant)
  341.                 x = -qAtan2(R(1,0), R(1,1));
  342.                 z = 0;
  343.              }
  344.           } else {
  345.              // not unique (x + z constant)
  346.              x = qAtan2(R(1,0), R(1,1));
  347.              z = 0;
  348.           }
  349.           ret = { x, y, z };
  350.        } break;
  351.     case 2: { // ZYX
  352.           y = qAsin(-R(2, 0));
  353.           if (y < M_PI_2) {
  354.              if (y > -M_PI_2) {
  355.                 z = qAtan2(R(1, 0), R(0, 0));
  356.                 x = qAtan2(R(2, 1), R(2, 2));
  357.              } else { // not unique (x + z constant)
  358.                 z = qAtan2(-R(0, 1),-R(0, 2));
  359.                 x = 0;
  360.              }
  361.           } else { // not unique (x - z constant)
  362.              z = -qAtan2(R(0, 1), R(0, 2));
  363.              x = 0;
  364.           }
  365.           ret = { z, y, x };
  366.        } break;
  367.     case 3: { // ZXY
  368.           x = qAsin(R(2, 1));
  369.           if (x < M_PI_2) {
  370.              if (x > -M_PI_2) {
  371.                 z = qAtan2(-R(0, 1), R(1, 1));
  372.                 y = qAtan2(-R(2, 0), R(2, 2));
  373.              } else { // not unique (y - z constant)
  374.                 z = -qAtan2(R(0, 2), R(0, 0));
  375.                 y = 0;
  376.              }
  377.           } else { // not unique (y + z constant)
  378.              z = qAtan2(R(0, 2), R(0, 0));
  379.              y = 0;
  380.           }
  381.           ret = { z, x, y };
  382.        } break;
  383.     }
  384.     return ret;
  385. }
  386.  
  387.  
  388. int FindMinAbsIndex(float list[2]) {
  389.    double x = qAbs(list[0]);
  390.    int index = 0;
  391.    for (int i = 1; i < 2; i++) {
  392.        if (qAbs(list[i]) < x) {
  393.            index = i;
  394.            x = qAbs(list[i]);
  395.        }
  396.    }
  397.    return index;
  398. }
  399.  
  400. QVector3D GetEulerZXZ(const QMatrix3x3 &R) {
  401.     // Options
  402.     float a_list[2] = { qAtan2(-R(0, 2), R(1, 2)), qAtan2(R(0, 2), -R(1, 2)) };
  403.     float c_list[2] = { qAtan2(R(2, 0), R(2, 1)), qAtan2(-R(2, 0), -R(2, 1)) };
  404.     float b_list[2] = { M_PI/2.0-qAsin(R(2, 2)), -M_PI/2.0+qAsin(R(2, 2)) };
  405.  
  406.     int min_a_index = FindMinAbsIndex(a_list);
  407.     int min_b_index = FindMinAbsIndex(b_list);
  408.     int min_c_index = FindMinAbsIndex(c_list);
  409.  
  410.     return { a_list[min_a_index],
  411.              b_list[min_b_index],
  412.              c_list[min_c_index]
  413.     };
  414. }
  415. QVector3D GetEulerZYX(const QMatrix3x3 &R) {
  416.    float a_list[2] = { qAtan2(R(1, 0), R(0, 0)), qAtan2(-R(1, 0), -R(0, 0)) };
  417.    float c_list[2] = { qAtan2(R(2, 1), R(2, 2)), qAtan2(-R(2, 1), -R(2, 2)) };
  418.    float b_list[2] = { -qAsin(R(2, 0)), qAsin(R(2, 0)) - M_PI };
  419.  
  420.     int min_a_index = FindMinAbsIndex(a_list);
  421.     int min_b_index = FindMinAbsIndex(b_list);
  422.     int min_c_index = FindMinAbsIndex(c_list);
  423.  
  424.     return { a_list[min_a_index],
  425.              b_list[min_b_index],
  426.              c_list[min_c_index]
  427.     };
  428. }
  429.  
  430. double angle3D(const QVector3D &a, const QVector3D &b) {
  431.     // Store some information about them for below
  432.     double dot = QVector3D::dotProduct(a, b);
  433.     double lengthA = a.length();
  434.     double lengthB = b.length();
  435.  
  436.     // Now to find the angle
  437.     return qAcos( dot / (lengthA * lengthB) ); // Theta = 3.06 radians or 175.87 degrees
  438. }
  439.  
  440. double angleFromDirection(const QVector3D &upDirection, const QVector3D &lookDirection, QVector3D &Yproj) {
  441.     Yproj = QVector3D(   -(lookDirection.y() * lookDirection.x()),
  442.                       1 - (lookDirection.y() * lookDirection.y()),
  443.                          -(lookDirection.y() * lookDirection.z()));
  444.     Yproj.normalize();
  445.  
  446.     // get absolute angle between Y projected and Up
  447.     double absAngle = angle3D(upDirection, Yproj);
  448.  
  449.     // magic formula
  450.     QVector3D cross = QVector3D::crossProduct(upDirection, Yproj);
  451.     double dot = QVector3D::dotProduct(lookDirection, cross);
  452.  
  453.     // set actual signed angle
  454.     return qRadiansToDegrees(((dot >= 0) ? absAngle : -absAngle));
  455. }
  456.  
  457. double angleFromDirection2(const QVector3D &a, const QVector3D &b, const QVector3D &ref) {
  458.     double absAngle = angle3D(a, b);
  459.  
  460.     QVector3D cross = QVector3D::crossProduct(a, b);
  461.     double dot = QVector3D::dotProduct(cross, ref);
  462.  
  463.     return qRadiansToDegrees(((dot < 0) ? -absAngle : absAngle));
  464. }
  465.  
  466. double angleFromDirection3(const QVector3D &a, const QVector3D &b, const QVector3D &ref) {
  467.     double absAngle = angle3D(a, ref);
  468.  
  469.     QVector3D cross = QVector3D::crossProduct(a, ref);
  470.     double dot = QVector3D::dotProduct(cross, b);
  471.  
  472.     return qRadiansToDegrees(((dot >= 0) ? absAngle : -absAngle));
  473. }
  474. double angleFromDirection3a(const QVector3D &a, const QVector3D &b, const QVector3D &ref) {
  475.     double absAngle = angle3D(a, ref);
  476.  
  477.     QVector3D cross = QVector3D::crossProduct(b, ref);
  478.     double dot = QVector3D::dotProduct(cross, a);
  479.  
  480.     return qRadiansToDegrees(((dot >= 0) ? absAngle : -absAngle));
  481. }
  482.  
  483. double angleFromDirection4(const QVector3D &a, const QVector3D &b) {
  484.     QVector3D ref(b.x(), 0, b.z());
  485.     ref.normalize();
  486.     double absAngle = angle3D(a, b);
  487.  
  488.     QVector3D cross = QVector3D::crossProduct(a, b);
  489.     double dot = QVector3D::dotProduct(cross, ref);
  490.  
  491.     return qRadiansToDegrees(((dot < 0) ? -absAngle : absAngle));
  492. }
  493.  
  494.  
  495. enum RotSeq { zyx, zyz, zxy, zxz, yxz, yxy, yzx, yzy, xyz, xyx, xzy, xzx };
  496.  
  497. void twoaxisrot(double r11, double r12, double r21, double r31, double r32, double res[]){
  498.   res[0] = qRadiansToDegrees(qAtan2( clampF(r11), clampF(r12) ));
  499.   res[1] = qRadiansToDegrees(qAcos ( clampF(r21 )));
  500.   res[2] = qRadiansToDegrees(qAtan2( clampF(r31), clampF(r32) ));
  501. }
  502.  
  503. void threeaxisrot(double r11, double r12, double r21, double r31, double r32, double res[]){
  504.   res[0] = qRadiansToDegrees(qAtan2( clampF(r31), clampF(r32) ));
  505.   res[1] = qRadiansToDegrees(qAsin ( clampF(r21) ));
  506.   res[2] = qRadiansToDegrees(qAtan2( clampF(r11), clampF(r12) ));
  507. }
  508.  
  509. void quaternion2Euler(const QQuaternion& q, double res[], RotSeq rotSeq) {
  510.     switch(rotSeq) {
  511.     case zyx:
  512.       threeaxisrot( 2*(q.x()*q.y() + q.scalar()*q.z()),
  513.                      q.scalar()*q.scalar() + q.x()*q.x() - q.y()*q.y() - q.z()*q.z(),
  514.                     -2*(q.x()*q.z() - q.scalar()*q.y()),
  515.                      2*(q.y()*q.z() + q.scalar()*q.x()),
  516.                      q.scalar()*q.scalar() - q.x()*q.x() - q.y()*q.y() + q.z()*q.z(),
  517.                      res);
  518.       break;
  519.  
  520.     case zyz:
  521.       twoaxisrot( 2*(q.y()*q.z() - q.scalar()*q.x()),
  522.                    2*(q.x()*q.z() + q.scalar()*q.y()),
  523.                    q.scalar()*q.scalar() - q.x()*q.x() - q.y()*q.y() + q.z()*q.z(),
  524.                    2*(q.y()*q.z() + q.scalar()*q.x()),
  525.                   -2*(q.x()*q.z() - q.scalar()*q.y()),
  526.                   res);
  527.       break;
  528.  
  529.     case zxy:
  530.       threeaxisrot( -2*(q.x()*q.y() - q.scalar()*q.z()),
  531.                       q.scalar()*q.scalar() - q.x()*q.x() + q.y()*q.y() - q.z()*q.z(),
  532.                       2*(q.y()*q.z() + q.scalar()*q.x()),
  533.                      -2*(q.x()*q.z() - q.scalar()*q.y()),
  534.                       q.scalar()*q.scalar() - q.x()*q.x() - q.y()*q.y() + q.z()*q.z(),
  535.                       res);
  536.       break;
  537.  
  538.     case zxz:
  539.       twoaxisrot( 2*(q.x()*q.z() + q.scalar()*q.y()),
  540.                   -2*(q.y()*q.z() - q.scalar()*q.x()),
  541.                    q.scalar()*q.scalar() - q.x()*q.x() - q.y()*q.y() + q.z()*q.z(),
  542.                    2*(q.x()*q.z() - q.scalar()*q.y()),
  543.                    2*(q.y()*q.z() + q.scalar()*q.x()),
  544.                    res);
  545.       break;
  546.  
  547.     case yxz:
  548.       threeaxisrot( 2*(q.x()*q.z() + q.scalar()*q.y()),
  549.                      q.scalar()*q.scalar() - q.x()*q.x() - q.y()*q.y() + q.z()*q.z(),
  550.                     -2*(q.y()*q.z() - q.scalar()*q.x()),
  551.                      2*(q.x()*q.y() + q.scalar()*q.z()),
  552.                      q.scalar()*q.scalar() - q.x()*q.x() + q.y()*q.y() - q.z()*q.z(),
  553.                      res);
  554.       break;
  555.  
  556.     case yxy:
  557.       twoaxisrot( 2*(q.x()*q.y() - q.scalar()*q.z()),
  558.                    2*(q.y()*q.z() + q.scalar()*q.x()),
  559.                    q.scalar()*q.scalar() - q.x()*q.x() + q.y()*q.y() - q.z()*q.z(),
  560.                    2*(q.x()*q.y() + q.scalar()*q.z()),
  561.                   -2*(q.y()*q.z() - q.scalar()*q.x()),
  562.                   res);
  563.       break;
  564.  
  565.     case yzx:
  566.       threeaxisrot( -2*(q.x()*q.z() - q.scalar()*q.y()),
  567.                       q.scalar()*q.scalar() + q.x()*q.x() - q.y()*q.y() - q.z()*q.z(),
  568.                       2*(q.x()*q.y() + q.scalar()*q.z()),
  569.                      -2*(q.y()*q.z() - q.scalar()*q.x()),
  570.                       q.scalar()*q.scalar() - q.x()*q.x() + q.y()*q.y() - q.z()*q.z(),
  571.                       res);
  572.       break;
  573.  
  574.     case yzy:
  575.       twoaxisrot( 2*(q.y()*q.z() + q.scalar()*q.x()),
  576.                   -2*(q.x()*q.y() - q.scalar()*q.z()),
  577.                    q.scalar()*q.scalar() - q.x()*q.x() + q.y()*q.y() - q.z()*q.z(),
  578.                    2*(q.y()*q.z() - q.scalar()*q.x()),
  579.                    2*(q.x()*q.y() + q.scalar()*q.z()),
  580.                    res);
  581.       break;
  582.  
  583.     case xyz:
  584.       threeaxisrot( -2*(q.y()*q.z() - q.scalar()*q.x()),
  585.                     q.scalar()*q.scalar() - q.x()*q.x() - q.y()*q.y() + q.z()*q.z(),
  586.                     2*(q.x()*q.z() + q.scalar()*q.y()),
  587.                    -2*(q.x()*q.y() - q.scalar()*q.z()),
  588.                     q.scalar()*q.scalar() + q.x()*q.x() - q.y()*q.y() - q.z()*q.z(),
  589.                     res);
  590.       break;
  591.  
  592.     case xyx:
  593.       twoaxisrot( 2*(q.x()*q.y() + q.scalar()*q.z()),
  594.                   -2*(q.x()*q.z() - q.scalar()*q.y()),
  595.                    q.scalar()*q.scalar() + q.x()*q.x() - q.y()*q.y() - q.z()*q.z(),
  596.                    2*(q.x()*q.y() - q.scalar()*q.z()),
  597.                    2*(q.x()*q.z() + q.scalar()*q.y()),
  598.                    res);
  599.       break;
  600.  
  601.     case xzy:
  602.       threeaxisrot( 2*(q.y()*q.z() + q.scalar()*q.x()),
  603.                      q.scalar()*q.scalar() - q.x()*q.x() + q.y()*q.y() - q.z()*q.z(),
  604.                     -2*(q.x()*q.y() - q.scalar()*q.z()),
  605.                      2*(q.x()*q.z() + q.scalar()*q.y()),
  606.                      q.scalar()*q.scalar() + q.x()*q.x() - q.y()*q.y() - q.z()*q.z(),
  607.                      res);
  608.       break;
  609.  
  610.     case xzx:
  611.       twoaxisrot( 2*(q.x()*q.z() - q.scalar()*q.y()),
  612.                    2*(q.x()*q.y() + q.scalar()*q.z()),
  613.                    q.scalar()*q.scalar() + q.x()*q.x() - q.y()*q.y() - q.z()*q.z(),
  614.                    2*(q.x()*q.z() + q.scalar()*q.y()),
  615.                   -2*(q.x()*q.y() - q.scalar()*q.z()),
  616.                   res);
  617.       break;
  618.     default:
  619.       qDebug() << "Unknown rotation sequence";
  620.       break;
  621.    }
  622. }
  623.  
  624.  
  625. qreal trace(const QMatrix3x3 &m) {
  626.     return m(0,0) + m(1, 1) + m(2,2);
  627. }
  628.  
  629. QMatrix3x3 rotationMatrix(qreal roll, qreal pitch, qreal yaw) {
  630.     /*
  631.     Return the rotation matrix when given the three Euler angles: roll, pitch and yaw.
  632.  
  633.     roll: cw rotation of the body about the x axis of the fixed frame (phi)
  634.     pitch: cw rotation of the body about the y axis of the fixed frame (theta)
  635.     yaw: cw rotation of the body about the z axis of the fixed frame (psi)
  636.     This rotation matrix can be used to map any vector in the moving reference
  637.     frame to the corresponding vector in the fixed frame by pre-multiplying it.
  638.  
  639.     By convention, the fixed z axis points down toward the center of the earth,
  640.     the x axis points North and the y axis points East.
  641.     A positive pitch is upward, a positive roll is to the right, a positive yaw is a right turn.
  642.  
  643.     >>> T = rotationMatrix(0,0,0)
  644.     >>> import sys
  645.     >>> abs(T-eye(3)) < EPS
  646.     array([[ True,  True,  True],
  647.            [ True,  True,  True],
  648.            [ True,  True,  True]], dtype=bool)
  649.  
  650.     >>> T = rotationMatrix(pi/2,pi/2,pi/2)
  651.     >>> abs(T-array([[0.,0,1],[0,1,0],[-1,0,0]])) < EPS
  652.     array([[ True,  True,  True],
  653.            [ True,  True,  True],
  654.            [ True,  True,  True]], dtype=bool)
  655.  
  656.     >>> T = rotationMatrix(pi/4,0,0)
  657.     >>> T
  658.     array([[ 1.        ,  0.        ,  0.        ],
  659.            [ 0.        ,  0.70710678, -0.70710678],
  660.            [-0.        ,  0.70710678,  0.70710678]])
  661.     >>> abs(T-array([[1.,0,0],[0,sqrt(2)/2,-sqrt(2)/2],[0,sqrt(2)/2,sqrt(2)/2]])) < EPS
  662.     array([[ True,  True,  True],
  663.            [ True,  True,  True],
  664.            [ True,  True,  True]], dtype=bool)
  665.  
  666.     */
  667.     QMatrix3x3 T;
  668.     T(0,0)=cos(pitch)*cos(yaw);
  669.     T(1,0)=cos(pitch)*sin(yaw);
  670.     T(2,0)=-sin(pitch);
  671.     T(0,1)=-cos(roll)*sin(yaw) + sin(roll)*sin(pitch)*cos(yaw);
  672.     T(1,1)=cos(roll)*cos(yaw) + sin(roll)*sin(pitch)*sin(yaw);
  673.     T(2,1)=sin(roll)*cos(pitch);
  674.     T(0,2)=sin(roll)*sin(yaw) + cos(roll)*sin(pitch)*cos(yaw);
  675.     T(1,2)=-sin(roll)*cos(yaw) + cos(roll)*sin(pitch)*sin(yaw);
  676.     T(2,2)=cos(roll)*cos(pitch);
  677.     return T;
  678. }
  679.  
  680. QVector4D eulerParameters(const QMatrix3x3 &T) {
  681.     /*
  682.     Given a rotation matrix, compute the four Euler parameters.
  683.  
  684.     These parameters represent a 4-D vector that defines an axis such that if the
  685.     xyz axes of the moving frame are rotated about it by an angle phi they will
  686.     line up with the XYZ of the fixed frame and vice-versa.
  687.  
  688.     See: http://www.u.arizona.edu/~pen/ame553/Notes/Lesson%2009.pdf
  689.  
  690.     >>> es = eulerParameters(eye(3))
  691.     >>> abs(array(es)-array([1.,0,0,0])) < EPS
  692.     array([ True,  True,  True,  True], dtype=bool)
  693.  
  694.     >>> T = rotationMatrix(pi/4,0,0)
  695.     >>> es = eulerParameters(T)
  696.     >>> abs(array(es)-array((0.92387953251128674, 0.38268343236508973, 0.0, 0.0))) < EPS
  697.     array([ True,  True,  True,  True], dtype=bool)
  698.     */
  699.     qreal tra = trace(T);
  700.     // take the positive square roots as a starting point
  701.     qreal e0 = sqrt(tra + 1.)/2.;
  702.     qreal e1 = sqrt(1+2*T(0,0)-tra)/2.;
  703.     qreal e2 = sqrt(1+2*T(1,1)-tra)/2.;
  704.     qreal e3 = sqrt(1+2*T(2,2)-tra)/2.;
  705.     QList<qreal> es;
  706.     es << e0;
  707.     es << e1;
  708.     es << e2;
  709.     es << e3;
  710.     int eimax = es.indexOf(qMax(qMax(qMax(e0, e1), e2), e3));
  711.  
  712.     // for best numerical stability, take the largest parameter as positive
  713.     // and use it to re-compute the others with correct signs
  714.     if (eimax == 0) {
  715.         e1 = (T(2,1)-T(1,2))/4/e0;
  716.         e2 = (T(0,2)-T(2,0))/4/e0;
  717.         e3 = (T(1,0)-T(0,1))/4/e0;
  718.     } else if (eimax == 1) {
  719.         e0 = (T(2,1)-T(1,2))/4/e1;
  720.         e2 = (T(1,0)+T(0,1))/4/e1;
  721.         e3 = (T(0,2)+T(2,0))/4/e1;
  722.     } else if (eimax == 2) {
  723.         e0 = (T(0,2)-T(2,0))/4/e2;
  724.         e1 = (T(1,0)+T(0,1))/4/e2;
  725.         e3 = (T(2,1)+T(1,2))/4/e2;
  726.     } else {
  727.         e0 = (T(1,0)-T(0,1))/4/e3;
  728.         e1 = (T(0,2)+T(2,0))/4/e3;
  729.         e2 = (T(2,1)+T(1,2))/4/e3;
  730.     }
  731.     return QVector4D(e0,e1,e2,e3);
  732. }
  733.  
  734. QVector3D eulerAngles1(const QVector4D &es) {
  735.     /*
  736.     Compute the roll, pitch and yaw from a tuple of the four Euler parameters.
  737.  
  738.     Note: this function is not intended to be used at the singular points
  739.     where pitch = +/- pi/2.  In general, you should use getEulerAngles() instead.
  740.     That function will handle the singular cases and call this function in the
  741.     non-singular cases
  742.  
  743.     >>> angles = (pi/6,pi/2-2*EPS,-pi/4)
  744.     >>> T = rotationMatrix(*angles)
  745.     >>> es = eulerParameters(T)
  746.     >>> newangles = eulerAngles(es)
  747.     Traceback (most recent call last):
  748.     ...
  749.     AssertionError
  750.  
  751.     >>> angles = (pi/6,pi/2-5*EPS,-pi/4)
  752.     >>> T = rotationMatrix(*angles)
  753.     >>> es = eulerParameters(T)
  754.     >>> newangles = eulerAngles(es)
  755.     >>> err = abs(array(angles)-array(newangles))
  756.     >>> err[0] < .03
  757.     True
  758.     >>> err[1] < 1.e-7
  759.     True
  760.     >>> err[2] < .06
  761.     True
  762.     */
  763.     // http://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles
  764.     // note arc
  765.     qreal e0 = es.x();
  766.     qreal e1 = es.y();
  767.     qreal e2 = es.z();
  768.     qreal e3 = es.w();
  769.  
  770.     float EPS = std::numeric_limits<float>::epsilon();
  771.  
  772.     qreal dy_roll = 2.0*(e0*e1 + e2*e3);
  773.     qreal dx_roll = (1.0 - 2.0*(e1*e1 + e2*e2));
  774.     qreal dy_yaw = 2.0*(e0*e3 + e1*e2);
  775.     qreal dx_yaw = (1.0 - 2.0*(e2*e2 + e3*e3));
  776.     // if both the dx and dy are tiny, the arctan is undefined.  Force it to zero to avoid numerical issues.
  777.     // This can happen when the pitch is +/- pi/2 and the roll and yaw are both even or odd multiples of pi.
  778.     // But forcing
  779.     Q_ASSERT ((qAbs(dx_roll) > EPS || qAbs(dy_roll) > EPS) && (qAbs(dx_yaw) > EPS || qAbs(dy_yaw) > EPS));
  780.  
  781.     qreal roll  = qAbs(dx_roll) > EPS || qAbs(dy_roll) > EPS? qAtan2(dy_roll, dx_roll) : 0.;
  782.     qreal pitch = qAsin(2.0*(e0*e2 - e3*e1));
  783.     qreal yaw   = qAbs(dx_yaw) > EPS || qAbs(dy_yaw) > EPS? qAtan2(dy_yaw, dx_yaw) : 0.;
  784.  
  785.     return QVector3D(roll, pitch, yaw);
  786. }
  787.  
  788. QVector3D getEulerAngles(const QMatrix3x3 &T) {
  789.     /*
  790.     Compute the roll, pitch and yaw from a rotation matrix.
  791.  
  792.     That function handles the singular cases directly and calls eulerParameters()
  793.     for non-singular cases
  794.  
  795.     >>> angles = (pi/6,pi/2-2*EPS,-pi/4)
  796.     >>> T = rotationMatrix(*angles)
  797.     >>> newangles = getEulerAngles(T)
  798.     >>> abs(angles[0]-angles[2]-newangles[0]) < EPS
  799.     True
  800.     >>> abs(angles[1]-newangles[1]) < 5*EPS
  801.     True
  802.  
  803.     >>> angles = (pi/6,pi/2-5*EPS,-pi/4)
  804.     >>> T = rotationMatrix(*angles)
  805.     >>> es = eulerParameters(T)
  806.     >>> newangles = eulerAngles(es)
  807.     >>> err = abs(array(angles)-array(newangles))
  808.     >>> err[0] < .03
  809.     True
  810.     >>> err[1] < 1.e-7
  811.     True
  812.     >>> err[2] < .06
  813.     True
  814.     */
  815.     float EPS = std::numeric_limits<float>::epsilon();
  816.  
  817.     if (qAbs(T(0,0)) < 4*EPS && qAbs(T(1,0)) < 4*EPS && qAbs(T(2,1)) < 4*EPS && qAbs(T(2,2)) < 4*EPS) {
  818.         qreal p, y, r;
  819.         if (T(2,0) < 0.) {
  820.             p = M_PI/2;
  821.             y = 0;
  822.             r = qAtan2(T(0,1),T(0,2));
  823.         } else {
  824.             p = -M_PI/2;
  825.             y = 0;
  826.             r = qAtan2(-T(0,1),-T(0,2));
  827.         }
  828.         return QVector3D(r, p, y);
  829.     } else {
  830.         QVector4D ep = eulerParameters(T);
  831.         return eulerAngles1(ep);
  832.     }
  833. }
  834.  
  835.  
  836.  
  837. QVector3D correctedEulerAngles(const QVector3D &angles, const QMatrix3x3 &Tc) {
  838.     /*
  839.     Compute the corrected Euler angles from measured values and the correction matrix
  840.  
  841.     Given a set of Euler angles (e.g. measurement data from the box)
  842.     and a correction matrix Tc, which takes coordinates from the plane to the box,
  843.     calculate a set of corrected Euler angles which represent the true roll, pitch
  844.     and yaw of the airplane.
  845.  
  846.     >>> Tc = getCorrectionMatrix(.5, .5, .5)
  847.     >>> ea = correctedEulerAngles(.9, -.2, .6, Tc)
  848.     >>> abs(array(ea) - array((0.73182631479752158, -0.35738772904903454, -0.10266899093274005))) < EPS
  849.     array([ True,  True,  True], dtype=bool)
  850.  
  851.     */
  852.     // get the matrix which maps from the box to the world system (measured)
  853.     QMatrix3x3 Tk = rotationMatrix(angles.x(), angles.y(), angles.z());
  854.  
  855.     // T0 maps from the plane to the box system so multiplying takes us from plane to world.
  856.     QMatrix3x3 T1 = Tk * Tc;
  857.  
  858.     QVector4D es = eulerParameters(T1);
  859.     QVector3D result = eulerAngles1(es);
  860.     return result;
  861. }
  862.  
  863.  
  864.  
  865. QVector3D xAxis(const QQuaternion &q)   {
  866.     qreal fTx  = 2.0f*q.x();
  867.     qreal fTy  = 2.0f*q.y();
  868.     qreal fTz  = 2.0f*q.z();
  869.     qreal fTwy = fTy*q.scalar();
  870.     qreal fTwz = fTz*q.scalar();
  871.     qreal fTxy = fTy*q.x();
  872.     qreal fTxz = fTz*q.x();
  873.     qreal fTyy = fTy*q.y();
  874.     qreal fTzz = fTz*q.z();
  875.  
  876.     return QVector3D(1.0f-(fTyy+fTzz), fTxy+fTwz, fTxz-fTwy);
  877. }
  878.  
  879. QVector3D projectedX(const QVector3D &q)   {
  880.     qreal fTxy = 2.0f*q.y()*q.x();
  881.     qreal fTxz = 2.0f*q.z()*q.x();
  882.     qreal fTyy = 2.0f*q.y()*q.y();
  883.     qreal fTzz = 2.0f*q.z()*q.z();
  884.  
  885.     return QVector3D(1.0f-(fTyy+fTzz), fTxy, fTxz);
  886. }
  887. //-----------------------------------------------------------------------
  888. QVector3D yAxis(const QQuaternion &q)   {
  889.     qreal fTx  = 2.0f*q.x();
  890.     qreal fTy  = 2.0f*q.y();
  891.     qreal fTz  = 2.0f*q.z();
  892.     qreal fTwx = fTx*q.scalar();
  893.     qreal fTwz = fTz*q.scalar();
  894.     qreal fTxx = fTx*q.x();
  895.     qreal fTxy = fTy*q.x();
  896.     qreal fTyz = fTz*q.y();
  897.     qreal fTzz = fTz*q.z();
  898.  
  899.     return QVector3D(fTxy-fTwz, 1.0f-(fTxx+fTzz), fTyz+fTwx);
  900. }
  901. QVector3D projectedY(const QVector3D &q)   {
  902.     qreal fTxx = 2.0f*q.x()*q.x();
  903.     qreal fTxy = 2.0f*q.y()*q.x();
  904.     qreal fTyz = 2.0f*q.z()*q.y();
  905.     qreal fTzz = 2.0f*q.z()*q.z();
  906.  
  907.     return QVector3D(fTxy, 1.0f-(fTxx+fTzz), fTyz);
  908. }
  909. //-----------------------------------------------------------------------
  910. QVector3D zAxis(const QQuaternion &q) {
  911.     qreal fTx  = 2.0f*q.x();
  912.     qreal fTy  = 2.0f*q.y();
  913.     qreal fTz  = 2.0f*q.z();
  914.     qreal fTwx = fTx*q.scalar();
  915.     qreal fTwy = fTy*q.scalar();
  916.     qreal fTxx = fTx*q.x();
  917.     qreal fTxz = fTz*q.x();
  918.     qreal fTyy = fTy*q.y();
  919.     qreal fTyz = fTz*q.y();
  920.  
  921.     return QVector3D(fTxz+fTwy, fTyz-fTwx, 1.0f-(fTxx+fTyy));
  922. }
  923. QVector3D projectedZ(const QVector3D &q) {
  924.     qreal fTxx = 2.0f*q.x()*q.x();
  925.     qreal fTxz = 2.0f*q.z()*q.x();
  926.     qreal fTyy = 2.0f*q.y()*q.y();
  927.     qreal fTyz = 2.0f*q.z()*q.y();
  928.  
  929.     return QVector3D(fTxz, fTyz, 1.0f-(fTxx+fTyy));
  930. }
  931. qreal getRoll(const QQuaternion &q, bool reprojectAxis) {
  932.     if (reprojectAxis)    {
  933.         // roll = atan2(localx.y, localx.x)
  934.         // pick parts of xAxis() implementation that we need
  935. //                      Real fTx  = 2.0*x;
  936.         qreal fTy  = 2.0f*q.y();
  937.         qreal fTz  = 2.0f*q.z();
  938.         qreal fTwz = fTz*q.scalar();
  939.         qreal fTxy = fTy*q.x();
  940.         qreal fTyy = fTy*q.y();
  941.         qreal fTzz = fTz*q.z();
  942.  
  943.         // Vector3(1.0-(fTyy+fTzz), fTxy+fTwz, fTxz-fTwy);
  944.  
  945.         return qAtan2(fTxy+fTwz, 1.0f-(fTyy+fTzz));
  946.  
  947.     }    else    {
  948.         return qAtan2(2*(q.x()*q.y() + q.scalar()*q.z()), q.scalar()*q.scalar() + q.x()*q.x() - q.y()*q.y() - q.z()*q.z());
  949.     }
  950. }
  951. //-----------------------------------------------------------------------
  952. qreal getPitch(const QQuaternion &q, bool reprojectAxis) {
  953.     if (reprojectAxis)    {
  954.         // pitch = atan2(localy.z, localy.y)
  955.         // pick parts of yAxis() implementation that we need
  956.         qreal fTx  = 2.0f*q.x();
  957. //                      Real fTy  = 2.0f*y;
  958.         qreal fTz  = 2.0f*q.z();
  959.         qreal fTwx = fTx*q.scalar();
  960.         qreal fTxx = fTx*q.x();
  961.         qreal fTyz = fTz*q.y();
  962.         qreal fTzz = fTz*q.z();
  963.  
  964.         // Vector3(fTxy-fTwz, 1.0-(fTxx+fTzz), fTyz+fTwx);
  965.         return qAtan2(fTyz+fTwx, 1.0f-(fTxx+fTzz));
  966.     }    else    {
  967.         // internal version
  968.         return qAtan2(2*(q.y()*q.z() + q.scalar()*q.x()), q.scalar()*q.scalar() - q.x()*q.x() - q.y()*q.y() + q.z()*q.z());
  969.     }
  970. }
  971. //-----------------------------------------------------------------------
  972. qreal getYaw(const QQuaternion &q, bool reprojectAxis) {
  973.     if (reprojectAxis)    {
  974.         // yaw = atan2(localz.x, localz.z)
  975.         // pick parts of zAxis() implementation that we need
  976.         qreal fTx  = 2.0f*q.x();
  977.         qreal fTy  = 2.0f*q.y();
  978.         qreal fTz  = 2.0f*q.z();
  979.         qreal fTwy = fTy*q.scalar();
  980.         qreal fTxx = fTx*q.x();
  981.         qreal fTxz = fTz*q.x();
  982.         qreal fTyy = fTy*q.y();
  983.  
  984.         // Vector3(fTxz+fTwy, fTyz-fTwx, 1.0-(fTxx+fTyy));
  985.  
  986.         return qAtan2(fTxz+fTwy, 1.0f-(fTxx+fTyy));
  987.  
  988.     }    else    {
  989.         // internal version
  990.         return qAsin(-2*(q.x()*q.z() - q.scalar()*q.y()));
  991.     }
  992. }
  993.  
  994. double angleFromDirection8(const QVector3D &a, const QVector3D &b, const QVector3D &ref) {
  995.     double absAngle = angle3D(a, b);
  996.  
  997.     QVector3D cross = QVector3D::crossProduct(a, b);
  998.     double dot = QVector3D::dotProduct(cross, ref);
  999.  
  1000.     return qRadiansToDegrees(((dot < 0) ? absAngle : absAngle));
  1001. }
  1002.  
  1003.  
  1004. // Decompose rotation by rotation of direction vector and twist, around direction vector composite = without_twist * twist == orientation
  1005. void dir_twist_decomposition(const xxquaternion& orientation, const vector3& default_dir, xxquaternion& dir_rotation, xxquaternion& twist_rotation) {
  1006.     vector3 rotated_dir(orientation.rotate(default_dir));
  1007.     dir_rotation.shortest_arc(default_dir, rotated_dir);
  1008.     twist_rotation = dir_rotation.inverted() * orientation;
  1009. }
  1010. void dir_twist_decomposition_2(const xxquaternion& orientation, const vector3& default_dir, xxquaternion& dir_rotation, xxquaternion& twist_rotation) {
  1011.     vector3 rotation_axis(orientation.x, orientation.y, orientation.z); // return projection v1 on to v2 (parallel component)
  1012.     // here can be optimized if default_dir is unit
  1013.     vector3 proj = projection(rotation_axis, default_dir);
  1014.     twist_rotation.set(proj.x, proj.y, proj.z, orientation.w);
  1015.     twist_rotation.normalize();
  1016.     dir_rotation = orientation * twist_rotation.inverted();
  1017. }*/
  1018. /*
  1019.    Decompose the rotation on to 2 parts.
  1020.    1. Twist - rotation around the "direction" vector
  1021.    2. Swing - rotation around axis that is perpendicular to "direction" vector
  1022.    The rotation can be composed back by
  1023.    rotation = swing * twist
  1024.  
  1025.    has singularity in case of swing_rotation close to 180 degrees rotation.
  1026.    if the input quaternion is of non-unit length, the outputs are non-unit as well
  1027.    otherwise, outputs are both unit
  1028. */
  1029. void swing_twist_decomposition(const QQuaternion &rotation, const QVector3D &direction, QQuaternion &swing, QQuaternion &twist) {
  1030.     QVector3D ra(rotation.x(), rotation.y(), rotation.z()); // rotation axis
  1031.     QVector3D p = (QVector3D::dotProduct(ra, direction) * direction); //projection(ra, direction); // return projection v1 on to v2  (parallel component)
  1032.  
  1033.     twist = QQuaternion(rotation.scalar(), p.x(), p.y(), p.z()).normalized();
  1034.     swing = rotation * twist.conjugated();
  1035. }
  1036. void toTwistSwing(const QQuaternion &q, qreal &tw, qreal &sx, qreal &sy) {
  1037.     // First test if the swing is in the singularity:
  1038.     if (qFuzzyIsNull(q.z()) && qFuzzyIsNull(q.scalar())) { sx = sy = M_PI; tw = 0; return; }
  1039.     qreal qw, qx, qy, qz;
  1040.  
  1041.     if (q.scalar() < 0) {
  1042.         qw = -q.scalar();
  1043.         qx = -q.x();
  1044.         qy = -q.y();
  1045.         qz = -q.z();
  1046.     } else {
  1047.         qw = q.scalar();
  1048.         qx = q.x();
  1049.         qy = q.y();
  1050.         qz = q.z();
  1051.     }
  1052.  
  1053.     qreal t = atan2(qz, qw);
  1054.     qreal s = atan2(sqrt(qx*qx+qy*qy), sqrt(qz*qz+qw*qw));
  1055.     qreal x=0.0, y=0.0, sins=sin(s);
  1056.  
  1057.     if (!qFuzzyIsNull(sins)) {
  1058.         qreal sint = sin(t);
  1059.         qreal cost = cos(t);
  1060.         y = (-qx*sint + qy*cost)/sins;
  1061.         x = ( qx*cost + qy*sint)/sins;
  1062.     }
  1063.  
  1064.     tw = qRadiansToDegrees(2.0*t);
  1065.     sx = qRadiansToDegrees(2.0*x*s);
  1066.     sy = qRadiansToDegrees(2.0*y*s);
  1067. }
  1068.  
  1069. void toSwingTwist(const QQuaternion &q, qreal &sx, qreal &sy, qreal &tw) {
  1070.     if (qFuzzyIsNull(q.z()) && qFuzzyIsNull(q.scalar())) { sx = sy = M_PI; tw = 0; return; }
  1071.     qreal qw, qx, qy, qz;
  1072.  
  1073.     if (q.scalar() < 0) {
  1074.         qw = -q.scalar();
  1075.         qx = -q.x();
  1076.         qy = -q.y();
  1077.         qz = -q.z();
  1078.     } else {
  1079.         qw = q.scalar();
  1080.         qx = q.x();
  1081.         qy = q.y();
  1082.         qz = q.z();
  1083.     }
  1084.  
  1085.     // Get the twist t:
  1086.     qreal t = 2.0 * atan2(qz,qw);
  1087.  
  1088.     qreal bet = atan2(sqrt(qx*qx+qy*qy), sqrt(qz*qz+qw*qw));
  1089.     qreal gam = t/2.0;
  1090.     qreal sinc = qFuzzyIsNull(bet)? 1.0 : sin(bet) / bet;
  1091.     qreal singam = sin(gam);
  1092.     qreal cosgam = cos(gam);
  1093.  
  1094.     sx = qRadiansToDegrees((2.0/sinc) * (cosgam*qx - singam*qy));
  1095.     sy = qRadiansToDegrees((2.0/sinc) * (singam*qx + cosgam*qy));
  1096.     tw = qRadiansToDegrees(t);
  1097. }
  1098.  
  1099. qreal len2(qreal x, qreal y, qreal z, qreal w) {
  1100.     return x * x + y * y + z * z + w * w;
  1101. }
  1102. qreal getAngleAround(const QQuaternion &q, const QVector3D &axis) {
  1103.     qreal d = QVector3D::dotProduct(q.vector(), axis);
  1104.     qreal l2 = len2(axis.x() * d, axis.y() * d, axis.z() * d, q.scalar());
  1105.     return qRadiansToDegrees(qFuzzyIsNull(l2) ? 0 : (2.0 * qAcos(qBound<qreal>(-1, (q.scalar() / qSqrt(l2)), 1))));
  1106. }
  1107.  
  1108. static QQuaternion OrthoX = QQuaternion::fromAxisAndAngle(1, 0, 0, 90); // X
  1109. static QQuaternion OrthoY = QQuaternion::fromAxisAndAngle(0, 1, 0, 90); // Y
  1110.  
  1111. void FindOrthonormals(const QVector3D &normal, QVector3D &orthonormal1, QVector3D &orthonormal2) {
  1112.     QVector3D w = OrthoX.rotatedVector(normal);
  1113.     float dot = QVector3D::dotProduct(normal, w);
  1114.     if (qAbs(dot) > 0.6) {
  1115.         w = OrthoY.rotatedVector(normal);
  1116.     }
  1117.     w.normalize();
  1118.  
  1119.     orthonormal1 = QVector3D::crossProduct(normal, w).normalized();
  1120.     orthonormal2 = QVector3D::crossProduct(normal, orthonormal1).normalized();
  1121. }
  1122. qreal FindQuaternionTwist(const QQuaternion &q, QVector3D axis) {
  1123.     axis.normalize();
  1124.     //get the plane the axis is a normal of
  1125.     QVector3D orthonormal1, orthonormal2;
  1126.     FindOrthonormals(axis, orthonormal1, orthonormal2);
  1127.  
  1128.     QVector3D transformed = q.rotatedVector(orthonormal1);
  1129.  
  1130.     // project transformed vector onto plane
  1131.     QVector3D flattened = transformed - (QVector3D::dotProduct(transformed, axis) * axis);
  1132.     flattened.normalize();
  1133.  
  1134.     // get angle between original vector and projected transform to get angle around normal
  1135.     return qRadiansToDegrees(qAcos(QVector3D::dotProduct(orthonormal1, flattened)));
  1136. }
  1137.  
  1138. qreal angleFromDirection2(const QVector3D &a, const QVector3D &b, const QVector3D &ref) {
  1139.     qreal absAngle = angle3D(a, b);
  1140.  
  1141.     QVector3D cross = QVector3D::crossProduct(a, b);
  1142.     qreal dot = QVector3D::dotProduct(cross, ref);
  1143.  
  1144.     return (dot < 0) ? -absAngle : absAngle;
  1145. }
  1146.  
  1147.  
  1148. QMatrix4x4 la;
  1149. la.lookAt(m_rotationVectorZ2, m_rotationVectorZ, m_rotationVectorY);
  1150. m_rotationMatrix = QMatrix4x4(la.normalMatrix());
  1151.  
  1152.  
  1153. qreal pitch = angleFromDirection2(m_rotationMatrix.mapVector(QVector3D(0, 1, 0)).normalized(), QVector3D(0, 0, 1).normalized(), m_rotationMatrix.mapVector(QVector3D(-1, 0, -1)).normalized());
  1154. qreal roll  = angleFromDirection2(m_rotationMatrix.mapVector(QVector3D(0, 1, 0)).normalized(), QVector3D(1, 0, 0).normalized(), m_rotationMatrix.mapVector(QVector3D(0, 0, -1)).normalized());
  1155. qreal yaw   = angleFromDirection2(m_rotationMatrix.mapVector(QVector3D(0, 0, 1)).normalized(), QVector3D(1, 0, 0).normalized(), m_rotationMatrix.mapVector(QVector3D(0, -1, 0)).normalized());
  1156.  
  1157. QQuaternion quat = QQuaternion::fromRotationMatrix(mat);
  1158.  
  1159. QQuaternion diffQuater = quat * m_prevQuat.inverted();
  1160. QQuaternion conjBoxQuater = m_prevQuat.conjugated();
  1161. QQuaternion velQuater = 2.0 * exp(log(diffQuater) / d_time) * conjBoxQuater;
  1162. //QQuaternion velQuater = ((diffQuater * 2.0) / d_time) * conjBoxQuater;
  1163.  
  1164.  
  1165.  
  1166. float angleInDegrees;
  1167. QVector3D rotationAxis;
  1168. velQuater.getAxisAndAngle(&rotationAxis, &angleInDegrees);
  1169. qDebug() << angleInDegrees << velQuater.scalar();
  1170.  
  1171. QVector3D angularDisplacement = rotationAxis * qDegreesToRadians(angleInDegrees);
  1172. QVector3D angularSpeed = angularDisplacement / d_time;
  1173. QVector3D relAngularSpeed = rotMat * angularSpeed;
  1174.  
  1175. QVector3D angles = velQuater.vector();//diffQuater.toEulerAngles();*/
  1176.  
  1177. qreal roll = angles[0];
  1178. qreal pitch = angles[1];
  1179. qreal yaw = angles[2];
  1180.  
  1181.  
  1182. m_prevQuat = quat;
  1183. //bool bHoldAttitude = false;
  1184.  
  1185.  
  1186. // BODY ROTATION VELOCITY (PITCH, YAW)
  1187. qreal fCosQx = qCos(pkUserAC->m_kData.m_kBodyAttitude.x());
  1188. qreal fSinQx = qSin(pkUserAC->m_kData.m_kBodyAttitude.x());
  1189. qreal fCosQy = qCos(pkUserAC->m_kData.m_kBodyAttitude.y());
  1190. qreal fSinQy = qSin(pkUserAC->m_kData.m_kBodyAttitude.y());
  1191. QVector3D kLocalUserAcAttitudeVec = QVector3D(fCosQx * fSinQy, -fSinQx, fCosQx * fCosQy);
  1192.  
  1193. fCosQx = qCos(m_kData.m_kBodyAttitude.x());
  1194. fSinQx = qSin(m_kData.m_kBodyAttitude.x());
  1195. fCosQy = qCos(m_kData.m_kBodyAttitude.y());
  1196. fSinQy = qSin(m_kData.m_kBodyAttitude.y());
  1197. QVector3D kLocalAcAttitudeVec = QVector3D(fCosQx * fSinQy, -fSinQx, fCosQx * fCosQy);
  1198.  
  1199. QVector3D kLocalAcRotationVec = QVector3D::crossProduct(kLocalAcAttitudeVec, kLocalUserAcAttitudeVec);
  1200. QVector3D kLocalAcRotationNormalizedVec = kLocalAcRotationVec.normalized();
  1201. QVector3D kBodyAcRotationVec = LocalToBodyTransformMatrix.mapVector(kLocalAcRotationNormalizedVec);
  1202. qreal fAngularVelocity = 132.1 * (1.0 - QVector3D::dotProduct(kLocalUserAcAttitudeVec, kLocalAcAttitudeVec));
  1203. kBodyAcRotationVec = fAngularVelocity * kBodyAcRotationVec;
  1204.  
  1205. // BODY ROTATION VELOCITY (ROLL)
  1206. QVector3D kBodyUserAcBankVec = QVector3D(qCos(pkUserAC->m_kData.m_kBodyAttitude.z()), qSin(pkUserAC->m_kData.m_kBodyAttitude.z()), 0);
  1207. QVector3D kBodyAcBankVec = QVector3D(qCos(m_kData.m_kBodyAttitude.z()), qSin(m_kData.m_kBodyAttitude.z()), 0);
  1208. QVector3D kBodyAcRollVec = QVector3D::crossProduct(kBodyAcBankVec, kBodyUserAcBankVec);
  1209. QVector3D kBodyAcRollNormalizedVec = kBodyAcRollVec.normalized();
  1210. FLOAT fRollVelocity = 132.1 * (1.0 - QVector3D::dotProduct(kBodyUserAcBankVec, kBodyAcBankVec));
  1211. kBodyAcRollVec = fRollVelocity * kBodyAcRollNormalizedVec;
  1212.  
  1213. kSimWriteData.m_kBodyRotationVelocity = bHoldAttitude ? QVector3D(kBodyAcRotationVec.x(), kBodyAcRotationVec.y(), kBodyAcRollVec.z()) : QVector3D(0, 0, 0);
  1214. kSimWriteData.m_kBodyVelocity = pkUserAC->m_kData.m_kBodyVelocity + QVector3D(kBodyAcToFormVelocity.x(), kBodyAcToFormVelocity.y(), kBodyAcToFormVelocity.z());
  1215.  
  1216.  
  1217.  
  1218. qreal pitch = angle3D(m_rotationMatrix * QVector3D(0, 1, 0), QVector3D(0, 0, 1));
  1219. qreal roll  = angle3D(m_rotationMatrix * QVector3D(0, 1, 0), QVector3D(1, 0, 0));
  1220. qreal yaw   = angle3D(m_rotationMatrix * QVector3D(0, 0, 1), QVector3D(1, 0, 0));
  1221.  
  1222.  
  1223. QVector3D V0 = m_rotationVectorY;
  1224. QVector3D V1 = m_rotationVectorX;
  1225. QVector3D V2 = m_rotationVectorX2;
  1226. QVector3D Normal = QVector3D::crossProduct(V1 - V0, V2 - V0).normalized();
  1227. qreal a = Normal.x();
  1228. qreal b = Normal.y();
  1229. qreal c = Normal.z();
  1230. qreal d = -QVector3D::dotProduct(V0, Normal);
  1231.  
  1232. QVector3D finalVector = QVector3D(-(m_rotationVectorX.y() * m_rotationVectorY.x()),
  1233.                                 1 -(m_rotationVectorX.y() * m_rotationVectorY.y()),
  1234.                                   -(m_rotationVectorX.y() * m_rotationVectorY.z()));
  1235.  
  1236. finalVector.normalize();
  1237. QVector3D test2;
  1238. float ang;
  1239. qq.getAxisAndAngle(&test2, &ang);
  1240. QVector3D finalVector = m_rotationVectorY + Normal * d;
  1241. QQuaternion test = (qq * QQuaternion::fromAxisAndAngle(finalVector, -ang));
  1242.  
  1243. m_rotationVectorZ2 = projectedZ(m_rotationVectorZ);
  1244. m_rotationVectorX2 = projectedX(m_rotationVectorX);
  1245. m_rotationVectorY2 = projectedY(m_rotationVectorY);
  1246.  
  1247.  
  1248. //pitch = angleFromDirection2(QVector3D(0, 1, 0).normalized() * m_rotationMatrix, QVector3D(0, 0, -1).normalized(), QVector3D(0, 0, 0)) - 90;
  1249. //roll = -(angleFromDirection(QVector3D(-1, 0, 0).normalized() * QMatrix4x4(mat), QVector3D(0, 0, 0)) - 90);
  1250. //pitch = -(angleFromDirection2(v1.normalized() * m_rotationMatrix2, v2.normalized(), v3.normalized()) - 90);
  1251. //pitch = angleFromDirection2(m_rotationMatrix.mapVector(QVector3D(0, 1, 0)).normalized(), QVector3D(0, 1, 0).normalized(), m_rotationMatrix.mapVector(QVector3D(-1, 0, -1)).normalized());
  1252. //pitch = -(angleFromDirection4(m_rotationMatrix.mapVector(QVector3D(0, 1, 0)).normalized(), (QVector3D(0, 0, 1)).normalized()) - 90);
  1253. //roll  =   angleFromDirection2(m_rotationMatrix.mapVector(QVector3D(0, 1, 0)).normalized(), QVector3D(1, 0, 0).normalized(), QVector3D(0, 0, -1).normalized() ) - 90;
  1254. //yaw   =   angleFromDirection2(m_rotationMatrix.mapVector(QVector3D(0, 0, 1)).normalized(), QVector3D(1, 0, 0).normalized(), QVector3D(0, -1, 0).normalized() ) - 90;
  1255. //m_rotationVectorY = (projectedY((m_rotationVectorY)));
  1256.  
  1257. //m_rotationVectorY2 += QVector3D(m_rotationVectorY.x(), 0, 0);
  1258.  
  1259. qq = QQuaternion::fromRotationMatrix(mat.transposed());
  1260. QQuaternion testtt = (QQuaternion::fromDirection(m_rotationVectorX, QVector3D(0, 1, 0)) * qq);
  1261. m_rotationMatrix = QMatrix4x4(testtt.toRotationMatrix());
  1262. testtt.getEulerAngles(&pitch, &yaw, &roll);
  1263.  
  1264. testtt = (QQuaternion::fromDirection(m_rotationVectorZ, m_rotationVectorY) * qq);
  1265. testtt.getEulerAngles(&pitch, &yaw, &roll);*/
  1266.  
  1267. m_rotationVectorY = (QQuaternion::fromDirection(m_rotationVectorX, m_rotationVectorY) * qq).vector();
  1268.  
  1269. m_rotationVectorY = QVector3D(0,
  1270.                              m_rotationVectorY.y() - m_rotationVectorY.x(),
  1271.                              m_rotationVectorY.z());
  1272.  
  1273.  
  1274. pitch = angleFromDirection3(m_rotationVectorY, QVector3D(1, 0, 0), (m_rotationVectorY2)) - 90;
  1275. roll  = angleFromDirection3(m_rotationVectorX, (projectedZ(projectedX(m_rotationVectorX))), projectedY(m_rotationVectorY));
  1276. yaw   = angleFromDirection3(m_rotationMatrix.mapVector(QVector3D(0, 0, 1)).normalized(), QVector3D(1, 0, 0).normalized(), m_rotationMatrix.mapVector(QVector3D(0, -1, 0)).normalized()) - 90;
  1277. pitch = (angleFromDirection2(m_rotationMatrix.mapVector(QVector3D(0, 1, 0)).normalized(), QVector3D(0, 0, 1).normalized(), m_rotationMatrix.mapVector(QVector3D(-1, 0, -1)).normalized()) + 90);
  1278. roll  = angleFromDirection2(m_rotationMatrix.mapVector(QVector3D(0, 1, 0)).normalized(), QVector3D(1, 0, 0).normalized(), m_rotationMatrix.mapVector(QVector3D(0, 0, -1)).normalized()) - 90;
  1279. yaw   = angleFromDirection2(m_rotationMatrix.mapVector(QVector3D(0, 0, 1)).normalized(), QVector3D(1, 0, 0).normalized(), m_rotationMatrix.mapVector(QVector3D(0, -1, 0)).normalized()) - 90;
  1280. pitch = angleFromDirection2(QVector3D(0, 1, 0).normalized() * m_rotationMatrix, QVector3D(0, 1, 0).normalized(), QVector3D(-1, 0, -1).normalized() * m_rotationMatrix);
  1281.  
  1282. //mat = mat.transposed();
  1283. //------ to find angle of rotaton of nose
  1284. QVector3D world_ref(0, 1, 0); // world.up
  1285. QVector3D nose        = QVector3D(mat(2, 0), mat(2, 1), mat(2, 2)); // transform.forward;
  1286.  
  1287. QVector3D nose_projection = QVector3D(0, nose.y(), 0).normalized();
  1288. QVector3D nose_axis       = QVector3D::crossProduct(world_ref, nose_projection).normalized();
  1289. //------ nose rotate axis
  1290. QQuaternion q1 = QQuaternion::rotationTo(world_ref,nose_axis);
  1291. QQuaternion q2 = QQuaternion::rotationTo(nose,     nose_axis);
  1292. float ax = qRadiansToDegrees(QQuaternion::dotProduct(q1, q2));
  1293. ax = ax - 90;
  1294.  
  1295. //------ to find angle of rotaton of wing
  1296. QVector3D wing        = QVector3D(mat(0, 0), mat(0, 1), mat(0, 2)); // transform.right;
  1297. QVector3D aux_axis    = QVector3D::crossProduct(world_ref,  nose).normalized();
  1298. QVector3D wing_ref    = QVector3D::crossProduct(nose,       aux_axis).normalized();
  1299.  
  1300. QQuaternion q3 = QQuaternion::rotationTo(wing_ref, nose);
  1301. QQuaternion q4 = QQuaternion::rotationTo(wing,     nose);
  1302. float az = qRadiansToDegrees(QQuaternion::dotProduct(q3, q4));
  1303.  
  1304. if (QVector3D::dotProduct(wing, nose_axis) > 0) {
  1305.     az = 360 - az;
  1306. }
  1307. az = az-90;
  1308.  
  1309. pitch = -angleFromDirection2(QVector3D(0, 1, 0).normalized() * m_rotationMatrix, QVector3D(0, 0, 1).normalized(), QVector3D(-1, 0, -1).normalized() * m_rotationMatrix);
  1310. roll  = angleFromDirection2(QVector3D(0, 1, 0).normalized() * m_rotationMatrix, QVector3D(1, 0, 0).normalized(), QVector3D(0, 0, -1).normalized() * m_rotationMatrix);
  1311. yaw   = angleFromDirection2(QVector3D(0, 0, 1).normalized() * m_rotationMatrix, QVector3D(1, 0, 0).normalized(), QVector3D(0, -1, 0).normalized() * m_rotationMatrix);
  1312.  
  1313. QVector3D a = v1.normalized() * QMatrix4x4(mat);
  1314. QVector3D b = v2.normalized();
  1315. qreal sina = QVector3D::crossProduct(a, b).length() / (a.length() * b.length());
  1316. qreal cosa = QVector3D::dotProduct(a, b) / (a.length() * b.length());*/
  1317.  
  1318. roll = angleFromDirection2(v1 * m_rotationMatrix.transposed(), v2);
  1319. pitch = -angleFromDirection(QVector3D(0, 1, 0) * m_rotationMatrix, QVector3D(0, 0, 0));
  1320. roll  =  angleFromDirection2(QVector3D(1, 0, 0) * m_rotationMatrix, QVector3D(0, 0, 0));
  1321.  
  1322.  
  1323.  
  1324. qq = QQuaternion::rotationTo(QVector3D(1, 0, 0), qq.vector());
  1325. qq.getEulerAngles(&pitch, &yaw, &roll);
  1326. float X = -cos(qDegreesToRadians(pitch))*cos(qDegreesToRadians(roll));
  1327. float Y = -sin(qDegreesToRadians(pitch))*cos(qDegreesToRadians(roll));
  1328. float Z = sin(qDegreesToRadians(pitch));
  1329.  
  1330. yaw  = qAtan2(2*qq.y()()*qq.scalar() - 2*qq.x()()*qq.z()(), 1 - 2*qq.y()()*qq.y()() - 2*qq.z()()*qq.z()());
  1331. pitch = qAtan2(2*qq.x()()*qq.scalar() - 2*qq.y()()*qq.z()(), 1 - 2*qq.x()()*qq.x()() - 2*qq.z()()*qq.z()());
  1332. roll   = qAsin(2*qq.x()()*qq.y()() + 2*qq.z()()*qq.scalar());
  1333.  
  1334.  
  1335.  
  1336. float *R = mat.data();
  1337. roll = qAtan2(R[1], R[4]);
  1338. pitch = qAsin(-R[7]);
  1339. yaw = qAtan2(-R[6], R[8]);
  1340.  
  1341.  
  1342. float epsilon=0.0001f;
  1343. if (mat(1,0) > (1.0f-epsilon)) { // singularity at north pole
  1344.     yaw = atan2(mat(0, 2),mat(2,2));
  1345.     pitch = M_PI / 2.0f;
  1346. } else if (mat(1,0) < -(1.0f-epsilon)) { // singularity at south pole
  1347.     yaw = atan2(mat(0,2),mat(2,2));
  1348.     pitch = M_PI / -2.0f;
  1349. } else {
  1350.     yaw = atan2(-mat(2,0),mat(0,0));
  1351.     pitch = asin(mat(1, 0));
  1352.     roll = atan2(-mat(1,2),mat(1,1));
  1353. }
RAW Paste Data
We use cookies for various purposes including analytics. By continuing to use Pastebin, you agree to our use of cookies as described in the Cookies Policy. OK, I Understand
 
Top