Skip to content

Commit

Permalink
Refactor PointManager
Browse files Browse the repository at this point in the history 
It looks like it is a controller... (but in that cas why exposing it to python ?)
Further refactoring should be added.
  • Loading branch information
@damienmarchal
damienmarchal committed Jul 12, 2024
1 parent f6c6942 commit a79ee6f
Show file tree
Hide file tree
Showing 4 changed files with 152 additions and 168 deletions.
2 changes: 1 addition & 1 deletion src/Cosserat/Binding/Binding_PointsManager.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -24,7 +24,7 @@
#include <pybind11/stl.h>
#include <Cosserat/engine/PointsManager.h>

typedef sofa::core::behavior::PointsManager PointsManager;
typedef cosserat::controller::PointsManager PointsManager;

namespace py {
using namespace pybind11;
Expand Down
21 changes: 6 additions & 15 deletions src/Cosserat/engine/PointsManager.cpp
View file
Original file line number Diff line number Diff line change
@@ -1,19 +1,10 @@

#include "PointsManager.inl"
#include <Cosserat/engine/PointsManager.inl>
#include <sofa/core/ObjectFactory.h>
#include <sofa/core/visual/VisualParams.h>
#include <math.h>
#include <assert.h> /* assert */

namespace sofa::core::behavior
namespace cosserat::controller
{

using namespace sofa::defaulttype;

SOFA_DECL_CLASS(PointsManager)

int PointsManagerClass = core::RegisterObject("add and remove "
"points from the state will taking into account the changes inside the modifier and the container")
.add<PointsManager>();

} // namespace sofa
int PointsManagerClass = sofa::core::RegisterObject(
R"doc(Add or remove controls point in a beam structure)doc")
.add<PointsManager>();
} // namespace cosserat::controller
83 changes: 37 additions & 46 deletions src/Cosserat/engine/PointsManager.h
View file
Original file line number Diff line number Diff line change
@@ -1,64 +1,55 @@
#pragma once

#include <Cosserat/config.h>
#include <sofa/defaulttype/VecTypes.h>
#include <sofa/core/objectmodel/KeypressedEvent.h>
#include <sofa/defaulttype/SolidTypes.h>
#include <sofa/core/behavior/BaseController.h>
#include <sofa/core/behavior/MechanicalState.h>
#include <sofa/component/topology/container/dynamic/fwd.h>
#include <sofa/core/topology/BaseTopology.h>
#include <sofa/component/topology/container/dynamic/PointSetTopologyContainer.h>
#include <sofa/component/topology/container/dynamic/PointSetTopologyModifier.h>
#include <sofa/component/topology/container/dynamic/EdgeSetTopologyContainer.h>
#include <sofa/component/topology/container/dynamic/TriangleSetTopologyContainer.h>
#include <sofa/helper/AdvancedTimer.h>
// #include <sofa/gl/template.h>
#include <sofa/core/behavior/Constraint.h>

typedef sofa::component::topology::container::dynamic::PointSetTopologyModifier PointSetTopologyModifier;
#include <sofa/core/behavior/MechanicalState.h>

using sofa::core::objectmodel::KeypressedEvent;
namespace sofa::core::behavior
namespace cosserat::controller
{
class SOFA_COSSERAT_API PointsManager : public sofa::core::objectmodel::BaseObject
{
namespace {
using sofa::component::topology::container::dynamic::PointSetTopologyModifier;
using sofa::core::topology::TopologyContainer;
}

public:
SOFA_CLASS(PointsManager, sofa::core::objectmodel::BaseObject);
// TODO(dmarchal: 2024-07-12) This class looks like a Controller (not a BaseObject)
class SOFA_COSSERAT_API PointsManager : public sofa::core::behavior::BaseController
{
public:
SOFA_CLASS(PointsManager, sofa::core::behavior::BaseController);

typedef sofa::defaulttype::Vec3dTypes DataTypes;
typedef DataTypes::VecCoord VecCoord;
typedef DataTypes::Coord Coord;
typedef DataTypes::Real Real;
PointsManager();
~PointsManager();

public:
PointsManager();
typedef type::Vec3 Vec3;
sofa::Data<sofa::type::Vec3> d_beamTip;
sofa::Data<double> d_radius;
sofa::Data<sofa::type::Vec4f> d_color;
sofa::Data<std::string> d_beamPath;

Data<Vec3> d_beamTip;
Data<double> d_radius;
Data<type::Vec4f> d_color;
Data<std::string> d_beamPath;
void init() override;
void handleEvent(sofa::core::objectmodel::Event *event) override;

PointSetTopologyModifier *m_modifier;
core::behavior::MechanicalState<DataTypes> *m_beam;
void addNewPointToState();
void removeLastPointfromState();

void init() override;
void handleEvent(sofa::core::objectmodel::Event *event) override;
// void draw(const core::visual::VisualParams *vparams);
private:
using MState = sofa::core::behavior::MechanicalState<sofa::defaulttype::Vec3Types>;

void addNewPointToState();
void removeLastPointfromState();
PointSetTopologyModifier *m_modifier;
MState *m_beam;

topology::TopologyContainer *getTopology()
{
return dynamic_cast<topology::TopologyContainer *>(getContext()->getTopology());
}

sofa::core::behavior::MechanicalState<DataTypes> *getMstate()
{
return dynamic_cast<sofa::core::behavior::MechanicalState<DataTypes> *>(getContext()->getMechanicalState());
}
};
auto getTopology() -> TopologyContainer*
{
return dynamic_cast<TopologyContainer*>(getContext()->getTopology());
}

auto getMstate() -> MState*
{
return dynamic_cast<MState *>(getContext()->getMechanicalState());
}
};

} // namespace sofa
} // namespace cosserat::controller
214 changes: 108 additions & 106 deletions src/Cosserat/engine/PointsManager.inl
View file
Original file line number Diff line number Diff line change
@@ -1,133 +1,135 @@
#pragma once

