void lcp_nsgs_SBM_buildLocalProblem(int rowNumber, const SparseBlockStructuredMatrix* const blmat, LinearComplementarityProblem* local_problem, double* q, double* z)
{
assert(blmat->blocksize0[rowNumber] > 0);
/* Position in vector blmat->block of the required diagonal block */
int diagPos = getDiagonalBlockPos(blmat, rowNumber);
/* Gets diagonal block = MLocal */
local_problem->M->matrix0 = blmat->block[diagPos];
local_problem->size = blmat->blocksize0[rowNumber];
int pos = 0;
if (rowNumber != 0)
{
local_problem->size -= blmat->blocksize0[rowNumber - 1];
pos = blmat->blocksize0[rowNumber - 1];
}
local_problem->M->size0 = local_problem->size;
local_problem->M->size1 = local_problem->size;
/* Computes qLocal */
/* qLocal = q[rowNumber] + sum (rowM.z),
sum over all non-diagonal blocks, rowM being the current
row of blocks of M
*/
cblas_dcopy_msan(local_problem->size, &q[pos], 1, local_problem->q, 1);
rowProdNoDiagSBM(blmat->blocksize0[blmat->blocknumber0 - 1], local_problem->size, rowNumber, blmat, z, local_problem->q, 0);
}
void getDiagonalBlock(NumericsMatrix* m, int numBlockRow, int numRow, int size, double ** Bout)
{
int storageType = m->storageType;
if (storageType == 0)
{
double * MM = m->matrix0;
double * elem = 0;
/* The part of MM which corresponds to the current block is copied into MLocal */
for (int i = 0; i < size; i++)
{
elem = MM + numRow + (numRow + i) * (m->size0);
for (int j = 0; j < size; j++)
{
(*Bout)[j + i * size] = *elem;
elem++;
}
}
}
else if (storageType == 1)
{
int diagPos = getDiagonalBlockPos(m->matrix1, numBlockRow);
(*Bout) = m->matrix1->block[diagPos];
}
}
void AC_fillMLocal(FrictionContactProblem * problem, FrictionContactProblem * localproblem, int contact)
{
NumericsMatrix * MGlobal = problem->M;
int n = 3 * problem->numberOfContacts;
// Dense storage
int storageType = MGlobal->storageType;
if (storageType == 0)
{
int in = 3 * contact, it = in + 1, is = it + 1;
int inc = n * in;
double * MM = MGlobal->matrix0;
double * MLocal = localproblem->M->matrix0;
/* The part of MM which corresponds to the current block is copied into MLocal */
MLocal[0] = MM[inc + in];
MLocal[1] = MM[inc + it];
MLocal[2] = MM[inc + is];
inc += n;
MLocal[3] = MM[inc + in];
MLocal[4] = MM[inc + it];
MLocal[5] = MM[inc + is];
inc += n;
MLocal[6] = MM[inc + in];
MLocal[7] = MM[inc + it];
MLocal[8] = MM[inc + is];
}
else if (storageType == 1)
{
int diagPos = getDiagonalBlockPos(MGlobal->matrix1, contact);
localproblem->M->matrix0 = MGlobal->matrix1->block[diagPos];
/* cblas_dcopy(9, MGlobal->matrix1->block[diagPos], 1, localproblem->M->matrix0 , 1); */
}
else
numericsError("FrictionContact3D_AlartCurnier:AC_fillMLocal() -", "unknown storage type for matrix M");
}
void frictionContact3D_projection_update_with_regularization(int contact, FrictionContactProblem * problem, FrictionContactProblem * localproblem, double* reaction, SolverOptions* options)
{
/* Build a local problem for a specific contact
reaction corresponds to the global vector (size n) of the global problem.
*/
/* Call the update function which depends on the storage for MGlobal/MBGlobal */
/* Build a local problem for a specific contact
reaction corresponds to the global vector (size n) of the global problem.
*/
/* The part of MGlobal which corresponds to the current block is copied into MLocal */
NumericsMatrix * MGlobal = problem->M;
int n = 3 * problem->numberOfContacts;
int storageType = MGlobal->storageType;
if (storageType == 0)
// Dense storage
{
int in = 3 * contact, it = in + 1, is = it + 1;
int inc = n * in;
double * MM = MGlobal->matrix0;
double * MLocal = localproblem->M->matrix0;
/* The part of MM which corresponds to the current block is copied into MLocal */
MLocal[0] = MM[inc + in];
MLocal[1] = MM[inc + it];
MLocal[2] = MM[inc + is];
inc += n;
MLocal[3] = MM[inc + in];
MLocal[4] = MM[inc + it];
MLocal[5] = MM[inc + is];
inc += n;
MLocal[6] = MM[inc + in];
MLocal[7] = MM[inc + it];
MLocal[8] = MM[inc + is];
}
else if (storageType == 1)
{
int diagPos = getDiagonalBlockPos(MGlobal->matrix1, contact);
/* for (int i =0 ; i< 3*3 ; i++) localproblem->M->matrix0[i] = MGlobal->matrix1->block[diagPos][i] ; */
cblas_dcopy(9, MGlobal->matrix1->block[diagPos], 1, localproblem->M->matrix0 , 1);
}
else
numericsError("FrictionContact3D_projection -", "unknown storage type for matrix M");
/**** Computation of qLocal = qBlock + sum over a row of blocks in MGlobal of the products MLocal.reactionBlock,
excluding the block corresponding to the current contact. ****/
frictionContact3D_nsgs_computeqLocal(problem, localproblem, reaction, contact);
double rho = options->dparam[3];
for (int i = 0 ; i < 3 ; i++) localproblem->M->matrix0[i + 3 * i] += rho ;
double *qLocal = localproblem->q;
int in = 3 * contact, it = in + 1, is = it + 1;
/* qLocal computation*/
qLocal[0] -= rho * reaction[in];
qLocal[1] -= rho * reaction[it];
qLocal[2] -= rho * reaction[is];
/* Friction coefficient for current block*/
localproblem->mu[0] = problem->mu[contact];
}
