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