C++ Matrix3::determinant方法代码示例

// 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();
}

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)
  {
//.........这里部分代码省略.........

// 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];
//.........这里部分代码省略.........

/* 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;
//.........这里部分代码省略.........

// 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);
}