All geek questions in one place

Euler angles and orientation of quaternions

how to convert the quintion orientation output of a wintracker II device only to Euler Angles output. Because Winthracker II gives Euler angles and the quaternion orientation. I want only for Euler angles.

+3
source share
1 answer

I implemented the algorithm described in this article and it works very well: http://www.euclideanspace.com/maths/geometry/rotations/conversions/quaternionToEuler/quat_2_euler_paper_ver2-1.pdf

The problem with the Wikipedia article mentioned in answer No. 1 is that it provides only the formula for XYZ rotation. The work given here provides a general algorithm that will work for any of the 12 sequences. You may need to read it several times and definitely work on an example. This is not the easiest to follow, but I have a block testing snot and it is pretty bulletproof.

In the first comment, here are the main components of my code. It should be enough for you to go:

The first class is "AxisType". The main feature that I use is 'getNextCircular ()' on it. It also makes it easy to convert code to 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 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);
}

}

, (Angle, RotationSequence, Quaternion), , .

+1

Source: https://habr.com/ru/post/1733020/

More articles:


All Articles