{
 "cells": [
  {
   "cell_type": "markdown",
   "metadata": {
    "nbsphinx": "hidden"
   },
   "source": [
    "This notebook is part of https://github.com/AudioSceneDescriptionFormat/splines, see also https://splines.readthedocs.io/.\n",
    "\n",
    "[back to rotation splines](index.ipynb)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# Non-Uniform Catmull--Rom-Like Rotation Splines\n",
    "\n",
    "> What is the best way to allow\n",
    "varying intervals between sequence points in parameter\n",
    "space?\n",
    ">\n",
    "> ---<cite data-cite-t=\"shoemake1985animating\">Shoemake (1985)</cite>, section 6: \"Questions\""
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "In the [uniform case](catmull-rom-uniform.ipynb)\n",
    "we have used\n",
    "[De Casteljau's algorithm with Slerp](de-casteljau.ipynb)\n",
    "to create a \"cubic\" rotation spline.\n",
    "To extend this to the non-uniform case,\n",
    "we can transform the parameter $t \\to \\frac{t - t_i}{t_{i+1} - t_i}$\n",
    "for each spline segment\n",
    "-- as shown in\n",
    "[the notebook about non-uniform Euclidean Bézier splines](../euclidean/bezier-non-uniform.ipynb).\n",
    "This is implemented in the class\n",
    "[splines.quaternion.DeCasteljau](../python-module/splines.quaternion.rst#splines.quaternion.DeCasteljau)."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Assuming the control points at the start and the end of each segment are given\n",
    "(from a sequence of quaternions to be interpolated),\n",
    "we'll also need a way to calculate the missing two control points.\n",
    "For inspiration,\n",
    "we can have a look at the\n",
    "[notebook about non-uniform (Euclidean) Catmull--Rom splines](../euclidean/catmull-rom-non-uniform.ipynb#Using-Non-Uniform-Bézier-Segments)\n",
    "which provides these equations:"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "\\begin{align*}\n",
    "\\boldsymbol{v}_i &= \\frac{\n",
    "\\boldsymbol{x}_{i+1} - \\boldsymbol{x}_i\n",
    "}{\n",
    "t_{i+1} - t_i\n",
    "}\n",
    "\\\\\n",
    "\\boldsymbol{\\dot{x}}_i\n",
    "&= \\frac{\n",
    "(t_{i+1} - t_i) \\, \\boldsymbol{v}_{i-1} + (t_i - t_{i-1}) \\, \\boldsymbol{v}_i\n",
    "}{\n",
    "t_{i+1} - t_{i-1}\n",
    "}\n",
    "\\\\\n",
    "\\boldsymbol{\\tilde{x}}_i^{(+)}\n",
    "&= \\boldsymbol{x}_i + \\frac{(t_{i+1} - t_i) \\, \\boldsymbol{\\dot{x}}_i}{3}\n",
    "\\\\\n",
    "\\boldsymbol{\\tilde{x}}_i^{(-)}\n",
    "&= \\boldsymbol{x}_i - \\frac{(t_i - t_{i-1}) \\, \\boldsymbol{\\dot{x}}_i}{3}\n",
    "\\end{align*}"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "With the\n",
    "[relative rotation](quaternions.ipynb#Relative-Rotation-(Global-Frame-of-Reference))\n",
    "$\\delta_i = q_{i+1} {q_i}^{-1}$\n",
    "we can try to \"translate\" this to quaternions\n",
    "(using some vector operations in the tangent space):"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "\\begin{align*}\n",
    "\\vec{\\rho}_{i} &= \\frac{\\ln(\\delta_{i})}{t_{i+1} - t_i}\n",
    "\\\\\n",
    "\\vec{\\omega}_i &=\n",
    "\\frac{\n",
    "(t_{i+1} - t_i) \\, \\vec{\\rho}_{i-1} + \n",
    "(t_i - t_{i-1}) \\, \\vec{\\rho}_{i}\n",
    "}{\n",
    "t_{i+1} - t_{i-1}\n",
    "}\n",
    "\\\\\n",
    "\\tilde{q}_i^{(+)}\n",
    "&\\overset{?}{=}\n",
    "\\exp\\left(\\frac{t_{i+1} - t_i}{3} \\, \\vec{\\omega}_i\\right) \\, q_i\n",
    "\\\\\n",
    "\\tilde{q}_i^{(-)}\n",
    "&\\overset{?}{=}\n",
    "\\exp\\left(\\frac{t_i - t_{i-1}}{3} \\, \\vec{\\omega}_i\\right)^{-1} \\, q_i,\n",
    "\\end{align*}"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "where $\\vec{\\rho}_{i}$ is the angular velocity\n",
    "along the great arc from $q_i$ to $q_{i+1}$\n",
    "within the parameter interval from $t_i$ to $t_{i+1}$\n",
    "and\n",
    "$\\vec{\\omega}_i$ is the angular velocity\n",
    "of the Catmull--Rom-like quaternion curve\n",
    "at the control point $q_i$\n",
    "(which is reached at parameter value $t_i$).\n",
    "Finally, $\\tilde{q}_i^{(-)}$ and $\\tilde{q}_i^{(+)}$\n",
    "are the Bézier-like control quaternions before and after $q_i$, respectively."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "from splines.quaternion import UnitQuaternion\n",
    "\n",
    "def cr_control_quaternions(qs, ts):\n",
    "    q_1, q0, q1 = qs\n",
    "    t_1, t0, t1 = ts\n",
    "    rho_in = q_1.rotation_to(q0).log_map() / (t0 - t_1)\n",
    "    rho_out = q0.rotation_to(q1).log_map() / (t1 - t0)\n",
    "    w0 = ((t1 - t0) * rho_in + (t0 - t_1) * rho_out) / (t1 - t_1)    \n",
    "    return [\n",
    "        UnitQuaternion.exp_map(-w0 * (t0 - t_1) / 3) * q0,\n",
    "        UnitQuaternion.exp_map(w0 * (t1 - t0) / 3) * q0,\n",
    "    ]"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "This approach is also implemented in the class\n",
    "[splines.quaternion.CatmullRom](../python-module/splines.quaternion.rst#splines.quaternion.CatmullRom)."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "To illustrate this, let's load NumPy,\n",
    "a few helpers from [helper.py](helper.py)\n",
    "and\n",
    "[splines.quaternion.canonicalized()](../python-module/splines.quaternion.rst#splines.quaternion.canonicalized)."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "import numpy as np\n",
    "np.set_printoptions(precision=4)\n",
    "from helper import angles2quat, animate_rotations, display_animation\n",
    "from splines.quaternion import canonicalized"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "The following function can create a closed spline\n",
    "using the above method to calculate control quaternions."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "from splines.quaternion import DeCasteljau\n",
    "\n",
    "def catmull_rom_curve(rotations, grid):\n",
    "    \"\"\"Create a closed Catmull-Rom-like quaternion curve.\"\"\"\n",
    "    assert len(rotations) + 1 == len(grid)\n",
    "    rotations = rotations[-1:] + rotations + rotations[:2]\n",
    "    # Avoid angles of more than 180 degrees (including the added rotations):\n",
    "    rotations = list(canonicalized(rotations))\n",
    "    first_interval = grid[1] - grid[0]\n",
    "    last_interval = grid[-1] - grid[-2]\n",
    "    extended_grid = [grid[0] - last_interval, *grid, grid[-1] + first_interval]\n",
    "    control_points = []\n",
    "    for qs, ts in zip(\n",
    "            zip(rotations, rotations[1:], rotations[2:]),\n",
    "            zip(extended_grid, extended_grid[1:], extended_grid[2:])):\n",
    "        q_before, q_after = cr_control_quaternions(qs, ts)\n",
    "        control_points.extend([q_before, qs[1], qs[1], q_after])\n",
    "    control_points = control_points[2:-2]\n",
    "    segments = list(zip(*[iter(control_points)] * 4))\n",
    "    return DeCasteljau(segments, grid)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "To try this out, we need a few example quaternions and time instances:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "rotations1 = [\n",
    "    angles2quat(0, 0, 180),\n",
    "    angles2quat(0, 45, 90),\n",
    "    angles2quat(90, 45, 0),\n",
    "    angles2quat(90, 90, -90),\n",
    "    angles2quat(180, 0, -180),\n",
    "    angles2quat(-90, -45, 180),\n",
    "]"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "grid1 = 0, 0.5, 2, 5, 6, 7, 9"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "cr = catmull_rom_curve(rotations1, grid1)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "def evaluate(spline, frames=200):\n",
    "    times = np.linspace(\n",
    "        spline.grid[0], spline.grid[-1], frames, endpoint=False)\n",
    "    return spline.evaluate(times)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "ani = animate_rotations(evaluate(cr))"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "display_animation(ani, default_mode='loop')"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Parameterization"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Instead of choosing arbitrary time intervals between control quaternions\n",
    "(via the `grid` argument),\n",
    "we can calculate time intervals based on the control quaternions themselves."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "rotations2 = [\n",
    "    angles2quat(90, 0, -45),\n",
    "    angles2quat(179, 0, 0),\n",
    "    angles2quat(181, 0, 0),\n",
    "    angles2quat(270, 0, -45),\n",
    "    angles2quat(0, 90, 90),\n",
    "]"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "We have seen uniform parameterization already in the\n",
    "[previous notebook](catmull-rom-uniform.ipynb),\n",
    "where each parameter interval is set to 1:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "uniform = catmull_rom_curve(rotations2, grid=range(len(rotations2) + 1))"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "For\n",
    "[chordal parameterization of Euclidean splines](../euclidean/catmull-rom-properties.ipynb#Chordal-Parameterization),\n",
    "we used the Euclidean distance as basis for calculating the time intervals.\n",
    "For rotation splines,\n",
    "it makes more sense to use rotation angles,\n",
    "which are proportional to the lengths of the great arcs\n",
    "between control quaternions:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "angles = np.array([\n",
    "    a.rotation_to(b).angle\n",
    "    for a, b in zip(rotations2, rotations2[1:] + rotations2[:1])])\n",
    "angles"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "The values are probably easier to understand when we show them in degrees:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "np.degrees(angles)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "chordal_grid = np.concatenate([[0], np.cumsum(angles)])"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "chordal = catmull_rom_curve(rotations2, grid=chordal_grid)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "For\n",
    "[centripetal parameterization of Euclidean splines](../euclidean/catmull-rom-properties.ipynb#Centripetal-Parameterization),\n",
    "we used the square root of the Euclidean distances,\n",
    "here we use the square root of the rotation angles:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "centripetal_grid = np.concatenate([[0], np.cumsum(np.sqrt(angles))])"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "centripetal = catmull_rom_curve(rotations2, grid=centripetal_grid)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "ani = animate_rotations({\n",
    "    'uniform': evaluate(uniform),\n",
    "    'centripetal': evaluate(centripetal),\n",
    "    'chordal': evaluate(chordal),\n",
    "})"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "display_animation(ani, default_mode='loop')"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "The class\n",
    "[splines.quaternion.CatmullRom](../python-module/splines.quaternion.rst#splines.quaternion.CatmullRom)\n",
    "provides a parameter `alpha` that allows arbitrary parameterization\n",
    "between *uniform* and *chordal*\n",
    "-- see also [parameterized parameterization of Euclidean splines](../euclidean/catmull-rom-properties.ipynb#Parameterized-Parameterization)."
   ]
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "Python 3",
   "language": "python",
   "name": "python3"
  },
  "language_info": {
   "codemirror_mode": {
    "name": "ipython",
    "version": 3
   },
   "file_extension": ".py",
   "mimetype": "text/x-python",
   "name": "python",
   "nbconvert_exporter": "python",
   "pygments_lexer": "ipython3",
   "version": "3.9.9"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 4
}