Matrix<double> Matrix<double>::Inverse(void) const
{
if ( GetColumnLength() != GetRowLength() )
{
return *this;
}
// Calculate Inversed-Matrix<double> by Cofactors.
const size_t rowCount = GetRowLength();
const size_t colCount = GetColumnLength();
Matrix<double> inversed(rowCount, colCount);
#if 0
double det = Determinant();
if ( 0 == det )
{
return *this;
}
for (size_t row = 0; row < rowCount; ++row)
{
for (size_t col = 0; col < colCount; ++col)
{
int sign = ((row + col) & 1) ? -1 : 1;
Matrix<double> m(GetCofactorMatrix(row, col));
inversed[col][row] = GetCofactor(row, col) / det;
}
}
#else
//--------------------------------
// Gauss-Jordan
//--------------------------------
const double threshold = 1.0e-10; // TBD
Matrix<double> m(rowCount, colCount * 2);
if ( m.IsNull() )
{
return m;
}
for (size_t row = 0; row < rowCount; ++row)
{
for (size_t col = 0; col < colCount; ++col)
{
m[row][col] = (*this)[row][col];
}
}
for (size_t row = 0; row < rowCount; ++row)
{
for (size_t col = colCount; col < colCount * 2; ++col)
{
if (row == (col - colCount))
{
m[row][col] = 1.0;
}
else
{
m[row][col] = 0.0;
}
}
}
// 各行を正規化
for (size_t r = 0; r < m.GetRowLength(); ++r )
{
m[r] /= m[r].GetMaximumAbsolute();
}
for (size_t r = 0; r < m.GetRowLength(); ++r )
{
// 注目している列の中で、最大値を持つ列を探す。
size_t index = r;
double maximum = fabs(m[r][r]);
// "k < m.GetRowLength() - 1" となっていたが、意図不明なので修正。
for (size_t k = index + 1; k < m.GetRowLength(); ++k )
{
if ( fabs(m[k][r]) > maximum )
{
index = k;
maximum = fabs(m[k][r]);
}
}
if ( maximum < threshold )
{
// ここに到達することはないはず。
DEBUG_LOG(" ~ 0 at r=%d\n", static_cast<int>(r));
m.Resize(0, 0); // NULL を返す。
return m;
}
// 必要なら入れ替える
if ( r != index )
{
const Vectord tmp(m[r]);
m[r] = m[index];
m[index] = tmp;
}
// 上で入れ替えたので 以降では index は不要、r を使用する。
// 注目している行の、注目している列の値を 1.0 にする
m[r] /= m[r][r]; // /= maximum; // maximum では符号が考慮されない。
// 他の行から引く。
for (size_t k = 0; k < m.GetRowLength(); ++k )
{
if ( k == r )
{
continue;
}
const Vectord tmp(m[r]);
m[k] -= (tmp * m[k][r]);
}
}
for (size_t row = 0; row < rowCount; ++row)
{
for (size_t col = 0; col < colCount; ++col)
{
inversed[row][col] = m[row][col+colCount];
}
}
#endif
return inversed;
}
Matrix4x4 Matrix4x4::operator-() const
{
const FLOAT32 fDeterminant = Determinant();
if(fabsf(fDeterminant) < FLT_EPSILON) {*this;}
return MatAdjoint(this) / fDeterminant;
}
double CalcTetBadnessGrad (const Point3d & p1, const Point3d & p2,
const Point3d & p3, const Point3d & p4, double h,
int pi, Vec<3> & grad)
{
double vol, l, ll, lll;
double err;
const Point3d *pp1, *pp2, *pp3, *pp4;
pp1 = &p1;
pp2 = &p2;
pp3 = &p3;
pp4 = &p4;
switch (pi)
{
case 2:
{
swap (pp1, pp2);
swap (pp3, pp4);
break;
}
case 3:
{
swap (pp1, pp3);
swap (pp2, pp4);
break;
}
case 4:
{
swap (pp1, pp4);
swap (pp3, pp2);
break;
}
}
Vec3d v1 (*pp1, *pp2);
Vec3d v2 (*pp1, *pp3);
Vec3d v3 (*pp1, *pp4);
Vec3d v4 (*pp2, *pp3);
Vec3d v5 (*pp2, *pp4);
Vec3d v6 (*pp3, *pp4);
vol = -Determinant (v1, v2, v3) / 6;
Vec3d gradvol;
Cross (v5, v4, gradvol);
gradvol *= (-1.0/6.0);
double ll1 = v1.Length2();
double ll2 = v2.Length2();
double ll3 = v3.Length2();
double ll4 = v4.Length2();
double ll5 = v5.Length2();
double ll6 = v6.Length2();
ll = ll1 + ll2 + ll3 + ll4 + ll5 + ll6;
l = sqrt (ll);
lll = l * ll;
if (vol <= 1e-24 * lll)
{
grad = Vec3d (0, 0, 0);
return 1e24;
}
Vec3d gradll1 (*pp2, *pp1);
Vec3d gradll2 (*pp3, *pp1);
Vec3d gradll3 (*pp4, *pp1);
gradll1 *= 2;
gradll2 *= 2;
gradll3 *= 2;
Vec3d gradll (gradll1);
gradll += gradll2;
gradll += gradll3;
/*
Vec3d gradll;
gradll = v1+v2+v3;
gradll *= -2;
*/
err = 0.0080187537 * lll / vol;
gradll *= (0.0080187537 * 1.5 * l / vol);
Vec3d graderr(gradll);
gradvol *= ( -0.0080187537 * lll / (vol * vol) );
graderr += gradvol;
if (h > 0)
{
/*
Vec3d gradll1 (*pp2, *pp1);
Vec3d gradll2 (*pp3, *pp1);
Vec3d gradll3 (*pp4, *pp1);
gradll1 *= 2;
gradll2 *= 2;
gradll3 *= 2;
*/
err += ll / (h*h) +
h*h * ( 1 / ll1 + 1 / ll2 + 1 / ll3 +
1 / ll4 + 1 / ll5 + 1 / ll6 ) - 12;
graderr += (1/(h*h) - h*h/(ll1*ll1)) * gradll1;
graderr += (1/(h*h) - h*h/(ll2*ll2)) * gradll2;
graderr += (1/(h*h) - h*h/(ll3*ll3)) * gradll3;
cout << "?";
}
double errpow;
if (teterrpow == 2)
{
errpow = err*err;
grad = (2 * err) * graderr;
}
else
{
errpow = pow (err, teterrpow);
grad = (teterrpow * errpow / err) * graderr;
}
return errpow;
}
inline T Det() throw(DimensionException) { return Determinant(); }
int Determinant(int a[][10],int n)
{
int i,j,j1,j2 ; // general loop and matrix subscripts
int det = 0 ; // init determinant
int **m = NULL ; // pointer to pointers to implement 2d
// square array
if (n < 1) { } // error condition, should never get here
else if (n == 1) { // should not get here
det = a[0][0] ;
}
else if (n == 2) { // basic 2X2 sub-matrix determinate
// definition. When n==2, this ends the
det = a[0][0] * a[1][1] - a[1][0] * a[0][1] ;// the recursion series
}
// recursion continues, solve next sub-matrix
else { // solve the next minor by building a
// sub matrix
det = 0 ; // initialize determinant of sub-matrix
// for each column in sub-matrix
for (j1 = 0 ; j1 < n ; j1++) {
// get space for the pointer list
m = (int **) malloc((n-1)* sizeof(int *)) ;
for (i = 0 ; i < n-1 ; i++)
m[i] = (int *) malloc((n-1)* sizeof(int)) ;
// i[0][1][2][3] first malloc
// m -> + + + + space for 4 pointers
// | | | | j second malloc
// | | | +-> _ _ _ [0] pointers to
// | | +----> _ _ _ [1] and memory for
// | +-------> _ a _ [2] 4 ints
// +----------> _ _ _ [3]
//
// a[1][2]
// build sub-matrix with minor elements excluded
for (i = 1 ; i < n ; i++) {
j2 = 0 ; // start at first sum-matrix column position
// loop to copy source matrix less one column
for (j = 0 ; j < n ; j++) {
if (j == j1) continue ; // don't copy the minor column element
m[i-1][j2] = a[i][j] ; // copy source element into new sub-matrix
// i-1 because new sub-matrix is one row
// (and column) smaller with excluded minors
j2++ ; // move to next sub-matrix column position
}
}
det += pow(-1.0,1.0 + j1 + 1.0) * a[0][j1] * Determinant(m,n-1) ;
// sum x raised to y power
// recursively get determinant of next
// sub-matrix which is now one
// row & column smaller
for (i = 0 ; i < n-1 ; i++) free(m[i]) ;// free the storage allocated to
// to this minor's set of pointers
free(m) ; // free the storage for the original
// pointer to pointer
}
}
return(det) ;
}
bool Matrix4::IsOrthogonal() const
{
// Determinant is 1 or -1
return Abs(1.0f - Abs(Determinant())) < EPSILON;
}
Matrix Matrix::Invert(MType eType) const
{
Matrix invert(*this);
switch(eType)
{
case MI_IDENTITY:
break;
case MI_UNIT_SCALE:
{
invert.m00 = invert.m11 = invert.m22 = 1 / m00;
break;
}
case MI_UNIT_SCALE_TRANSLATE:
{
Real reciprocalScale = 1 / m00;
invert.m00 = invert.m11 = invert.m22 = reciprocalScale;
invert.m03 *= -reciprocalScale;
invert.m13 *= -reciprocalScale;
invert.m23 *= -reciprocalScale;
break;
}
case MI_SCALE:
{
Real s0Xs1 = m00 * m11;
Real s1Xs2 = m11 * m22;
Real s0Xs2 = m00 * m22;
Real reciprocalS0Xs1Xs2 = 1 / (s0Xs1 * m22);
invert.m00 = reciprocalS0Xs1Xs2 * s1Xs2;
invert.m11 = reciprocalS0Xs1Xs2 * s0Xs2;
invert.m22 = reciprocalS0Xs1Xs2 * s0Xs1;
break;
}
case MI_ROTATE:
{
Swap(invert.m10,invert.m01);
Swap(invert.m20,invert.m02);
Swap(invert.m21,invert.m12);
break;
}
case MI_TRANSLATE:
{
invert.m03 = -this->m03;
invert.m13 = -this->m13;
invert.m23 = -this->m23;
break;
}
case MI_3X3DMATRIX:
{
Real det = Determinant(MI_AFFINE);
if (Fabs(det) < 0.0)
{
throw NoInverse();
}
// compute inverse to save divides
Real det_inv = 1.0 / det;
//
// // compute inverse using m-1 = adjoint(m)/det(m)
invert.m00 = det_inv * (this->m11*this->m22 - this->m21*this->m12);
invert.m10 = -det_inv * (this->m10*this->m22 - this->m20*this->m12);
invert.m20 = det_inv * (this->m10*this->m21 - this->m20*this->m11);
invert.m01 = -det_inv * (this->m01*this->m22 - this->m21*this->m02);
invert.m11 = det_inv * (this->m00*this->m22 - this->m20*this->m02);
invert.m21 = -det_inv * (this->m00*this->m21 - this->m20*this->m01);
invert.m02 = det_inv * (this->m01*this->m12 - this->m11*this->m02);
invert.m12 = -det_inv * (this->m00*this->m12 - this->m10*this->m02);
invert.m22 = det_inv * (this->m00*this->m11 - this->m10*this->m01);
}
break;
case MI_SCALE_TRANSLATE:
{
Real s0Xs1 = this->m00 * this->m11;
Real s1Xs2 = this->m11 * this->m22;
Real s0Xs2 = this->m00 * this->m22;
Real reciprocalS0Xs1Xs2 = 1 / (s0Xs1 * m22);
invert.m00 = reciprocalS0Xs1Xs2 * s1Xs2;
invert.m11 = reciprocalS0Xs1Xs2 * s0Xs2;
invert.m22 = reciprocalS0Xs1Xs2 * s0Xs1;
invert.m03 *= -invert.m00;
invert.m13 *= -invert.m11;
invert.m23 *= -invert.m22;
break;
}
case MI_ROTATE_TRANSLATE:
{
Swap(invert.m10,invert.m01);
Swap(invert.m20,invert.m02);
Swap(invert.m21,invert.m12);
invert.m03 = -(invert.m00 * this->m03 + invert.m01 * this->m13 + invert.m02 * this->m23);
invert.m13 = -(invert.m10 * this->m03 + invert.m11 * this->m13 + invert.m12 * this->m23);
invert.m23 = -(invert.m20 * this->m03 + invert.m21 * this->m13 + invert.m22 * this->m23);
break;
}
case MI_AFFINE:
{
Real det = Determinant(MI_AFFINE);
if (Fabs(det) < 0.0)
{
throw NoInverse();
}
// compute inverse to save divides
Real det_inv = 1.0 / det;
//
// // compute inverse using m-1 = adjoint(m)/det(m)
invert.m00 = det_inv * (this->m11*this->m22 - this->m21*this->m12);
invert.m10 = -det_inv * (this->m10*this->m22 - this->m20*this->m12);
invert.m20 = det_inv * (this->m10*this->m21 - this->m20*this->m11);
invert.m01 = -det_inv * (this->m01*this->m22 - this->m21*this->m02);
invert.m11 = det_inv * (this->m00*this->m22 - this->m20*this->m02);
invert.m21 = -det_inv * (this->m00*this->m21 - this->m20*this->m01);
invert.m02 = det_inv * (this->m01*this->m12 - this->m11*this->m02);
invert.m12 = -det_inv * (this->m00*this->m12 - this->m10*this->m02);
invert.m22 = det_inv * (this->m00*this->m11 - this->m10*this->m01);
invert.m03 = -(invert.m00 * this->m03 + invert.m01 * this->m13 + invert.m02 * this->m23);
invert.m13 = -(invert.m10 * this->m03 + invert.m11 * this->m13 + invert.m12 * this->m23);
invert.m23 = -(invert.m20 * this->m03 + invert.m21 * this->m13 + invert.m22 * this->m23);
}
break;
default:
{
Real det = Determinant(MI_GENERIC);
if (Fabs(det) < 0.0)
{
throw NoInverse();
}
// compute inverse to save divides
Real det_inv = 1.0 / det;
invert.m00 = det_inv * Determinant3X3(m11,m12,m13,m21,m22,m23,m31,m32,m33);
invert.m10 = -det_inv * Determinant3X3(m10,m12,m13,m20,m22,m23,m30,m32,m33);
invert.m20 = det_inv * Determinant3X3(m10,m11,m13,m20,m21,m23,m30,m31,m33);
invert.m30 = -det_inv * Determinant3X3(m10,m11,m12,m20,m21,m22,m30,m31,m32);
invert.m01 = -det_inv * Determinant3X3(m01,m02,m03,m21,m22,m23,m31,m32,m33);
invert.m11 = det_inv * Determinant3X3(m00,m02,m03,m20,m22,m23,m30,m32,m33);
invert.m21 = -det_inv * Determinant3X3(m00,m01,m03,m20,m21,m23,m30,m31,m33);
invert.m31 = det_inv * Determinant3X3(m00,m01,m02,m20,m21,m22,m30,m31,m32);
invert.m02 = det_inv * Determinant3X3(m01,m02,m03,m11,m12,m13,m31,m32,m33);
invert.m12 = -det_inv * Determinant3X3(m00,m02,m03,m10,m12,m13,m30,m32,m33);
invert.m22 = det_inv * Determinant3X3(m00,m01,m03,m10,m11,m13,m30,m31,m33);
invert.m32 = -det_inv * Determinant3X3(m00,m01,m02,m10,m11,m12,m30,m31,m32);
invert.m03 = -det_inv * Determinant3X3(m01,m02,m03,m11,m12,m13,m21,m22,m23);
invert.m13 = det_inv * Determinant3X3(m00,m02,m03,m10,m12,m13,m20,m22,m23);
invert.m23 = -det_inv * Determinant3X3(m00,m01,m03,m10,m11,m13,m20,m21,m23);
invert.m33 = det_inv * Determinant3X3(m00,m01,m02,m10,m11,m12,m20,m21,m22);
}
break;
}
return invert;
}
double Matrixf3Measure(const Matrixf3& mx)
{
double d = Determinant(mx);
return (d >= 0 ? +1 : -1) * pow(fabs(d), 1.0 / 3.0);
}
bool float3x4::HasNegativeScale() const
{
return Determinant() < 0.f;
}
Trivector<Scalar, ToFrame> Permutation<FromFrame, ToFrame>::operator()(
Trivector<Scalar, FromFrame> const& trivector) const {
return Trivector<Scalar, ToFrame>(Determinant() * trivector.coordinates());
}
void AbstractCardiacMechanicsSolver<ELASTICITY_SOLVER,DIM>::AddActiveStressAndStressDerivative(c_matrix<double,DIM,DIM>& rC,
unsigned elementIndex,
unsigned currentQuadPointGlobalIndex,
c_matrix<double,DIM,DIM>& rT,
FourthOrderTensor<DIM,DIM,DIM,DIM>& rDTdE,
bool addToDTdE)
{
for(unsigned i=0; i<DIM; i++)
{
mCurrentElementFibreDirection(i) = this->mChangeOfBasisMatrix(i,0);
}
//Compute the active tension and add to the stress and stress-derivative
double I4_fibre = inner_prod(mCurrentElementFibreDirection, prod(rC, mCurrentElementFibreDirection));
double lambda_fibre = sqrt(I4_fibre);
double active_tension = 0;
double d_act_tension_dlam = 0.0; // Set and used if assembleJacobian==true
double d_act_tension_d_dlamdt = 0.0; // Set and used if assembleJacobian==true
GetActiveTensionAndTensionDerivs(lambda_fibre, currentQuadPointGlobalIndex, addToDTdE,
active_tension, d_act_tension_dlam, d_act_tension_d_dlamdt);
double detF = sqrt(Determinant(rC));
rT += (active_tension*detF/I4_fibre)*outer_prod(mCurrentElementFibreDirection,mCurrentElementFibreDirection);
// amend the stress and dTdE using the active tension
double dTdE_coeff1 = -2*active_tension*detF/(I4_fibre*I4_fibre); // note: I4_fibre*I4_fibre = lam^4
double dTdE_coeff2 = active_tension*detF/I4_fibre;
double dTdE_coeff_s1 = 0.0; // only set non-zero if we apply cross fibre tension (in 2/3D)
double dTdE_coeff_s2 = 0.0; // only set non-zero if we apply cross fibre tension (in 2/3D)
double dTdE_coeff_s3 = 0.0; // only set non-zero if we apply cross fibre tension and implicit (in 2/3D)
double dTdE_coeff_n1 = 0.0; // only set non-zero if we apply cross fibre tension in 3D
double dTdE_coeff_n2 = 0.0; // only set non-zero if we apply cross fibre tension in 3D
double dTdE_coeff_n3 = 0.0; // only set non-zero if we apply cross fibre tension in 3D and implicit
if(IsImplicitSolver())
{
double dt = mNextTime-mCurrentTime;
//std::cout << "d sigma / d lamda = " << d_act_tension_dlam << ", d sigma / d lamdat = " << d_act_tension_d_dlamdt << "\n" << std::flush;
dTdE_coeff1 += (d_act_tension_dlam + d_act_tension_d_dlamdt/dt)*detF/(lambda_fibre*I4_fibre); // note: I4_fibre*lam = lam^3
}
bool apply_cross_fibre_tension = (this->mrElectroMechanicsProblemDefinition.GetApplyCrossFibreTension()) && (DIM > 1);
if(apply_cross_fibre_tension)
{
double sheet_cross_fraction = mrElectroMechanicsProblemDefinition.GetSheetTensionFraction();
for(unsigned i=0; i<DIM; i++)
{
mCurrentElementSheetDirection(i) = this->mChangeOfBasisMatrix(i,1);
}
double I4_sheet = inner_prod(mCurrentElementSheetDirection, prod(rC, mCurrentElementSheetDirection));
// amend the stress and dTdE using the active tension
dTdE_coeff_s1 = -2*sheet_cross_fraction*detF*active_tension/(I4_sheet*I4_sheet); // note: I4*I4 = lam^4
if(IsImplicitSolver())
{
double dt = mNextTime-mCurrentTime;
dTdE_coeff_s3 = sheet_cross_fraction*(d_act_tension_dlam + d_act_tension_d_dlamdt/dt)*detF/(lambda_fibre*I4_sheet); // note: I4*lam = lam^3
}
rT += sheet_cross_fraction*(active_tension*detF/I4_sheet)*outer_prod(mCurrentElementSheetDirection,mCurrentElementSheetDirection);
dTdE_coeff_s2 = active_tension*sheet_cross_fraction*detF/I4_sheet;
if (DIM>2)
{
double sheet_normal_cross_fraction = mrElectroMechanicsProblemDefinition.GetSheetNormalTensionFraction();
for(unsigned i=0; i<DIM; i++)
{
mCurrentElementSheetNormalDirection(i) = this->mChangeOfBasisMatrix(i,2);
}
double I4_sheet_normal = inner_prod(mCurrentElementSheetNormalDirection, prod(rC, mCurrentElementSheetNormalDirection));
dTdE_coeff_n1 =-2*sheet_normal_cross_fraction*detF*active_tension/(I4_sheet_normal*I4_sheet_normal); // note: I4*I4 = lam^4
rT += sheet_normal_cross_fraction*(active_tension*detF/I4_sheet_normal)*outer_prod(mCurrentElementSheetNormalDirection,mCurrentElementSheetNormalDirection);
dTdE_coeff_n2 = active_tension*sheet_normal_cross_fraction*detF/I4_sheet_normal;
if(IsImplicitSolver())
{
double dt = mNextTime-mCurrentTime;
dTdE_coeff_n3 = sheet_normal_cross_fraction*(d_act_tension_dlam + d_act_tension_d_dlamdt/dt)*detF/(lambda_fibre*I4_sheet_normal); // note: I4*lam = lam^3
}
}
}
if(addToDTdE)
{
c_matrix<double,DIM,DIM> invC = Inverse(rC);
for (unsigned M=0; M<DIM; M++)
{
for (unsigned N=0; N<DIM; N++)
{
for (unsigned P=0; P<DIM; P++)
{
for (unsigned Q=0; Q<DIM; Q++)
{
rDTdE(M,N,P,Q) += dTdE_coeff1 * mCurrentElementFibreDirection(M)
* mCurrentElementFibreDirection(N)
* mCurrentElementFibreDirection(P)
* mCurrentElementFibreDirection(Q)
+ dTdE_coeff2 * mCurrentElementFibreDirection(M)
* mCurrentElementFibreDirection(N)
* invC(P,Q);
if(apply_cross_fibre_tension)
{
rDTdE(M,N,P,Q) += dTdE_coeff_s1 * mCurrentElementSheetDirection(M)
* mCurrentElementSheetDirection(N)
* mCurrentElementSheetDirection(P)
* mCurrentElementSheetDirection(Q)
+ dTdE_coeff_s2 * mCurrentElementSheetDirection(M)
* mCurrentElementSheetDirection(N)
* invC(P,Q)
+ dTdE_coeff_s3 * mCurrentElementSheetDirection(M)
* mCurrentElementSheetDirection(N)
* mCurrentElementFibreDirection(P)
* mCurrentElementFibreDirection(Q);
if (DIM>2)
{
rDTdE(M,N,P,Q) += dTdE_coeff_n1 * mCurrentElementSheetNormalDirection(M)
* mCurrentElementSheetNormalDirection(N)
* mCurrentElementSheetNormalDirection(P)
* mCurrentElementSheetNormalDirection(Q)
+ dTdE_coeff_n2 * mCurrentElementSheetNormalDirection(M)
* mCurrentElementSheetNormalDirection(N)
* invC(P,Q)
+ dTdE_coeff_n3 * mCurrentElementSheetNormalDirection(M)
* mCurrentElementSheetNormalDirection(N)
* mCurrentElementFibreDirection(P)
* mCurrentElementFibreDirection(Q);
}
}
}
}
}
}
}
// ///\todo #2180 The code below applies a cross fibre tension in the 2D case. Things that need doing:
// // * Refactor the common code between the block below and the block above to avoid duplication.
// // * Handle the 3D case.
// if(this->mrElectroMechanicsProblemDefinition.GetApplyCrossFibreTension() && DIM > 1)
// {
// double sheet_cross_fraction = mrElectroMechanicsProblemDefinition.GetSheetTensionFraction();
//
// for(unsigned i=0; i<DIM; i++)
// {
// mCurrentElementSheetDirection(i) = this->mChangeOfBasisMatrix(i,1);
// }
//
// double I4_sheet = inner_prod(mCurrentElementSheetDirection, prod(rC, mCurrentElementSheetDirection));
//
// // amend the stress and dTdE using the active tension
// double dTdE_coeff_s1 = -2*sheet_cross_fraction*detF*active_tension/(I4_sheet*I4_sheet); // note: I4*I4 = lam^4
//
// ///\todo #2180 The code below is specific to the implicit cardiac mechanics solver. Currently
// // the cross-fibre code is only tested using the explicit solver so the code below fails coverage.
// // This will need to be added back in once an implicit test is in place.
// double lambda_sheet = sqrt(I4_sheet);
// if(IsImplicitSolver())
// {
// double dt = mNextTime-mCurrentTime;
// dTdE_coeff_s1 += (d_act_tension_dlam + d_act_tension_d_dlamdt/dt)/(lambda_sheet*I4_sheet); // note: I4*lam = lam^3
// }
//
// rT += sheet_cross_fraction*(active_tension*detF/I4_sheet)*outer_prod(mCurrentElementSheetDirection,mCurrentElementSheetDirection);
//
// double dTdE_coeff_s2 = active_tension*detF/I4_sheet;
// if(addToDTdE)
// {
// for (unsigned M=0; M<DIM; M++)
// {
// for (unsigned N=0; N<DIM; N++)
// {
// for (unsigned P=0; P<DIM; P++)
// {
// for (unsigned Q=0; Q<DIM; Q++)
// {
// rDTdE(M,N,P,Q) += dTdE_coeff_s1 * mCurrentElementSheetDirection(M)
// * mCurrentElementSheetDirection(N)
// * mCurrentElementSheetDirection(P)
// * mCurrentElementSheetDirection(Q)
//
// + dTdE_coeff_s2 * mCurrentElementFibreDirection(M)
// * mCurrentElementFibreDirection(N)
// * invC(P,Q);
//
// }
// }
// }
// }
// }
// }
}
Bivector<Scalar, ToFrame> Permutation<FromFrame, ToFrame>::operator()(
Bivector<Scalar, FromFrame> const& bivector) const {
return Bivector<Scalar, ToFrame>(
Determinant() * (*this)(bivector.coordinates()));
}
//求逆矩阵
Matrix4& Matrix4::Invert() {
Matrix4 output;
//用于储存每次的余子式
Matrix3 tmp;
//////////////////////////
// 为什么又错了？？？ //
//////////////////////////
/*
float firstPart;
//先算分母，再求倒
firstPart =
(var[0][0] * var[1][1] * var[2][2] * var[3][3]) +
(var[0][1] * var[1][2] * var[2][3] * var[3][0]) +
(var[0][2] * var[1][3] * var[2][0] * var[3][1]) +
(var[0][3] * var[1][0] * var[2][1] * var[3][2]) -
(var[0][3] * var[1][2] * var[2][1] * var[3][0]) -
(var[0][2] * var[1][1] * var[2][0] * var[3][3]) -
(var[0][1] * var[1][0] * var[2][3] * var[3][2]) -
(var[0][0] * var[1][3] * var[2][2] * var[3][1]);
//求倒
firstPart = 1.0f / firstPart;
*/
//最里层的循环
int x, y;
//用于标识行列式的x y
int x_tmp, y_tmp;
for (int lop = 0; lop < 4; lop++) {
for (int lop2 = 0; lop2 < 4; lop2++) {
tmp.SetZero();
x_tmp = 0;
y_tmp = 0;
//为output矩阵的元素求值
//一行行一列列的进行计算
for (x = 0; x < 4; x++) {
y_tmp = 0;
//跳过当前元素
if (x == lop) {
continue;
}
for (y = 0; y < 4; y++) {
//跳过当前元素
if (y == lop2) {
continue;
}
tmp.var[x_tmp][y_tmp++] = var[x][y];
}
x_tmp++;
}
output.var[lop2][lop] = Determinant(tmp);
//带-1
if ((lop + 1 + lop2 + 1) % 2) {
output.var[lop2][lop] = (-1.0f)*output.var[lop2][lop];
}
}
}
//output = output * firstPart;
*this = output;
return *this;
}
bool Matrix4::IsOrthonormal() const
{
// Determinant is 1
return Abs(1.0f - Determinant()) < EPSILON;
}
void CompressibleExponentialLaw<DIM>::ComputeStressAndStressDerivative(c_matrix<double,DIM,DIM>& rC,
c_matrix<double,DIM,DIM>& rInvC,
double pressure /* not used */,
c_matrix<double,DIM,DIM>& rT,
FourthOrderTensor<DIM,DIM,DIM,DIM>& rDTdE,
bool computeDTdE)
{
static c_matrix<double,DIM,DIM> C_transformed;
static c_matrix<double,DIM,DIM> invC_transformed;
// The material law parameters are set up assuming the fibre direction is (1,0,0)
// and sheet direction is (0,1,0), so we have to transform C,inv(C),and T.
// Let P be the change-of-basis matrix P = (\mathbf{m}_f, \mathbf{m}_s, \mathbf{m}_n).
// The transformed C for the fibre/sheet basis is C* = P^T C P.
// We then compute T* = T*(C*), and then compute T = P T* P^T.
this->ComputeTransformedDeformationTensor(rC, rInvC, C_transformed, invC_transformed);
// Compute T*
c_matrix<double,DIM,DIM> E = 0.5*(C_transformed - mIdentity);
double QQ = 0;
for (unsigned M=0; M<DIM; M++)
{
for (unsigned N=0; N<DIM; N++)
{
QQ += mB[M][N]*E(M,N)*E(M,N);
}
}
assert(QQ < 10.0);///\todo #2193 This line is to trap for large deformations which lead to blow up in the exponential Uysk model
double multiplier = mA*exp(QQ);
rDTdE.Zero();
double J = sqrt(Determinant(rC));
for (unsigned M=0; M<DIM; M++)
{
for (unsigned N=0; N<DIM; N++)
{
rT(M,N) = multiplier*mB[M][N]*E(M,N) + mCompressibilityParam * J*log(J)*invC_transformed(M,N);
if (computeDTdE)
{
for (unsigned P=0; P<DIM; P++)
{
for (unsigned Q=0; Q<DIM; Q++)
{
rDTdE(M,N,P,Q) = multiplier * mB[M][N] * (M==P)*(N==Q)
+ 2*multiplier*mB[M][N]*mB[P][Q]*E(M,N)*E(P,Q)
+ mCompressibilityParam * (J*log(J) + J) * invC_transformed(M,N) * invC_transformed(P,Q)
- mCompressibilityParam * 2*J*log(J) * invC_transformed(M,P) * invC_transformed(Q,N);
}
}
}
}
}
// Now do: T = P T* P^T and dTdE_{MNPQ} = P_{Mm}P_{Nn}P_{Pp}P_{Qq} dT*dE*_{mnpq}
this->TransformStressAndStressDerivative(rT, rDTdE, computeDTdE);
}
////////////////////////////////////////////////////////////////////////////////
// Is Invertible ?
bool Matrix3x3::IsInvertible() const
{
return (Determinant() != 0.0f);
}
neBool SearchResult::SearchEETri(s32 flag, s32 aIndex, s32 bIndex, neBool & assigned)
{
assigned = false;
gEdgeStack.Init();
neByte edgeIndex;
if (flag == 0) //fv
{
// face of convex A
// vertex of triangle B
for (s32 i = 0; i < objA.mesh.numNeighbour; i++) // for each edge neighbour of Face aIndex
{
int j = 0;
while ((edgeIndex = objB.mesh.VertGetEdgeNeighbour(bIndex, j)) != 0xff)
{
gEdgeStack.Push(objA.mesh.FaceGetEdgeNeighbour(aIndex, i),
objB.mesh.VertGetEdgeNeighbour(bIndex, j));
j++;
}
}
}
else //vf
{
//vertex of convex A
//face of triangle B
s32 i = 0;
//for each edge neighbour incident to Vertex aIndex
while ((edgeIndex = objA.mesh.VertGetEdgeNeighbour(aIndex, i)) != 0xff)
{
for (s32 j = 0; j < objB.mesh.numNeighbour; j++)
{
gEdgeStack.Push(objA.mesh.VertGetEdgeNeighbour(aIndex, i),
objB.mesh.FaceGetEdgeNeighbour(bIndex, j));
}
i++;
}
}
while (!gEdgeStack.IsEmpty())
{
_num_edge_test++;
s32 edgeP, edgeQ;
gEdgeStack.Pop(edgeP, edgeQ);
// does the edge form a face
neV3 a = objA.GetWorldNormalByEdge1(edgeP);
neV3 b = objA.GetWorldNormalByEdge2(edgeP);
neV3 c = objB.GetWorldNormalByEdge1(edgeQ) * -1.0f;
neV3 d = objB.GetWorldNormalByEdge2(edgeQ) * -1.0f;
c += (TriEdgeDir[edgeQ] * 0.01f);
d += (TriEdgeDir[edgeQ] * 0.01f);
c.Normalize();
d.Normalize();
f32 cba = Determinant(c,b,a);
f32 dba = Determinant(d,b,a);
f32 prod0 = cba * dba;
if (prod0 >= -1.0e-6f)
{
continue;
}
f32 adc = Determinant(a,d,c);
f32 bdc = Determinant(b,d,c);
f32 prod1 = adc * bdc;
if (prod1 >= -1.0e-6f)
{
continue;
}
f32 prod2 = cba * bdc;
if (prod2 <= 1.0e-6f)
{
continue;
}
neV3 ai, bi;
neV3 naj, nbj;
objA.GetWorldEdgeVerts(edgeP, ai, bi);
objB.GetWorldEdgeVerts(edgeQ, naj, nbj);
naj *= -1.0f; nbj *= -1.0f;
neV3 ainaj = ai + naj;
neV3 ainbj = ai + nbj;
neV3 binaj = bi + naj;
//neV3 binbj = bi + nbj;
neV3 diff1 = ainaj - ainbj;
neV3 diff2 = ainaj - binaj ;
Face testFace;
testFace.normal = diff1.Cross(diff2);
f32 len = testFace.normal.Length();
if (neIsConsiderZero(len))
{
continue;
}
testFace.normal *= (1.0f / len);
testFace.k = testFace.normal.Dot(ainaj);
f32 testD = funcD(testFace);
if (testD >= 0)
return false;
if (testD <= dMax)
continue;
assigned = true;
dMax = testD;
face = testFace;
indexA = edgeP;
indexB = edgeQ;
typeA = SearchResult::EDGE;
typeB = SearchResult::EDGE;
// push
s32 i, j;
s32 vindex;
vindex = objB.mesh.EdgeGetVert1(edgeQ);
i = 0;
while ((j = objB.mesh.VertGetEdgeNeighbour(vindex, i)) != 0xff)
{
if (j != edgeQ)
gEdgeStack.Push(edgeP, j);
i++;
}
vindex = objB.mesh.EdgeGetVert2(edgeQ);
i = 0;
while ((j = objB.mesh.VertGetEdgeNeighbour(vindex, i)) != 0xff)
{
if (j != edgeQ)
gEdgeStack.Push(edgeP, j);
i++;
}
vindex = objA.mesh.EdgeGetVert1(edgeP);
i = 0;
while((j = objA.mesh.VertGetEdgeNeighbour(vindex, i)) != 0xff)
{
if (j != edgeP)
gEdgeStack.Push(j, edgeQ);
i++;
}
vindex = objA.mesh.EdgeGetVert2(edgeP);
//for (i = 0; i < objA.mesh.VertGetNumEdgeNeighbour(vindex); i++)
i = 0;
while ((j = objA.mesh.VertGetEdgeNeighbour(vindex, i)) != 0xff)
{
if (j != edgeP)
gEdgeStack.Push(j, edgeQ);
i++;
}
}
return true;
}
bool Matrix4::IsSingular() const
{
return (Determinant() == 0);
}
