void addLoadToStorage(Storage &forceStore, SimTK::Vec3 force, SimTK::Vec3 point, SimTK::Vec3 torque)
{
int nLoads = forceStore.getColumnLabels().getSize()/9;
string labels[9] = { "forceX", "forceY", "forceZ", "pointX", "pointY", "pointZ","torqueX", "torqueY", "torqueZ"};
char suffix[2];
sprintf(suffix, "%d", nLoads);
Array<string> col_labels;
col_labels.append("time");
StateVector dataRow;
dataRow.setTime(0);
double data[9];
for(int i = 0; i<9; i++){
col_labels.append(labels[i]);
if(i<3){
data[i] = force[i]; continue;
}
else if(i<6){
data[i] = point[i-3]; continue;
}
else
data[i] = torque[i-6];
}
dataRow.setStates(0, 9, data);
Storage *forces = NULL;
Storage tempStore;
if(nLoads == 0)
forces = &forceStore;
else if (nLoads > 0)
forces = &tempStore;
else
throw OpenSim::Exception("addLoadToStorage: ERROR");
forces->setColumnLabels(col_labels);
forces->append(dataRow);
dataRow.setTime(1.0);
forces->append(dataRow);
dataRow.setTime(2.0);
forces->append(dataRow);
if (nLoads > 0)
forces->addToRdStorage(forceStore, 0.0, 1.0);
}
bool Manager::doIntegration(SimTK::State& s, int step, double dtFirst ) {
if(!_integ) {
throw Exception("Manager::doIntegration(): "
"Integrator has not been set. Construct the Manager "
"with an integrator, or call Manager::setIntegrator().");
}
// CLEAR ANY INTERRUPT
// Halts must arrive during an integration.
clearHalt();
double dt/*,dtPrev*/,tReal;
double time =_ti;
dt=dtFirst;
if(dt>_dtMax) dt = _dtMax;
//dtPrev=dt;
// CHECK SPECIFIED DT STEPPING
if(_specifiedDT) {
if(_tArray.getSize()<=0) {
string msg="IntegRKF.integrate: ERR- specified dt stepping not";
msg += "possible-- empty time array.";
throw( Exception(msg) );
}
double first = _tArray[0];
double last = _tArray.getLast();
if((getTimeArrayStep(_ti)<0) || (_ti<first) || (_tf>last)) {
string msg="IntegRKF.integrate: ERR- specified dt stepping not";
msg +="possible-- time array does not cover the requested";
msg +=" integration interval.";
throw(Exception(msg));
}
}
// RECORD FIRST TIME STEP
if(!_specifiedDT) {
resetTimeAndDTArrays(time);
if(_tArray.getSize()<=0) {
_tArray.append(time);
}
}
bool fixedStep = false;
double fixedStepSize;
if( _constantDT || _specifiedDT) fixedStep = true;
// If _system is has been set we should be integrating a CMC system
// not the model's system.
const SimTK::System& sys = _system ? *_system
: _model->getMultibodySystem();
// Only initialize a TimeStepper if it hasn't been done yet
if (_timeStepper == NULL) initializeTimeStepper(sys, s);
SimTK::Integrator::SuccessfulStepStatus status;
if( fixedStep ) {
dt = getFixedStepSize(getTimeArrayStep(_ti));
} else {
_integ->setReturnEveryInternalStep(true);
}
if( s.getTime()+dt >= _tf ) dt = _tf - s.getTime();
// We need to be at a valid stage to initialize the controls, but only when
// we are integrating the complete model system, not the CMC system. This
// is very ugly and a cleaner solution is required- aseth
if(_system == NULL)
sys.realize(s, SimTK::Stage::Velocity); // this is multibody system
initialize(s, dt);
if( fixedStep){
s.updTime() = time;
sys.realize(s, SimTK::Stage::Acceleration);
if(_performAnalyses)_model->updAnalysisSet().step(s, step);
tReal = s.getTime();
if( _writeToStorage ) {
SimTK::Vector stateValues = _model->getStateVariableValues(s);
StateVector vec;
vec.setStates(tReal, stateValues);
getStateStorage().append(vec);
if(_model->isControlled())
_controllerSet->storeControls(s,step);
}
}
double stepToTime = _tf;
// LOOP
while( time < _tf ) {
if( fixedStep ){
fixedStepSize = getNextTimeArrayTime( time ) - time;
if( fixedStepSize + time >= _tf ) fixedStepSize = _tf - time;
_integ->setFixedStepSize( fixedStepSize );
stepToTime = time + fixedStepSize;
}
// stepTo() does not return if it fails. However, the final step
// is returned once as an ordinary return; by the time we get
// EndOfSimulation status we have already seen the state and don't
// need to record it again.
status = _timeStepper->stepTo(stepToTime);
if( status != SimTK::Integrator::EndOfSimulation ) {
const SimTK::State& s = _integ->getState();
if(_performAnalyses)_model->updAnalysisSet().step(s,step);
tReal = s.getTime();
if( _writeToStorage) {
SimTK::Vector stateValues = _model->getStateVariableValues(s);
StateVector vec;
vec.setStates(tReal, stateValues);
getStateStorage().append(vec);
if(_model->isControlled())
_controllerSet->storeControls(s, step);
}
step++;
}
else
halt();
time = _integ->getState().getTime();
// CHECK FOR INTERRUPT
if(checkHalt()) break;
}
finalize(_integ->updAdvancedState() );
s = _integ->getState();
// CLEAR ANY INTERRUPT
clearHalt();
return true;
}
