SofaSimulator.h

Go to the documentation of this file.

/*****************************************************************************
 * $CAMITK_LICENCE_BEGIN$
 *
 * CamiTK - Computer Assisted Medical Intervention ToolKit
 * (c) 2001-2021 Univ. Grenoble Alpes, CNRS, Grenoble INP, TIMC, 38000 Grenoble, France
 *
 * Visit http://camitk.imag.fr for more information
 *
 * This file is part of CamiTK.
 *
 * CamiTK is free software: you can redistribute it and/or modify
 * it under the terms of the GNU Lesser General Public License version 3
 * only, as published by the Free Software Foundation.
 *
 * CamiTK is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU Lesser General Public License version 3 for more details.
 *
 * You should have received a copy of the GNU Lesser General Public License
 * version 3 along with CamiTK.  If not, see <http://www.gnu.org/licenses/>.
 *
 * $CAMITK_LICENCE_END$
 ****************************************************************************/
 
 
#ifndef SIMULATOR_SIMULATORS_SOFA_SOFASIMULATOR_H
#define SIMULATOR_SIMULATORS_SOFA_SOFASIMULATOR_H
 
#include "InteractiveSimulator.h"
 
#include <sofa/simulation/tree/TreeSimulation.h>
#include <sofa/component/container/MechanicalObject.h>
#include <sofa/defaulttype/VecTypes.h>
#include <sofa/simulation/common/Node.h>
#include <sofa/simulation/tree/GNode.h>
 
// simplification of sofa interface
#define SofaVector sofa::helper::vector
 
// Loads for Sofa
#include "TranslationConstraint.h"
 
namespace std {
typedef pair<unsigned int, unsigned int> MechanicalObjectDOFIndex;
typedef map <unsigned int, MechanicalObjectDOFIndex> AtomDOFMap;
typedef map<unsigned int, unsigned int> MechanicalObjectAtomDOFMap;
typedef map<unsigned int, unsigned int> MechanicalObjectDOFAtomMap;
}
 
class SofaSimulator : public InteractiveSimulator {
 
public:
    SofaSimulator(MonitoringManager* monitoringManager);
 
    SofaSimulator(MonitoringManager* monitoringManager, const char* file);
 
    virtual ~SofaSimulator();
 
    void doMove(double dt);
    void init();
    void getPosition(int index, double position[3]);
    void getForce(int index, double force[3]);
    void end() {}
 
    void createPml(const char* inputFile, const char* pmlFile);
 
    std::string getScnFile();
 
private:
    sofa::simulation::Node* getGNode();
 
    std::MechanicalObjectAtomDOFMap& getMechanicalObjectAtomDOFMap(unsigned int mechObjectIndex);
 
    std::MechanicalObjectDOFAtomMap& getMechanicalObjectDOFAtomMap(unsigned int mechObjectIndex);
 
    sofa::defaulttype::Vec3Types::Coord getDOFPosition(unsigned int atomIndex);
 
    sofa::defaulttype::Vec3Types::Coord getDOFPosition(unsigned int mechObjectIndex, unsigned int dofIndex);
 
    sofa::defaulttype::Vec3Types::Deriv getDOFForce(unsigned int atomIndex);
 
    sofa::defaulttype::Vec3Types::Deriv getDOFForce(unsigned int mechObjectIndex, unsigned int dofIndex);
 
    unsigned int getAtomIndex(unsigned int mechObjectIndex, unsigned int dofIndex);
 
    sofa::component::container::MechanicalObject<sofa::defaulttype::Vec3Types>* getMechanicalObject(unsigned int mechObjectIndex);
 
    unsigned int getNumberOfMechanicalObjects();
 
    sofa::defaulttype::Vec3Types::VecCoord getMechanicalObjectDOFPosition(unsigned int mechObjectIndex);
 
    sofa::defaulttype::Vec3Types::VecDeriv getMechanicalObjectDOFForce(unsigned int mechObjectIndex);
 
    void build();
 
    void buildConstraints();
 
#if defined(SOFA_1_0_RC1) || defined(SOFA_SVN) || defined(SOFA_STABLE)
    sofa::simulation::Node::SPtr groot;
#endif
 
#ifdef MML_SOFA_1_0_BETA4
    sofa::simulation::Node* groot;
#endif
 
    SofaVector<BaseMechanicalState*> mechanicalObjects;
 
    std::AtomDOFMap atomsToDOF;
 
    std::vector<std::MechanicalObjectAtomDOFMap*> mechanicalObjectAtomDOFMap;
 
    std::vector<std::MechanicalObjectDOFAtomMap*> mechanicalObjectDOFAtomMap;
 
    std::vector<TranslationConstraint<sofa::defaulttype::Vec3Types> *> translations;
 
    std::string scnFile;
};
 
// -------------------- getGNode --------------------
inline sofa::simulation::Node* SofaSimulator::getGNode() {
    if (groot == nullptr) {
        //-- TODO build a sofaGRoot using the nodes
    }
 
#if defined(SOFA_1_0_RC1) || defined(SOFA_SVN) || defined(SOFA_STABLE)
    return groot.get();
#endif
 
#ifdef MML_SOFA_1_0_BETA4
    return groot;
#endif
}
 
// -------------------- getNumberOfMechanicalObjects --------------------
inline unsigned int SofaSimulator::getNumberOfMechanicalObjects() {
    return mechanicalObjects.size();
}
 
// -------------------- getMechanicalObjectAtomDOFMap --------------------
inline std::MechanicalObjectAtomDOFMap& SofaSimulator::getMechanicalObjectAtomDOFMap(unsigned int mechObjectIndex) {
    return (*mechanicalObjectAtomDOFMap[mechObjectIndex]);
}
 
// -------------------- getMechanicalObjectDOFAtomMap --------------------
inline std::MechanicalObjectDOFAtomMap& SofaSimulator::getMechanicalObjectDOFAtomMap(unsigned int mechObjectIndex) {
    return (*mechanicalObjectDOFAtomMap[mechObjectIndex]);
}
 
// -------------------- getMechanicalObjectDOFPosition --------------------
inline sofa::defaulttype::Vec3Types::VecCoord SofaSimulator::getMechanicalObjectDOFPosition(unsigned int mechObjectIndex) {
    return (*getMechanicalObject(mechObjectIndex)->getX());
}
 
// -------------------- getMechanicalObjectDOFForce --------------------
inline sofa::defaulttype::Vec3Types::VecDeriv SofaSimulator::getMechanicalObjectDOFForce(unsigned int mechObjectIndex) {
    return (*getMechanicalObject(mechObjectIndex)->getF());
}
 
// -------------------- getScnFile --------------------
inline std::string SofaSimulator::getScnFile() {
    return scnFile;
}
 
#endif // SIMULATOR_SIMULATORS_SOFA_SOFASIMULATOR_H