Ejemplo n.º 1
0
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;
}
Ejemplo n.º 2
0
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();

}