"""
Functions to perform operations over vectors and matrices;
deal with homogeneous transforms; convert angles and other structures.
"""
import itertools
from math import sqrt, pi, cos, sin, acos, atan, atan2, degrees, radians
import operator
import numpy as np
from . import generic as gut
# TODO: attribute the code sections that were taken from somewhere else
#==============================================================================
# added to the original refactored code
#==============================================================================
[docs]def radians_to_degrees(radians_):
return degrees(radians_)
# TODO: combine with the corresponding scalar-argument function
[docs]def vec3_radians_to_degrees(vector_):
result = []
for radians_ in vector_:
result.append(radians_to_degrees(radians_))
return tuple(result)
[docs]def degrees_to_radians(degrees_):
return radians(degrees_)
# TODO: combine with the corresponding scalar-argument function
[docs]def vec3_degrees_to_radians(vector_):
result = []
for degrees_ in vector_:
result.append(degrees_to_radians(degrees_))
return tuple(result)
[docs]def np_matrix_to_tuple(array_):
"""Convert Numpy 2D array (i.e. matrix) to a tuple of tuples.
source: http://stackoverflow.com/a/10016379/556413
Example:
>>> arr = numpy.array(((2, 2), (2, -2)))
>>> np_matrix_to_tuple(arr)
((2, 2), (2, -2))
:param array_: 2D array (i.e. matrix)
:type array_: :class:`numpy.ndarray`
:return: matrix as tuple of tuples
"""
return tuple(tuple(x) for x in array_)
[docs]def matrix_multiply(matrix1, matrix2):
"""Return the matrix multiplication of ``matrix1`` and ``matrix2``.
:param matrix1: LxM matrix
:param matrix2: MxN matrix
:return: LxN matrix, product of ``matrix1`` and ``matrix2``
:rtype: tuple of tuples
"""
# TODO: check objects are valid, or use exceptions to catch errors raised
# by numpy
a1 = np.array(matrix1)
a2 = np.array(matrix2)
result = np.dot(a1, a2)
if result.ndim == 1:
return tuple(result)
else:
return np_matrix_to_tuple(result)
[docs]def matrix_as_tuple(matrix_):
"""Convert ``matrix_`` to a tuple.
Example:
>>> matrix_as_tuple(((1, 2), (3, 4)))
(1, 2, 3, 4)
:param matrix_: nested tuples
:type matrix_: tuple
:return: ``matrix_`` flattened as a tuple
:rtype: tuple
"""
#TODO: improve a lot
return gut.nested_iterable_to_tuple(matrix_)
[docs]def matrix_as_3x3_tuples(tuple_9):
"""Return ``tuple_9`` as a 3-tuple of 3-tuples.
:param tuple_9: tuple of 9 elements
:return: ``tuple_9`` formatted as tuple of tuples
"""
#TODO: convert it to handle square matrices of any size
matrix = None
if isinstance(tuple_9, tuple):
if len(tuple_9) == 9:
matrix = (tuple_9[0:3], tuple_9[3:6], tuple_9[6:9])
return matrix
[docs]def calc_acceleration(time_step, vel0, vel1):
"""Calculate the vectorial substraction ``vel1 - vel0`` divided by
``time step``. If any of the vectors is ``None``, then ``None`` is returned.
``vel1`` is the velocity measured ``time_step`` seconds after ``vel0``.
"""
if vel0 is None or vel1 is None:
return None
vel_diff = sub3(vel1, vel0)
return div_by_scalar3(vel_diff, time_step)
[docs]def vector_matrix_vector(vector_, matrix_):
r"""Return the product of ``vector_`` transposed, ``matrix_`` and
``vector`` again, which is a scalar value.
.. math::
v^\top \mathbf{M} v
"""
return np.dot(np.dot(np.array(vector_).T, np.array(matrix_)),
np.array(vector_))
#==============================================================================
# Original code but formatted and some refactor
#==============================================================================
[docs]def sign(x):
"""Return ``1.0`` if ``x`` is positive, ``-1.0`` otherwise."""
if x > 0.0:
return 1.0
else:
return -1.0
[docs]def length2(vector):
"""Return the length of a 2-dimension ``vector``."""
return sqrt(vector[0] ** 2 + vector[1] ** 2)
[docs]def length3(vector):
"""Return the length of a 3-dimension ``vector``."""
#TODO: convert it so it can handle vector of any dimension
return sqrt(vector[0] ** 2 + vector[1] ** 2 + vector[2] ** 2)
[docs]def neg3(vector):
"""Return the negation of 3-dimension ``vector``."""
#TODO: convert it so it can handle vector of any dimension
return (-vector[0], -vector[1], -vector[2])
[docs]def add3(vector1, vector2):
"""Return the sum of 3-dimension ``vector1`` and ``vector2``."""
#TODO: convert it so it can handle vector of any dimension
return (vector1[0] + vector2[0],
vector1[1] + vector2[1],
vector1[2] + vector2[2])
[docs]def sub3(vector1, vector2):
"""Return the difference between 3-dimension ``vector1`` and ``vector2``."""
#TODO: convert it so it can handle vector of any dimension
return (vector1[0] - vector2[0],
vector1[1] - vector2[1],
vector1[2] - vector2[2])
[docs]def mult_by_scalar3(vector, scalar):
"""Return 3-dimension ``vector`` multiplied by ``scalar``."""
#TODO: convert it so it can handle vector of any dimension
return (vector[0] * scalar, vector[1] * scalar, vector[2] * scalar)
[docs]def div_by_scalar3(vector, scalar):
"""Return 3-dimension ``vector`` divided by ``scalar``."""
#TODO: convert it so it can handle vector of any dimension
return (vector[0] / scalar, vector[1] / scalar, vector[2] / scalar)
[docs]def dist3(vector1, vector2):
"""Return the distance between point 3-dimension ``vector1`` and
``vector2``.
"""
#TODO: convert it so it can handle vector of any dimension
return length3(sub3(vector1, vector2))
[docs]def norm3(vector):
"""Return the unit length vector parallel to 3-dimension ``vector``."""
#l = length3(vector)
#if l > 0.0:
# return (vector[0] / l, vector[1] / l, vector[2] / l)
#else:
# return (0.0, 0.0, 0.0)
return unitize(vector)
[docs]def unitize(vector_):
"""Unitize a vector, i.e. return a unit-length vector parallel to
``vector``.
"""
len_ = sqrt(sum(itertools.imap(operator.mul, vector_, vector_)))
size_ = len(vector_)
if len_ > 0.0:
div_vector = (len_,) * size_ # (len_, len_, len_, ...)
return tuple(itertools.imap(operator.div, vector_, div_vector))
else:
return (0.0, 0.0, 0.0)
[docs]def dot_product3(vector1, vector2):
"""Return the dot product of 3-dimension ``vector1`` and ``vector2``."""
return dot_product(vector1, vector2)
[docs]def dot_product(vec1, vec2):
"""Efficient dot-product operation between two vectors of the same size.
source: http://docs.python.org/library/itertools.html
"""
return sum(itertools.imap(operator.mul, vec1, vec2))
[docs]def cross_product(vector1, vector2):
"""Return the cross product of 3-dimension ``vector1`` and ``vector2``."""
return (vector1[1] * vector2[2] - vector1[2] * vector2[1],
vector1[2] * vector2[0] - vector1[0] * vector2[2],
vector1[0] * vector2[1] - vector1[1] * vector2[0])
[docs]def project3(vector, unit_vector):
"""Return projection of 3-dimension ``vector`` onto unit 3-dimension
``unit_vector``.
"""
#TODO: convert it so it can handle vector of any dimension
return mult_by_scalar3(vector, dot_product3(norm3(vector), unit_vector))
[docs]def acos_dot3(vector1, vector2):
"""Return the angle between unit 3-dimension ``vector1`` and ``vector2``."""
x = dot_product3(vector1, vector2)
if x < -1.0:
return pi
elif x > 1.0:
return 0.0
else:
return acos(x)
[docs]def rotate3(rot, vector):
"""Return the rotation of 3-dimension ``vector`` by 3x3 (row major) matrix
``rot``.
"""
return (vector[0] * rot[0] + vector[1] * rot[1] +
vector[2] * rot[2],
vector[0] * rot[3] + vector[1] * rot[4] +
vector[2] * rot[5],
vector[0] * rot[6] + vector[1] * rot[7] +
vector[2] * rot[8])
[docs]def transpose3(matrix):
"""Return the inversion (transpose) of 3x3 rotation matrix ``matrix``."""
#TODO: convert it so it can handle vector of any dimension
return (matrix[0], matrix[3], matrix[6],
matrix[1], matrix[4], matrix[7],
matrix[2], matrix[5], matrix[8])
[docs]def z_axis(rot):
"""Return the z-axis vector from 3x3 (row major) rotation matrix
``rot``.
"""
#TODO: convert it so it can handle vector of any dimension, and any column
return (rot[2], rot[5], rot[8])
#==============================================================================
# TESTS
#==============================================================================
def _test_angular_conversions(angle_):
#x = 2.0/3*pi
y = radians_to_degrees(angle_)
z = degrees_to_radians(y)
dif = angle_ - z
print('radians: %f' % angle_)
print('degrees: %f' % y)
print('difference: %f' % dif)
if __name__ == '__main__':
_test_angular_conversions(2.0 / 3 * pi)
_test_angular_conversions(4.68 * pi)
radians_ = (2.0 / 3 * pi, 2.0 * pi, 1.0 / 4 * pi)
degrees_ = (120, 360, 45)
print(vec3_radians_to_degrees(radians_))
print(vec3_degrees_to_radians(degrees_))
print(radians_)