several-ellipses-bouncing-uneven-street-JMversion.ipynb

{
 "cells": [
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "import sympy as sm\n",
    "import sympy.physics.mechanics as me\n",
    "import numpy as np\n",
    "\n",
    "from scipy.optimize import minimize, root\n",
    "from scipy.integrate import solve_ivp\n",
    "import itertools as itt\n",
    "from matplotlib import animation\n",
    "from IPython.display import HTML\n",
    "import matplotlib as mp\n",
    "import matplotlib\n",
    "from matplotlib import patches\n",
    "import matplotlib.pyplot as plt\n",
    "%matplotlib inline\n",
    "import time\n",
    "import copy\n",
    "matplotlib.rcParams['animation.embed_limit'] = 2**128"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Needed to exit the loop when trying to place the ellipses, and when finding contact points during integration."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "class Rausspringen(Exception):\n",
    "    pass"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "This creates a decorator to test functions for usage of CPU time, line by line.\\\n",
    "To see the results, this line: *profiler.print_stats()* must be added. "
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "from line_profiler import LineProfiler\n",
    "\n",
    "profiler = LineProfiler()\n",
    "\n",
    "def profile(func):\n",
    "    def inner(*args, **kwargs):\n",
    "        profiler.add_function(func)\n",
    "        profiler.enable_by_count()\n",
    "        return func(*args, **kwargs)\n",
    "    return inner\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Homogenious ellipses of mass *m* and parameters *a, b* are dropped or thrown on an uneven street. A particle of mass $m_o$ may be attached anywhere within each ellipse.\\\n",
    "The street is a 'curve' in the X/Y plane, gravitation points in the negative Y - direction.\\\n",
    "The impact is modelled using the **Hunt-Crossley method**, details below.\n",
    "\n",
    "I cannot model friction between the ellipse and the street (see sympy issue 25307).\n",
    "\n",
    "**Parameters**\n",
    "- *N*: inertial frame\n",
    "- $A_{list}$: frames fixed to the ellipses\n",
    "- $O$: point fixed in *N*\n",
    "- $Dmc_{list}$: center of the ellipses\n",
    "- $Po_{list}$: location of the particles fixed to the ellipses\n",
    "\n",
    "- $q_{list}, u_{list}$: angle of rotation of the ellipses, its speed\n",
    "- x_list, y_list, ux_list, uy_list*: coordinates of the center of the ellipse, its speeds\n",
    "\n",
    "- $\\epsilon_{s_{list}}$: contact angel for colission with the street\n",
    "- *xs_list*: holds the X coordinates of the colission points on the street.\n",
    "- *ls_list*: distance from ellipses to the street.\n",
    "- *le_list*: distance from one ellipse to another one\n",
    "- $\\epsilon_{e_{list}}$ holds the colission angles of each pair of ellipses\n",
    "\n",
    "\n",
    "- $m, m_o$: mass of the ellipse, of the particle attached to the ellipse\n",
    "- *a, b*: semi axes of the ellipse\n",
    "- *amplitude, frequenz*: parameters for the street.\n",
    "- $i_{ZZ}$: moment of inertia of the ellipse around the Z axis\n",
    "- $\\alpha_{list}, \\beta_{list}$: determine the location of the particel w.r.t. Dmc\n",
    "- $\\nu_s, \\nu_e, EY_s, EY_e$: Poisson's ratio and Young's mudulus of street and ellipses respectively"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "start = time.time()\n",
    "\n",
    "#====================\n",
    "n = 2\n",
    "#====================\n",
    "if n < 2 or isinstance(n, int) != True:\n",
    "    raise Exception('n must be an integer > 1')\n",
    "\n",
    "    \n",
    "N = me.ReferenceFrame('N')\n",
    "O = me.Point('O')\n",
    "O.set_vel(N, 0)\n",
    "t = me.dynamicsymbols._t\n",
    "\n",
    "m, mo, g, a, b, reibung = sm.symbols('m, mo, g, a, b, reibung')\n",
    "nue, nus, EYe, EYs, ctau = sm.symbols('nue, nus EYe, EYs, ctau')\n",
    "amplitude, frequenz = sm.symbols('amplitude, frequenz')\n",
    "x = sm.symbols('x')\n",
    "\n",
    "q_list = list(me.dynamicsymbols(f'q:{n}'))\n",
    "x_list = list(me.dynamicsymbols(f'x:{n}'))\n",
    "y_list = list(me.dynamicsymbols(f'y:{n}'))\n",
    "\n",
    "u_list  = list(me.dynamicsymbols(f'u:{n}'))\n",
    "ux_list = list(me.dynamicsymbols(f'ux:{n}'))\n",
    "uy_list = list(me.dynamicsymbols(f'uy:{n}'))\n",
    "\n",
    "A_list     = list(sm.symbols(f'A:{n}', cls=me.ReferenceFrame))\n",
    "Dmc_list   = list(sm.symbols(f'Dmc:{n}', cls=me.Point))\n",
    "Po_list    = list(sm.symbols(f'Po:{n}', cls=me.Point))\n",
    "CPhes_list = list(sm.symbols(f'CPhes:{n}', cls= me.Point))                 # contact point on the ellipse w.r.t. the street\n",
    "CPhss_list = list(sm.symbols(f'CPhss:{n}', cls=me.Point))                  # \"Partner point\" on the street\n",
    "CPhee_list = list(sm.symbols(f'CPhee:{n}', cls=me.Point))                   # Contact point of ellipse when colliding with another ellipse\n",
    "\n",
    "rhodts_list   = list(sm.symbols(f'rhodts:{n}'))  \n",
    "xs_list       = list(sm.symbols(f'xs:{n}'))\n",
    "ls_list       = list(sm.symbols(f'ls:{n}'))\n",
    "epsilons_list = list(sm.symbols(f'epsilons:{n}'))\n",
    "alpha_list    = list(sm.symbols(f'alpha:{n}'))\n",
    "beta_list     = list(sm.symbols(f'beta:{n}'))\n",
    "\n",
    "rhodte_list   = [sm.symbols(f'rhodte{i}{j}') for i, j in itt.permutations(range(n), r=2)]\n",
    "le_list       = [sm.symbols(f'le{i}{j}') for i, j in itt.permutations(range(n), r=2)]\n",
    "epsilone_list = [sm.symbols(f'epsilone{i}{j}, epsilone{j}{i}') for i, j in itt.permutations(range(n), r=2)]\n",
    "richtung_list = [sm.symbols(f'richtungx{i}{j}, richtungy{i}{j}')for i, j in itt.permutations(range(n), r=2)]\n",
    "\n",
    "Body1 = []\n",
    "Body2 = []\n",
    "iZZ   = 0.25 * m * (a**2 + b**2)  # moment of inertia of the ellipse w.r.t its center, normal to its plane\n",
    "\n",
    "for i in range(n):\n",
    "    A_list[i].orient_axis(N, q_list[i], N.z)\n",
    "    A_list[i].set_ang_vel(N, u_list[i] * N.z)\n",
    "\n",
    "    Dmc_list[i].set_pos(O, x_list[i]*N.x + y_list[i]*N.y)\n",
    "    Dmc_list[i].set_vel(N, ux_list[i]*N.x + uy_list[i]*N.y)\n",
    "\n",
    "    CPhes_list[i].set_pos(Dmc_list[i], a*sm.cos(epsilons_list[i])*A_list[i].x + b*sm.sin(epsilons_list[i])*A_list[i].y)\n",
    "    CPhes_list[i].v2pt_theory(Dmc_list[i], N, A_list[i])\n",
    "\n",
    "    Po_list[i].set_pos(Dmc_list[i], a*alpha_list[i]*A_list[i].x + b*beta_list[i]*A_list[i].y)\n",
    "    Po_list[i].v2pt_theory(Dmc_list[i], N, A_list[i])\n",
    "\n",
    "    I = me.inertia(A_list[i], 0, 0, iZZ)                                              \n",
    "    body = me.RigidBody('body' + str(i), Dmc_list[i], A_list[i], m, (I, Dmc_list[i]))\n",
    "    teil = me.Particle('teil' + str(i), Po_list[i], mo)\n",
    "    Body1.append(body)\n",
    "    Body2.append(teil)\n",
    "\n",
    "BODY = Body1 + Body2"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Model the street.\\\n",
    "It is a parabola, open to the top, with superimposed sinus waves.\\\n",
    "Then I calculate the formula for its osculating circle, the formula of which I found in the internet."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "#Modeling the street\n",
    "#============================================\n",
    "rumpel = 5  # the higher the number the more 'uneven the street'\n",
    "#============================================\n",
    "def gesamt1(x, amplitude, frequenz):\n",
    "    strasse = sum([amplitude/j * sm.sin(j*frequenz * x) for j in range(1, rumpel)])\n",
    "    strassen_form = (frequenz/2. * x)**2\n",
    "    gesamt = strassen_form + strasse\n",
    "    return gesamt\n",
    "\n",
    "r_max = ((sm.S(1.) + (gesamt1(x, amplitude, frequenz).diff(x))**2 )**sm.S(3/2)/\n",
    "         gesamt1(x, amplitude, frequenz).diff(x, 2))"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "**Find the point where the ellipse hits the street**\\\n",
    "The idea is as follows:\\\n",
    "When the ellipse hits the street, the tangent at the ellipse at the hitting point, and the tangent at the street at the hitting point must be parallel. So, I look for the point where the ellipse would touch the street if it was inflated to touch the street for every point of the integration time. This sequence of potential hitting points will eventually give the real hitting point.\n",
    "- let $CP_{hs}$ be the point of the street where a multiple of the vector $\\hat n$ which is normal to the tanget of the ellipse at $CP_h \\in$ *circumference of ellipse* intersects with the street below\n",
    "-  is the tangent of the ellipse at $CP_h$ parallel to the tangent of the street at $CP_{hs}$?\n",
    "- if YES, $CP_{hs}$ is a potential impact point.\n",
    "- collect all potential impact points, and select the one closest to the ellipse. This is the point the ellipse would touch, if it were 'blown up' to just touch the street.\n",
    "\n",
    "All this has to be done numerically during integration, I could not come up with another way.\n",
    "\n",
    "In more **detail**:\\\n",
    "To get the derivative of the ellipse at the point $(x, y) \\in $ {circumference of ellipse} calculate: $\\dfrac{d}{dx}(\\dfrac{x^2}{a^2}\\space + \\space \\dfrac{y^2}{b^2} \\space = \\space 1.)$ to get:\\\n",
    "$\\dfrac{dy}{dx} = - \\dfrac{b^2}{a^2} \\cdot \\dfrac{x}{y} \\space$ for $y \\neq 0$\\\n",
    "hence the normalized tanget vector is\\\n",
    "$t_{ellipse} = (\\hat{ A.x + \\dfrac{dy}{dx} \\cdot A.y}) \\space$ for $\\space y \\neq 0$\\\n",
    "$t_{ellipse}$ = +/-$A.y \\space \\space \\space \\space \\space  \\space \\space \\space \\space \\space \\space \\space \\space \\space \\space \\space \\space \\space \\space \\space$ for $\\space   y = 0.$\n",
    "\n",
    "Therefore the normal vector is:\\\n",
    "$\\hat n = (\\hat{ \\dfrac{dy}{dx} \\cdot A.x - A.y}) \\space$ for $\\space y \\neq 0$\\\n",
    "\n",
    "$\\hat n = \\space A.x \\space \\space \\space \\space \\space \\space \\space \\space \\space \\space \\space \\space \\space \\space \\space \\space \\space \\space \\space \\space$ for $\\space   y = 0., x = a$\n",
    "\n",
    "$\\hat n$ = -$A.x \\space \\space \\space \\space \\space \\space \\space \\space \\space \\space \\space \\space \\space \\space \\space \\space \\space \\space \\space \\space$ for $\\space   y = 0., x = -a$\n",
    "\n",
    "Find the **location** of $CP_{hs}$:\\\n",
    "$CP_{hs} \\in gesamt(x, parameters), \\space$ where the function *gesamt(x, parameters)* models the street.\n",
    "\n",
    "Let $l = |{}^{CP_{hd}} r^{CP_h}| = |l \\hat n|$, then we get two equations:\n",
    "\n",
    "$(l\\hat n \\cdot N.x) = x$\\\n",
    "$(l\\hat n \\cdot N.y) = gesamt(x), \\space$\n",
    "\n",
    "to be solved during each step of the numerical integration.\\\n",
    "I solve it only if  $ \\hat n \\cdot N.y \\leq 0.$ For the shape of my street, this seems adequate and saves integration time.\n",
    "\n",
    "I take $\\sin(\\theta) = | \\space tanget_{ellipse} \\times tangent_{street} \\space |$  $\\space$ as a measure how 'parallel' the tangents at the collision points are, where the tangents are unit vectors, and $\\theta \\space$ is the angle between them.\n",
    "\n",
    "**NOTE**\\\n",
    "Initially I tried to get the distance from an ellipse to the street similarly as I do it with the ellipses: Set up the function *distance(ellipse to street)* = $f(\\epsilon, x, parameters)$, where $\\epsilon$ gives a point on the circumference of the ellipse, and x gives a point on the street, and minimize it. This did not work for me for two reasons:\n",
    "- using scipy's *minimize()* directly was very slow, and probably too inacurate for solve_ivp to get a result. It would terminate with an error message like *rerquired accuracy cannot be achieved* or similar.\n",
    "- solving $\\nabla_{\\epsilon, x} f(\\epsilon, x, parameters) = 0$. This is only a necessary condition. When the osculating radius of the street is smaller than the one of the ellipses, the coordinates of the impact point of ellipse and streetmay depend *discontinuously* of the generalized coordinates of the ellipse. Hence the *old* location of the minimal distance may not be a good initial guess for the next location, and the solution may converge to some other local extreme value of the distance function. Whatever the reasons, I never got it to work.\\\n",
    "(This does not happen between collissions of ellipse with each other - and there it worked fine, see below) \n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "scrolled": false
   },
   "outputs": [],
   "source": [
    "def equation_street(i, delta):\n",
    "# This function returns the equations to be solven for l and for x, as explained above.\n",
    "#find the vector normal to the tanget at the unrotated ellipse at the point CPh\n",
    "    CPhx = a * sm.cos(delta)\n",
    "    CPhy = b * sm.sin(delta)\n",
    "    ausdruckk = (sm.Abs(CPhy) <= 1.e-15)\n",
    "    ausdruckg = (sm.Abs(CPhy) >  1.e-15)\n",
    "\n",
    "    dydx = sm.Piecewise((-b**2/a**2 * CPhx/CPhy, ausdruckg), (1.e15, ausdruckk), (1., True))\n",
    "\n",
    "    hilfsx = sm.Piecewise((1., delta == sm.S(0)), (-1., delta == sm.pi ), (-dydx, delta < sm.pi/2.), (-dydx, delta < sm.pi), (dydx, delta < 3./2.*sm.pi), (dydx, delta < 2.*sm.pi), (1., True) )\n",
    "    hilfsy = sm.Piecewise((0., ausdruckk),                                                       (1., delta < sm.pi/2.),    (1., delta < sm.pi), (-1.,  delta < 3./2.*sm.pi), (-1,     delta < 2.*sm.pi), (1., True) )\n",
    "\n",
    "    nhat = (hilfsx*A_list[i].x + hilfsy*A_list[i].y).normalize()\n",
    "#print('nhat DS',me.find_dynamicsymbols(nhat, reference_frame=N))\n",
    "#print('nhat FS', nhat.free_symbols(reference_frame=N))\n",
    "    CPhss_list[i].set_pos(CPhes_list[i], ls_list[i]*nhat)\n",
    "    hilfs1 = CPhss_list[i].pos_from(O)\n",
    "\n",
    "# CPhs_ort to be solved for l and for x numerically later duering each step of the integration.\n",
    "    CPhs_ort = sm.Matrix([me.dot(hilfs1, N.x) - xs_list[i], me.dot(hilfs1, N.y) - gesamt1(xs_list[i], amplitude, frequenz)])\n",
    "#    print('CPhs_ort DS', me.find_dynamicsymbols(CPhs_ort))\n",
    "#    print('CPhs_ort FS', CPhs_ort.free_symbols)\n",
    "#    print(f'CPhs_ort has {sm.count_ops(CPhs_ort)} operations. After cse it has {sm.count_ops(sm.cse(CPhs_ort))}')\n",
    "    return CPhs_ort       # nhat, that are retuned for use in other functions only\n",
    "\n",
    "def nhat_that(i, delta):\n",
    "# This function returns the equations to be solven for l and for x, as explained above.\n",
    "#find the vector normal to the tanget at the unrotated ellipse at the point CPh\n",
    "    CPhx = a * sm.cos(delta)\n",
    "    CPhy = b * sm.sin(delta)\n",
    "    ausdruckk = (sm.Abs(CPhy) <= 1.e-15)\n",
    "    ausdruckg = (sm.Abs(CPhy) >  1.e-15)\n",
    "\n",
    "    dydx = sm.Piecewise((-b**2/a**2 * CPhx/CPhy, ausdruckg), (1.e15, ausdruckk), (1., True))\n",
    "\n",
    "    hilfsx = sm.Piecewise((1., delta == sm.S(0)), (-1., delta == sm.pi ), (-dydx, delta < sm.pi/2.), (-dydx, delta < sm.pi), (dydx, delta < 3./2.*sm.pi), (dydx, delta < 2.*sm.pi), (1., True) )\n",
    "    hilfsy = sm.Piecewise((0., ausdruckk),                                                       (1., delta < sm.pi/2.),    (1., delta < sm.pi), (-1.,  delta < 3./2.*sm.pi), (-1,     delta < 2.*sm.pi), (1., True) )\n",
    "    nhat = (hilfsx*A_list[i].x + hilfsy*A_list[i].y).normalize()\n",
    "    that = (A_list[i].x + dydx*A_list[i].y).normalize()\n",
    "    return [nhat, that]\n",
    "\n",
    "\n",
    "\n",
    "def parallel_street(i):\n",
    "# this function returns the sin of the deviation of the tangents of ellipse and street from being parallel, see above\n",
    "    strasse = gesamt1(x, amplitude, frequenz)\n",
    "    strassedx = strasse.diff(x).subs({x: xs_list[i]})\n",
    "    tangente_strasse = (N.x + strassedx *N.y).normalize()\n",
    "    that = nhat_that(i, epsilons_list[i])[1]\n",
    "    parallel = (that.cross(tangente_strasse)).magnitude()\n",
    "    return parallel\n",
    "\n",
    "equation_street_lam       = []\n",
    "equation_street_jakob_lam = []\n",
    "for i in range(n):\n",
    "    CPhs_ort1 = equation_street(i, epsilons_list[i])\n",
    "    jakob     = CPhs_ort1.jacobian([xs_list[i], ls_list[i]])\n",
    "    print('equation FS', CPhs_ort1.free_symbols)\n",
    "    print('equation DS', me.find_dynamicsymbols(CPhs_ort1)) \n",
    "    equation_street_lam.append(sm.lambdify([xs_list[i], ls_list[i]] + q_list + x_list + y_list + [xs_list[j] for j in range(n) if j !=i] + [ls_list[j] for j in range(n) if j !=i] + epsilons_list + [a, b, amplitude, frequenz], CPhs_ort1, cse=True))\n",
    "    equation_street_jakob_lam.append(sm.lambdify([xs_list[i], ls_list[i]] + q_list + x_list + y_list + [xs_list[j] for j in range(n) if j !=i] + [ls_list[j] for j in range(n) if j !=i] + epsilons_list + [a, b, amplitude, frequenz], jakob, cse=True))\n",
    "\n",
    "\n",
    "parallel_street_lam = []\n",
    "for i in range(n):\n",
    "    p1 = parallel_street(i)\n",
    "    print('parallel Fs', p1.free_symbols)\n",
    "    print('parallel DS', me.find_dynamicsymbols(p1)) \n",
    "    parallel_street_lam.append(sm.lambdify(q_list + xs_list + epsilons_list + ls_list + [a, b, amplitude, frequenz], p1, cse=True))\n",
    "\n",
    "# This is needed only for the plot with the initial conditions\n",
    "liste_lam     = []\n",
    "nhat_lam      = []\n",
    "senkrecht_lam = []\n",
    "for i in range(n):\n",
    "    CPhx = a * sm.cos(epsilons_list[i])\n",
    "    CPhy = b * sm.sin(epsilons_list[i])\n",
    "    CPha = me.Point('CPha')\n",
    "    CPhe = me.Point('CPhe')\n",
    "    CPha.set_pos(Dmc_list[i], CPhx*A_list[i].x + CPhy*A_list[i].y)\n",
    "    nhat = nhat_that(i, epsilons_list[i])[0]\n",
    "    CPhe.set_pos(CPha, 1.*nhat)\n",
    "    liste = [[me.dot(punkt.pos_from(O), uv) for uv in (N.x, N.y)] for punkt in (CPha, CPhe)]\n",
    "    liste_lam.append(sm.lambdify(q_list + x_list + y_list + epsilons_list + [a, b], liste, cse=True))\n",
    "\n",
    "    nhat = nhat_that(i, epsilons_list[i])[0]\n",
    "\n",
    "    print('nhat FS', me.dot(nhat, N.x).free_symbols)\n",
    "    print('nhat DS', me.find_dynamicsymbols(me.dot(nhat, N.x))) \n",
    "    nhat_lam.append(sm.lambdify(q_list + epsilons_list + [a, b], [me.dot(nhat, uv) for uv in (N.x, N.y)]))\n",
    "\n",
    "    that, nhat = nhat_that(i, epsilons_list[i])[0], nhat_that(i, epsilons_list[i])[1]\n",
    "    senkrecht = me.dot(nhat, that)\n",
    "    senkrecht_lam.append(sm.lambdify(q_list + epsilons_list + [a, b], senkrecht, cse=True))"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "**Find the potential collision points of any two ellipses**\\\n",
    "I assume, that no more than two ellipses start a collision at the same time. Since the initial conditons are random, I believe that Prob(more than two ellipses start to collide at the same time) $\\approx 0.$\\\n",
    "In order to get the potential contact points, say, $CPhe_i, CPhe_j$, I simply try to find the minimum distance between any two points on the circumferences on the respective ellipses.\\\n",
    "I do this in two ways:\n",
    "- minimize $|{}^{CPhe_i} \\bar r^{CPhe_j}(\\epsilon_i, \\epsilon_j)|$ directly, using scipy's minimize function\n",
    "- calculate $\\dfrac{d}{d\\epsilon_i} |{}^{CPhe_i} \\bar r^{CPhe_j}(\\epsilon_i, \\epsilon_j)|$ and $\\dfrac{d}{d\\epsilon_j} |{}^{CPhe_i} \\bar r^{CPhe_j}(\\epsilon_i, \\epsilon_j)|$, and solve for $\\epsilon_i, \\epsilon_j$. This is only a sufficient condition for a minimum, it could also give a maximum. With the right initial guess, scipy's **root** function should give the minimum. During integration, this seems to be faster than the first option. \n",
    "\n",
    "\n",
    "If $| {}^{CPhe_i} \\bar r^{CPhe_j} | \\approx 0.$ it becomes numerically critical to get the direction ${}^{CPhe_i} \\bar r^{CPhe_j}$. So, I fix the direction before the distance becomes too small. See also the comment in the numerical integration."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "def CPhxe(epsilon):\n",
    "    return a * sm.cos(epsilon)\n",
    "def CPhye(epsilon):\n",
    "    return b * sm.sin(epsilon)\n",
    "\n",
    "def vorzeichen(i, j, epsilon1, epsilon2):\n",
    "# the idea is this: I calculate the triangle abc, with a := Dmc_i - CPh_i, b := CPhe_j, and get c using\n",
    "# the cosine theorem: c^2 = a^2 + b^2 - 2 * a * b * cos(gamma)\n",
    "# while c < Dmc_i.pos_from(Dmc_j).magnitude, the ellipses are separated.\n",
    "    Pi = me.Point('Pi')\n",
    "    Pj = me.Point('Pj')\n",
    "    Pi.set_pos(Dmc_list[i], CPhxe(epsilon1)*A_list[i].x + CPhye(epsilon1)*A_list[i].y)\n",
    "    Pj.set_pos(Dmc_list[j], CPhxe(epsilon2)*A_list[j].x + CPhye(epsilon2)*A_list[j].y)\n",
    "\n",
    "    rr = Dmc_list[i].pos_from(Dmc_list[j]).magnitude()\n",
    "    r1 = Dmc_list[i].pos_from(Pi)\n",
    "    r2 = Dmc_list[j].pos_from(Pj)\n",
    "    gamma_cos = (me.dot(r1.normalize(), r2.normalize()))\n",
    "    r1 = r1.magnitude()\n",
    "    r2 = r2.magnitude() \n",
    "    r3 = sm.sqrt(r1**2 + r2**2 - 2. * r1 * r2 * gamma_cos)\n",
    "    hilfs1 = rr - r3\n",
    "    hilfs2 = sm.Piecewise((-1., hilfs1 <= 0.), (1., hilfs1 > 0.))\n",
    "    return hilfs2  # -1., if the ellipses have penetrated\n",
    "\n",
    "\n",
    "def distanzCPheiCPhej(i, j, epsilon1, epsilon2):\n",
    "    P1, P2 = sm.symbols('P1, P2', cls=me.Point)\n",
    "    P1.set_pos(Dmc_list[i], CPhxe(epsilon1)*A_list[i].x + CPhye(epsilon1)*A_list[i].y)\n",
    "    P2.set_pos(Dmc_list[j], CPhxe(epsilon2)*A_list[j].x + CPhye(epsilon2)*A_list[j].y)\n",
    "    vektor = P2.pos_from(P1)\n",
    "    return vektor.magnitude() * vorzeichen(i, j, epsilon1, epsilon2)    \n",
    "\n",
    "def richtungCPheiCPhej(i, j, epsilon1, epsilon2):\n",
    "    P1, P2 = sm.symbols('P1, P2', cls=me.Point)\n",
    "    P1.set_pos(Dmc_list[i], CPhxe(epsilon1)*A_list[i].x + CPhye(epsilon1)*A_list[i].y)\n",
    "    P2.set_pos(Dmc_list[j], CPhxe(epsilon2)*A_list[j].x + CPhye(epsilon2)*A_list[j].y)\n",
    "    vektor = P2.pos_from(P1).normalize()\n",
    "    return [me.dot(vektor, uv) for uv in (N.x, N.y) ]\n",
    "\n",
    "# This function will be minimized numerically during integratrion to get the distance from ellipse_i to ellipse_j\n",
    "# ...._lam for the direct minimization, ...lam1 for the gradient method.\n",
    "min_distanzCPheiCPhej_lam  = []\n",
    "min_distanzCPheiCPhej_lam1 = []\n",
    "jakob_lam = []\n",
    "jakob_lam1 = []\n",
    "\n",
    "zaehler = -1\n",
    "for i, j in itt.permutations(range(n), r=2):\n",
    "    zaehler += 1\n",
    "    abstand = distanzCPheiCPhej(i, j, epsilone_list[zaehler][0], epsilone_list[zaehler][1])\n",
    "    abstanddeidej = [abstand.diff(epsilone_list[zaehler][0]), abstand.diff(epsilone_list[zaehler][1])]\n",
    "    jakob = sm.Matrix([abstand.diff(epsilone_list[zaehler][0]), abstand.diff(epsilone_list[zaehler][1])])\n",
    "    \n",
    "    hilfs1 = sm.Matrix([abstand.diff(epsilone_list[zaehler][0]), abstand.diff(epsilone_list[zaehler][1])])\n",
    "    jakob1 = hilfs1.jacobian([epsilone_list[zaehler][0], epsilone_list[zaehler][1]])\n",
    "\n",
    "    min_distanzCPheiCPhej_lam.append((sm.lambdify([epsilone_list[zaehler][0], epsilone_list[zaehler][1]] + q_list + x_list + y_list + [a, b], abstand, cse=True)))\n",
    "    min_distanzCPheiCPhej_lam1.append(sm.lambdify([epsilone_list[zaehler][0], epsilone_list[zaehler][1]] + q_list + x_list + y_list + [a, b], abstanddeidej, cse=True))\n",
    "\n",
    "    jakob_lam1.append(sm.lambdify([epsilone_list[zaehler][0], epsilone_list[zaehler][1]] + q_list + x_list + y_list + [a, b], jakob1, cse=True))\n",
    "    jakob_lam.append(sm.lambdify([epsilone_list[zaehler][0], epsilone_list[zaehler][1]] + q_list + x_list + y_list + [a, b], jakob, cse=True))\n",
    "\n",
    "\n",
    "\n",
    "richtung_lam = []\n",
    "zaehler = -1\n",
    "for i, j in itt.permutations(range(n), r=2):\n",
    "    zaehler +=1\n",
    "    richtungij = richtungCPheiCPhej(i, j, epsilone_list[zaehler][0], epsilone_list[zaehler][1])\n",
    "    richtung_lam.append((sm.lambdify([epsilone_list[zaehler][0], epsilone_list[zaehler][1]] + q_list + x_list + y_list + [a, b], richtungij, cse=True)))\n",
    "\n",
    "\n",
    "# just needed for plotting initial situation\n",
    "epsilonei = sm.symbols('epsilonei')\n",
    "CPhe = list(sm.symbols('CPhe' + str(i), cls=me.Point) for i in range(n))\n",
    "\n",
    "CPhe_list = []\n",
    "for i in range(n):\n",
    "    CPhe[i].set_pos(Dmc_list[i], CPhxe(epsilonei)*A_list[i].x + CPhye(epsilonei)*A_list[i].y )\n",
    "    CPhe_list.append([me.dot(CPhe[i].pos_from(O), uv) for uv in (N.x, N.y)])\n",
    "CPhe_list_lam = sm.lambdify(q_list + x_list + y_list + [a, b, epsilonei], CPhe_list, cse=True)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "**Calculate the impact speeds**"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "speed_dict = {sm.Derivative(i, t): j for i, j in zip(q_list + x_list + y_list, u_list + ux_list + uy_list)}\n",
    "def equation_rhodts(i, delta):\n",
    "# returns the speed component of the potential contact point on the ellipse in the direction of the 'impact line'    \n",
    "    vs = me.msubs(CPhes_list[i].pos_from(O).diff(t, N), speed_dict)\n",
    "    vs = me.dot(vs, nhat_that(i, delta)[0])\n",
    "    return vs\n",
    "\n",
    "def equation_rhodte(i, j, deltai, deltaj, richtungx, richtungy):\n",
    "# returns the speed component of the difference between ellipse_i and ellipse_j, in direction of their impact line.\n",
    "    P1, P2 = sm.symbols('P1, P2', cls=me.Point)\n",
    "    P1.set_pos(Dmc_list[i], a*sm.cos(deltai)*A_list[i].x + b*sm.cos(deltai)*A_list[i].y)\n",
    "    P2.set_pos(Dmc_list[j], a*sm.cos(deltaj)*A_list[j].x + b*sm.cos(deltaj)*A_list[j].y)\n",
    "\n",
    "    vs1 = me.msubs(P1.pos_from(O).diff(t, N), speed_dict)\n",
    "    vs2 = me.msubs(P2.pos_from(O).diff(t, N), speed_dict)\n",
    "    speed = me.dot(vs2 - vs1, richtungx*N.x + richtungy*N.y)\n",
    "    return speed\n",
    "    \n",
    "\n",
    "rhodts_lam = []\n",
    "for i in range(n):\n",
    "    speed = equation_rhodts(i, epsilons_list[i])\n",
    "    print('rhodts FS', speed.free_symbols)\n",
    "    print('rhodts DS', me.find_dynamicsymbols(speed)) \n",
    "    rhodts_lam.append(sm.lambdify(q_list + x_list + y_list + u_list + ux_list + uy_list + epsilons_list + [a, b, amplitude, frequenz], speed, cse=True))\n",
    "\n",
    "rhodte_lam = []\n",
    "zaehler = -1\n",
    "for i, j in itt.permutations(range(n), r=2):\n",
    "    zaehler += 1\n",
    "    speed = equation_rhodte(i, j, epsilone_list[zaehler][0], epsilone_list[zaehler][1], richtung_list[zaehler][0], richtung_list[zaehler][1])\n",
    "    print('rhodte FS', speed.free_symbols)\n",
    "    print('rhodte DS', me.find_dynamicsymbols(speed)) \n",
    "    rhodte_lam.append(sm.lambdify(q_list + x_list + y_list + u_list + ux_list + uy_list + epsilone_list + richtung_list + [a, b], speed, cse=True))"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "**Force on $CP_h$ during impact of an ellipse with the street**\n",
    "\n",
    "**Force acting on $CP_h$ during impact**\\\n",
    "I use Hunt_Crossley's method to calculate it.\n",
    "\n",
    "*Hunt Crossley's method*\n",
    " \n",
    "My reference is this article, given to me by JM\\\n",
    "https://www.sciencedirect.com/science/article/pii/S0094114X23000782 \\\n",
    "\n",
    " \n",
    "This is with dissipation during the collision, the general force is given in (63) as\\\n",
    "$f_n = k_0 \\cdot \\rho + \\chi \\cdot \\dot \\rho$, with $k_0$ as above, $\\rho$ the penetration, and $\\dot\\rho$ the speed of the penetration.\\\n",
    "In the article it is stated, that $n = \\frac{3}{2}$ is a good choice, it is derived in Hertz' approach. Of course, $\\rho, \\dot\\rho$ must be the signed magnitudes of the respective vectors.\n",
    "\n",
    "A more realistic force is given in (64) as:\\\n",
    "$f_n = k_0 \\cdot \\rho^n + \\chi \\cdot \\rho^n\\cdot \\dot \\rho$, as this avoids discontinuity at the moment of impact.\n",
    "\n",
    "**Hunt and Crossley** give this value for $\\chi$, see table 1:\n",
    "\n",
    "$\\chi = \\dfrac{3}{2} \\cdot(1 - c_\\tau) \\cdot \\dfrac{k_0}{\\dot \\rho^{(-)}}$, \n",
    "where $c_\\tau = \\dfrac{v_1^{(+)} - v_2^{(+)}}{v_1^{(-)} - v_2^{(-)}}$, where $v_i^{(-)}, v_i^{(+)}$ are the speeds of $body_i$, before and after the collosion, see (45), $\\dot\\rho^{(-)}$ is the speed right at the time the impact starts. $c_\\tau$ is an experimental factor, apparently around 0.8 for steel.\n",
    "\n",
    "Using (64), this results in their expression for the force:\n",
    "\n",
    "$f_n = k_0 \\cdot \\rho^n \\left[1 + \\dfrac{3}{2} \\cdot(1 - c_\\tau) \\cdot \\dfrac{\\dot\\rho}{\\dot\\rho^{(-)}}\\right]$\n",
    "\n",
    "with $k_0 = \\frac{4}{3\\cdot(\\sigma_1 + \\sigma_2)} \\cdot \\sqrt{\\frac{R_1 \\cdot R_2}{R_1 + R_2}}$, where $\\sigma_i = \\frac{1 - \\nu_i^2}{E_i}$, with $\\nu_i$ = Poisson's ratio, $E_i$ = Young\"s modulus, $R_1, R_2$ the radii of the colliding bodies, $\\rho$ the penetration depth. All is near equations (54) and (61) of this article.\n",
    "\n",
    "1. Penetration depth $rho$:\\\n",
    "From the description in the cell above, it is clear that\\\n",
    "$rho = |l| \\cdot H(-l) \\space$, with l from above (found numerically), and $H(...)$ being the heaviside function.\n",
    "\n",
    "\n",
    "2. Determine $R_1, R_2$ in the above formulas:\\\n",
    "For a function $y = f(x)$ the signed curvature is:\\\n",
    "$\\kappa = \\dfrac{\\frac{d^2}{dx^2} f(x)}{(1 + (\\frac{d}{dx} f(x))^2)^{\\frac{3}{2}} }$\\\n",
    "For an ellipse, $\\kappa = \\dfrac{a \\cdot b}{ \\left( \\sqrt{a^2 \\sin^2(\\delta) + b^2 \\cos^2(\\delta)} \\right)^3}  > 0 \\space \\space \\forall \\delta \\in [0, 2 \\pi), \\space$ where $\\delta$ is the angle from A.x to the point.\\\n",
    "As an approximation for $R_1, R_2$ I take the radius of the osculating circle, which is:\\\n",
    "$R_i = \\dfrac{1}{\\kappa_i} \\space$ If the penetration depth is no too large, this should be o.k.\\\n",
    "Note, that I allow *negative* $R_2$: This means, the street is concave from the ellipse's point of view.\\\n",
    "At a contact point, either $R_2 > 0$ or $|R_2| \\leq R_1$ there should be no problems.\\\n",
    "I do not know, whether this approach is within the **validity of the H-C method**\n",
    "\n",
    "\n",
    "3. Penetration speed $\\frac{d}{dt} \\rho(t)$\\\n",
    "Only the component of $\\frac{d}{dt} CP_h(t)$ is relevant, hence:\\\n",
    "$\\frac{d}{dt} \\rho(t) = \\frac{d}{dt} CP_h \\cdot \\hat n$\n",
    "\n",
    "*spring energy* =   $ k_0 \\cdot \\int_{0}^{\\rho} k^{3/2}\\,dk$ = $k_0 \\cdot\\frac{2}{5} \\cdot \\rho^{5/2}$\\\n",
    "I assume, the dissipated energy cannot be given in closed form, at least the article does not give one.\n",
    "\n",
    "*Note*  \n",
    "$c_\\tau = 1.$ gives **Hertz's** solution to the impact problem, also described in the article.\n",
    "\n",
    "\n",
    "**Friction when the ellipse hits the street**\\\n",
    "This cannot be done, seems it will result in too many *Piecewise(...)* terms, see sympy issue #25307, hence I do not show the simple code."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "scrolled": false
   },
   "outputs": [],
   "source": [
    "def impact_force_street(i, delta, rhodts_list):\n",
    "# impact force on CPh\n",
    "# curvature of the ellipse at the point (a*cos(delta) / b*sin(delta)) from the internet\n",
    "    kappa1 = (a * b) / (sm.sqrt((a*sm.sin(delta))**2 + (b*sm.cos(delta))**2))**3\n",
    "\n",
    "# formula for the curvature of a function. From the internet.\n",
    "    hilfs = gesamt1(x, amplitude, frequenz)\n",
    "    hilfsdx = hilfs.diff(x).subs({x: xs_list[i]})\n",
    "    hilfsdxdx = hilfs.diff(x, 2).subs({x: xs_list[i]})\n",
    "    kappa2 = sm.Piecewise((hilfsdxdx**2 / (1. + hilfsdx**2)**1.5, hilfsdxdx != 0.), (1.e-15, True))\n",
    "\n",
    "    R1 = 1. / kappa1\n",
    "    R2 = 1. / kappa2\n",
    "    sigmae = (1. - nue**2) / EYe\n",
    "    sigmas = (1. - nus**2) / EYs\n",
    "    k0 = 4./3. * 1./(sigmae + sigmas) * sm.sqrt(R1*R2 / (R1 + R2))\n",
    " \n",
    "    rhodt = equation_rhodts(i, delta)\n",
    "    l = ls_list[i]\n",
    "    rho   = sm.Abs(l) * sm.Heaviside(-l, sm.S(0))\n",
    "#print('rhodt DS', me.find_dynamicsymbols(rhodt))\n",
    "#print('rhodt FS', rhodt.free_symbols)\n",
    "    nhat = nhat_that(i, delta)[0]\n",
    "    fHC_betrag = k0 * rho**(3/2) * (1. + 3./2. * (1. - ctau) * (rhodt) / sm.Abs(rhodts_list[i])) \n",
    "    fHC = fHC_betrag * (-nhat) * sm.Heaviside(-l, sm.S(0))  # force is acting on CPhes hence the minus sign.\n",
    "#    print('fHC DS', me.find_dynamicsymbols(fHC, reference_frame=N))\n",
    "#    print('fHC FS', fHC.free_symbols(reference_frame=N))\n",
    "\n",
    "    return fHC\n",
    "\n",
    "#print('fHC DS', me.find_dynamicsymbols(fHC, reference_frame=N))\n",
    "#print('fHC FS', fHC.free_symbols(reference_frame=N))"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "**Collision between any two ellipses**\\\n",
    "I use Hunt-Crossley's method to model it.\n",
    "\n",
    "*Friction when the ellipse hits another ellipse*\\\n",
    "This cannot be done, seems it will result in too many *Piecewise(...)* terms, see sympy issue #25307, hence I do not show the simple code."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "#@profile\n",
    "def impact_force_ellipse(i, j, epsiloni, epsilonj, l, rhodtellipse, richtungx, richtungy):\n",
    "# this calculates the force of ellipse_i on ellipse_j during their collision.\n",
    "# i, j are the respective ellipses\n",
    "# epsilone list of angles\n",
    "# l is the distance between ellipse_i and ellipse_j, negative during penetration\n",
    "# rhodtellipse is the collision speed right before impact.\n",
    "    \n",
    "# curvature of the ellipse at the point (a*cos(delta) / b*sin(delta)) from the internet\n",
    "    kappa1 = (a * b) / (sm.sqrt((a*sm.sin(epsiloni))**2 + (b*sm.cos(epsiloni))**2))**3\n",
    "    kappa2 = (a * b) / (sm.sqrt((a*sm.sin(epsilonj))**2 + (b*sm.cos(epsilonj))**2))**3\n",
    "\n",
    "    R1 = 1. / kappa1\n",
    "    R2 = 1. / kappa2\n",
    "    sigmae = (1. - nue**2) / EYe\n",
    "    k0 = 4./3. * 1./(sigmae + sigmae) * sm.sqrt(R1*R2 / (R1 + R2))\n",
    "\n",
    "    P1, P2 = sm.symbols('P1, P2', cls=me.Point)\n",
    "    P1.set_pos(Dmc_list[i], CPhxe(epsiloni)*A_list[i].x + CPhye(epsiloni)*A_list[i].y)\n",
    "    P2.set_pos(Dmc_list[j], CPhxe(epsilonj)*A_list[j].x + CPhye(epsilonj)*A_list[j].y)\n",
    "    \n",
    "    \n",
    "    vei = P1.pos_from(O).diff(t, N)\n",
    "    vej = P2.pos_from(O ).diff(t, N)\n",
    "# only the speed in direction of the collision is important here\n",
    "    richtung = richtungx*N.x + richtungy*N.y                            # points from ellipse_i to ellipse_j\n",
    "    rhodt = me.msubs(me.dot(vei - vej, richtung), speed_dict)\n",
    "    rho   = sm.Abs(l) * sm.Heaviside(-l, sm.S(0))\n",
    "#    print('rhodt DS', me.find_dynamicsymbols(rhodt))\n",
    "#    print('rhodt FS', rhodt.free_symbols)\n",
    "\n",
    "    fHC_betrag = k0 * rho**(3/2) * (1. + 3./2. * (1. - ctau) * (rhodt) / sm.Abs(rhodtellipse))\n",
    "    fHC = fHC_betrag * richtung * sm.Heaviside(-l, sm.S(0))\n",
    "    return fHC"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "**Kane's equations**\\\n",
    "There is nothing special here."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "scrolled": false
   },
   "outputs": [],
   "source": [
    "start1 = time.time()\n",
    "\n",
    "FL_gravity = [(Dmc_list[i], -m*g*N.y) for i in range(n)] + [(Po_list[i], -mo*g*N.y) for i in range(n)]\n",
    "\n",
    "FL_impact_street  = []\n",
    "for i in range(n):\n",
    "    FL_impact_street.append((CPhes_list[i], impact_force_street(i, epsilons_list[i], rhodts_list)))\n",
    "\n",
    "CPhejj = list(sm.symbols(f'CPhejj:{n}', cls =me.Point))\n",
    "FL_impact_ellipse = []\n",
    "zaehler = -1\n",
    "for i, j in itt.permutations(range(n), r=2):\n",
    "    zaehler += 1    \n",
    "    CPhejj[j].set_pos(Dmc_list[j], a*sm.cos(epsilone_list[zaehler][1])*A_list[j].x + b*sm.sin(epsilone_list[zaehler][1])*A_list[j].y)\n",
    "    CPhejj[j].set_vel(N, CPhejj[j].pos_from(O).diff(t, N))\n",
    "    FL_impact_ellipse.append((CPhejj[j], impact_force_ellipse(i, j, epsilone_list[zaehler][0], epsilone_list[zaehler][1], le_list[zaehler], rhodte_list[zaehler], richtung_list[zaehler][0], richtung_list[zaehler][1])))\n",
    "\n",
    "FL = FL_gravity + FL_impact_street + FL_impact_ellipse\n",
    "\n",
    "kd = [i - j.diff(t) for i, j in zip((u_list + ux_list + uy_list), (q_list + x_list + y_list))]\n",
    "\n",
    "q_ind = q_list + x_list + y_list\n",
    "u_ind = u_list + ux_list + uy_list\n",
    "\n",
    "#@profile\n",
    "def KANE():\n",
    "    for _ in range(1):\n",
    "        KM = me.KanesMethod(N, q_ind=q_ind, u_ind=u_ind, kd_eqs=kd) \n",
    "        (fr, frstar) = KM.kanes_equations(BODY, FL)\n",
    "        MM = KM.mass_matrix_full\n",
    "        force = KM.forcing_full\n",
    "    return MM, force\n",
    "MM, force = KANE()\n",
    "\n",
    "print('force DS', me.find_dynamicsymbols(force))\n",
    "print('force free symbols', force.free_symbols)\n",
    "print(f'force has {sm.count_ops(force)} operations, {sm.count_ops(sm.cse(force))} operations after cse', '\\n')\n",
    "\n",
    "print('MM DS', me.find_dynamicsymbols(MM))\n",
    "print('MM free symbols', MM.free_symbols)\n",
    "print(f'MM has {sm.count_ops(MM)} operations, {sm.count_ops(sm.cse(MM))} operations after cse', '\\n')\n",
    "\n",
    "print(f'it took {time.time() - start1 :.5f} sec to establish Kanes equations')"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Here the *sympy functions* are converted to *numpy functions* so numerical calculations may be done.\\\n",
    "Before, I calculate the functions for the *potential energy*, for the *kinetic energy* and for the *spring energy*."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "scrolled": false
   },
   "outputs": [],
   "source": [
    "start1 = time.time()\n",
    "# ENERGIES\n",
    "pot_energie = sum([m * g * me.dot(punkt.pos_from(O), N.y) for punkt in Dmc_list]) + sum([mo * g * me.dot(punkt.pos_from(O), N.y) for punkt in Po_list])\n",
    "kin_energie = sum([koerper.kinetic_energy(N) for koerper in BODY])\n",
    "\n",
    "spring_energie = 0.\n",
    "# collision between ellipose and stree\n",
    "for i in range(n):\n",
    "      kappa1 = (a * b) / (sm.sqrt((a*sm.sin(epsilons_list[i]))**2 + (b*sm.cos(epsilons_list[i]))**2))**3\n",
    "# formula for the curvature of a function. From the internet.\n",
    "      hilfs = gesamt1(x, amplitude, frequenz)\n",
    "      hilfsdx = hilfs.diff(x).subs({x: xs_list[i]})\n",
    "      hilfsdxdx = hilfs.diff(x, 2).subs({x: xs_list[i]})\n",
    "      kappa2 = sm.Piecewise((hilfsdxdx**2 / (1. + hilfsdx**2)**1.5, hilfsdxdx != 0.), (1.e-15, True))\n",
    "\n",
    "      R1 = 1. / kappa1\n",
    "      R2 = 1. / kappa2\n",
    "      sigmae = (1. - nue**2) / EYe\n",
    "      sigmas = (1. - nus**2) / EYs\n",
    "      k0 = 4./3. * 1./(sigmae + sigmas) * sm.sqrt(R1*R2 / (R1 + R2))\n",
    "      spring_energie += 2./5. * k0 * sm.Abs(ls_list[i])**(5/2) * sm.Heaviside(-ls_list[i], 0.)\n",
    "\n",
    "# collisions between ellipses\n",
    "zaehler = -1\n",
    "for i, j in itt.permutations(range(n), r=2):\n",
    "      zaehler += 1\n",
    "      kappa1 = (a * b) / (sm.sqrt((a*sm.sin(epsilone_list[zaehler][0]))**2 + (b*sm.cos(epsilone_list[zaehler][0]))**2))**3\n",
    "      kappa2 = (a * b) / (sm.sqrt((a*sm.sin(epsilone_list[zaehler][1]))**2 + (b*sm.cos(epsilone_list[zaehler][1]))**2))**3\n",
    "\n",
    "      R1 = 1. / kappa1\n",
    "      R2 = 1. / kappa2\n",
    "      sigmae = (1. - nue**2) / EYe\n",
    "      k0 = 4./3. * 1./(sigmae + sigmae) * sm.sqrt(R1*R2 / (R1 + R2))\n",
    "\n",
    "      rho = sm.Abs(le_list[zaehler])\n",
    "      rho = rho**(5/2)\n",
    "      spring_energie += (k0 * 2./5. * rho * sm.Heaviside(-le_list[zaehler], 0.))  * 0.5               # factor 0.5 needed, because I count it twice\n",
    "\n",
    "\n",
    "#-------------------------------------------------------------------------------------------------------------------------------------------------------------------\n",
    "#-------------------------------------------------------------------------------------------------------------------------------------------------------------------\n",
    "# LAMBDIFICATION\n",
    "qL = q_ind + u_ind\n",
    "pL = [m, mo, g, a, b, amplitude, frequenz] + [ctau, EYe, EYs, nue, nus] + xs_list + epsilons_list + ls_list + alpha_list + beta_list + rhodts_list + epsilone_list + le_list + rhodte_list + richtung_list\n",
    "\n",
    "MM_lam    = sm.lambdify(qL + pL, MM, cse=True)\n",
    "force_lam = sm.lambdify(qL + pL, force, cse=True)\n",
    "\n",
    "gesamt     = gesamt1(x, amplitude, frequenz)\n",
    "gesamt_lam = sm.lambdify([x, amplitude, frequenz], gesamt, cse=True)\n",
    "\n",
    "Po_ort_lam = []\n",
    "for i in range(n):\n",
    "      Po_ort_lam.append(sm.lambdify(q_list + x_list + y_list + [a, b] + alpha_list + beta_list, [me.dot(Po_list[i].pos_from(O), uv) for uv in (N.x, N.y)], cse=True ))\n",
    "\n",
    "pot_lam    = sm.lambdify(qL + pL, pot_energie, cse=True)\n",
    "kin_lam    = sm.lambdify(qL + pL, kin_energie, cse=True)\n",
    "spring_lam = sm.lambdify(qL + pL, spring_energie, cse=True)\n",
    "\n",
    "r_max_lam = sm.lambdify([x, amplitude, frequenz], r_max, cse=True)\n",
    "#k0_lam = sm.lambdify(qL + pL, k0*sm.Heaviside(-l, 0), cse = True)\n",
    "\n",
    "# for the initial conditions only\n",
    "Dmc_distanz = [Dmc_list[i].pos_from(Dmc_list[j]).magnitude() for i, j in itt.permutations(range(n), r=2)]\n",
    "Dmc_distanz_lam = sm.lambdify(x_list + y_list, Dmc_distanz, cse=True)\n",
    "\n",
    "force1_lam = []\n",
    "for i in range(n):\n",
    "      impact = impact_force_street(i, epsilons_list[i], rhodts_list)\n",
    "      force1_lam.append(sm.lambdify(qL + pL, [ me.dot(impact, uv) for uv in (N.x, N.y)], cse=True))\n",
    "print(f'it took {time.time() - start1 :.5f} sec to do the lambdification')"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "**Numerical integration**\n",
    "- the parameters and the initial values of independent coordinates are set. The ellipses are placed at random, and then I check that they do not overlap with each other. For this, I assume for simplicity that the ellipses are circles with radius $R_0 = a \\bigvee b$\n",
    "- an exception is raised if $\\alpha$ or $\\beta$ are selected such that the particle will be outside of the ellipse. \n",
    "- I check whether the minimum osculating cycle of the street is smaller than the max. osculating circle of the ellipse. If it is smaller, second contact points may occur.\n",
    "\n",
    "I plot the initial location of the ellipse. This plot also gives possible contact points. The closest one is marked on the street.\\\n",
    "Also, the relative initial generalized speeds are indicated."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "scrolled": false
   },
   "outputs": [],
   "source": [
    "start2 = time.time()\n",
    "\n",
    "#=============================================\n",
    "# Input parameters\n",
    "#=============================================\n",
    "m1  = 1.\n",
    "mo1 = 1.\n",
    "g1  = 9.8\n",
    "a1  = 2.\n",
    "b1  = 1.\n",
    "\n",
    "amplitude1  = 1.\n",
    "frequenz1   = 0.25\n",
    "\n",
    "EYe1        = 1.e7  \n",
    "EYs1        = 1.e7\n",
    "ctau1       = 0.9\n",
    "nue1        = 0.28\n",
    "nus1        = 0.28\n",
    "\n",
    "alpha_list1 = [0.5 for _ in range(n)]\n",
    "beta_list1  = [0.5 for _ in range(n)]\n",
    "\n",
    "intervall = 0. 1                            \n",
    "X1        = 4.                                        # Initial locations must be between -X1, X1 in X - coordinates\n",
    "Y1        = 11.                                       # initial Y - coordinates must be between the street and Y1\n",
    "\n",
    "min_winkel = 0.1                                      # max sin(angle) how the tangents of the street and the ellipse may differ for a contact point\n",
    "np.random.seed(123456789)                             # for any other random configurtation I tried, it wourd rum 'forever'! (like I aborted after it had not finished 2 sec of simulation in 10 hours of running)\n",
    "\n",
    "\n",
    "#-----------------------------------------------------------------------------------------------------------------------------------------------------------------\n",
    "# ensure the observers are inside the ellipses\n",
    "lokation = np.array([(alpha_list1[i]/a1)**2 + (beta_list1[i]/b1)**2 - 1. for i in range(n)])\n",
    "if any(i > 0. for i in lokation):\n",
    "    raise Exception('Particle is outside the ellipse')\n",
    "\n",
    "#-------------------------------------------------------------------------------------------------------------------------------------------------------------------\n",
    "q_list1 = [*np.random.choice(np.linspace(-np.pi, np.pi, 100), size=n)]                  # initial angle of the ellipses\n",
    "alpha_list1    = [0.5 for _ in range(len(alpha_list))]                                  # location of observer\n",
    "beta_list1     = [0.5 for _ in range(len(beta_list))]                                   # dto.\n",
    "epsilons_list1 = [0. for _ in range(n)]                                                 # values of no consequence\n",
    "xs_list1       = [1. for _ in range(n)]                                                 # values of no consequence\n",
    "ls_list1       = [1. for _ in range(n)]                                                 # values of no consequence\n",
    "\n",
    "rhodte_list1     = [1. for _ in range(n*(n-1))]                                         # values of no consequence\n",
    "richtung_list1   = [1. for _ in range(n*(n-1))]                                         # values of no consequence\n",
    "\n",
    "\n",
    "ux_list1 = list((-1.5)**(i+1) for i in range(n))                                        # initial speed of center of the ellipse in x direction\n",
    "uy_list1 = [(-1.5)*(i+1) for i in range(n)]                                             # initial speed of center of the ellipse in Z direction\n",
    "u_list1  = list(-1. * np.random.choice(np.linspace(-5., 5., 100), size=n))              # initial rotationmal speed\n",
    " \n",
    "schritte = int(intervall * 40000)\n",
    "\n",
    "#-------------------------------------------------------------------------------------------------------------------------------------------------------------------\n",
    "# 1. randomly place the ellipses as described above\n",
    "# This is needed to check, whether an ellipse collides with the street.\n",
    "# it will only 'work' if the ellipse intersects the street. If the ellispe is placed on the wrong side of the street entirely, it will not notice this mistake.\n",
    "# I did not bother to catch this error.\n",
    "P1 = me.Point('P1')\n",
    "x  = sm.symbols('x')\n",
    "P1.set_pos(O, x*N.x + gesamt1(x, amplitude, frequenz)*N.y)\n",
    "kollision_lam = []\n",
    "for i in range(n):\n",
    "    kollision = Dmc_list[i].pos_from(P1).magnitude()\n",
    "    kollision_lam.append(sm.lambdify([x_list[i], y_list[i]] + [x, amplitude, frequenz], kollision, cse=True))\n",
    "\n",
    "zaehler = 0\n",
    "r01     = max(a1, b1)\n",
    "while zaehler <= 500:\n",
    "    zaehler += 1\n",
    "    try:\n",
    "        x_listen = []\n",
    "        y_listen = []\n",
    "        test1    = True\n",
    "        for i in range(n):\n",
    "            x_listen.append(np.random.choice(np.linspace(-X1 + r01, X1 - r01, 100)))\n",
    "            hilfs = np.linspace(x_listen[-1] - r01, x_listen[-1] + r01, 100)\n",
    "            hilfs_max = np.max(gesamt_lam(hilfs, amplitude1, frequenz1))\n",
    "            y_listen.append(np.random.choice(np.linspace(gesamt_lam(x_listen[-1], amplitude1, frequenz1) + 1.*r01, Y1 - r01, 100)))\n",
    "            test1 = test1 and np.all(np.array(kollision_lam[i](hilfs, y_listen[i], hilfs, amplitude1, frequenz1 )) > 1.*r01 )\n",
    "\n",
    "        test = np.all(np.array(Dmc_distanz_lam(*x_listen, *y_listen)) - 3.*r01 > 0.)\n",
    "        x_list1 = x_listen\n",
    "        y_list1 = y_listen\n",
    "        \n",
    "        if test == True and test1 == True:\n",
    "            raise Rausspringen\n",
    "    except:\n",
    "        break\n",
    "\n",
    "if zaehler <= 500:\n",
    "    print(f'it took {zaehler} rounds to get valid initial conditions')\n",
    "else:\n",
    "    raise Exception(' no good location for ellipses found, make X1 and/or Y1 larger, or try again.')\n",
    "#--------------------------------------------------------------------------------------------------------------------------------------------------------------------\n",
    "#find the largest admissible r_max, given strasse, amplitude, frequenz\n",
    "r_max = max(a1**2/b1, b1**2/a1)  # max osculating circle of an ellipse\n",
    "def func2(x, args):\n",
    "# just needed to get the arguments matching for minimize\n",
    "    return np.abs(r_max_lam(x, *args))\n",
    "\n",
    "x0 = 0.1            # initial guess\n",
    "minimal = minimize(func2, x0, [amplitude1, frequenz1])\n",
    "if r_max < (x111 := minimal.get('fun')):\n",
    "    print('selected r_max of the ellipse = {} is less than the minimal osculating circle of the street = {:.2f}'\n",
    "          .format(r_max, x111), '\\n')\n",
    "else:\n",
    "    print('selected r_max of the ellipse =  {} is larger than the minimal osculating circle of the street = {:.2f}'\n",
    "          .format(r_max, x111), '\\n')\n",
    "\n",
    "#---------------------------------------------------------------------------------------------------------------------------------------------------------------------- \n",
    "# numerically find x1 = X coordinate of CPhs and l1 := distance from CPh to CPhs for the initial condition\n",
    "# and make a plot of the initial situation\n",
    "def func_x1_l1(x0, args):\n",
    "    ergebnis = equation_street_lam[kk](*x0, *args)\n",
    "    return ergebnis.reshape(2)\n",
    "\n",
    "def funce(x0, args):\n",
    "    return min_distanzCPheiCPhej_lam[zaehler](*x0, *args)\n",
    "\n",
    "def func_jakob(x0, args):\n",
    "    return jakob_lam[zaehler](*x0, *args).reshape(2)\n",
    "\n",
    "\n",
    "\n",
    "# This is to asign colors of 'plasma' to the ellipses.\n",
    "Test = mp.colors.Normalize(0, n)\n",
    "Farbe = mp.cm.ScalarMappable(Test, cmap='plasma')\n",
    "farben = [Farbe.to_rgba(l) for l in range(n)]    # color of the starting position\n",
    "\n",
    "fig, ax = plt.subplots(figsize=(10, 10))\n",
    "ax.set_aspect('equal')\n",
    "ax.set_title(f'possible contact points are where the tangents of street and ellipse differ by less than {np.rad2deg(np.arcsin(min_winkel)):.1f}° \\n The dots on the street indicates the closest one \\n The magenta arrows indicate the (relative) initial speeds \\n' +\n",
    "             f'The white dots are the particles' )\n",
    " \n",
    "for kk in range(n):\n",
    "    TEST = []\n",
    "    TEST1 = []\n",
    "    x0 = list((-100., 100.))\n",
    "    for epsilon in np.linspace(1.e-15, 2.*np.pi, int(25/min_winkel)):\n",
    "        epsilons_list1[kk] = epsilon\n",
    "        if nhat_lam[kk](*q_list1, *epsilons_list1, a1, b1)[1] <= 0.:\n",
    "            args1 = q_list1 + x_list1 + y_list1 + [xs_list1[j] for j in range(n) if j != kk] + [ls_list1[j] for j in range(n) if j != kk] + epsilons_list1 + [a1, b1, amplitude1, frequenz1]\n",
    "            ergebnis = root(func_x1_l1, x0, args1, method='broyden1')\n",
    "            x0 = ergebnis.x\n",
    "            xs_list1[kk] = x0[0]\n",
    "            ls_list1[kk] = x0[1]\n",
    "   \n",
    "            if parallel_street_lam[kk](*q_list1, *xs_list1, *epsilons_list1, *ls_list1, a1, b1, amplitude1, frequenz1) < min_winkel:\n",
    "                TEST1.append(liste_lam[kk](*q_list1, *x_list1, *y_list1, *epsilons_list1, a1, b1))\n",
    "                TEST.append((*x0, epsilon))\n",
    "    kontakt = min(TEST, key = lambda k: k[1])\n",
    "    epsilons_list1[kk] = kontakt[2]\n",
    "    xs_list1[kk]       = kontakt[0]\n",
    "    ls_list1[kk]       = kontakt[1]\n",
    "     \n",
    "\n",
    "    Cax = np.array([TEST1[i][0][0] for i in range(len(TEST1))])\n",
    "    Cay = np.array([TEST1[i][0][1] for i in range(len(TEST1))])\n",
    "    Cex = np.array([TEST1[i][1][0] for i in range(len(TEST1))])\n",
    "    Cey = np.array([TEST1[i][1][1] for i in range(len(TEST1))])\n",
    "\n",
    "    elli = patches.Ellipse((x_list1[kk], y_list1[kk]), width=2.*a1, height=2.*b1, angle=np.rad2deg(q_list1[kk]), zorder=1, fill=True, color=farben[kk], ec='black')\n",
    "    ax.add_patch(elli)\n",
    "    weite = X1 + max(a1, b1)\n",
    "    ax.plot(x_list1[kk], y_list1[kk], color='yellow', marker='o', markersize=2)\n",
    "    ax.plot(Po_ort_lam[kk](*q_list1, *x_list1, *y_list1, a1, b1, *alpha_list1, *beta_list1)[0], Po_ort_lam[kk](*q_list1, *x_list1, *y_list1, a1, b1, *alpha_list1, *beta_list1)[1], color='white', marker='o', markersize=5)\n",
    "    ax.plot(kontakt[0], gesamt1(kontakt[0], amplitude1, frequenz1), color=farben[kk], marker='o', markersize=7)\n",
    "    ax.plot(np.linspace(-weite, weite, 100), gesamt_lam(np.linspace(-weite, weite, 100), amplitude1, frequenz1), color='blue')\n",
    "    for i in range(len(Cax)):\n",
    "        x_werte = [Cax[i], Cex[i]]\n",
    "        y_werte = [Cay[i], Cey[i]]\n",
    "        ax.plot(x_werte, y_werte)\n",
    "        ax.arrow(Cax[i], Cay[i], Cex[i]-Cax[i], Cey[i]- Cay[i], shape='full', width=0.025)\n",
    "\n",
    "# plot the initial speeds\n",
    "ux_np = np.array(ux_list1)\n",
    "uy_np = np.array(uy_list1)\n",
    "u_np  = np.array(u_list1)\n",
    "# normalize the initial speed arrows.\n",
    "groesse = max(np.max(np.abs(ux_np)), np.max(np.abs(uy_np)))\n",
    "if np.abs(groesse) < 1.e-5:\n",
    "    groesse = 1.\n",
    "ux_np = ux_np/groesse * 2. * max(a1, b1)    # If I do not multiply with max(a1, b1) the arrows are too short to be seen properly\n",
    "uy_np = uy_np/groesse * 2. * max(a1, b1)\n",
    "u_np  = u_np / np.max(np.abs(u_np))\n",
    "\n",
    "# plot the linear speed arrows\n",
    "style = \"Simple, tail_width=0.5, head_width=4, head_length=8\"\n",
    "kw = dict(arrowstyle=style, color=\"magenta\")\n",
    "for i in range(n):\n",
    "    a11 = patches.FancyArrowPatch((x_list1[i], y_list1[i]), (x_list1[i] + ux_np[i], y_list1[i] + uy_np[i]), **kw)\n",
    "    ax.add_patch(a11)\n",
    "\n",
    "# plot the curved arrows for the rotational speed\n",
    "for j, i in enumerate(u_np):\n",
    "    farbe = 'magenta'\n",
    "    if i >= 0.: \n",
    "        zeit = np.linspace(0., i * 1.9 * np.pi, 100)\n",
    "        xk = 1.5 * b1 * np.cos(zeit) + x_list1[j]\n",
    "        yk = 1.5 * b1 * np.sin(zeit) + y_list1[j]\n",
    "        ax.plot(xk, yk, color = farbe)\n",
    "        ax.arrow(xk[95], yk[95], xk[95] - xk[94], yk[95] - yk[94], shape='full', lw=0.5, length_includes_head=True, head_width=0.5, color=farbe)\n",
    "    else:\n",
    "        zeit = np.linspace(2.*np.pi, (1+i) * 1.9 * np.pi, 100)\n",
    "        xk = 1.5 * b1 * np.cos(zeit) + x_list1[j]\n",
    "        yk = 1.5 * b1 * np.sin(zeit) + y_list1[j]\n",
    "        ax.plot(xk, yk, color = farbe)\n",
    "        ax.arrow(xk[95], yk[95], xk[95] - xk[94], yk[95] - yk[94], shape='full', lw=0.5, length_includes_head=True, head_width=0.5, color=farbe)\n",
    "\n",
    "# find possible collision points between the ellipses\n",
    "zaehler        = -1\n",
    "TEST3          = []\n",
    "le_hilfs       = []\n",
    "epsilone_hilfs = []\n",
    "le_list1       = []\n",
    "epsilone_list1 = []\n",
    "\n",
    "for i, j in itt.permutations(range(n), r=2):\n",
    "    zaehler += 1\n",
    "    x0 = (0., 0.)                                                       # initial guess\n",
    "    args1 = q_list1 + x_list1 + y_list1 + [a1, b1]\n",
    "# here I use the minimize() method, as I do not have a good guess for epsilon_i, epsilon_j to minimize the distance\n",
    "# in the integration, I use the gradient method, as there are good initial guesses available, and the gradient method seems faster and more accurate.\n",
    "    epsilon_min = minimize(funce, x0, args1, jac=func_jakob, tol=1.e-12)\n",
    "    min_eps = epsilon_min.x\n",
    "    ll_min = funce(min_eps, args1)\n",
    "    TEST3.append([ll_min, i, j, min_eps[0], min_eps[1], zaehler])\n",
    "    le_hilfs.append(ll_min)\n",
    "    le_list1.append(ll_min)\n",
    "    epsilone_hilfs.append((min_eps[0], min_eps[1]))                     # needed later to re - calculate the 'contact epsilon' and the distances from ellipse_i to ellipse_j \n",
    "    epsilone_list1.append((min_eps[0], min_eps[1]))\n",
    "\n",
    "for kk in range(len(TEST3)):\n",
    "    koerper1 = TEST3[kk][1]\n",
    "    koerper2 = TEST3[kk][2]\n",
    "    epsilon1 = TEST3[kk][3]\n",
    "    epsilon2 = TEST3[kk][4]\n",
    "    x11 = CPhe_list_lam(*q_list1, *x_list1, *y_list1, a1, b1, epsilon1)[koerper1][0]\n",
    "    x12 = CPhe_list_lam(*q_list1, *x_list1, *y_list1, a1, b1, epsilon2)[koerper2][0]\n",
    "    y11 = CPhe_list_lam(*q_list1, *x_list1, *y_list1, a1, b1, epsilon1)[koerper1][1]\n",
    "    y12 = CPhe_list_lam(*q_list1, *x_list1, *y_list1, a1, b1, epsilon2)[koerper2][1]\n",
    "    \n",
    "    ax.plot([x11, x12], [y11, y12], color=farben[koerper1], linestyle='dotted')\n",
    "\n",
    "#------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------\n",
    "# fill in balance parameters\n",
    "print('angles on ellipse for contact with street: {}°'.format([np.rad2deg(epsilons_list1[i]) for i in range(n)]))\n",
    "rhodts_list1 = []\n",
    "for i in range(n):\n",
    "    rhodts_list1. append(rhodts_lam[i](*q_list1, *x_list1, *y_list1, *u_list1, *ux_list1, *uy_list1, *epsilons_list1, a1, b1, amplitude1, frequenz1))\n",
    "print('impact speeds:                            ', rhodts_list1)\n",
    "print('potential X coordinates of contact point: ', xs_list1)\n",
    "print('\\n')\n",
    "for i in range(n):\n",
    "    print(f'initial distance of ellise_{i} from the street is {ls_list1[i]:.3f}')\n",
    "print('\\n')\n",
    "zaehler = -1\n",
    "for i, j in itt.permutations(range(n), r=2):\n",
    "    zaehler += 1\n",
    "    if i < j:\n",
    "        print(f'Measured in their body fixed frames, {np.rad2deg(epsilone_list1[zaehler][0]):.1f}° and {np.rad2deg(epsilone_list1[zaehler][1]):.1f}° are the angles at which ellipse_{i} and ellipse_{j} would collide')\n",
    "\n",
    "zaehler = -1\n",
    "for i, j in itt.permutations(range(n), r=2):\n",
    "    zaehler += 1\n",
    "    if i < j:\n",
    "        print(f'the initial distance of ellipse_{i} and ellipse={j} is {le_list1[zaehler]:.3f}')\n",
    "print('\\n')\n",
    "\n",
    "zaehler = -1\n",
    "for i, j in itt.permutations(range(n), r=2):\n",
    "    zaehler += 1\n",
    "    args1 = [epsilone_list1[zaehler][0], epsilone_list1[zaehler][1]] + q_list1 + x_list1 + y_list1 + [a1, b1]\n",
    "    richtung_list1[zaehler] = richtung_lam[zaehler](*args1)\n",
    "\n",
    "# pL    = [m, mo, g, a, b, amplitude, frequenz] + [ctau, EYe, EYs, nue, nus] + xs_list + epsilons_list + ls_list + alpha_list + beta_list + rhodts_list + epsilone_list + le_list + rhodte_list + richtung_list\n",
    "pL_vals = [m1, mo1, g1, a1, b1, amplitude1, frequenz1] + [ctau1, EYe1, EYs1, nue1, nus1] + xs_list1 + epsilons_list1 + ls_list1 + alpha_list1 + beta_list1 + rhodts_list1 + epsilone_list1 + le_list1 + rhodte_list1 + richtung_list1\n",
    "\n",
    "y0 = q_list1 + x_list1 + y_list1 + u_list1 + ux_list1 + uy_list1\n",
    "print('\\n')\n",
    "print('starting values are:   ', y0)\n",
    "print('\\n')\n",
    "print(f'it took {(time.time() - start2):.3f} sec to get valid initial conditions')\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Here the actual **integration** starts\\\n",
    "1.\n",
    "I look for the contact point of the ellipses with the street as described above.\\\n",
    "I start considering only int(zaehler / min winkel) points around the ellipse. If this does not find a 'good' point, I try with more points.\\\n",
    "This is done with the *try...raise Exception()* loop.\\\n",
    "2.\n",
    "I try to minimize the distance between ellipses by solving $\\dfrac{d}{d\\epsilon_i} |{}^{CPhe_i} \\bar r^{CPhe_j}(\\epsilon_i, \\epsilon_j)|$ and $\\dfrac{d}{d\\epsilon_j} |{}^{CPhe_i} \\bar r^{CPhe_j}(\\epsilon_i, \\epsilon_j)|$, for $\\epsilon_i, \\epsilon_j$\\\n",
    "As this is oinly a necessary condition, the correct solution depends on the initial guess. Very near to a collision, I switch from the $\\nabla$ method to using minimize(..), with the Jacobian, and a small tolerance.\\\n",
    "This seems better near a contact point.\\\n",
    "3.\n",
    "Unless I make max_step very small, the integration does not work.\\\n",
    "I assume, of course I do not know!, that the reason is this: finding the distances is not very accurate and may not depend continuously on the coordinates. So, when *solve_ipv* tries to improve the accuracy by switching back and forth in time, and changing the step size, this inaccuracy makes it fail.\\\n",
    "(of course only important when the penetration depths are negative - but then again, without collisions, this would be a very simple simulation)\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "scrolled": false
   },
   "outputs": [],
   "source": [
    "#==============================\n",
    "cut_off = 90000\n",
    "zaehler    =  5\n",
    "max_step = 0.0001 \n",
    "#==============================\n",
    "start1 = time.time()\n",
    "         \n",
    "zaehler1      = zaehler\n",
    "nixwars       = [0] * n\n",
    "kontaktes     = [[] for _ in range(n)]\n",
    "fehler        = 0\n",
    "call_minimize = 0\n",
    "\n",
    "zeitee    = [[] for _ in range(n*(n-1))]\n",
    "laengeee  = [[] for _ in range(n*(n-1))]     \n",
    "epsilonee = [[] for _ in range(n*(n-1))]     \n",
    "\n",
    "def func_x1_l1_1(x0, args):\n",
    "    kk = args[-1]\n",
    "    args1 = [args[jj] for jj in range(len(args) - 1)]\n",
    "    ergebnis = equation_street_lam[kk](*x0, *args1)\n",
    "    return ergebnis.reshape(2)\n",
    "\n",
    "def func_x1_l1_1_jakob(x0, args):\n",
    "    kk = args[-1]\n",
    "    args1 = [args[jj] for jj in range(len(args) - 1)]\n",
    "    ergebnis = equation_street_jakob_lam[kk](*x0, *args1)\n",
    "    return ergebnis\n",
    "\n",
    "\n",
    "def funce1didj(x0, args):\n",
    "    zaehler = args[-1]\n",
    "    args1 = [args[i] for i in range(len(args)-1)]\n",
    "    return min_distanzCPheiCPhej_lam1[zaehler](*x0, *args1)\n",
    "\n",
    "def funce1didj_jakob(x0, args):\n",
    "    zaehler = args[-1]\n",
    "    args1 = [args[i] for i in range(len(args)-1)]\n",
    "    return jakob_lam1[zaehler](*x0, *args1)\n",
    "\n",
    "\n",
    "def funce1(x0, args):\n",
    "    zaehler = args[-1]\n",
    "    args1 = [args[i] for i in range(len(args)-1)]\n",
    "    return min_distanzCPheiCPhej_lam[zaehler](*x0, *args1)\n",
    "\n",
    "def funce1_abs(x0, args): \n",
    "# the 'unsigned distance between ellipse. Used when the ellipse are very close to each other\n",
    "    zaehler = args[-1]\n",
    "    args1 = [args[i] for i in range(len(args)-1)]\n",
    "    return np.abs(min_distanzCPheiCPhej_lam[zaehler](*x0, *args1))\n",
    "\n",
    "def func_jakob1(x0, args):\n",
    "    zaehler = args[-1]\n",
    "    args1 = [args[i] for i in range(len(args)-1)]\n",
    "    return jakob_lam[zaehler](*x0, *args1).reshape(2)\n",
    "   \n",
    "#@profile\n",
    "def ellipse_street(y, args, kk, zaehler1):\n",
    "    TEST = []   #TEST = List.empty_list(types.float64)\n",
    "    x0  = (args[12 + kk], args[12 + 2*n + kk])                    \n",
    "    for epsilon in np.linspace(1.e-15, 2.*np.pi, int(zaehler1/min_winkel)):\n",
    "        args[12 + n + kk] = epsilon\n",
    "        if nhat_lam[kk](*[y[i] for i in range(n)], *[args[12 + n + jj] for jj in range(n)], a1, b1)[1] <= 0.:\n",
    "            args1 = [y[i] for i in range(3*n)] + [args[12 + jj] for jj in range(n) if jj != kk] + [args[12 + 2*n +jj] for jj in range(n) if jj != kk] + [args[12 + n + jj] for jj in range(n)] + [a1, b1, amplitude1, frequenz1] + [kk]\n",
    "            ergebnis = root(func_x1_l1_1, x0, args1, jac=func_x1_l1_1_jakob)\n",
    "            args[12 + kk]       = ergebnis.x[0]\n",
    "            args[12 + 2*n + kk] = ergebnis.x[1]\n",
    "                \n",
    "            if parallel_street_lam[kk](*[y[jj] for jj in range(n)], *[args[12 + jj] for jj in range(3*n)], a1, b1, amplitude1, frequenz1) < min_winkel:\n",
    "                TEST.append((*ergebnis.x, epsilon))\n",
    "    \n",
    "    return TEST\n",
    "\n",
    "\n",
    "\n",
    "#@profile\n",
    "def gradient(t, y, args):\n",
    "    global zaehler1, nixwars, fehler, call_minimize\n",
    "\n",
    "# find possible collision points between the ellipses, and also find rhodtemax\n",
    "# pL_vals = [m1, mo1, g1, a1, b1, amplitude1, frequenz1] + [ctau1, EYe1, EYs1, nue1, nus1] + xs_list1 + epsilons_list1 + ls_list1 + alpha_list1 + beta_list1 + rhodts_list1 + epsilone_list1 + le_list1 + rhodte_list1 + richtung_list1\n",
    "    zaehler3 = -1  \n",
    "    for _ in range(n*(n-1)):\n",
    "        zaehler3 += 1\n",
    "        x0 = args[12 + 6*n + zaehler3]\n",
    "        if args[12 + 6*n + n*(n-1) + zaehler3] <= 0.0002      :\n",
    "            args1 = [y[ij] for ij in range(3*n)] + [a1, b1] + [zaehler3]\n",
    "# if the ellipses are very close to each other, I switch to minimize: \n",
    "# - as this does not happen often, it does not slow down the simulation too much\n",
    "# - it better finds the distance.\n",
    "# Note, that I niminize the 'unsigned' distance\n",
    "            epsilon_min = minimize(funce1_abs, x0, args1, jac=func_jakob1, tol=1.e-10)\n",
    "            call_minimize += 1\n",
    "            if epsilon_min.success != True:\n",
    "                fehler += 1\n",
    "#                print(f'mimimize message is: {epsilon_min.message}, it happened at time t = {t:.3f}, integration time {(time.time() - start1):.3f}')\n",
    "            min_eps = epsilon_min.x % (2. * np.pi)\n",
    "            args[12 + 6*n + zaehler3]           = (min_eps[0], min_eps[1])\n",
    "\n",
    "        else:\n",
    "            args1 = [y[ij] for ij in range(3*n)] + [a1, b1] + [zaehler3]\n",
    "            epsilon_min = root(funce1didj, x0, args1, jac=funce1didj_jakob)\n",
    "            min_eps = epsilon_min.x % (2.* np.pi)\n",
    "             \n",
    "        ll_min = funce1(min_eps, args1)\n",
    "        args[12 + 6*n + n*(n-1) + zaehler3] = ll_min\n",
    "        args[12 + 6*n + zaehler3]           = (min_eps[0], min_eps[1])\n",
    "        laengeee[zaehler3].append(ll_min)\n",
    "        epsilonee[zaehler3].append([min_eps[0], min_eps[1]])\n",
    "        zeitee[zaehler3].append(t)\n",
    "\n",
    "        if 0. <= ll_min <= 0.01: \n",
    "            args1 = [args[12 + 6*n + jj] for jj in range(n*(n-1))] + [args[12 + 6*n + 3*n*(n-1) + jj] for jj in range(n*(n-1)) ] + [a1, b1]\n",
    "            rhodteiej = rhodte_lam[zaehler3](*y, *args1)\n",
    "            args[12 + 6*n + 2*n*(n-1) + zaehler3] = rhodteiej\n",
    "        \n",
    "# If the distance between the contact points becomes very small, calculating the direction CPhe_i.pos_from(CPhej) seems to become numerically difficult. This allows me to fix the direction once \n",
    "# the distance is small. Of course, mechanically speaking not correct.\n",
    "        if ll_min > 2.e-4:\n",
    "            args1 = [args[12 + 6*n + zaehler3][0], args[12 + 6*n + zaehler3][1]]  + [y[jj] for jj in range(3*n)] + [a1, b1]\n",
    "            args[12 + 6*n + 3*n*(n-1) + zaehler3] = richtung_lam[zaehler3](*args1)\n",
    "\n",
    "# After penetration, the direction has to be reversed to ensure that the contact is the force of ellipse_i on ellipse_j \n",
    "        elif ll_min < -2.e-4:\n",
    "             args1 = [args[12 + 6*n + zaehler3][0], args[12 + 6*n + zaehler3][1]]  + [y[jj] for jj in range(3*n)] + [a1, b1]\n",
    "             args[12 + 6*n + 3*n*(n-1) + zaehler3] = (-richtung_lam[zaehler3](*args1)[0], -richtung_lam[zaehler3](*args1)[1])\n",
    "        else:\n",
    "            pass\n",
    "#==========================================================================================================================================================================================================\n",
    "    \n",
    "# numerically determine the closest potential contact point of the ellipses to the street\n",
    "# pL_vals = [m1, mo1, g1, a1, b1, amplitude1, frequenz1] + [ctau1, EYe1, EYs1, nue1, nus1] + xs_list1 + epsilons_list1 + ls_list1 + alpha_list1 + beta_list1 + rhodts_list1 + epsilone_list1 + le_list1 + rhodte_list1 + richtung_list1\n",
    "       \n",
    "    for kk in range(n):\n",
    " \n",
    "        while nixwars[kk] < cut_off:\n",
    "            try:\n",
    "                TEST = ellipse_street(y, args, kk, zaehler1)\n",
    "                \n",
    "                if len(TEST) > 0:\n",
    "                    raise Rausspringen\n",
    "                zaehler1 = 5 * zaehler1\n",
    "                nixwars[kk] += 1\n",
    "        \n",
    "                if nixwars[kk] > cut_off: \n",
    "                    raise Exception(f'At {t:.3f} sec fsolve(..) did not find a solution for the {nixwars[kk]}th time for ellipse_{kk}. Hence integration was terminated')\n",
    "#                print(f'at time {t:.6f} no contact point was found immediately. Totally {nixwars} such occurences so far, running time is {(time.time() - start1):.3f}, zaehler1 is {zaehler1}')\n",
    "\n",
    "\n",
    "            except:            \n",
    "                kontakt = min(TEST, key = lambda k: k[1])\n",
    "                args[12 + n + kk]   = kontakt[2]                    #epsilons_list1[kk] = kontakt[2]\n",
    "                args[12 + kk]       = kontakt[0]                    #xs_list1[kk]       = kontakt[0]\n",
    "                args[12 + 2*n + kk] = kontakt[1]                    #ls_list1[kk]       = kontakt[1]\n",
    "                zaehler1 = zaehler\n",
    "                kontaktes[kk].append(kontakt)\n",
    "                break\n",
    "\n",
    "# determine the speed right at the impact time           \n",
    "        if 0. <= args[12 + 2*n + kk] <= 0.1:\n",
    "            args[12 + 5*n + kk] = rhodts_lam[kk](*y, *[args[12 + n + jj] for jj in range(n)], a1, b1, amplitude1, frequenz1) \n",
    "                    \n",
    "    sol = np.linalg.solve(MM_lam(*y, *args), force_lam(*y, *args))\n",
    "    return np.array(sol).T[0]\n",
    "\n",
    "\n",
    "times = np.linspace(0., intervall, schritte)\n",
    "t_span = (0., intervall)\n",
    "\n",
    "resultat1 = solve_ivp(gradient, t_span, y0, t_eval = times, args=(pL_vals,), max_step=max_step) #, method='BDF' , atol=1.e-4, rtol=1.e-4) \n",
    "resultat = resultat1.y.T\n",
    "print('Shape of result: ', resultat.shape)\n",
    "event_dict = {-1: 'Integration failed', 0: 'Integration finished successfully', 1: 'some termination event'}\n",
    "print(event_dict[resultat1.status], ' the message is: ', resultat1.message)\n",
    "print(f'on {nixwars} occasions a contact point with the street was not found immediately' )\n",
    "if call_minimize != 0:\n",
    "    print(f'Trying to find the distance between ellipses minimize(...) had some problem in {fehler} cases, in {(fehler/call_minimize * 100):.3f} % of total calls of minimize')\n",
    "print('\\n')\n",
    "print('the integration made {} function calls. It took {:.3f} sec'.format(resultat1.nfev, time.time() - start1))\n",
    "profiler.print_stats() "
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Initially, I had problems finding the distances of the ellipses from each other, so I made this plot to help me find out where the problem might be.\\\n",
    "Now it mostly works, but I felt no need to delete it."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "fig, ax = plt.subplots(figsize=(10,5))\n",
    "bishin = len(zeitee[0])\n",
    "zaehler = -1\n",
    "for i, j in itt.permutations(range(n), r=2):\n",
    "    zaehler += 1\n",
    "#    if j > i:\n",
    "    ax.plot(zeitee[zaehler][: bishin], laengeee[zaehler][:bishin], label=f'distance of ellipse_{i} from ellipse_{j}')\n",
    "ax.set_title('Distances of ellipses from each other, as calculated during integration')\n",
    "ax.set_xlabel('time (sec)')\n",
    "ax.set_ylabel('distance (m)')\n",
    "ax.legend();"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Plot the generalized coordinates you want to see."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "scrolled": false
   },
   "outputs": [],
   "source": [
    "fig, ax = plt.subplots(figsize=(10, 5))\n",
    "bezeichnung = [str(i) for i in qL]\n",
    "zeigen = [j for j in range(2*n, 3*n)] + [j for j in range(5*n, 6*n)]\n",
    "for i in zeigen:\n",
    "    ax.plot(times[: resultat.shape[0]], resultat[:, i], label=bezeichnung[i])\n",
    "ax.set_xlabel('time (sec)')\n",
    "ax.set_ylabel('units depend on which gen. coordinates were selected')\n",
    "ax.set_title('generalized coordinates')\n",
    "ax.legend();"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "**Location and distance of contact point**\\\n",
    "The distance is needed for the spring energy.\\\n",
    "The closest distance to a possible contact point is available only during numerical integration.\\\n",
    "I collect them during integration, and match them as closely as possible to the points in time given by the result.\n",
    "- *epsilonee* holds the contact angles of $ellipse_i$ and $ellipse_j$ during each step of the numerical integration\n",
    "- *laengeee* holds the distance between $ellipse_i$ and $ellipse_j$ during each step of the numerical integration\n",
    "- *kontaktes* holds (among other data) the distance of $ellipse_i$ from the street.\n",
    "- *zeitee* holds the time of the storage in the lists above\n",
    "- *indexe* holds the location in *zeitee* closest to the respective time in times. \n",
    "\n",
    "All else is pretty obvious.\n",
    "\n",
    "**NOTE**: I only look at $\\dfrac{zaehler}{min_{winkel}}$ ( = 50 with my parameters) positions on the circumference of the ellipse to find the closest distance to the street. Hence, it may not find the distance, but some other point on the street, where the 'tangent criterium' is met.\\\n",
    "This only seems to happen when the elllipse is far away from the street, so it does not matter. If it is close to the street, it seems to find the closest distance. The distance is only needed for the EOM if it is negative. "
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "scrolled": false
   },
   "outputs": [],
   "source": [
    "zeitee = np.array(zeitee)\n",
    "\n",
    "indexe  = []\n",
    "\n",
    "for zeit in times:\n",
    "    zaehler1 = np.min(np.argwhere(np.array(zeitee[0, :] >= zeit)))\n",
    "    indexe.append(zaehler1)\n",
    "\n",
    "if len(indexe) != len(times):\n",
    "    raise Exception('Something happened')\n",
    "\n",
    "laenge = [[] for _ in range(n*(n-1))]\n",
    "winkel = [[] for _ in range(n*(n-1))]\n",
    "\n",
    "zaehler = -1\n",
    "for _ in range(n*(n-1)):\n",
    "    zaehler += 1\n",
    "    for i in range(len(times)):\n",
    "        winkel[zaehler].append(epsilonee[zaehler][indexe[i]])\n",
    "        laenge[zaehler].append(laengeee[zaehler][indexe[i]])\n",
    "kontaktee = np.array(laenge)\n",
    "winkel    = np.array(winkel)\n",
    "\n",
    "kontakte = [[] for _ in range(n)]\n",
    "for kk in range(n):\n",
    "    for i in range(len(times)):\n",
    "        kontakte[kk].append(kontaktes[kk][indexe[i]])\n",
    "kontakte = np.array(kontakte)\n",
    "    \n",
    "fig, (ax1, ax2, ax3, ax4) = plt.subplots(4, 1, figsize=(10,20))\n",
    "for kk in range(n):\n",
    "    ax1.plot(times, kontakte[kk, :, 1], label ='ellipse_' + str(kk))\n",
    "ax1.set_title('distance of the ellipses from the street')\n",
    "#ax.set_xlabel('time (sec)')\n",
    "ax1.set_ylabel('distance (m)')\n",
    "ax1.legend()\n",
    "\n",
    "#fig, ax = plt.subplots(figsize=(10,5))\n",
    "test1 = [[] for _ in range(n)]\n",
    "test2 = [[] for _ in range(n)]\n",
    "zaehler = [0. for _ in range(n+1)]\n",
    "for kk in range(n):\n",
    "    for i in  range(kontakte.shape[1]):\n",
    "        if kontakte[kk, i, 1] <= 0.:\n",
    "            test1[kk]. append(times[i])\n",
    "            test2[kk].append(kontakte[kk, i, 1])\n",
    "            zaehler[kk] += 1\n",
    "        else:\n",
    "            test1[kk]. append(times[i])\n",
    "            test2[kk].append(0.)\n",
    "\n",
    "\n",
    "    ax2.plot(test1[kk], test2[kk], label='ellipse_' + str(kk))\n",
    "ax2.set_title('Penetration close up view')\n",
    "#ax2.set_xlabel('time (sec)')\n",
    "ax2.set_ylabel('penetration depth (m)')\n",
    "ax2.legend()\n",
    "for kk in range(n):\n",
    "    print(f'There are {zaehler[kk]} points where penetration between ellipse_{kk} and street takes place, {zaehler[kk]/schritte * 100:.3f} % of total points')\n",
    "#---------------------------------------------------------------------------------------------------------------------------------------\n",
    "#fig, ax = plt.subplots(figsize=(10,5))\n",
    "zaehler3 = -1\n",
    "for i, j in itt.permutations(range(n), r=2):\n",
    "    zaehler3 += 1\n",
    "#    if i < j:\n",
    "    ax3.plot(times, kontaktee[zaehler3, :], label =f'ellipse_{i} from ellipse_{j}')\n",
    "ax3.set_title('distance of contact point from the ellipses')\n",
    "#ax3.set_xlabel('time (sec)')\n",
    "ax3.set_ylabel('distance (m)')\n",
    "ax3.legend()\n",
    "\n",
    "#fig, ax = plt.subplots(figsize=(10,5))\n",
    "test1 = [[] for _ in range(n*(n-1))]\n",
    "test2 = [[] for _ in range(n*(n-1))]\n",
    "zaehler = [0. for _ in range(n*(n-1)+1)]\n",
    "for kk in range(n*(n-1)):\n",
    "    for i in  range(kontaktee.shape[1]):\n",
    "        if kontaktee[kk, i] <= 0.:\n",
    "            test1[kk]. append(times[i])\n",
    "            test2[kk].append(kontaktee[kk, i])\n",
    "            zaehler[kk] += 1\n",
    "        else:\n",
    "            test1[kk]. append(times[i])\n",
    "            test2[kk].append(0.)\n",
    "\n",
    "zaehler3 = -1\n",
    "for i, j in itt.permutations(range(n), r=2):\n",
    "    zaehler3 += 1\n",
    "#    if i < j:\n",
    "    ax4.plot(test1[zaehler3], test2[zaehler3], label=f'ellipse_{i} from ellipse_{j}')\n",
    "ax4.set_title('Penetration close up view')\n",
    "ax4.set_xlabel('time (sec)')\n",
    "ax4.set_ylabel('penetration depth (m)')\n",
    "ax4.legend()\n",
    "zaehler3 = -1\n",
    "for i, j in itt.permutations(range(n), r=2):\n",
    "    zaehler3 += 1\n",
    "    if i < j:\n",
    "        print(f'There are {zaehler[zaehler3]} points where penetration between ellipse{i} and ellipse_{j} takes place, {zaehler[zaehler3]/schritte * 100:.3f} % of total points')"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "**Energies of the system**\\\n",
    "For $c_{\\tau} = 1$ total energy should be constant, else it should drop monotonically.\\\n",
    "It does not always do so, numerical problems? I do not know."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "scrolled": false
   },
   "outputs": [],
   "source": [
    "kin_np    = np.empty(schritte)    \n",
    "pot_np    = np.empty(schritte)\n",
    "spring_np = np.empty(schritte)\n",
    "total_np  = np.empty(schritte)\n",
    "\n",
    "pL_vals2 = copy.deepcopy(pL_vals)\n",
    "\n",
    "for i in range(schritte):\n",
    "    for kk in range(n):\n",
    "        pL_vals2[12 + kk]        = kontakte[kk, i, 0]\n",
    "        pL_vals2[12 + 2*n + kk]  = kontakte[kk, i, 1]\n",
    "        pL_vals2[12 + n + kk]    = kontakte[kk, i, 2]\n",
    "\n",
    "    for kk in range(n*(n-1)):\n",
    "        pL_vals2[12 + 6*n + n*(n-1) + kk] = kontaktee[kk, i]\n",
    "        pL_vals2[12 + 6*n + kk]           = winkel[kk, i]\n",
    "\n",
    "    kin_np[i]      =    kin_lam(*[resultat[i, j] for j in range(resultat.shape[1])], *pL_vals2)\n",
    "    pot_np[i]      =    pot_lam(*[resultat[i, j] for j in range(resultat.shape[1])], *pL_vals2)\n",
    "    spring_np[i]   = spring_lam(*[resultat[i, j] for j in range(resultat.shape[1])], *pL_vals2)\n",
    "    total_np[i]    = kin_np[i] + pot_np[i] + spring_np[i]\n",
    "        \n",
    "fig, ax = plt.subplots(figsize=(10, 5))\n",
    "ax.plot(times, pot_np, label='potential energy')\n",
    "ax.plot(times, kin_np, label='kinetic energy')\n",
    "ax.plot(times, spring_np, label='spring energy')\n",
    "ax.plot(times, total_np, label='total energy')\n",
    "ax.set_xlabel('time (sec)')\n",
    "ax.set_ylabel(\"energy (Nm)\")\n",
    "ax.set_title(f'Energies of the system, with ctau = {ctau1}')\n",
    "ax.legend();\n",
    "total_max = np.max(total_np)\n",
    "total_min = np.min(total_np)\n",
    "if ctau1 == 1.:\n",
    "    print('max deviation of total energy from being constant is {:.2e} % of max total energy'.\n",
    "          format((total_max - total_min)/total_max * 100))"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "**Animate** the motion of the ellipse\\\n",
    "The dotted lines show the 'closest' contact point, that is the point where the ellipse would touch the street if it were 'blown up' to just touch the street at this specific point in time.\\\n",
    "(If the ellipse is far away from the street, the above statement may not be true, see explanation above.)\\\n",
    "The lines connecting the ellipses...as above.\\\n",
    "As HCML is slow, I limit to number of points considered to around *schrittzahl*"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "scrolled": false
   },
   "outputs": [],
   "source": [
    "#======================\n",
    "schrittzahl = 2500\n",
    "#======================\n",
    "# reduce the number of points considered to around schrittzahl\n",
    "faktor = max(1, int(resultat.shape[0] / schrittzahl))\n",
    "resultat1 = []\n",
    "times1    = []\n",
    "kontakte1 = [[] for _ in range(n)]\n",
    "for i in range(resultat.shape[0]):\n",
    "    if i % faktor == 0:\n",
    "        resultat1.append(resultat[i, :])\n",
    "        times1.append(times[i])\n",
    "        \n",
    "for kk in range(n):\n",
    "    for i in range(resultat.shape[0]):\n",
    "        if i % faktor == 0:\n",
    "            kontakte1[kk].append(kontakte[kk, i, 0])\n",
    "\n",
    "schritte1 = len(times1)\n",
    "resultat1 = np.array(resultat1)\n",
    "print(f'Here, {schritte1} ponts in time are considerted')\n",
    "# contact points / lines of ellipse_i with ellipse_j\n",
    "# 1.) reduce the points in time considered to around schrittzahl\n",
    "zaehler = -1\n",
    "winkel_wenig = [[] for _ in range(n*(n-1))]\n",
    "for jj, jjj in itt.permutations(range(n), r=2):\n",
    "    zaehler += 1\n",
    "    for i in range(resultat.shape[0]):\n",
    "        if i % faktor == 0:\n",
    "            winkel_wenig[zaehler].append(winkel[zaehler, i])\n",
    "\n",
    "winkel_np = np.array(winkel_wenig)\n",
    "\n",
    "# 2.) Get the coordinates of the potential impact points of ellipse_i with ellipse_j, sort them, so their connecting line can be plotted\n",
    "zaehler = -1\n",
    "delta_list= list(list(sm.symbols(f'delta{i}{j}, delta{j}{i}')) for i, j in itt.permutations(range(n), r=2))\n",
    "coords_list = []\n",
    "coords_lam = []\n",
    "for i, j in itt.permutations(range(n), r=2):\n",
    "    zaehler += 1\n",
    "    CPi, CPj = sm.symbols('CPi, CPj', cls=me.Point)\n",
    "    CPi.set_pos(Dmc_list[i], a * sm.cos(delta_list[zaehler][0])*A_list[i].x + b * sm.sin(delta_list[zaehler][0])*A_list[i].y)\n",
    "    CPj.set_pos(Dmc_list[j], a * sm.cos(delta_list[zaehler][1])*A_list[j].x + b * sm.sin(delta_list[zaehler][1])*A_list[j].y)\n",
    "    coords = [[me.dot(CPi.pos_from(O), uv) for uv in (N.x, N.y)], [me.dot(CPj.pos_from(O), uv) for uv in (N.x, N.y)]]\n",
    "    coords_lam.append(sm.lambdify(q_list + x_list + y_list + delta_list[zaehler] +  [a, b], coords, cse=True)) \n",
    "\n",
    "coords_list  = []\n",
    "for i in range(n*(n-1)):\n",
    "    coords_list1 = []\n",
    "    for j in range(schritte1):\n",
    "        coords_list1.append(coords_lam[i](*[resultat1[j, jj] for jj in range(3*n)], *winkel_np[i, j], a1, b1))\n",
    "    coords_list.append(coords_list1)\n",
    "coords_list = np.array(coords_list)\n",
    "\n",
    "X_werte_elli = [[] for _ in range(n*(n-1))]\n",
    "Y_werte_elli = [[] for _ in range(n*(n-1))]\n",
    "\n",
    "for zaehler in range(n*(n-1)):\n",
    "    for i in range(schritte1):\n",
    "        X_werte_elli[zaehler].append([coords_list[zaehler, i, 0, 0], coords_list[zaehler, i, 1, 0]])\n",
    "        Y_werte_elli[zaehler].append([coords_list[zaehler, i, 0, 1], coords_list[zaehler, i, 1, 1]])\n",
    "\n",
    "X_werte_elli = np.array(X_werte_elli)\n",
    "Y_werte_elli = np.array(Y_werte_elli)\n",
    "\n",
    "# 3.) get the coordinates of the points, for better visibility\n",
    "X_point = np.empty((X_werte_elli.shape[0], X_werte_elli.shape[1]))\n",
    "Y_point = np.empty((Y_werte_elli.shape[0], Y_werte_elli.shape[1]))\n",
    "\n",
    "for zaehler in range(n*(n-1)):\n",
    "    for i in range(schritte1):\n",
    "        X_point[zaehler, i] = X_werte_elli[zaehler, i, 0]\n",
    "        Y_point[zaehler, i] = Y_werte_elli[zaehler, i, 0]\n",
    "\n",
    "                \n",
    "resultat1 = np.array(resultat1)\n",
    "times1    = np.array(times1)\n",
    "kontakte1 = np.array(kontakte1)\n",
    "\n",
    "# This is to asign colors of 'plasma' to the ellipses.\n",
    "Test = mp.colors.Normalize(0, n)\n",
    "Farbe = mp.cm.ScalarMappable(Test, cmap='plasma')\n",
    "farben = [Farbe.to_rgba(l) for l in range(n)]    # color of the starting position\n",
    "\n",
    "Dmcx = np.empty((n, schritte1))\n",
    "Dmcy = np.empty((n, schritte1))\n",
    "for kk in range(n):\n",
    "    for i in range(schritte1):\n",
    "        Dmcx[kk, i] = resultat1[i, n + kk]\n",
    "        Dmcy[kk, i] = resultat1[i, 2 *n + kk]\n",
    "\n",
    "Po_lam = []\n",
    "for kk in range(n):\n",
    "    Po_lam.append(sm.lambdify(qL + pL, [me.dot(Po_list[kk].pos_from(O), uv) for uv in (N.x, N.y)]))\n",
    "Pox = np.empty((n, schritte1, 1))\n",
    "Poy = np.empty((n, schritte1, 1))\n",
    "\n",
    "for kk in range(n):\n",
    "    for i in range(schritte1):\n",
    "            Pox[kk, i] = Po_lam[kk](*[resultat1[i, j] for j in range(resultat.shape[1])], *pL_vals)[0]\n",
    "            Poy[kk, i] = Po_lam[kk](*[resultat1[i, j] for j in range(resultat.shape[1])], *pL_vals)[1]\n",
    "\n",
    "# needed to give the picture the right size.\n",
    "xmin = np.min(Dmcx)\n",
    "xmax = np.max(Dmcx)\n",
    "ymin = np.min(Dmcy)\n",
    "ymax = np.max(Dmcy)\n",
    "\n",
    "# Data to draw the uneven street\n",
    "cc = max(a1, b1)\n",
    "strassex = np.linspace(xmin - 1.*cc, xmax + 1.*cc, schritte1)\n",
    "strassey = [gesamt_lam(strassex[i], amplitude1, frequenz1) for i in range(schritte1)]\n",
    "ymin = np.min(strassey)\n",
    "\n",
    "def animate_pendulum(times1, Dmcx, Dmcy, Pox, Poy ):\n",
    "    \n",
    "    fig, ax = plt.subplots(figsize=(8, 8), subplot_kw={'aspect': 'equal'})\n",
    "    \n",
    "    ax.axis('on')\n",
    "    ax.set_xlim(xmin - 1.*cc, xmax + 1.*cc)\n",
    "    ax.set_ylim(ymin - 1., ymax + 1.*cc)\n",
    "    ax.plot(strassex, strassey)\n",
    "    \n",
    "#    ax.plot(test_np, test1_np, color='green')\n",
    "    LINE1 = ['line1' + str(i) for i in range(n)]\n",
    "    LINE2 = ['line2' + str(i) for i in range(n)]\n",
    "    LINE3 = ['line3' + str(i) for i in range(n)]\n",
    "    LINE4 = ['line4' + str(i) for i in range(n)]\n",
    "    LINE5 = ['line5' + str(i) for i in range(n)]\n",
    "\n",
    "    for kk in range(n):\n",
    "        line1, = ax.plot([], [], lw=0.5, color='yellow', marker='o', markeredgecolor='black')                      # center of the ellipse\n",
    "        line2, = ax.plot([], [], 'o', color=\"white\", markeredgecolor='black')                                # particle on the ellipse\n",
    "        line3  = ax.axvline(kontakte1[kk, 0], linestyle='--', color=farben[kk])\n",
    "        line4  = ax.axhline(gesamt_lam(kontakte1[kk, 0], amplitude1, frequenz1), linestyle='--', color=farben[kk])\n",
    "        LINE1[kk] = line1\n",
    "        LINE2[kk] = line2\n",
    "        LINE3[kk] = line3\n",
    "        LINE4[kk] = line4\n",
    "    \n",
    "        elli = patches.Ellipse((Dmcx[kk, 0], Dmcy[kk, 0]), width=2.*a1, height=2.*b1, angle=np.rad2deg(resultat1[0, kk]), zorder=1, fill=True, color=farben[kk], ec='black')\n",
    "        LINE5[kk] = ax.add_patch(elli)\n",
    "\n",
    "    LINE6 = [f'line6{i}{j}' for i, j in itt.permutations(range(n), r=2)]\n",
    "    LINE7 = [f'line7{i}{j}' for i, j in itt.permutations(range(n), r=2)]\n",
    "    \n",
    "    zaehler = -1\n",
    "    for ii, jj in itt.permutations(range(n), r=2):\n",
    "        zaehler += 1\n",
    "        line6, = ax.plot([], [], linestyle = '-', lw=0.5, color=farben[ii])\n",
    "        line7, = ax.plot([], [], marker = 'o', color=farben[ii], markeredgecolor='white')\n",
    "\n",
    "        LINE6[zaehler] = line6\n",
    "        LINE7[zaehler] = line7\n",
    "\n",
    "\n",
    "    def animate(i):\n",
    "        message = (f'Running time {times1[i]:.2f} sec \\n The white dot is the particle \\n The doted lines of the same color cross at the (potential) impact points \\n The lines connecting the ellipses give their potential impact points.')\n",
    "        ax.set_title(message, fontsize=12)\n",
    "        ax.set_xlabel('X direction', fontsize=12)\n",
    "        ax.set_ylabel('Y direction', fontsize=12)\n",
    "\n",
    "        for kk in range(n):\n",
    "            LINE5[kk].set_center((Dmcx[kk, i], Dmcy[kk, i]))\n",
    "            LINE5[kk].set_angle(np.rad2deg(resultat1[i, kk]))\n",
    "                       \n",
    "            LINE1[kk].set_data([Dmcx[kk, i]], [Dmcy[kk, i]])                  \n",
    "            LINE2[kk].set_data(Pox[kk, i], Poy[kk ,i])      \n",
    "            LINE3[kk].set_xdata([kontakte1[kk, i], kontakte1[kk, i]])\n",
    "            wert = gesamt_lam(kontakte1[kk, i], amplitude1, frequenz1)\n",
    "            LINE4[kk].set_ydata([wert, wert])\n",
    "\n",
    "        for zaehler in range(n*(n-1)):\n",
    "            LINE6[zaehler].set_data(X_werte_elli[zaehler, i], Y_werte_elli[zaehler, i])\n",
    "            LINE7[zaehler].set_data([X_point[zaehler, i]], [Y_point[zaehler, i]])\n",
    "                  \n",
    "        return LINE1 + LINE2 + LINE3 + LINE4 + LINE5 + LINE6\n",
    "\n",
    "    anim = animation.FuncAnimation(fig, animate, frames=schritte1,\n",
    "                                   interval=2000*np.max(times1) / schritte1,\n",
    "                                   blit=True)\n",
    "    plt.close(fig)\n",
    "    return anim\n",
    "\n",
    "anim = animate_pendulum(times1, Dmcx, Dmcy, Pox, Poy)\n",
    "print(f'it took {time.time() - start :.3f} sec to run the program BEFORE HTML')\n",
    "HTML(anim.to_jshtml())    # needed, when run on an iPad, I know no other way to do it. It is SLOW!"
   ]
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "Python 3 (ipykernel)",
   "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
}