Project: s41467-025-61292-1 | Function: Muscle::ApplyForceToBody (cpp)

system: You are an expert C++ programmer implementing algorithms from scientific papers.
Complete the masked function based on the algorithm description.
Follow C++ best practices and maintain consistent code style.
Return **only** the complete C++ function code.

user: Complete this C++ function:

**DESCRIPTION:** Applies muscle forces to connected body segments based on muscle activation and length-tension relationships for biomechanically realistic gait simulation

**KEY TERMS:** {
  "GetForce()": "total muscle force (active + passive)",
  "mAnchors": "muscle attachment points",
  "mCachedAnchorPositions": "current anchor positions",
  "dir": "force direction vector",
  "addExtForce": "apply external force to body",
  "activation": "muscle activation level",
  "f": "muscle force magnitude"
}

**CONTEXT CODE:** 
	lbs_body_nodes.push_back(bn);
	lbs_local_positions.push_back(bn->getTransform().inverse() * glob_pos);
	lbs_weights.push_back(1.0);

	mAnchors.push_back(new Anchor(lbs_body_nodes, lbs_local_positions, lbs_weights));

	int n = mAnchors.size();
	if (n > 1)
		l_mt0 += (mAnchors[n - 1]->GetPoint() - mAnchors[n - 2]->GetPoint()).norm();

	l_mt0_original = l_mt0;

	mCachedAnchorPositions.resize(n);
}
void Muscle::
	SetMuscle()
{
	int n = mAnchors.size();
	l_mt0 = 0;
	for (int i = 1; i < n; i++)
		l_mt0 += (mAnchors[i]->GetPoint() - mAnchors[i - 1]->GetPoint()).norm();
	l_mt0_original = l_mt0;

	Update();
	Eigen::MatrixXd Jt = GetJacobianTranspose();
	auto Ap = GetForceJacobianAndPassive();
	Eigen::VectorXd JtA = Jt * Ap.first;
	num_related_dofs = 0;
	related_dof_indices.clear();

	related_vec = Eigen::VectorXd::Zero(JtA.rows());

	for (int i = 0; i < JtA.rows(); i++)
	{
		if (std::abs(JtA[i]) > 1E-10)
		{
			num_related_dofs++;
			related_dof_indices.push_back(i);
		}
		if (JtA[i] > 1E-10)
			related_vec[i] = 1;
		else if (JtA[i] < -1E-10)
			related_vec[i] = -1;
		else
			related_vec[i] = 0;
	}
}

void Muscle::
 void Muscle::ApplyForceToBody()
 {
     // Calculate the total muscle force
     double force = GetForce();

     // Calculate the direction vector of the muscle force
     Eigen::Vector3d dir = Eigen::Vector3d::Zero();
     for (int i = 1; i < mAnchors.size(); i++)
     {
         dir += (mCachedAnchorPositions[i] - mCachedAnchorPositions[i - 1]).normalized();
     }
     dir.normalize();

     // Apply the force to each anchor point
     for (int i = 0; i < mAnchors.size(); i++)
     {
         // The force is distributed equally among the anchor points
         Eigen::Vector3d forceVec = force * dir;

         // Apply the force to the body node
         mAnchors[i]->bodynodes[0]->addExtForce(forceVec);
     }
 }
double Muscle::
	GetNormalizedLength()
{
	return l_m / l_m0;
}
void Muscle::
	Update()
{
	for (int i = 0; i < mAnchors.size(); i++)
		mCachedAnchorPositions[i] = mAnchors[i]->GetPoint();
	l_mt = Getl_mt();

	l_m = l_mt - l_t0;
}
double
Muscle::
	GetForce()
{
	return Getf_A() * activation + Getf_p();
}
double
Muscle::
	Getf_A()
{
	return f0 * g_al(l_m / l_m0);
}
double
Muscle::
	Getf_p()
{
	return f0 * g_pl(l_m / l_m0);
}
double
Muscle::
	Getl_mt()
{
	length = 0.0;
	for (int i = 1; i < mAnchors.size(); i++)
		length += (mCachedAnchorPositions[i] - mCachedAnchorPositions[i - 1]).norm();

	return length / l_mt0;
}
Eigen::VectorXd
Muscle::
	GetRelatedJtA()
{
	Eigen::MatrixXd Jt_reduced = GetReducedJacobianTranspose();
	Eigen::VectorXd A = GetForceJacobianAndPassive().first;
	Eigen::VectorXd JtA_reduced = Jt_reduced * A;
	return JtA_reduced;
}

Eigen::MatrixXd
Muscle::
	GetReducedJacobianTranspose()
{
	const auto &skel = mAnchors[0]->bodynodes[0]->getSkeleton();
	Eigen::MatrixXd Jt(num_related_dofs, 3 * mAnchors.size());

	Jt.setZero();
	for (int i = 0; i < mAnchors.size(); i++)
	{
		auto bn = mAnchors[i]->bodynodes[0];
		dart::math::Jacobian J = dart::math::Jacobian::Zero(6, num_related_dofs);
		for (int j = 0; j < num_related_dofs; j++)
		{
			auto &indices = bn->getDependentGenCoordIndices();
			int idx = std::find(indices.begin(), indices.end(), related_dof_indices[j]) - indices.begin();
			if (idx != indices.size())
				J.col(j) = bn->getJacobian().col(idx);
		}
		Eigen::Vector3d offset = mAnchors[i]->bodynodes[0]->getTransform().inverse() * mCachedAnchorPositions[i];
		dart::math::LinearJacobian JLinear = J.bottomRows<3>() + J.topRows<3>().colwise().cross(offset);
		Jt.block(0, i * 3, num_related_dofs, 3) = (bn->getTransform().linear() * JLinear).transpose();
	}
	return Jt;
}


Complete the function implementation replacing "throw std::runtime_error("Not implemented");".