歐拉角和四元數方向

Question

如何將wintracker II設備的四元數方向輸出轉換爲歐拉角輸出。因爲Wintracker II設備輸出歐拉角和四元數方向。我只想輸出歐拉角輸出。

Answer 1

我已經實現在本文中描述的算法，它工作得很好： http://www.euclideanspace.com/maths/geometry/rotations/conversions/quaternionToEuler/quat_2_euler_paper_ver2-1.pdf

在回答＃1中列出的維基百科文章的問題是，它僅提供了X-Y-Z軸旋轉的公式。這裏引用的論文給出了一種適用於12個序列中的任何一個的通用算法。您可能需要閱讀它幾次，並確實通過該示例。這不是最容易遵循的，但我有單位測試了它的鼻涕，它非常防彈。

根據第一條評論，這裏是我的代碼的主要組件。應該足以讓你走：

第一類是'AxisType'。我使用的主要功能是'getNextCircular（）'。它還使得我的代碼易於轉換爲Vectors。

public enum AxisType { 

X("X"), 
Y("Y"), 
Z("Z"); 

String label; 

AxisType(final String label) { 
     this.label = label; 
} 

/** 
* Converts an axis type to a vector. 
* 
* @return 
*/ 
public Vector3D toVector3D() { 
    if (equals(AxisType.X)) { 
     return new Vector3D(1,0,0); 
    } else if (equals(AxisType.Y)) { 
     return new Vector3D(0,1,0); 
    } else { 
     return new Vector3D(0,0,1); 
    } 
} 

/** 
* gets the next circular axis from this one circular: </br> <code> 
* X --> Y 
* </br> 
* Y --> Z 
* </br> 
* Z --> X 
* </code> 
* 
* @return 
*/ 
public AxisType nextCircular() { 
    if (equals(AxisType.X)) { 
     return AxisType.Y; 
    } else if (equals(AxisType.Y)) { 
     return AxisType.Z; 
    } else { 
     return AxisType.X; 
    } 
} 

@Override 
public String toString() { 
    return label; 
} 
} 

其次EulerOrder，它代表軸的的特定排序（所以XYX，ZYX，等）和一堆靜​​態構造的。這是一個很大的樣板，但在這裏它是...

