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