void RestShapeSpringsForceField<Rigid3fTypes>::addDForce(const core::MechanicalParams* mparams, DataVecDeriv& df, const DataVecDeriv& dx)
{
sofa::helper::WriteAccessor< DataVecDeriv > df1 = df;
sofa::helper::ReadAccessor< DataVecDeriv > dx1 = dx;
Real kFactor = (Real)mparams->kFactorIncludingRayleighDamping(this->rayleighStiffness.getValue());
const VecReal& k = stiffness.getValue();
const VecReal& k_a = angularStiffness.getValue();
unsigned int curIndex = 0;
for (unsigned int i=0; i<m_indices.size(); i++)
{
curIndex = m_indices[i]; // Fix by FF, just a guess
getVCenter(df1[curIndex]) -= getVCenter(dx1[curIndex]) * (i < k.size() ? k[i] : k[0]) * kFactor ;
getVOrientation(df1[curIndex]) -= getVOrientation(dx1[curIndex]) * (i < k_a.size() ? k_a[i] : k_a[0]) * kFactor ;
}
}
void RestShapeSpringsForceField<Rigid3fTypes>::addForce(const core::MechanicalParams* /* mparams */ /* PARAMS FIRST */, 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];
const unsigned int ext_index = m_ext_indices[i];
// translation
Vec3f dx = p1[index].getCenter() - p0[ext_index].getCenter();
getVCenter(f1[index]) -= dx * (i < k.size() ? k[i] : k[0]) ;
// rotation
Quatf dq = p1[index].getOrientation() * p0[ext_index].getOrientation().inverse();
Vec3f dir;
Real angle=0;
dq.normalize();
if (dq[3] < 0.999999999999999)
dq.quatToAxis(dir, angle);
dq.quatToAxis(dir, angle);
//std::cout<<"dq : "<<dq <<" dir :"<<dir<<" angle :"<<angle<<std::endl;
getVOrientation(f1[index]) -= dir * angle * (i < k_a.size() ? k_a[i] : k_a[0]) ;
}
}