#include "PointsManager.h"
#include <Cosserat/config.h>
#include <Cosserat/engine/PointsManager.h>

#include <sofa/defaulttype/VecTypes.h>
#include <sofa/core/objectmodel/KeypressedEvent.h>
#include <sofa/defaulttype/SolidTypes.h>
#include <sofa/core/topology/BaseTopology.h>
#include <sofa/component/topology/container/dynamic/PointSetTopologyContainer.h>
#include <sofa/component/topology/container/dynamic/PointSetTopologyModifier.h>
#include <sofa/component/topology/container/dynamic/EdgeSetTopologyContainer.h>
#include <sofa/component/topology/container/dynamic/TriangleSetTopologyContainer.h>
#include <sofa/helper/AdvancedTimer.h>
#include <sofa/core/behavior/Constraint.h>

#include <sofa/type/Quat.h>
#include <sofa/core/visual/VisualParams.h>
#include <sofa/simulation/AnimateBeginEvent.h>
#include <sofa/component/statecontainer/MechanicalObject.h>

namespace sofa::core::behavior
namespace cosserat::controller
{
using sofa::core::objectmodel::KeypressedEvent;

PointsManager::PointsManager()
: d_beamTip(initData(&d_beamTip, "beamTip", "The beam tip")),
d_radius(initData(&d_radius, double(1), "radius", "sphere radius")),
d_color(initData(&d_color, sofa::type::Vec4f(1, 0, 0, 1), "color", "Default color is (1,0,0,1)")),
d_beamPath(initData(&d_beamPath, "beamPath", "path to beam state"))
{
this->f_listening.setValue(true);
}

PointsManager::~PointsManager(){}

void PointsManager::init()
{
Inherit1::init();

if (getTopology() == NULL)
msg_error() << "Error cannot find the topology";

PointsManager::PointsManager()
: d_beamTip(initData(&d_beamTip, "beamTip", "The beam tip")),
d_radius(initData(&d_radius, double(1), "radius", "sphere radius")),
d_color(initData(&d_color, type::Vec4f(1, 0, 0, 1), "color", "Default color is (1,0,0,1)")),
d_beamPath(initData(&d_beamPath, "beamPath", "path to beam state"))
if (getMstate() == NULL)
msg_error() << "Error cannot find the mechanical state";

this->getContext()->get(m_beam, d_beamPath.getValue());
if (m_beam == nullptr)
msg_error() << "Cannot find the beam collision state : " << d_beamPath.getValue();

this->getContext()->get(m_modifier);
if (m_modifier == NULL)
{
this->f_listening.setValue(true);
msg_error() << " Error cannot find the EdgeSetTopologyModifier";
return;
}
}

void PointsManager::init()
{
Inherit1::init();
void PointsManager::addNewPointToState()
{
auto x = sofa::helper::getWriteAccessor(*getMstate()->write(sofa::core::VecCoordId::position()));
auto xRest = sofa::helper::getWriteAccessor(*getMstate()->write(sofa::core::VecCoordId::restPosition()));
auto xfree = sofa::helper::getWriteAccessor(*getMstate()->write(sofa::core::VecCoordId::freePosition()));
auto xforce = sofa::helper::getWriteAccessor(*getMstate()->write(sofa::core::VecDerivId::force()));
const auto &beam = m_beam->readPositions();

if (getTopology() == NULL)
msg_error() << "Error cannot find the topology";
unsigned nbPoints = this->getTopology()->getNbPoints();
// do not take the last point because there is a bug
// TODO(dmarchal 2024-07-12) fix the bug you are refering to in the previous line

if (getMstate() == NULL)
msg_error() << "Error cannot find the topology";
size_t beamSz = beam.size();

this->getContext()->get(m_beam, d_beamPath.getValue());
if (m_beam == nullptr)
msg_error() << "Cannot find the beam collision state : " << d_beamPath.getValue();
m_modifier->addPoints(1, true);
sofa::type::Vec3 pos = beam[beamSz - 1];

this->getContext()->get(m_modifier);
if (m_modifier == NULL)
{
msg_error() << " Error cannot find the EdgeSetTopologyModifier";
return;
}
}
x.resize(nbPoints + 1);
xRest.resize(nbPoints + 1);
xfree.resize(nbPoints + 1);
xforce.resize(nbPoints + 1);

void PointsManager::addNewPointToState()
x[nbPoints] = pos;
xRest[nbPoints] = pos;
xfree[nbPoints] = pos;
xforce[nbPoints] = sofa::type::Vec3(0, 0, 0);

m_modifier->notifyEndingEvent();
}

void PointsManager::removeLastPointfromState()
{
// do not take the last point because there is a bug
// TODO(dmarchal 2024-07-12) fix the bug you are refering to in the previous line

unsigned nbPoints = getTopology()->getNbPoints();

if (nbPoints == 0)
{
helper::WriteAccessor<Data<VecCoord>> x = *this->getMstate()->write(core::VecCoordId::position());
helper::WriteAccessor<Data<VecCoord>> xRest = *this->getMstate()->write(core::VecCoordId::restPosition());
helper::WriteAccessor<Data<VecCoord>> xfree = *this->getMstate()->write(core::VecCoordId::freePosition());
helper::WriteAccessor<Data<VecCoord>> xforce = *this->getMstate()->write(core::VecDerivId::force());
const helper::ReadAccessor<Data<VecCoord>> &beam = m_beam->readPositions();
unsigned nbPoints = this->getTopology()->getNbPoints(); // do not take the last point because there is a bug

size_t beamSz = beam.size();
m_modifier->addPoints(1, true);

Vec3 pos = beam[beamSz - 1];
// std::cout << "beam tip is =-----> " << pos << std::endl;
// std::cout << "nbPoints is equal :" << nbPoints << std::endl;
// std::cout << "x.size is equal :" << x.size() << std::endl;

x.resize(nbPoints + 1);
xRest.resize(nbPoints + 1);
xfree.resize(nbPoints + 1);
xforce.resize(nbPoints + 1);

x[nbPoints] = pos;
xRest[nbPoints] = pos;
xfree[nbPoints] = pos;
xforce[nbPoints] = Vec3(0, 0, 0);

m_modifier->notifyEndingEvent();
// std::cout << "End addNewPointToState " << std::endl;
// std::cout << "End notifyEndingEvent " << std::endl;
msg_info() << "No more points";
return;
}

void PointsManager::removeLastPointfromState()
{
helper::WriteAccessor<Data<VecCoord>> x = *this->getMstate()->write(core::VecCoordId::position());
helper::WriteAccessor<Data<VecCoord>> xfree = *this->getMstate()->write(core::VecCoordId::freePosition());
unsigned nbPoints = this->getTopology()->getNbPoints(); // do not take the last point because there is a bug
sofa::type::vector<unsigned int> Indices;
auto x = sofa::helper::getWriteAccessor(*getMstate()->write(sofa::core::VecCoordId::position()));
auto xfree = sofa::helper::getWriteAccessor(*getMstate()->write(sofa::core::VecCoordId::freePosition()));

if (nbPoints > 0)
{
Indices.push_back(nbPoints - 1);
m_modifier->removePoints(Indices, true);
x.resize(nbPoints - 1);
msg_info() << "the size is equal :" << nbPoints;
xfree.resize(nbPoints - 1);
}
else
{
msg_error() << "Error cannot remove the last point because there is no point in the state";
}
m_modifier->notifyEndingEvent();
}
sofa::type::vector<unsigned int> indices = {nbPoints - 1};
m_modifier->removePoints(indices, true);

x.resize(nbPoints - 1);
msg_info() << "the size is equal :" << nbPoints;
xfree.resize(nbPoints - 1);
m_modifier->notifyEndingEvent();
}

void PointsManager::handleEvent(sofa::core::objectmodel::Event *event)
void PointsManager::handleEvent(sofa::core::objectmodel::Event *event)
{
if (KeypressedEvent::checkEventType(event))
{
if (KeypressedEvent::checkEventType(event))
KeypressedEvent *ev = static_cast<KeypressedEvent *>(event);
switch (ev->getKey())
{
KeypressedEvent *ev = static_cast<KeypressedEvent *>(event);
switch (ev->getKey())
{
case 'S':
case 's':
msg_info() << "A point is created ." ;
addNewPointToState();
break;
case 'L':
case 'l':
msg_info() <<("Remove point from state \n");
removeLastPointfromState();
break;
}
case 'S':
case 's':
msg_info() << "A point is created ." ;
addNewPointToState();
break;
case 'L':
case 'l':
msg_info() <<("Remove point from state \n");
removeLastPointfromState();
break;
}
}
}

// void PointsManager::draw(const core::visual::VisualParams *vparams)
// {

// helper::ReadAccessor<Data<VecCoord>> x = *this->getMstate()->read(core::VecCoordId::position());
// if (!x.size())
// return; // if no points return
// glDisable(GL_LIGHTING);
// for (unsigned int j = 0; j < x.size(); j++)
// {
// glColor3f(1.0, 1.0, 0.0);
// vparams->drawTool()->drawSphere(x[j], d_radius.getValue() * 0.001);
// }
// glEnable(GL_LIGHTING);
// }

} // Sofa
} // namespace cosserat::controller

0 comments on commit a79ee6f

Please sign in to comment.