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