bool ConstraintSet::setControlParameter(int id, ConstraintAction action, double data, double timestep)
{
ConstraintValues values;
ConstraintSingleValue value;
values.values = &value;
values.number = 0;
values.action = action;
values.id = id;
value.action = action;
value.id = id;
switch (action) {
case ACT_NONE:
return true;
case ACT_VALUE:
value.yd = data;
values.number = 1;
break;
case ACT_VELOCITY:
value.yddot = data;
values.number = 1;
break;
case ACT_TOLERANCE:
values.tolerance = data;
break;
case ACT_FEEDBACK:
values.feedback = data;
break;
case ACT_ALPHA:
values.alpha = data;
break;
default:
assert(action==ACT_NONE);
}
return setControlParameters(&values, 1, timestep);
}
void CopyPose::updateControlOutput(const Timestamp& timestamp)
{
//IMO this should be done, no idea if it is enough (wrt Distance impl)
Twist y = diff(F_identity, m_internalPose);
bool found = true;
if (!timestamp.substep) {
if (!timestamp.reiterate) {
found = popPose(timestamp.cacheTimestamp);
}
}
if (m_constraintCallback && (m_substep || (!timestamp.reiterate && !timestamp.substep))) {
// initialize first callback the application to get the current values
int i=0;
if (m_outputControl & CTL_POSITION) {
updateValues(y.vel, &m_values[i++], &m_pos, CTL_POSITIONX);
}
if (m_outputControl & CTL_ROTATION) {
updateValues(y.rot, &m_values[i++], &m_rot, CTL_ROTATIONX);
}
if ((*m_constraintCallback)(timestamp, m_values, m_nvalues, m_constraintParam)) {
setControlParameters(m_values, m_nvalues, (found && timestamp.interpolate)?timestamp.realTimestep:0.0);
}
}
if (m_outputControl & CTL_POSITION) {
updateOutput(y.vel, &m_pos, CTL_POSITIONX);
}
if (m_outputControl & CTL_ROTATION) {
updateOutput(y.rot, &m_rot, CTL_ROTATIONX);
}
}
