virtual double metric(stk_classic::mesh::Entity& element, bool& valid)
{
valid = true;
JacobianUtil jacA, jacSA, jacW;
jacSA.m_scale_to_unit = true;
double SA_ = 0.0, A_ = 0.0, W_ = 0.0; // current and reference detJ
jacA(A_, *m_eMesh, element, m_coord_field_current, m_topology_data);
jacSA(SA_, *m_eMesh, element, m_coord_field_current, m_topology_data);
jacW(W_, *m_eMesh, element, m_coord_field_original, m_topology_data);
double val=0.0, val_shape=0.0;
val_shape = 0.0;
//val_shape = std::numeric_limits<double>::max();
for (int i=0; i < jacA.m_num_nodes; i++)
{
double detAi = jacA.m_detJ[i];
double detSAi = jacSA.m_detJ[i];
//double detWi = jacW.m_detJ[i];
double FAi = Frobenius(jacA.m_detJ[i]);
double FWi = Frobenius(jacW.m_detJ[i]);
if (detAi < 0)
{
valid = false;
}
double shape_metric = 0.0;
//MsqMatrix<3,3>& A = jacA.m_J[i];
double scale_factor = (FAi < FWi ? FAi/FWi : (FAi > 1.e-6? FWi / FAi : 1.0));
scale_factor = FAi/FWi;
//scale_factor = 1.0;
//double sign_SA = (detSAi > 0.0 ? 1.0 : -1.0);
//double fac = 0.2;
//shape_metric = scale_factor* (detSAi > fac ? fac : detSAi);
shape_metric = scale_factor*detSAi;
val_shape += shape_metric;
//val_shape = std::min(val_shape, shape_metric);
//std::cout << "tmp srk i= " << i << " detAi = " << detAi << " detSAi= " << detSAi << " shape_metric= " << shape_metric << " val_shape= " << val_shape << " scale_factor= " << scale_factor << " FAi= " << FAi << " FWi= " << FWi << std::endl;
}
val = -val_shape;
//std::cout << "tmp srk val = " << val << std::endl;
return val;
}
virtual double metric(stk_classic::mesh::Entity& element, bool& valid)
{
valid = true;
JacobianUtil jacA, jacW;
int spatialDim = m_eMesh->get_spatial_dim();
double A_ = 0.0, W_ = 0.0; // current and reference detJ
jacA(A_, *m_eMesh, element, m_coord_field_current, m_topology_data);
jacW(W_, *m_eMesh, element, m_coord_field_original, m_topology_data);
double val=0.0, val_shape=0.0;
MsqMatrix<3,3> Ident;
identity(Ident);
MsqMatrix<3,3> WI, T;
for (int i=0; i < jacA.m_num_nodes; i++)
{
double detAi = jacA.m_detJ[i];
if (detAi < 0)
{
valid = false;
}
MsqMatrix<3,3>& W = jacW.m_J[i];
MsqMatrix<3,3>& A = jacA.m_J[i];
// frob2 = h^2 + h^2 + 1
// frob21 = 2 h^2
// f = h sqrt(2)
// det = h*h
// met = f*f / (det*2) - 1
// frob3 = 3 h^2
// f = h sqrt(3)
// det = h*h*h
// met = f*f*f/(3^3/2 *det) - 1 = f*f*f/(3*sqrt(3)*det) - 1
double shape_metric = 0.0;
if (std::fabs(detAi) > 1.e-15)
{
inverse(W, WI);
product(A, WI, T);
double d = det(T);
double f = my_sqr_Frobenius(T);
if (spatialDim==2)
{
// all our jacobians are 3D, with a 1 in the 3,3 slot for 2d, so we subtract it here
f = f - 1.0;
f = std::sqrt(f);
double fac = 2.0;
double den = fac * d;
shape_metric = (f*f)/den - 1.0;
}
else
{
f = std::sqrt(f);
double fac = 3.0*std::sqrt(3.0);
double den = fac * d;
shape_metric = (f*f*f)/den - 1.0;
}
//shape_metric = std::fabs(shape_metric);
//shape_metric = f/std::pow(den,1./3.) - 1.0;
}
val_shape += shape_metric;
//val_shape += std::fabs(shape_metric);
//val_shape += shape_metric*shape_metric;
}
val = val_shape;
//val = val_shape*val_shape;
return val;
}
void btSolve2LinearConstraint::resolveUnilateralPairConstraint(
btRigidBody* body1,
btRigidBody* body2,
const btMatrix3x3& world2A,
const btMatrix3x3& world2B,
const btVector3& invInertiaADiag,
const btScalar invMassA,
const btVector3& linvelA,const btVector3& angvelA,
const btVector3& rel_posA1,
const btVector3& invInertiaBDiag,
const btScalar invMassB,
const btVector3& linvelB,const btVector3& angvelB,
const btVector3& rel_posA2,
btScalar depthA, const btVector3& normalA,
const btVector3& rel_posB1,const btVector3& rel_posB2,
btScalar depthB, const btVector3& normalB,
btScalar& imp0,btScalar& imp1)
{
(void)linvelA;
(void)linvelB;
(void)angvelB;
(void)angvelA;
imp0 = btScalar(0.);
imp1 = btScalar(0.);
btScalar len = btFabs(normalA.length()) - btScalar(1.);
if (btFabs(len) >= SIMD_EPSILON)
return;
btAssert(len < SIMD_EPSILON);
//this jacobian entry could be re-used for all iterations
btJacobianEntry jacA(world2A,world2B,rel_posA1,rel_posA2,normalA,invInertiaADiag,invMassA,
invInertiaBDiag,invMassB);
btJacobianEntry jacB(world2A,world2B,rel_posB1,rel_posB2,normalB,invInertiaADiag,invMassA,
invInertiaBDiag,invMassB);
//const btScalar vel0 = jacA.getRelativeVelocity(linvelA,angvelA,linvelB,angvelB);
//const btScalar vel1 = jacB.getRelativeVelocity(linvelA,angvelA,linvelB,angvelB);
const btScalar vel0 = normalA.dot(body1->getVelocityInLocalPoint(rel_posA1)-body2->getVelocityInLocalPoint(rel_posA1));
const btScalar vel1 = normalB.dot(body1->getVelocityInLocalPoint(rel_posB1)-body2->getVelocityInLocalPoint(rel_posB1));
// btScalar penetrationImpulse = (depth*contactTau*timeCorrection) * massTerm;//jacDiagABInv
btScalar massTerm = btScalar(1.) / (invMassA + invMassB);
// calculate rhs (or error) terms
const btScalar dv0 = depthA * m_tau * massTerm - vel0 * m_damping;
const btScalar dv1 = depthB * m_tau * massTerm - vel1 * m_damping;
// dC/dv * dv = -C
// jacobian * impulse = -error
//
//impulse = jacobianInverse * -error
// inverting 2x2 symmetric system (offdiagonal are equal!)
//
btScalar nonDiag = jacA.getNonDiagonal(jacB,invMassA,invMassB);
btScalar invDet = btScalar(1.0) / (jacA.getDiagonal() * jacB.getDiagonal() - nonDiag * nonDiag );
//imp0 = dv0 * jacA.getDiagonal() * invDet + dv1 * -nonDiag * invDet;
//imp1 = dv1 * jacB.getDiagonal() * invDet + dv0 * - nonDiag * invDet;
imp0 = dv0 * jacA.getDiagonal() * invDet + dv1 * -nonDiag * invDet;
imp1 = dv1 * jacB.getDiagonal() * invDet + dv0 * - nonDiag * invDet;
//[a b] [d -c]
//[c d] inverse = (1 / determinant) * [-b a] where determinant is (ad - bc)
//[jA nD] * [imp0] = [dv0]
//[nD jB] [imp1] [dv1]
}
