Angles d'Euler et orientation Quaternion

Question

Comment convertir la sortie d'orientation Quaternion du périphérique Wintracker II en sortie Euler Angles uniquement. Parce que les périphériques Wintracker II sortent les angles d'Euler et l'orientation Quaternion. je veux la sortie des angles d'Euler seulement.

Answer 1

J'ai mis en œuvre l'algorithme décrit dans le présent document et il fonctionne très bien: http://www.euclideanspace.com/maths/geometry/rotations/conversions/quaternionToEuler/quat_2_euler_paper_ver2-1.pdf

Le problème avec l'article de Wikipedia énumérés dans la réponse n ° 1 est qu'il ne fournit que la formule de la rotation X-Y-Z. L'article référencé ici donne un algorithme général qui fonctionnera pour n'importe laquelle des 12 séquences. Vous devrez peut-être le lire à quelques reprises, et certainement travailler à travers l'exemple. Ce n'est pas le plus facile à suivre, mais j'ai testé la morve à l'unité et c'est assez à l'épreuve des balles.

Par le premier commentaire, voici les principaux composants de mon code. Cela devrait suffire pour démarrer:

La première classe est 'AxisType'. La fonctionnalité principale que j'utilise est le 'getNextCircular()' sur celui-ci. Cela facilite également les conversions dans mon code vers les vecteurs.

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; 
} 
} 

Suivi par EulerOrder, ce qui représente un ordre spécifique de l'axe du (de sorte XYX, ZYX, etc) et un groupe de constructeurs statiques. Il y a beaucoup de passe-partout, mais ici il est ...

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; 
} 
} 

Cela nous amène à l'algorithme lui-même. J'ai gardé les noms de variables de M. Hugh pour faciliter le débogage avec le document original.

/** 
* 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); 
} 

} 

Il y a quelques classes manquantes ici que je ne l'ai pas inclus (Angle, RotationSequence, Quaternion), mais je crois que le code ci-dessus donne des gens un saut très solide point hors de.