quaternion.py

from .models import Point
import numpy as np 

def cross(a, b):
  x = a.y * b.z - a.z * b.y
  y = a.z * b.x - a.x * b.z
  z = a.x * b.y - a.y * b.x

  return Point(x, y, z)

# get vector pointing from point a to point b
def vector_from_to(a, b):
  return Point(b.x - a.x, b.y - a.y, b.z - a.z)

def scale(v, d):
  return Point(v.x / d, + v.y / d , v.z / d)

def dot(a, b):
  return a.x * b.x + a.y * b.y + a.z * b.z

def length_squared(v):
  return v.x**2 + v.y**2 + v.z**2

def add_vectors(a, b):
  return Point(a.x + b.x, a.y + b.y, a.z + b.z)

# quaternion representing rotation of vector q to vector v
def quaternion_from_to(a, b):
  
  # get cross product 
  v = cross(a, b)

  # get scalar component
  w = np.sqrt(length_squared(a) * length_squared(b)) + dot(a, b)

  # normalize q
  d = w * w + length_squared(v)
  w = w / d
  v = scale(v, d)

  return (w, v)

def quaternion_conjugate(q):
  qw, qv = q
  return qw, scale(qv, -1)

def hamilton_product(n, m):
  a1, p = n
  b1, c1, d1 = p.x, p.y, p.z
  
  a2, r = m
  b2, c2, d2 = r.x, r.y, r.z

  w = a1 * a2 - b1 * b2 - c1 * c2 - d1 * d2
  x = a1 * b2 + b1 * a2 + c1 * d2 - d1 * c2
  y = a1 * c2 - b1 * d2 + c1 * a2 + d1 * b2
  z = a1 * d2 + b1 * c2 - c1 * b2 + d1 * a2

  return (w, Point(x, y, z))

def rotate_vector_with_quarternion(p, q):
  # qw, qv = q (scalar qw), (vector(qv))
  q_ = quaternion_conjugate(q)

  qp = hamilton_product(q, (0, p))
  qpq_ = hamilton_product(qp, q_)

  w, r = qpq_
  assert w == 0
  return r

# rotate plane from normal v to normal u around point a, compute the new position point b after rotation? 
def change_normal(v, u, a, b):
    
  q = quaternion_from_to(v, u)
  # get point direction relative to pivot
  v = vector_from_to(a, b)
  # rotate this vector
  v = rotate_vector_with_quarternion(q, v)
  # get new point 
  b = add_vectors(a, b)
  return b