3D Rotations about Arbitrary Axis – Euler–Rodrigues and Matrix Methods

Code is written in python, uses pyglet to draw lines and vertices. All vertex calculations is done without using 3D methods of any library.

Euler-Roudrigues rotation and rotation about x, y, and z axis are given separately. Rotation method can be selected from code.

Left, right, up, and down arrow keys moves the cube.

x, y, and z keys rotates cube with the specified axes.

This code is for demonstration purposes only, so it is neither optimal nor complete. Use it as is.

bitbucket url: https://bitbucket.org/iclykofte/3d_transformations

 

import pyglet
import pyglet.window.key
import pyglet.window
from pyglet.gl import *
import numpy as np
from math import sin, cos, sqrt


win = pyglet.window.Window()


translate_x = 0
translate_y = 0
translate_z = 0

DEGREE_TO_RADIAN = 0.0174533

rotation_angle_x = 0.0
rotation_angle_y = 0.0
rotation_angle_z = 0.0

rotation_axis = 'z'

center = [0, 0, 0]



paneFront = [
    [-50,-50,-50,1],
    [50,-50,-50,1],
    [50,50,-50,1],
    [-50,50,-50,1]
]
paneRear = [
    [-50,-50,50,1],
    [50,-50,50,1],
    [50,50,50,1],
    [-50,50,50,1],
]
paneTop = [
    [-50,50,-50,1],
    [50,50,-50,1],
    [50,50,50,1],
    [-50,50,50,1],
]
paneBottom = [
    [-50,-50,-50,1],
    [50,-50,-50,1],
    [50,-50,50,1],
    [-50,-50,50,1],
]
paneLeft = [
    [-50,-50,-50,1],
    [-50,50,-50,1],
    [-50,50,50,1],
    [-50,-50,50,1],
]
paneRight = [
    [50,-50,-50,1],
    [50,50,-50,1],
    [50,50,50,1],
    [50,-50,50,1],
]

viewPaneFront = [
    [-50,-50,-50,1],
    [50,-50,-50,1],
    [50,50,-50,1],
    [-50,50,-50,1]
]
viewPaneRear = [
    [-50,-50,50,1],
    [50,-50,50,1],
    [50,50,50,1],
    [-50,50,50,1],
]
viewPaneTop = [
    [-50,50,-50,1],
    [50,50,-50,1],
    [50,50,50,1],
    [-50,50,50,1],
]
viewPaneBottom = [
    [-50,-50,-50,1],
    [50,-50,-50,1],
    [50,-50,50,1],
    [-50,-50,50,1],
]
viewPaneLeft = [
    [-50,-50,-50,1],
    [-50,50,-50,1],
    [-50,50,50,1],
    [-50,-50,50,1],
]
viewPaneRight = [
    [50,-50,-50,1],
    [50,50,-50,1],
    [50,50,50,1],
    [50,-50,50,1],
]



def to2d(v):
    return [v[0],v[1],0]


def translate_vertex(v):
    global translate_x
    global translate_y
    global translate_z

    translationMatrix = np.array([[1,0,0,translate_x],
                              [0,1,0,translate_y],
                              [0,0,1,translate_z],
                              [0,0,0,1]])

    source = np.array(v)
    target = translationMatrix.dot(source)

    return target

def translate_to_origin_matrix():
    global translate_x
    global translate_y
    global translate_z

    return np.array([[1,0,0,-translate_x],
                              [0,1,0,-translate_y],
                              [0,0,1,-translate_z],
                              [0,0,0,1]])


def translate_back_matrix():
    global translate_x
    global translate_y
    global translate_z

    return np.array([[1,0,0,translate_x],
                              [0,1,0,translate_y],
                              [0,0,1,translate_z],
                              [0,0,0,1]])


def rotationMatrixX():
    return np.array(
        [[1,0,0,0],
        [0, cos(rotation_angle_x),-sin(rotation_angle_x),0],
        [0, sin(rotation_angle_x),cos(rotation_angle_x),0],
        [0,0,0,1]]
    )

def rotationMatrixY():
    return np.array(
        [[cos(rotation_angle_y),0,sin(rotation_angle_y),0],
        [0,1,0,0],
        [-sin(rotation_angle_y),0,cos(rotation_angle_y),0],
        [0,0,0,1]]
    )

def rotationMatrixZ():
    return np.array(
        [[cos(rotation_angle_z),-sin(rotation_angle_z),0,0],
        [sin(rotation_angle_z),cos(rotation_angle_z),0,0],
        [0,0,1,0],
        [0,0,0,1]]
    )


def rotate_vertex_roudrigues(v):
    global rotation_angle_x
    global rotation_angle_y
    global rotation_angle_z

    center_vector_x = [1000,0,0]
    center_vector_y = [0,1000,0]
    center_vector_z = [0,0,1000]

    x1 = rodrigues_rotation_matrix(center_vector_x, rotation_angle_x)
    y1 = rodrigues_rotation_matrix(center_vector_y, rotation_angle_y)
    z1 = rodrigues_rotation_matrix(center_vector_z, rotation_angle_z)

    x2 = np.zeros((3,4))
    x2[:,:-1] = x1
    x2 = np.vstack((x2, np.array([0,0,0,1])))

    y2 = np.zeros((3,4))
    y2[:,:-1] = y1
    y2 = np.vstack((y2, np.array([0,0,0,1])))

    z2 = np.zeros((3,4))
    z2[:,:-1] = z1
    z2 = np.vstack((z2, np.array([0,0,0,1])))

    return z2.dot(y2.dot(x2.dot(v)))


