Project: s41467-025-61292-1 | Function: Muscle::AddAnchor (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:** Creates muscle attachment points using linear blend skinning to smoothly interpolate muscle paths across skeletal segments for anatomically accurate modeling

**KEY TERMS:** {
  "lbs_body_nodes": "linear blend skinning body nodes",
  "lbs_weights": "skinning weights based on distance",
  "lbs_local_positions": "local attachment positions",
  "distance": "distance to each body node",
  "total_weight": "normalization factor",
  "l_mt0": "muscle-tendon unit rest length",
  "glob_pos": "global attachment position"
}

**CONTEXT CODE:** #include "Muscle.h"

using namespace MASS;
using namespace dart::dynamics;
std::vector<int> sort_indices(const std::vector<double> &val)
{
	std::vector<int> idx(val.size());
	std::iota(idx.begin(), idx.end(), 0);

	std::sort(idx.begin(), idx.end(), [&val](int i1, int i2)
			  { return val[i1] < val[i2]; });

	return idx;
}
Anchor::
	Anchor(std::vector<BodyNode *> bns, std::vector<Eigen::Vector3d> lps, std::vector<double> ws)
	: bodynodes(bns), local_positions(lps), weights(ws), num_related_bodies(bns.size())
{
}

Eigen::Vector3d
Anchor::
	GetPoint()
{
	Eigen::Vector3d p;
	p.setZero();
	for (int i = 0; i < num_related_bodies; i++)
		p += weights[i] * (bodynodes[i]->getTransform() * local_positions[i]);
	return p;
}

Muscle::
	Muscle(std::string _name, double _f0, double _lm0, double _lt0, double _pen_angle, double lmax, double _type1_fraction)
	: selected(false), name(_name), f0_original(_f0), f0(_f0), l_m0(_lm0), l_m(l_mt - l_t0), l_t0(_lt0), l_mt0(0.0), l_mt(1.0), activation(0.0), f_toe(0.33), k_toe(3.0), k_lin(51.878788), e_toe(0.02), e_t0(0.033), k_pe(5.5), e_mo(0.3), gamma(0.45), l_mt_max(lmax), type1_fraction(_type1_fraction)
{
	l_min = 0.99;
	l_max = 1.00;
	f_min = 0.99;
	f_max = 1.00;
}

void Muscle::
 void Muscle::
     AddAnchor(const dart::dynamics::SkeletonPtr &skel, dart::dynamics::BodyNode *bn, const Eigen::Vector3d &glob_pos, int num_related_bodies)
 {
     std::vector<dart::dynamics::BodyNode *> lbs_body_nodes;
     std::vector<Eigen::Vector3d> lbs_local_positions;
     std::vector<double> lbs_weights;

     // Get all body nodes in the skeleton
     std::vector<dart::dynamics::BodyNode*> allBodyNodes = skel->getBodyNodes();

     // Calculate distances and weights
     std::vector<double> distances;
     for (auto bodyNode : allBodyNodes) {
         Eigen::Vector3d bodyNodePos = bodyNode->getTransform().translation();
         double dist = (bodyNodePos - glob_pos).norm();
         distances.push_back(dist);
     }

     // Sort indices based on distance
     std::vector<int> sortedIndices = sort_indices(distances);

     // Select the closest 'num_related_bodies' body nodes
     int numSelected = std::min(num_related_bodies, static_cast<int>(allBodyNodes.size()));
     for (int i = 0; i < numSelected; ++i) {
         int idx = sortedIndices[i];
         dart::dynamics::BodyNode* bodyNode = allBodyNodes[idx];
         lbs_body_nodes.push_back(bodyNode);

         // Calculate weight based on distance
         double total_weight = 0.0;
         for (int j = 0; j < numSelected; ++j) {
             total_weight += 1.0 / (distances[sortedIndices[j]] + 1e-6);
         }

         double weight = (1.0 / (distances[idx] + 1e-6)) / total_weight;
         lbs_weights.push_back(weight);

         // Calculate local position
         Eigen::Vector3d local_pos = bodyNode->getTransform().inverse() * glob_pos;
         lbs_local_positions.push_back(local_pos);
     }

     // Normalize weights
     double sum_weights = std::accumulate(lbs_weights.begin(), lbs_weights.end(), 0.0);
     for (auto& weight : lbs_weights) {
         weight /= sum_weights;
     }

     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::
	AddAnchor(dart::dynamics::BodyNode *bn, const Eigen::Vector3d &glob_pos)
{
	std::vector<dart::dynamics::BodyNode *> lbs_body_nodes;
	std::vector<Eigen::Vector3d> lbs_local_positions;
	std::vector<double> lbs_weights;

	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)
		{


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