To main page | 3dengine.org

---

Quaternions (C++ & Python)

---

Quats are mostly used in Heading, pitch and roll angles.

C++

Reference implementation

#include <math>

class Quat
{
public:
    Quat(): w(1),x(0),y(0),z(0) {};
    Quat(float x1, float y1, float z1, float degrees)
    {
        float angle = float((degrees / 180.0f) * 3.1415926);
        float result = (float)sin( angle / 2.0f );
        w = (float)cos( angle / 2.0f );
        x = float(x1 * result);
        y = float(y1 * result);
        z = float(z1 * result);
    };
    void gl_matrix(double *m)
    {
        if(!m) return;
        m[0] = 1.0f - 2.0f * ( y * y + z * z ); 
        m[1] = 2.0f * (x * y + z * w);
        m[2] = 2.0f * (x * z - y * w);
        m[3] = 0.0f;  
        m[ 4] = 2.0f * ( x * y - z * w );  
        m[ 5] = 1.0f - 2.0f * ( x * x + z * z ); 
        m[ 6] = 2.0f * (z * y + x * w );  
        m[ 7] = 0.0f;  
        m[ 8] = 2.0f * ( x * z + y * w );
        m[ 9] = 2.0f * ( y * z - x * w );
        m[10] = 1.0f - 2.0f * ( x * x + y * y );  
        m[11] = 0.0f;  
        m[12] = 0;  
        m[13] = 0;  
        m[14] = 0;  
        m[15] = 1.0f;
    };
    Quat operator* (const Quat &b) {
        Quat r;
        r.w = w*b.w - x*b.x - y*b.y - z*b.z;
        r.x = w*b.x + x*b.w + y*b.z - z*b.y;
        r.y = w*b.y + y*b.w + z*b.x - x*b.z;
        r.z = w*b.z + z*b.w + x*b.y - y*b.x;
        return(r);
    };
private:
    float w,x,y,z;
};

Example usage:

// the order of multiplication is important!
Quat Q1 = Quat(0,0,1, roll) * Quat(1,0,0, pitch) * Quat(0,1,0, heading);
double matrix_for_openGL[16];
Q1.gl_matrix(&matrix_for_openGL);

Now matrix_for_openGL has modelview matrix for roll, pitch, heading angles.

Python

import math
class Quat:
    """
        Quaternion Class
        >>> roll = 30
        >>> pitch = 45
        >>> heading = 30
        >>> Q1 = Quat(0,0,1, roll) * Quat(1,0,0, pitch) * \
               Quat(0,1,0, heading)
        >>> Q1.gl_matrix()
        [0.57322332027622536, 0.73919890801437749, \
        -0.3535533898605015, 0.0, -0.3535533898605015, \
        0.61237244705783134, 0.70710677171312086, 0.0, \
        0.73919890801437749, -0.28033009198934633, \
        0.61237244705783134, 0.0, 0, 0, 0, 1.0]
    """
    def __init__(self, *args, **kwargs):
        if 'w' in kwargs:
            self.w = kwargs['w']
            self.x = kwargs['x']
            self.y = kwargs['y']
            self.z = kwargs['z']
        elif len(args)==4:
            angle = (float(args[3]) / 180.) * 3.1415926
            result = math.sin(angle / 2.)
            self.w = math.cos(angle / 2.)
            self.x = float(args[0]) * result
            self.y = float(args[1]) * result
            self.z = float(args[2]) * result
        else:
            self.w = 1.
            self.x = 0.
            self.y = 0.
            self.z = 0.

    def gl_matrix(self):
        """
           Returns OpenGL compatible modelview matrix
        """
        m = [0.] * 16
        m[0] = 1.0 - 2.0 * ( self.y * self.y + self.z * self.z )
        m[1] = 2.0 * (self.x * self.y + self.z * self.w)
        m[2] = 2.0 * (self.x * self.z - self.y * self.w)
        m[3] = 0.0 
        m[ 4] = 2.0 * ( self.x * self.y - self.z * self.w ) 
        m[ 5] = 1.0 - 2.0 * ( self.x * self.x + self.z * self.z )
        m[ 6] = 2.0 * (self.z * self.y + self.x * self.w ) 
        m[ 7] = 0.0 
        m[ 8] = 2.0 * ( self.x * self.z + self.y * self.w )
        m[ 9] = 2.0 * ( self.y * self.z - self.x * self.w )
        m[10] = 1.0 - 2.0 * ( self.x * self.x + self.y * self.y ) 
        m[11] = 0.0 
        m[12] = 0 
        m[13] = 0 
        m[14] = 0 
        m[15] = 1.0
        return m
    
    def __mul__(self, b):
        r = Quat()
        r.w = self.w*b.w - self.x*b.x - self.y*b.y - self.z*b.z
        r.x = self.w*b.x + self.x*b.w + self.y*b.z - self.z*b.y
        r.y = self.w*b.y + self.y*b.w + self.z*b.x - self.x*b.z
        r.z = self.w*b.z + self.z*b.w + self.x*b.y - self.y*b.x
        return(r)

Example usage:

roll = 30
pitch = 45
heading = 30
# the order of multiplication is important
Q1 = Quat(0,0,1, roll) * Quat(1,0,0, pitch) * Quat(0,1,0, heading)
print Q1.gl_matrix()

---