{
 "cells": [
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# Calculating mortar projection matrices\n",
    "\n",
    "Author(s): Jukka Aho\n",
    "\n",
    "**Abstract**: Evaluate mortar projection matrices in 2d and 3d"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 54,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "using JuliaFEM\n",
    "using JuliaFEM: Seg2, Basis, Field, FieldSet, dinterpolate, interpolate, get_connectivity\n",
    "using PyPlot\n",
    "using ForwardDiff"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Create some test boundaries:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 55,
   "metadata": {
    "collapsed": false
   },
   "outputs": [
    {
     "data": {
      "text/plain": [
       "rlinspace (generic function with 1 method)"
      ]
     },
     "execution_count": 55,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "function rlinspace(X1, X2, R, n, p0)\n",
    "    fx(ϕ, xc) = R*cos(ϕ) + xc\n",
    "    fy(ϕ, yc) = R*sin(ϕ) + yc\n",
    "    function F(p)\n",
    "        ϕ1, ϕ2, xc, yc = p\n",
    "        return [\n",
    "            fx(ϕ1, xc) - X1[1]\n",
    "            fy(ϕ1, yc) - X1[2]\n",
    "            fx(ϕ2, xc) - X2[1]\n",
    "            fy(ϕ2, yc) - X2[2]\n",
    "        ]\n",
    "    end\n",
    "    J = ForwardDiff.jacobian(F)\n",
    "    p = copy(p0)\n",
    "    for i=1:10\n",
    "        dp = -J(p) \\ F(p)\n",
    "        p += dp\n",
    "        if norm(dp) < 1.0e-9\n",
    "            println(\"Converged. p = $p\")\n",
    "            break\n",
    "        end\n",
    "    end\n",
    "    ϕ1, ϕ2, xc, yc = p\n",
    "    ϕ = linspace(ϕ2, ϕ1, n)\n",
    "    return fx(ϕ, xc), fy(ϕ, yc)\n",
    "end"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 56,
   "metadata": {
    "collapsed": false
   },
   "outputs": [
    {
     "data": {
      "text/plain": [
       "interpolate (generic function with 11 methods)"
      ]
     },
     "execution_count": 56,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "function FEM.interpolate(basis::Basis, field::Field, xis::Array{Vector,1})\n",
    "    [FEM.interpolate(basis, field, xi) for xi in xis]\n",
    "end\n",
    "#FEM.interpolate(Γ₁[1], \"geometry\", Vector[[-1.0], [1.0]], 0.0)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 78,
   "metadata": {
    "collapsed": false
   },
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "Converged. p = [4.2905787641613236,5.624156522861784,0.04706336239279979,6.561746550428803]"
     ]
    },
    {
     "data": {
      "image/png": "[encoded data, 25228 characters]",
      "text/plain": [
       "PyPlot.Figure(PyObject <matplotlib.figure.Figure object at 0x000000002F321F98>)"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    },
    {
     "data": {
      "text/plain": [
       "(-0.1,4.0)"
      ]
     },
     "execution_count": 78,
     "metadata": {},
     "output_type": "execute_result"
    },
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "\n",
      "Converged. p = [2.110067133637596,1.2802355090457196,1.5675520985912952,-3.7904167887303712]\n"
     ]
    }
   ],
   "source": [
    "srand(42)\n",
    "\n",
    "nsl = 6\n",
    "nm = 5\n",
    "\n",
    "ϕ11 = 5.4\n",
    "ϕ12 = 4.6\n",
    "R1 = 5\n",
    "xc1 = -0.8\n",
    "yc1 = 9.9\n",
    "X1 = [-2.0, 2.0]\n",
    "X2 = [4.0, 3.5]\n",
    "x1, y1 = rlinspace(X1, X2, R1, nsl, [ϕ11, ϕ12, xc1, yc1])\n",
    "\n",
    "ϕ11 = 2.2\n",
    "ϕ12 = 1.1\n",
    "R1 = 5\n",
    "xc1 = 1.9\n",
    "yc1 = -4.1\n",
    "X1 = [-1.0, 0.5]\n",
    "X2 = [3.0, 1.0]\n",
    "x2, y2 = rlinspace(X1, X2, R1, nsl, [ϕ11, ϕ12, xc1, yc1])\n",
    "\n",
    "#x1 = linspace(0, 3, nsl) + rand(nsl)*0.1\n",
    "#y1 = 0.5*rand(nsl)\n",
    "#x2 = linspace(0, 3, nm) + rand(nm)*0.1 + 0.4\n",
    "#y2 = 0.5*rand(nm) + 0.5\n",
    "\n",
    "\"\"\"\n",
    "Calculate element local normal field\n",
    "\"\"\"\n",
    "function calculate_normals!(element::Seg2, time, field_name=\"normal\")\n",
    "    normal_fieldset = FieldSet(field_name)\n",
    "    normal_field = Field(time, Vector[])\n",
    "    tangent(xi) = dinterpolate(element, \"geometry\", xi, time)\n",
    "    for xi in Vector[[-1.0], [1.0]]\n",
    "        t = tangent(xi)\n",
    "        n = [-t[2], t[1]]\n",
    "        push!(normal_field, n/norm(n))\n",
    "    end\n",
    "    push!(normal_fieldset, normal_field)\n",
    "    push!(element, normal_fieldset)\n",
    "end\n",
    "\n",
    "function create_elements(X, sid=0)\n",
    "    Γ = []\n",
    "    nnodes = size(X, 2)\n",
    "    nelements = nnodes-1\n",
    "    for i=1:nelements\n",
    "        con = sid+[i, i+1]\n",
    "        el = Seg2(con)\n",
    "        push!(el, FieldSet(\"geometry\", [Field(0.0, Vector[X[:, i], X[:, i+1]])]))\n",
    "        push!(Γ, el)\n",
    "    end\n",
    "    return Γ\n",
    "end\n",
    "\n",
    "function plot_element(el; plot_with_normal=false)\n",
    "    # create a array of vectors\n",
    "    xis = linspace([-1.0], [1.0], 3)\n",
    "    time = 0.0\n",
    "    coords = interpolate(el, \"geometry\", xis, time)\n",
    "    ncoords = interpolate(el, \"geometry\", xis, time)\n",
    "    normals = interpolate(el, \"normal\", xis, time)\n",
    "    xs = [X[1] for X in coords]\n",
    "    ys = [X[2] for X in coords]\n",
    "    plot(xs, ys, \"-\")\n",
    "    plot([xs[1], xs[end]], [ys[1], ys[end]], \"ko\")\n",
    "    plot([xs[1], xs[end]], [ys[1], ys[end]], \"ko\")\n",
    "    if plot_with_normal\n",
    "        for i=1:3\n",
    "            p0 = ncoords[i]\n",
    "            p1 = ncoords[i]+0.3*normals[i]\n",
    "            plot([p0[1], p1[1]], [p0[2], p1[2]], \"-k\")\n",
    "        end\n",
    "    end\n",
    "end\n",
    "\n",
    "Γ₁ = create_elements([x1 y1]', 0)\n",
    "Γ₂ = create_elements([x2 y2]', nsl);\n",
    "for el in [Γ₁; Γ₂]\n",
    "    calculate_normals!(el, 0.0)\n",
    "end\n",
    "\n",
    "figure(figsize=(10, 3))\n",
    "for el in Γ₁\n",
    "    plot_element(el; plot_with_normal=true)\n",
    "end\n",
    "for el in Γ₂\n",
    "    plot_element(el; plot_with_normal=false)\n",
    "end\n",
    "axis(\"equal\")\n",
    "ylim(-0.1, 4.0)\n",
    "#axis(\"off\")"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### Calculating surface normals\n",
    "\n",
    "To define so called \"continuous normal field\", normals must be unambiguous in nodes. This can be done by averaging normals of adjacent elements:\n",
    "\\begin{equation}\n",
    "\\mathbf{n}_{k}=\\frac{\\sum_{e=1}^{n_{k}^{\\mathrm{adj}}} \\mathbf{n}_{k}^{\\left(e\\right)}}{\\left\\Vert \\sum_{e=1}^{n_{k}^{\\mathrm{adj}}} \\mathbf{n}_{k}^{\\left(e\\right)}\\right\\Vert }.\n",
    "\\end{equation}\n",
    "\n",
    "In practice, we take average of all normal vectors connecting to some arbitrary node."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 79,
   "metadata": {
    "collapsed": false
   },
   "outputs": [
    {
     "data": {
      "text/plain": [
       "JuliaFEM.Field{Array{Array{T,1},1}}(0.0,1,Array{T,1}[[0.7021480119880804,-0.7120310170639945],[0.7021480119880804,-0.7120310170639945]])"
      ]
     },
     "execution_count": 79,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "FEM.interpolate(Γ₁[1][\"normal\"], 0.0)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 80,
   "metadata": {
    "collapsed": false
   },
   "outputs": [
    {
     "data": {
      "image/png": "[encoded data, 24736 characters]",
      "text/plain": [
       "PyPlot.Figure(PyObject <matplotlib.figure.Figure object at 0x000000002F310630>)"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    },
    {
     "data": {
      "text/plain": [
       "(-0.1,3.6)"
      ]
     },
     "execution_count": 80,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "function average_normals!(elements, time, normal_field=\"normal\")\n",
    "    d = Dict()\n",
    "    # calculate sum of normals connecting to node k\n",
    "    for el in elements\n",
    "        c = get_connectivity(el)\n",
    "        n = interpolate(el[normal_field], time).values\n",
    "        for (ci, ni) in zip(c, n)\n",
    "            d[ci] = haskey(d, ci) ? d[ci] + ni : ni\n",
    "        end\n",
    "    end\n",
    "    # norm\n",
    "    for (ci, ni) in d\n",
    "        d[ci] /= norm(d[ci])\n",
    "    end\n",
    "    # update back to elements\n",
    "    for el in elements\n",
    "        c = get_connectivity(el)\n",
    "        #new_normals = Field(time, [d[ci] for ci in c])\n",
    "        #set_field(el, normal_field, new_normals)\n",
    "        #el[normal_field][end].values = new_normals\n",
    "        #push!(el[normal_field], new_normals)\n",
    "        el[normal_field][end].values = [d[ci] for ci in c]\n",
    "    end\n",
    "end\n",
    "\n",
    "average_normals!(Γ₁, 0.0)\n",
    "average_normals!(Γ₂, 0.0)\n",
    "\n",
    "figure(figsize=(10, 3))\n",
    "for el in Γ₁\n",
    "    plot_element(el; plot_with_normal=true)\n",
    "end\n",
    "for el in Γ₂\n",
    "    plot_element(el; plot_with_normal=false)\n",
    "end\n",
    "axis(\"equal\")\n",
    "ylim(-0.1, 3.6)\n",
    "#axis(\"off\")"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 6,
   "metadata": {
    "collapsed": false
   },
   "outputs": [
    {
     "data": {
      "text/plain": [
       "calc_projection (generic function with 1 method)"
      ]
     },
     "execution_count": 6,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "function newton(R, dR, x0=0.0; max_iterations=10, tol=1.0e-9)\n",
    "    x = x0\n",
    "    for i=1:max_iterations\n",
    "        dx = -R(x)/dR(x)\n",
    "        x += dx\n",
    "        if abs(dx) < tol\n",
    "            break\n",
    "        end\n",
    "    end\n",
    "    x\n",
    "end\n",
    "\n",
    "\"\"\"\n",
    "Find projection from slave nodes to master element.\n",
    "\n",
    "Parameters\n",
    "----------\n",
    "sel :: Element\n",
    "    slave element\n",
    "mel :: Element\n",
    "    master element\n",
    "sxi :: Vector\n",
    "    projection point in slave side (typically [-1.0] or [1.0])\n",
    "\n",
    "Returns\n",
    "-------\n",
    "mxi :: Vector\n",
    "    point in master element corresponding to xi in slave\n",
    "\n",
    "\"\"\"\n",
    "function calc_projection_from_slave_to_master(sel, mel, sxi; solver_options...)\n",
    "    # slave side point\n",
    "    X1 = interpolate(sel, :Geometry, sxi)\n",
    "    N1 = interpolate(sel, :Normals, sxi)\n",
    "    # master element function and their derivatives\n",
    "    X2(xi) = interpolate(mel, :Geometry, xi)\n",
    "    dX2(xi) = dinterpolate(mel, :Geometry, xi)\n",
    "    # residual & solution\n",
    "    R(xi) = det([X2(xi)-X1 N1])\n",
    "    dR(xi) = det([dX2(xi) N1])\n",
    "    mxi = newton(R, dR; solver_options...)\n",
    "    return [mxi]\n",
    "end\n",
    "\n",
    "\"\"\"\n",
    "Find projection from master to slave element.\n",
    "\n",
    "Parameters\n",
    "----------\n",
    "sel :: Element\n",
    "    slave element\n",
    "mel :: Element\n",
    "    master element\n",
    "mxi :: Vector\n",
    "    projection point in master side (typically [-1.0] or [1.0])\n",
    "\n",
    "Returns\n",
    "-------\n",
    "sxi :: Vector\n",
    "    point in slave element corresponding to xi in master\n",
    "\"\"\"\n",
    "function calc_projection_from_master_to_slave(sel, mel, mxi; solver_options...)\n",
    "    # slave element functions and their derivatives\n",
    "    X1(xi) = interpolate(sel, :Geometry, xi)\n",
    "    dX1(xi) = dinterpolate(sel, :Geometry, xi)\n",
    "    N1(xi) = interpolate(sel, :Normals, xi)\n",
    "    dN1(xi) = dinterpolate(sel, :Normals, xi)\n",
    "    # master side point\n",
    "    X2 = interpolate(mel, :Geometry, mxi)\n",
    "    # residual & solution\n",
    "    R(xi) = det([X1(xi)-X2 N1(xi)])\n",
    "    dR(xi) = det([dX1(xi) N1(xi)]) + det([X1(xi)-X2 dN1(xi)])\n",
    "    sxi = newton(R, dR; solver_options...)\n",
    "    return [sxi]\n",
    "end\n",
    "\n",
    "function has_projection(xi1, xi2)\n",
    "    l = abs(xi1[2]-xi1[1])[1]\n",
    "    return l > 1.0e-9\n",
    "end\n",
    "\n",
    "function calc_projection(sel, mel; clamp=true)\n",
    "    xi1a = calc_projection_from_master_to_slave(sel, mel, [-1.0])\n",
    "    xi1b = calc_projection_from_master_to_slave(sel, mel, [ 1.0])\n",
    "    xi2a = calc_projection_from_slave_to_master(sel, mel, [-1.0])\n",
    "    xi2b = calc_projection_from_slave_to_master(sel, mel, [ 1.0])\n",
    "    xi1 = Vector[xi1a, xi1b]\n",
    "    xi2 = Vector[xi2a, xi2b]\n",
    "    if clamp\n",
    "        clamp!(xi1, -1, 1)\n",
    "        clamp!(xi2, -1, 1)\n",
    "    end\n",
    "    return xi1, xi2\n",
    "end"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Visualize results"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 7,
   "metadata": {
    "collapsed": false
   },
   "outputs": [
    {
     "data": {
      "text/plain": [
       "integrate_segment (generic function with 1 method)"
      ]
     },
     "execution_count": 7,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "function integrate_segment(sel, mel, xi1, xi2)\n",
    "    nsel = get_number_of_basis_functions(sel)\n",
    "    nmel = get_number_of_basis_functions(mel)\n",
    "    De = zeros(nsel, nsel)\n",
    "    Me = zeros(nsel, nmel)\n",
    "    return De, Me\n",
    "end"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 8,
   "metadata": {
    "collapsed": false
   },
   "outputs": [
    {
     "data": {
      "image/png": "[encoded data, 19056 characters]",
      "text/plain": [
       "PyPlot.Figure(PyObject <matplotlib.figure.Figure object at 0x7faed5beb5d0>)"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    },
    {
     "data": {
      "text/plain": [
       "(-3.0,5.0,-0.1,3.6)"
      ]
     },
     "execution_count": 8,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "function plot_all()\n",
    "    figure(figsize=(10, 3))\n",
    "    for el in Γ₁\n",
    "        plot_element(el; plot_with_normal=true)\n",
    "    end\n",
    "    for el in Γ₂\n",
    "        plot_element(el; plot_with_normal=false)\n",
    "    end\n",
    "    for sel in Γ₁\n",
    "        for mel in Γ₂\n",
    "            xi1, xi2 = calc_projection(sel, mel)\n",
    "            if has_projection(xi1, xi2)\n",
    "                X1 = interpolate(sel, :Geometry, xi1)\n",
    "                X2 = interpolate(mel, :Geometry, xi2)\n",
    "                for s=1:2\n",
    "                    x = [X1[s][1], X2[s][1]]\n",
    "                    y = [X1[s][2], X2[s][2]]\n",
    "                    plot(x, y, \"-kx\")\n",
    "                end\n",
    "                De, Me = integrate_segment(sel, mel, xi1, xi2)\n",
    "            end\n",
    "        end\n",
    "    end\n",
    "    axis(\"equal\")\n",
    "    ylim(-0.1, 3.6)\n",
    "    axis(\"off\")\n",
    "end\n",
    "\n",
    "plot_all()"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "A little experience, raise the degree of elements, smooth geometry, and test projection for 2nd order:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 9,
   "metadata": {
    "collapsed": false
   },
   "outputs": [
    {
     "ename": "LoadError",
     "evalue": "LoadError: BoundsError: attempt to access (2,)\n  at index [2]\nwhile loading In[9], in expression starting on line 13",
     "output_type": "error",
     "traceback": [
      "LoadError: BoundsError: attempt to access (2,)\n  at index [2]\nwhile loading In[9], in expression starting on line 13",
      "",
      " in getindex at tuple.jl:8",
      " in get_detJ at /home/jukka/.julia/v0.5/JuliaFEM/src/equations.jl:50",
      " in fit_derivative_field! at /home/jukka/.julia/v0.5/JuliaFEM/src/elements.jl:434",
      " [inlined code] from In[9]:14",
      " in anonymous at no file:0"
     ]
    }
   ],
   "source": [
    "# to raise degree of elements and smooth geometry\n",
    "function tangent(el, xi)\n",
    "    normal = interpolate(el, :Normals, xi)\n",
    "    [0 -1; 1 0]'*normal\n",
    "end\n",
    "for el in [Γ₁; Γ₂]\n",
    "    set_degree(el, 2)\n",
    "    for field in (:Geometry, :Normals)\n",
    "        push!(el.fields[field], [0.0, 0.0])\n",
    "        el.fields[field] = el.fields[field][1:get_number_of_basis_functions(el)]\n",
    "    end\n",
    "end\n",
    "for el in [Γ₁; Γ₂]\n",
    "    fit_derivative_field!(el, :Geometry, tangent, Int[1, 2])\n",
    "end\n",
    "plot_all()"
   ]
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "Julia 0.4.0",
   "language": "julia",
   "name": "julia-0.4"
  },
  "language_info": {
   "file_extension": ".jl",
   "mimetype": "application/julia",
   "name": "julia",
   "version": "0.4.0"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 0
}
