<commit_before>// Author: <NAME>, INRIA-UJF, (C) 2006
//
// Copyright: See COPYING file that comes with this distribution
#ifndef SOFA_COMPONENT_FORCEFIELD_SPRINGFORCEFIELD_INL
#define SOFA_COMPONENT_FORCEFIELD_SPRINGFORCEFIELD_INL

#include <sofa/component/forcefield/SpringForceField.h>
#include <sofa/component/topology/MeshTopology.h>
#include <sofa/helper/io/MassSpringLoader.h>
#include <sofa/helper/system/config.h>
#include <assert.h>
#include <iostream>
#include <GL/gl.h>
#include <iostream>
using std::cerr;
using std::endl;

namespace sofa
{

namespace component
{

namespace forcefield
{

template<class DataTypes>
void SpringForceField<DataTypes>::parse(core::objectmodel::BaseObjectDescription* arg)
{
    if (arg->getAttribute("filename"))
        this->load(arg->getAttribute("filename"));
    this->InteractionForceField::parse(arg);
}

template <class DataTypes>
class SpringForceField<DataTypes>::Loader : public helper::io::MassSpringLoader
{
public:
    SpringForceField<DataTypes>* dest;
    Loader(SpringForceField<DataTypes>* dest) : dest(dest) {}
    virtual void addSpring(int m1, int m2, double ks, double kd, double initpos)
    {
        dest->springs.push_back(Spring(m1,m2,ks,kd,initpos));
    }
};

template <class DataTypes>
bool SpringForceField<DataTypes>::load(const char *filename)
{
    if (filename && filename[0])
    {
        Loader loader(this);
        return loader.load(filename);
    }
    else return false;
}

template <class DataTypes>
void SpringForceField<DataTypes>::initFromTopology()
{
    if (springs.empty())
    {
        sofa::component::topology::MeshTopology* _mesh = dynamic_cast<sofa::component::topology::MeshTopology*>(this->getContext()->getTopology());
        if( _mesh != NULL )
        {
            VecDeriv& P1 = *object1->getX();
            VecDeriv& P2 = *object2->getX();
            for( unsigned i=0; i<_mesh->getLines().size(); i++ )
            {
                int a =_mesh->getLines()[i][0];
                int b = _mesh->getLines()[i][1];
                Deriv gap = P1[a] - P2[b];
                addSpring(a,b,ks.getValue(),kd.getValue(),sqrt(defaulttype::dot(gap,gap)));
            }
        }
    }
}

template <class DataTypes>
void SpringForceField<DataTypes>::init()
{
    this->InteractionForceField::init();
    //initFromTopology();
}

template<class DataTypes>
void SpringForceField<DataTypes>::addSpringForce(double& ener, VecDeriv& f1, const VecCoord& p1, const VecDeriv& v1, VecDeriv& f2, const VecCoord& p2, const VecDeriv& v2, int /*i*/, const Spring& spring)
{
    int a = spring.m1;
    int b = spring.m2;
    Coord u = p2[b]-p1[a];
    Real d = u.norm();
    Real inverseLength = 1.0f/d;
    u *= inverseLength;
    Real elongation = (Real)(d - spring.initpos);
    ener += elongation * elongation * spring.ks /2;
    Deriv relativeVelocity = v2[b]-v1[a];
    Real elongationVelocity = dot(u,relativeVelocity);
    Real forceIntensity = (Real)(spring.ks*elongation+spring.kd*elongationVelocity);
    Deriv force = u*forceIntensity;
    f1[a]+=force;
    f2[b]-=force;
}

template<class DataTypes>
void SpringForceField<DataTypes>::addForce()
{
    assert(this->object1);
    assert(this->object2);
    VecDeriv& f1 = *this->object1->getF();
    const VecCoord& p1 = *this->object1->getX();
    const VecDeriv& v1 = *this->object1->getV();
    VecDeriv& f2 = *this->object2->getF();
    const VecCoord& p2 = *this->object2->getX();
    const VecDeriv& v2 = *this->object2->getV();
    f1.resize(p1.size());
    f2.resize(p2.size());
    m_potentialEnergy = 0;
    for (unsigned int i=0; i<this->springs.size(); i++)
    {
        this->addSpringForce(m_potentialEnergy,f1,p1,v1,f2,p2,v2, i, this->springs[i]);
    }
}

template<class DataTypes>
void SpringForceField<DataTypes>::addDForce()
{
    std::cerr << "SpringForceField does not support implicit integration. Use StiffSpringForceField instead.\n";
}

template<class DataTypes>
void SpringForceField<DataTypes>::draw()
{
    if (!((this->object1 == this->object2)?getContext()->getShowForceFields():getContext()->getShowInteractionForceFields())) return;
    const VecCoord& p1 = *this->object1->getX();
    const VecCoord& p2 = *this->object2->getX();
//         cerr<<"SpringForceField<DataTypes>::draw(), p1.size = "<<p1.size()<<endl;
//         cerr<<"SpringForceField<DataTypes>::draw(), p1 = "<<p1<<endl;
//         cerr<<"SpringForceField<DataTypes>::draw(), p2 = "<<p2<<endl;
    glDisable(GL_LIGHTING);
    bool external = (this->object1!=this->object2);
    //if (!external)
    //	glColor4f(1,1,1,1);
    glBegin(GL_LINES);
    for (unsigned int i=0; i<this->springs.size(); i++)
    {
        Real d = (p2[this->springs[i].m2]-p1[this->springs[i].m1]).norm();
        if (external)
        {
            if (d<this->springs[i].initpos*0.9999)
                glColor4f(1,0,0,1);
            else
                glColor4f(0,1,0,1);
        }
        else
        {
            if (d<this->springs[i].initpos*0.9999)
                glColor4f(1,0.5f,0,1);
            else
                glColor4f(0,1,0.5f,1);
        }
        glVertex3d(p1[this->springs[i].m1][0],p1[this->springs[i].m1][1],p1[this->springs[i].m1][2]);
        glVertex3d(p2[this->springs[i].m2][0],p2[this->springs[i].m2][1],p2[this->springs[i].m2][2]);
    }
    glEnd();
}

} // namespace forcefield

} // namespace component

} // namespace sofa