public class EulerOrder 
{ 
private final AxisType[] axisOrder; 

/** 
* generic constructor 
* 
* @param first 
* @param second 
* @param third 
*/ 
public EulerOrder(
     final AxisType first, 
     final AxisType second, 
     final AxisType third) 
{ 
    axisOrder = new AxisType[] { 
     first, 
     second, 
     third 
    }; 
} 

/** 
* @return the cartesian axis that represent this sequence 
*/ 
public Vector3D[] orderedAxis() 
{ 
    return new Vector3D[] { 
     axisOrder[0].toVector3D(), 
     axisOrder[1].toVector3D(), 
     axisOrder[2].toVector3D() 
    }; 
} 

public AxisType getAxisType(
     final int index) 
{ 

    if ((index > 2) || (index < 0)) 
    { 
     throw new ArrayIndexOutOfBoundsException(
       "EulerOrder[index] called with an invalid index"); 
    } 

    return axisOrder[index]; 
} 

/** 
* <code> 
* X->Y->* 
* </br> 
* Y->Z->* 
* </br> 
* Z->X->* 
* </code> 
* 
* @return true if the first two rotations are in a circular order 
*/ 
public boolean isCircular() 
{ 
    // true if the first 2 roations are in one of these orders 
    // X-Y 
    // Y-Z 
    // Z-X 
    return axisOrder[0].nextCircular().equals(
      axisOrder[1]); 
} 

/** 
* <code> 
* X->*->X 
* </br> 
* Y->*->Y 
* </br> 
* Z->*->Z 
* </code> 
* 
* @return true if the first and last axis are the same 
*/ 
public boolean isRepeating() 
{ 
    // returns true if the first and last axis are the same 
    // X->*->X 
    // Y->*->Y 
    // Z->*->Z 
    return axisOrder[0] == axisOrder[2]; 
} 

@Override 
public String toString() 
{ 
    final StringBuffer buffer = new StringBuffer(); 
    buffer.append(axisOrder[0].toString()); 
    buffer.append(axisOrder[1].toString()); 
    buffer.append(axisOrder[2].toString()); 
    return buffer.toString(); 
} 

/* STATIC CONSTRUCTORS FOR THE 12 POSSIBLE EULER SEQUENCES */ 

public static EulerOrder XYZ() 
{ 
    return new EulerOrder(
      AxisType.X, 
      AxisType.Y, 
      AxisType.Z); 
} 

public static EulerOrder YZX() 
{ 
    return new EulerOrder(
      AxisType.Y, 
      AxisType.Z, 
      AxisType.X); 
} 

public static EulerOrder ZXY() 
{ 
    return new EulerOrder(
      AxisType.Z, 
      AxisType.X, 
      AxisType.Y); 
} 

public static EulerOrder ZYX() 
{ 
    return new EulerOrder(
      AxisType.Z, 
      AxisType.Y, 
      AxisType.X); 
} 

public static EulerOrder YXZ() 
{ 
    return new EulerOrder(
      AxisType.Y, 
      AxisType.X, 
      AxisType.Z); 
} 

public static EulerOrder XZY() 
{ 
    return new EulerOrder(
      AxisType.X, 
      AxisType.Z, 
      AxisType.Y); 
} 

public static EulerOrder XYX() 
{ 
    return new EulerOrder(
      AxisType.X, 
      AxisType.Y, 
      AxisType.X); 
} 

public static EulerOrder XZX() 
{ 
    return new EulerOrder(
      AxisType.X, 
      AxisType.Z, 
      AxisType.X); 
} 

public static EulerOrder YZY() 
{ 
    return new EulerOrder(
      AxisType.Y, 
      AxisType.Z, 
      AxisType.Y); 
} 

public static EulerOrder YXY() 
{ 
    return new EulerOrder(
      AxisType.Y, 
      AxisType.X, 
      AxisType.Y); 
} 

public static EulerOrder ZXZ() 
{ 
    return new EulerOrder(
      AxisType.Z, 
      AxisType.X, 
      AxisType.Z); 
} 

public static EulerOrder ZYZ() 
{ 
    return new EulerOrder(
      AxisType.Z, 
      AxisType.Y, 
      AxisType.Z); 
} 

public static EulerOrder parse(String eulerOrder) 
{ 
    if(eulerOrder.equals("XYZ")) return XYZ(); 
    if(eulerOrder.equals("XZY")) return XZY(); 
    if(eulerOrder.equals("YZX")) return YZX(); 
    if(eulerOrder.equals("YXZ")) return YXZ(); 
    if(eulerOrder.equals("ZYX")) return ZYX(); 
    if(eulerOrder.equals("ZXY")) return ZXY(); 

    if(eulerOrder.equals("XYX")) return XYX(); 
    if(eulerOrder.equals("XZX")) return XZX(); 
    if(eulerOrder.equals("YZY")) return YZY(); 
    if(eulerOrder.equals("YXY")) return YXY(); 
    if(eulerOrder.equals("ZYZ")) return ZYZ(); 
    if(eulerOrder.equals("ZXZ")) return ZXZ(); 

    return null; 
} 

@Override 
public boolean equals(Object o) { 
    if (this == o) return true; 
    if (o == null || getClass() != o.getClass()) return false; 

    EulerOrder that = (EulerOrder) o; 

    if (!Arrays.equals(axisOrder, that.axisOrder)) return false; 

    return true; 
} 

@Override 
public int hashCode() { 
    return axisOrder != null ? Arrays.hashCode(axisOrder) : 0; 
} 
} 

這給我們帶來了算法本身。我保留了休先生的變量名稱，以便於用原始文檔進行調試。

