void SolverThread::updateB(float dt)
{
unsigned totalPoints = m_mesh->numPoints();
Vector3F * X = m_mesh->X();
float * mass = m_mesh->M();
Vector3F * b = rightHandSide();
for(unsigned k=0;k<totalPoints;k++) {
float m_i = mass[k];
b[k].setZero();
MatrixMap tmp = m_K_row[k];
MatrixMap::iterator Kbegin = tmp.begin();
MatrixMap::iterator Kend = tmp.end();
for (MatrixMap::iterator K = Kbegin; K != Kend;++K)
{
unsigned j = K->first;
Matrix33F K_ij = K->second;
Vector3F x_j = X[j];
Vector3F prod = K_ij * x_j;
b[k] -= prod;
}
b[k] -= m_F0[k];
b[k] += m_F[k];
b[k] *= dt;
b[k] += m_V[k]*m_i;
}
}
void SolverThread::stepPhysics(float dt)
{
computeForces();
clearStiffnessAssembly();
if(bUseStiffnessWarping)
updateOrientation();
else
resetOrientation();
stiffnessAssembly();
// addPlasticityForce(dt);
dynamicsAssembly(dt);
#if SOLVEONGPU
solveGpu(m_V, m_stiffnessMatrix);
#else
solve(m_V);
#endif
updatePosition(dt);
#if ENABLE_DBG
dbglg.write("Re");
unsigned totalTetrahedra = m_mesh->numTetrahedra();
FEMTetrahedronMesh::Tetrahedron * tetrahedra = m_mesh->tetrahedra();
for(unsigned k=0;k<totalTetrahedra;k++) {
dbglg.write(k);
dbglg.write(tetrahedra[k].Re.str());
}
dbglg.writeMat33(m_stiffnessMatrix->valueBuf(),
m_stiffnessMatrix->numNonZero(),
"K ");
dbglg.write("Rhs");
unsigned totalPoints = m_mesh->numPoints();
for(unsigned k=0;k<totalPoints;k++) {
dbglg.write(k);
dbglg.write(rightHandSide()[k].str());
dbglg.newLine();
}
dbglg.write("F0");
for(unsigned k=0;k<totalPoints;k++) {
dbglg.write(k);
dbglg.write(m_F0[k].str());
dbglg.newLine();
}
#endif
}
void initRightHandSide(ColumnMatrix local_b,double stepSize)
{
#ifdef USE_OPENMP
#pragma omp parallel for schedule(static)
for(int column=0;column<local_b->localSize;column++)
{
for(int row=0;row<local_b->globalSize;row++)
{
local_b->data[row + column*local_b->globalSize] = stepSize*stepSize * rightHandSide((column+local_b->displacement[local_b->commRank])*stepSize,row*stepSize);
}
}
#else
for(int column=0;column<local_b->localSize;column++)
{
for(int row=0;row<local_b->globalSize;row++)
{
local_b->data[row + column*local_b->globalSize] = stepSize*stepSize * rightHandSide((column+local_b->displacement[local_b->commRank])*stepSize,row*stepSize);
}
}
#endif
}
void CudaConjugateGradientSolver::solve(void * X,
CudaCSRMatrix * A,
void * fixed,
float * error)
{
// cglg.writeVec3(m_rhs, m_dimension, "cg b", CudaDbgLog::FAlways);
//cglg.writeMat33(A->valueBuf(),
// A->numNonZero(),
// " cg A ", CudaDbgLog::FAlways);
cuConjugateGradient_prevresidual((float3 *)previous(),
(float3 *)residual(),
(mat33 *)A->deviceValue(),
(uint *)A->deviceRowPtr(),
(uint *)A->deviceColInd(),
(uint *)fixed,
(float3 *)X,
(float3 *)rightHandSide(),
m_dimension);
for(int i=0; i<FemGlobal::CGSolverMaxNumIterations; i++) {
cuConjugateGradient_Ax((float3 *)previous(),
(float3 *)updated(),
(float3 *)residual(),
(float *)diff(),
(float *)diff2(),
(mat33 *)A->deviceValue(),
(uint *)A->deviceRowPtr(),
(uint *)A->deviceColInd(),
(uint *)fixed,
m_dimension);
float d =0;
float d2=0;
m_reduce->sum<float>(d, (float *)diff(), m_dimension);
m_reduce->sum<float>(d2, (float *)diff2(), m_dimension);
if(fabs(d2)< 1e-10f)
d2 = 1e-10f;
float d3 = d/d2;
cuConjugateGradient_addX((float3 *)X,
(float3 *)residual(),
(float *)diff(),
(float3 *)previous(),
(float3 *)updated(),
d3,
(uint *)fixed,
m_dimension);
float d1 = 0.f;
m_reduce->sum<float>(d1, (float *)diff(), m_dimension);
if(error) *error = d1;
if(d1 < 0.01f)
break;
if(fabs(d)<1e-10f)
d = 1e-10f;
float d4 = d1/d;
cuConjugateGradient_addResidual((float3 *)previous(),
(float3 *)residual(),
d4,
(uint *)fixed,
m_dimension);
}
}
