void Fem3DElementTetrahedron::initialize(const SurgSim::Math::OdeState& state)
{
// Test the validity of the physical parameters
FemElement::initialize(state);
for (auto nodeId = m_nodeIds.cbegin(); nodeId != m_nodeIds.cend(); nodeId++)
{
SURGSIM_ASSERT(*nodeId >= 0 && *nodeId < state.getNumNodes())
<< "Invalid nodeId " << *nodeId << " expected in range [0.." << state.getNumNodes() - 1 << "]";
}
// Store the rest state for this tetrahedron in m_x0
getSubVector(state.getPositions(), m_nodeIds, 3, &m_x0);
// Verify the Counter clock-wise condition
auto A = getSubVector(m_x0, 0, 3);
auto B = getSubVector(m_x0, 1, 3);
auto C = getSubVector(m_x0, 2, 3);
auto D = getSubVector(m_x0, 3, 3);
SurgSim::Math::Vector3d AB = B - A;
SurgSim::Math::Vector3d AC = C - A;
SurgSim::Math::Vector3d AD = D - A;
SURGSIM_LOG_IF(AB.cross(AC).dot(AD) < 0, SurgSim::Framework::Logger::getDefaultLogger(), WARNING)
<< "Tetrahedron ill-defined (ABC defined counter clock viewed from D) with node ids[" <<
m_nodeIds[0] << ", " << m_nodeIds[1] << ", " << m_nodeIds[2] << ", " << m_nodeIds[3] << "]";
// Pre-compute the mass and stiffness matrix
computeMass(state, &m_M);
computeStiffness(state, &m_K);
}
void Fem3DElementCube::initialize(const SurgSim::Math::OdeState& state)
{
// Test the validity of the physical parameters
FemElement::initialize(state);
// Set the shape functions coefficients
// Ni(epsilon, eta, mu) = (1 + epsilon * sign(epsilon_i))(1 + eta * sign(eta_i))(1 + mu * sign(mu_i))/8
std::array<double, 8> tmpEpsilon = {{ -1.0, +1.0, +1.0, -1.0, -1.0, +1.0, +1.0, -1.0}};
std::array<double, 8> tmpEta = {{ -1.0, -1.0, +1.0, +1.0, -1.0, -1.0, +1.0, +1.0}};
std::array<double, 8> tmpMu = {{ -1.0, -1.0, -1.0, -1.0, +1.0, +1.0, +1.0, +1.0}};
m_shapeFunctionsEpsilonSign = tmpEpsilon;
m_shapeFunctionsEtaSign = tmpEta;
m_shapeFunctionsMuSign = tmpMu;
// Store the 8 nodeIds in order
for (auto nodeId = m_nodeIds.cbegin(); nodeId != m_nodeIds.cend(); ++nodeId)
{
SURGSIM_ASSERT(*nodeId >= 0 && *nodeId < state.getNumNodes()) <<
"Invalid nodeId " << *nodeId << " expected in range [0.." << state.getNumNodes() - 1 << "]";
}
// Store the rest state for this cube in m_elementRestPosition
getSubVector(state.getPositions(), m_nodeIds, 3, &m_elementRestPosition);
// Compute the cube rest volume
m_restVolume = getVolume(state);
// Pre-compute the mass and stiffness matrix
computeMass(state, &m_M);
computeStiffness(state, &m_strain, &m_stress, &m_K);
}
void SetUp() override
{
m_nodeIds[0] = 3;
m_nodeIds[1] = 1;
m_nodeIds[2] = 14;
m_nodeIds[3] = 9;
m_nodeIdsAsVector.assign(m_nodeIds.cbegin(), m_nodeIds.cend());
m_restState.setNumDof(3, 15);
m_invalidState.setNumDof(3, 15);
Vector& x0 = m_restState.getPositions();
Vector& invalidx0 = m_invalidState.getPositions();
std::array<Vector3d, 4> points = {{
Vector3d(0.0, 0.0, 0.0), Vector3d(1.0, 0.0, 0.0),
Vector3d(0.0, 1.0, 0.0), Vector3d(0.0, 0.0, 1.0)
}
};
// Tet is aligned with the axis (X,Y,Z), centered on (0.0, 0.0, 0.0), embedded in a cube of size 1
for (size_t nodeId = 0; nodeId < 4; ++nodeId)
{
SurgSim::Math::getSubVector(x0, m_nodeIds[nodeId], 3) = points[nodeId];
SurgSim::Math::getSubVector(invalidx0, m_nodeIds[nodeId], 3) = points[nodeId];
}
// In the invalid state, the tetrahedron is degenerated to a triangle (last 2 points are equal)
SurgSim::Math::getSubVector(invalidx0, m_nodeIds[3], 3) = points[2];
m_rho = 1000.0;
m_E = 1e6;
m_nu = 0.45;
Vector3d axis(1.0, 0.2, -0.3);
axis.normalize();
m_rotation = SurgSim::Math::makeRotationMatrix(4.1415, axis);
m_R12x12 = Eigen::Matrix<double, 12, 12>::Zero();
for (size_t nodeId = 0; nodeId < 4; ++nodeId)
{
m_R12x12.block<3, 3>(3 * nodeId, 3 * nodeId) = m_rotation;
}
m_translation = Vector3d(1.2, 2.3, 3.4);
}
SurgSim::Math::Vector Fem3DElementTetrahedron::computeCartesianCoordinate(
const SurgSim::Math::OdeState& state,
const SurgSim::Math::Vector& naturalCoordinate) const
{
SURGSIM_ASSERT(isValidCoordinate(naturalCoordinate))
<< "naturalCoordinate must be normalized and length 4.";
const Vector& x = state.getPositions();
Vector3d p0 = getSubVector(x, m_nodeIds[0], 3);
Vector3d p1 = getSubVector(x, m_nodeIds[1], 3);
Vector3d p2 = getSubVector(x, m_nodeIds[2], 3);
Vector3d p3 = getSubVector(x, m_nodeIds[3], 3);
return naturalCoordinate(0) * p0
+ naturalCoordinate(1) * p1
+ naturalCoordinate(2) * p2
+ naturalCoordinate(3) * p3;
}
double Fem3DElementTetrahedron::getVolume(const SurgSim::Math::OdeState& state) const
{
/// Computes the tetrahedron volume 1/6 * | 1 p0x p0y p0z |
/// | 1 p1x p1y p1z |
/// | 1 p2x p2y p2z |
/// | 1 p3x p3y p3z |
const Vector& x = state.getPositions();
auto p0 = getSubVector(x, m_nodeIds[0], 3);
auto p1 = getSubVector(x, m_nodeIds[1], 3);
auto p2 = getSubVector(x, m_nodeIds[2], 3);
auto p3 = getSubVector(x, m_nodeIds[3], 3);
// fabs is necessary if we don't pay attention to the indexing !
// If the tetrahedron verify ABC counter clock wise viewed from D, this determinant is always positive = 6V
// i.e. dot( cross(AB, AC), AD ) > 0
// Otherwise, it can happen that this determinant is negative = -6V !!
return (det(p1, p2, p3) - det(p0, p2, p3) + det(p0, p1, p3) - det(p0, p1, p2)) / 6.0;
}
SurgSim::Math::Vector Fem3DElementCube::computeCartesianCoordinate(
const SurgSim::Math::OdeState& state,
const SurgSim::Math::Vector& naturalCoordinate) const
{
SURGSIM_ASSERT(isValidCoordinate(naturalCoordinate))
<< "naturalCoordinate must be normalized and length 8 within [0 1].";
Vector3d cartesianCoordinate(0.0, 0.0, 0.0);
const Vector& positions = state.getPositions();
for (int i = 0; i < 8; i++)
{
cartesianCoordinate += naturalCoordinate(i) * getSubVector(positions, m_nodeIds[i], 3);
}
return cartesianCoordinate;
}
void Fem3DElementCube::evaluateJ(const SurgSim::Math::OdeState& state, double epsilon, double eta, double mu,
SurgSim::Math::Matrix33d* J,
SurgSim::Math::Matrix33d* Jinv,
double* detJ) const
{
using SurgSim::Framework::Logger;
SURGSIM_ASSERT(J != nullptr) << "Trying to evaluate J with a nullptr for matrix J";
Vector3d p[8];
for (size_t index = 0; index < 8; index++)
{
p[index] = state.getPosition(m_nodeIds[index]);
}
// Zero out the matrix J
J->setZero();
// Compute J = d(x,y,z)/d(epsilon,eta,mu)
// Note that (x,y,z) = for(i in {0..7}){ (x,y,z) += (xi,yi,zi).Ni(epsilon,eta,mu)}
for (size_t index = 0; index < 8; ++index)
{
for (size_t axis = 0; axis < 3; ++axis)
{
(*J)(0, axis) += p[index][axis] * dShapeFunctiondepsilon(index, epsilon, eta, mu);
(*J)(1, axis) += p[index][axis] * dShapeFunctiondeta(index, epsilon, eta, mu);
(*J)(2, axis) += p[index][axis] * dShapeFunctiondmu(index, epsilon, eta, mu);
}
}
if (Jinv != nullptr && detJ != nullptr)
{
bool invertible;
J->computeInverseAndDetWithCheck(*Jinv, *detJ, invertible);
SURGSIM_ASSERT(invertible) <<
"Found a non invertible matrix J\n" << *J << "\ndet(J)=" << *detJ <<
") while computing Fem3DElementCube stiffness matrix\n";
SURGSIM_LOG_IF(*detJ <= 1e-8 && *detJ >= -1e-8, Logger::getLogger("Physics"), WARNING) <<
"Found an invalid matrix J\n" << *J << "\ninvertible, but det(J)=" << *detJ <<
") while computing Fem3DElementCube stiffness matrix\n";
}
}
void SetUp() override
{
using SurgSim::Math::getSubVector;
using SurgSim::Math::getSubMatrix;
using SurgSim::Math::addSubMatrix;
m_nodeIds[0] = 3;
m_nodeIds[1] = 1;
m_nodeIds[2] = 14;
m_nodeIds[3] = 9;
std::vector<size_t> m_nodeIdsVectorForm; // Useful for assembly helper function
m_nodeIdsVectorForm.push_back(m_nodeIds[0]);
m_nodeIdsVectorForm.push_back(m_nodeIds[1]);
m_nodeIdsVectorForm.push_back(m_nodeIds[2]);
m_nodeIdsVectorForm.push_back(m_nodeIds[3]);
m_restState.setNumDof(3, 15);
Vector& x0 = m_restState.getPositions();
// Tet is aligned with the axis (X,Y,Z), centered on (0.1, 1.2, 2.3), embedded in a cube of size 1
getSubVector(m_expectedX0, 0, 3) = getSubVector(x0, m_nodeIds[0], 3) = Vector3d(0.1, 1.2, 2.3);
getSubVector(m_expectedX0, 1, 3) = getSubVector(x0, m_nodeIds[1], 3) = Vector3d(1.1, 1.2, 2.3);
getSubVector(m_expectedX0, 2, 3) = getSubVector(x0, m_nodeIds[2], 3) = Vector3d(0.1, 2.2, 2.3);
getSubVector(m_expectedX0, 3, 3) = getSubVector(x0, m_nodeIds[3], 3) = Vector3d(0.1, 1.2, 3.3);
// The tet is part of a cube of size 1x1x1 (it occupies 1/6 of the cube's volume)
m_expectedVolume = 1.0 / 6.0;
m_rho = 1000.0;
m_E = 1e6;
m_nu = 0.45;
m_expectedMassMatrix.setZero(3*15, 3*15);
m_expectedDampingMatrix.setZero(3*15, 3*15);
m_expectedStiffnessMatrix.setZero(3*15, 3*15);
m_expectedStiffnessMatrix2.setZero(3*15, 3*15);
m_vectorOnes.setOnes(3*15);
Eigen::Matrix<double, 12, 12> M = Eigen::Matrix<double, 12, 12>::Zero();
{
M.diagonal().setConstant(2.0);
M.block(0, 3, 9, 9).diagonal().setConstant(1.0);
M.block(0, 6, 6, 6).diagonal().setConstant(1.0);
M.block(0, 9, 3, 3).diagonal().setConstant(1.0);
M.block(3, 0, 9, 9).diagonal().setConstant(1.0);
M.block(6, 0, 6, 6).diagonal().setConstant(1.0);
M.block(9, 0, 3, 3).diagonal().setConstant(1.0);
}
M *= m_rho * m_expectedVolume / 20.0;
addSubMatrix(M, m_nodeIdsVectorForm, 3 , &m_expectedMassMatrix);
Eigen::Matrix<double, 12, 12> K = Eigen::Matrix<double, 12, 12>::Zero();
{
// Calculation done by hand from
// http://www.colorado.edu/engineering/CAS/courses.d/AFEM.d/AFEM.Ch09.d/AFEM.Ch09.pdf
// ai = {}
// bi = {-1 1 0 0}
// ci = {-1 0 1 0}
// di = {-1 0 0 1}
Eigen::Matrix<double, 6, 12> B = Eigen::Matrix<double, 6, 12>::Zero();
Eigen::Matrix<double, 6, 6> E = Eigen::Matrix<double, 6, 6>::Zero();
B(0, 0) = -1; B(0, 3) = 1;
B(1, 1) = -1; B(1, 7) = 1;
B(2, 2) = -1; B(2, 11) = 1;
B(3, 0) = -1; B(3, 1) = -1; B(3, 4) = 1; B(3, 6) = 1;
B(4, 1) = -1; B(4, 2) = -1; B(4, 8) = 1; B(4, 10) = 1;
B(5, 0) = -1; B(5, 2) = -1; B(5, 5) = 1; B(5, 9) = 1;
B *= 1.0 / (6.0 * m_expectedVolume);
E.block(0, 0, 3, 3).setConstant(m_nu);
E.block(0, 0, 3, 3).diagonal().setConstant(1.0 - m_nu);
E.block(3, 3, 3, 3).diagonal().setConstant(0.5 - m_nu);
E *= m_E / (( 1.0 + m_nu) * (1.0 - 2.0 * m_nu));
K = m_expectedVolume * B.transpose() * E * B;
}
addSubMatrix(K, m_nodeIdsVectorForm, 3 , &m_expectedStiffnessMatrix);
// Expecte stiffness matrix given for our case in:
// http://www.colorado.edu/engineering/CAS/courses.d/AFEM.d/AFEM.Ch09.d/AFEM.Ch09.pdf
double E = m_E / (12.0*(1.0 - 2.0*m_nu)*(1.0 + m_nu));
double n0 = 1.0 - 2.0 * m_nu;
double n1 = 1.0 - m_nu;
K.setZero();
// Fill up the upper triangle part first (without diagonal elements)
K(0, 1) = K(0, 2) = K(1, 2) = 1.0;
K(0, 3) = -2.0 * n1; K(0, 4) = -n0; K(0, 5) = -n0;
K(1, 3) = -2.0 * m_nu; K(1, 4) = -n0;
K(2, 3) = -2.0 * m_nu; K(2, 5) = -n0;
K(0, 6) = - n0; K(0, 7) = -2.0 * m_nu;
K(1, 6) = - n0; K(1, 7) = -2.0 * n1; K(1, 8) = - n0;
K(2, 7) = - 2.0 * m_nu; K(2, 8) = -n0;
K(0, 9) = - n0; K(0, 11) = -2.0 * m_nu;
K(1, 10) = - n0; K(1, 11) = -2.0 * m_nu;
K(2, 9) = - n0; K(2, 10) = - n0; K(2, 11) = -2.0 * n1;
K(3, 7) = K(3, 11) = 2.0 * m_nu;
K(4, 6) = n0;
K(5, 9) = n0;
K(7, 11) = 2.0 * m_nu;
K(8, 10) = n0;
K += K.transpose().eval(); // symmetric part (do not forget the .eval() !)
K.block(0,0,3,3).diagonal().setConstant(4.0 - 6.0 * m_nu); // diagonal elements
K.block(3,3,9,9).diagonal().setConstant(n0); // diagonal elements
K(3, 3) = K(7, 7) = K(11, 11) = 2.0 * n1; // diagonal elements
K *= E;
addSubMatrix(K, m_nodeIdsVectorForm, 3 , &m_expectedStiffnessMatrix2);
}
void Fem3DElementTetrahedron::computeShapeFunctions(const SurgSim::Math::OdeState& state,
double* volume,
std::array<double, 4>* ai,
std::array<double, 4>* bi,
std::array<double, 4>* ci,
std::array<double, 4>* di) const
{
// The tetrahedron nodes 3D position {a,b,c,d}
Vector3d a = getSubVector(state.getPositions(), m_nodeIds[0], 3);
Vector3d b = getSubVector(state.getPositions(), m_nodeIds[1], 3);
Vector3d c = getSubVector(state.getPositions(), m_nodeIds[2], 3);
Vector3d d = getSubVector(state.getPositions(), m_nodeIds[3], 3);
*volume = getVolume(state);
// See http://www.colorado.edu/engineering/CAS/courses.d/AFEM.d/AFEM.Ch09.d/AFEM.Ch09.pdf for more details.
// Relationship between the notations in this source code and the document mentioned above:
// a(x1 y1 z1) b(x2 y2 z2) c(x3 y3 z3) d(x4 y4 z4)
// Shape functions link the 3D space (x, y, z) to the natural parametrization (sigmai) of the shape.
// (i.e. sigmai are the barycentric coordinates for the respective 4 nodes of the tetrahedon)
// (1) ( 1 1 1 1) (sigma1)
// (x) = (x1 x2 x3 x4) (sigma2)
// (y) (y1 y2 y3 y4) (sigma3)
// (z) (z1 z2 z3 z4) (sigma4)
//
// The shape functions Ni(x, y, z) are given by the inverse relationship:
// (sigma1) ( 1 1 1 1)^-1 (1) (a[0] b[0] c[0] d[0]) (1) | 1 1 1 1|
// (sigma2) = (x1 x2 x3 x4) (x) = 1/6V (a[1] b[1] c[1] d[1]) (x) where |x1 x2 x3 x4| = 6V
// (sigma3) (y1 y2 y3 y4) (y) (a[2] b[2] c[2] d[2]) (y) |y1 y2 y3 y4|
// (sigma4) (z1 z2 z3 z4) (z) (a[3] b[3] c[3] d[3]) (z) |z1 z2 z3 z4|
// Computes the shape functions parameters m_ai (noted 6V0i in the document mentioned above, eq 9.12)
// m_ai[0] = 6V01 = 6V(origin,b,c,d) = x2(y3z4 - y4z3) + x3(y4z2 - y2z4) + x4(y2z3 - y3z2) = |b c d|
// m_ai[1] = 6V02 = 6V(origin,c,d,a) = x1(y4z3 - y3z4) + x3(y1z4 - y4z1) + x4(y3z1 - y1z3) = -|a c d|
// m_ai[2] = 6V03 = 6V(origin,d,a,b) = x1(y2z4 - y4z2) + x2(y4z1 - y1z4) + x4(y1z2 - y2z1) = |a b d|
// m_ai[3] = 6V04 = 6V(origin,a,b,c) = x1(y3z2 - y2z3) + x2(y1z3 - y3z1) + x3(y2z1 - y1z2) = -|a b c|
(*ai)[0] = det(b, c, d);
(*ai)[1] = -det(a, c, d);
(*ai)[2] = det(a, b, d);
(*ai)[3] = -det(a, b, c);
// Computes the shape function parameters m_bi (noted ai in the document mentioned above, eq 9.11)
// m_bi[0] = y42z32 - y32z42 = (y4-y2)(z3-z2) - (y3-y2)(z4-z2) = |1 y2 z2| = |1 by bz|
// -|1 y3 z3| -|1 cy cz|
// |1 y4 z4| |1 dy dz|
//
// m_bi[1] = y31z43 - y34z13 = (y3-y1)(z4-z3) - (y3-y4)(z1-z3) = |1 y1 z1| = |1 ay az|
// |1 y3 z3| |1 cy cz|
// |1 y4 z4| |1 dy dz|
//
// m_bi[2] = y24z14 - y14z24 = (y2-y4)(z1-z4) - (y1-y4)(z2-z4) = |1 y1 z1| = |1 ay az|
// -|1 y2 z2| -|1 by bz|
// |1 y4 z4| |1 dy dz|
//
// m_bi[3] = y13z21 - y12z31 = (y1-y3)(z2-z1) - (y1-y2)(z3-z1) = |1 y1 z1| = |1 ay az|
// |1 y2 z2| |1 by bz|
// |1 y3 z3| |1 cy cz|
{
Vector3d atilde(1, a[1], a[2]);
Vector3d btilde(1, b[1], b[2]);
Vector3d ctilde(1, c[1], c[2]);
Vector3d dtilde(1, d[1], d[2]);
(*bi)[0] = -det(btilde, ctilde, dtilde);
(*bi)[1] = det(atilde, ctilde, dtilde);
(*bi)[2] = -det(atilde, btilde, dtilde);
(*bi)[3] = det(atilde, btilde, ctilde);
}
// Computes the shape function parameters m_ci (noted bi in the document mentioned above, eq 9.11)
// m_ci[0] = x32z42 - x42z32 = (x3-x2)(z4-z2) - (x4-x2)(z3-z2) = |1 x2 z2| = |1 bx bz|
// |1 x3 z3| |1 cx cz|
// |1 x4 z4| |1 dx dz|
//
// m_ci[1] = x43z31 - x13z34 = (x4-x3)(z3-z1) - (x1-x3)(z3-z4) = |1 x1 z1| = |1 ax az|
// -|1 x3 z3| -|1 cx cz|
// |1 x4 z4| |1 dx dz|
//
// m_ci[2] = x14z24 - x24z14 = (x1-x4)(z2-z4) - (x2-x4)(z1-z4) = |1 x1 z1| = |1 ax az|
// |1 x2 z2| |1 bx bz|
// |1 x4 z4| |1 dx dz|
//
// m_ci[3] = x21z13 - x31z12 = (x2-x1)(z1-z3) - (x3-x1)(z1-z2) = |1 x1 z1| = |1 ax az|
// -|1 x2 z2| -|1 bx bz|
// |1 x3 z3| |1 cx cz|
{
Vector3d atilde(1, a[0], a[2]);
Vector3d btilde(1, b[0], b[2]);
Vector3d ctilde(1, c[0], c[2]);
Vector3d dtilde(1, d[0], d[2]);
(*ci)[0] = det(btilde, ctilde, dtilde);
(*ci)[1] = -det(atilde, ctilde, dtilde);
(*ci)[2] = det(atilde, btilde, dtilde);
(*ci)[3] = -det(atilde, btilde, ctilde);
}
// Computes the shape function parameters m_di (noted ci in the document mentioned above, eq 9.11)
// m_di[0] = x42y32 - x32y42 = (x4-x2)(y3-y2) - (x3-x2)(y4-y2) = |1 x2 y2| = |1 bx by|
// -|1 x3 y3| -|1 cx cy|
// |1 x4 y4| |1 dx dy|
//
// m_di[1] = x31y43 - x34y13 = (x3-x1)(y4-y3) - (x3-x4)(y1-y3) = |1 x1 y1| = |1 ax ay|
// |1 x3 y3| |1 cx cy|
// |1 x4 y4| |1 dx dy|
//
// m_di[2] = x24y14 - x14y24 = (x2-x4)(y1-y4) - (x1-x4)(y2-y4) = |1 x1 y1| = |1 ax ay|
// -|1 x2 y2| -|1 bx by|
// |1 x4 y4| |1 dx dy|
//
// m_di[3] = x13y21 - x12y31 = (x1-x3)(y2-y1) - (x1-x2)(y3-y1) = |1 x1 y1| = |1 ax ay|
// |1 x2 y2| |1 bx by|
// |1 x3 y3| |1 cx cy|
{
Vector3d atilde(1, a[0], a[1]);
Vector3d btilde(1, b[0], b[1]);
Vector3d ctilde(1, c[0], c[1]);
Vector3d dtilde(1, d[0], d[1]);
(*di)[0] = -det(btilde, ctilde, dtilde);
(*di)[1] = det(atilde, ctilde, dtilde);
(*di)[2] = -det(atilde, btilde, dtilde);
(*di)[3] = det(atilde, btilde, ctilde);
}
}