#endif
<commit_msg>r862/sofa-dev : FIX : SpringForceField initialisation from MechanicalState and topology in context<commit_after>// Author: <NAME>, INRIA-UJF, (C) 2006
//
// Copyright: See COPYING file that comes with this distribution
#ifndef SOFA_COMPONENT_FORCEFIELD_SPRINGFORCEFIELD_INL
#define SOFA_COMPONENT_FORCEFIELD_SPRINGFORCEFIELD_INL

#include <sofa/component/forcefield/SpringForceField.h>
#include <sofa/component/topology/MeshTopology.h>
#include <sofa/helper/io/MassSpringLoader.h>
#include <sofa/helper/system/config.h>
#include <assert.h>
#include <iostream>
#include <GL/gl.h>
#include <iostream>
using std::cerr;
using std::endl;

namespace sofa
{

namespace component
{

namespace forcefield
{

template<class DataTypes>
void SpringForceField<DataTypes>::parse(core::objectmodel::BaseObjectDescription* arg)
{
    if (arg->getAttribute("filename"))
        this->load(arg->getAttribute("filename"));
    this->InteractionForceField::parse(arg);
}

template <class DataTypes>
class SpringForceField<DataTypes>::Loader : public helper::io::MassSpringLoader
{
public:
    SpringForceField<DataTypes>* dest;
    Loader(SpringForceField<DataTypes>* dest) : dest(dest) {}
    virtual void addSpring(int m1, int m2, double ks, double kd, double initpos)
    {
        dest->springs.push_back(Spring(m1,m2,ks,kd,initpos));
    }
};

template <class DataTypes>
bool SpringForceField<DataTypes>::load(const char *filename)
{
    if (filename && filename[0])
    {
        Loader loader(this);
        return loader.load(filename);
    }
    else return false;
}

template <class DataTypes>
void SpringForceField<DataTypes>::initFromTopology()
{
    sofa::component::topology::MeshTopology* _mesh = dynamic_cast<sofa::component::topology::MeshTopology*>(this->getContext()->getTopology());
    if( _mesh != NULL )
    {
        if(!object1)
            object1 = dynamic_cast<sofa::core::componentmodel::behavior::MechanicalState<DataTypes>*>(this->getContext()->getMechanicalState());
        if(!object2)
            object2 = object1;
        if(!object1)
        {
            std::cerr<<"ERROR(StiffSpringForceField): object must have a MechanicalState.\n";
            return;
        }
        VecDeriv& P1 = *object1->getX();
        VecDeriv& P2 = *object2->getX();
        for( unsigned i=0; i<_mesh->getLines().size(); i++ )
        {
            int a =_mesh->getLines()[i][0];
            int b = _mesh->getLines()[i][1];
            Deriv gap = P1[a] - P2[b];
            addSpring(a,b,ks.getValue(),kd.getValue(),sqrt(defaulttype::dot(gap,gap)));
        }
    }
}

template <class DataTypes>
void SpringForceField<DataTypes>::init()
{
    this->InteractionForceField::init();
    if (springs.empty())
        initFromTopology();
}

template<class DataTypes>
void SpringForceField<DataTypes>::addSpringForce(double& ener, VecDeriv& f1, const VecCoord& p1, const VecDeriv& v1, VecDeriv& f2, const VecCoord& p2, const VecDeriv& v2, int /*i*/, const Spring& spring)
{
    int a = spring.m1;
    int b = spring.m2;
    Coord u = p2[b]-p1[a];
    Real d = u.norm();
    Real inverseLength = 1.0f/d;
    u *= inverseLength;
    Real elongation = (Real)(d - spring.initpos);
    ener += elongation * elongation * spring.ks /2;
    Deriv relativeVelocity = v2[b]-v1[a];
    Real elongationVelocity = dot(u,relativeVelocity);
    Real forceIntensity = (Real)(spring.ks*elongation+spring.kd*elongationVelocity);
    Deriv force = u*forceIntensity;
    f1[a]+=force;
    f2[b]-=force;
}

template<class DataTypes>
void SpringForceField<DataTypes>::addForce()
{
    assert(this->object1);
    assert(this->object2);
    VecDeriv& f1 = *this->object1->getF();
    const VecCoord& p1 = *this->object1->getX();
    const VecDeriv& v1 = *this->object1->getV();
    VecDeriv& f2 = *this->object2->getF();
    const VecCoord& p2 = *this->object2->getX();
    const VecDeriv& v2 = *this->object2->getV();
    f1.resize(p1.size());
    f2.resize(p2.size());
    m_potentialEnergy = 0;
    for (unsigned int i=0; i<this->springs.size(); i++)
    {
        this->addSpringForce(m_potentialEnergy,f1,p1,v1,f2,p2,v2, i, this->springs[i]);
    }
}

template<class DataTypes>
void SpringForceField<DataTypes>::addDForce()
{
    std::cerr << "SpringForceField does not support implicit integration. Use StiffSpringForceField instead.\n";
}

template<class DataTypes>
void SpringForceField<DataTypes>::draw()
{
    if (!((this->object1 == this->object2)?getContext()->getShowForceFields():getContext()->getShowInteractionForceFields())) return;
    const VecCoord& p1 = *this->object1->getX();
    const VecCoord& p2 = *this->object2->getX();
//         cerr<<"SpringForceField<DataTypes>::draw(), p1.size = "<<p1.size()<<endl;
//         cerr<<"SpringForceField<DataTypes>::draw(), p1 = "<<p1<<endl;
//         cerr<<"SpringForceField<DataTypes>::draw(), p2 = "<<p2<<endl;
    glDisable(GL_LIGHTING);
    bool external = (this->object1!=this->object2);
    //if (!external)
    //	glColor4f(1,1,1,1);
    glBegin(GL_LINES);
    for (unsigned int i=0; i<this->springs.size(); i++)
    {
        Real d = (p2[this->springs[i].m2]-p1[this->springs[i].m1]).norm();
        if (external)
        {
            if (d<this->springs[i].initpos*0.9999)
                glColor4f(1,0,0,1);
            else
                glColor4f(0,1,0,1);
        }
        else
        {
            if (d<this->springs[i].initpos*0.9999)
                glColor4f(1,0.5f,0,1);
            else
                glColor4f(0,1,0.5f,1);
        }
        glVertex3d(p1[this->springs[i].m1][0],p1[this->springs[i].m1][1],p1[this->springs[i].m1][2]);
        glVertex3d(p2[this->springs[i].m2][0],p2[this->springs[i].m2][1],p2[this->springs[i].m2][2]);
    }
    glEnd();
}

} // namespace forcefield

} // namespace component

} // namespace sofa

#endif

{'fraction_non_alphanumeric': 0.16669636493228795, 'fraction_numerical': 0.02521382751247327, 'mean_word_length': 3.504414125200642, 'pattern_counts': {'":': 0, '<': 123, '<?xml version=': 0, '>': 175, 'https://': 0, 'lorem ipsum': 0, 'www.': 0, 'xml': 0}, 'pii_count': 0, 'substrings_counts': 0, 'word_list_counts': {'cursed_substrings.json': 0, 'profanity_word_list.json': 0, 'sexual_word_list.json': 0, 'zh_pornsignals.json': 0}}
{'dir': 'git-commits-cleaned', 'id': '6811904', 'n_tokens_mistral': 3983, 'n_tokens_neox': 3905, 'n_words': 673}