Skip to content
This repository

HTTPS clone URL

Subversion checkout URL

You can clone with HTTPS or Subversion.

Download ZIP
branch: master
Fetching contributors…

Octocat-spinner-32-eaf2f5

Cannot retrieve contributors at this time

executable file 325 lines (280 sloc) 9.646 kb
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75 76 77 78 79 80 81 82 83 84 85 86 87 88 89 90 91 92 93 94 95 96 97 98 99 100 101 102 103 104 105 106 107 108 109 110 111 112 113 114 115 116 117 118 119 120 121 122 123 124 125 126 127 128 129 130 131 132 133 134 135 136 137 138 139 140 141 142 143 144 145 146 147 148 149 150 151 152 153 154 155 156 157 158 159 160 161 162 163 164 165 166 167 168 169 170 171 172 173 174 175 176 177 178 179 180 181 182 183 184 185 186 187 188 189 190 191 192 193 194 195 196 197 198 199 200 201 202 203 204 205 206 207 208 209 210 211 212 213 214 215 216 217 218 219 220 221 222 223 224 225 226 227 228 229 230 231 232 233 234 235 236 237 238 239 240 241 242 243 244 245 246 247 248 249 250 251 252 253 254 255 256 257 258 259 260 261 262 263 264 265 266 267 268 269 270 271 272 273 274 275 276 277 278 279 280 281 282 283 284 285 286 287 288 289 290 291 292 293 294 295 296 297 298 299 300 301 302 303 304 305 306 307 308 309 310 311 312 313 314 315 316 317 318 319 320 321 322 323 324
# -*- coding: utf-8 -*-
"""3x3 Matrix which supports rotation, translation, scale and skew.

Matrices are laid out in row-major format and can be loaded directly
into OpenGL.
To convert to column-major format, transpose the array using the
numpy.array.T method.
"""
from __future__ import absolute_import, division, print_function, unicode_literals
import math

import numpy

from pyrr import quaternion
from pyrr.utils import all_parameters_as_numpy_arrays

def create_identity():
    """Creates a new matrix33 and sets it to
an identity matrix.

:rtype: numpy.array
:return: A matrix representing an identity matrix with shape (3,3).
"""
    return numpy.identity( 3, dtype = 'float' )

@all_parameters_as_numpy_arrays
def create_from_matrix44( mat ):
    """Creates a Matrix33 from a Matrix44.

:rtype: numpy.array
:return: A matrix with shape (3,3) with the input matrix rotation.
"""
    return numpy.array( mat[ 0:3, 0:3 ] )

def create_from_eulers( eulers ):
    """Creates a matrix from the specified Euler rotations.

:param numpy.array eulers: A set of euler rotations in the format
specified by the euler modules.
:rtype: numpy.array
:return: A matrix with shape (3,3) with the euler's rotation.
"""
    pitchOver2 = eulers[ 0 ] * 0.5
    rollOver2 = eulers[ 1 ] * 0.5
    yawOver2 = eulers[ 2 ] * 0.5
    
    sinPitch = math.sin( pitchOver2 )
    cosPitch = math.cos( pitchOver2 )
    sinRoll = math.sin( rollOver2 )
    cosRoll = math.cos( rollOver2 )
    sinYaw = math.sin( yawOver2 )
    cosYaw = math.cos( yawOver2 )
    
    return numpy.array(
        [
            # m1
            [
                # m11 = cy * cr + sy * sp * sr
                (cosYaw * cosRoll) + (sinYaw * sinPitch * sinRoll),
                # m12 = -cy * sr + sy * sp * cr
                (-cosYaw * sinRoll) + (sinYaw * sinPitch * cosRoll),
                # m13 = sy * cp
                sinYaw * cosPitch
                ],
            # m2
            [
                # m21 = sr * cp
                sinRoll * cosPitch,
                # m22 = cr * cp
                cosRoll * cosPitch,
                # m23 = -sp
                -sinPitch
                ],
            # m3
            [
                # m31 = -sy * cr + cy * sp * sr
                (-sinYaw * cosRoll) + (cosYaw * sinPitch * sinRoll),
                # m32 = sr * sy + cy * sp * cr
                (sinRoll * sinYaw) + (cosYaw * sinPitch * cosRoll),
                # m33 = cy * cp
                cosYaw * cosPitch
                ]
            ]
        )

