import numpy as np

from quaternion import qrot, qinverse


def normalize_screen_coordinates(X, w, h):
  assert X.shape[-1] == 2
  # Normalize so that [0, w] is mapped to [-1, 1], while preserving the aspect ratio
  return X / w * 2 - [1, h / w]


def image_coordinates(X, w, h):
  assert X.shape[-1] == 2
  # Reverse camera frame normalization
  return (X + [1, h / w]) * w / 2


def world_to_camera(X, R, t):
  Rt = qinverse(R)
  Q = np.tile(Rt, (*X.shape[:-1], 1))
  V = X - t
  return qrot(Q, V)


def camera_to_world(X, R, t):
  Q = np.tile(R, (*X.shape[:-1], 1))
  V = X
  return qrot(Q, V) + t