{
 "cells": [
  {
   "cell_type": "code",
   "execution_count": 7,
   "metadata": {},
   "outputs": [],
   "source": [
    "import sympy as sm"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 8,
   "metadata": {},
   "outputs": [],
   "source": [
    "sm.init_printing()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 9,
   "metadata": {},
   "outputs": [],
   "source": [
    "theta, alpha = sm.symbols(\"theta, alpha\")"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 10,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/latex": [
       "$\\displaystyle \\left[\\begin{matrix}\\cos{\\left(\\theta \\right)} & \\sin{\\left(\\theta \\right)} & 0\\\\- \\sin{\\left(\\theta \\right)} & \\cos{\\left(\\theta \\right)} & 0\\\\0 & 0 & 1\\end{matrix}\\right]$"
      ],
      "text/plain": [
       "⎡cos(θ)   sin(θ)  0⎤\n",
       "⎢                  ⎥\n",
       "⎢-sin(θ)  cos(θ)  0⎥\n",
       "⎢                  ⎥\n",
       "⎣   0       0     1⎦"
      ]
     },
     "execution_count": 10,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "B_C_A = sm.Matrix([[sm.cos(theta), sm.sin(theta), 0],\n",
    "                   [-sm.sin(theta), sm.cos(theta), 0],\n",
    "                   [0, 0, 1]])\n",
    "B_C_A"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 11,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/latex": [
       "$\\displaystyle \\left[\\begin{matrix}1 & 0 & 0\\\\0 & \\cos{\\left(\\alpha \\right)} & \\sin{\\left(\\alpha \\right)}\\\\0 & - \\sin{\\left(\\alpha \\right)} & \\cos{\\left(\\alpha \\right)}\\end{matrix}\\right]$"
      ],
      "text/plain": [
       "⎡1     0       0   ⎤\n",
       "⎢                  ⎥\n",
       "⎢0  cos(α)   sin(α)⎥\n",
       "⎢                  ⎥\n",
       "⎣0  -sin(α)  cos(α)⎦"
      ]
     },
     "execution_count": 11,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "C_C_B = sm.Matrix([[1, 0, 0],\n",
    "                   [0, sm.cos(alpha), sm.sin(alpha)],\n",
    "                   [0, -sm.sin(alpha), sm.cos(alpha)]])\n",
    "C_C_B"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 12,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/latex": [
       "$\\displaystyle \\left[\\begin{matrix}\\cos{\\left(\\theta \\right)} & \\sin{\\left(\\theta \\right)} & 0\\\\- \\sin{\\left(\\theta \\right)} \\cos{\\left(\\alpha \\right)} & \\cos{\\left(\\alpha \\right)} \\cos{\\left(\\theta \\right)} & \\sin{\\left(\\alpha \\right)}\\\\\\sin{\\left(\\alpha \\right)} \\sin{\\left(\\theta \\right)} & - \\sin{\\left(\\alpha \\right)} \\cos{\\left(\\theta \\right)} & \\cos{\\left(\\alpha \\right)}\\end{matrix}\\right]$"
      ],
      "text/plain": [
       "⎡    cos(θ)          sin(θ)        0   ⎤\n",
       "⎢                                      ⎥\n",
       "⎢-sin(θ)⋅cos(α)  cos(α)⋅cos(θ)   sin(α)⎥\n",
       "⎢                                      ⎥\n",
       "⎣sin(α)⋅sin(θ)   -sin(α)⋅cos(θ)  cos(α)⎦"
      ]
     },
     "execution_count": 12,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "C_C_A = C_C_B * B_C_A\n",
    "C_C_A"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 13,
   "metadata": {},
   "outputs": [],
   "source": [
    "import sympy.physics.mechanics as me"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 14,
   "metadata": {},
   "outputs": [],
   "source": [
    "A = me.ReferenceFrame('A')\n",
    "B = me.ReferenceFrame('B')\n",
    "C = me.ReferenceFrame('C')"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 15,
   "metadata": {},
   "outputs": [],
   "source": [
    "B.orient_axis(A, theta, A.z)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 18,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "image/png": "iVBORw0KGgoAAAANSUhEUgAAABcAAAAVCAYAAACt4nWrAAAAOXRFWHRTb2Z0d2FyZQBNYXRwbG90bGliIHZlcnNpb24zLjguNCwgaHR0cHM6Ly9tYXRwbG90bGliLm9yZy8fJSN1AAAACXBIWXMAABJ0AAASdAHeZh94AAAB4UlEQVR4nM3UTYhOcRTH8c+DCJGZhLBASiQ1E8lCCNmNDVITdmQxJfKyUMfZWGDJQhIbKRbyUhZkUlYsKDUoslASC5JoYmYs7v9hvDzTvFg4q/u/95zvOf9fv3tqfX19GkVmtmBjOV6KiK6GyX+JWiN4Zk7CI8xFDV1YGhFfBgsfNcC3U5iHPTiERTgxWDANJs/MrbiIIxGR5d0x7EdbRFwfNvxfxUCy/N/wMfWHzGzGQbRiPpoxAR/xDFdwcrhumYkDWIc5mFyaN2MFjuFeZk4c8uToxVPcxxu8x1gsxKaS24rdBmnJP9ySmbOwrNxkvOoH2oHFJeVORKwd0uSZ2YRzaCvARjF7MOBf4Djr5x4ZKMYNCZ6ZE1QT16MTO/EyInoy8xI29y/MzDZcLcf1EXE7M1eV2hq21N0yBaP71d6IiOcFPA1rfp8qIq7hdDmeycwZKllrOB8Rl+uyvMWH0gQOZ+Z09GEbpja4+V6sxgLVBp2OF+ig+DwivuFov6ImlecPlga3/kaOiM9oR08B96I9Ij79gJfE49iFJ/iKd7iA5XjdYHIq99QlHaXa/xjhViw6P1bJ9hAtKnmXRMSrYcMzs4ab2IAHWIm7qpt2Yt1ItmJHAXdjR0R0Yzs+q9y17zu2Cpf87xXnRgAAAABJRU5ErkJggg==",
      "text/latex": [
       "$\\displaystyle \\mathbf{\\hat{a}_x}$"
      ],
      "text/plain": [
       "a_x"
      ]
     },
     "execution_count": 18,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "A.x"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 19,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/latex": [
       "$\\displaystyle \\left[\\begin{matrix}\\cos{\\left(\\theta \\right)} & \\sin{\\left(\\theta \\right)} & 0\\\\- \\sin{\\left(\\theta \\right)} & \\cos{\\left(\\theta \\right)} & 0\\\\0 & 0 & 1\\end{matrix}\\right]$"
      ],
      "text/plain": [
       "⎡cos(θ)   sin(θ)  0⎤\n",
       "⎢                  ⎥\n",
       "⎢-sin(θ)  cos(θ)  0⎥\n",
       "⎢                  ⎥\n",
       "⎣   0       0     1⎦"
      ]
     },
     "execution_count": 19,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "B.dcm(A)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 20,
   "metadata": {},
   "outputs": [],
   "source": [
    "C.orient_axis(B, alpha, B.x)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 21,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/latex": [
       "$\\displaystyle \\left[\\begin{matrix}1 & 0 & 0\\\\0 & \\cos{\\left(\\alpha \\right)} & \\sin{\\left(\\alpha \\right)}\\\\0 & - \\sin{\\left(\\alpha \\right)} & \\cos{\\left(\\alpha \\right)}\\end{matrix}\\right]$"
      ],
      "text/plain": [
       "⎡1     0       0   ⎤\n",
       "⎢                  ⎥\n",
       "⎢0  cos(α)   sin(α)⎥\n",
       "⎢                  ⎥\n",
       "⎣0  -sin(α)  cos(α)⎦"
      ]
     },
     "execution_count": 21,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "C.dcm(B)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 22,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/latex": [
       "$\\displaystyle \\left[\\begin{matrix}\\cos{\\left(\\theta \\right)} & \\sin{\\left(\\theta \\right)} & 0\\\\- \\sin{\\left(\\theta \\right)} \\cos{\\left(\\alpha \\right)} & \\cos{\\left(\\alpha \\right)} \\cos{\\left(\\theta \\right)} & \\sin{\\left(\\alpha \\right)}\\\\\\sin{\\left(\\alpha \\right)} \\sin{\\left(\\theta \\right)} & - \\sin{\\left(\\alpha \\right)} \\cos{\\left(\\theta \\right)} & \\cos{\\left(\\alpha \\right)}\\end{matrix}\\right]$"
      ],
      "text/plain": [
       "⎡    cos(θ)          sin(θ)        0   ⎤\n",
       "⎢                                      ⎥\n",
       "⎢-sin(θ)⋅cos(α)  cos(α)⋅cos(θ)   sin(α)⎥\n",
       "⎢                                      ⎥\n",
       "⎣sin(α)⋅sin(θ)   -sin(α)⋅cos(θ)  cos(α)⎦"
      ]
     },
     "execution_count": 22,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "C.dcm(A)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 23,
   "metadata": {},
   "outputs": [],
   "source": [
    "A = me.ReferenceFrame(\"A\")\n",
    "C = me.ReferenceFrame(\"C\")"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 24,
   "metadata": {},
   "outputs": [],
   "source": [
    "psi, theta, phi = sm.symbols('psi, theta, varphi')"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 25,
   "metadata": {},
   "outputs": [],
   "source": [
    "C.orient_body_fixed(A, (phi, theta, phi), 'ZXZ')"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 26,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/latex": [
       "$\\displaystyle \\left[\\begin{matrix}- \\sin^{2}{\\left(\\varphi \\right)} \\cos{\\left(\\theta \\right)} + \\cos^{2}{\\left(\\varphi \\right)} & \\sin{\\left(\\varphi \\right)} \\cos{\\left(\\theta \\right)} \\cos{\\left(\\varphi \\right)} + \\sin{\\left(\\varphi \\right)} \\cos{\\left(\\varphi \\right)} & \\sin{\\left(\\theta \\right)} \\sin{\\left(\\varphi \\right)}\\\\- \\sin{\\left(\\varphi \\right)} \\cos{\\left(\\theta \\right)} \\cos{\\left(\\varphi \\right)} - \\sin{\\left(\\varphi \\right)} \\cos{\\left(\\varphi \\right)} & - \\sin^{2}{\\left(\\varphi \\right)} + \\cos{\\left(\\theta \\right)} \\cos^{2}{\\left(\\varphi \\right)} & \\sin{\\left(\\theta \\right)} \\cos{\\left(\\varphi \\right)}\\\\\\sin{\\left(\\theta \\right)} \\sin{\\left(\\varphi \\right)} & - \\sin{\\left(\\theta \\right)} \\cos{\\left(\\varphi \\right)} & \\cos{\\left(\\theta \\right)}\\end{matrix}\\right]$"
      ],
      "text/plain": [
       "⎡               2                     2                                        ↪\n",
       "⎢          - sin (varphi)⋅cos(θ) + cos (varphi)             sin(varphi)⋅cos(θ) ↪\n",
       "⎢                                                                              ↪\n",
       "⎢                                                                          2   ↪\n",
       "⎢-sin(varphi)⋅cos(θ)⋅cos(varphi) - sin(varphi)⋅cos(varphi)            - sin (v ↪\n",
       "⎢                                                                              ↪\n",
       "⎣                   sin(θ)⋅sin(varphi)                                         ↪\n",
       "\n",
       "↪                                                           ⎤\n",
       "↪ ⋅cos(varphi) + sin(varphi)⋅cos(varphi)  sin(θ)⋅sin(varphi)⎥\n",
       "↪                                                           ⎥\n",
       "↪                    2                                      ⎥\n",
       "↪ arphi) + cos(θ)⋅cos (varphi)            sin(θ)⋅cos(varphi)⎥\n",
       "↪                                                           ⎥\n",
       "↪ -sin(θ)⋅cos(varphi)                           cos(θ)      ⎦"
      ]
     },
     "execution_count": 26,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "C.dcm(A)"
   ]
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "Turtle",
   "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.8"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 2
}