void AttachmentConstraint::PBDProject(VectorX& x, const SparseMatrix& inv_mass, unsigned int ns)
{
// LOOK
ScalarType k_prime = 1 - std::pow(1-*(m_p_pbd_stiffness), 1.0/ns);
EigenVector3 p = x.block_vector(m_p0);
EigenVector3 dp = m_fixd_point-p;
x.block_vector(m_p0) += k_prime * dp;
}
// sping gradient: k*(current_length-rest_length)*current_direction;
void SpringConstraint::EvaluateGradient(const VectorX& x, VectorX& gradient)
{
// TODO
//EigenVector3 g_i = (*(m_p_stiffness))*(x.block_vector(m_p0) - m_fixd_point);
//gradient.block_vector(m_p0) += g_i;
EigenVector3 p1=x.block_vector(m_p1);
EigenVector3 p2=x.block_vector(m_p2);
float currentLength=(p1-p2).norm();
float force=(*this->m_p_stiffness)*(currentLength-this->m_rest_length);
EigenVector3 n1=p1-p2,n2=p2-p1;
n1.normalize();n2.normalize();
gradient.block_vector(m_p1)+=n1*force;
gradient.block_vector(m_p2)+=n2*force;
}
void SpringConstraint::PBDProject(VectorX& x, const SparseMatrix& inv_mass, unsigned int ns)
{
// TODO
// change project order
ScalarType k_prime = 1 - std::pow(1-*(m_p_pbd_stiffness), 1.0/ns);
float rest_length=m_rest_length;
EigenVector3 p1 = x.block_vector(m_p1);
EigenVector3 p2 = x.block_vector(m_p2);
float current_length=(p1-p2).norm();
EigenVector3 current_direction=(p1-p2)/current_length;
EigenVector3 dp=(current_length-rest_length)*current_direction;
ScalarType w1=inv_mass.coeff(m_p1,m_p1);
ScalarType w2=inv_mass.coeff(m_p2,m_p2);
x.block_vector(m_p1) -= k_prime * dp*w1/(w1+w2);
x.block_vector(m_p2) += k_prime * dp*w2/(w1+w2);
}
// attachment spring gradient: k*(current_length)*current_direction
void AttachmentConstraint::EvaluateGradient(const VectorX& x, VectorX& gradient)
{
// LOOK
EigenVector3 g_i = (*(m_p_stiffness))*(x.block_vector(m_p0) - m_fixd_point);
gradient.block_vector(m_p0) += g_i;
}
