• API
• FAQ
• Tools
• Archive
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
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.
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.
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));
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
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);
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.

Top