"""Functions for creating and manipulating symbolic homogeneous transforms using sympy.

Copyright (c) 2014-2017, Garth Zeglin.  All rights reserved. Licensed under the terms
of the BSD 3-clause license as included in LICENSE.

- All transforms are represented as a 4x4 homogenous matrix.
- All operators are pure Python for portability.
- Transform composition is just normal matrix multiplication.

N.B. this is not well tested, efficient, or optimized.

"""
import sympy as sym

def rotation_x(angle):
    """Return a homogeneous 4x4 transform to rotate 'angle' radians around X."""
    sn = sym.sin(angle)
    cs = sym.cos(angle)
    return sym.Matrix([[   1,   0,   0,   0],
                       [   0,  cs, -sn,   0],
                       [   0,  sn,  cs,   0],
                       [   0,   0,   0,   1]])

def rotation_y( angle ):
    """Return a homogeneous 4x4 transform to rotate 'angle' radians around Y."""
    sn = sym.sin(angle)
    cs = sym.cos(angle)
    return sym.Matrix([[ cs,   0,  sn,   0],
                       [  0,   1,   0,   0],
                       [-sn,   0,  cs,   0],
                       [  0,   0,   0,   1]])

def rotation_z( angle ):
    """Return a homogeneous 4x4 transform to rotate 'angle' radians around Z."""
    sn = sym.sin(angle)
    cs = sym.cos(angle)
    return sym.Matrix([[ cs, -sn,   0,   0],
                       [ sn,  cs,   0,   0],                         
                       [  0,   0,   1,   0],
                       [  0,   0,   0,   1]])

def translation( x, y, z ):
    """Return a homogeneous 4x4 transform to translate along the given vector specified as individual components."""
    return sym.Matrix([[  1,   0,   0,   x],
                       [  0,   1,   0,   y],                         
                       [  0,   0,   1,   z],
                       [  0,   0,   0,   1]])