void RestShapeSpringsForceField<Rigid3Types>::addForce(const core::MechanicalParams* /* mparams */, DataVecDeriv& f, const DataVecCoord& x, const DataVecDeriv& /* v */)
{
sofa::helper::WriteAccessor< DataVecDeriv > f1 = f;
sofa::helper::ReadAccessor< DataVecCoord > p1 = x;
sofa::helper::ReadAccessor< DataVecCoord > p0 = *getExtPosition();
f1.resize(p1.size());
if (recompute_indices.getValue())
{
recomputeIndices();
}
const VecReal& k = stiffness.getValue();
const VecReal& k_a = angularStiffness.getValue();
for (unsigned int i = 0; i < m_indices.size(); i++)
{
const unsigned int index = m_indices[i];
unsigned int ext_index = m_indices[i];
if(useRestMState)
ext_index= m_ext_indices[i];
// translation
if (i >= m_pivots.size())
{
Vec3d dx = p1[index].getCenter() - p0[ext_index].getCenter();
getVCenter(f1[index]) -= dx * (i < k.size() ? k[i] : k[0]) ;
}
else
{
CPos localPivot = p0[ext_index].getOrientation().inverseRotate(m_pivots[i] - p0[ext_index].getCenter());
CPos rotatedPivot = p1[index].getOrientation().rotate(localPivot);
CPos pivot2 = p1[index].getCenter() + rotatedPivot;
CPos dx = pivot2 - m_pivots[i];
getVCenter(f1[index]) -= dx * (i < k.size() ? k[i] : k[0]) ;
}
// rotation
Quatd dq = p1[index].getOrientation() * p0[ext_index].getOrientation().inverse();
Vec3d dir;
double angle=0;
dq.normalize();
if (dq[3] < 0)
{
dq = dq * -1.0;
}
if (dq[3] < 0.999999999999999)
dq.quatToAxis(dir, angle);
getVOrientation(f1[index]) -= dir * angle * (i < k_a.size() ? k_a[i] : k_a[0]);
}
}
