# This module defines a class representing quaternions.
# It contains just what is needed for using quaternions as representations
# of rotations in 3d space. To be completed later...
#
# Written by: Konrad Hinsen <hinsen@cnrs-orleans.fr>
# Last revision: 2001-1-10
#

import Numeric, Transformation

class Quaternion:

    """Quaternion (hypercomplex number)

    This implementation of quaternions is not complete; only the features
    needed for representing rotation matrices by quaternions are
    implemented.

    Constructor:

    - Quaternion(|q0|, |q1|, |q2|, |q3|)  (from four real components)

    - Quaternion(|q|)  (from a sequence containing the four components)

    Quaternions support addition, subtraction, and multiplication,
    as well as multiplication and division by scalars. Division
    by quaternions is not provided, because quaternion multiplication
    is not associative. Use multiplication by the inverse instead.

    The four components can be extracted by indexing.
    """

    def __init__(self, *data):
	if len(data) == 1:
	    self.array = Numeric.array(data[0])
	elif len(data) == 4:
	    self.array = Numeric.array(data)

    is_quaternion = 1

    def __getitem__(self, item):
        return self.array[item]

    def __add__(self, other):
	return Quaternion(self.array+other.array)

    def __sub__(self, other):
	return Quaternion(self.array-other.array)

    def __mul__(self, other):
	if isQuaternion(other):
	    return Quaternion(Numeric.dot(self.asMatrix(),
					  other.asMatrix())[:, 0])
	else:
	    return Quaternion(self.array*other)

    def __rmul__(self, other):
	if isQuaternion(other):
	    raise ValueError, 'Not yet implemented'
	else:
	    return Quaternion(self.array*other)

    def __div__(self, other):
	if isQuaternion(other):
	    raise ValueError, 'Division by quaternions is not allowed.'
	else:
	    return Quaternion(self.array/other)

    def __rdiv__(self, other):
        raise ValueError, 'Division by quaternions is not allowed.'

    def __repr__(self):
	return 'Quaternion(' + str(list(self.array)) + ')'

    def dot(self, other):
	return Numeric.add.reduce(self.array*other.array)

    def norm(self):
        "Returns the norm."
	return Numeric.sqrt(self.dot(self))

    def normalized(self):
        "Returns the quaternion scaled to norm 1."
	return self/self.norm()

    def inverse(self):
        "Returns the inverse."
        import LinearAlgebra
        inverse = LinearAlgebra.inverse(self.asMatrix())
        return Quaternion(inverse[:, 0])

    def asMatrix(self):
        "Returns a 4x4 matrix representation."
	return Numeric.dot(self._matrix, self.array)
    _matrix = Numeric.zeros((4,4,4))
    _matrix[0,0,0] =  1
    _matrix[0,1,1] = -1
    _matrix[0,2,2] = -1
    _matrix[0,3,3] = -1
    _matrix[1,0,1] =  1
    _matrix[1,1,0] =  1
    _matrix[1,2,3] = -1
    _matrix[1,3,2] =  1
    _matrix[2,0,2] =  1
    _matrix[2,1,3] =  1
    _matrix[2,2,0] =  1
    _matrix[2,3,1] = -1
    _matrix[3,0,3] =  1
    _matrix[3,1,2] = -1
    _matrix[3,2,1] =  1
    _matrix[3,3,0] =  1

    def asRotation(self):
        """Returns the corresponding rotation matrix (the quaternion
        must be normalized)."""
	if Numeric.fabs(self.norm()-1.) > 1.e-5:
	    raise ValueError, 'Quaternion not normalized'
	d = Numeric.dot(Numeric.dot(self._rot, self.array), self.array)
	return Transformation.Rotation(d)

    _rot = Numeric.zeros((3,3,4,4))
    _rot[0,0, 0,0] =  1
    _rot[0,0, 1,1] =  1
    _rot[0,0, 2,2] = -1
    _rot[0,0, 3,3] = -1
    _rot[1,1, 0,0] =  1
    _rot[1,1, 1,1] = -1
    _rot[1,1, 2,2] =  1
    _rot[1,1, 3,3] = -1
    _rot[2,2, 0,0] =  1
    _rot[2,2, 1,1] = -1
    _rot[2,2, 2,2] = -1
    _rot[2,2, 3,3] =  1
    _rot[0,1, 1,2] =  2
    _rot[0,1, 0,3] = -2
    _rot[0,2, 0,2] =  2
    _rot[0,2, 1,3] =  2
    _rot[1,0, 0,3] =  2
    _rot[1,0, 1,2] =  2
    _rot[1,2, 0,1] = -2
    _rot[1,2, 2,3] =  2
    _rot[2,0, 0,2] = -2
    _rot[2,0, 1,3] =  2
    _rot[2,1, 0,1] =  2
    _rot[2,1, 2,3] =  2


# Type check
def isQuaternion(x):
    "Returns 1 if |x| is a quaternion."
    return hasattr(x,'is_quaternion')

# Test data

if __name__ == '__main__':

    from VectorModule import Vector
    axis = Vector(1., -2., 1.).normal()
    phi = 0.2
    sin_phi_2 = Numeric.sin(0.5*phi)
    cos_phi_2 = Numeric.cos(0.5*phi)
    quat = Quaternion(cos_phi_2, sin_phi_2*axis[0],
                      sin_phi_2*axis[1], sin_phi_2*axis[2])
    rot = quat.asRotation()
    print rot.axisAndAngle()