void ThreadPiece::gradient_twist(double& grad)
{
grad = (_angle_twist- _next_piece->_angle_twist)/(2.0*(_my_thread->rest_length()));
if (_prev_piece != NULL)
{
grad += (_angle_twist - _prev_piece->_angle_twist)/(2.0*(_my_thread->rest_length()));
} else {
grad += (_angle_twist)/(2.0*(_my_thread->rest_length()));
}
grad *= 2.0*TWIST_COEFF;
//non-isotropic, so affects curvature energy too
Vector2d w_j(_curvature_binormal.dot(_material_frame.col(2)), -_curvature_binormal.dot(_material_frame.col(1)) );
Vector2d w_j_1(_next_piece->_curvature_binormal.dot(_material_frame.col(2)), -_next_piece->_curvature_binormal.dot(_material_frame.col(1)) );
if (_prev_piece != NULL)
{
grad += (w_j.dot(JB*w_j))/(2.0*(_my_thread->rest_length()));
}
grad += (w_j_1.dot(JB*w_j_1))/(2.0*(_my_thread->rest_length()));
}
int p(int t, int y[], int s) {
int i = 0;
double sum = 0.0, temp;
while (i < s) {
temp = y[i]/(1.0*w_j(i, s)*(t - i));
sum = sum + temp;
i++;
}
sum = l(t, s)*sum;
i = round(sum);
return i;
}