def create_from_quaternion( quat ):
    """Creates a matrix with the same rotation as a quaternion.

:param quat: The quaternion to create the matrix from.
:rtype: numpy.array
:return: A matrix with shape (3,3) with the quaternion's rotation.
"""
    x = quat[ 0 ]
    y = quat[ 1 ]
    z = quat[ 2 ]
    w = quat[ 3 ]

    y2 = y**2
    x2 = x**2
    z2 = z**2
    xy = x * y
    xz = x * z
    yz = y * z
    wx = w * x
    wy = w * y
    wz = w * z
    
    return numpy.array(
        [
            # m1
            [
                # m11 = 1.0 - 2.0 * (q.y * q.y + q.z * q.z)
                1.0 - 2.0 * (y2 + z2),
                # m12 = 2.0 * (q.x * q.y + q.w * q.z)
                2.0 * (xy + wz),
                # m13 = 2.0 * (q.x * q.z - q.w * q.y)
                2.0 * (xz - wy)
                ],
            # m2
            [
                # m21 = 2.0 * (q.x * q.y - q.w * q.z)
                2.0 * (xy - wz),
                # m22 = 1.0 - 2.0 * (q.x * q.x + q.z * q.z)
                1.0 - 2.0 * (x2 + z2),
                # m23 = 2.0 * (q.y * q.z + q.w * q.x)
                2.0 * (yz + wx)
                ],
            # m3
            [
                # m31 = 2.0 * (q.x * q.z + q.w * q.y)
                2.0 * (xz + wy),
                # m32 = 2.0 * (q.y * q.z - q.w * q.x)
                2.0 * (yz - wx),
                # m33 = 1.0 - 2.0 * (q.x * q.x + q.y * q.y)
                1.0 - 2.0 * (x2 + y2)
                ]
            ]
        )

def create_from_inverse_of_quaternion( quat ):
    """Creates a matrix with the inverse rotation of a quaternion.

:param numpy.array quat: The quaternion to make the matrix from (shape 4).
:rtype: numpy.array
:return: A matrix with shape (3,3) that respresents the inverse of
the quaternion.
"""
    x = quat[ 0 ]
    y = quat[ 1 ]
    z = quat[ 2 ]
    w = quat[ 3 ]

    x2 = x**2
    y2 = y**2
    z2 = z**2
    wx = w * x
    wy = w * y
    xy = x * y
    wz = w * z
    xz = x * z
    yz = y * z
    
    return numpy.array(
        [
            # m1
            [
                # m11 = 1.0 - 2.0 * (q.y * q.y + q.z * q.z)
                1.0 - 2.0 * (y2 + z2),
                # m12 = 2.0 * (q.x * q.y - q.w * q.z)
                2.0 * (xy - wz),
                # m13 = 2.0 * ( q.x * q.z + q.w * q.y)
                2.0 * (xz + wy)
                ],
            # m2
            [
                # m21 = 2.0 * (q.x * q.y + q.w * q.z)
                2.0 * (xy + wz),
                # m22 = 1.0 - 2.0 * (q.x * q.x + q.z * q.z)
                1.0 - 2.0 * (x2 + z2),
                # m23 = 2.0 * (q.y * q.z - q.w * q.x)
                2.0 * (yz - wx)
                ],
            # m3
            [
                # m31 = 2.0 * (q.x * q.z - q.w * q.y)
                2.0 * (xz - wy),
                # m32 = 2.0 * (q.y * q.z + q.w * q.x)
                2.0 * (yz - wx),
                # m33 = 1.0 - 2.0 * (q.x * q.x + q.y * q.y)
                1.0 - 2.0 * (x2 + y2)
                ]
            ]
        )