def rotate_vertex_matrix(v):
    global rotation_angle_x
    global rotation_angle_y
    global rotation_angle_z

    return rotationMatrixZ().dot(rotationMatrixY().dot(rotationMatrixX().dot(v)))



#rotation_method = rotate_vertex_roudrigues
rotation_method = rotate_vertex_matrix


def translate3d():
    global paneLeft
    global paneRight
    global paneTop
    global paneBottom
    global paneFront
    global paneRear

    global viewPaneLeft
    global viewPaneRight
    global viewPaneTop
    global viewPaneBottom
    global viewPaneFront
    global viewPaneRear

    global rotation_method

    for i,v in enumerate(paneLeft):
        temp_vertex = rotation_method(v)
        viewPaneLeft[i] = translate_vertex(temp_vertex)

    for i,v in enumerate(paneRight):
        temp_vertex = rotation_method(v)
        viewPaneRight[i] = translate_vertex(temp_vertex)

    for i,v in enumerate(paneTop):
        temp_vertex = rotation_method(v)
        viewPaneTop[i] = translate_vertex(temp_vertex)

    for i,v in enumerate(paneBottom):
        temp_vertex = rotation_method(v)
        viewPaneBottom[i] = translate_vertex(temp_vertex)

    for i,v in enumerate(paneFront):
        temp_vertex = rotation_method(v)
        viewPaneFront[i] = translate_vertex(temp_vertex)

    for i,v in enumerate(paneRear):
        temp_vertex = rotation_method(v)
        viewPaneRear[i] = translate_vertex(temp_vertex)


@win.event
def on_key_press(symbol, modifiers):
    global translate_x
    global translate_y

    global rotation_axis

    global rotation_angle_x
    global rotation_angle_y
    global rotation_angle_z

    global center

    if symbol == pyglet.window.key.LEFT:
        translate_x -= 10
        translate3d()
    elif symbol == pyglet.window.key.RIGHT:
        translate_x += 10
        translate3d()
    elif symbol == pyglet.window.key.UP:
        translate_y += 10
        translate3d()
    elif symbol == pyglet.window.key.DOWN:
        translate_y -= 10
        translate3d()
    elif symbol == pyglet.window.key.X:
        rotation_axis = 'x'
        rotation_angle_x += DEGREE_TO_RADIAN
        translate3d()
    elif symbol == pyglet.window.key.Y:
        rotation_axis = 'y'
        rotation_angle_y += DEGREE_TO_RADIAN
        translate3d()
    elif symbol == pyglet.window.key.Z:
        rotation_axis = 'z'
        rotation_angle_z += DEGREE_TO_RADIAN
        translate3d()


@win.event
def on_draw():
    global paneLeft
    global paneRight
    global paneTop
    global paneBottom
    global paneFront
    global paneRear

    # Clear buffers
    glClear(GL_COLOR_BUFFER_BIT)

    # Draw outlines only
    glPolygonMode(GL_FRONT_AND_BACK, GL_LINE)


    # Draw some stuff
    glBegin(GL_QUADS)

    glColor3f(1.0,0.0,0.0)
    for v in viewPaneLeft:
        v2d = to2d(v)
        glVertex3f(v2d[0],v2d[1],v2d[2])

    for v in viewPaneRight:
        v2d = to2d(v)
        glVertex3f(v2d[0],v2d[1],v2d[2])

    for v in viewPaneTop:
        v2d = to2d(v)
        glVertex3f(v2d[0],v2d[1],v2d[2])

    for v in viewPaneBottom:
        v2d = to2d(v)
        glVertex3f(v2d[0],v2d[1],v2d[2])

    for v in viewPaneFront:
        v2d = to2d(v)
        glVertex3f(v2d[0],v2d[1],v2d[2])

    for v in viewPaneRear:
        v2d = to2d(v)
        glVertex3f(v2d[0],v2d[1],v2d[2])

    glEnd()


"""Euler-Rodrigues"""
def rodrigues_rotation_matrix(axis, theta):
    axis = np.asarray(axis)
    theta = np.asarray(theta)
    axis = axis/sqrt(np.dot(axis, axis))
    a = cos(theta/2.0)
    b, c, d = -axis*sin(theta/2.0)
    aa, bb, cc, dd = a*a, b*b, c*c, d*d
    bc, ad, ac, ab, bd, cd = b*c, a*d, a*c, a*b, b*d, c*d
    return np.array([[aa+bb-cc-dd, 2*(bc+ad), 2*(bd-ac)],
                     [2*(bc-ad), aa+cc-bb-dd, 2*(cd+ab)],
                     [2*(bd+ac), 2*(cd-ab), aa+dd-bb-cc]])


pyglet.app.run()

 

 

Leave a Reply

Your email address will not be published. Required fields are marked *