diff --git a/src/JuliaFEM.jl b/src/JuliaFEM.jl index 9c71664..2f102db 100644 --- a/src/JuliaFEM.jl +++ b/src/JuliaFEM.jl @@ -74,6 +74,8 @@ export find_intersection, calc_reflection, calc_normal ### Mortar methods ### include("problems_mortar.jl") +include("problems_mortar_2d.jl") +include("problems_mortar_3d.jl") include("problems_mortar_2d_autodiff.jl") export calculate_normals, calculate_normals!, diff --git a/src/petsc.jl b/src/petsc.jl deleted file mode 100644 index ba0cbde..0000000 --- a/src/petsc.jl +++ /dev/null @@ -1,98 +0,0 @@ -# This file is a part of JuliaFEM. -# License is MIT: see https://github.com/JuliaFEM/JuliaFEM.jl/blob/master/LICENSE.md - -# PETSc interface for solver - -using PETSc - -import JuliaFEM.Core: solve - -""" -Parameters ----------- -preconditioner : "jacobi" -ksp_type: "bcgs", "gmres"? -""" -function solve(K, f, C, g, ::Type{Val{:PETSc_GMRES}}; preconditioner=nothing) - t0 = time() - dim = size(K, 1) - - # make sure C is square - boundary_dofs = unique(rowvals(C)) - boundary_dofs2 = unique(rowvals(C')) - @assert length(boundary_dofs) == length(boundary_dofs2) - @assert setdiff(Set(boundary_dofs), Set(boundary_dofs2)) == Set() - all_dofs = unique(rowvals(K)) - interior_dofs = setdiff(all_dofs, boundary_dofs) - info("PETSc: all dofs = $(length(all_dofs))") - info("PETSc: interior dofs = $(length(interior_dofs))") - info("PETSc: boundary dofs = $(length(boundary_dofs))") - # solve displacement on known boundary - LUF = lufact(C[boundary_dofs, boundary_dofs]) - u = zeros(dim) - u[boundary_dofs] = LUF \ full(g[boundary_dofs]) - info("PETSc: displacement on boundary solved.") - normub = norm(u[boundary_dofs]) - if isapprox(normub, 0.0) - info("PETSc: homogeneous dirichlet boundary") - end - - # interior domain and lagrange multipliers - - t = time() - # this is completely unnecessary step and will be removed in future. - # --> - info("PETSc: creating matrices in PETSc format.") - ninterior_dofs = length(interior_dofs) - - # nz, see https://github.com/JuliaParallel/PETSc.jl/issues/52 - d = Dict{Int64, Int64}() - for i in rowvals(K) - haskey(d, i) ? (d[i] += 1) : (d[i] = 1) - end - nz = maximum(values(d)) - - A = PETSc.Mat(Float64, ninterior_dofs, ninterior_dofs; nz=nz) - info("PETSc: $ninterior_dofs interior dofs, assembling to PETSc Mat") - for (i, j, v) in zip(findnz(K[interior_dofs, interior_dofs])...) - A[i, j] = v - end - - fi = f[interior_dofs] - - b = PETSc.Vec(Float64, ninterior_dofs, PETSc.C.VECMPI) - for (i, j, v) in zip(findnz(sparse(f[interior_dofs]))...) - b[i] = v - end - - info("PETSc: initialization of matrices in ", time()-t, " seconds") - # <-- - - kspg = PETSc.KSP(A, ksp_monitor="") - - # apply preconditioner if defined - if !isa(preconditioner, Void) - info("PETSc: preconditioner: $preconditioner") - pc = PETSc.PC(Float64, comm=PETSc.comm(kspg), pc_type=preconditioner) - PETSc.chk(PETSc.C.PCSetOperators(pc.p, A.p, A.p)) - kspg = PETSc.KSP(pc, ksp_monitor="") - end - - info("PETSc: performing ksp GMRES solve") - x = kspg \ b - info("PETSc: finished ksp solve") - info("PETSc: ksp info:\n",petscview(kspg)) - for (i, d) in enumerate(interior_dofs) - u[d] = x[i] - end - - la = zeros(dim) - Kib = K[interior_dofs, boundary_dofs] - Kbb = K[boundary_dofs, boundary_dofs] - la[boundary_dofs] = LUF \ full(Kib'*u[interior_dofs] - Kbb*u[boundary_dofs]) - - info("PETSc: solved in ", time()-t0, " seconds. norm = ", norm(u)) - return u, la -end - -info("PETSc interface loaded.") diff --git a/src/problems_mortar.jl b/src/problems_mortar.jl index 02bfbfb..8722617 100644 --- a/src/problems_mortar.jl +++ b/src/problems_mortar.jl @@ -31,112 +31,6 @@ function get_formulation_type(problem::Problem{Mortar}) =# end -typealias MortarElements2D Union{Seg2, Seg3} -typealias MortarElements3D Union{Tri3, Tri6, Quad4} - -function newton(f, df, x; tol=1.0e-6, max_iterations=10) - for i=1:max_iterations - dx = -f(x)/df(x) - x += dx - if norm(dx) < tol - return x - end - end - error("Newton iteration did not converge in $max_iterations iterations") -end - -function cross2(a, b) - cross([a; 0], [b; 0])[3] -end - -function get_slave_elements(problem::Problem) - filter(el -> haskey(el, "master elements"), get_elements(problem)) -end - -function project_from_master_to_slave{E<:MortarElements2D}(slave_element::Element{E}, x2, time) - x1_ = slave_element["geometry"](time) - n1_ = slave_element["normal"](time) - x1(xi1) = vec(get_basis(slave_element, [xi1], time))*x1_ - dx1(xi1) = vec(get_dbasis(slave_element, [xi1], time))*x1_ - n1(xi1) = vec(get_basis(slave_element, [xi1], time))*n1_ - dn1(xi1) = vec(get_dbasis(slave_element, [xi1], time))*n1_ - R(xi1) = cross2(x1(xi1)-x2, n1(xi1)) - dR(xi1) = cross2(dx1(xi1), n1(xi1)) + cross2(x1(xi1)-x2, dn1(xi1)) - xi1 = nothing - try - xi1 = newton(R, dR, 0.0) - catch - warn("projection from master to slave failed with following arguments:") - warn("slave element x1: $x1_") - warn("slave element n1: $n1_") - warn("master element x2: $x2") - warn("time: $time") - len = norm(x1_[2] - x1_[1]) - midpnt = mean(x1_) - dist = norm(midpnt - x2) - distval = dist/len - warn("midpoint of slave element: $midpnt") - warn("length of slave element: $len") - warn("distance between midpoint of slave element and x2: $dist") - warn("charasteristic measure: $distval") - rethrow() - end - return xi1 -end - -function project_from_slave_to_master{E<:MortarElements2D}(master_element::Element{E}, x1, n1, time) - x2_ = master_element["geometry"](time) - x2(xi2) = vec(get_basis(master_element, [xi2], time))*x2_ - dx2(xi2) = vec(get_dbasis(master_element, [xi2], time))*x2_ - cross2(a, b) = cross([a; 0], [b; 0])[3] - R(xi2) = cross2(x2(xi2)-x1, n1) - dR(xi2) = cross2(dx2(xi2), n1) - xi2 = newton(R, dR, 0.0) - return xi2 -end - -function calculate_normals(elements, time, ::Type{Val{1}}; rotate_normals=false) - tangents = Dict{Int64, Vector{Float64}}() - for element in elements - conn = get_connectivity(element) - X1 = element("geometry", time) - dN = get_dbasis(element, [0.0], time) - tangent = vec(sum([kron(dN[:,i], X1[i]') for i=1:length(X1)])) - for nid in conn - if haskey(tangents, nid) - tangents[nid] += tangent - else - tangents[nid] = tangent - end - end - end - - Q = [0.0 -1.0; 1.0 0.0] - normals = Dict{Int64, Vector{Float64}}() - S = collect(keys(tangents)) - for j in S - tangents[j] /= norm(tangents[j]) - normals[j] = Q*tangents[j] - end - - if rotate_normals - for j in S - normals[j] = -normals[j] - end - end - - return normals, tangents -end - -function calculate_normals!(elements, time, ::Type{Val{1}}; rotate_normals=false) - normals, tangents = calculate_normals(elements, time, Val{1}; rotate_normals=rotate_normals) - for element in elements - conn = get_connectivity(element) - update!(element, "normal", time => [normals[j] for j in conn]) - update!(element, "tangent", time => [tangents[j] for j in conn]) - end -end - function assemble!(problem::Problem{Mortar}, time::Float64) if problem.properties.dimension == -1 problem.properties.dimension = dim = size(first(problem.elements), 1) @@ -148,454 +42,3 @@ function assemble!(problem::Problem{Mortar}, time::Float64) assemble!(problem, time, dimension, use_forwarddiff) end -function assemble!(problem::Problem{Mortar}, time::Float64, ::Type{Val{1}}, ::Type{Val{false}}) - - props = problem.properties - field_dim = get_unknown_field_dimension(problem) - field_name = get_parent_field_name(problem) - slave_elements = get_slave_elements(problem) - - # 1. calculate nodal normals and tangents for slave element nodes j ∈ S - normals, tangents = calculate_normals(slave_elements, time, Val{1}; - rotate_normals=props.rotate_normals) - update!(slave_elements, "normal", normals) - update!(slave_elements, "tangent", tangents) - - # 2. loop all slave elements - for slave_element in slave_elements - - nsl = length(slave_element) - X1 = slave_element("geometry", time) - n1 = slave_element("normal", time) - - # 3. loop all master elements - for master_element in slave_element("master elements", time) - - nm = length(master_element) - X2 = master_element("geometry", time) - - # 3.1 calculate segmentation - xi1a = project_from_master_to_slave(slave_element, X2[1], time) - xi1b = project_from_master_to_slave(slave_element, X2[2], time) - xi1 = clamp([xi1a; xi1b], -1.0, 1.0) - l = 1/2*abs(xi1[2]-xi1[1]) - isapprox(l, 0.0) && continue # no contribution in this master element - - # 3.2. bi-orthogonal basis - De = zeros(nsl, nsl) - Me = zeros(nsl, nsl) - Ae = zeros(nsl, nsl) - if props.dual_basis - for ip in get_integration_points(slave_element, 3) - detJ = slave_element(ip, time, Val{:detJ}) - w = ip.weight*detJ*l - xi = ip.coords[1] - xi_s = dot([1/2*(1-xi); 1/2*(1+xi)], xi1) - N1 = vec(get_basis(slave_element, xi_s, time)) - De += w*diagm(N1) - Me += w*N1*N1' - end - Ae = De*inv(Me) - else - Ae = eye(nsl) - end - - # 3.3. loop integration points of one integration segment and calculate - # local mortar matrices - fill!(De, 0.0) - fill!(Me, 0.0) - ge = zeros(field_dim*nsl) - for ip in get_integration_points(slave_element, 2) - detJ = slave_element(ip, time, Val{:detJ}) - w = ip.weight*detJ*l - xi = ip.coords[1] - xi_s = dot([1/2*(1-xi); 1/2*(1+xi)], xi1) - N1 = vec(get_basis(slave_element, xi_s, time)) - Phi = Ae*N1 - # project gauss point from slave element to master element in direction n_s - X_s = N1*X1 # coordinate in gauss point - n_s = N1*n1 # normal direction in gauss point - xi_m = project_from_slave_to_master(master_element, X_s, n_s, time) - N2 = vec(get_basis(master_element, xi_m, time)) - X_m = N2*X2 - De += w*Phi*N1' - Me += w*Phi*N2' - if props.adjust - haskey(slave_element, "displacement") || continue - haskey(master_element, "displacement") || continue - norm(mean(X1) - X2[1]) / norm(X1[2] - X1[1]) < props.distval || continue - norm(mean(X1) - X2[2]) / norm(X1[2] - X1[1]) < props.distval || continue - u1 = slave_element("displacement", time) - u2 = master_element("displacement", time) - x_s = X_s + N1*u1 - x_m = X_m + N2*u2 - ge += w*vec((x_m-x_s)*Phi') - end - end - - # add contribution to contact virtual work - sdofs = get_gdofs(problem, slave_element) - mdofs = get_gdofs(problem, master_element) - - for i=1:field_dim - lsdofs = sdofs[i:field_dim:end] - lmdofs = mdofs[i:field_dim:end] - add!(problem.assembly.C1, lsdofs, lsdofs, De) - add!(problem.assembly.C1, lsdofs, lmdofs, -Me) - add!(problem.assembly.C2, lsdofs, lsdofs, De) - add!(problem.assembly.C2, lsdofs, lmdofs, -Me) - end - add!(problem.assembly.g, sdofs, ge) - - end # master elements done - - end # slave elements done, contact virtual work ready - -end - -## Mesh tie 2d end - -## 3d Mortar mesh tie - -function project_vertex_to_auxiliary_plane(p::Vector, x0::Vector, n0::Vector) - return p - dot(p-x0, n0)*n0 -end - -function inv3(P::Matrix) - n, m = size(P) - @assert n == m == 3 - a, b, c, d, e, f, g, h, i = P - A = e*i - f*h - B = -d*i + f*g - C = d*h - e*g - D = -b*i + c*h - E = a*i - c*g - F = -a*h + b*g - G = b*f - c*e - H = -a*f + c*d - I = a*e - b*d - return 1/(a*A + b*B + c*C)*[A B C; D E F; G H I] -end - -function vertex_inside_polygon(q, P; atol=1.0e-6) - N = length(P) - angle = 0.0 - for i=1:N - A = P[i] - q - B = P[mod(i,N)+1] - q - c = norm(A)*norm(B) - isapprox(c, 0.0; atol=atol) && return true - cosa = dot(A,B)/c - isapprox(cosa, 1.0; atol=atol) && return false - isapprox(cosa, -1.0; atol=atol) && return true - try - angle += acos(cosa) - catch - info("Unable to calculate acos($(ForwardDiff.get_value(cosa))) when determining is a vertex inside polygon.") - info("Polygon is: $(ForwardDiff.get_value(P)) and vertex under consideration is $(ForwardDiff.get_value(q))") - info("Polygon corner point in loop: A=$(ForwardDiff.get_value(A)), B=$(ForwardDiff.get_value(B))") - info("c = ||A||*||B|| = $(ForwardDiff.get_value(c))") - rethrow() - end - end - return isapprox(angle, 2*pi; atol=atol) -end - -function calculate_centroid(P) - N = length(P) - P0 = P[1] - areas = [norm(1/2*cross(P[i]-P0, P[mod(i,N)+1]-P0)) for i=2:N] - centroids = [1/3*(P0+P[i]+P[mod(i,N)+1]) for i=2:N] - C = 1/sum(areas)*sum(areas.*centroids) - return C -end - -function get_cells(P, C) - N = length(P) - cells = Vector[] - # shared edge etc. - N < 3 && return cells - # trivial case, polygon already triangle / quadrangle - #N == 3 && return Vector[P] - #N == 4 && return Vector[P] - #V = sum([cross(P[i], P[mod(i,N)+1]) for i=1:N]) - #A = 1/2*abs(dot(n, V)) - #info("A = $A") - cells = Vector[Vector[C, P[i], P[mod(i,N)+1]] for i=1:N] - return cells - - maxa = 0.0 - maxj = 0 - for i=1:N - A = P[i] - C - B = P[mod(i,N)+1] - C - theta = acos(dot(A,B)/(norm(A)*norm(B))) - if theta > maxa - maxa = theta - maxj = i - end - end - info("max angle $(maxa/pi*180) at index $maxj, N=$N") - indices = mod(collect(maxj:maxj+N), N) - info("indices = $indices") -end - -function get_polygon_clip(xs, xm, n; debug=false) - # objective: search does line xm1 - xm2 clip xs - nm = length(xm) - ns = length(xs) - P = Vector{Float64}[] - - # 1. test is master point inside slave, if yes, add to clip - for i=1:nm - if vertex_inside_polygon(xm[i], xs) - debug && info("1. $(xm[i]) inside S -> push") - push!(P, xm[i]) - end - end - - # 2. test is slave point inside master, if yes, add to clip - for i=1:ns - if vertex_inside_polygon(xs[i], xm) - xs[i] in P && continue - debug && info("2. $(xs[i]) inside M -> push") - push!(P, xs[i]) - end - end - - for i=1:nm - # 2. find possible intersection - xm1 = xm[i] - xm2 = xm[mod(i,nm)+1] - #info("intersecting line $xm1 -> $xm2") - for j=1:ns - xs1 = xs[j] - xs2 = xs[mod(j,ns)+1] - #info("clipping polygon edge $xs1 -> $xs2") - tnom = dot(cross(xm1-xs1, xm2-xm1), n) - tdenom = dot(cross(xs2-xs1, xm2-xm1), n) - isapprox(tdenom, 0) && continue - t = tnom/tdenom - (0 <= t <= 1) || continue - q = xs1 + t*(xs2 - xs1) - #info("t=$t, q=$q, q ∈ xm ? $(vertex_inside_polygon(q, xm))") - if vertex_inside_polygon(q, xm) - q in P && continue - debug && info("3. $q inside M -> push") - push!(P, q) - end - end - end - - return P -end - -function project_vertex_to_surface{E}(p::Vector, x0::Vector, n0::Vector, - element::Element{E}, x::DVTI, time::Real; max_iterations::Int=10, iter_tol::Float64=1.0e-9) - basis(xi) = get_basis(element, xi, time) - dbasis(xi) = get_dbasis(element, xi, time) - f(theta) = basis(theta[1:2])*x - theta[3]*n0 - p - L(theta) = inv3([dbasis(theta[1:2])*x -n0]) -# L2(theta) = inv(ForwardDiff.get_value([dbasis(theta[2:3])*x -n0])) - # FIXME: for some reason forwarddiff gives NaN's here. - theta = zeros(3) - dtheta = zeros(3) - for i=1:max_iterations - dtheta = L(theta) * f(theta) - theta -= dtheta - if norm(dtheta) < iter_tol - return theta[1:2], theta[3] - end - end - - info("failed to project vertex from auxiliary plane back to surface") - info("element type: $E") - info("element connectivity: $(get_connectivity(element))") - info("auxiliary plane: x0 = $x0, n0 = $n0") - info("element geometry: $(x.data)") - info("vertex to project: $p") - info("parameter vector before giving up: $theta") - info("increment in parameter vector before giving up: $dtheta") - info("norm(dtheta) before giving up: $(norm(dtheta))") - info("f([0.0, 0.0, 0.0]) = $(f([0.0, 0.0, 0.0]))") - info("L([0.0, 0.0, 0.0]) = $(L([0.0, 0.0, 0.0]))") - - info("iterations:") - theta = zeros(3) - dtheta = zeros(3) - for i=1:max_iterations - info("iter $i, theta = $theta") - info("f = $(f(theta))") - info("L = $(L(theta))") - dtheta = L(theta) * f(theta) - info("dtheta = $(dtheta)") - theta -= dtheta - end - - error("project_point_to_surface: did not converge in $max_iterations iterations!") -end - -function calculate_normals(elements, time, ::Type{Val{2}}; rotate_normals=false) - normals = Dict{Int64, Vector{Float64}}() - for element in elements - conn = get_connectivity(element) - J = transpose(element([0.0, 0.0], time, Val{:Jacobian})) - normal = cross(J[:,1], J[:,2]) - for nid in conn - if haskey(normals, nid) - normals[nid] += normal - else - normals[nid] = normal - end - end - end - # normalize to unit normal - S = collect(keys(normals)) - for j in S - normals[j] /= norm(normals[j]) - end - if rotate_normals - for j in S - normals[j] = -normals[j] - end - end - return normals -end - -function check_orientation!(P, n; debug=false) - C = mean(P) - np = length(P) - s = [dot(n, cross(P[i]-C, P[mod(i+1,np)+1]-C)) for i=1:np] - all(s .< 0) && return - debug && info("polygon not in ccw order, fixing") - # project points to new orthogonal basis Q and sort there - t1 = (P[1]-C)/norm(P[1]-C) - t2 = cross(n, t1) - Q = [n t1 t2] - sort!(P, lt=(A, B) -> begin - A_proj = Q'*(A-C) - B_proj = Q'*(B-C) - a = atan2(A_proj[3], A_proj[2]) - b = atan2(B_proj[3], B_proj[2]) - return a > b - end) -end - -function assemble!(problem::Problem{Mortar}, time::Real, ::Type{Val{2}}, ::Type{Val{false}}; debug=true) - - props = problem.properties - field_dim = get_unknown_field_dimension(problem) - field_name = get_parent_field_name(problem) - slave_elements = get_slave_elements(problem) - area = 0.0 - - # 1. calculate nodal normals and tangents for slave element nodes j ∈ S - normals = calculate_normals(slave_elements, time, Val{2}; - rotate_normals=props.rotate_normals) - update!(slave_elements, "normal", normals) - - # 2. loop all slave elements - for slave_element in slave_elements - - slave_element_nodes = get_connectivity(slave_element) - nsl = length(slave_element) - X1 = slave_element("geometry", time) - n1 = Field([normals[j] for j in slave_element_nodes]) - - # project slave nodes to auxiliary plane (x0, Q) - #xi = get_reference_element_midpoint(slave_element) - xi = [1/3, 1/3] - N = vec(get_basis(slave_element, xi, time)) - x0 = N*X1 - n0 = N*n1 - S = Vector[project_vertex_to_auxiliary_plane(p, x0, n0) for p in X1] - - # 3. loop all master elements - for master_element in slave_element("master elements", time) - - master_element_nodes = get_connectivity(master_element) - nm = length(master_element) - X2 = master_element("geometry", time) - - # 3.1 project master nodes to auxiliary plane and create polygon clipping - M = Vector[project_vertex_to_auxiliary_plane(p, x0, n0) for p in X2] - P = get_polygon_clip(S, M, n0) - length(P) < 3 && continue # no clipping or shared edge (no volume) - check_orientation!(P, n0) - C0 = calculate_centroid(P) - - De = zeros(nsl, nsl) - Me = zeros(nsl, nm) - ge = zeros(field_dim*nsl) - - # 4. loop integration cells - for cell in get_cells(P, C0) - virtual_element = Element(Tri3) - update!(virtual_element, "geometry", cell) - #x_cell = Field(cell) - - # 5. loop integration point of integration cell - for ip in get_integration_points(virtual_element, 3) - N = vec(get_basis(virtual_element, ip, time)) - #dN = vec(get_dbasis(virtual_element, ip, time)) - #JC = transpose(sum([kron(dNC[:,j], x_cell[j]') for j=1:length(x_cell)])) - #wC = ip.weight*norm(cross(JC[:,1], JC[:,2])) - detJ = virtual_element(ip, time, Val{:detJ}) - w = ip.weight*detJ - - # project gauss point from auxiliary plane to master and slave element - #x_gauss = N*x_cell - x_gauss = virtual_element("geometry", ip, time) - if isnan(x_gauss[1]) - info("is nan") - info("x_gauss = $x_gauss") - info("cell = $cell") - info("C0 = $C0") - info("P = $P") - info("S = $S") - info("M = $M") - info("n0 = $n0") - error("nan, unable to continue") - end - xi_s, alpha = project_vertex_to_surface(x_gauss, x0, n0, slave_element, X1, time) - xi_m, alpha = project_vertex_to_surface(x_gauss, x0, n0, master_element, X2, time) - - # add contributions - N1 = vec(get_basis(slave_element, xi_s, time)) - N2 = vec(get_basis(master_element, xi_m, time)) - De += w*N1*N1' - Me += w*N1*N2' - if props.adjust - u1 = slave_element("displacement", time) - u2 = master_element("displacement", time) - x_s = N1*(X1+u1) - x_m = N2*(X2+u2) - ge += w*vec((x_m-x_s)*N1') - end - area += w - end # integration points done - - end # integration cells done - - # 6. add contribution to contact virtual work - sdofs = get_gdofs(problem, slave_element) - mdofs = get_gdofs(problem, master_element) - - for i=1:field_dim - lsdofs = sdofs[i:field_dim:end] - lmdofs = mdofs[i:field_dim:end] - add!(problem.assembly.C1, lsdofs, lsdofs, De) - add!(problem.assembly.C1, lsdofs, lmdofs, -Me) - add!(problem.assembly.C2, lsdofs, lsdofs, De) - add!(problem.assembly.C2, lsdofs, lmdofs, -Me) - end - add!(problem.assembly.g, sdofs, ge) - - end # master elements done - - end # slave elements done, contact virtual work ready - - debug && info("area of interface: $area") - -end - diff --git a/src/problems_mortar_2d.jl b/src/problems_mortar_2d.jl index 74da3dc..de42cdf 100644 --- a/src/problems_mortar_2d.jl +++ b/src/problems_mortar_2d.jl @@ -1,704 +1,213 @@ # This file is a part of JuliaFEM. # License is MIT: see https://github.com/JuliaFEM/JuliaFEM.jl/blob/master/LICENSE.md -# Mortar projection calculation for 2d, in initial configuration X +typealias MortarElements2D Union{Seg2, Seg3} -""" Find projection from slave nodes to master element, i.e. find xi2 from -master element corresponding to the xi1. -""" -function project_from_slave_to_master{S,M}(slave::Element{S}, master::Element{M}, xi1::Vector, time::Real; max_iterations=5, tol=1.0e-9) - - # slave side geometry and normal direction at xi1 - X1 = slave("geometry", xi1, time) - N1 = slave("normal-tangential coordinates", xi1, time)[:,1] - - # master side geometry at xi2 - master_basis(xi2) = get_basis(M, [xi2]) - master_dbasis(xi2) = get_dbasis(M, [xi2]) - master_geometry = master("geometry")(time) - - function X2(xi2) - N = master_basis(xi2) - return sum([N[i]*master_geometry[i] for i=1:length(N)]) - end - - function dX2(xi2) - dN = master_dbasis(xi2) - return sum([dN[i]*master_geometry[i] for i=1:length(dN)]) - end - - # equation to solve - R(xi2) = det([X2(xi2)-X1 N1]') - dR(xi2) = det([dX2(xi2) N1]') - - # solve using Newton iterations - xi2 = 0.0 +function newton(f, df, x; tol=1.0e-6, max_iterations=10) for i=1:max_iterations - dxi2 = -R(xi2) / dR(xi2) - xi2 += dxi2 - if norm(dxi2) < tol - return Float64[xi2] + dx = -f(x)/df(x) + x += dx + if norm(dx) < tol + return x + end + end + error("Newton iteration did not converge in $max_iterations iterations") +end + +function cross2(a, b) + cross([a; 0], [b; 0])[3] +end + +function get_slave_elements(problem::Problem) + filter(el -> haskey(el, "master elements"), get_elements(problem)) +end + +function project_from_master_to_slave{E<:MortarElements2D}(slave_element::Element{E}, x2, time) + x1_ = slave_element["geometry"](time) + n1_ = slave_element["normal"](time) + x1(xi1) = vec(get_basis(slave_element, [xi1], time))*x1_ + dx1(xi1) = vec(get_dbasis(slave_element, [xi1], time))*x1_ + n1(xi1) = vec(get_basis(slave_element, [xi1], time))*n1_ + dn1(xi1) = vec(get_dbasis(slave_element, [xi1], time))*n1_ + R(xi1) = cross2(x1(xi1)-x2, n1(xi1)) + dR(xi1) = cross2(dx1(xi1), n1(xi1)) + cross2(x1(xi1)-x2, dn1(xi1)) + xi1 = nothing + try + xi1 = newton(R, dR, 0.0) + catch + warn("projection from master to slave failed with following arguments:") + warn("slave element x1: $x1_") + warn("slave element n1: $n1_") + warn("master element x2: $x2") + warn("time: $time") + len = norm(x1_[2] - x1_[1]) + midpnt = mean(x1_) + dist = norm(midpnt - x2) + distval = dist/len + warn("midpoint of slave element: $midpnt") + warn("length of slave element: $len") + warn("distance between midpoint of slave element and x2: $dist") + warn("charasteristic measure: $distval") + rethrow() + end + return xi1 +end + +function project_from_slave_to_master{E<:MortarElements2D}(master_element::Element{E}, x1, n1, time) + x2_ = master_element["geometry"](time) + x2(xi2) = vec(get_basis(master_element, [xi2], time))*x2_ + dx2(xi2) = vec(get_dbasis(master_element, [xi2], time))*x2_ + cross2(a, b) = cross([a; 0], [b; 0])[3] + R(xi2) = cross2(x2(xi2)-x1, n1) + dR(xi2) = cross2(dx2(xi2), n1) + xi2 = newton(R, dR, 0.0) + return xi2 +end + +function calculate_normals(elements, time, ::Type{Val{1}}; rotate_normals=false) + tangents = Dict{Int64, Vector{Float64}}() + for element in elements + conn = get_connectivity(element) + X1 = element("geometry", time) + dN = get_dbasis(element, [0.0], time) + tangent = vec(sum([kron(dN[:,i], X1[i]') for i=1:length(X1)])) + for nid in conn + if haskey(tangents, nid) + tangents[nid] += tangent + else + tangents[nid] = tangent + end end end - println("slave element geometry") - dump(slave("geometry", time).data) - println("master element geometry") - dump(master("geometry", time).data) - error("find projection from slave to master: did not converge") -end - -""" Find projection from master surface to slave point, i.e. find xi1 from slave -element corresponding to the xi2. """ -function project_from_master_to_slave{S,M}(slave::Element{S}, master::Element{M}, xi2::Vector, time::Real; max_iterations=5, tol=1.0e-9) - - # slave side geometry and normal direction at xi1 - - slave_geometry = slave("geometry")(time) - slave_normals = slave("normal-tangential coordinates")(time) - slave_basis(xi) = get_basis(S, [xi]) - slave_dbasis(xi) = get_dbasis(S, [xi]) - - function X1(xi1) - N = slave_basis(xi1) - return sum([N[i]*slave_geometry[i] for i=1:length(N)]) + Q = [0.0 -1.0; 1.0 0.0] + normals = Dict{Int64, Vector{Float64}}() + S = collect(keys(tangents)) + for j in S + tangents[j] /= norm(tangents[j]) + normals[j] = Q*tangents[j] end - function dX1(xi1) - dN = slave_dbasis(xi1) - return sum([dN[i]*slave_geometry[i] for i=1:length(dN)]) - end - - function N1(xi1) - N = slave_basis(xi1) - return sum([N[i]*slave_normals[i] for i=1:length(N)])[:,1] - end - - function dN1(xi1) - dN = slave_dbasis(xi1) - return sum([dN[i]*slave_normals[i] for i=1:length(dN)])[:,1] - end - - # master side geometry at xi2 - X2 = master("geometry", xi2, time) - - # equation to solve - R(xi1) = det([X1(xi1)-X2 N1(xi1)]') - dR(xi1) = det([dX1(xi1) N1(xi1)]') + det([X1(xi1)-X2 dN1(xi1)]') - - # go! - xi1 = 0.0 - for i=1:max_iterations - dxi1 = -R(xi1) / dR(xi1) - xi1 += dxi1 - if norm(dxi1) < tol - return Float64[xi1] + if rotate_normals + for j in S + normals[j] = -normals[j] end end - println("slave element geometry") - dump(slave("geometry", time).data) - println("master element geometry") - dump(master("geometry", time).data) - error("find projection from master to slave: did not converge") + return normals, tangents end -# for deformed state - -""" Find projection from slave nodes to master element, i.e. find xi2 from -master element corresponding to the xi1. -""" -function project_from_slave_to_master{S,M}(slave::Element{S}, master::Element{M}, xi1::Vector, time::Real, ::Type{Val{:deformed}}; max_iterations=5, tol=1.0e-9) - - # slave side geometry and normal direction at xi1 - x1 = slave("geometry", xi1, time) - if haskey(slave, "displacement") - x1 += slave("displacement", xi1, time) +function calculate_normals!(elements, time, ::Type{Val{1}}; rotate_normals=false) + normals, tangents = calculate_normals(elements, time, Val{1}; rotate_normals=rotate_normals) + for element in elements + conn = get_connectivity(element) + update!(element, "normal", time => [normals[j] for j in conn]) + update!(element, "tangent", time => [tangents[j] for j in conn]) end - N1 = slave("normal-tangential coordinates", xi1, time)[:,1] - - # master side geometry at xi2 - master_basis(xi2) = get_basis(M, [xi2]) - master_dbasis(xi2) = get_dbasis(M, [xi2]) - master_geometry = master("geometry")(time) - if haskey(master, "displacement") - master_geometry += master("displacement")(time) - end - - function x2(xi2) - N = master_basis(xi2) - return sum([N[i]*master_geometry[i] for i=1:length(N)]) - end - - function dx2(xi2) - dN = master_dbasis(xi2) - return sum([dN[i]*master_geometry[i] for i=1:length(dN)]) - end - - # equation to solve - R(xi2) = det([x2(xi2)-x1 N1]') - dR(xi2) = det([dx2(xi2) N1]') - - # solve using Newton iterations - xi2 = 0.0 - for i=1:max_iterations - dxi2 = -R(xi2) / dR(xi2) - xi2 += dxi2 - if norm(dxi2) < tol - return Float64[xi2] - end - end - - println("slave element geometry") - dump(slave("geometry", time).data) - println("master element geometry") - dump(master("geometry", time).data) - error("find projection from slave to master: did not converge") end -""" Find projection from master surface to slave point, i.e. find xi1 from slave -element corresponding to the xi2. """ -function project_from_master_to_slave{S,M}(slave::Element{S}, master::Element{M}, xi2::Vector, time::Real, ::Type{Val{:deformed}}; max_iterations=5, tol=1.0e-9) +function assemble!(problem::Problem{Mortar}, time::Float64, ::Type{Val{1}}, ::Type{Val{false}}) - # slave side geometry and normal direction at xi1 - - slave_geometry = slave("geometry")(time) - if haskey(slave, "displacement") - slave_geometry += slave("displacement")(time) - end - slave_normals = slave("normal-tangential coordinates")(time) - slave_basis(xi) = get_basis(S, [xi]) - slave_dbasis(xi) = get_dbasis(S, [xi]) - - function x1(xi1) - N = slave_basis(xi1) - return sum([N[i]*slave_geometry[i] for i=1:length(N)]) - end - - function dx1(xi1) - dN = slave_dbasis(xi1) - return sum([dN[i]*slave_geometry[i] for i=1:length(dN)]) - end - - function N1(xi1) - N = slave_basis(xi1) - return sum([N[i]*slave_normals[i] for i=1:length(N)])[:,1] - end - - function dN1(xi1) - dN = slave_dbasis(xi1) - return sum([dN[i]*slave_normals[i] for i=1:length(dN)])[:,1] - end - - # master side geometry at xi2 - x2 = master("geometry", xi2, time) - if haskey(master, "displacement") - x2 += master("displacement", xi2, time) - end - - # equation to solve - R(xi1) = det([x1(xi1)-x2 N1(xi1)]') - dR(xi1) = det([dx1(xi1) N1(xi1)]') + det([x1(xi1)-x2 dN1(xi1)]') - - # go! - xi1 = 0.0 - for i=1:max_iterations - dxi1 = -R(xi1) / dR(xi1) - xi1 += dxi1 - if norm(dxi1) < tol - return Float64[xi1] - end - end - - println("slave element geometry") - dump(slave("geometry", time).data) - println("master element geometry") - dump(master("geometry", time).data) - error("find projection from master to slave: did not converge") -end - -# Mortar assembly 2d - -# quadratic not tested yet -typealias MortarElements2D Union{Seg2} - -function assemble!{E<:MortarElements2D}(assembly::Assembly, problem::Problem{Mortar}, - slave_element::Element{E}, time::Real) - # for finite deformation we need to use incremental formulation - assemble!(assembly, problem, slave_element, time, Val{problem.properties.formulation}) -end - -""" Assemble 2d mortar contribution. Mortar matrices are assembled at initial -configuration X, so this works for tie contact and small sliding contact. """ -function assemble!{E<:MortarElements2D}(assembly::Assembly, problem::Problem{Mortar}, - slave_element::Element{E}, time::Real, ::Type{Val{:total}}) - - # slave element must have a set of master elements - haskey(slave_element, "master elements") || return props = problem.properties + field_dim = get_unknown_field_dimension(problem) + field_name = get_parent_field_name(problem) + slave_elements = get_slave_elements(problem) - # get dimension and name of PARENT field - field_dim = problem.dimension - field_name = problem.parent_field_name + # 1. calculate nodal normals and tangents for slave element nodes j ∈ S + normals, tangents = calculate_normals(slave_elements, time, Val{1}; + rotate_normals=props.rotate_normals) + update!(slave_elements, "normal", normals) + update!(slave_elements, "tangent", tangents) - slave_dofs = get_gdofs(slave_element, field_dim) - nnodes = size(slave_element, 2) + # 2. loop all slave elements + for slave_element in slave_elements - # slave side quantities: rotation matrix, geometry, displacement, reaction force - Q = slave_element("normal-tangential coordinates", time) - Z = zeros(nnodes, nnodes) - if nnodes == 2 - Q2 = [Q[1] Z; Z Q[2]] - elseif nnodes == 3 - Q2 = [Q[1] Z Z; Z Q[2] Z; Z Z Q[3]] - end - X1 = vec(slave_element("geometry", time)) - u1 = zeros(2*nnodes) - if haskey(slave_element, "displacement") - u1 = vec(slave_element("displacement", time)) - end - x1 = X1 + u1 - la = zeros(2*nnodes) - if haskey(slave_element, "reaction force") - la = vec(slave_element("reaction force", time)) - end - la = Q2'*la + nsl = length(slave_element) + X1 = slave_element("geometry", time) + n1 = slave_element("normal", time) - G = zeros(2*nnodes) - g = zeros(2*nnodes) - local_assembly = Assembly() + # 3. loop all master elements + for master_element in slave_element("master elements", time) - for master_element in slave_element["master elements"] + nm = length(master_element) + X2 = master_element("geometry", time) - X2 = vec(master_element("geometry", time)) - u2 = zeros(2*nnodes) - if haskey(master_element, "displacement") - u2 = vec(master_element("displacement", time)) - end - x2 = X2 + u2 + # 3.1 calculate segmentation + xi1a = project_from_master_to_slave(slave_element, X2[1], time) + xi1b = project_from_master_to_slave(slave_element, X2[2], time) + xi1 = clamp([xi1a; xi1b], -1.0, 1.0) + l = 1/2*abs(xi1[2]-xi1[1]) + isapprox(l, 0.0) && continue # no contribution in this master element - # if distance between elements is "far enough" cannot expect contact - if props.contact && (props.minimum_distance < Inf) - slave_midpoint = Float64[mean(x1[1:field_dim:2]), mean(x1[2:field_dim:2])] - master_midpoint = Float64[mean(x2[1:field_dim:2]), mean(x2[2:field_dim:2])] - if norm(slave_midpoint - master_midpoint) > props.minimum_distance - continue + # 3.2. bi-orthogonal basis + De = zeros(nsl, nsl) + Me = zeros(nsl, nsl) + Ae = zeros(nsl, nsl) + if props.dual_basis + for ip in get_integration_points(slave_element, 3) + detJ = slave_element(ip, time, Val{:detJ}) + w = ip.weight*detJ*l + xi = ip.coords[1] + xi_s = dot([1/2*(1-xi); 1/2*(1+xi)], xi1) + N1 = vec(get_basis(slave_element, xi_s, time)) + De += w*diagm(N1) + Me += w*N1*N1' + end + Ae = De*inv(Me) + else + Ae = eye(nsl) end - end - master_dofs = get_gdofs(master_element, field_dim) - xi1a = project_from_master_to_slave(slave_element, master_element, [-1.0], time) - xi1b = project_from_master_to_slave(slave_element, master_element, [ 1.0], time) - xi1 = clamp([xi1a xi1b], -1.0, 1.0) - l = 1/2*abs(xi1[2]-xi1[1]) - isapprox(l, 0.0) && continue # no contribution - - # Calculate slave side projection matrix D - Ae = zeros(nnodes, nnodes) - De = zeros(nnodes, nnodes) - Me = zeros(nnodes, nnodes) - if problem.properties.dual_basis # Construct dual basis - for ip in get_integration_points(slave_element, Val{5}) - J = get_jacobian(slave_element, ip, time) - w = ip.weight*norm(J)*l - xi = 1/2*(1-ip.xi)*xi1[1] + 1/2*(1+ip.xi)*xi1[2] - N = slave_element(xi, time) - De += w*diagm(vec(N)) - Me += w*N'*N + # 3.3. loop integration points of one integration segment and calculate + # local mortar matrices + fill!(De, 0.0) + fill!(Me, 0.0) + ge = zeros(field_dim*nsl) + for ip in get_integration_points(slave_element, 2) + detJ = slave_element(ip, time, Val{:detJ}) + w = ip.weight*detJ*l + xi = ip.coords[1] + xi_s = dot([1/2*(1-xi); 1/2*(1+xi)], xi1) + N1 = vec(get_basis(slave_element, xi_s, time)) + Phi = Ae*N1 + # project gauss point from slave element to master element in direction n_s + X_s = N1*X1 # coordinate in gauss point + n_s = N1*n1 # normal direction in gauss point + xi_m = project_from_slave_to_master(master_element, X_s, n_s, time) + N2 = vec(get_basis(master_element, xi_m, time)) + X_m = N2*X2 + De += w*Phi*N1' + Me += w*Phi*N2' + if props.adjust + haskey(slave_element, "displacement") || continue + haskey(master_element, "displacement") || continue + norm(mean(X1) - X2[1]) / norm(X1[2] - X1[1]) < props.distval || continue + norm(mean(X1) - X2[2]) / norm(X1[2] - X1[1]) < props.distval || continue + u1 = slave_element("displacement", time) + u2 = master_element("displacement", time) + x_s = X_s + N1*u1 + x_m = X_m + N2*u2 + ge += w*vec((x_m-x_s)*Phi') + end end - Ae = De*inv(Me) - else # Standard Lagrange basis - for ip in get_integration_points(slave_element, Val{5}) - J = get_jacobian(slave_element, ip, time) - w = ip.weight*norm(J)*l - xi = 1/2*(1-ip.xi)*xi1[1] + 1/2*(1+ip.xi)*xi1[2] - N = slave_element(xi, time) - De += w*N'*N - end - Ae = eye(nnodes) - end - C1S2 = zeros(2*nnodes, 2*nnodes) - C1M2 = zeros(2*nnodes, 2*nnodes) - - # Slave side already done; it's De - for i=1:field_dim - C1S2[i:field_dim:end,i:field_dim:end] += De - end - - # Calculate master side projection matrix M - for ip in get_integration_points(slave_element, Val{5}) - J = get_jacobian(slave_element, ip, time) - w = ip.weight*norm(J)*l - # integration point on slave side segment - xi_slave = 1/2*(1-ip.xi)*xi1[1] + 1/2*(1+ip.xi)*xi1[2] - # projected integration point to master side element - xi_master = project_from_slave_to_master(slave_element, master_element, xi_slave, time) - N1 = slave_element(xi_slave, time) - N2 = master_element(xi_master, time) - M = w*kron(Ae*N1', N2) + # add contribution to contact virtual work + sdofs = get_gdofs(problem, slave_element) + mdofs = get_gdofs(problem, master_element) + for i=1:field_dim - C1M2[i:field_dim:end,i:field_dim:end] += M + lsdofs = sdofs[i:field_dim:end] + lmdofs = mdofs[i:field_dim:end] + add!(problem.assembly.C1, lsdofs, lsdofs, De) + add!(problem.assembly.C1, lsdofs, lmdofs, -Me) + add!(problem.assembly.C2, lsdofs, lsdofs, De) + add!(problem.assembly.C2, lsdofs, lmdofs, -Me) end - end + add!(problem.assembly.g, sdofs, ge) - # Calculate normal-tangential constraints and weighted gap - C2S2 = Q2'*C1S2 - C2M2 = Q2'*C1M2 - G += -(C2S2*X1 - C2M2*X2) - g += -(C2S2*x1 - C2M2*x2) + end # master elements done - # Add contributions - add!(local_assembly.C1, slave_dofs, slave_dofs, C1S2) - add!(local_assembly.C1, slave_dofs, master_dofs, -C1M2) - add!(local_assembly.C2, slave_dofs, slave_dofs, C2S2) - add!(local_assembly.C2, slave_dofs, master_dofs, -C2M2) - - end # all master elements are done - - # if only equality constraints, i.e., mesh tying problem, we're done for this element. - if !props.contact - append!(assembly, local_assembly) - return - end - - add!(local_assembly.g, slave_dofs, G) - - lan = la[1:field_dim:end] - lat = la[2:field_dim:end] - gn = g[1:field_dim:end] - gt = g[2:field_dim:end] - - # normal condition - cn = 1.0 # complemementarity parameter - Cn = lan - max(0, lan - cn*gn) - inactive_nodes = find(lan - cn*gn .<= 0) - active_nodes = find(lan - cn*gn .> 0) - - # if all nodes inactive, nothing to contribute. - if length(active_nodes) == 0 - return - end - - # manipulate local assembly (remove rows from it based on active set) - # before adding it to global assembly - C1 = sparse(local_assembly.C1) - C2 = sparse(local_assembly.C2) - D = spzeros(size(C2)...) - g = sparse(local_assembly.g) - - node_ids = get_connectivity(slave_element) - - # normal constraint: remove inactive nodes - for j in node_ids[inactive_nodes] - if length(props.always_in_contact) != 0 - j in props.always_in_contact && continue - end - gdofs = [2*(j-1)+1, 2*(j-1)+2] - # λⱼ = 0 ∀ j ∈ S - C1[gdofs,:] = 0 - C2[gdofs,:] = 0 - D[gdofs,:] = 0 - g[gdofs,:] = 0 - end - - for (i, j) in enumerate(node_ids[active_nodes]) - gdofs = [2*(j-1)+1, 2*(j-1)+2] - #D[gdofs[2],gdofs] = C2[gdofs[2],gdofs] - D[gdofs[2],gdofs] = Q[i][:,2] - C2[gdofs[2],:] = 0 - g[gdofs[2],:] = 0 - end - - local_assembly.C1 = C1 - local_assembly.C2 = C2 - local_assembly.D = D - local_assembly.g = g - append!(assembly, local_assembly) - - if props.store_debug_info - slave_element["g"] = g - slave_element["c"] = c - slave_element["C1"] = C1 - slave_element["C2"] = C2 - slave_element["D"] = D - slave_element["active nodes"] = active_nodes - end + end # slave elements done, contact virtual work ready end -function assemble!{E<:MortarElements2D}(assembly::Assembly, problem::Problem{Mortar}, - slave_element::Element{E}, time::Real, ::Type{Val{:incremental}}) - - # slave element must have a set of master elements - haskey(slave_element, "master elements") || return - props = problem.properties - - # get dimension and name of PARENT field - field_dim = problem.dimension - field_name = problem.parent_field_name - - slave_dofs = get_gdofs(slave_element, field_dim) - nnodes = size(slave_element, 2) - - # slave side quantities: rotation matrix, geometry, displacement, reaction force - Q = slave_element("normal-tangential coordinates", time) - Z = zeros(nnodes, nnodes) - if nnodes == 2 - Q2 = [Q[1] Z; Z Q[2]] - elseif nnodes == 3 - Q2 = [Q[1] Z Z; Z Q[2] Z; Z Z Q[3]] - end - X1 = vec(slave_element("geometry", time)) - u1 = zeros(2*nnodes) - if haskey(slave_element, "displacement") - u1 = vec(slave_element("displacement", time)) - end - x1 = X1 + u1 - la = zeros(2*nnodes) - if haskey(slave_element, "reaction force") - la = vec(slave_element("reaction force", time)) - end - la = Q2'*la - - G = zeros(2*nnodes) - g = zeros(2*nnodes) - local_assembly = Assembly() - - has_contribution = false - - for master_element in slave_element["master elements"] - - X2 = vec(master_element("geometry", time)) - u2 = zeros(2*nnodes) - if haskey(master_element, "displacement") - u2 = vec(master_element("displacement", time)) - end - x2 = X2 + u2 - - # if distance between elements is "far enough" cannot expect contact - if props.contact && (props.minimum_distance < Inf) - slave_midpoint = Float64[mean(x1[1:field_dim:2]), mean(x1[2:field_dim:2])] - master_midpoint = Float64[mean(x2[1:field_dim:2]), mean(x2[2:field_dim:2])] - if norm(slave_midpoint - master_midpoint) > props.minimum_distance - continue - end - end - - master_dofs = get_gdofs(master_element, field_dim) - xi1a = project_from_master_to_slave(slave_element, master_element, [-1.0], time, Val{:deformed}) - xi1b = project_from_master_to_slave(slave_element, master_element, [ 1.0], time, Val{:deformed}) - xi1 = clamp([xi1a xi1b], -1.0, 1.0) - l = 1/2*abs(xi1[2]-xi1[1]) - isapprox(l, 0.0) && continue # no contribution - - # Calculate slave side projection matrix D - Ae = zeros(nnodes, nnodes) - De = zeros(nnodes, nnodes) - Me = zeros(nnodes, nnodes) - if problem.properties.dual_basis # Construct dual basis - for ip in get_integration_points(slave_element, Val{5}) - J = get_jacobian(slave_element, ip, time, Val{:deformed}) - w = ip.weight*norm(J)*l - xi = 1/2*(1-ip.xi)*xi1[1] + 1/2*(1+ip.xi)*xi1[2] - N = slave_element(xi, time) - De += w*diagm(vec(N)) - Me += w*N'*N - end - Ae = De*inv(Me) - else # Standard Lagrange basis - for ip in get_integration_points(slave_element, Val{5}) - J = get_jacobian(slave_element, ip, time, Val{:deformed}) - w = ip.weight*norm(J)*l - xi = 1/2*(1-ip.xi)*xi1[1] + 1/2*(1+ip.xi)*xi1[2] - N = slave_element(xi, time) - De += w*N'*N - end - Ae = eye(nnodes) - end - - C1S2 = zeros(2*nnodes, 2*nnodes) - C1M2 = zeros(2*nnodes, 2*nnodes) - - # Slave side already done; it's De - for i=1:field_dim - C1S2[i:field_dim:end,i:field_dim:end] += De - end - - # Calculate master side projection matrix M - for ip in get_integration_points(slave_element, Val{5}) - J = get_jacobian(slave_element, ip, time, Val{:deformed}) - w = ip.weight*norm(J)*l - # integration point on slave side segment - xi_slave = 1/2*(1-ip.xi)*xi1[1] + 1/2*(1+ip.xi)*xi1[2] - # projected integration point to master side element - xi_master = project_from_slave_to_master(slave_element, master_element, - xi_slave, time, Val{:deformed}) - N1 = slave_element(xi_slave, time) - N2 = master_element(xi_master, time) - M = w*kron(Ae*N1', N2) - for i=1:field_dim - C1M2[i:field_dim:end,i:field_dim:end] += M - end - end - - # Calculate normal-tangential constraints and weighted gap - C2S2 = Q2'*C1S2 - C2M2 = Q2'*C1M2 - G += props.gap_sign*(C2S2*X1 - C2M2*X2) - g += props.gap_sign*(C2S2*x1 - C2M2*x2) - - # Add contributions - add!(local_assembly.C1, slave_dofs, slave_dofs, C1S2) - add!(local_assembly.C1, slave_dofs, master_dofs, -C1M2) - add!(local_assembly.C2, slave_dofs, slave_dofs, C2S2) - add!(local_assembly.C2, slave_dofs, master_dofs, -C2M2) - has_contribution = true - - end # all master elements are done - - if !has_contribution - return - end - - add!(local_assembly.g, slave_dofs, g) - - # if only equality constraints, i.e., mesh tying problem, we're done for this element. - if !props.contact - append!(assembly, local_assembly) - return - end - - lan = la[1:field_dim:end] - lat = la[2:field_dim:end] - gn = g[1:field_dim:end] - gt = g[2:field_dim:end] - - # normal condition - cn = 1.0 # complemementarity parameter - Cn = lan - max(0, lan - cn*gn) - inactive_nodes = find(lan - cn*gn .<= 0) - active_nodes = find(lan - cn*gn .> 0) - - # if all nodes inactive, nothing to contribute. - if length(active_nodes) == 0 - return - end - - # manipulate local assembly (remove rows from it based on active set) - # before adding it to global assembly - C1 = sparse(local_assembly.C1) - C2 = sparse(local_assembly.C2) - D = spzeros(size(C2)...) - g = sparse(local_assembly.g) - - node_ids = get_connectivity(slave_element) - - # normal constraint: remove inactive nodes - for j in node_ids[inactive_nodes] - if length(props.always_in_contact) != 0 - j in props.always_in_contact && continue - end - gdofs = [2*(j-1)+1, 2*(j-1)+2] - # λⱼ = 0 ∀ j ∈ S - C1[gdofs,:] = 0 - C2[gdofs,:] = 0 - D[gdofs,:] = 0 - g[gdofs,:] = 0 - end - - for (i, j) in enumerate(node_ids[active_nodes]) - gdofs = [2*(j-1)+1, 2*(j-1)+2] - #D[gdofs[2],gdofs] = C2[gdofs[2],gdofs] - D[gdofs[2],gdofs] = Q[i][:,2] - C2[gdofs[2],:] = 0 - g[gdofs[2],:] = 0 - end - - local_assembly.C1 = C1 - local_assembly.C2 = C2 - local_assembly.D = D - local_assembly.g = g - append!(assembly, local_assembly) - - if props.store_debug_info - slave_element["g"] = g - slave_element["c"] = c - slave_element["C1"] = C1 - slave_element["C2"] = C2 - slave_element["D"] = D - slave_element["active nodes"] = active_nodes - end - -end - -function calculate_gap_vector{E<:MortarElements2D}( - problem::Problem{Mortar}, slave_element::Element{E}, - time::Real) - - # slave element must have a set of master elements - haskey(slave_element, "master elements") || return - props = problem.properties - - # get dimension and name of PARENT field - field_dim = problem.dimension - field_name = problem.parent_field_name - - nnodes = size(slave_element, 2) - gap = zeros(2*nnodes) - - for master_element in slave_element["master elements"] - - xi1a = project_from_master_to_slave(slave_element, master_element, [-1.0], time, Val{:deformed}) - xi1b = project_from_master_to_slave(slave_element, master_element, [ 1.0], time, Val{:deformed}) - xi1 = clamp([xi1a xi1b], -1.0, 1.0) - l = 1/2*abs(xi1[2]-xi1[1]) - isapprox(l, 0.0) && continue # no contribution - - # Calculate biorthogonal basis - Ae = zeros(nnodes, nnodes) - De = zeros(nnodes, nnodes) - Me = zeros(nnodes, nnodes) - for ip in get_integration_points(slave_element, Val{5}) - J = get_jacobian(slave_element, ip, time, Val{:deformed}) - w = ip.weight*norm(J)*l - xi = 1/2*(1-ip.xi)*xi1[1] + 1/2*(1+ip.xi)*xi1[2] - N = slave_element(xi, time) - De += w*diagm(vec(N)) - Me += w*N'*N - end - Ae = De*inv(Me) - - # Calculate weighted gap - for ip in get_integration_points(slave_element, Val{5}) - J = get_jacobian(slave_element, ip, time, Val{:deformed}) - w = ip.weight*norm(J)*l - # integration point on slave side segment - xi_slave = 1/2*(1-ip.xi)*xi1[1] + 1/2*(1+ip.xi)*xi1[2] - # projected integration point to master side element - xi_master = project_from_slave_to_master(slave_element, master_element, xi_slave, time, Val{:deformed}) - X1 = slave_element("geometry", xi_slave, time) - u1 = zeros(2*nnodes) - if haskey(slave_element, "displacement") - u1 = slave_element("displacement", xi_slave, time) - end - x1 = X1 + u1 - X2 = master_element("geometry", xi_master, time) - u2 = zeros(2*nnodes) - if haskey(master_element, "displacement") - u2 = master_element("displacement", xi_master, time) - end - x2 = X2 + u2 - Q = slave_element("normal-tangential coordinates", xi_slave, time) - g = -Q'*(x1-x2) - N1 = slave_element(xi_slave, time) - Phi = vec(Ae*N1') - gap[1:field_dim:end] += w*g[1]*Phi - gap[2:field_dim:end] += w*g[2]*Phi - end - - end # all master elements are done - - return gap - -end diff --git a/src/problems_mortar_3d.jl b/src/problems_mortar_3d.jl index 58591a9..7499ed0 100644 --- a/src/problems_mortar_3d.jl +++ b/src/problems_mortar_3d.jl @@ -1,672 +1,347 @@ # This file is a part of JuliaFEM. # License is MIT: see https://github.com/JuliaFEM/JuliaFEM.jl/blob/master/LICENSE.md -# Mortar projection calculation for 3d cases +typealias MortarElements3D Union{Tri3, Tri6, Quad4} -""" Construct auxiliary plane for surface. """ -function create_auxiliary_plane{E}(element::Element{E}, time::Real) - xi = get_reference_element_midpoint(E) - x0 = element("geometry", xi, time) - ntbasis = element("normal-tangential coordinates", xi, time) - return x0, ntbasis +function project_vertex_to_auxiliary_plane(p::Vector, x0::Vector, n0::Vector) + return p - dot(p-x0, n0)*n0 end +function inv3(P::Matrix) + n, m = size(P) + @assert n == m == 3 + a, b, c, d, e, f, g, h, i = P + A = e*i - f*h + B = -d*i + f*g + C = d*h - e*g + D = -b*i + c*h + E = a*i - c*g + F = -a*h + b*g + G = b*f - c*e + H = -a*f + c*d + I = a*e - b*d + return 1/(a*A + b*B + c*C)*[A B C; D E F; G H I] +end -""" -Project point q onto a plane given by a point p and normal n. - -Parameters ----------- -q::Array{Float64, 2} - point to project (row vector) -x0::Array{Float64, 2} - origo of plane -n::Array{Float64, 2} - normal vector of plane - -Returns -------- -y::Array{Float64, 2} - projected point - -Examples --------- -julia> p = [-0.5 -1.0 4.0]' -julia> x0 = [0.0 0.075 0.675]' -julia> n = [0.1485860 0.0784519 0.9857830]' -julia> project_node_to_auxiliary_plane(p, x0, n) -3-element Array{Float64,1}: - 0.963455 - -1.2447 - 0.925247 - -Notes ------ -[1](http://stackoverflow.com/questions/8942950/how-do-i-find-the-orthogonal-projection-of-a-point-onto-a-plane) - -""" -function project_vertex_to_auxiliary_plane(p::Vector, x0::Vector, Q::Matrix) - n = Q[:,1] - ph = p - dot(p-x0, n)*n - qproj = Q'*(ph-x0) - if abs(qproj[1]) > 1.0e-2 - # we should have something very little for normal direction if projected - # properly - info("project_point_to_auxiliary_plane(): vertex not projected correctly.") - info("p: $(ForwardDiff.get_value(p))") - info("x0: $(ForwardDiff.get_value(x0))") - info("Q: \n$(ForwardDiff.get_value(Q))") - info("qproj: $(ForwardDiff.get_value(qproj))") - error("Failed to project vertex to auxiliary plane.") +function vertex_inside_polygon(q, P; atol=1.0e-6) + N = length(P) + angle = 0.0 + for i=1:N + A = P[i] - q + B = P[mod(i,N)+1] - q + c = norm(A)*norm(B) + isapprox(c, 0.0; atol=atol) && return true + cosa = dot(A,B)/c + isapprox(cosa, 1.0; atol=atol) && return false + isapprox(cosa, -1.0; atol=atol) && return true + try + angle += acos(cosa) + catch + info("Unable to calculate acos($(ForwardDiff.get_value(cosa))) when determining is a vertex inside polygon.") + info("Polygon is: $(ForwardDiff.get_value(P)) and vertex under consideration is $(ForwardDiff.get_value(q))") + info("Polygon corner point in loop: A=$(ForwardDiff.get_value(A)), B=$(ForwardDiff.get_value(B))") + info("c = ||A||*||B|| = $(ForwardDiff.get_value(c))") + rethrow() + end end - return qproj[2:3] + return isapprox(angle, 2*pi; atol=atol) end -project_point_to_auxiliary_plane = project_vertex_to_auxiliary_plane +function calculate_centroid(P) + N = length(P) + P0 = P[1] + areas = [norm(1/2*cross(P[i]-P0, P[mod(i,N)+1]-P0)) for i=2:N] + centroids = [1/3*(P0+P[i]+P[mod(i,N)+1]) for i=2:N] + C = 1/sum(areas)*sum(areas.*centroids) + return C +end -""" -Find edge intersections of two planar arbitrary shape polygons. +function get_cells(P, C) + N = length(P) + cells = Vector[] + # shared edge etc. + N < 3 && return cells + # trivial case, polygon already triangle / quadrangle + #N == 3 && return Vector[P] + #N == 4 && return Vector[P] + #V = sum([cross(P[i], P[mod(i,N)+1]) for i=1:N]) + #A = 1/2*abs(dot(n, V)) + #info("A = $A") + cells = Vector[Vector[C, P[i], P[mod(i,N)+1]] for i=1:N] + return cells -Parameters ----------- -S::Array{Float64,2} -M::Array{Float64,2} + maxa = 0.0 + maxj = 0 + for i=1:N + A = P[i] - C + B = P[mod(i,N)+1] - C + theta = acos(dot(A,B)/(norm(A)*norm(B))) + if theta > maxa + maxa = theta + maxj = i + end + end + info("max angle $(maxa/pi*180) at index $maxj, N=$N") + indices = mod(collect(maxj:maxj+N), N) + info("indices = $indices") +end -Matrices with size (2, n) where n is number of vertices of each polygon. +function get_polygon_clip(xs, xm, n; debug=false) + # objective: search does line xm1 - xm2 clip xs + nm = length(xm) + ns = length(xs) + P = Vector{Float64}[] -Returns -------- -P::Array{Float64,2} - Intersection points of polygons -n::Array{Float64,2} - Neighbour info matrix with size (ns, mn). This keeps information which - edges of polygons are intersecting. See further explanation in example - below. + # 1. test is master point inside slave, if yes, add to clip + for i=1:nm + if vertex_inside_polygon(xm[i], xs) + debug && info("1. $(xm[i]) inside S -> push") + push!(P, xm[i]) + end + end -Examples --------- -Find intersection points of two triangles: - -julia> S = [0 0; 3 0; 0 3]' -julia> M = [-1 1; 2 -1/2; 1 3/2]' -julia> P, n = get_edge_intersections(S, M) -julia> P -2x4 Array{Float64,2}: - 1.0 1.75 0.0 0.0 - 0.0 0.0 0.5 1.25 -julia> n -3x3 Array{Int64,2}: - 1 1 0 - 0 0 0 - 1 0 1) - -So intersection points are: (1.00, 0.00), (1.75, 0.00), (0.00, 0.50), (0.00, 1.25). -"Neighbour matrix" can be interpreted as following: - - 1 1 0 <--> First edge of S intersects edges 1 and 2 of M - 0 0 0 <--> Second edge of S doesn't intersect at all - 1 0 1 <--> Third edge of S intersects with edges 1 and 3 of M - -""" -function get_edge_intersections(S::Matrix, M::Matrix) - ns = size(S, 2) - nm = size(M, 2) - P = zeros(2, 0) - n = zeros(Int64, ns, nm) - k = 0 + # 2. test is slave point inside master, if yes, add to clip for i=1:ns - for j=1:nm - b = M[:,j]-S[:,i] - A = [S[:,mod(i,ns)+1]-S[:,i] -M[:,mod(j,nm)+1]+M[:,j]] - if rank(ForwardDiff.get_value(A)) == 2 - r = A\b - if (r[1]>=0) & (r[1]<=1) & (r[2]>=0) & (r[2]<=1) # intersection found - k += 1 - f = S[:,i]+r[1]*(S[:,mod(i,ns)+1] - S[:,i]) - f = f'' - P = hcat(P, f) - n[i, j] = 1 - end + if vertex_inside_polygon(xs[i], xm) + xs[i] in P && continue + debug && info("2. $(xs[i]) inside M -> push") + push!(P, xs[i]) + end + end + + for i=1:nm + # 2. find possible intersection + xm1 = xm[i] + xm2 = xm[mod(i,nm)+1] + #info("intersecting line $xm1 -> $xm2") + for j=1:ns + xs1 = xs[j] + xs2 = xs[mod(j,ns)+1] + #info("clipping polygon edge $xs1 -> $xs2") + tnom = dot(cross(xm1-xs1, xm2-xm1), n) + tdenom = dot(cross(xs2-xs1, xm2-xm1), n) + isapprox(tdenom, 0) && continue + t = tnom/tdenom + (0 <= t <= 1) || continue + q = xs1 + t*(xs2 - xs1) + #info("t=$t, q=$q, q ∈ xm ? $(vertex_inside_polygon(q, xm))") + if vertex_inside_polygon(q, xm) + q in P && continue + debug && info("3. $q inside M -> push") + push!(P, q) end end end - return P, n -end - -""" -Find any points laying inside or border of triangle. - -Parameters ----------- -Y::Array{Float64, 2} - Triangle coordinates in 2×3 matrix -X::Array{Float64, 2} - List of points to test in 2×n matrix - -Returns -------- -P::Array{Float64, 2} - List of points in triangle in 2×m matrix, where m is number of points inside triangle - -Examples --------- -julia> S = [0.0 0.0; 3.0 0.0; 0.0 3.0]' # triangle corner points -julia> pts = [-1.0 1.0; 2.0 -0.5; 1.0 1.5; 0.5 1.5]' # points to tests -julia> points_in_triangle(S, pts) -2x2 Array{Float64,2}: - 1.0 0.5 - 1.5 1.5 -""" -function get_points_inside_triangle(Y::Matrix, X::Matrix) - @assert size(Y, 2) == 3 # "Point in TRIANGLE..." - P = zeros(2, 0) - v0 = Y[:,2] - Y[:,1] - v1 = Y[:,3] - Y[:,1] # find interior points of X in Y - d00 = (v0'*v0)[1] - d01 = (v0'*v1)[1] - d11 = (v1'*v1)[1] # using baricentric coordinates - id = 1/(d00*d11 - d01*d01) - for i=1:size(X, 2) - v2 = X[:,i] - Y[:,1] - d02 = (v0'*v2)[1] - d12 = (v1'*v2)[1] - u = (d11*d02-d01*d12)*id - v = (d00*d12-d01*d02)*id - if (u>=0) & (v>=0) & (u+v<=1) # also include nodes on the boundary - P = hcat(P, X[:,i]'') - end - end return P end -""" -Determine is point P inside or on boudary of polygon X. - -http://paulbourke.net/geometry/polygonmesh/#insidepoly -""" -function is_point_inside_convex_polygon(P, X) - x, y = P - for i=1:length(X) - x0, y0 = X[i] - x1, y1 = X[mod(i, length(X))+1] - if (y-y0)*(x1-x0) - (x-x0)*(y1-y0) < 0 - return false - end - end - return true -end - -function get_points_inside_convex_polygon(pts, X) - # TODO: Make more readable - X2 = [X[:,i] for i=1:size(X,2)] - c = filter(P->is_point_inside_convex_polygon(P, X2), [pts[:,i] for i=1:size(pts, 2)]) - return length(c) == 0 ? zeros(2, 0) : hcat(c...) -end - -""" Return unique objects with some given tolerance. This is used in next function - because traditional unique() command returns row vectors as non-unique if they - differs only a "little". -""" -function uniquetol(P, dim::Int; args...) - @assert dim == 2 - items = Vector[P[:,i] for i=1:size(P,dim)] - new_items = Vector[] - for item in items - has_found = false - for new_item in new_items - if isapprox(ForwardDiff.get_value(item), ForwardDiff.get_value(new_item); args...) - has_found = true - break - end - end - if !has_found - push!(new_items, item) - end - end - return reshape([new_items...;], length(new_items[]), length(new_items)) -end - - -""" -Make polygon clipping of shapes S and M. - -Parameters ----------- -S::Array{Float64, 2} -M::Array{Float64, 2} - Shapes to clip. Needs to be triangles at the moment. - -Returns -------- -Array{Float64, 2}, Array{Float64, 2} -- Polygon vertices in 2×n matrix, sorted in counter-clockwise order. -- 3×3 "neighbouring" matrix, see example. - -Examples --------- -julia> S = [0 0; 3 0; 0 3]' -julia> M = [-1 1; 2 -1/2; 2 2]' -julia> P, n = clip_polygon(S, M) -julia> P -2x6 Array{Float64,2}: - 0.0 1.0 2.0 2.0 1.25 0.0 - 0.5 0.0 0.0 1.0 1.75 1.33333, -julia> n -3x3 Array{Int64,2}: - 1 0 1 <- first edge of M ([-1 1; 2 -1/2]') intersects with edges 1 and 3 of S ([0 0; 3 0]' and [0 3; 0 0]') - 1 1 0 <- second edge of M ([2 -1/2; 2 2]') intersects with edges 1 and 2 of S - 0 1 1 <- third edge of M ([2 2; -1 1]') intersects with edgse 2 and 3 of S - -""" -function clip_polygon(S::Matrix, M::Matrix) - P1, neighbours = get_edge_intersections(M, S) - #P2 = get_points_inside_triangle(M, S) - #P3 = get_points_inside_triangle(S, M) - P2 = get_points_inside_convex_polygon(M, S) - P3 = get_points_inside_convex_polygon(S, M) -# info("polygon clipping: P1 = $P1") -# info("polygon clipping: P2 = $P2") -# info("polygon clipping: P3 = $P3") -# info("hcat P = $P") - P = hcat(P1, P2, P3) - if length(P) == 0 - return nothing, nothing - end - P = uniquetol(P, 2) - meanval = mean(P, 2) - tmp = P .- meanval - angles = atan2(tmp[2,:], tmp[1,:]) - angles = reshape(angles, length(angles)) - order = sortperm(angles) - return P[:, order], neighbours -end - - -""" -Calculate polygon geometric center point - -Parameters ----------- -P::Array{Float64, 2} - Polygon vertices in 2×n matrix - -Returns -------- -Array{Float63, 2} - Center point - -Examples --------- -julia> P -2x6 Array{Float64,2}: - 0.0 1.0 2.0 2.0 1.25 0.0 - 0.5 0.0 0.0 1.0 1.75 1.33333, -julia> C = get_polygon_cp(P) -2x1 Array{Float64,2}: - 1.039740 - 0.804701 - -""" -function calculate_polygon_centerpoint(P::Matrix) - n = size(P, 2) - A = 0.0 - for i=1:n - A += 1/2*(P[1,i]*P[2,mod(i,n)+1] - P[1,mod(i,n)+1]*P[2,i]) - end - Cx = 0.0 - Cy = 0.0 - for i=1:n - inext = mod(i, n)+1 - Cx += 1/(6*A)*(P[1,i] + P[1,inext])*(P[1,i]*P[2,inext] - P[1,inext]*P[2,i]) - Cy += 1/(6*A)*(P[2,i] + P[2,inext])*(P[1,i]*P[2,inext] - P[1,inext]*P[2,i]) - end - return [Cx, Cy] -end - -""" -Project point from auxiliary plane to parametric surface given by (ξ₁, ξ₂) - -Parameters ----------- -p::Array{Float64,1} - point in auxiliary plane, in (n,t1,t2) coordinate system -x0::Array{Float64,1} - origo of auxiliary plane cs -Q::Array{Float64,2} - basis of auxiliary plane cs -x::Array{Float64,2} - surface node coords -basis::Array{Float64,2} - surface basis functions -dbasis::Array{Float64,2} - partial derivatives of surface basis functions - -Returns -------- -Array{Float64,2} - solution vector (d, ξ₁, ξ₂) where d is distance to surface - -Examples --------- -Define surface with node points, basis + dbasis - -julia> xquad = [ -... -2.5 -2.0 1.0 -... 2.5 -2.0 0.7 -... 2.0 2.3 0.0 -... -2.0 2.0 1.0]' -julia> basis(xi) = [ -... (1-xi[1])(1-xi[2])/4 -... (1+xi[1])(1-xi[2])/4 -... (1+xi[1])(1+xi[2])/4 -... (1-xi[1])(1+xi[2])/4] -julia> dbasis(xi) = [ -... -(1-xi[2])/4 -(1-xi[1])/4 -... (1-xi[2])/4 -(1+xi[1])/4 -... (1+xi[2])/4 (1+xi[1])/4 -... -(1+xi[2])/4 (1-xi[1])/4] - -We aim to find point p, which we first project to auxiliary plane defined as following -julia> p = [-2.5 -2.0 1.0]' -julia> x0 = [0.0 0.075 0.675]' -julia> Q = [ -... 0.1485860 0.9888990 0.0000000 -... 0.0784519 -0.0117877 0.9968480 -... 0.9857830 -0.1481180 -0.0793325] - -Our projected point is therefore -julia> n = Q[:,1] # first component is normal direction -julia> ph = project_node_to_auxiliary_plane(p, x0, n) -julia> ph = Q'(ph-x0) -julia> ph -3x1 Array{Float64,2}: - 1.33264e-7 - -2.49593 - -2.09424 - -Our point ph is now in auxiliary plane in n,t1,t2 coordinate system. Next we -project it back to surface defined by xquad*basis - -julia> theta = project_point_from_plane_to_surface(ph, x0, Q, xquad, basis, dbasis) -julia> theta -3x1 Array{Float64,2}: - -0.213874 - -0.999999 - -1.0 - -We see that our ξ₁ = ξ₂ = -1 so we found first point of xquad -[-2.5 -2.0 1.0]' correctly. - -julia> xquad*basis(theta[2:3]) -3-element Array{Float64,1}: - -2.5 - -2.0 - 1.0 - -""" -function project_point_from_plane_to_surface{E}(p::Vector, x0::Vector, Q::Matrix, - element::Element{E}, time::Real; max_iterations::Int=10, iter_tol::Float64=1.0e-9) - x = element("geometry", time) - return project_point_from_plane_to_surface(p, x0, Q, element, x, time; - max_iterations=max_iterations, iter_tol=iter_tol) -end -function project_vertex_from_plane_to_surface{E}(p::Vector, x0::Vector, Q::Matrix, - element::Element{E}, x, time::Real; max_iterations::Int=10, iter_tol::Float64=1.0e-9) - basis(xi) = get_basis(E, xi) - dbasis(xi) = get_dbasis(E, xi) - ph = Q*[0; p] + x0 - n = Q[:,1] - b(theta) = ph + theta[1]*n - basis(theta[2:3])*x - J(theta) = [n -dbasis(theta[2:3])*x] +function project_vertex_to_surface{E}(p::Vector, x0::Vector, n0::Vector, + element::Element{E}, x::DVTI, time::Real; max_iterations::Int=10, iter_tol::Float64=1.0e-9) + basis(xi) = get_basis(element, xi, time) + dbasis(xi) = get_dbasis(element, xi, time) + f(theta) = basis(theta[1:2])*x - theta[3]*n0 - p + L(theta) = inv3([dbasis(theta[1:2])*x -n0]) +# L2(theta) = inv(ForwardDiff.get_value([dbasis(theta[2:3])*x -n0])) + # FIXME: for some reason forwarddiff gives NaN's here. theta = zeros(3) dtheta = zeros(3) for i=1:max_iterations - # FIXME: gives NaN if partials in J - dtheta = ForwardDiff.get_value(J(theta)) \ -b(theta) - theta += dtheta - if norm(ForwardDiff.get_value(dtheta)) < iter_tol - return theta + dtheta = L(theta) * f(theta) + theta -= dtheta + if norm(dtheta) < iter_tol + return theta[1:2], theta[3] end end + info("failed to project vertex from auxiliary plane back to surface") info("element type: $E") info("element connectivity: $(get_connectivity(element))") - info("auxiliary plane: x0 = $(ForwardDiff.get_value(x0)), Q = $(ForwardDiff.get_value(Q))") - info("point coordinates on plane: $(ForwardDiff.get_value(p))") - info("element geometry: $(ForwardDiff.get_value(x.data))") - info("ph: $(ForwardDiff.get_value(ph))") - info("normal direction: $(ForwardDiff.get_value(n))") - info("parameter vector before giving up: $(ForwardDiff.get_value(theta))") - info("increment in parameter vector before giving up: $(ForwardDiff.get_value(dtheta))") - info("b([0.0, 0.0, 0.0]) = $(ForwardDiff.get_value(b([0.0, 0.0, 0.0])))") - info("J([0.0, 0.0, 0.0]) = $(ForwardDiff.get_value(J([0.0, 0.0, 0.0])))") + info("auxiliary plane: x0 = $x0, n0 = $n0") + info("element geometry: $(x.data)") + info("vertex to project: $p") + info("parameter vector before giving up: $theta") + info("increment in parameter vector before giving up: $dtheta") + info("norm(dtheta) before giving up: $(norm(dtheta))") + info("f([0.0, 0.0, 0.0]) = $(f([0.0, 0.0, 0.0]))") + info("L([0.0, 0.0, 0.0]) = $(L([0.0, 0.0, 0.0]))") - info("iterations were") + info("iterations:") theta = zeros(3) dtheta = zeros(3) for i=1:max_iterations - info("iter $i, theta = $(ForwardDiff.get_value(theta))") - info("b = $(ForwardDiff.get_value(b(theta)))") - info("J = $(ForwardDiff.get_value(J(theta)))") - dtheta = ForwardDiff.get_value(J(theta)) \ -b(theta) - info("dtheta = $(ForwardDiff.get_value(dtheta))") - theta += dtheta - if norm(dtheta) < iter_tol - return theta - end + info("iter $i, theta = $theta") + info("f = $(f(theta))") + info("L = $(L(theta))") + dtheta = L(theta) * f(theta) + info("dtheta = $(dtheta)") + theta -= dtheta end error("project_point_to_surface: did not converge in $max_iterations iterations!") end -typealias MortarElements3D Union{Tri3, Quad4} - -function assemble!{E<:MortarElements3D}(assembly::Assembly, problem::Problem{Mortar}, - slave_element::Element{E}, time::Real, ::Type{Val{:total}}) - assemble!(assembly, problem, slave_element, time, Val{problem.properties.formulation}) +function calculate_normals(elements, time, ::Type{Val{2}}; rotate_normals=false) + normals = Dict{Int64, Vector{Float64}}() + for element in elements + conn = get_connectivity(element) + J = transpose(element([0.0, 0.0], time, Val{:Jacobian})) + normal = cross(J[:,1], J[:,2]) + for nid in conn + if haskey(normals, nid) + normals[nid] += normal + else + normals[nid] = normal + end + end + end + # normalize to unit normal + S = collect(keys(normals)) + for j in S + normals[j] /= norm(normals[j]) + end + if rotate_normals + for j in S + normals[j] = -normals[j] + end + end + return normals end -function assemble!{E<:MortarElements3D}(assembly::Assembly, problem::Problem{Mortar}, - slave_element::Element{E}, time::Real, ::Type{Val{:total}}) - haskey(slave_element, "master elements") || return - field_dim = get_unknown_field_dimension(problem) - field_name = get_parent_field_name(problem) - slave_dofs = get_gdofs(slave_element, field_dim) +function check_orientation!(P, n; debug=false) + C = mean(P) + np = length(P) + s = [dot(n, cross(P[i]-C, P[mod(i+1,np)+1]-C)) for i=1:np] + all(s .< 0) && return + debug && info("polygon not in ccw order, fixing") + # project points to new orthogonal basis Q and sort there + t1 = (P[1]-C)/norm(P[1]-C) + t2 = cross(n, t1) + Q = [n t1 t2] + sort!(P, lt=(A, B) -> begin + A_proj = Q'*(A-C) + B_proj = Q'*(B-C) + a = atan2(A_proj[3], A_proj[2]) + b = atan2(B_proj[3], B_proj[2]) + return a > b + end) +end + +function assemble!(problem::Problem{Mortar}, time::Real, ::Type{Val{2}}, ::Type{Val{false}}; debug=true) props = problem.properties - if props.formulation == :Standard && props.normal_condition == :Contact - error("for contact choose Dual formulation.""") - end + field_dim = get_unknown_field_dimension(problem) + field_name = get_parent_field_name(problem) + slave_elements = get_slave_elements(problem) + area = 0.0 - # create auxiliary plane and project slave nodes to it - # x0 = origo, Q = local basis - x0, Q = create_auxiliary_plane(slave_element, time) + # 1. calculate nodal normals and tangents for slave element nodes j ∈ S + normals = calculate_normals(slave_elements, time, Val{2}; + rotate_normals=props.rotate_normals) + update!(slave_elements, "normal", normals) - # 1. project slave nodes to auxiliary plane - Sl = Vector{Float64}[] - for p in slave_element("geometry", time) - push!(Sl, project_point_to_auxiliary_plane(p, x0, Q)) - end - S = hcat(Sl...) + # 2. loop all slave elements + for slave_element in slave_elements - for master_element in slave_element["master elements"] + slave_element_nodes = get_connectivity(slave_element) + nsl = length(slave_element) + X1 = slave_element("geometry", time) + n1 = Field([normals[j] for j in slave_element_nodes]) - # if distance between elements is "far enough" cannot expect contact - if (props.normal_condition == :Contact) || props.inequality_constraints - slave_midpoint = slave_element("geometry", [0.0, 0.0], time) - master_midpoint = master_element("geometry", [0.0, 0.0], time) - if norm(slave_midpoint - master_midpoint) > props.minimum_distance - continue - end - end + # project slave nodes to auxiliary plane (x0, Q) + #xi = get_reference_element_midpoint(slave_element) + xi = [1/3, 1/3] + N = vec(get_basis(slave_element, xi, time)) + x0 = N*X1 + n0 = N*n1 + S = Vector[project_vertex_to_auxiliary_plane(p, x0, n0) for p in X1] - master_dofs = get_gdofs(master_element, field_dim) + # 3. loop all master elements + for master_element in slave_element("master elements", time) - # 2. project master nodes to auxiliary plane - M = Vector{Float64}[] - for p in master_element("geometry", time) - push!(M, project_point_to_auxiliary_plane(p, x0, Q)) - end - M = hcat(M...) + master_element_nodes = get_connectivity(master_element) + nm = length(master_element) + X2 = master_element("geometry", time) - # 3. create polygon clipping on auxiliary plane - P = nothing - neighbours = nothing - try - P, neighbours = clip_polygon(S, M) - catch - info("polygon clipping failed") - info("S = ") - dump(S) - info("M = ") - dump(M) - info("original Sl = ") - info(Sl) - error("cannot continue") - end - isa(P, Void) && continue # no clipping + # 3.1 project master nodes to auxiliary plane and create polygon clipping + M = Vector[project_vertex_to_auxiliary_plane(p, x0, n0) for p in X2] + P = get_polygon_clip(S, M, n0) + length(P) < 3 && continue # no clipping or shared edge (no volume) + check_orientation!(P, n0) + C0 = calculate_centroid(P) - # shared edge but no shared volume. skipping - size(P, 2) < 3 && continue + De = zeros(nsl, nsl) + Me = zeros(nsl, nm) + ge = zeros(field_dim*nsl) - C = calculate_polygon_centerpoint(P) - npts = size(P, 2) # number of vertices in polygon + # 4. loop integration cells + for cell in get_cells(P, C0) + virtual_element = Element(Tri3) + update!(virtual_element, "geometry", cell) + #x_cell = Field(cell) - # loop vertices and create temporary integrate cells - # TODO: basically when npts == 3 or npts == 4 we could integrate without splitting to cells. - nnodes = size(slave_element, 2) - C1S3 = zeros(3*nnodes, 3*nnodes) - C1M3 = zeros(3*nnodes, 3*nnodes) + # 5. loop integration point of integration cell + for ip in get_integration_points(virtual_element, 3) + N = vec(get_basis(virtual_element, ip, time)) + #dN = vec(get_dbasis(virtual_element, ip, time)) + #JC = transpose(sum([kron(dNC[:,j], x_cell[j]') for j=1:length(x_cell)])) + #wC = ip.weight*norm(cross(JC[:,1], JC[:,2])) + detJ = virtual_element(ip, time, Val{:detJ}) + w = ip.weight*detJ - for pnt=1:npts # integration of mortar matrices begin - cell = Field(Vector{Float64}[C, P[:,pnt], P[:,mod(pnt,npts)+1]]) + # project gauss point from auxiliary plane to master and slave element + #x_gauss = N*x_cell + x_gauss = virtual_element("geometry", ip, time) + if isnan(x_gauss[1]) + info("is nan") + info("x_gauss = $x_gauss") + info("cell = $cell") + info("C0 = $C0") + info("P = $P") + info("S = $S") + info("M = $M") + info("n0 = $n0") + error("nan, unable to continue") + end + xi_s, alpha = project_vertex_to_surface(x_gauss, x0, n0, slave_element, X1, time) + xi_m, alpha = project_vertex_to_surface(x_gauss, x0, n0, master_element, X2, time) - # calculate slave side projection matrix D - # construct dual basis - Ae = zeros(nnodes, nnodes) - De = zeros(nnodes, nnodes) - Me = zeros(nnodes, nnodes) - if problem.properties.formulation == :Dual # Construct dual basis - for ip in get_integration_points(Tri3, Val{5}) - N = get_basis(Tri3, ip.xi) - xi = vec(N*cell) - theta = project_point_from_plane_to_surface(xi, x0, Q, slave_element, time) - xi_slave = theta[2:3] - N1 = slave_element(xi_slave, time) - # jacobian determinant on integration cell - dNC = get_dbasis(Tri3, ip.xi) - JC = sum([kron(dNC[:,j], cell[j]') for j=1:length(cell)]) - wC = ip.weight*det(JC) - De += wC*diagm(vec(N1)) - Me += wC*N1'*N1 - end - Ae = De*inv(Me) - end + # add contributions + N1 = vec(get_basis(slave_element, xi_s, time)) + N2 = vec(get_basis(master_element, xi_m, time)) + De += w*N1*N1' + Me += w*N1*N2' + if props.adjust + u1 = slave_element("displacement", time) + u2 = master_element("displacement", time) + x_s = N1*(X1+u1) + x_m = N2*(X2+u2) + ge += w*vec((x_m-x_s)*N1') + end + area += w + end # integration points done + + end # integration cells done + + # 6. add contribution to contact virtual work + sdofs = get_gdofs(problem, slave_element) + mdofs = get_gdofs(problem, master_element) + for i=1:field_dim - C1S3[i:field_dim:end,i:field_dim:end] += De + lsdofs = sdofs[i:field_dim:end] + lmdofs = mdofs[i:field_dim:end] + add!(problem.assembly.C1, lsdofs, lsdofs, De) + add!(problem.assembly.C1, lsdofs, lmdofs, -Me) + add!(problem.assembly.C2, lsdofs, lsdofs, De) + add!(problem.assembly.C2, lsdofs, lmdofs, -Me) end + add!(problem.assembly.g, sdofs, ge) - # Calculate master side projection matrix M - for ip in get_integration_points(Tri3, Val{5}) - # gauss point in auxiliary plane - #N = get_basis(E, ip.xi) - N = get_basis(Tri3, ip.xi) - xi = vec(N*cell) # xi defined in auxilary plane + end # master elements done - # find projection of gauss point to master and slave elements - theta1 = project_point_from_plane_to_surface(xi, x0, Q, slave_element, time) - theta2 = project_point_from_plane_to_surface(xi, x0, Q, master_element, time) - xi_slave = theta1[2:3] - xi_master = theta2[2:3] + end # slave elements done, contact virtual work ready - # evaluate shape functions values in gauss point and add contribution to matrices - N1 = slave_element(xi_slave, time) - N2 = master_element(xi_master, time) + debug && info("area of interface: $area") - # jacobian determinant on integration cell - dNC = get_dbasis(Tri3, ip.xi) - JC = sum([kron(dNC[:,j], cell[j]') for j=1:length(cell)]) - wC = ip.weight*det(JC) - - # extend matrices according to the problem dimension (3) - @assert length(slave_dofs) == length(master_dofs) - Me = wC*Ae*N1'*N2 - for k=1:field_dim - C1M3[k:field_dim:end,k:field_dim:end] += Me - end - end - end # integration of mortar matrices done. - - # constraints in normal-tangential direction and initial weighted gap - X1 = vec(slave_element("geometry", time)) - X2 = vec(master_element("geometry", time)) - Q_ = slave_element("normal-tangential coordinates", time) - Z = zeros(3, 3) - if nnodes == 3 - Q3 = [Q Z Z; Z Q Z; Z Z Q] - elseif nnodes == 4 - Q3 = [Q Z Z Z; Z Q Z Z; Z Z Q Z; Z Z Z Q] - end - D3 = zeros(3*nnodes, 3*nnodes) - C2S3 = Q3'*C1S3 - C2M3 = Q3'*C1M3 - G = -(C2S3*X1 - C2M3*X2) - - # complementarity condition - if haskey(slave_element, "displacement") - u1 = vec(slave_element("displacement", time)) - else - u1 = zeros(3*nnodes) - end - if haskey(master_element, "displacement") - u2 = vec(master_element("displacement", time)) - else - u2 = zeros(3*nnodes) - end - x1 = X1 + u1 - x2 = X2 + u2 - if haskey(slave_element, "reaction force") - la = vec(slave_element("reaction force", time)) - else - la = zeros(3*nnodes) - end - g = -(C2S3*x1 - C2M3*x2) - c = Q3'*la - g - inactive_nodes = find(c[1:field_dim:end] .<= 0) - active_nodes = find(c[1:field_dim:end] .> 0) - - # normal constraint: remove inactive nodes if normal condition is set to contact - if problem.properties.normal_condition == :Contact - for j in inactive_nodes - dofs = [3*(j-1)+1, 3*(j-1)+2, 3*(j-1)+3] - G[dofs] = 0 - C1S3[dofs,:] = 0 - C1M3[dofs,:] = 0 - C2S3[dofs,:] = 0 - C2M3[dofs,:] = 0 - end - end - - # tangential constraint: stick or slip - if problem.properties.tangential_condition == :Slip - D3 = copy(C2S3) - D3[1:field_dim:end, :] = 0 - C2S3[2:field_dim:end, :] = 0 - C2M3[2:field_dim:end, :] = 0 - C2S3[3:field_dim:end, :] = 0 - C2M3[3:field_dim:end, :] = 0 - end - - # add contributions - add!(assembly.C1, slave_dofs, slave_dofs, C1S3) - add!(assembly.C1, slave_dofs, master_dofs, -C1M3) - add!(assembly.C2, slave_dofs, slave_dofs, C2S3) - add!(assembly.C2, slave_dofs, master_dofs, -C2M3) - add!(assembly.D, slave_dofs, slave_dofs, D3) - add!(assembly.c, slave_dofs, c) - add!(assembly.g, slave_dofs, G) - end end - diff --git a/test/runtests.jl b/test/runtests.jl index a8872cf..355df78 100644 --- a/test/runtests.jl +++ b/test/runtests.jl @@ -15,6 +15,8 @@ function run_tests(; verbose=true) verbose && info("$i $test_file") end + t0 = Base.time() + body = quote @testset "JuliaFEM" begin for fn in $test_files @@ -24,6 +26,8 @@ function run_tests(; verbose=true) end eval(body) + t1 = round(Base.time()-t0, 2) + info("Testing completed in $t1 seconds.") end run_tests()