Quaternions

def addVec(vecA,vecB,*vecs):
  """ Returns the sum of scalars or vectors """
  def Run(*args):
    r = 0
    for i in args:
      r += i
    return r

  items = [vecA,vecB]
  items.extend(vecs)

  try:  #Check if is iterable
    return [Run(*i) for i in zip(*items)]

  except TypeError, te:
    return Run(*items)



def subVec (vecA, vecB):
  """ Given 2 vectors subtracts the second to the first """
  try:
    return [i[0]-i[1] for i in zip(vecA,vecB)]

  except TypeError, te:
    return vecA - vecB



    # Quaternions
def normalize(v, tolerance=0.00001):
  mag2 = sum(n * n for n in v)
  if abs(mag2 - 1.0) > tolerance:
    mag = sqrt(mag2)
    v = tuple(n / mag for n in v)
  return v

def q_mult(q1, q2):
  w1, x1, y1, z1 = q1
  w2, x2, y2, z2 = q2
  w = w1 * w2 - x1 * x2 - y1 * y2 - z1 * z2
  x = w1 * x2 + x1 * w2 + y1 * z2 - z1 * y2
  y = w1 * y2 + y1 * w2 + z1 * x2 - x1 * z2
  z = w1 * z2 + z1 * w2 + x1 * y2 - y1 * x2
  return w, x, y, z

def q_conjugate(q):
  w, x, y, z = q
  return (w, -x, -y, -z)

def qv_mult(q1, v1):
  q2 = (0.0,) + v1
  return q_mult(q_mult(q1, q2), q_conjugate(q1))[1:]

def axisangle_to_q(v, theta):
  v = normalize(v)
  x, y, z = v
  theta /= 2
  w = cos(theta)
  x = x * sin(theta)
  y = y * sin(theta)
  z = z * sin(theta)
  return w, x, y, z

def q_to_axisangle(q):
  w, v = q[0], q[1:]
  theta = acos(w) * 2.0
  return normalize(v), theta

def rotateVectorAxisAngle(vector, axis, angle, center=None):
  def Run(vector, axis, angle):
    vector =  tuple(vector)
    axis = tuple (axis)
    quat = axisangle_to_q(axis,angle)
    return list(qv_mult(quat, vector))

  if center == None:
    return Run(vector, axis, angle)

  else:
    vector = subVec(vector,center)
    vector = Run(vector, axis, angle)
    return addVec(vector,center)

def rotateVectorQuat(vector, quat, center=None):
  def Run(vector, quat):
    vector = tuple(vector)
    return list(qv_mult(quat, vector))

  if center == None:
    return Run(vector,quat)

  else:
    vector = subVec(vector,center)
    vector = Run(vector,quat)
    return addVec(vector,center)

def makeQuat(v,tetha):
  return axisangle_to_q(tuple(v), tetha)