diff --git a/src/integrate.jl b/src/integrate.jl
index abbdf09..3ddc6a5 100644
--- a/src/integrate.jl
+++ b/src/integrate.jl
@@ -126,6 +126,23 @@ function get_integration_points(::Type{Quad4}, ::Type{Val{3}})
     return pts
 end
 
+function get_integration_points(::Type{Quad4}, ::Type{Val{5}})
+    p = [
+        -1/3*sqrt(5 + 2*sqrt(10/7)),
+        -1/3*sqrt(5 - 2*sqrt(10/7)),
+         0.0,
+         1/3*sqrt(5 - 2*sqrt(10/7)),
+         1/3*sqrt(5 + 2*sqrt(10/7))]
+    w = [
+        (322-13*sqrt(70))/900,
+        (322+13*sqrt(70))/900,
+         128/225,
+        (322+13*sqrt(70))/900,
+        (322-13*sqrt(70))/900]
+    pts = vec([IntegrationPoint([p[i], p[j]], w[i]*w[j]) for i=1:5, j=1:5])
+    return pts
+end
+
 function get_integration_points(::Type{Quad4})
     return get_integration_points(Quad4, Val{2})
 end
diff --git a/src/mortar.jl b/src/mortar.jl
index 73d9edb..454f5cf 100644
--- a/src/mortar.jl
+++ b/src/mortar.jl
@@ -729,9 +729,32 @@ function assemble!{E<:MortarElements2D}(assembly::Assembly, problem::Problem{Mor
 
             X1 = slave_element("geometry", xi_gauss, time)
             X2 = master_element("geometry", xi_projected, time)
-            g = norm(X2-X1)
-            gh = w*Phi*g
-            add!(assembly.g, slave_dofs[1:field_dim:end], gh)
+
+            x1 = copy(X1)
+            x2 = copy(X2)
+            if haskey(slave_element, "displacement")
+                u1 = slave_element("displacement", xi_gauss, time)
+                x1 = x1 + u1
+            end
+            if haskey(master_element, "displacement")
+                u2 = slave_element("displacement", xi_projected, time)
+                x2 = x2 + u2
+            end
+
+            n = Q[:,1]
+            Gn = -dot(n, X1-X2)
+            Gh = w*Phi*Gn
+            if haskey(slave_element, "reaction force")
+                gn = -dot(n, x1-x2)
+                #gh = w*Phi*gn
+                la = slave_element("reaction force", xi_gauss, time)
+                lan = dot(n, la)
+                c = w*Phi*(-lan + gn)
+                #info("lambda(n) = $lan, wug(n) = $Gh, wdg(n) = $gh, c=$c")
+                #info("c: $c")
+                add!(assembly.c, slave_dofs[1:field_dim:end], c)
+            end
+            add!(assembly.g, slave_dofs[1:field_dim:end], Gh)
         end
     end
 end
diff --git a/src/problems.jl b/src/problems.jl
index 6274185..0c5931f 100644
--- a/src/problems.jl
+++ b/src/problems.jl
@@ -21,6 +21,7 @@ type Assembly
     C2 :: SparseMatrixCOO
     D :: SparseMatrixCOO
     g :: SparseMatrixCOO
+    c :: SparseMatrixCOO
 
     u :: Vector{Float64}  # solution vector u
     u_prev :: Vector{Float64}  # previous solution vector u
@@ -44,6 +45,7 @@ function Assembly()
         SparseMatrixCOO(),
         SparseMatrixCOO(),
         SparseMatrixCOO(),
+        SparseMatrixCOO(),
         [], [], Inf,
         [], [], Inf,
         [], [], true)
@@ -57,6 +59,7 @@ function Base.empty!(assembly::Assembly)
     empty!(assembly.C2)
     empty!(assembly.D)
     empty!(assembly.g)
+    empty!(assembly.c)
     assembly.changed = true
 end
 
diff --git a/src/utils.jl b/src/utils.jl
index 0825979..f382cc8 100644
--- a/src/utils.jl
+++ b/src/utils.jl
@@ -90,9 +90,12 @@ end
 """
 function get_rotation_matrix(elements, time)
     Q = Dict{Int64, Matrix{Float64}}()
+    ndim = 0
     for element in elements
         node_ids = get_connectivity(element)
         q = element("normal-tangential coordinates", time).data
+        ndim == 0 && (ndim = size(q, 1))
+        ndim != size(q, 1) && error("2d and 3d rotation matrices in one element set?")
         for (qi, node_id) in zip(q, node_ids)
             if haskey(Q, node_id)
                 @assert isapprox(Q[node_id], qi)
@@ -103,7 +106,7 @@ function get_rotation_matrix(elements, time)
     end
     R = SparseMatrixCOO()
     for (k, q) in Q
-        dofs = [(k-1)*2+1, (k-1)*2+2]
+        dofs = Int[ndim*(k-1)+j for j=1:ndim]
         add!(R, dofs, dofs, q)
     end
     return R