This is the Pseudo code of Direct Position-Based Solver for Stiff Rods

XPBDRod.cpp

// Eqations are in paper: Direct Position-Based Solver for Stiff Rods, Crispin Deul et.al. 2018
Real m_rodYoungModulus; //in Equation 5
Real m_rodRadius; //in Equation 6
Real m_rodInvZeroStretchStiffness;//in Equation 24: zero-stretch compliance \sigma^{-1}
Real m_rodDensity; //in Equation 28
Vector6r m_complianceVector; //Equation 24: Diagonal compliance matrix \alpha is representd as a 6x1 vector
Real timestep = 0.005;
void init()
{
	m_rodYoungModulus = 1.0e3;
	m_rodRadius = 0.5;
	m_rodInvZeroStretchStiffness = 10e-9;
	m_rodDensity = 1000;

	// init Equation 24
	Real tmp = m_rodYoungModulus * M_PI * m_rodRadius * m_rodRadius * m_rodRadius * m_rodRadius;
	m_complianceVector[0] = m_rodInvZeroStretchStiffness;
	m_complianceVector[1] = m_rodInvZeroStretchStiffness;
	m_complianceVector[2] = m_rodInvZeroStretchStiffness;
	m_complianceVector[3] = 4/ tmp;
	m_complianceVector[4] = 4/ tmp;
	m_complianceVector[5] = 2/ tmp;
	m_complianceVector * = 1/(timestep * temestep);
}


void initConstraints(Vector3r p0, Quaternionr q1, Vector3r p1, Quaternionr q2)
{
	// compute the rest length between two points p0 and p1 at rest pose;
   	m_restLength = (p0 - p1).norm();

   	// compute rest Darboux Vector
   	m_restDarbouxVector = q1.conjugate() * q2;
  	Quaternionr omega_plus, omega_minus;
   	omega_plus.coeffs() = m_restDarbouxVector.coeffs() + Quaternionr(1, 0, 0, 0).coeffs();
   	omega_minus.coeffs() = m_restDarbouxVector.coeffs() - Quaternionr(1, 0, 0, 0).coeffs();
	if (omega_minus.squaredNorm() > omega_plus.squaredNorm())
		m_restDarbouxVector.coeffs() *= -1.0;
}

// All of the positions are in world coordinates.
void solvingConstraints(Vector3r x0, Quaternionr q0, Vector3r x1, Quaternionr q1, Vector6r& lambda, 
	Vector3r& corrx0,Quaternionr& corrq0,Vector3r& corrx1,Quaternionr& corrq1)
{
	// construct constraint function C(x)
	Quaternionr omega = q0.conjugate() * q1;   //darboux vector
	Quaternionr omega_plus;
	omega_plus.coeffs() = omega.coeffs() + restDarbouxVector.coeffs();     //delta Omega with -Omega_0
	omega.coeffs() = omega.coeffs() - restDarbouxVector.coeffs();                 //delta Omega with + omega_0
	if (omega.squaredNorm() > omega_plus.squaredNorm()) omega = omega_plus;
	Vector6r C(x) = (x0-x1, omega,omega.y,omega.z);

	// construct stiffness control term 
	for(i = 0,i<6;i++) 
		alphaLambda = lambda[i] * m_complianceVector[i];

	// sum C(x) and stiffness control term
	Vector6r numerator;
	for(i = 0,i<6;i++) 
		numerator[i] = C(i) + alphaLambda(i);

	// compute the Jacobian of C(x) with respect to (x0,q0) and (x1,q1);
	computeJacobian(J1,x0,q0,q1,true);
	computeJacobian(J2,x1,q1,q0,false);

	// construct mass Matrix using Equation 28
	Real invInertia = m_rodDensity * m_restLength * 4 /(M_PI * m_rodRadius^4);
	Matrix6r invM0,invM1;
	invM0.diagnal = {invMass0,invMass0,invMass0,invInertia,invInertia,0.5 * invInertia};
	invM1.diagnal = {invMass1,invMass1,invMass1,invInertia,invInertia,0.5 * invInertia};

	// compute demoniator
	Matrix6r alpha;
	alpha.diagnal = {m_complianceVector[0],m_complianceVector[1],m_complianceVector[2],m_complianceVector[3],m_complianceVector[4],m_complianceVector[5]};
	Matrix6r demoniator = J1 * invM0 * J1.transpose() + J2 *invM1 * J2.transpose() + alpha;

	Vector6r deltaLambda = demoniator.inverse() * numerator;

	// update Lambda at each iteration
	lambda += deltaLambda;

	Vector6r deltaX0 =  J1 * deltaLambda;
	Vector6r deltaX1 =  J2 * deltaLambda;

	// position and orientation correction
	corrx0 = invMass0   * Vector3r(deltaX0[0],deltaX0[1],deltaX0[2]);
	corrq0 = invInertia * Quaternionr(0,deltaX0[3],deltaX0[4],deltaX0[5]);
	corrx1 = invMass1   * Vector3r(deltaX1[0],deltaX1[1],deltaX1[2]);
	corrq2 = invInertia * Quaternionr(0,deltaX1[3],deltaX1[4],deltaX1[5]);
}

void computeJacobian(Matrix6r& jac,Vector3r position, Quaternionr q0, Quaternionr q1, bool isFirst)
{
	//Jacobian is a Block matrix 
	Matrix3r uperLeft,uperRight,lowerLeft,lowerRight;
	if(isFirst)
	{
		uperLeft  =  Matrix3r::Identity();
		uperRight = -Matrix3r::CrossProductMatrixFromVector(position);
		lowerLeft =  Matrix3r::Zeros();
	}
	else
	{
		uperLeft  = -Matrix3r::Identity();
		uperRight =  Matrix3r::CrossProductMatrixFromVector(position);
		lowerLeft =  Matrix3r::Zeros();

	}

	//compute Partial differential of Darboux vector with respect Q using Euqation 11 and 27
	Matrix3x4 partial_q;
	Matrix4x3 G_q;
	
	computePartial(q0,q1,partial_q,G_q);		

	lowerRight = partial_q * G_q;
	jac = {uperLeft,uperRight,lowerLeft,lowerRight};
}

update Jacobian Function

change Darboux Vector computing

您好，请问您已经实现了Direct Position-Based Solver for Stiff Rods这篇论文了吗 ？ 能否分享下源代码，不胜感激！

我在github上InteractiveComputerGraphics / PositionBasedDynamics的库里已经找到了实现的代码，看起来有些复杂吃力，看了您的代码感到很好易懂，希望您能指点下 ，非常感谢

我在github上InteractiveComputerGraphics / PositionBasedDynamics的库里已经找到了实现的代码，看起来有些复杂吃力，看了您的代码感到很好易懂，希望您能指点下 ，非常感谢

可以啊，可以发我邮件yupeng@buaa.edu.cn