/** 
* This class is a direct implementation of the algorithm described in the 
* paper: "Quaternion to Euler Angle Conversion for Arbitrary Rotation Sequence 
* Using Geometric Methods" by Noel H Hughes 
* 
* All variables are named using the names that the author uses in the paper to 
* ensure tracability with the original document 
* 
* The general algorithm for this is really quite simple: Given a unit 
* quaternion and a desired sequence, decompose that quaternion into a sequence 
* of 3 rotations about the principle axis in the correct sequence. 
* 
* This involves 2 steps: step 1: take the last axis of rotation and rotate it 
* through the quaternion. From this, you can determine (with some clever trig 
* and the knowledge of the order of the first two rotations) what the first two 
* angles are step 2: construct a quaternion from the first 2 rotations, and run 
* the next circular axis after the last axis (ie, if the last axis of rotation 
* is 'X', then use 'Y') through the original quaternion and the new 2-step one. 
* The included angle between these two vectors must be your third Euler angle. 
* Using some clever cross product tests you can determine the sign and you're 
* done! 
* 
* Note - This has been tested extensively to make sure that the angles that are 
* returned produce an equivalent rotation using the same sequence as the 
* original. This does not, in fact, quarantee that you will get the same 
* angles! There is a 'short way' and a 'long way' to get from here to there, 
* and as of yet I haven't figured out how to force one over the other 
*/ 
public class EulerAngleDecomposer 
{ 

private static EulerAngleDecomposer instance = null; 

// made the constructor private because this is a singleton 
private EulerAngleDecomposer() 
{ 

} 

public static EulerAngleDecomposer getInstance() 
{ 
    if(instance == null) 
     instance = new EulerAngleDecomposer(); 

    return instance; 
} 

private class IndexData 
{ 
    // for all of these indices: 
    // 0 = X 
    // 1 = Y 
    // 2 = Z 

    private final AxisType m_i1; // zero based index of first euler rotation 
    private final AxisType m_i1n; // next circular index following i1 
    private final AxisType m_i1nn; // next circular index following i1n 

    private final AxisType m_i2; // zero based index of second euler rotation 
    private final AxisType m_i2n; // next circular index following i2 
    private final AxisType m_i2nn; // next circular index following i2n 

    private final AxisType m_i3; // zero based index of third euler rotation 
    private final AxisType m_i3n; // next circular index following i3 
    private final AxisType m_i3nn; // next circular index following i3n 

    // m_unitAxis[0] = first euler rotation axis 
    // m_unitAxis[1] = second euler rotation axis 
    // m_unitAxis[2] = third euler rotation axis 
    private final Vector3D[] m_unitAxis; 

    // create from a EulerOrder 
    public IndexData(
      final EulerOrder order) 
    { 
     m_i1 = order.getAxisType(0); 
     m_i2 = order.getAxisType(1); 
     m_i3 = order.getAxisType(2); 

     // now populate m_ixn, ans ixnn's 
     m_i1n = m_i1.nextCircular(); 
     m_i1nn = m_i1n.nextCircular(); 

     m_i2n = m_i2.nextCircular(); 
     m_i2nn = m_i2n.nextCircular(); 

     m_i3n = m_i3.nextCircular(); 
     m_i3nn = m_i3n.nextCircular(); 

     m_unitAxis = order.orderedAxis(); 
    } 

    // first axis of rotation 
    public Vector3D V1() 
    { 
     return m_unitAxis[0]; 
    } 

    // second axis of rotation 
    public Vector3D V2() 
    { 
     return m_unitAxis[1]; 
    } 

    // third axis of rotation 
    public Vector3D V3() 
    { 
     return m_unitAxis[2]; 
    } 

    // next axis after V3 (circular) 
    // a little table: 
    // V3()  -->  V3n() 
    // X  -->  Y 
    // Y  -->  Z 
    // Z  -->  X 

    public Vector3D V3n() 
    { 
     return m_i3n.toVector3D(); 
    } 

    // first rotation axis 
    public AxisType i1() 
    { 
     return m_i1; 
    } 

    // next circular axis folowing i1() 
    // not to be confused with the second axis of rotation (i2) 
    public AxisType i1n() 
    { 
     return m_i1n; 
    } 

