void CArrayInt2D::SetRows(int iNumRows)
{
if (miWidth == 0)
{
miHeight = iNumRows;
}
else
{
if (iNumRows > miHeight)
{
if (miHeight > 0)
{
InsertRows(miHeight, iNumRows-miHeight);
}
else
{
InsertRows(0, iNumRows);
}
}
else if (iNumRows < miHeight)
{
RemoveRows(iNumRows, miHeight-iNumRows);
}
}
}
// ÑÑÑÑÑÑÑÑÑÑÑÑÑÑÑÑÑÑÑÑÑÑÑÑÑÑÑÑÑÑÑÑÑÑÑÑÑÑÑÑÑÑÑÑÑÑÑÑÑÑÑÑÑÑÑÑÑÑÑÑÑÑÑÑÑÑÑÑÑÑÑÑÑÑÑÑÑÑÑÑÑÑÑÑÑÑÑÑÑÑÑÑÑÑÑÑÑÑÑÑÑÑÑÑÑÑÑÑÑÑÑÑÑÑÑÑÑÑÑÑÑÑ
// ¥ RemoveElementFromTable /*e*/
// ÑÑÑÑÑÑÑÑÑÑÑÑÑÑÑÑÑÑÑÑÑÑÑÑÑÑÑÑÑÑÑÑÑÑÑÑÑÑÑÑÑÑÑÑÑÑÑÑÑÑÑÑÑÑÑÑÑÑÑÑÑÑÑÑÑÑÑÑÑÑÑÑÑÑÑÑÑÑÑÑÑÑÑÑÑÑÑÑÑÑÑÑÑÑÑÑÑÑÑÑÑÑÑÑÑÑÑÑÑÑÑÑÑÑÑÑÑÑÑÑÑÑ
// Removes an element from the table. It must be in the hierarchy and in the table.
void CHierarchicalTable::RemoveElementFromTable(
CHierarchicalElement *inElement,
CHierarchicalGroup::TVisibilityOperation inOp)
{
STableCell cellPos;
CalcInsertPosition(inElement,cellPos);
ArrayIndexT numRowsToRemove=0;
switch (inOp)
{
case CHierarchicalGroup::kHideHeaderElementOnly:
case CHierarchicalGroup::kHideHeaderAndSubGroup:
if (inElement->IsInTable())
{
numRowsToRemove=1;
RemoveFromTable(inElement);
}
if (inOp==CHierarchicalGroup::kHideHeaderElementOnly)
break;
else
;// Fall through...
case CHierarchicalGroup::kHideSubGroupOnly:
CHierarchicalGroup *group=dynamic_cast<CHierarchicalGroup*>(inElement);
if (group && group->GetSubList())
{
CHierListIndexerT<CHierarchicalElement> indexer(group->GetSubList());
CHierarchicalElement *element;
while (element=indexer.GetNextData())
{
if (element->IsInTable())
{
numRowsToRemove++;
RemoveFromTable(element);
}
}
}
break;
}
if (numRowsToRemove)
{
// If only removing the sub group then start an element lower
if (inOp==CHierarchicalGroup::kHideSubGroupOnly && inElement->IsInTable())
cellPos.row++;
RemoveRows(numRowsToRemove,cellPos.row,true);
}
}
int CentralDifferencesSparse::DoTimestep()
{
PerformanceCounter counterForceAssemblyTime;
forceModel->GetInternalForce(q, internalForces);
for (int i=0; i<r; i++)
internalForces[i] *= internalForceScalingFactor;
counterForceAssemblyTime.StopCounter();
forceAssemblyTime = counterForceAssemblyTime.GetElapsedTime();
if (tangentialDampingMode > 0)
if (timestepIndex % tangentialDampingMode == 0)
DecomposeSystemMatrix(); // this routines also updates the damping and system matrices
// update equation is (see WRIGGERS P.: Computational Contact Mechanics. John Wiley & Sons, Ltd., 2002., page 275) :
//
// (M + dt / 2 * C) * q(t+1) = (dt)^2 * (fext(t) - fint(q(t))) + dt / 2 * C * q(t-1) + M * (2q(t) - q(t-1))
//
// (M + dt / 2 * C) * (q(t+1) - q(t)) = (dt)^2 * (fext(t) - fint(q(t))) + dt / 2 * C * (q(t-1) - q(t)) + M * (q(t) - q(t-1))
// fext are the external forces
// fint is the vector of internal forces
// compute rhs = (dt)^2 * (fext - fint(q(t))) + dt / 2 * C * (q(t-1) - q(t)) + M * (q(t) - q(t-1))
// first, compute rhs = M * (q - q_1)
for (int i=0; i<r; i++)
buffer[i] = q[i] - q_1[i];
massMatrix->MultiplyVector(buffer, rhs);
// rhs += dt / 2 * dampingMatrix * (q_{n-1} - q_n)
for (int i=0; i<r; i++)
qdelta[i] = q_1[i] - q[i];
rayleighDampingMatrix->MultiplyVector(qdelta, buffer);
for (int i=0; i<r; i++)
rhs[i] += 0.5 * timestep * buffer[i];
// rhs += dt * dt * (fext - fint(q(t)))
double timestep2 = timestep * timestep;
for (int i=0; i<r; i++)
rhs[i] += timestep2 * (externalForces[i] - internalForces[i]);
// now rhs contains the correct value
RemoveRows(r, rhsConstrained, rhs, numConstrainedDOFs, constrainedDOFs);
PerformanceCounter counterSystemSolveTime;
memset(buffer, 0, sizeof(double) * r);
#ifdef SPOOLES
int info = spoolesSolver->SolveLinearSystem(buffer, rhsConstrained);
char solverString[16] = "SPOOLES";
#endif
#ifdef PARDISO
int info = pardisoSolver->SolveLinearSystem(buffer, rhsConstrained);
char solverString[16] = "PARDISO";
#endif
#ifdef PCG
int info = jacobiPreconditionedCGSolver->SolveLinearSystemWithJacobiPreconditioner(buffer, rhsConstrained, 1e-6, 10000);
if (info > 0)
info = 0;
char solverString[16] = "PCG";
#endif
InsertRows(r, buffer, qdelta, numConstrainedDOFs, constrainedDOFs);
counterSystemSolveTime.StopCounter();
systemSolveTime = counterSystemSolveTime.GetElapsedTime();
if (info != 0)
{
printf("Error: %s sparse solver returned non-zero exit status %d.\n", solverString, (int)info);
return 1;
}
// the new value of q is now in buffer
// update velocity, and previous and current positions
for (int i=0; i<r; i++)
{
q_1[i] = q[i];
qvel[i] = qdelta[i] / timestep;
qaccel[i] = (qvel[i] - qvel_1[i]) / timestep;
qvel_1[i] = qvel[i];
qaccel_1[i] = qaccel[i];
q[i] += qdelta[i];
}
timestepIndex++;
return 0;
}
int ImplicitNewmarkSparse::DoTimestep()
{
int numIter = 0;
double error0 = 0; // error after the first step
double errorQuotient;
// store current amplitudes and set initial guesses for qaccel, qvel
for(int i=0; i<r; i++)
{
q_1[i] = q[i];
qvel_1[i] = qvel[i];
qaccel_1[i] = qaccel[i];
qaccel[i] = alpha1 * (q[i] - q_1[i]) - alpha2 * qvel_1[i] - alpha3 * qaccel_1[i];
qvel[i] = alpha4 * (q[i] - q_1[i]) + alpha5 * qvel_1[i] + alpha6 * qaccel_1[i];
}
do
{
int i;
/*
printf("q:\n");
for(int i=0; i<r; i++)
printf("%G ", q[i]);
printf("\n");
printf("Internal forces:\n");
for(int i=0; i<r; i++)
printf("%G ", internalForces[i]);
printf("\n");
*/
PerformanceCounter counterForceAssemblyTime;
forceModel->GetForceAndMatrix(q, internalForces, tangentStiffnessMatrix);
counterForceAssemblyTime.StopCounter();
forceAssemblyTime = counterForceAssemblyTime.GetElapsedTime();
//tangentStiffnessMatrix->Print();
//tangentStiffnessMatrix->Save("K");
// scale internal forces
for(i=0; i<r; i++)
internalForces[i] *= internalForceScalingFactor;
*tangentStiffnessMatrix *= internalForceScalingFactor;
memset(qresidual, 0, sizeof(double) * r);
if (useStaticSolver)
{
// no operation
}
else
{
// build effective stiffness: add mass matrix and damping matrix to tangentStiffnessMatrix
tangentStiffnessMatrix->ScalarMultiply(dampingStiffnessCoef, rayleighDampingMatrix);
rayleighDampingMatrix->AddSubMatrix(dampingMassCoef, *massMatrix);
rayleighDampingMatrix->ScalarMultiplyAdd(alpha4, tangentStiffnessMatrix);
//*tangentStiffnessMatrix += alpha4 * *rayleighDampingMatrix;
tangentStiffnessMatrix->AddSubMatrix(alpha4, *dampingMatrix, 1);
tangentStiffnessMatrix->AddSubMatrix(alpha1, *massMatrix);
// compute force residual, store it into aux variable qresidual
// qresidual = M * qaccel + C * qvel - externalForces + internalForces
massMatrix->MultiplyVector(qaccel, qresidual);
rayleighDampingMatrix->MultiplyVectorAdd(qvel, qresidual);
dampingMatrix->MultiplyVectorAdd(qvel, qresidual);
}
// add externalForces, internalForces
for(i=0; i<r; i++)
{
qresidual[i] += internalForces[i] - externalForces[i];
qresidual[i] *= -1;
qdelta[i] = qresidual[i];
}
/*
printf("internal forces:\n");
for(int i=0; i<r; i++)
printf("%G ", internalForces[i]);
printf("\n");
printf("external forces:\n");
for(int i=0; i<r; i++)
printf("%G ", externalForces[i]);
printf("\n");
printf("residual:\n");
for(int i=0; i<r; i++)
printf("%G ", -qresidual[i]);
printf("\n");
*/
double error = 0;
for(i=0; i<r; i++)
error += qresidual[i] * qresidual[i];
// on the first iteration, compute initial error
if (numIter == 0)
{
error0 = error;
errorQuotient = 1.0;
}
else
{
// error divided by the initial error, before performing this iteration
errorQuotient = error / error0;
}
if (errorQuotient < epsilon * epsilon)
{
break;
}
//tangentStiffnessMatrix->Save("Keff");
RemoveRows(r, bufferConstrained, qdelta, numConstrainedDOFs, constrainedDOFs);
systemMatrix->AssignSuperMatrix(tangentStiffnessMatrix);
// solve: systemMatrix * buffer = bufferConstrained
PerformanceCounter counterSystemSolveTime;
memset(buffer, 0, sizeof(double) * r);
#ifdef SPOOLES
SPOOLESSolver solver(systemMatrix);
int info = solver.SolveLinearSystem(buffer, bufferConstrained);
char solverString[16] = "SPOOLES";
#endif
#ifdef PARDISO
int info = pardisoSolver->ComputeCholeskyDecomposition(systemMatrix);
if (info == 0)
info = pardisoSolver->SolveLinearSystem(buffer, bufferConstrained);
char solverString[16] = "PARDISO";
#endif
#ifdef PCG
int info = jacobiPreconditionedCGSolver->SolveLinearSystemWithJacobiPreconditioner(buffer, bufferConstrained, 1e-6, 10000);
if (info > 0)
info = 0;
char solverString[16] = "PCG";
#endif
if (info != 0)
{
printf("Error: %s sparse solver returned non-zero exit status %d.\n", solverString, (int)info);
return 1;
}
counterSystemSolveTime.StopCounter();
systemSolveTime = counterSystemSolveTime.GetElapsedTime();
InsertRows(r, buffer, qdelta, numConstrainedDOFs, constrainedDOFs);
/*
printf("qdelta:\n");
for(int i=0; i<r; i++)
printf("%G ", qdelta[i]);
printf("\n");
exit(1);
*/
// update state
for(i=0; i<r; i++)
{
q[i] += qdelta[i];
qaccel[i] = alpha1 * (q[i] - q_1[i]) - alpha2 * qvel_1[i] - alpha3 * qaccel_1[i];
qvel[i] = alpha4 * (q[i] - q_1[i]) + alpha5 * qvel_1[i] + alpha6 * qaccel_1[i];
}
for(int i=0; i<numConstrainedDOFs; i++)
q[constrainedDOFs[i]] = qvel[constrainedDOFs[i]] = qaccel[constrainedDOFs[i]] = 0.0;
numIter++;
}
while (numIter < maxIterations);
/*
printf("qvel:\n");
for(int i=0; i<r; i++)
printf("%G ", qvel[i]);
printf("\n");
printf("qaccel:\n");
for(int i=0; i<r; i++)
printf("%G ", qaccel[i]);
printf("\n");
*/
//printf("Num iterations performed: %d\n",numIter);
//if ((numIter >= maxIterations) && (maxIterations > 1))
//{
//printf("Warning: method did not converge in max number of iterations.\n");
//}
return 0;
}
// sets the state based on given q, qvel
// automatically computes acceleration assuming zero external force
int ImplicitNewmarkSparse::SetState(double * q_, double * qvel_)
{
memcpy(q, q_, sizeof(double)*r);
if (qvel_ != NULL)
memcpy(qvel, qvel_, sizeof(double)*r);
else
memset(qvel, 0, sizeof(double)*r);
for(int i=0; i<numConstrainedDOFs; i++)
q[constrainedDOFs[i]] = qvel[constrainedDOFs[i]] = 0.0;
// M * qaccel + C * qvel + R(q) = P_0
// R(q) = P_0 = 0
// i.e. M * qaccel = - C * qvel - R(q)
forceModel->GetForceAndMatrix(q, internalForces, tangentStiffnessMatrix);
*rayleighDampingMatrix = dampingStiffnessCoef * (*tangentStiffnessMatrix);
rayleighDampingMatrix->AddSubMatrix(dampingMassCoef, *massMatrix);
// buffer = C * qvel
rayleighDampingMatrix->MultiplyVector(qvel, buffer);
dampingMatrix->MultiplyVectorAdd(qvel, buffer);
for(int i=0; i<r; i++)
buffer[i] = -buffer[i] - internalForces[i];
// solve M * qaccel = buffer
RemoveRows(r, bufferConstrained, buffer, numConstrainedDOFs, constrainedDOFs);
// use tangentStiffnessMatrix as the buffer place
tangentStiffnessMatrix->ResetToZero();
tangentStiffnessMatrix->AddSubMatrix(1.0, *massMatrix);
tangentStiffnessMatrix->AddSubMatrix(1.0, *dampingMatrix, 1);
systemMatrix->AssignSuperMatrix(tangentStiffnessMatrix); // must go via a matrix with tangentStiffnessMatrix's topology, because the AssignSuperMatrix indices were computed with respect to such topology
memset(buffer, 0, sizeof(double) * r);
#ifdef SPOOLES
SPOOLESSolver solver(systemMatrix);
int info = solver.SolveLinearSystem(buffer, bufferConstrained);
char solverString[16] = "SPOOLES";
#endif
//massMatrix->Save("M");
//systemMatrix->Save("A");
#ifdef PARDISO
pardisoSolver->ComputeCholeskyDecomposition(systemMatrix);
int info = pardisoSolver->SolveLinearSystem(buffer, bufferConstrained);
char solverString[16] = "PARDISO";
#endif
#ifdef PCG
int info = jacobiPreconditionedCGSolver->SolveLinearSystemWithJacobiPreconditioner(buffer, bufferConstrained, 1e-6, 10000);
if (info > 0)
info = 0;
char solverString[16] = "PCG";
#endif
if (info != 0)
{
printf("Error: %s sparse solver returned non-zero exit status %d.\n", solverString, (int)info);
return 1;
}
InsertRows(r, buffer, qaccel, numConstrainedDOFs, constrainedDOFs);
return 0;
}
int VolumeConservingIntegrator::DoTimestep() {
int numIter = 0;
//Error after the first step
double error0 = 0;
double errorQuotient;
// store current amplitudes and set initial guesses for qaccel, qvel
for (int i = 0; i < r; i++) {
qaccel_1[i] = qaccel[i] = 0;
q_1[i] = q[i];
qvel_1[i] = qvel[i];
}
do {
int i;
/*
printf("q:\n");
for(int i=0; i<r; i++)
printf("%G ", q[i]);
printf("\n");
printf("Internal forces:\n");
for(int i=0; i<r; i++)
printf("%G ", internalForces[i]);
printf("\n");
*/
PerformanceCounter counterForceAssemblyTime;
forceModel->GetForceAndMatrix(q, internalForces, tangentStiffnessMatrix);
counterForceAssemblyTime.StopCounter();
forceAssemblyTime = counterForceAssemblyTime.GetElapsedTime();
//tangentStiffnessMatrix->Print();
//tangentStiffnessMatrix->Save("K");
//Scale internal forces
for (i = 0; i < r; i++)
internalForces[i] *= internalForceScalingFactor;
*tangentStiffnessMatrix *= internalForceScalingFactor;
memset(qresidual, 0, sizeof(double) * r);
if (useStaticSolver) {
// fint + K * qdelta = fext
// add externalForces, internalForces
for (i = 0; i < r; i++) {
qresidual[i] = externalForces[i] - internalForces[i];
qdelta[i] = qresidual[i];
}
} else {
tangentStiffnessMatrix->ScalarMultiply(dampingStiffnessCoef,
rayleighDampingMatrix);
rayleighDampingMatrix->AddSubMatrix(dampingMassCoef, *massMatrix);
// build effective stiffness:
// Keff = M + h D + h^2 * K
// compute force residual, store it into aux variable qresidual
// qresidual = h * (-D qdot - fint + fext - h * K * qdot))
//add mass matrix and damping matrix to tangentStiffnessMatrix
*tangentStiffnessMatrix *= timestep;
*tangentStiffnessMatrix += *rayleighDampingMatrix;
tangentStiffnessMatrix->AddSubMatrix(1.0, *dampingMatrix, 1); // at this point, tangentStiffnessMatrix = h * K + D
tangentStiffnessMatrix->MultiplyVector(qvel, qresidual);
*tangentStiffnessMatrix *= timestep;
tangentStiffnessMatrix->AddSubMatrix(1.0, *massMatrix);
// add externalForces, internalForces
for (i = 0; i < r; i++) {
qresidual[i] += internalForces[i] - externalForces[i];
qresidual[i] *= -timestep;
qdelta[i] = qresidual[i];
}
}
/*
printf("internal forces:\n");
for(int i=0; i<r; i++)
printf("%G ", internalForces[i]);
printf("\n");
printf("external forces:\n");
for(int i=0; i<r; i++)
printf("%G ", externalForces[i]);
printf("\n");
printf("residual:\n");
for(int i=0; i<r; i++)
printf("%G ", -qresidual[i]);
printf("\n");
*/
double error = 0;
for (i = 0; i < r; i++)
error += qresidual[i] * qresidual[i];
// on the first iteration, compute initial error
if (numIter == 0) {
error0 = error;
errorQuotient = 1.0;
} else {
// rel error wrt to initial error before performing this iteration
errorQuotient = error / error0;
}
if (errorQuotient < epsilon * epsilon)
break;
//tangentStiffnessMatrix->Save("Keff");
RemoveRows(r, bufferConstrained, qdelta, numConstrainedDOFs,
constrainedDOFs);
systemMatrix->AssignSuperMatrix(tangentStiffnessMatrix);
// solve: systemMatrix * qdelta = qresidual
PerformanceCounter counterSystemSolveTime;
memset(buffer, 0, sizeof(double) * r);
#ifdef SPOOLES
int info;
if (numSolverThreads > 1)
{
SPOOLESSolverMT * solver = new SPOOLESSolverMT(systemMatrix, numSolverThreads);
info = solver->SolveLinearSystem(buffer, bufferConstrained);
delete(solver);
}
else
{
SPOOLESSolver * solver = new SPOOLESSolver(systemMatrix);
info = solver->SolveLinearSystem(buffer, bufferConstrained);
delete(solver);
}
char solverString[16] = "SPOOLES";
#endif
#ifdef PARDISO
int info = pardisoSolver->ComputeCholeskyDecomposition(systemMatrix);
if (info == 0)
info = pardisoSolver->SolveLinearSystem(buffer, bufferConstrained);
char solverString[16] = "PARDISO";
#endif
//Profile finds this function as a hotspot
#ifdef PCG
int info =
jacobiPreconditionedCGSolver->SolveLinearSystemWithJacobiPreconditioner(
buffer, bufferConstrained, 1e-6, 10000);
if (info > 0)
info = 0;
char solverString[16] = "PCG";
#endif
if (info != 0) {
printf(
"Error: %s sparse solver returned non-zero exit status %d.\n",
solverString, (int) info);
exit(-1);
return 1;
}
counterSystemSolveTime.StopCounter();
systemSolveTime = counterSystemSolveTime.GetElapsedTime();
InsertRows(r, buffer, qdelta, numConstrainedDOFs, constrainedDOFs);
/*
printf("qdelta:\n");
for(int i=0; i<r; i++)
printf("%G ", qdelta[i]);
printf("\n");
exit(1);
*/
// update state
if (useStaticSolver) {
for (i = 0; i < r; i++) {
q[i] += qdelta[i];
qvel[i] = (q[i] - q_1[i]) / timestep;
}
} else {
for (i = 0; i < r; i++) {
qvel[i] += qdelta[i];
q[i] += timestep * qvel[i];
}
}
for (int i = 0; i < numConstrainedDOFs; i++)
q[constrainedDOFs[i]] = qvel[constrainedDOFs[i]] = qaccel[constrainedDOFs[i]] = 0.0;
numIter++;
} while (numIter < maxIterations);
/*
printf("q:\n");
for(int i=0; i<r; i++)
printf("%G ", q[i]);
printf("\n");
printf("qvel:\n");
for(int i=0; i<r; i++)
printf("%G ", qvel[i]);
printf("\n");
*/
//printf("Num iterations performed: %d\n",numIter);
//if ((numIter >= maxIterations) && (maxIterations > 1))
//{
//printf("Warning: method did not converge in max number of iterations.\n");
//}
return 0;
}
void CArrayInt2D::RemoveRow(int iRow)
{
RemoveRows(iRow, 1);
}
void Matrix<real>::RemoveRowsColumns(int columnStart, int columnEnd)
{
RemoveColumns(columnStart, columnEnd);
RemoveRows(columnStart, columnEnd);
}
