void RestShapeSpringsForceField<Rigid3fTypes>::draw(const core::visual::VisualParams* vparams)
{
if (!vparams->displayFlags().getShowForceFields() || !drawSpring.getValue())
return; /// \todo put this in the parent class
vparams->drawTool()->saveLastState();
vparams->drawTool()->setLightingEnabled(false);
sofa::helper::ReadAccessor< DataVecCoord > p0 = *getExtPosition();
sofa::helper::ReadAccessor< DataVecCoord > p = this->mstate->read(core::VecCoordId::position());
sofa::helper::vector<sofa::defaulttype::Vector3> vertices;
for (unsigned int i=0; i<m_indices.size(); i++)
{
const unsigned int index = m_indices[i];
sofa::defaulttype::Vector3 v0(p[index].getCenter()[0],
p[index].getCenter()[1],
p[index].getCenter()[2]);
unsigned int tempIndex = (useRestMState) ? m_ext_indices[i] : index;
sofa::defaulttype::Vector3 v1(p0[tempIndex].getCenter()[0],
p0[tempIndex].getCenter()[1],
p0[tempIndex].getCenter()[2]);
vertices.push_back(v0);
vertices.push_back(v1);
}
vparams->drawTool()->drawLines(vertices,5,springColor.getValue());
vparams->drawTool()->restoreLastState();
}
void RestShapeSpringsForceField<Vec3dTypes>::draw(const core::visual::VisualParams* vparams)
{
#ifndef SOFA_NO_OPENGL
if (!vparams->displayFlags().getShowForceFields())
return; /// \todo put this in the parent class
sofa::helper::ReadAccessor< DataVecCoord > p0 = *getExtPosition();
sofa::helper::ReadAccessor< DataVecCoord > p = this->mstate->read(core::VecCoordId::position());
const VecIndex& indices = m_indices;
const VecIndex& ext_indices = (useRestMState ? m_ext_indices : m_indices);
for (unsigned int i=0; i<indices.size(); i++)
{
const unsigned int index = indices[i];
const unsigned int ext_index = ext_indices[i];
glDisable(GL_LIGHTING);
glBegin(GL_LINES);
glColor3f(0,1,0);
glVertex3f( (GLfloat)p[index][0], (GLfloat)p[index][1], (GLfloat)p[index][2] );
glVertex3f( (GLfloat)p0[ext_index][0], (GLfloat)p0[ext_index][1], (GLfloat)p0[ext_index][2] );
glEnd();
}
#endif /* SOFA_NO_OPENGL */
}
void RestShapeSpringsForceField<Rigid3fTypes>::addForce(const core::MechanicalParams* /* mparams */ /* PARAMS FIRST */, DataVecDeriv& f, const DataVecCoord& x, const DataVecDeriv& /* v */)
{
sofa::helper::WriteAccessor< DataVecDeriv > f1 = f;
sofa::helper::ReadAccessor< DataVecCoord > p1 = x;
sofa::helper::ReadAccessor< DataVecCoord > p0 = *getExtPosition();
f1.resize(p1.size());
if (recompute_indices.getValue())
{
recomputeIndices();
}
const VecReal& k = stiffness.getValue();
const VecReal& k_a = angularStiffness.getValue();
for (unsigned int i=0; i<m_indices.size(); i++)
{
const unsigned int index = m_indices[i];
const unsigned int ext_index = m_ext_indices[i];
// translation
Vec3f dx = p1[index].getCenter() - p0[ext_index].getCenter();
getVCenter(f1[index]) -= dx * (i < k.size() ? k[i] : k[0]) ;
// rotation
Quatf dq = p1[index].getOrientation() * p0[ext_index].getOrientation().inverse();
Vec3f dir;
Real angle=0;
dq.normalize();
if (dq[3] < 0.999999999999999)
dq.quatToAxis(dir, angle);
dq.quatToAxis(dir, angle);
//std::cout<<"dq : "<<dq <<" dir :"<<dir<<" angle :"<<angle<<std::endl;
getVOrientation(f1[index]) -= dir * angle * (i < k_a.size() ? k_a[i] : k_a[0]) ;
}
}
static void stabilizerTask(void* param)
{
uint32_t tick;
uint32_t lastWakeTime;
vTaskSetApplicationTaskTag(0, (void*)TASK_STABILIZER_ID_NBR);
//Wait for the system to be fully started to start stabilization loop
systemWaitStart();
// Wait for sensors to be calibrated
lastWakeTime = xTaskGetTickCount ();
while(!sensorsAreCalibrated()) {
vTaskDelayUntil(&lastWakeTime, F2T(RATE_MAIN_LOOP));
}
// Initialize tick to something else then 0
tick = 1;
while(1) {
vTaskDelayUntil(&lastWakeTime, F2T(RATE_MAIN_LOOP));
getExtPosition(&state);
#ifdef ESTIMATOR_TYPE_kalman
stateEstimatorUpdate(&state, &sensorData, &control);
#else
sensorsAcquire(&sensorData, tick);
stateEstimator(&state, &sensorData, tick);
#endif
commanderGetSetpoint(&setpoint, &state);
sitAwUpdateSetpoint(&setpoint, &sensorData, &state);
stateController(&control, &setpoint, &sensorData, &state, tick);
powerDistribution(&control);
tick++;
}
}
void RestShapeSpringsForceField<Rigid3fTypes>::draw(const core::visual::VisualParams* vparams)
{
#ifndef SOFA_NO_OPENGL
if (!vparams->displayFlags().getShowForceFields())
return; /// \todo put this in the parent class
const VecIndex& indices = points.getValue();
const VecIndex& ext_indices=external_points.getValue();
sofa::helper::ReadAccessor< DataVecCoord > p0 = *getExtPosition();
sofa::helper::ReadAccessor< DataVecCoord > p = this->mstate->read(core::VecCoordId::position());
// if(ext_indices.size() == indices.size())
for (unsigned int i=0; i<indices.size(); i++)
{
const unsigned int index = indices[i];
glDisable(GL_LIGHTING);
glLineWidth(4.0);
glBegin(GL_LINES);
glColor3f(0,1,0);
glVertex3fv(&p[index].getCenter().elems[0]);
if(useRestMState)
{
const unsigned int ext_index = ext_indices[i];
glVertex3fv(&p0[ext_index].getCenter().elems[0]);
}
else
{
glVertex3fv(&p0[index].getCenter().elems[0]);
}
glEnd();
}
#endif /* SOFA_NO_OPENGL */
}