def create_from_scale( scale ):
    """Creates an identity matrix with the scale set.

:param numpy.array scale: The scale to apply as a vector (shape 3).
:rtype: numpy.array
:return: A matrix with shape (3,3) with the scale
set to the specified vector.
"""
    # apply the scale to the values diagonally
    # down the matrix
    return numpy.diagflat( scale )

def create_from_x_rotation( theta ):
    """Creates a matrix with the specified rotation about the X axis.

:param float theta: The rotation, in radians, about the X-axis.
:rtype: numpy.array
:return: A matrix with the shape (3,3) with the specified rotation about
the X-axis.
.. seealso:: http://en.wikipedia.org/wiki/Rotation_matrix#In_three_dimensions
"""
    cosT = math.cos( theta )
    sinT = math.sin( theta )

    return numpy.array(
        [
            [ 1.0, 0.0, 0.0 ],
            [ 0.0, cosT,-sinT ],
            [ 0.0, sinT, cosT ]
            ]
        )

def create_from_y_rotation( theta ):
    """Creates a matrix with the specified rotation about the Y axis.

:param float theta: The rotation, in radians, about the Y-axis.
:rtype: numpy.array
:return: A matrix with the shape (3,3) with the specified rotation about
the Y-axis.
.. seealso:: http://en.wikipedia.org/wiki/Rotation_matrix#In_three_dimensions
"""
    cosT = math.cos( theta )
    sinT = math.sin( theta )
    
    return numpy.array(
        [
            [ cosT, 0.0, sinT ],
            [ 0.0, 1.0, 0.0 ],
            [-sinT, 0.0, cosT ]
            ]
        )

def create_from_z_rotation( theta ):
    """Creates a matrix with the specified rotation about the Z axis.

:param float theta: The rotation, in radians, about the Z-axis.
:rtype: numpy.array
:return: A matrix with the shape (3,3) with the specified rotation about
the Z-axis.
.. seealso:: http://en.wikipedia.org/wiki/Rotation_matrix#In_three_dimensions
"""
    cosT = math.cos( theta )
    sinT = math.sin( theta )
    
    return numpy.array(
        [
            [ cosT,-sinT, 0.0 ],
            [ sinT, cosT, 0.0 ],
            [ 0.0, 0.0, 1.0 ]
            ]
        )

def apply_to_vector( mat, vec ):
    """Apply a matrix to a vector.

The matrix's rotation are applied to the vector.
Supports multiple matrices and vectors.

:param numpy.array mat: The rotation / translation matrix.
Can be a list of matrices.
:param numpy.array vec: The vector to modify.
Can be a list of vectors.
:rtype: numpy.array
:return: The vectors rotated by the specified matrix.
"""
    if vec.size == 3:
        return numpy.dot( vec, mat )
    elif vec.size == 4:
        # convert to vec3 and undo w component
        vec3 = vec[:-1] / vec[-1]
        vec3 = numpy.dot( vec3, mat )
        # convert back to vec4
        return numpy.array( [ vec3[0], vec3[1], vec3[2], 1.0 ] )
    else:
        raise ValueError( "Vector size unsupported" )

def multiply( m1, m2, out = None ):
    """Multiply two matricies, m1 . m2.

This is essentially a wrapper around
numpy.dot( m1, m2 )

:param numpy.array m1: The first matrix.
Can be a list of matrices.
:param numpy.array m2: The second matrix.
Can be a list of matrices.
:rtype: numpy.array
:return: A matrix that results from multiplying m1 by m2.
"""
    # using an input as the out value will cause corruption
    if out == m1 or out == m2:
        raise ValueError( "Output must not be one of the inputs, use assignment instead" )

    return numpy.dot( m1, m2, out = out )

def inverse( mat ):
    """Returns the inverse of the matrix.

This is essentially a wrapper around numpy.linalg.inv.

:param numpy.array m: A matrix.
:rtype: numpy.array
:return: The inverse of the specified matrix.

.. seealso:: http://docs.scipy.org/doc/numpy/reference/generated/numpy.linalg.inv.html
"""
    return numpy.linalg.inv( mat )
Something went wrong with that request. Please try again.