{
 "cells": [
  {
   "cell_type": "code",
   "execution_count": 3,
   "metadata": {},
   "outputs": [],
   "source": [
    "import sympy as sp\n",
    "import numpy as np"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 17,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/latex": [
       "$\\displaystyle \\left[\\begin{matrix}\\cos{\\left(\\phi \\right)} \\cos{\\left(\\psi \\right)} & - \\sin{\\left(\\psi \\right)} \\cos{\\left(\\phi \\right)} & \\sin{\\left(\\phi \\right)} & x\\\\\\sin{\\left(\\omega \\right)} \\sin{\\left(\\phi \\right)} \\cos{\\left(\\psi \\right)} + \\sin{\\left(\\psi \\right)} \\cos{\\left(\\omega \\right)} & - \\sin{\\left(\\omega \\right)} \\sin{\\left(\\phi \\right)} \\sin{\\left(\\psi \\right)} + \\cos{\\left(\\omega \\right)} \\cos{\\left(\\psi \\right)} & - \\sin{\\left(\\omega \\right)} \\cos{\\left(\\phi \\right)} & y\\\\\\sin{\\left(\\omega \\right)} \\sin{\\left(\\psi \\right)} - \\sin{\\left(\\phi \\right)} \\cos{\\left(\\omega \\right)} \\cos{\\left(\\psi \\right)} & \\sin{\\left(\\omega \\right)} \\cos{\\left(\\psi \\right)} + \\sin{\\left(\\phi \\right)} \\sin{\\left(\\psi \\right)} \\cos{\\left(\\omega \\right)} & \\cos{\\left(\\omega \\right)} \\cos{\\left(\\phi \\right)} & z\\\\0 & 0 & 0 & 1\\end{matrix}\\right]$"
      ],
      "text/plain": [
       "Matrix([\n",
       "[                                 cos(phi)*cos(psi),                                  -sin(psi)*cos(phi),             sin(phi), x],\n",
       "[sin(omega)*sin(phi)*cos(psi) + sin(psi)*cos(omega), -sin(omega)*sin(phi)*sin(psi) + cos(omega)*cos(psi), -sin(omega)*cos(phi), y],\n",
       "[sin(omega)*sin(psi) - sin(phi)*cos(omega)*cos(psi),  sin(omega)*cos(psi) + sin(phi)*sin(psi)*cos(omega),  cos(omega)*cos(phi), z],\n",
       "[                                                 0,                                                   0,                    0, 1]])"
      ]
     },
     "execution_count": 17,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "omega = sp.symbols('omega')\n",
    "psi = sp.symbols('psi')\n",
    "phi = sp.symbols('phi')\n",
    "x = sp.symbols('x')\n",
    "y = sp.symbols('y')\n",
    "z = sp.symbols('z')\n",
    "\n",
    "\n",
    "rx = sp.Matrix([\n",
    "    [1, 0, 0, 0],\n",
    "    [0, sp.cos(omega), -sp.sin(omega), 0],\n",
    "    [0, sp.sin(omega), sp.cos(omega), 0],\n",
    "    [0, 0, 0, 1],\n",
    "])\n",
    "\n",
    "ry = sp.Matrix([\n",
    "    [sp.cos(phi), 0, sp.sin(phi), 0, ],\n",
    "    [0, 1, 0, 0],\n",
    "    [-sp.sin(phi), 0, sp.cos(phi), 0],\n",
    "    [0, 0, 0, 1],\n",
    "])\n",
    "\n",
    "rz = sp.Matrix([\n",
    "    [sp.cos(psi), -sp.sin(psi), 0, 0],\n",
    "    [sp.sin(psi), sp.cos(psi), 0, 0],\n",
    "    [0, 0, 1, 0],\n",
    "    [0, 0, 0, 1],\n",
    "])\n",
    "\n",
    "t = sp.Matrix([\n",
    "    [0, 0, 0, x],\n",
    "    [0, 0, 0, y],\n",
    "    [0, 0, 0, z],\n",
    "    [0, 0, 0, 0],\n",
    "])\n",
    "\n",
    "tm = t + rx * ry * rz\n",
    "tm"
   ]
  }
 ],
 "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.11.5"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 2
}