    // next circular axis following i1n() 
    // not to be confused with the third axis of rotation (i3) 
    public AxisType i1nn() 
    { 
     return m_i1nn; 
    } 
} 

public RotationSequence DecomposeFromQuaternion(
     final Quaternion q, 
     final EulerOrder order) 
{ 
    // crappy variable name, I know 
    // it's used a lot, so I wanted a one letter 
    // one! 
    final IndexData d = new IndexData(
      order); 

    final Vector3D v3Rot = q.Rotate(
      d.V3()).unit(); // q->GetRotatedVector(d.V3()).unit(); 

    // recall: 
    // i1;  // zero based index of first euler rotation 
    // i1n;  // next circular index following i1 
    // i1nn; // next circular index following i1n 

    Angle theta1 = Angle.Zero(); 
    Angle theta2 = Angle.Zero(); 
    Angle theta3 = Angle.Zero(); 

    if (order.isRepeating()) 
    { 
     if (order.isCircular()) 
     { 

      // circular, repeating 
      //theta1 = atan2(v3Rot[d.i1n()], -v3Rot[d.i1nn()]); 
      theta1 = Angle.fromRadians(org.apache.commons.math3.util.FastMath.atan2(
        v3Rot.at(d.i1n()), 
        -v3Rot.at(d.i1nn()))); 
      theta2 = Angle.fromRadians(-org.apache.commons.math3.util.FastMath.acos(v3Rot.at(d.i1()))); 
     } 
     else 
     { 

      // non circular, repeating 
      theta1 = Angle.fromRadians(org.apache.commons.math3.util.FastMath.atan2(
        v3Rot.at(d.i1nn()), 
        v3Rot.at(d.i1n()))); 
      theta2 = Angle.fromRadians(org.apache.commons.math3.util.FastMath.acos(v3Rot.at(d.i1()))); 
     } 

     // By convention, repeating sequences restrict theta2 to 0-->180 
     if (theta2.radians() < 0) 
     { 
      // need to resolve the ambiguity restrict theta2 to 0 --> 180 
      theta2 = theta2.negate(); 
      //theta1 = theta1 - pi; 
     } 

     // special case where theta2 is zero, which is somewhat nonsense 
     // for a repeating sequence 
     // in this case, put all the entire angle into theta3 
     if ((theta2.radians() == 0) || (theta2.radians() == Math.PI)) 
     { 
      theta1 = Angle.Zero(); 
     } 
    } 
    else 
    // non-repeating sequence 
    { 
     if (order.isCircular()) 
     { 
      // circular, non-repeating 
      theta1 = Angle.fromRadians(org.apache.commons.math3.util.FastMath.atan2(
        -v3Rot.at(d.i1n()), 
        v3Rot.at(d.i1nn()))); 
      theta2 = Angle.fromRadians(-org.apache.commons.math3.util.FastMath.asin(-v3Rot.at(d.i1()))); 
     } 
     else 
     { 
      // non circular, non repeating 
      theta1 = Angle.fromRadians(org.apache.commons.math3.util.FastMath.atan2(
        v3Rot.at(d.i1nn()), 
        v3Rot.at(d.i1n()))); 
      theta2 = Angle.fromRadians(-org.apache.commons.math3.util.FastMath.asin(v3Rot.at(d.i1()))); 
     } 
    } 

    // Create the Q12 quaternion using the first two axis and angles 
    final Quaternion Q1 = Quaternion.createFromAxisAngle(
      d.V1(), 
      theta1); 
    final Quaternion Q2 = Quaternion.createFromAxisAngle(
      d.V2(), 
      theta2); 

    final Quaternion Q12 = Q1.times(Q2); 
    /* Q12 = Q1 * Q2 */ 

    // get the next circular vector after V3 
    final Vector3D V3n = d.V3n(); 

    // rotate V3n through Q12 and q 
    final Vector3D V3n12 = Q12.Rotate(V3n); 
    final Vector3D V3nG = q.Rotate(V3n); 

    // get the angle between them - theta3 
    theta3 = Vector3D.angleBetween(
      V3n12, 
      V3nG); 

    // use a cross product to determine the direction of the angle 
    final Vector3D Vc = Vector3D.crossProduct(
      V3n12, 
      V3nG); 

    final double m = Vector3D.dotProduct(
      Vc, 
      v3Rot); 

    final double sign = m > 0 ? 1.0 : -1.0; 

    theta3 = Angle.fromRadians(sign * org.apache.commons.math3.util.FastMath.abs(theta3.radians())); 

    return new RotationSequence(
      order, 
      theta1, 
      theta2, 
      theta3); 
} 

} 

有幾類在這裏失去了，我還沒有包括（角，RotationSequence，四元），但我相信，上面的代碼給鄉親一個非常堅實的起跳點。