// Transform VectorValues into valid Rot3
Values normalizeRelaxedRotations(const VectorValues& relaxedRot3) {
gttic(InitializePose3_computeOrientationsChordal);
Matrix ppm = Matrix::Zero(3,3); // plus plus minus
ppm(0,0) = 1; ppm(1,1) = 1; ppm(2,2) = -1;
Values validRot3;
BOOST_FOREACH(const VectorValues::value_type& it, relaxedRot3) {
Key key = it.first;
if (key != keyAnchor) {
const Vector& rotVector = it.second;
Matrix3 rotMat;
rotMat(0,0) = rotVector(0); rotMat(0,1) = rotVector(1); rotMat(0,2) = rotVector(2);
rotMat(1,0) = rotVector(3); rotMat(1,1) = rotVector(4); rotMat(1,2) = rotVector(5);
rotMat(2,0) = rotVector(6); rotMat(2,1) = rotVector(7); rotMat(2,2) = rotVector(8);
Matrix U, V; Vector s;
svd(rotMat, U, s, V);
Matrix3 normalizedRotMat = U * V.transpose();
// std::cout << "rotMat \n" << rotMat << std::endl;
// std::cout << "U V' \n" << U * V.transpose() << std::endl;
// std::cout << "V \n" << V << std::endl;
if(normalizedRotMat.determinant() < 0)
normalizedRotMat = U * ppm * V.transpose();
Rot3 initRot = Rot3(normalizedRotMat);
validRot3.insert(key, initRot);
}
}
ErrorCode LinearHex::integrateFcn(const double *field, const double *verts, const int nverts, const int ndim,
const int num_tuples, double *work, double *result)
{
assert(field && verts && num_tuples != -1);
double tmp_result[8];
ErrorCode rval = MB_SUCCESS;
for (int i = 0; i < num_tuples; i++) result[i] = 0.0;
CartVect x;
Matrix3 J;
for(unsigned int j1 = 0; j1 < LinearHex::gauss_count; ++j1) {
x[0] = LinearHex::gauss[j1][1];
double w1 = LinearHex::gauss[j1][0];
for(unsigned int j2 = 0; j2 < LinearHex::gauss_count; ++j2) {
x[1] = LinearHex::gauss[j2][1];
double w2 = LinearHex::gauss[j2][0];
for(unsigned int j3 = 0; j3 < LinearHex::gauss_count; ++j3) {
x[2] = LinearHex::gauss[j3][1];
double w3 = LinearHex::gauss[j3][0];
rval = evalFcn(x.array(),field, ndim, num_tuples, NULL, tmp_result);
if (MB_SUCCESS != rval) return rval;
rval = jacobianFcn(x.array(), verts, nverts, ndim, work, J[0]);
if (MB_SUCCESS != rval) return rval;
double tmp_det = w1*w2*w3*J.determinant();
for (int i = 0; i < num_tuples; i++) result[i] += tmp_result[i]*tmp_det;
}
}
}
return MB_SUCCESS;
}// LinearHex::integrate_vector()
ErrorCode EvalSet::evaluate_reverse(EvalFcn eval, JacobianFcn jacob, InsideFcn inside_f,
const double *posn, const double *verts, const int nverts,
const int ndim, const double tol, double *work,
double *params, bool *inside) {
// TODO: should differentiate between epsilons used for
// Newton Raphson iteration, and epsilons used for curved boundary geometry errors
// right now, fix the tolerance used for NR
const double error_tol_sqr = tol*tol;
CartVect *cvparams = reinterpret_cast<CartVect*>(params);
const CartVect *cvposn = reinterpret_cast<const CartVect*>(posn);
// initialize to center of element
*cvparams = CartVect(-.4);
CartVect new_pos;
// evaluate that first guess to get a new position
ErrorCode rval = (*eval)(cvparams->array(), verts, ndim, ndim, work, new_pos.array());
if (MB_SUCCESS != rval) return rval;
// residual is diff between old and new pos; need to minimize that
CartVect res = new_pos - *cvposn;
Matrix3 J;
int iters=0;
// while |res| larger than tol
while (res % res > error_tol_sqr) {
if(++iters>10) {
if (inside) {
// if we haven't converged but we're outside, that's defined as success
*inside = (*inside_f)(params, ndim, tol);
if (!(*inside)) return MB_SUCCESS;
}
return MB_FAILURE;
}
// get jacobian at current params
rval = (*jacob)(cvparams->array(), verts, nverts, ndim, work, J[0]);
double det = J.determinant();
assert(det > std::numeric_limits<double>::epsilon());
// new params tries to eliminate residual
*cvparams -= J.inverse(1.0/det) * res;
// get the new forward-evaluated position, and its difference from the target pt
rval = (*eval)(params, verts, ndim, ndim, work, new_pos.array());
if (MB_SUCCESS != rval) return rval;
res = new_pos - *cvposn;
}
if (inside)
*inside = (*inside_f)(params, ndim, tol);
return MB_SUCCESS;
}// Map::evaluate_reverse()
// Decribe for debugging use or output on some errors
void MPMBase::Describe(void)
{ cout << "# pt: pos=(" << pos.x << "," << pos.y << "," << pos.z << ") mass=" << mp <<
" matl=" << matnum << " elem=" << inElem << endl;
cout << "# vel=(" << vel.x << "," << vel.y << "," << vel.z << ") " << UnitsController::Label(CUVELOCITY_UNITS) << endl;
Matrix3 pF = GetDeformationGradientMatrix();
cout << "# F=" << pF << ", |F|=" << pF.determinant() << endl;
double rho0=theMaterials[MatID()]->rho;
double rho = rho0*UnitsController::Scaling(1.e-6)/theMaterials[MatID()]->GetCurrentRelativeVolume(this);
cout << "# P= " << pressure*rho << " " << UnitsController::Label(PRESSURE_UNITS) << endl;
cout << "# sigmaii=(" << sp.xx*rho << "," << sp.yy*rho << "," << sp.zz << ") " << UnitsController::Label(PRESSURE_UNITS) << endl;
cout << "# tauij=(" << sp.xy*rho << "," << sp.xz*rho << "," << sp.yz << ") " << UnitsController::Label(PRESSURE_UNITS) << endl;
cout << "# T= " << pTemperature << " prev T=" << pPreviousTemperature << endl;
}
/* Given matrix of incremental deformation dF = exp(dt*grad v), increment particle strain,
rotation, and LeftCauchy Green strain (latter is assumed to be stored in the particle's
plastic strain tensor (which is accessed also with GetAltStrainTensor().
New new F is dF.F, which is used to find new strain
New B = dF.(Old B).dF^T
Returns |dF|
*/
double HyperElastic::IncrementDeformation(MPMBase *mptr,Matrix3 du,Tensor *Btrial,int np) const
{
// get incremental deformation gradient
const Matrix3 dF = du.Exponential(incrementalDefGradTerms);
// current deformation gradient
Matrix3 pF = mptr->GetDeformationGradientMatrix();
// new deformation matrix
const Matrix3 F = dF*pF;
mptr->SetDeformationGradientMatrix(F);
// increment Left Cauchy tensor B = F.F^T = dF.old B.dF^T
// plain stress will need to update B.zz when known
Matrix3 pBold = mptr->GetElasticLeftCauchyMatrix();
// elements of dF.B
Matrix3 dFoldB = dF*pBold;
// return trial B (if provided) or store new B on the particle
Tensor *pB = Btrial!=NULL ? Btrial : mptr->GetAltStrainTensor() ;
pB->xx = dFoldB(0,0)*dF(0,0) + dFoldB(0,1)*dF(0,1) + dFoldB(0,2)*dF(0,2);
pB->xy = dFoldB(0,0)*dF(1,0) + dFoldB(0,1)*dF(1,1) + dFoldB(0,2)*dF(1,2);
pB->yy = dFoldB(1,0)*dF(1,0) + dFoldB(1,1)*dF(1,1) + dFoldB(1,2)*dF(1,2);
pB->zz = dFoldB(2,0)*dF(2,0) + dFoldB(2,1)*dF(2,1) + dFoldB(2,2)*dF(2,2);
if(np == THREED_MPM)
{ pB->xz = dFoldB(0,0)*dF(2,0) + dFoldB(0,1)*dF(2,1) + dFoldB(0,2)*dF(2,2);
pB->yz = dFoldB(1,0)*dF(2,0) + dFoldB(1,1)*dF(2,1) + dFoldB(1,2)*dF(2,2);
}
// return |dF|
return dF.determinant();
}
Real RotationFit(const vector<Vector3>& a,const vector<Vector3>& b,Matrix3& R)
{
Assert(a.size() == b.size());
assert(a.size() >= 3);
Matrix3 C;
C.setZero();
for(size_t k=0;k<a.size();k++) {
for(int i=0;i<3;i++)
for(int j=0;j<3;j++)
C(i,j) += a[k][j]*b[k][i];
}
//let A=[a1 ... an]^t, B=[b1 ... bn]^t
//solve for min sum of squares of E=ARt-B
//let C=AtB
//solution is given by CCt = RtCtCR
//Solve C^tR = R^tC with SVD CC^t = R^tC^tCR
//CtRX = RtCX
//C = RCtR
//Ct = RtCRt
//=> CCt = RCtCRt
//solve SVD of C and Ct (giving eigenvectors of CCt and CtC
//C = UWVt => Ct=VWUt
//=> UWUt = RVWVtRt
//=> U=RV => R=UVt
Matrix mC(3,3),mCtC(3,3);
Copy(C,mC);
SVDecomposition<Real> svd;
if(!svd.set(mC)) {
cerr<<"RotationFit: Couldn't set svd of covariance matrix"<<endl;
R.setIdentity();
return Inf;
}
Matrix mR;
mR.mulTransposeB(svd.U,svd.V);
Copy(mR,R);
if(R.determinant() < 0) { //it's a mirror
svd.sortSVs();
if(!FuzzyZero(svd.W(2),(Real)1e-2)) {
cerr<<"RotationFit: Uhh... what do we do? SVD of rotation doesn't have a zero singular value"<<endl;
/*
cerr<<svd.W<<endl;
cerr<<"Press any key to continue"<<endl;
getchar();
*/
}
//negate the last column of V
Vector vi;
svd.V.getColRef(2,vi);
vi.inplaceNegative();
mR.mulTransposeB(svd.V,svd.U);
Copy(mR,R);
Assert(R.determinant() > 0);
}
Real sum=0;
for(size_t k=0;k<a.size();k++)
sum += b[k].distanceSquared(R*a[k]);
return sum;
}
RigidTransform3
closestRigidTransform(std::vector<CoordinateFrame3> const & src, std::vector<CoordinateFrame3> const & dst)
{
alwaysAssertM(src.size() == dst.size(), "Source and destination sets must have same number of frames");
if (src.empty())
return RigidTransform3::identity();
else if (src.size() == 1)
return RigidTransform3(dst[0]) * RigidTransform3(src[0].inverse());
else if (src.size() == 2)
{
// First map line segment to line segment...
Vector3 src_mean = 0.5f * (src[0].getTranslation() + src[1].getTranslation());
Vector3 dst_mean = 0.5f * (dst[0].getTranslation() + dst[1].getTranslation());
static Real const MIN_SQLEN = 1.0e-8f;
Vector3 src_axis = src[1].getTranslation() - src[0].getTranslation();
Vector3 dst_axis = dst[1].getTranslation() - dst[0].getTranslation();
if (src_axis.squaredLength() > MIN_SQLEN && dst_axis.squaredLength() > MIN_SQLEN)
{
Matrix3 rot = Matrix3::rotationArc(src_axis, dst_axis);
// Now rotate around the mapped segment to align the z axes of the frames...
Vector3 src_dir = rot * (src[0].getRotation().getColumn(2) + src[1].getRotation().getColumn(2));
Vector3 dst_dir = dst[0].getRotation().getColumn(2) + dst[1].getRotation().getColumn(2);
// Transform src_dir and dst_dir to be perpendicular to dst_axis
dst_axis = dst_axis.fastUnit();
src_dir = src_dir - src_dir.dot(dst_axis) * dst_axis;
dst_dir = dst_dir - dst_dir.dot(dst_axis) * dst_axis;
if (src_dir.squaredLength() > MIN_SQLEN && dst_dir.squaredLength() > MIN_SQLEN)
{
src_dir = src_dir.fastUnit();
dst_dir = dst_dir.fastUnit();
Vector3 src_perp = dst_axis.cross(src_dir);
Vector3 dst_perp = dst_axis.cross(dst_dir);
Matrix3 src_basis = basisMatrix(src_dir, src_perp, dst_axis);
Matrix3 dst_basis = basisMatrix(dst_dir, dst_perp, dst_axis);
Matrix3 extra_rot = dst_basis * src_basis.transpose(); // inverse == tranpose for rotation matrices
rot = extra_rot * rot;
}
CoordinateFrame3 cf(RigidTransform3::_fromAffine(AffineTransform3(rot, dst_mean - rot * src_mean)));
// qDebug().nospace() << "R = " << cf.getRotation().toString() << ", T = " << cf.getTranslation().toString();
return RigidTransform3(cf);
}
else
return RigidTransform3::translation(dst_mean - src_mean);
}
// ICP algorithm of Arun et al. 1987.
size_t num_frames = src.size();
Vector3 src_mean = Vector3::zero(), dst_mean = Vector3::zero();
for (size_t i = 0; i < num_frames; ++i)
{
CoordinateFrame3 const & src_frame = src[i];
CoordinateFrame3 const & dst_frame = dst[i];
// qDebug().nospace() << "src[" << i << "] = R: " << src_frame.getRotation().toString() << ", T: "
// << src_frame.getTranslation().toString();
// qDebug().nospace() << "dst[" << i << "] = R: " << dst_frame.getRotation().toString() << ", T: "
// << dst_frame.getTranslation().toString();
src_mean += src_frame.getTranslation();
dst_mean += dst_frame.getTranslation();
}
src_mean /= num_frames;
dst_mean /= num_frames;
Matrix3 corr = Matrix3::zero();
Vector3 src_pt, dst_pt;
for (size_t i = 0; i < num_frames; ++i)
{
CoordinateFrame3 const & src_frame = src[i];
CoordinateFrame3 const & dst_frame = dst[i];
src_pt = src_frame.getTranslation() - src_mean;
dst_pt = dst_frame.getTranslation() - dst_mean;
for (int r = 0; r < 3; ++r)
for (int c = 0; c < 3; ++c)
corr(r, c) += src_pt[r] * dst_pt[c];
}
Matrix3 U, V;
TheaArray<Real> diag;
Algorithms::SVD::compute(corr, U, diag, V);
Matrix3 U_T = U.transpose();
Matrix3 rotation = V * U_T;
if (rotation.determinant() < 0)
{
// FIXME: One of the columns (which?) of V should be negated. See Eggert et al. '97, "Estimating 3D rigid transformations: a
// comparison of four major algorithms".
//
// For now, we'll do the dumb but safe thing and test each option for the minimum error.
THEA_WARNING << "Estimated rigid transform involves reflection, fixing by negating a column of V";
Real min_sqerr = -1;
for (int i = 0; i < 3; ++i)
{
// Swap i'th column of V
Matrix3 V_i = V;
V_i(0, i) = -V_i(0, i);
V_i(1, i) = -V_i(1, i);
V_i(2, i) = -V_i(2, i);
Matrix3 candidate_rot = V_i * U_T;
Real sqerr = 0;
for (size_t j = 0; j < num_frames; ++j)
{
CoordinateFrame3 const & src_frame = src[j];
CoordinateFrame3 const & dst_frame = dst[j];
src_pt = src_frame.getTranslation() - src_mean;
dst_pt = dst_frame.getTranslation() - dst_mean;
sqerr += (candidate_rot * src_pt - dst_pt).squaredLength();
}
if (min_sqerr < 0 || sqerr < min_sqerr)
{
rotation = candidate_rot;
min_sqerr = sqerr;
}
}
}
Vector3 translation = dst_mean - rotation * src_mean;
CoordinateFrame3 cf(RigidTransform3::_fromAffine(AffineTransform3(rotation, translation)));
// qDebug().nospace() << "R = " << rotation.toString() << ", T = " << translation.toString();
return RigidTransform3(cf);
}
// Apply Constitutive law, check np to know what type
void TaitLiquid::MPMConstitutiveLaw(MPMBase *mptr,Matrix3 du,double delTime,int np,void *properties,
ResidualStrains *res,int historyOffset) const
{
#ifdef NO_SHEAR_MODEL
// get incremental deformation gradient
const Matrix3 dF = du.Exponential(incrementalDefGradTerms);
// decompose dF into dR and dU
Matrix3 dR;
Matrix3 dU = dF.RightDecompose(&dR,NULL);
// current deformation gradient
double detdF = dF.determinant();
Matrix3 pF = mptr->GetDeformationGradientMatrix();
Matrix3 F = dR*pF;
if(np==THREED_MPM)
F.Scale(pow(detdF,1./3.));
else
F.Scale2D(sqrt(detdF));
// new deformation matrix with volume change onle
mptr->SetDeformationGradientMatrix(F);
#else
#ifdef ELASTIC_B_MODEL
// get incremental deformation gradient
const Matrix3 dF = du.Exponential(incrementalDefGradTerms);
double detdF = dF.determinant();
// current deformation gradient
Matrix3 pF = mptr->GetDeformationGradientMatrix();
// new deformation matrix
const Matrix3 F = dF*pF;
mptr->SetDeformationGradientMatrix(F);
#else
// Update total deformation gradient, and calculate trial B
double detdF = IncrementDeformation(mptr,du,NULL,np);
#endif
#endif
// Get new J and save result on the particle
double J = detdF * mptr->GetHistoryDble(J_History,historyOffset);
mptr->SetHistoryDble(J_History,J,historyOffset);
#ifdef ELASTIC_B_MODEL
// store pressure strain as elastic B
Tensor *pB = mptr->GetAltStrainTensor() ;
if(np==THREED_MPM || np==AXISYMMETRIC_MPM)
{ double J23 = pow(J,2./3.);
pB->xx = J23;
pB->yy = J23;
pB->zz = J23;
}
else
{ pB->xx = J;
pB->yy = J;
}
#endif
// account for residual stresses
double dJres = GetIncrementalResJ(mptr,res);
double Jres = dJres * mptr->GetHistoryDble(J_History+1,historyOffset);
mptr->SetHistoryDble(J_History+1,Jres,historyOffset);
double Jeff = J/Jres;
// new Kirchhoff pressure (over rho0) from Tait equation
double p0=mptr->GetPressure();
double pressure = J*TAIT_C*Ksp*(exp((1.-Jeff)/TAIT_C)-1.);
mptr->SetPressure(pressure);
// incremental energy per unit mass - dilational part
double avgP = 0.5*(p0+pressure);
double delV = 1. - 1./detdF;
double workEnergy = -avgP*delV;
// incremental residual energy per unit mass
double delVres = 1. - 1./dJres;
double resEnergy = -avgP*delVres;
// viscosity term = 2 eta (0.5(grad v) + 0.5*(grad V)^T - (1/3) tr(grad v) I) = 2 eta dev(grad v)
// (i.e., deviatoric part of the symmetric strain tensor, 2 is for conversion to engineering shear strain)
// simple shear rate = |2 dev(grad v)|
Matrix3 shear;
double c[3][3];
double shearRate;
c[0][0] = (2.*du(0,0)-du(1,1)-du(2,2))/3.;
c[1][1] = (2.*du(1,1)-du(0,0)-du(2,2))/3.;
c[2][2] = (2.*du(2,2)-du(0,0)-du(1,1))/3.;
c[0][1] = 0.5*(du(0,1)+du(1,0));
c[1][0] = c[0][1];
shearRate = c[0][0]*c[0][0] + c[1][1]*c[1][1] + c[2][2]*c[2][2]
+ 2.*c[0][1]*c[0][1];
if(np==THREED_MPM)
{ c[0][2] = 0.5*(du(0,2)+du(2,0));
c[2][0] = c[0][2];
c[1][2] = 0.5*(du(1,2)+du(2,1));
c[2][1] = c[1][2];
shearRate += 2.*(c[0][2]*c[0][2] + c[1][2]*c[1][2]);
shear.set(c);
}
else
shear.set(c[0][0],c[0][1],c[1][0],c[1][1],c[2][2]);
shearRate = 2.*sqrt(shearRate)/delTime;
// Store shear rate
mptr->SetHistoryDble(J_History+2,shearRate,historyOffset);
// Get effective visocisy
double twoetaspRate = 0.;
if(numViscosity==1)
{ twoetaspRate = TwoEtasp[0];
}
else
{ shearRate = log10(shearRate);
if(shearRate < logShearRate[0])
twoetaspRate = TwoEtasp[0];
else if(shearRate > logShearRate[numViscosity-1])
twoetaspRate = TwoEtasp[numViscosity-1];
else
{ // interpolate
for(int i=1;i<numViscosity;i++)
{ if(shearRate <= logShearRate[i])
{ // between i-1 and i
double fract = (logShearRate[i]-shearRate)/(logShearRate[i]-logShearRate[i-1]);
twoetaspRate = fract*TwoEtasp[i-1] + (1.-fract)*TwoEtasp[i];
break;
}
}
}
}
// Get Kirchoff shear stress (over rho0)
shear.Scale(J*twoetaspRate/delTime);
// update deviatoric stress
Tensor *sp=mptr->GetStressTensor();
sp->xx = shear(0,0);
sp->yy = shear(1,1);
sp->zz = shear(2,2);
sp->xy = shear(0,1);
if(np==THREED_MPM)
{ sp->xz = shear(0,2);
sp->yz = shear(1,2);
}
// shear work per unit mass = tau.du = tau.tau*delTime/twoetaspRate
double shearWork = sp->xx*sp->xx + sp->yy*sp->yy + sp->zz*sp->zz + 2.*sp->xy*sp->xy;
if(np==THREED_MPM) shearWork += 2.*(sp->xz*sp->xz + sp->yz*sp->yz);
shearWork *= delTime/twoetaspRate;
mptr->AddWorkEnergyAndResidualEnergy(workEnergy+shearWork,resEnergy);
// particle isentropic temperature increment dT/T = - J (K/K0) gamma0 Delta(V)/V
// Delta(V)/V = 1. - 1/detdF (total volume)
double Kratio = Jeff*(1.+pressure/(TAIT_C*Ksp));
double dTq0 = -J*Kratio*gamma0*mptr->pPreviousTemperature*delV;
// heat energy is Cv (dT - dTq0) -dPhi
// Here do Cv (dT - dTq0)
// dPhi = shearWork is lost due to shear term
IncrementHeatEnergy(mptr,res->dT,dTq0,shearWork);
}
/* Take increments in strain and calculate new Particle: strains, rotation strain,
stresses, strain energy,
du are (gradient rates X time increment) to give deformation gradient change
For Axisymmetry: x->R, y->Z, z->theta, np==AXISYMMETRIC_MPM, otherwise dvzz=0
This material tracks pressure and stores deviatoric stress only in particle stress tensor
*/
void Viscoelastic::MPMConstitutiveLaw(MPMBase *mptr,Matrix3 du,double delTime,int np,void *properties,ResidualStrains *res,int historyOffset) const
{
// current previous deformation gradient and stretch
Matrix3 pFnm1 = mptr->GetDeformationGradientMatrix();
// get incremental deformation gradient and decompose it
const Matrix3 dF = du.Exponential(incrementalDefGradTerms);
Matrix3 dR;
Matrix3 dVstretch = dF.LeftDecompose(&dR,NULL);
// decompose to get previous stretch
Matrix3 Vnm1 = pFnm1.LeftDecompose(NULL,NULL);
// get strain increments de = (dV-I) dR Vnma dRT
dVstretch(0,0) -= 1.;
dVstretch(1,1) -= 1.;
dVstretch(2,2) -= 1.;
Matrix3 Vrot = Vnm1.RMRT(dR);
Matrix3 detot = dVstretch*Vrot;
// Update total deformation gradient
Matrix3 pF = dF*pFnm1;
mptr->SetDeformationGradientMatrix(pF);
// Effective strain by deducting thermal strain (no shear thermal strain because isotropic)
double eres=CTE*res->dT;
if(DiffusionTask::active)
eres+=CME*res->dC;
// update pressure
double dTq0 = 0.,dispEnergy = 0.;
double traceDe = detot.trace();
double delV = traceDe - 3.*eres;
#ifdef USE_KIRCHOFF_STRESS
// tracking J
double detdF = dF.determinant();
double **h =(double **)(mptr->GetHistoryPtr(0));
double J = detdF*h[mptrHistory][MGJ_HISTORY];
h[mptrHistory][MGJ_HISTORY] = J;
UpdatePressure(mptr,delV,res,eres,detdF,J,delTime,dTq0,dispEnergy);
#else
UpdatePressure(mptr,delV,res,eres,&dF,delTime,dTq0,dispEnergy);
#endif
// deviatoric strains increment in de
// Actually de is finding 2*(dev e) to avoid many multiplies by two
Tensor de;
double dV = traceDe/3.;
de.xx = 2.*(detot(0,0) - dV);
de.yy = 2.*(detot(1,1) - dV);
de.zz = 2.*(detot(2,2) - dV);
de.xy = 2.*detot(0,1);
if(np==THREED_MPM)
{ de.xz = 2.*detot(0,2);
de.yz = 2.*detot(1,2);
}
// Find initial 2*e(t) (deviatoric strain) in ed
Tensor ed;
double thirdV = Vrot.trace()/3.;
ed.xx = 2.*(Vrot(0,0)-thirdV);
ed.yy = 2.*(Vrot(1,1)-thirdV);
ed.zz = 2.*(Vrot(2,2)-thirdV);
ed.xy = 2.*Vrot(0,1);
if(np==THREED_MPM)
{ ed.xz = 2.*Vrot(0,2);
ed.yz = 2.*Vrot(1,2);
}
// increment particle deviatoric stresses - elastic part
double dsig[6];
dsig[XX] = Gered*de.xx;
dsig[YY] = Gered*de.yy;
dsig[ZZ] = Gered*de.zz;
dsig[XY] = Gered*de.xy;
if(np==THREED_MPM)
{ dsig[XZ] = Gered*de.xz;
dsig[YZ] = Gered*de.yz;
}
// get internal variable increments, update them, add to incremental stress, and get dissipated energy6
Tensor dak;
double **ak =(double **)(mptr->GetHistoryPtr(0));
int k;
for(k=0;k<ntaus;k++)
{ double tmp = exp(-delTime/tauk[k]);
double tmpm1 = tmp-1.;
double tmpp1 = tmp+1.;
double arg = 0.25*delTime/tauk[k]; // 0.25 because e's have factor of 2
dak.xx = tmpm1*ak[XX_HISTORY][k] + arg*(tmpp1*ed.xx + de.xx);
dak.yy = tmpm1*ak[YY_HISTORY][k] + arg*(tmpp1*ed.yy + de.yy);
dak.xy = tmpm1*ak[XY_HISTORY][k] + arg*(tmpp1*ed.xy + de.xy);
dak.zz = tmpm1*ak[ZZ_HISTORY][k] + arg*(tmpp1*ed.zz + de.zz);
ak[XX_HISTORY][k] += dak.xx;
ak[YY_HISTORY][k] += dak.yy;
ak[ZZ_HISTORY][k] += dak.zz;
ak[XY_HISTORY][k] += dak.xy;
// add to stress increments
dsig[XX] -= TwoGkred[k]*dak.xx;
dsig[YY] -= TwoGkred[k]*dak.yy;
dsig[ZZ] -= TwoGkred[k]*dak.zz;
dsig[XY] -= TwoGkred[k]*dak.xy;
// dissipation
dispEnergy += TwoGkred[k]*(dak.xx*(0.5*(ed.xx+0.5*de.xx)-ak[XX_HISTORY][k]+0.5*dak.xx)
+ dak.yy*(0.5*(ed.yy+0.5*de.yy)-ak[YY_HISTORY][k]+0.5*dak.yy)
+ dak.zz*(0.5*(ed.zz+0.5*de.zz)-ak[ZZ_HISTORY][k]+0.5*dak.zz)
+ dak.xy*(0.5*(ed.xy+0.5*de.xy)-ak[XY_HISTORY][k]+0.5*dak.xy));
// extra terms for 3D
if(np==THREED_MPM)
{ // internal variables
dak.xz = tmpm1*ak[XZ_HISTORY][k] + arg*(tmpp1*ed.xz + de.xz);
dak.yz = tmpm1*ak[YZ_HISTORY][k] + arg*(tmpp1*ed.yz + de.yz);
ak[XZ_HISTORY][k] += dak.xz;
ak[YZ_HISTORY][k] += dak.yz;
// stresses
dsig[XZ] -= TwoGkred[k]*dak.xz;
dsig[YZ] -= TwoGkred[k]*dak.yz;
// dissipation
dispEnergy += TwoGkred[k]*(dak.xz*(0.5*(ed.xz+0.5*de.xz)-ak[XZ_HISTORY][k]+0.5*dak.xz)
+ dak.yz*(0.5*(ed.yz+0.5*de.yz)-ak[YZ_HISTORY][k]+0.5*dak.yz));
}
}
// Update particle deviatoric stresses
Tensor *sp=mptr->GetStressTensor();
//Tensor st0 = *sp;
if(np==THREED_MPM)
{ // incremental rotate of prior stress
Matrix3 stn(sp->xx,sp->xy,sp->xz,sp->xy,sp->yy,sp->yz,sp->xz,sp->yz,sp->zz);
Matrix3 str = stn.RMRT(dR);
#ifdef USE_KIRCHOFF_STRESS
// convert sigma(n)/rho(n) to sigma(n)/rho(n+1) and add dsigma/rho(n+1)
sp->xx = detdF*str(0,0)+J*dsig[XX];
sp->yy = detdF*str(1,1)+J*dsig[YY];
sp->xy = detdF*str(0,1)+J*dsig[XY];
sp->zz = detdF*str(2,2)+J*dsig[ZZ];
sp->yz = detdF*str(1,2)+J*dsig[YZ];
sp->xz = detdF*str(0,2)+J*dsig[XZ];
#else
sp->xx = str(0,0)+dsig[XX];
sp->yy = str(1,1)+dsig[YY];
sp->xy = str(0,1)+dsig[XY];
sp->zz = str(2,2)+dsig[ZZ];
sp->yz = str(1,2)+dsig[YZ];
sp->xz = str(0,2)+dsig[XZ];
#endif
}
else
{ // incremental rotate of prior stress
Matrix3 stn(sp->xx,sp->xy,sp->xy,sp->yy,sp->zz);
Matrix3 str = stn.RMRT(dR);
#ifdef USE_KIRCHOFF_STRESS
// convert sigma(n)/rho(n) to sigma(n)/rho(n+1) and add dsigma/rho(n+1)
sp->xx = detdF*str(0,0)+J*dsig[XX];
sp->yy = detdF*str(1,1)+J*dsig[YY];
sp->xy = detdF*str(0,1)+J*dsig[XY];
sp->zz = detdF*sp->zz+J*dsig[ZZ];
#else
sp->xx = str(0,0)+dsig[XX];
sp->yy = str(1,1)+dsig[YY];
sp->xy = str(0,1)+dsig[XY];
sp->zz += dsig[ZZ];
#endif
}
// incremental work energy = shear energy (dilation and residual energy done in update pressure)
double shearEnergy = sp->xx*detot(0,0) + sp->yy*detot(1,1) + sp->zz*detot(2,2) + sp->xy*de.xy;
if(np==THREED_MPM)
{ shearEnergy += sp->xz*de.xz + sp->yz*de.yz;
}
mptr->AddWorkEnergyAndResidualEnergy(shearEnergy,0.);
// disispated energy per unit mass (dPhi/(rho0 V0)) (nJ/g)
mptr->AddPlastEnergy(dispEnergy);
// heat energy is Cv dT - dPhi
// The dPhi is subtracted here because it will show up in next
// time step within Cv dT (if adibatic heating occurs)
// The Cv dT was done in update pressure
IncrementHeatEnergy(mptr,res->dT,dTq0,dispEnergy);
}
// Apply Constitutive law, check np to know what type
void TaitLiquid::MPMConstitutiveLaw(MPMBase *mptr,Matrix3 du,double delTime,int np,void *properties,ResidualStrains *res) const
{
#ifdef NO_SHEAR_MODEL
// get incremental deformation gradient
const Matrix3 dF = du.Exponential(incrementalDefGradTerms);
// decompose dF into dR and dU
Matrix3 dR;
Matrix3 dU = dF.RightDecompose(&dR,NULL);
// current deformation gradient
double detdF = dF.determinant();
Matrix3 pF = mptr->GetDeformationGradientMatrix();
Matrix3 F = dR*pF;
if(np==THREED_MPM)
F.Scale(pow(detdF,1./3.));
else
F.Scale2D(sqrt(detdF));
// new deformation matrix with volume change onle
mptr->SetDeformationGradientMatrix(F);
#else
// Update total deformation gradient, and calculate trial B
double detdF = IncrementDeformation(mptr,du,NULL,np);
#endif
// Get new J and save result on the particle
double J = detdF * mptr->GetHistoryDble(J_history);
mptr->SetHistoryDble(J_history,J);
// account for residual stresses
double dresStretch,resStretch = GetResidualStretch(mptr,dresStretch,res);
double Jres = resStretch*resStretch*resStretch;
double Jeff = J/Jres;
// new Kirchhoff pressure (over rho0) from Tait equation
double p0=mptr->GetPressure();
double pressure = J*TAIT_C*Ksp*(exp((1.-Jeff)/TAIT_C)-1.);
mptr->SetPressure(pressure);
// incremental energy per unit mass - dilational part
double avgP = 0.5*(p0+pressure);
double delV = 1. - 1./detdF;
double workEnergy = -avgP*delV;
// incremental residual energy per unit mass
double delVres = 1. - 1./(dresStretch*dresStretch*dresStretch);
double resEnergy = -avgP*delVres;
// viscosity term = 2 eta (0.5(grad v) + 0.5*(grad V)^T - (1/3) tr(grad v) I)
// (i.e., divatoric part of the symmetric strain tensor, 2 is for conversion to engineering shear strain)
Matrix3 shear;
double c[3][3];
c[0][0] = (2.*du(0,0)-du(1,1)-du(2,2))/3.;
c[1][1] = (2.*du(1,1)-du(0,0)-du(2,2))/3.;
c[2][2] = (2.*du(2,2)-du(0,0)-du(1,1))/3.;
c[0][1] = 0.5*(du(0,1)+du(1,0));
c[1][0] = c[0][1];
if(np==THREED_MPM)
{ c[0][2] = 0.5*(du(0,2)+du(2,0));
c[2][0] = c[0][2];
c[1][2] = 0.5*(du(1,2)+du(2,1));
c[2][1] = c[1][2];
shear.set(c);
}
else
shear.set(c[0][0],c[0][1],c[1][0],c[1][1],c[2][2]);
// Get Kirchoff shear stress (over rho0)
shear.Scale(J*TwoEtasp/delTime);
// update deviatoric stress
Tensor *sp=mptr->GetStressTensor();
sp->xx = shear(0,0);
sp->yy = shear(1,1);
sp->zz = shear(2,2);
sp->xy = shear(0,1);
if(np==THREED_MPM)
{ sp->xz = shear(0,2);
sp->yz = shear(1,2);
}
// shear work per unit mass = tau.du = tau.tau*delTime/TwoEtasp
double shearWork = sp->xx*sp->xx + sp->yy*sp->yy + sp->zz*sp->zz + 2.*sp->xy*sp->xy;
if(np==THREED_MPM) shearWork += 2.*(sp->xz*sp->xz + sp->yz*sp->yz);
shearWork *= delTime/TwoEtasp;
mptr->AddWorkEnergyAndResidualEnergy(workEnergy+shearWork,resEnergy);
// particle isentropic temperature increment dT/T = - J (K/K0) gamma0 Delta(V)/V
// Delta(V)/V = 1. - 1/detdF (total volume)
double Kratio = Jeff*(1.+pressure/(TAIT_C*Ksp));
double dTq0 = -J*Kratio*gamma0*mptr->pPreviousTemperature*delV;
// heat energy is Cv (dT - dTq0) -dPhi
// Here do Cv (dT - dTq0)
// dPhi = shearWork is lost due to shear term
IncrementHeatEnergy(mptr,res->dT,dTq0,shearWork);
}
