double DiagonalMass<Rigid2dTypes, Rigid2dMass>::getPotentialEnergy( const core::MechanicalParams* /*mparams*/ /* PARAMS FIRST */, const DataVecCoord& x) const
{
double e = 0;
const MassVector &masses= f_mass.getValue();
const VecCoord& _x = x.getValue();
// gravity
Vec3d g ( this->getContext()->getGravity() );
Deriv theGravity;
DataTypes::set
( theGravity, g[0], g[1], g[2]);
for (unsigned int i=0; i<_x.size(); i++)
{
e -= getVCenter(theGravity) * masses[i].mass * _x[i].getCenter();
}
return e;
}
SReal DiagonalMass<RigidTypes, RigidMass>::getPotentialEnergyRigidImpl( const MechanicalParams* mparams,
const DataVecCoord& x) const
{
SOFA_UNUSED(mparams) ;
SReal e = 0;
const MassVector &masses= d_vertexMass.getValue();
const VecCoord& _x = x.getValue();
// gravity
Vec3d g ( this->getContext()->getGravity() );
Deriv theGravity;
RigidTypes::set( theGravity, g[0], g[1], g[2]);
for (unsigned int i=0; i<_x.size(); i++)
{
e -= getVCenter(theGravity) * masses[i].mass * _x[i].getCenter();
}
return e;
}
void ClosestPointRegistrationForceFieldCam<DataTypes, DepthTypes>::addForce(const core::MechanicalParams* /*mparams*/,DataVecDeriv& _f , const DataVecCoord& _x , const DataVecDeriv& _v )
{
int t = (int)this->getContext()->getTime();
if (t == 0)
{
extractTargetPCD();
initSegmentation();
}
else if (t%niterations == 0)
{
extractTargetPCD();
segment();
}
if(ks.getValue()==0) return;
VecDeriv& f = *_f.beginEdit(); //WDataRefVecDeriv f(_f);
const VecCoord& x = _x.getValue(); //RDataRefVecCoord x(_x);
const VecDeriv& v = _v.getValue(); //RDataRefVecDeriv v(_v);
ReadAccessor< Data< VecCoord > > tn(targetNormals);
ReadAccessor< Data< VecCoord > > tp(targetPositions);
const vector<Spring>& s= this->springs.getValue();
this->dfdx.resize(s.size());
this->closestPos.resize(s.size());
updateClosestPoints();
m_potentialEnergy = 0;
// get attraction/ projection factors
Real attrF=(Real) blendingFactor.getValue();
if(attrF<(Real)0.) attrF=(Real)0.;
if(attrF>(Real)1.) attrF=(Real)1.;
Real projF=((Real)1.-attrF);
if(tp.size()==0)
for (unsigned int i=0; i<s.size(); i++)
closestPos[i]=x[i];
else {
// count number of attractors
cnt.resize(s.size()); cnt.fill(0); if(attrF>0) for (unsigned int i=0; i<tp.size(); i++) if(!targetIgnored[i]) cnt[closestTarget[i].begin()->second]++;
if(theCloserTheStiffer.getValue())
{
// find the min and the max distance value from source point to target point
min=0;
max=0;
for (unsigned int i=0; i<x.size(); i++)
{
if(min==0 || min>closestSource[i].begin()->first) min=closestSource[i].begin()->first;
if(max==0 || max<closestSource[i].begin()->first) max=closestSource[i].begin()->first;
}
}
// compute targetpos = projF*closestto + attrF* sum closestfrom / count
// projection to point or plane
if(projF>0) {
for (unsigned int i=0; i<s.size(); i++)
if(!sourceIgnored[i]) {
unsigned int id=closestSource[i].begin()->second;
if(projectToPlane.getValue() && tn.size()!=0) closestPos[i]=(x[i]+tn[id]*dot(tp[id]-x[i],tn[id]))*projF;
else closestPos[i]=tp[id]*projF;
if(!cnt[i]) closestPos[i]+=x[i]*attrF;
}
else {
closestPos[i]=x[i]*projF;
if(!cnt[i]) closestPos[i]+=x[i]*attrF;
}
}
else for (unsigned int i=0; i<s.size(); i++) { if(!cnt[i]) closestPos[i]=x[i]; else closestPos[i].fill(0); }
// attraction
if(attrF>0)
for (unsigned int i=0; i<tp.size(); i++)
if(!targetIgnored[i]) {
unsigned int id=closestTarget[i].begin()->second;
closestPos[id]+=tp[i]*attrF/(Real)cnt[id];
sourceIgnored[id]=false;
}
}
for (unsigned int i=0; i<s.size(); i++)
{
//serr<<"addForce() between "<<springs[i].m1<<" and "<<closestPos[springs[i].m1]<<sendl;
this->addSpringForce(m_potentialEnergy,f,x,v, i, s[i]);
}
_f.endEdit();
}
