C++ StateVector::setStates方法代码示例

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
//.........这里部分代码省略.........