Representing Rotations as Quaternions

Quaternions are mathematical constructs that can be used to rotate a geometrical point through a particular angle around a particular axis. They avoid many of the problems associated with performing rotations using Euler angles (yaw, pitch, roll), notably the phenomenon of “gimbal lock”, which causes numerical difficulties as one of the rotation axes of a body gets very close to another.

To put it another way: What’s the longitude of the north pole? Don’t know, do ya, smart guy?

The following Java code can be used to perform a few test rotations using quaternions. Information about how to compile and run Java programs is available in a previous post. Oh, and sorry about the tiny font. I was trying to avoid having a horizontal scroll bar, and “quaternion” is kind of a long word.

public class QuaternionRotationTest
{
    public static void main(String[] args)
    {
        Coords coordsToRotate = new Coords(1, 2, 3);

        System.out.println("coordsToRotate is " + coordsToRotate.toString());

        Quaternion[] quaternionsForRotation = new Quaternion[]
        {
            new Quaternion( new Coords(0, 0, 1), Math.PI / 2 ),
            new Quaternion( new Coords(1, 0, 0), Math.PI / 2 ),
        };

        // rotate around one axis

        Coords coordsRotatedOnce = quaternionsForRotation[0].applyToCoordsInPlaceAsRotation
        (
            coordsToRotate.clone()
        );

        System.out.println("coordsRotatedOnce is " + coordsRotatedOnce.toString());

        // reverse a single rotation

        Quaternion quaternionForRotation0Inverted = quaternionsForRotation[0].clone().invertInPlace();

        Coords coordsRotatedOnceAndBack = quaternionForRotation0Inverted.applyToCoordsInPlaceAsRotation
        (
            coordsRotatedOnce.clone()
        );

        System.out.println("coordsRotatedOnceAndBack is " + coordsRotatedOnceAndBack.toString());

        // perform two separate rotations

        Coords coordsRotatedTwice = quaternionsForRotation[1].applyToCoordsInPlaceAsRotation
        (
            coordsRotatedOnce.clone()
        );

        System.out.println("coordsRotatedTwice is " + coordsRotatedTwice.toString());

        // perform two premultiplied rotations : note the reversed order

        Quaternion quaternionsMultiplied = quaternionsForRotation[1].clone().multiplyInPlace
        (
            quaternionsForRotation[0]
        );

        Coords coordsRotatedPremultiplied = quaternionsMultiplied.applyToCoordsInPlaceAsRotation
        (
            coordsToRotate.clone()
        ); 

        System.out.println("coordsRotatedPremultiplied is " + coordsRotatedPremultiplied.toString());

    }
}

class Coords
{
    public double x;
    public double y;
    public double z;

    public Coords(double x, double y, double z)
    {
        this.x = x;
        this.y = y;
        this.z = z;
    }

    public Coords clone()
    {
        return new Coords(this.x, this.y, this.z);
    }

    public Coords divideInPlace(double divisor)
    {
        this.x /= divisor;
        this.y /= divisor;
        this.z /= divisor;

        return this;
    }

    public double magnitude()
    {
        return Math.sqrt(this.x * this.x + this.y * this.y + this.z * this.z);
    }

    public Coords normalizeInPlace()
    {
        return this.divideInPlace(this.magnitude());    
    }

    public Coords overwriteWith(double x, double y, double z)
    {
        this.x = x;
        this.y = y;
        this.z = z;

        return this;
    }

    public String toString()
    {
        return "(" + this.x + "," + this.y + "," + this.z + ")";
    }
}

class Quaternion
{
    public double w;
    public double x;
    public double y;
    public double z;

    public Quaternion(Coords axisToRotateAround, double radiansToRotate)
    {
        double radiansToRotateHalf = radiansToRotate / 2;        
        double sineOfRadiansToRotateHalf = Math.sin(radiansToRotateHalf);

        this.w = Math.cos(radiansToRotateHalf);
        this.x = axisToRotateAround.x * sineOfRadiansToRotateHalf;
        this.y = axisToRotateAround.y * sineOfRadiansToRotateHalf;
        this.z = axisToRotateAround.z * sineOfRadiansToRotateHalf;

        this.normalizeInPlace();
    }

    private Quaternion(double w, double x, double y, double z)
    {
        this.w = w;
        this.x = x;
        this.y = y;
        this.z = z;
    }

    public Coords applyToCoordsInPlaceAsRotation(Coords coordsToRotate)
    {
        Quaternion coordsToRotateAsQuaternion = new Quaternion
        (
            0,
            coordsToRotate.x,
            coordsToRotate.y,
            coordsToRotate.z
        );

        Quaternion result = this.clone().multiplyInPlace
        (
            coordsToRotateAsQuaternion    
        ).multiplyInPlace
        (
            this.clone().invertInPlace()
        );

        coordsToRotate.overwriteWith(result.x, result.y, result.z);

        return coordsToRotate;
    }    

    public Quaternion clone()
    {
        return new Quaternion(this.w, this.x, this.y, this.z);
    }

    public Quaternion divideInPlace(double divisor)
    {
        this.w /= divisor;
        this.x /= divisor;
        this.y /= divisor;
        this.z /= divisor;

        return this;
    }

    public Quaternion invertInPlace()
    {
        double magnitude = this.magnitude();

        this.divideInPlace(magnitude * magnitude);

        this.x *= -1;
        this.y *= -1;
        this.z *= -1;

        return this;
    }

    public Quaternion multiplyInPlace(Quaternion other)
    {
        return this.overwriteWith
        (
            this.w * other.w - this.x * other.x - this.y * other.y - this.z * other.z,
            this.w * other.x + this.x * other.w + this.y * other.z - this.z * other.y,
            this.w * other.y - this.x * other.z + this.y * other.w + this.z * other.x,
            this.w * other.z + this.x * other.y - this.y * other.x + this.z * other.w
        );
    }

    public double magnitude()
    {
        return Math.sqrt(this.w * this.w + this.x * this.x + this.y * this.y + this.z * this.z);
    }

    public Quaternion normalizeInPlace()
    {
        return this.divideInPlace(this.magnitude());
    }

    public Quaternion overwriteWith(Quaternion other)
    {
        this.overwriteWith(other.w, other.x, other.y, other.z);

        return this;
    }

    public Quaternion overwriteWith(double w, double x, double y, double z)
    {
        this.w = w;
        this.x = x;
        this.y = y;
        this.z = z;

        return this;
    }
}
This entry was posted in Uncategorized and tagged , , , , . Bookmark the permalink.

Leave a Reply

Fill in your details below or click an icon to log in:

WordPress.com Logo

You are commenting using your WordPress.com account. Log Out / Change )

Twitter picture

You are commenting using your Twitter account. Log Out / Change )

Facebook photo

You are commenting using your Facebook account. Log Out / Change )

Google+ photo

You are commenting using your Google+ account. Log Out / Change )

Connecting to %s