still broken autodiff for 3d

2026-09-16 16:53:19 +00:00 · 2016-02-26 13:37:38 +02:00
parent f3ef7aabea
commit 28e193fa95
1 changed files with 292 additions and 43 deletions
@@ -1,6 +1,190 @@
 # This file is a part of JuliaFEM.
 # License is MIT: see https://github.com/JuliaFEM/JuliaFEM.jl/blob/master/LICENSE.md

+""" Fast inverse of 3x3 matrix. """
+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 vertex `p` from element surface to auxiliary plane defined
+with centerpoint `x0` and normal direction `n0`.
+"""
+function project_vertex_to_auxiliary_plane(p::Vector, x0::Vector, n0::Vector)
+    return p - dot(p-x0, n0)*n0
+end
+
+""" Project vertex `p` from auxiliary plane (x0, n0) back to element surface.
+
+This requires solving nonlinear system of equations
+
+f(α,ξ₁,ξ₂) = Nₖξₖ - αn₀ - p = 0
+
+"""
+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(E, xi)
+    dbasis(xi) = get_dbasis(E, xi)
+    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
+        norm(ForwardDiff.get_value(dtheta)) < iter_tol && return theta[1:2], theta[3]
+    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)), n0 = $(ForwardDiff.get_value(n0))")
+    info("element geometry: $(ForwardDiff.get_value(x.data))")
+    info("vertex to project: $(ForwardDiff.get_value(p))")
+    info("parameter vector before giving up: $(ForwardDiff.get_value(theta)')")
+    info("increment in parameter vector before giving up: $(ForwardDiff.get_value(dtheta)')")
+    info("norm(dtheta) before giving up: $(ForwardDiff.get_value(norm(dtheta)))")
+    info("f([0.0, 0.0, 0.0]) = $(ForwardDiff.get_value(f([0.0, 0.0, 0.0]))')")
+    info("L([0.0, 0.0, 0.0]) = $(ForwardDiff.get_value(L([0.0, 0.0, 0.0])))")
+
+    info("iterations:")
+    theta = zeros(3)
+    dtheta = zeros(3)
+    for i=1:max_iterations
+        info("iter $i, theta = $(ForwardDiff.get_value(theta)')")
+        info("f = $(ForwardDiff.get_value(f(theta))')")
+        info("L = $(ForwardDiff.get_value(L(theta)))")
+#       info("L2 = $(ForwardDiff.get_value(L2(theta)))")
+        dtheta = L(theta) * f(theta)
+        info("dtheta = $(ForwardDiff.get_value(dtheta)')")
+        theta -= dtheta
+    end
+
+    error("project_point_to_surface: did not converge in $max_iterations iterations!")
+end
+
+""" Test is q inside sm.
+http://bbs.dartmouth.edu/~fangq/MATH/download/source/Determining%20if%20a%20point%20lies%20on%20the%20interior%20of%20a%20polygon.htm
+"""
+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]
+    #info("areas: $areas")
+    #info("centroids: $centroids")
+    C = 1/sum(areas)*sum(areas.*centroids)
+    return C
+end
+
+function get_polygon_clip(xs, xm, n)
+    # objective: search does line xm1 - xm2 clip xs
+    nm = length(xm)
+    ns = length(xs)
+    P = []
+
+    # 1. test is master point inside slave, if yes, add to clip
+    for i=1:nm
+        vertex_inside_polygon(xm[i], xs) && push!(P, xm[i])
+    end
+
+    # 2. test is slave point inside master, if yes, add to clip
+    for i=1:ns
+        vertex_inside_polygon(xs[i], xm) && push!(P, xs[i])
+    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))")
+            vertex_inside_polygon(q, xm) && push!(P, q)
+        end
+    end
+
+    return P
+end
+
+""" Divide polygon to triangle cells. """
+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
+
+
 """ Assemble Mortar problem for three-dimensional problems, i.e. for Tri3, Tri6, Quad4, Quad8, Quad9 elements. """
 function assemble!(problem::Problem{Mortar}, time::Real, ::Type{Val{3}})

@@ -16,9 +200,12 @@ function assemble!(problem::Problem{Mortar}, time::Real, ::Type{Val{3}})
        la = reshape(x[ndofs+1:end], field_dim, nnodes)
        fc = zeros(u)
        gap = zeros(u)
-        gap_added = zeros(size(u)...)
+#       gap_added = zeros(size(u)...)
        C = zeros(la)
        all_slave_nodes = Set{Int64}()
+        slave_surface_area = 0.0
+        slave_surface_area_2 = 0.0
+        slave_element_areas = []

        # 1. calculate and average node normals for slave element nodes
        normal = zeros(u)
@@ -42,17 +229,16 @@ function assemble!(problem::Problem{Mortar}, time::Real, ::Type{Val{3}})
            end
        end
        # calculate tangents
-        for i in 1:size(normal, 2)
-            i in all_slave_nodes || continue
+        for i in all_slave_nodes
            normal[:,i] /= norm(normal[:,i])
-            u1 = normal[:,i]
-            j = indmax(abs(u1))
-            v2 = zeros(3)
-            v2[mod(j,3)+1] = 1.0
-            u2 = v2 - dot(u1,v2)/dot(v2,v2)*v2
-            u3 = cross(u1,u2)
-            tangent1[:,i] = u2/norm(u2)
-            tangent2[:,i] = u3/norm(u3)
+            U1 = normal[:,i]
+            j = indmax(abs(U1))
+            V2 = zeros(3)
+            V2[mod(j,3)+1] = 1.0
+            U2 = V2 - dot(U1,V2)/dot(V2,V2)*V2
+            U3 = cross(U1,U2)
+            tangent1[:,i] = U2/norm(U2)
+            tangent2[:,i] = U3/norm(U3)
        end
        if props.rotate_normals
            for i=1:size(normal, 2)
@@ -62,7 +248,9 @@ function assemble!(problem::Problem{Mortar}, time::Real, ::Type{Val{3}})

        # 2. loop slave elements and find contact segments
        for slave_element in get_elements(problem)
+            slave_element_area = 0.0
            haskey(slave_element, "master elements") || continue
+            info("new slave element")

            slave_element_nodes = get_connectivity(slave_element)
            X1 = slave_element("geometry", time)
@@ -78,11 +266,13 @@ function assemble!(problem::Problem{Mortar}, time::Real, ::Type{Val{3}})
            # create auxiliary plane (x0, Q)
            xi = get_reference_element_midpoint(slave_element)
            N = vec(get_basis(slave_element, xi))
+            #Q = [N*n1 N*t1 N*t2]
            x0 = N*x1
-            Q = [N*n1 N*t1 N*t2]
+            n0 = N*n1

            # project slave nodes to auxiliary plane
-            S = hcat([project_vertex_to_auxiliary_plane(p, x0, Q) for p in x1]...)
+            #S = hcat([project_vertex_to_auxiliary_plane(p, x0, Q) for p in x1]...)
+            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"]
@@ -98,7 +288,8 @@ function assemble!(problem::Problem{Mortar}, time::Real, ::Type{Val{3}})
                distance > props.maximum_distance && continue

                # project master nodes to auxiliary plane
-                M = hcat([project_vertex_to_auxiliary_plane(p, x0, Q) for p in x2]...)
+                #M = hcat([project_vertex_to_auxiliary_plane(p, x0, Q) for p in x2]...)
+                M = Vector[project_vertex_to_auxiliary_plane(p, x0, n0) for p in x2]

                # create polygon clipping on auxiliary plane
 #=
@@ -114,19 +305,52 @@ function assemble!(problem::Problem{Mortar}, time::Real, ::Type{Val{3}})
                    error("cannot continue")
                end
 =#
-                P, neighbours = clip_polygon(S, M)
-                isa(P, Void) && continue # no clipping
-                info("polygon clip found: S = $(ForwardDiff.get_value(S)), M = $(ForwardDiff.get_value(M)), P = $(ForwardDiff.get_value(P))")
+                #P, neighbours = clip_polygon(S, M)
+                #isa(P, Void) && continue # no clipping
+                #info("polygon clip found: S = $(ForwardDiff.get_value(S)), M = $(ForwardDiff.get_value(M)), P = $(ForwardDiff.get_value(P))")
                # special case, shared edge but no shared volume
-                size(P, 2) < 3 && continue
-                gap_added += 1
+                #size(P, 2) < 3 && continue
+                #gap_added += 1
+                P = get_polygon_clip(S, M, n0)
+                length(P) < 3 && continue # no clipping or shared edge (no volume)

                # clip polygon centerpoint
                #C0 = calculate_polygon_centerpoint(P)
-                C0 = vec(mean(P, 2))
-                npts = size(P, 2) # number of vertices in polygon
-                for pnt=1:npts # loop integration cells
-                    x_cell = Field(Vector[C0, P[:,pnt], P[:,mod(pnt,npts)+1]])
+                #C0 = vec(mean(P, 2))
+                act = setdiff(collect(1:3), indmax(n0))
+                function comparator(A,B)
+                    A -= mean(P)
+                    B -= mean(P)
+                    atan2(A[act[1]], A[act[2]]) < atan2(B[act[1]], B[act[2]])
+                end
+                sort!(P, lt=comparator)
+                #sort!(P, lt=(A,B) -> dot(n0, cross(A-C0, B-C0)) > 0)
+                #npts = size(P, 2) # number of vertices in polygon
+                #for pnt=1:npts # loop integration cells
+                #    x_cell = Field(Vector[C0, P[:,pnt], P[:,mod(pnt,npts)+1]])
+                #info("integrating cell")
+                #info("P = $(ForwardDiff.get_value(P))")
+
+                for i=2:length(P)
+                    V = cross(P[i]-P[1], P[mod(i,length(P))+1]-P[1])
+                    slave_element_area += 1/2*dot(n0, V)
+                end
+                C0 = calculate_centroid(P)
+                #=
+                for x_cell in get_cells(P, C0)
+                    V = cross(x_cell[2]-x_cell[1], x_cell[3]-x_cell[1])
+                    slave_element_area += 1/2*dot(n0, V)
+                end
+                =#
+
+                for x_cell_ in get_cells(P, C0)
+                    #info("cell : $(ForwardDiff.get_value(x_cell_))")
+                    x_cell = Field(x_cell_)
+                    V = cross(x_cell[2]-x_cell[1], x_cell[3]-x_cell[1])
+                    #info("V = $(ForwardDiff.get_value(V))")
+                    #slave_element_area += 1/2*dot(n0, V)
+#                   slave_surface_area_2 += 1/2*jC
+#                   slave_element_area += 1/2*jC
 #=
                    try
                    catch
@@ -144,9 +368,10 @@ function assemble!(problem::Problem{Mortar}, time::Real, ::Type{Val{3}})
                    for ip in get_integration_points(Tri3, Val{5})
                        N = vec(get_basis(Tri3, ip.xi))
                        x_g = N*x_cell
+#=
                        theta = zeros(3)
                        try
-                            theta = project_vertex_from_plane_to_surface(x_g, x0, Q, slave_element, x1, time)
+                            theta = project_vertex_to_surface(x_g, x0, n0, slave_element, x1, time)
                        catch
                            info("creating dual basis did fail for finding projection back to slave surface.")
                            info("cell coords in auxiliary plane are:")
@@ -159,12 +384,19 @@ function assemble!(problem::Problem{Mortar}, time::Real, ::Type{Val{3}})
                            rethrow()
                        end
                        xi_slave = theta[2:3]
+=#
+                        xi_slave, alpha = project_vertex_to_surface(x_g, x0, n0, slave_element, x1, time)
                        N1 = slave_element(xi_slave, time)

-                        # jacobian determinant on integration cell
+                        # jacobian determinant on integration cell combined with integration weight
                        dNC = get_dbasis(Tri3, ip.xi)
-                        JC = sum([kron(dNC[:,j], x_cell[j]') for j=1:length(x_cell)])
-                        wC = ip.weight*det(JC)
+                        JC = transpose(sum([kron(dNC[:,j], x_cell[j]') for j=1:length(x_cell)]))
+                        jC = norm(cross(JC[:,1], JC[:,2]))
+                        #info("dNC = $dNC")
+                        #info("x_cell = $(ForwardDiff.get_value(x_cell.data))")
+                        #info("JC = $(ForwardDiff.get_value(JC))")
+                        #det(JC)
+                        wC = ip.weight*jC

                        De += wC*diagm(vec(N1))
                        Me += wC*N1'*N1
@@ -176,28 +408,31 @@ function assemble!(problem::Problem{Mortar}, time::Real, ::Type{Val{3}})
                        N = vec(get_basis(Tri3, ip.xi))
                        x_g = N*x_cell
                        # project gauss point back to element surfaces
-                        theta1 = project_vertex_from_plane_to_surface(x_g, x0, Q, slave_element, x1, time)
-                        theta2 = project_vertex_from_plane_to_surface(x_g, x0, Q, master_element, x2, time)
-                        xi_slave = theta1[2:3]
-                        xi_master = theta2[2:3]
+                        xi_slave, alpha = project_vertex_to_surface(x_g, x0, n0, slave_element, x1, time)
+                        xi_master, alpha = project_vertex_to_surface(x_g, x0, n0, master_element, x2, time)

                        # evaluate shape functions, calculate contact force and gap
                        N1 = vec(get_basis(slave_element, xi_slave))
                        N2 = vec(get_basis(master_element, xi_master))
                        Phi = Ae*N1

-                        # jacobian determinant of integration cell
                        dNC = get_dbasis(Tri3, ip.xi)
-                        JC = sum([kron(dNC[:,j], x_cell[j]') for j=1:length(x_cell)])
-                        wC = ip.weight*det(JC)
+                        JC = transpose(sum([kron(dNC[:,j], x_cell[j]') for j=1:length(x_cell)]))
+                        jC = norm(cross(JC[:,1], JC[:,2]))
+                        #dNC = get_dbasis(Tri3, ip.xi)
+                        #JC = sum([kron(dNC[:,j], x_cell[j]') for j=1:length(x_cell)])
+                        #jC = det(JC)
+                        wC = ip.weight*jC

                        x_s = N1*x1
                        n_s = N1*n1
                        x_m = N2*x2
                        la_s = Phi*la1
                        gn = props.gap_sign*dot(n_s, x_s - x_m)
-                        fc[:,slave_element_nodes] += wC*la_s*N1'
-                        fc[:,master_element_nodes] -= wC*la_s*N2'
+                        fc[:,slave_element_nodes] += -1*wC*la_s*N1'
+                        fc[:,master_element_nodes] -= -1*wC*la_s*N2'
+                        gap[1,slave_element_nodes] += wC*gn*Phi'
+
                        wg = wC*gn*Phi'
                        if any(isnan(wg))
                            info("gap has NaNs!")
@@ -214,13 +449,15 @@ function assemble!(problem::Problem{Mortar}, time::Real, ::Type{Val{3}})
                            info("P = $(ForwardDiff.get_value(P))")
                            error("fix this")
                        end
-                        gap[1,slave_element_nodes] += wg
-                        gap_added[1,slave_element_nodes] += 1
+#                       gap_added[1,slave_element_nodes] += 1
+                        slave_surface_area += wC
+                        #slave_element_area += wC
                    end # done integrating cell

                end # done for all cells in this segment

            end # done all master elements for this slave element
+            push!(slave_element_areas, slave_element_area)

        end # done all slave elements

@@ -233,6 +470,16 @@ function assemble!(problem::Problem{Mortar}, time::Real, ::Type{Val{3}})
        info("size of la = $(size(la))")
        info("size of C = $(size(C))")
        info("S = $all_slave_nodes")
+        info("slave surface area: $(ForwardDiff.get_value(slave_surface_area))")
+        info("slave surface area 2: $(ForwardDiff.get_value(slave_surface_area_2))")
+        info("slave element areas:")
+        for (i, a) in enumerate(slave_element_areas)
+            info("element $i, area = $(ForwardDiff.get_value(a))")
+        end
+        for (i, element) in enumerate(get_elements(problem))
+            haskey(element, "master elements") || continue
+            info("element $i geometry: $(element("geometry", time).data)")
+        end

        for (i, j) in enumerate(all_slave_nodes)
            if j in props.always_inactive
@@ -250,9 +497,9 @@ function assemble!(problem::Problem{Mortar}, time::Real, ::Type{Val{3}})
                C[1,j] += gap[1, j]
                C[2,j] += dot(t1, la[:,j])
                C[3,j] += dot(t2, la[:,j])
- #          else
- #              C[:,j] = la[:,j]
- #          end
+#           else
+#               C[:,j] = la[:,j]
+#           end
        end

        for (i, j) in enumerate(all_slave_nodes)
@@ -262,8 +509,8 @@ function assemble!(problem::Problem{Mortar}, time::Real, ::Type{Val{3}})
            lai = ForwardDiff.get_value(la[:,j])
            ui = ForwardDiff.get_value(u[:,j])
            ni = ForwardDiff.get_value(normal[:,j])
-            gai = gap_added[:,j]
-            info("$i/$j: C = $Ci, f = $fci, gap = $gapi, la = $lai, u = $ui, n = $ni, gai = $gai")
+#           gai = gap_added[:,j]
+            info("$i/$j: C = $Ci, f = $fci, gap = $gapi, la = $lai, u = $ui, n = $ni")
        end

        return vec([fc C])
@@ -288,6 +535,7 @@ function assemble!(problem::Problem{Mortar}, time::Real, ::Type{Val{3}})
    f = b[1:ndofs]
    g = b[ndofs+1:end]

+#=
    slaves = [101,108,111,112,113,120,123,124,125,126,129,130,149,150,151,152]
    for j in slaves
        dofs = [3*(j-1)+1, 3*(j-1)+2, 3*(j-1)+3]
@@ -298,6 +546,7 @@ function assemble!(problem::Problem{Mortar}, time::Real, ::Type{Val{3}})
        info("lambdas: $(D[dofs,:])")
        info("f  = $(f[dofs]), g = $(g[dofs])")
    end
+=#

    empty!(problem.assembly)
    add!(problem.assembly.K, K)