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