diff --git a/src/SoftRobots.Inverse/component/constraint/BarycentricCenterEffector.h b/src/SoftRobots.Inverse/component/constraint/BarycentricCenterEffector.h index c0fcf47..b486164 100644 --- a/src/SoftRobots.Inverse/component/constraint/BarycentricCenterEffector.h +++ b/src/SoftRobots.Inverse/component/constraint/BarycentricCenterEffector.h @@ -75,10 +75,6 @@ class SOFA_SOFTROBOTS_INVERSE_API BarycentricCenterEffector : public PositionEff /// traversal of graph creation and modification, void init() override; - /// According to BaseObject::reset - /// this method should be used to reset the object in its initial state. - void reset() override; - /// According to BaseObject::draw /// this method should be used to render internal data of this object, /// for debugging purposes. diff --git a/src/SoftRobots.Inverse/component/constraint/BarycentricCenterEffector.inl b/src/SoftRobots.Inverse/component/constraint/BarycentricCenterEffector.inl index 4de55b5..43779ea 100644 --- a/src/SoftRobots.Inverse/component/constraint/BarycentricCenterEffector.inl +++ b/src/SoftRobots.Inverse/component/constraint/BarycentricCenterEffector.inl @@ -94,22 +94,21 @@ void BarycentricCenterEffector::init() } } -template -void BarycentricCenterEffector::reset() -{ -} template void BarycentricCenterEffector::computeBarycenter() { - ReadAccessor > positions = m_state->readPositions(); - const unsigned int nbp = m_state->getSize(); - Coord barycenter = Coord(); - for (unsigned int i=0; i > positions = m_state->readPositions(); + if (const sofa::Size nbp = m_state->getSize()) + { + const Coord barycenter = std::accumulate(positions->begin(), positions->end(), Coord{}, std::plus()) / nbp; + d_barycenter.setValue(barycenter); + } + else + { + d_barycenter.setValue(Coord{}); + msg_error() << "Trying to compute a barycenter from an empty list of positions."; + } } template @@ -201,9 +200,8 @@ void BarycentricCenterEffector::draw(const VisualParams* vparams) return; computeBarycenter(); - Coord b = d_barycenter.getValue(); - std::vector positions; - positions.push_back(b); + const Coord& b = d_barycenter.getValue(); + std::vector positions(1, b); vparams->drawTool()->drawPoints(positions, float(5.), RGBAColor(0.0f,0.0f,1.0f,1.0f)); }