Page Menu
Home
c4science
Search
Configure Global Search
Log In
Files
F79502097
resolution_augmented_lagrangian.cc
No One
Temporary
Actions
Download File
Edit File
Delete File
View Transforms
Subscribe
Mute Notifications
Award Token
Subscribers
None
File Metadata
Details
File Info
Storage
Attached
Created
Mon, Aug 26, 18:48
Size
40 KB
Mime Type
text/x-c++
Expires
Wed, Aug 28, 18:48 (1 d, 22 h)
Engine
blob
Format
Raw Data
Handle
20169027
Attached To
rAKA akantu
resolution_augmented_lagrangian.cc
View Options
/**
* @file resolution_augmented_lagrangian.cc
*
* @author Alejandro M. Aragón <alejandro.aragon@epfl.ch>
*
* @date creation: Mon Sep 15 2014
* @date last modification: Wed Sep 17 2014
*
* @brief contact resolution classes
*
* @section LICENSE
*
* Copyright (©) 2014 EPFL (Ecole Polytechnique Fédérale de Lausanne)
* Laboratory (LSMS - Laboratoire de Simulation en Mécanique des Solides)
*
* Akantu is free software: you can redistribute it and/or modify it under the
* terms of the GNU Lesser General Public License as published by the Free
* Software Foundation, either version 3 of the License, or (at your option) any
* later version.
*
* Akantu is distributed in the hope that it will be useful, but WITHOUT ANY
* WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR
* A PARTICULAR PURPOSE. See the GNU Lesser General Public License for more
* details.
*
* You should have received a copy of the GNU Lesser General Public License
* along with Akantu. If not, see <http://www.gnu.org/licenses/>.
*
*/
//#include "resolution_augmented_lagrangian.hh"
#include "resolution_augmented_lagrangian.hh"
#define COUT(name) std::cout << std::string(#name) << ": " << name << std::endl;
__BEGIN_AKANTU__
template
<
int
Dim
>
void
ContactResolution
<
Dim
,
_static
,
_augmented_lagrangian
>::
initialize
()
{
// give priority to command line arguments instead of those in a file
if
(
contact_argparser
.
has
(
"aka_penalty"
))
options_
[
Epsilon
]
=
contact_argparser
[
"aka_penalty"
];
else
flags_
[
Automatic_penalty_parameter
]
=
true
;
if
(
contact_argparser
.
has
(
"aka_alpha"
))
options_
[
Alpha
]
=
contact_argparser
[
"aka_alpha"
];
if
(
contact_argparser
.
has
(
"aka_utol"
))
options_
[
Multiplier_tol
]
=
contact_argparser
[
"aka_utol"
];
if
(
contact_argparser
.
has
(
"aka_ntol"
))
options_
[
Newton_tol
]
=
contact_argparser
[
"aka_ntol"
];
if
(
contact_argparser
.
has
(
"aka_usteps"
))
options_
[
Multiplier_max_steps
]
=
contact_argparser
[
"aka_usteps"
];
if
(
contact_argparser
.
has
(
"aka_nsteps"
))
options_
[
Newton_max_steps
]
=
contact_argparser
[
"aka_nsteps"
];
if
(
contact_argparser
.
has
(
"aka_verbose"
))
flags_
[
Verbose
]
=
true
;
}
template
<
int
Dim
>
ContactResolution
<
Dim
,
_static
,
_augmented_lagrangian
>::
ContactResolution
(
model_type
&
m
)
:
Parsable
(
_st_contact
),
model_
(
m
),
multiplier_dumper_
(
m
.
getMesh
().
getNbNodes
(),
3
),
pressure_dumper_
(
m
.
getMesh
().
getNbNodes
(),
3
)
{
// register dumpers
m
.
getMesh
().
addDumpFieldExternal
(
"multipliers"
,
multiplier_dumper_
);
m
.
getMesh
().
addDumpFieldExternal
(
"pressure"
,
pressure_dumper_
);
// register parameters from the file
registerParam
(
"penalty"
,
options_
[
Epsilon
],
_pat_parsable
,
"Penalty parameter for Augmented-Lagrangian formulation"
);
registerParam
(
"alpha"
,
options_
[
Alpha
],
1.
,
_pat_parsable
,
"Multiplier for values of the penalty parameter"
);
registerParam
(
"utol"
,
options_
[
Multiplier_tol
],
1e-4
,
_pat_parsable
,
"Tolerance used for multipliers in the Uzawa method"
);
registerParam
(
"ntol"
,
options_
[
Newton_tol
],
1e-4
,
_pat_parsable
,
"Tolerance used in the Newton-Raphson inner convergence loop"
);
registerParam
(
"usteps"
,
options_
[
Multiplier_max_steps
],
100.
,
_pat_parsable
,
"Maximum number of steps allowed in the Uzawa loop"
);
registerParam
(
"nsteps"
,
options_
[
Newton_max_steps
],
100.
,
_pat_parsable
,
"Maximum number of steps allowed in the Newton-Raphson loop"
);
// register parameters from the command line
contact_argparser
.
addArgument
(
"--aka_penalty"
,
"Penalty parameter for Augmented-Lagrangian formulation"
,
1
,
cppargparse
::
_float
);
contact_argparser
.
addArgument
(
"--aka_alpha"
,
"Multiplier for values of the penalty parameter"
,
1
,
cppargparse
::
_float
);
contact_argparser
.
addArgument
(
"--aka_utol"
,
"Tolerance used for multipliers in the Uzawa method"
,
1
,
cppargparse
::
_float
);
contact_argparser
.
addArgument
(
"--aka_ntol"
,
"Tolerance used in the Newton-Raphson inner convergence loop"
,
1
,
cppargparse
::
_float
);
contact_argparser
.
addArgument
(
"--aka_usteps"
,
"Maximum number of steps allowed in the Uzawa loop"
,
1
,
cppargparse
::
_float
);
contact_argparser
.
addArgument
(
"--aka_nsteps"
,
"Maximum number of steps allowed in the Newton-Raphson loop"
,
1
,
cppargparse
::
_float
);
contact_argparser
.
addArgument
(
"--aka_verbose"
,
"Verbose output flag"
,
0
,
cppargparse
::
_boolean
);
}
template
<
int
Dim
>
void
ContactResolution
<
Dim
,
_static
,
_augmented_lagrangian
>::
solveContactStepImpl
(
SearchBase
*
sp
,
Int2Type
<
_generalized_newton
>
gn
)
{
ContactResolution
&
cd
=
*
this
;
model_
.
implicitPred
();
model_
.
updateResidual
();
AKANTU_DEBUG_ASSERT
(
model_
.
stiffness_matrix
!=
NULL
,
"You should first initialize the implicit solver and "
"assemble the stiffness matrix"
);
//** comments that start like this will comment on the work that has to be
//** done for the implementation of the frictional terms in the code
UInt
k
=
0
;
UInt
ntotal
=
0
;
std
::
list
<
int
>
nt
;
// get global stiffness matrix and force vector
SparseMatrix
&
K
=
model_
.
getStiffnessMatrix
();
Array
<
Real
>
&
F
=
model_
.
getResidual
();
// get size of the whole system
UInt
original
=
model_
.
increment
->
getSize
()
*
Dim
;
UInt
size
=
original
+
sm_
.
size
();
//** the size variable at this point is computed by adding only the number
//** of slave nodes because for each slave node there's a lagrangian
//** multiplier for the normal contact assigned to it. In the case of
//** frictional contact, this variable will have to account for 2 (3)
//** multipliers for each in slave node in 2D (3D), accounting for the
//** tangential components.
contact_status_
=
std
::
map
<
UInt
,
bool
>
();
status_change_
=
std
::
map
<
UInt
,
int
>
();
size_t
ccc
=
0
;
for
(
auto
g
:
gaps_
)
{
if
(
std
::
abs
(
g
.
second
)
<=
1.e-10
)
{
contact_status_
[
g
.
first
]
=
true
;
++
ccc
;
}
}
Array
<
Real
>
solution
(
size
);
Array
<
Real
>
rhs
(
size
);
rhs
.
clear
();
// extend data structures to consider Lagrange multipliers
K
.
resize
(
size
);
//** with the right value of the 'size' variable, all these data structures
//** will be resized accordingly
cout
<<
std
::
boolalpha
;
if
(
cd
[
Verbose
])
cout
<<
"- Start Generalized Newton:"
<<
endl
;
UInt
j
=
0
;
bool
converged
=
false
;
bool
converged_multiplier
=
false
;
Real
fnorm
=
0
;
do
{
Real
nerror
=
0.
;
cd
.
niter_
=
j
;
// assemble material matrix
model_
.
assembleStiffnessMatrix
();
// copy residual to rhs
std
::
copy_n
(
F
.
storage
(),
original
,
rhs
.
storage
());
// compute contribution to tangent matrix and residual
Real
dummy
;
computeTangentAndResidual
(
solution
,
rhs
,
sp
,
dummy
,
gn
);
//** The computeTangentAndResidual is where the major development for the
//** frictional part will take place.
//** All the terms coded in this function into the stiffness matrix and the
//** force vector take into account only the normal contact component. The
//** implementation of the frictional part will include terms for the
//** tangential multipliers. The process I followed for the implementation
//** was to code the stiffness matrix terms from the book by Laursen
//** (Computational Contact and Impact Mechanics). I then took the terms
//** involving the lagrangian multiplier part from the thesis by Grzegorz
//** Pietrzak (Continuum mechanics modelling and augmented Lagrangian
//** formulation of large deformation frictional contact problems) as
//** these terms were missing in the Laursen book. Some little changes had
//** to be made into these terms, as Laursen and Pietrzak use different
//** conventions for the sign of the gap function. In Pietrzak's thesis,
//** the residual terms are given following Eqn. 6.20 (page 146), and the
//** stiffness terms in following equation 6.25 (page 149).
//** I suggest to start by the implementation of the Uzawa method, as it is
//** not required to code the tangential lagrangian multiplier terms of
//** Pietrzak
// solve
model_
.
template
solve
<
IntegrationScheme2ndOrder
::
_displacement_corrector
>
(
solution
,
1.
,
true
,
true
,
rhs
);
// copy the solution of the system to increment for the primal variable only
std
::
copy_n
(
solution
.
begin
(),
model_
.
increment
->
getSize
()
*
Dim
,
model_
.
increment
->
storage
());
// copy the solution of the system to increment for the lagrange multiplier
size_t
m
=
0
;
std
::
vector
<
Real
>
multiplier_check
(
multipliers_
.
size
());
vector_type
v_old
(
multipliers_
.
size
());
vector_type
v_new
(
v_old
);
for
(
auto
&
pair
:
multipliers_
)
{
v_old
[
m
]
=
pair
.
second
;
v_new
[
m
]
=
v_old
[
m
]
+
solution
[
original
+
m
];
multiplier_check
[
m
]
=
pair
.
second
;
multipliers_
[
pair
.
first
]
+=
solution
[
original
+
m
];
multiplier_check
[
m
]
-=
pair
.
second
;
++
m
;
}
Real
sum_multiplier
=
0.
;
for
(
auto
m
:
multiplier_check
)
sum_multiplier
+=
m
*
m
;
//** this check basically computes the L_2 norm of the lagrange multiplier
//** difference, so this test may change when implementing the frictional
//** part
Real
mnorm
=
sqrt
(
sum_multiplier
);
Real
abs_tol
=
cd
[
Multiplier_tol
];
if
(
j
==
0
)
fnorm
=
mnorm
;
converged_multiplier
=
(
mnorm
<=
abs_tol
||
mnorm
<=
abs_tol
*
abs_tol
*
fnorm
);
model_
.
implicitCorr
();
model_
.
updateResidual
();
converged
=
model_
.
template
testConvergence
<
_scc_increment
>
(
cd
[
Newton_tol
],
nerror
);
if
(
cd
[
Verbose
])
{
size_t
w
=
10
;
cout
<<
std
::
setw
(
2
)
<<
j
<<
": Primal: "
<<
std
::
setw
(
w
)
<<
std
::
setprecision
(
4
)
<<
std
::
right
<<
nerror
<<
" <= "
<<
cd
[
Newton_tol
]
<<
" = "
<<
std
::
left
<<
std
::
setw
(
5
)
<<
(
nerror
<
cd
[
Newton_tol
])
<<
"
\t
Dual: "
<<
std
::
setw
(
w
)
<<
std
::
setprecision
(
4
)
<<
std
::
right
<<
mnorm
<<
" <= "
<<
cd
[
Multiplier_tol
]
<<
" = "
<<
(
mnorm
<=
cd
[
Multiplier_tol
])
<<
", "
<<
std
::
setw
(
w
)
<<
mnorm
<<
" <= "
<<
abs_tol
*
abs_tol
*
fnorm
<<
" = "
<<
(
mnorm
<=
abs_tol
*
abs_tol
*
fnorm
)
<<
endl
;
}
++
j
;
AKANTU_DEBUG_INFO
(
"["
<<
_scc_increment
<<
"] Convergence iteration "
<<
std
::
setw
(
std
::
log10
(
cd
[
Newton_max_steps
]))
<<
j
<<
": error "
<<
nerror
<<
(
converged
?
" < "
:
" > "
)
<<
cd
[
Newton_tol
]
<<
std
::
endl
);
}
while
(
!
(
converged
&&
converged_multiplier
)
&&
j
<
cd
[
Newton_max_steps
]);
if
(
j
==
cd
[
Newton_max_steps
])
{
cout
<<
"*** ERROR *** Newton-Raphson loop did not converge within max "
"number of iterations: "
<<
cd
[
Newton_max_steps
]
<<
endl
;
exit
(
1
);
}
nt
.
push_back
(
j
);
ntotal
+=
j
;
AKANTU_DEBUG_INFO
(
"["
<<
_scc_increment
<<
"] Uzawa convergence iteration "
<<
std
::
setw
(
std
::
log10
(
cd
[
Newton_max_steps
]))
<<
k
<<
std
::
endl
);
cout
<<
"Generalized Newton iterations: "
<<
j
<<
endl
;
// dump vtk files
this
->
dump
();
}
template
<
int
Dim
>
void
ContactResolution
<
Dim
,
_static
,
_augmented_lagrangian
>::
solveContactStepImpl
(
SearchBase
*
sp
,
Int2Type
<
_uzawa
>
uz
)
{
ContactResolution
&
cd
=
*
this
;
model_
.
implicitPred
();
model_
.
updateResidual
();
AKANTU_DEBUG_ASSERT
(
model_
.
stiffness_matrix
!=
NULL
,
"You should first initialize the implicit solver and "
"assemble the stiffness matrix"
);
//** comments that start like this will comment on the work that has to be
//** done for the implementation of the frictional terms in the code
// implementation of the Uzawa method for solving contact
bool
uzawa_converged
=
false
;
static
UInt
step
=
0
;
UInt
k
=
0
;
UInt
ntotal
=
0
;
std
::
list
<
int
>
nt
;
std
::
ofstream
ofs
;
ofs
.
open
(
"iterations.out"
,
std
::
ofstream
::
out
|
std
::
ofstream
::
app
);
// initialize Lagrange multipliers
// NOTE: It doesn't make any difference to start from the previous
// converged solution of Lagrange multipliers
real_map
lambda_new
;
cout
<<
std
::
boolalpha
;
if
(
cd
[
Verbose
])
cout
<<
"- Start Uzawa:"
<<
endl
;
do
{
Real
uerror
=
0.
;
bool
converged
=
false
;
UInt
j
=
0
;
cd
.
uiter_
=
k
;
do
{
Real
nerror
=
0.
;
cd
.
niter_
=
j
;
// assemble material matrix
model_
.
assembleStiffnessMatrix
();
// compute contribution to tangent matrix and residual
uzawa_converged
=
computeTangentAndResidual
(
lambda_new
,
sp
,
uerror
,
uz
);
//** The computeTangentAndResidual is where the major development for the
//** frictional part will take place.
//** All the terms coded in this function into the stiffness matrix and
//** the force vector take into account only the normal contact component.
//** The implementation of the frictional part will include terms for the
//** tangential multipliers. The process I followed for the implementation
//** was to code the stiffness matrix terms from the book by Laursen
//** (Computational Contact and Impact Mechanics).
//** I suggest you start by the implementing the Uzawa method first,
//** before jumping to the more involved implementation of the tangential
//** lagrangian multiplier terms of Pietrzak
// solve
model_
.
template
solve
<
IntegrationScheme2ndOrder
::
_displacement_corrector
>
(
*
model_
.
increment
,
1.
,
true
,
true
);
model_
.
implicitCorr
();
model_
.
updateResidual
();
converged
=
model_
.
template
testConvergence
<
_scc_increment
>
(
cd
[
Newton_tol
],
nerror
);
if
(
cd
[
Verbose
])
cout
<<
" Newton: "
<<
j
<<
", "
<<
nerror
<<
" < "
<<
cd
[
Newton_tol
]
<<
" = "
<<
(
nerror
<
cd
[
Newton_tol
])
<<
endl
;
++
j
;
AKANTU_DEBUG_INFO
(
"["
<<
_scc_increment
<<
"] Convergence iteration "
<<
std
::
setw
(
std
::
log10
(
cd
[
Newton_max_steps
]))
<<
j
<<
": error "
<<
nerror
<<
(
converged
?
" < "
:
" > "
)
<<
cd
[
Newton_tol
]
<<
std
::
endl
);
}
while
(
!
converged
&&
j
<
cd
[
Newton_max_steps
]);
if
(
cd
[
Verbose
])
cout
<<
" Uzawa: "
<<
k
<<
", "
<<
uerror
<<
" < "
<<
cd
[
Multiplier_tol
]
<<
" = "
<<
(
uerror
<
cd
[
Multiplier_tol
])
<<
endl
;
if
(
j
==
cd
[
Newton_max_steps
])
{
cout
<<
"*** ERROR *** Newton-Raphson loop did not converge within max "
"number of iterations: "
<<
cd
[
Newton_max_steps
]
<<
endl
;
exit
(
1
);
}
nt
.
push_back
(
j
);
ntotal
+=
j
;
// increment uzawa loop counter
++
k
;
AKANTU_DEBUG_INFO
(
"["
<<
_scc_increment
<<
"] Uzawa convergence iteration "
<<
std
::
setw
(
std
::
log10
(
cd
[
Newton_max_steps
]))
<<
k
<<
std
::
endl
);
// update lagrange multipliers
cd
.
multipliers_
=
lambda_new
;
}
while
(
!
uzawa_converged
&&
k
<
cd
[
Multiplier_max_steps
]);
if
(
k
==
cd
[
Multiplier_max_steps
])
{
cout
<<
"*** ERROR *** Uzawa loop did not converge within max number of "
"iterations: "
<<
cd
[
Multiplier_max_steps
]
<<
endl
;
exit
(
1
);
}
cout
<<
"Summary: Uzawa ["
<<
k
<<
"]: Newton ["
<<
ntotal
<<
"]:"
;
for
(
int
n
:
nt
)
cout
<<
" "
<<
n
;
cout
<<
endl
;
ofs
<<
std
::
setw
(
10
)
<<
++
step
<<
std
::
setw
(
10
)
<<
k
<<
std
::
setw
(
10
)
<<
ntotal
<<
endl
;
ofs
.
close
();
this
->
dump
();
}
template
<
int
Dim
>
void
ContactResolution
<
Dim
,
_static
,
_augmented_lagrangian
>::
dump
()
{
multiplier_dumper_
.
clear
();
pressure_dumper_
.
clear
();
for
(
auto
v
:
multipliers_
)
{
element_type
&
el
=
sm_
[
v
.
first
];
if
(
el
==
element_type
())
continue
;
auto
n
=
el
.
normal
();
Real
lambda
=
v
.
second
;
for
(
size_t
i
=
0
;
i
<
n
.
size
();
++
i
)
multiplier_dumper_
(
v
.
first
,
i
)
=
lambda
*
n
[
i
];
// dump pressures only if area is associated with node
auto
it
=
areas_
.
find
(
v
.
first
);
if
(
it
!=
areas_
.
end
())
for
(
size_t
i
=
0
;
i
<
n
.
size
();
++
i
)
{
Real
a
=
it
->
second
;
assert
(
a
!=
0.
);
pressure_dumper_
(
v
.
first
,
i
)
=
lambda
*
n
[
i
]
/
a
;
}
else
cout
<<
"*** WARNING *** Zero area for slave node "
<<
v
.
first
<<
endl
;
}
model_
.
dump
();
}
template
<
int
Dim
>
void
ContactResolution
<
Dim
,
_static
,
_augmented_lagrangian
>::
getPenaltyValues
()
{
cout
<<
"*** INFO *** Obtaining penalty parameters automatically. "
;
const
SparseMatrix
&
Kconst
=
model_
.
getStiffnessMatrix
();
Real
ave
=
0.
;
size_t
k
=
0
;
// loop over pairs
for
(
auto
it
=
sm_
.
begin
();
it
!=
sm_
.
end
();
++
it
)
{
auto
slave
=
it
->
first
;
auto
master
=
it
->
second
;
if
(
master
!=
element_type
())
{
std
::
vector
<
UInt
>
conn
(
master
.
numNodes
()
+
1
);
// 1 slave (not hardcoded)
conn
[
0
]
=
slave
;
for
(
UInt
i
=
0
;
i
<
master
.
numNodes
();
++
i
)
conn
[
1
+
i
]
=
master
.
node
(
i
);
// compute normal
vector_type
nu
=
master
.
normal
();
// carry out stiffness multiplication with the normal
// the product Kij*nj would give the force for a unit displacement
// (i.e., the stiffness needed to move the node by 1)
matrix_type
r
(
Kconst
.
getSize
(),
master
.
numNodes
()
+
1
);
// loop over stifness matrix dimension
for
(
size_t
i
=
0
;
i
<
Kconst
.
getSize
();
++
i
)
// loop over problem dimensions
for
(
int
j
=
0
;
j
<
Dim
;
++
j
)
// loop over nodes considered
for
(
size_t
k
=
0
;
k
<
master
.
numNodes
()
+
1
;
++
k
)
r
(
i
,
k
)
+=
Kconst
(
i
,
conn
[
k
]
+
j
)
*
nu
(
j
);
// get results (norm of each column in r)
vector_type
rsum
(
master
.
numNodes
()
+
1
);
for
(
size_t
i
=
0
;
i
<
rsum
.
size
();
++
i
)
for
(
size_t
j
=
0
;
j
<
r
.
rows
();
++
j
)
rsum
(
i
)
+=
r
(
j
,
i
)
*
r
(
j
,
i
);
// get average value as the penalty parameter
Real
epsilon
=
0.
;
for
(
size_t
i
=
0
;
i
<
rsum
.
size
();
++
i
)
epsilon
+=
sqrt
(
rsum
(
i
));
epsilon
/=
master
.
numNodes
()
+
1
;
penalty_
[
slave
]
=
epsilon
;
ave
+=
penalty_
[
slave
];
++
k
;
}
// dummy master
else
{
// carry out stiffness multiplication with the normal
// the product Kij*nj would give the force for a unit displacement
// (i.e., the stiffness needed to move the node by 1)
vector_type
r
(
Kconst
.
getSize
());
// loop over stifness matrix dimension
for
(
size_t
i
=
0
;
i
<
Kconst
.
getSize
();
++
i
)
// loop over problem dimensions
for
(
int
j
=
0
;
j
<
Dim
;
++
j
)
// loop over nodes considered
r
(
i
)
+=
Kconst
(
i
,
slave
+
j
)
*
1.
/
Dim
;
// get results (norm of each column in r)
Real
epsilon
=
0
;
for
(
size_t
i
=
0
;
i
<
r
.
size
();
++
i
)
epsilon
+=
r
(
i
)
*
r
(
i
);
epsilon
=
sqrt
(
epsilon
);
penalty_
[
slave
]
=
epsilon
;
ave
+=
penalty_
[
slave
];
++
k
;
}
}
cout
<<
"Average value: "
<<
(
*
this
)[
Alpha
]
*
ave
/
k
<<
endl
;
}
template
<
int
dim
>
struct
TangentTraits
;
template
<>
struct
TangentTraits
<
2
>
{
constexpr
static
UInt
dim
=
2
;
constexpr
static
ElementType
master_type
=
_segment_2
;
constexpr
static
InterpolationType
interpolation_type
=
_itp_lagrange_segment_2
;
typedef
Point
<
dim
>
point_type
;
typedef
array
::
Array
<
1
,
Real
>
vector_type
;
typedef
array
::
Array
<
2
,
Real
>
matrix_type
;
typedef
SolidMechanicsModel
model_type
;
template
<
class
element_type
>
static
bool
projects
(
const
point_type
&
s
,
const
element_type
&
master
,
const
Array
<
Real
>
&
position
)
{
return
has_projection
(
s
,
point_type
(
&
position
(
master
.
node
(
0
))),
point_type
(
&
position
(
master
.
node
(
1
))));
}
template
<
class
real_tuple
,
class
element_type
,
class
vector_type
>
static
std
::
tuple
<
matrix_type
,
vector_type
>
computeTangentAndResidual
(
model_type
&
model
,
real_tuple
t
,
element_type
&
master
,
const
vector_type
&
sh
,
const
matrix_type
&
dsh
,
const
vector_type
&
N
)
{
const
Array
<
Real
>
&
position
=
model
.
getCurrentPosition
();
Real
gap
=
std
::
get
<
0
>
(
t
);
Real
s1
=
std
::
get
<
1
>
(
t
);
Real
s2
=
std
::
get
<
2
>
(
t
);
// compute the point on the surface
point_type
a
(
&
position
(
master
.
node
(
0
)));
point_type
b
(
&
position
(
master
.
node
(
1
)));
vector_type
nu
=
master
.
normal
();
// compute vector T
point_type
tau
=
dsh
(
0
,
0
)
*
a
+
dsh
(
0
,
1
)
*
b
;
vector_type
T
(
dim
*
(
master
.
numNodes
()
+
1
));
for
(
UInt
i
=
0
;
i
<
dim
;
++
i
)
{
T
[
i
]
=
tau
[
i
];
for
(
UInt
j
=
0
;
j
<
master
.
numNodes
();
++
j
)
T
[(
1
+
j
)
*
dim
+
i
]
=
-
tau
[
i
]
*
sh
[
j
];
}
// compute N1
vector_type
N1
(
dim
*
(
master
.
numNodes
()
+
1
));
for
(
UInt
i
=
0
;
i
<
dim
;
++
i
)
{
for
(
UInt
j
=
0
;
j
<
master
.
numNodes
();
++
j
)
N1
[(
1u
+
j
)
*
dim
+
i
]
=
-
nu
[
i
]
*
dsh
(
0u
,
j
);
}
// compute m11
Real
m11
=
tau
*
tau
;
// compute D1
vector_type
D1
=
T
+
gap
*
N1
;
D1
*=
1.
/
m11
;
// Note: N1bar = N1 - k11*D1, but since k11 = 0 for 2D, then
// N1bar = N1
vector_type
&
N1bar
=
N1
;
// stiffness matrix (only non-zero terms for 2D implementation)
matrix_type
kc
=
s1
*
N
*
transpose
(
N
);
// first term
kc
+=
(
s2
*
gap
*
m11
)
*
N1bar
*
transpose
(
N1bar
);
// second term
kc
-=
s2
*
D1
*
transpose
(
N1
);
// sixth term
kc
-=
s2
*
N1
*
transpose
(
D1
);
// eight term
// residual vector
vector_type
fc
=
s2
*
N
;
assert
(
kc
.
rows
()
==
fc
.
size
());
return
std
::
make_tuple
(
kc
,
fc
);
}
template
<
class
element_type
>
static
std
::
tuple
<
point_type
,
vector_type
>
compute_projection
(
const
point_type
&
s
,
element_type
&
master
)
{
Distance_minimizator
<
dim
,
master_type
>
dm
(
s
,
master
.
coordinates
());
vector_type
xi
(
1
,
dm
.
master_coordinates
()[
0
]);
return
std
::
make_tuple
(
dm
.
point
(),
xi
);
}
};
template
<>
struct
TangentTraits
<
3
>
{
constexpr
static
UInt
dim
=
3
;
constexpr
static
ElementType
master_type
=
_triangle_3
;
constexpr
static
InterpolationType
interpolation_type
=
_itp_lagrange_triangle_3
;
typedef
Point
<
dim
>
point_type
;
typedef
array
::
Array
<
1
,
Real
>
vector_type
;
typedef
array
::
Array
<
2
,
Real
>
matrix_type
;
typedef
SolidMechanicsModel
model_type
;
template
<
class
element_type
>
static
bool
projects
(
const
point_type
&
s
,
const
element_type
&
master
,
const
Array
<
Real
>
&
position
)
{
return
point_has_projection_to_triangle
(
s
,
point_type
(
&
position
(
master
.
node
(
0
))),
point_type
(
&
position
(
master
.
node
(
1
))),
point_type
(
&
position
(
master
.
node
(
2
))));
}
template
<
class
real_tuple
,
class
element_type
,
class
vector_type
>
static
std
::
tuple
<
matrix_type
,
vector_type
>
computeTangentAndResidual
(
model_type
&
model
,
real_tuple
t
,
element_type
&
master
,
const
vector_type
&
sh
,
const
matrix_type
&
dsh
,
const
vector_type
&
N
)
{
const
Array
<
Real
>
&
position
=
model
.
getCurrentPosition
();
Real
gap
=
std
::
get
<
0
>
(
t
);
Real
s1
=
std
::
get
<
1
>
(
t
);
Real
s2
=
std
::
get
<
2
>
(
t
);
Real
s3
=
std
::
get
<
3
>
(
t
);
// compute the point on the surface
point_type
a
(
&
position
(
master
.
node
(
0
)));
point_type
b
(
&
position
(
master
.
node
(
1
)));
point_type
c
(
&
position
(
master
.
node
(
2
)));
vector_type
nu
=
master
.
normal
();
point_type
tau1
=
dsh
(
0
,
0
)
*
a
+
dsh
(
0
,
1
)
*
b
+
dsh
(
0
,
2
)
*
c
;
point_type
tau2
=
dsh
(
1
,
0
)
*
a
+
dsh
(
1
,
1
)
*
b
+
dsh
(
1
,
2
)
*
c
;
vector_type
nucheck
(
3
);
Math
::
vectorProduct3
(
&
tau1
[
0
],
&
tau2
[
0
],
&
nucheck
[
0
]);
Math
::
normalize3
(
&
nucheck
[
0
]);
if
((
nucheck
-
nu
)().
norm
()
>
1.0e-10
)
{
cout
<<
"*** ERROR *** Normal failed"
<<
endl
;
cout
<<
"nu1: "
<<
nu
<<
endl
;
cout
<<
"nu2: "
<<
nucheck
<<
endl
;
exit
(
1
);
}
// compute vectors T1, T2, N1, N2
size_t
vsize
=
dim
*
(
master
.
numNodes
()
+
1
);
vector_type
T1
(
vsize
),
T2
(
vsize
),
N1
(
vsize
),
N2
(
vsize
);
for
(
UInt
i
=
0
;
i
<
dim
;
++
i
)
{
T1
[
i
]
=
tau1
[
i
];
T2
[
i
]
=
tau2
[
i
];
for
(
UInt
j
=
0
;
j
<
master
.
numNodes
();
++
j
)
{
T1
[(
1
+
j
)
*
dim
+
i
]
=
-
tau1
[
i
]
*
sh
[
j
];
T2
[(
1
+
j
)
*
dim
+
i
]
=
-
tau2
[
i
]
*
sh
[
j
];
N1
[(
1
+
j
)
*
dim
+
i
]
=
-
nu
[
i
]
*
dsh
(
0u
,
j
);
N2
[(
1
+
j
)
*
dim
+
i
]
=
-
nu
[
i
]
*
dsh
(
1u
,
j
);
}
}
// compute matrix A = m + k*g (but kappa is zero for linear elements)
Real
A11
=
tau1
*
tau1
;
Real
A12
=
tau1
*
tau2
;
Real
A22
=
tau2
*
tau2
;
Real
detA
=
A11
*
A22
-
A12
*
A12
;
// compute vectors D1, D2
vector_type
D1
=
(
1
/
detA
)
*
(
A22
*
(
T1
+
gap
*
N1
)()
-
A12
*
(
T2
+
gap
*
N2
)())();
vector_type
D2
=
(
1
/
detA
)
*
(
A11
*
(
T2
+
gap
*
N2
)()
-
A12
*
(
T1
+
gap
*
N1
)())();
// Note: N1bar = N1 - k12*D2, but since k12 = 0 for linear elements, then
// N1bar = N1, N2bar = N2
vector_type
&
N1bar
=
N1
;
vector_type
&
N2bar
=
N2
;
// stiffness matrix (only non-zero terms for 3D implementation with linear
// elements)
// get covariant terms (det(A) = det(inv(A))
Real
m11
=
A22
/
detA
;
Real
m12
=
-
A12
/
detA
;
Real
m22
=
A11
/
detA
;
// 1st term:
// epsilon * Heaviside(lambda + epsilon gap) * N * N' = s1 * N * N'
matrix_type
kc
=
s1
*
N
*
transpose
(
N
);
// 2nd term:
// t_N * gap * m_11 * N1_bar * N1_bar', where t_N = <lambda + epsilon*gap>
kc
+=
(
s3
*
m11
)
*
N1bar
*
transpose
(
N1bar
);
// 3rd and 4th terms:
// t_N * gap * m_12 * (N1_bar * N2_bar' + N2_bar * N1_bar')
matrix_type
tmp
=
N1bar
*
transpose
(
N2bar
);
tmp
+=
N2bar
*
transpose
(
N1bar
);
kc
+=
(
s3
*
m12
)
*
tmp
;
// 5th term:
// t_N * gap * m_22 * N2_bar * N2_bar'
kc
+=
(
s3
*
m22
)
*
N2bar
*
transpose
(
N2bar
);
// 6th term:
// - t_N * D1 * N1'
kc
-=
s2
*
D1
*
transpose
(
N1
);
// 7th term:
// - t_N * D2 * N2'
kc
-=
s2
*
D2
*
transpose
(
N2
);
// 8th term:
// - t_N * N1 * D1'
kc
-=
s2
*
N1
*
transpose
(
D1
);
// 9th term:
// - t_N * N2 * D2'
kc
-=
s2
*
N2
*
transpose
(
D2
);
// residual vector
vector_type
fc
=
s2
*
N
;
assert
(
kc
.
rows
()
==
fc
.
size
());
return
std
::
make_tuple
(
kc
,
fc
);
}
//! Function template specialization for inversion of a \f$ 3 \times 3 \f$
// matrix.
template
<
class
matrix_type
>
static
std
::
pair
<
matrix_type
,
Real
>
invert
(
matrix_type
&
A
)
{
// obtain determinant of the matrix
Real
det
=
A
[
0
][
0
]
*
(
A
[
1
][
1
]
*
A
[
2
][
2
]
-
A
[
1
][
2
]
*
A
[
2
][
1
])
-
A
[
0
][
1
]
*
(
A
[
1
][
0
]
*
A
[
2
][
2
]
-
A
[
1
][
2
]
*
A
[
2
][
0
])
+
A
[
0
][
2
]
*
(
A
[
1
][
0
]
*
A
[
2
][
1
]
-
A
[
1
][
1
]
*
A
[
2
][
0
]);
// compute inverse
matrix_type
inv
(
3
,
3
,
1.
/
det
);
inv
[
0
][
0
]
*=
A
[
1
][
1
]
*
A
[
2
][
2
]
-
A
[
1
][
2
]
*
A
[
2
][
1
];
inv
[
0
][
1
]
*=
A
[
0
][
2
]
*
A
[
2
][
1
]
-
A
[
0
][
1
]
*
A
[
2
][
2
];
inv
[
0
][
2
]
*=
-
A
[
0
][
2
]
*
A
[
1
][
1
]
+
A
[
0
][
1
]
*
A
[
1
][
2
];
inv
[
1
][
0
]
*=
A
[
1
][
2
]
*
A
[
2
][
0
]
-
A
[
1
][
0
]
*
A
[
2
][
2
];
inv
[
1
][
1
]
*=
A
[
0
][
0
]
*
A
[
2
][
2
]
-
A
[
0
][
2
]
*
A
[
2
][
0
];
inv
[
1
][
2
]
*=
A
[
0
][
2
]
*
A
[
1
][
0
]
-
A
[
0
][
0
]
*
A
[
1
][
2
];
inv
[
2
][
0
]
*=
-
A
[
1
][
1
]
*
A
[
2
][
0
]
+
A
[
1
][
0
]
*
A
[
2
][
1
];
inv
[
2
][
1
]
*=
A
[
0
][
1
]
*
A
[
2
][
0
]
-
A
[
0
][
0
]
*
A
[
2
][
1
];
inv
[
2
][
2
]
*=
-
A
[
0
][
1
]
*
A
[
1
][
0
]
+
A
[
0
][
0
]
*
A
[
1
][
1
];
return
std
::
make_pair
(
inv
,
det
);
}
template
<
class
vector_type
,
class
point_type
>
static
vector_type
invert_map
(
const
point_type
&
s
,
const
point_type
&
a
,
const
point_type
&
b
,
const
point_type
&
c
)
{
typedef
array
::
Array
<
2
,
Real
>
matrix_type
;
// matrix for inverse
matrix_type
A
=
{
{
b
[
0
]
-
a
[
0
],
c
[
0
]
-
a
[
0
],
a
[
0
]
},
{
b
[
1
]
-
a
[
1
],
c
[
1
]
-
a
[
1
],
a
[
1
]
},
{
b
[
2
]
-
a
[
2
],
c
[
2
]
-
a
[
2
],
a
[
2
]
}
};
std
::
pair
<
matrix_type
,
Real
>
Ainv
=
invert
(
A
);
vector_type
x
=
{
s
[
0
],
s
[
1
],
s
[
2
]
};
vector_type
r1
=
Ainv
.
first
*
x
;
return
vector_type
{
r1
[
0
],
r1
[
1
]
};
// return only the first two components of r1
}
template
<
class
element_type
>
static
std
::
tuple
<
point_type
,
vector_type
>
compute_projection
(
const
point_type
&
s
,
element_type
&
master
)
{
auto
coord
=
master
.
coordinates
();
// compute the point on the surface
point_type
a
(
coord
[
0
]);
point_type
b
(
coord
[
1
]);
point_type
c
(
coord
[
2
]);
point_type
p
=
closest_point_to_triangle
(
s
,
a
,
b
,
c
);
vector_type
xi
=
invert_map
<
vector_type
,
point_type
>
(
p
,
a
,
b
,
c
);
// Distance_minimizator<dim, TangentTraits<dim>::master_type> dm(
// s, master.coordinates());
// xi = vector_type(dim - 1);
// for (int i = 0; i < dim - 1; ++i)
// xi[i] = dm.master_coordinates()[i];
// point_type p = dm.point();
return
std
::
make_tuple
(
p
,
xi
);
}
};
template
<
int
dim
>
bool
ContactResolution
<
dim
,
_static
,
_augmented_lagrangian
>::
computeTangentAndResidual
(
real_map
&
lambda_new
,
SearchBase
*
cp
,
Real
&
error
,
Int2Type
<
_uzawa
>
)
{
const
Array
<
Real
>
&
position
=
model_
.
getCurrentPosition
();
const
Real
tol
=
(
*
this
)[
Multiplier_tol
];
// get global stiffness matrix and force vector
SparseMatrix
&
K
=
model_
.
getStiffnessMatrix
();
Array
<
Real
>
&
F
=
model_
.
getResidual
();
const
Array
<
Int
>
&
eqnum
=
model_
.
getDOFSynchronizer
().
getLocalDOFEquationNumbers
();
static
bool
auto_flag
=
true
;
if
(
auto_flag
)
{
auto_flag
=
false
;
if
(
!
(
*
this
)[
Automatic_penalty_parameter
])
{
Real
epsilon
=
(
*
this
)[
Epsilon
];
for
(
auto
it
=
sm_
.
begin
();
it
!=
sm_
.
end
();
++
it
)
penalty_
[
it
->
first
]
=
epsilon
;
cout
<<
"*** INFO *** Uniform penalty parameter used for all slaves: "
<<
epsilon
<<
endl
;
;
}
// else get penalty values automatically
else
getPenaltyValues
();
}
Real
lm_diff
=
0
;
Real
lm_max
=
0
;
auto
it
=
sm_
.
begin
();
while
(
it
!=
sm_
.
end
())
{
auto
slave
=
it
->
first
;
Real
epsilon
=
(
*
this
)[
Alpha
]
*
penalty_
[
slave
];
AKANTU_DEBUG_ASSERT
(
epsilon
!=
0
,
"Penalty value cannot be zero"
);
// get slave point
point_type
s
(
&
position
(
slave
));
auto
master
=
it
->
second
;
bool
no_master
=
master
==
element_type
();
// if node lies outside triangle
if
(
no_master
||
!
TangentTraits
<
dim
>::
projects
(
s
,
master
,
position
))
{
auto
r
=
cp
->
search
(
&
position
(
slave
));
// try to find a new master
if
(
r
!=
-
1
)
{
it
->
second
=
master
=
element_type
(
model_
,
TangentTraits
<
dim
>::
master_type
,
r
);
}
// else remove master-slave pair from simulation
else
{
master
=
element_type
();
gaps_
.
erase
(
slave
);
lambda_new
.
erase
(
slave
);
++
it
;
continue
;
}
}
assert
(
master
.
type
==
TangentTraits
<
dim
>::
master_type
);
Distance_minimizator
<
dim
,
TangentTraits
<
dim
>::
master_type
>
dm
(
s
,
master
.
coordinates
());
vector_type
xi
=
vector_type
(
dim
-
1
);
for
(
int
i
=
0
;
i
<
dim
-
1
;
++
i
)
xi
[
i
]
=
dm
.
master_coordinates
()[
i
];
point_type
p
=
dm
.
point
();
// compute normal
vector_type
nu
=
master
.
normal
();
point_type
nup
(
static_cast
<
const
Real
*>
(
nu
.
data
()));
// compute and save gap
Real
gap
=
-
(
nup
*
(
s
-
p
));
gaps_
[
slave
]
=
gap
;
Real
lambda_hat
=
multipliers_
[
slave
]
+
epsilon
*
gap
;
if
(
lambda_hat
<
0
)
{
// increase iterator
++
it
;
// save value of lambda
lambda_new
[
slave
]
=
0
;
continue
;
}
Real
s1
=
epsilon
*
Heaviside
(
lambda_hat
);
Real
s2
=
Macauley
(
lambda_hat
);
// max(0,lambda_hat)
Real
s3
=
s2
*
gap
;
std
::
vector
<
UInt
>
conn
(
master
.
numNodes
()
+
1
);
// 1 slave (not hardcoded)
conn
[
0
]
=
slave
;
for
(
UInt
i
=
0
;
i
<
master
.
numNodes
();
++
i
)
conn
[
1
+
i
]
=
master
.
node
(
i
);
// evaluate shape functions at slave master coordinate
vector_type
sh
(
master
.
numNodes
());
InterpolationElement
<
TangentTraits
<
dim
>::
interpolation_type
>::
computeShapes
(
xi
,
sh
);
// compute vector N
vector_type
N
(
dim
*
(
master
.
numNodes
()
+
1
));
for
(
UInt
i
=
0
;
i
<
dim
;
++
i
)
{
N
[
i
]
=
nu
[
i
];
for
(
UInt
j
=
0
;
j
<
master
.
numNodes
();
++
j
)
N
[(
1
+
j
)
*
dim
+
i
]
=
-
nu
[
i
]
*
sh
[
j
];
}
matrix_type
dsh
(
dim
-
1
,
master
.
numNodes
());
InterpolationElement
<
TangentTraits
<
dim
>::
interpolation_type
>::
computeDNDS
(
xi
,
dsh
);
// obtain contribution to stiffness matrix and force vector depending on
// the dimension
auto
t
=
TangentTraits
<
dim
>::
computeTangentAndResidual
(
model_
,
std
::
make_tuple
(
gap
,
s1
,
s2
,
s3
),
master
,
sh
,
dsh
,
N
);
matrix_type
&
kc
=
std
::
get
<
0
>
(
t
);
vector_type
&
fc
=
std
::
get
<
1
>
(
t
);
// assemble local components into global matrix and vector
std
::
vector
<
UInt
>
eq
;
for
(
UInt
i
=
0
;
i
<
conn
.
size
();
++
i
)
for
(
UInt
j
=
0
;
j
<
dim
;
++
j
)
eq
.
push_back
(
eqnum
(
conn
[
i
]
*
dim
+
j
));
for
(
UInt
i
=
0
;
i
<
kc
.
rows
();
++
i
)
{
F
[
eq
[
i
]]
+=
fc
(
i
);
for
(
UInt
j
=
i
;
j
<
kc
.
columns
();
++
j
)
{
K
.
addToProfile
(
eq
[
i
],
eq
[
j
]);
K
(
eq
[
i
],
eq
[
j
])
+=
kc
(
i
,
j
);
}
}
// update multiplier
lambda_new
[
slave
]
=
s2
;
Real
lm_old
=
multipliers_
[
slave
];
lm_max
+=
lm_old
*
lm_old
;
lm_old
-=
s2
;
lm_diff
+=
lm_old
*
lm_old
;
// increase iterator
++
it
;
}
if
(
lm_max
<
tol
)
{
error
=
sqrt
(
lm_diff
);
return
sqrt
(
lm_diff
)
<
tol
;
}
error
=
sqrt
(
lm_diff
/
lm_max
);
return
sqrt
(
lm_diff
/
lm_max
)
<
tol
;
}
template
<
typename
T
>
Point
<
2
,
T
>
closest_point_to_triangle
(
const
Point
<
2
,
T
>
&
p
,
const
Point
<
2
,
T
>
&
a
,
const
Point
<
2
,
T
>
&
b
,
const
Point
<
2
,
T
>
&
c
)
{
return
Point
<
2
,
T
>
();
}
template
<
int
dim
>
bool
ContactResolution
<
dim
,
_static
,
_augmented_lagrangian
>::
computeTangentAndResidual
(
Array
<
Real
>
&
solution
,
Array
<
Real
>
&
F
,
SearchBase
*
cp
,
Real
&
error
,
Int2Type
<
_generalized_newton
>
)
{
const
Array
<
Real
>
&
position
=
model_
.
getCurrentPosition
();
// get global stiffness matrix and force vector
SparseMatrix
&
K
=
model_
.
getStiffnessMatrix
();
const
Array
<
Int
>
&
eqnum
=
model_
.
getDOFSynchronizer
().
getLocalDOFEquationNumbers
();
static
bool
auto_flag
=
true
;
if
(
auto_flag
)
{
auto_flag
=
false
;
if
(
!
(
*
this
)[
Automatic_penalty_parameter
])
{
Real
epsilon
=
(
*
this
)[
Epsilon
];
for
(
auto
it
=
sm_
.
begin
();
it
!=
sm_
.
end
();
++
it
)
penalty_
[
it
->
first
]
=
epsilon
;
cout
<<
"*** INFO *** Uniform penalty parameter used for all slaves: "
<<
epsilon
<<
endl
;
;
}
// else get penalty values automatically
else
getPenaltyValues
();
}
// size of original system
UInt
original
=
model_
.
increment
->
getSize
()
*
dim
;
// multiplier count
size_t
kk
=
0
;
auto
it
=
sm_
.
begin
();
while
(
it
!=
sm_
.
end
())
{
auto
slave
=
it
->
first
;
Real
epsilon
=
(
*
this
)[
Alpha
]
*
penalty_
[
slave
];
if
(
status_change_
[
slave
]
!=
0
)
{
;
epsilon
*=
(
status_change_
[
slave
]
+
1.
);
}
AKANTU_DEBUG_ASSERT
(
epsilon
!=
0
,
"Penalty value cannot be zero"
);
// get slave point
point_type
s
(
&
position
(
slave
));
auto
master
=
it
->
second
;
bool
no_master
=
master
==
element_type
();
static
std
::
map
<
UInt
,
bool
>
excluded
;
// if node lies outside triangle
if
(
no_master
||
!
TangentTraits
<
dim
>::
projects
(
s
,
master
,
position
))
{
auto
r
=
cp
->
search
(
&
position
(
slave
));
// try to find a new master
if
(
r
!=
-
1
)
{
it
->
second
=
master
=
element_type
(
model_
,
TangentTraits
<
dim
>::
master_type
,
r
);
no_master
=
false
;
}
// else remove master-slave pair from simulation
else
{
master
=
element_type
();
no_master
=
true
;
excluded
[
slave
]
=
true
;
}
}
Real
gap
;
vector_type
xi
;
if
(
!
no_master
)
{
assert
(
master
.
type
==
TangentTraits
<
dim
>::
master_type
);
auto
tuple
=
TangentTraits
<
dim
>::
compute_projection
(
s
,
master
);
point_type
&
p
=
std
::
get
<
0
>
(
tuple
);
xi
=
std
::
get
<
1
>
(
tuple
);
// compute normal
vector_type
nu
=
master
.
normal
();
point_type
nup
(
static_cast
<
const
Real
*>
(
nu
.
data
()));
// Real old_gap = gaps_[slave];
// compute and save gap
gap
=
-
(
nup
*
(
s
-
p
));
gaps_
[
slave
]
=
gap
;
// track status
// if node in contact
if
(
contact_status_
[
slave
])
{
if
(
gap
<
-
1.e-10
)
{
contact_status_
[
slave
]
=
false
;
++
status_change_
[
slave
];
// cout<<"["<<status_change_[slave]<<"] changing to
// non-contact status for node "<<slave<<". Gap from "<<old_gap<<" to
// "<<gap<<endl;
}
}
else
{
if
(
gap
>=
-
1.e-10
)
{
contact_status_
[
slave
]
=
true
;
++
status_change_
[
slave
];
// cout<<"["<<status_change_[slave]<<"] changing to contact
// status for node "<<slave<<". Gap from "<<old_gap<<" to
// "<<gap<<endl;
}
}
}
Real
lambda_hat
=
multipliers_
[
slave
]
+
epsilon
*
gap
;
// no contact
if
(
lambda_hat
<
0
||
excluded
[
slave
])
{
size_t
ii
=
original
+
kk
;
// add contribution to stiffness matrix and residual vector
F
[
ii
]
=
multipliers_
[
slave
]
/
epsilon
;
K
.
addToProfile
(
ii
,
ii
);
K
(
ii
,
ii
)
+=
-
1
/
epsilon
;
}
// contact
else
{
Real
s1
=
epsilon
*
Heaviside
(
lambda_hat
);
Real
s2
=
Macauley
(
lambda_hat
);
// max(0,lambda_hat)
Real
s3
=
s2
*
gap
;
std
::
vector
<
UInt
>
conn
(
master
.
numNodes
()
+
1
);
// 1 slave (not hardcoded)
conn
[
0
]
=
slave
;
for
(
UInt
i
=
0
;
i
<
master
.
numNodes
();
++
i
)
conn
[
1
+
i
]
=
master
.
node
(
i
);
// evaluate shape functions at slave master coordinate
vector_type
nu
=
master
.
normal
();
vector_type
sh
(
master
.
numNodes
());
InterpolationElement
<
TangentTraits
<
dim
>::
interpolation_type
>::
computeShapes
(
xi
,
sh
);
// compute vector N
vector_type
N
(
dim
*
(
master
.
numNodes
()
+
1
));
for
(
UInt
i
=
0
;
i
<
dim
;
++
i
)
{
N
[
i
]
=
nu
[
i
];
for
(
UInt
j
=
0
;
j
<
master
.
numNodes
();
++
j
)
N
[(
1
+
j
)
*
dim
+
i
]
=
-
nu
[
i
]
*
sh
[
j
];
}
matrix_type
dsh
(
dim
-
1
,
master
.
numNodes
());
InterpolationElement
<
TangentTraits
<
dim
>::
interpolation_type
>::
computeDNDS
(
xi
,
dsh
);
// obtain contribution to stiffness matrix and force vector depending on
// the dimension
auto
t
=
TangentTraits
<
dim
>::
computeTangentAndResidual
(
model_
,
std
::
make_tuple
(
gap
,
s1
,
s2
,
s3
),
master
,
sh
,
dsh
,
N
);
matrix_type
&
kc
=
std
::
get
<
0
>
(
t
);
vector_type
&
fc
=
std
::
get
<
1
>
(
t
);
Array
<
bool
>
&
boundary
=
model_
.
getBlockedDOFs
();
// assemble local components into global matrix and vector not taking into
// account fixed dofs
std
::
vector
<
UInt
>
eq
(
conn
.
size
()
*
dim
);
std
::
vector
<
bool
>
fixed
(
conn
.
size
()
*
dim
,
false
);
for
(
UInt
i
=
0
;
i
<
conn
.
size
();
++
i
)
for
(
UInt
j
=
0
;
j
<
dim
;
++
j
)
{
eq
.
at
(
i
*
dim
+
j
)
=
eqnum
(
conn
[
i
]
*
dim
+
j
);
fixed
.
at
(
i
*
dim
+
j
)
=
boundary
(
conn
[
i
],
j
);
}
for
(
UInt
i
=
0
;
i
<
kc
.
rows
();
++
i
)
{
// if dof is blocked, don't add terms
if
(
fixed
.
at
(
i
))
continue
;
F
[
eq
[
i
]]
+=
fc
(
i
);
for
(
UInt
j
=
i
;
j
<
kc
.
columns
();
++
j
)
{
K
.
addToProfile
(
eq
[
i
],
eq
[
j
]);
K
(
eq
[
i
],
eq
[
j
])
+=
kc
(
i
,
j
);
}
}
// terms corresponding to lagrangian multiplier contribution
size_t
ii
=
original
+
kk
;
// assemble contribution to force vector
F
[
ii
]
=
-
gap
;
// assemble contribution to stiffness matrix (only upper-triangular)
for
(
UInt
i
=
0
;
i
<
N
.
size
();
++
i
)
{
K
.
addToProfile
(
eq
[
i
],
ii
);
K
(
eq
[
i
],
ii
)
-=
N
[
i
];
}
}
// increment multiplier counter
++
kk
;
// increase iterator
++
it
;
}
return
true
;
}
template
<
int
Dim
>
std
::
ostream
&
operator
<<
(
std
::
ostream
&
os
,
const
ContactResolution
<
Dim
,
_static
,
_augmented_lagrangian
>
&
cr
)
{
typedef
typename
ContactResolution
<
Dim
,
_static
,
_augmented_lagrangian
>::
element_type
element_type
;
os
<<
"Augmented-Lagrangian resolution type. Parameters:"
<<
endl
;
if
(
cr
[
Automatic_penalty_parameter
])
cout
<<
"
\t
penalty = auto"
<<
endl
;
else
cout
<<
"
\t
penalty = "
<<
cr
[
Epsilon
]
<<
endl
;
cout
<<
"
\t
alpha = "
<<
cr
[
Alpha
]
<<
endl
;
cout
<<
"
\t
utol = "
<<
cr
[
Multiplier_tol
]
<<
endl
;
cout
<<
"
\t
ntol = "
<<
cr
[
Newton_tol
]
<<
endl
;
cout
<<
"
\t
usteps = "
<<
cr
[
Multiplier_max_steps
]
<<
endl
;
cout
<<
"
\t
nsteps = "
<<
cr
[
Newton_max_steps
]
<<
endl
;
cout
<<
"
\t
verbose = "
<<
cr
[
Verbose
]
<<
endl
;
cout
<<
"
\n
Slave nodes: "
;
for
(
auto
it
=
cr
.
sm_
.
begin
();
it
!=
cr
.
sm_
.
end
();
++
it
)
os
<<
it
->
first
<<
" "
;
os
<<
endl
;
// loop over pairs
cout
<<
"
\n
Slave master pairs"
<<
endl
;
for
(
auto
it
=
cr
.
sm_
.
begin
();
it
!=
cr
.
sm_
.
end
();
++
it
)
{
auto
slave
=
it
->
first
;
auto
master
=
it
->
second
;
os
<<
"
\t
slave: "
<<
slave
<<
", Master: "
;
if
(
master
==
element_type
())
os
<<
"none"
<<
endl
;
else
os
<<
master
<<
endl
;
}
return
os
;
}
template
std
::
ostream
&
operator
<<
(
std
::
ostream
&
,
const
ContactResolution
<
2
,
_static
,
_augmented_lagrangian
>
&
);
template
std
::
ostream
&
operator
<<
(
std
::
ostream
&
,
const
ContactResolution
<
3
,
_static
,
_augmented_lagrangian
>
&
);
template
class
ContactResolution
<
2
,
_static
,
_augmented_lagrangian
>
;
template
class
ContactResolution
<
3
,
_static
,
_augmented_lagrangian
>
;
__END_AKANTU__
Event Timeline
Log In to Comment