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