SofaSimulator.h

Go to the documentation of this file.

/*****************************************************************************
 * $CAMITK_LICENCE_BEGIN$
 *
 * CamiTK - Computer Assisted Medical Intervention ToolKit
 * (c) 2001-2018 Univ. Grenoble Alpes, CNRS, TIMC-IMAG UMR 5525 (GMCAO)
 *
 * 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 == NULL) {
        //-- 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