C++ StatesTrajectory::getSize方法代码示例

void testCopying() {
    Model model("gait2354_simbody.osim");
    auto& state = model.initSystem();

    StatesTrajectory states;
    {
        state.setTime(0.5);
        states.append(state);
        state.setTime(1.3);
        states.append(state);
        state.setTime(3.5);
        states.append(state);
    }

    {
        StatesTrajectory statesCopyConstruct(states);
        // Ideally we'd check for equality (operator==()), but State does not
        // have an equality operator.
        SimTK_TEST_EQ((int)statesCopyConstruct.getSize(), (int)states.getSize());
        for (size_t i = 0; i < states.getSize(); ++i) {
            SimTK_TEST_EQ(statesCopyConstruct[i].getTime(), states[i].getTime());
        }
    }

    {
        StatesTrajectory statesCopyAssign;
        statesCopyAssign = states;
        SimTK_TEST_EQ((int)statesCopyAssign.getSize(), (int)states.getSize());
        for (size_t i = 0; i < states.getSize(); ++i) {
            SimTK_TEST_EQ(statesCopyAssign[i].getTime(), states[i].getTime());
        }
    }
}

void testBoundsCheck() {
    Model model("gait2354_simbody.osim");
    const auto& state = model.initSystem();
    StatesTrajectory states;
    states.append(state);
    states.append(state);
    states.append(state);
    states.append(state);
    
    #ifdef NDEBUG
        // In DEBUG, Visual Studio puts asserts into the index operator.
        states[states.getSize() + 100];
        states[4];
        states[5];
    #endif
    SimTK_TEST_MUST_THROW_EXC(states.get(4), IndexOutOfRange);
    SimTK_TEST_MUST_THROW_EXC(states.get(states.getSize() + 100),
                              IndexOutOfRange);
}

void tableAndTrajectoryMatch(const Model& model,
                             const TimeSeriesTable& table,
                             const StatesTrajectory& states,
                             std::vector<std::string> columns = {}) {

    const auto stateNames = model.getStateVariableNames();

    size_t numColumns{};
    if (columns.empty()) {
        numColumns = stateNames.getSize();
    } else {
        numColumns = columns.size();
    }
    SimTK_TEST(table.getNumColumns() == numColumns);
    SimTK_TEST(table.getNumRows() == states.getSize());

    const auto& colNames = table.getColumnLabels();

    // Test that the data table has exactly the same numbers.
    for (size_t itime = 0; itime < states.getSize(); ++itime) {
        // Test time.
        SimTK_TEST(table.getIndependentColumn()[itime] ==
                   states[itime].getTime());

        // Test state values.
        for (size_t icol = 0; icol < table.getNumColumns(); ++icol) {
            const auto& stateName = colNames[icol];

            const auto& valueInStates = model.getStateVariableValue(
                    states[itime], stateName);
            const auto& column = table.getDependentColumnAtIndex(icol);
            const auto& valueInTable = column[static_cast<int>(itime)];

            SimTK_TEST(valueInStates == valueInTable);
        }
    }
}

void testPopulateTrajectoryAndStatesTrajectoryReporter() {
    Model model("gait2354_simbody.osim");

    // To assist with creating interesting (non-zero) coordinate values:
    model.updCoordinateSet().get("pelvis_ty").setDefaultLocked(true);

    // Also, test the StatesTrajectoryReporter.
    auto* statesCol = new StatesTrajectoryReporter();
    statesCol->setName("states_collector_all_steps");
    model.addComponent(statesCol);

        const double finalTime = 0.05;
    {
        auto& state = model.initSystem();
    
        SimTK::RungeKuttaMersonIntegrator integrator(model.getSystem());
        SimTK::TimeStepper ts(model.getSystem(), integrator);
        ts.initialize(state);
        ts.setReportAllSignificantStates(true);
        integrator.setReturnEveryInternalStep(true);
    
        StatesTrajectory states;
        std::vector<double> times;
        while (ts.getState().getTime() < finalTime) {
            ts.stepTo(finalTime);
            times.push_back(ts.getState().getTime());
            // StatesTrajectory API for appending states:
            states.append(ts.getState());
            // For the StatesTrajectoryReporter:
            model.getMultibodySystem().realize(ts.getState(), SimTK::Stage::Report);
        }
    
        // Make sure we have all the states
        SimTK_TEST_EQ((int)states.getSize(), (int)times.size());
        SimTK_TEST_EQ((int)statesCol->getStates().getSize(), (int)times.size());
        // ...and that they aren't all just references to the same single state.
        for (int i = 0; i < (int)states.getSize(); ++i) {
            SimTK_TEST_EQ(states[i].getTime(), times[i]);
            SimTK_TEST_EQ(statesCol->getStates()[i].getTime(), times[i]);
        }
    }

    // Test the StatesTrajectoryReporter with a constant reporting interval.
    statesCol->clear();
    auto* statesColInterval = new StatesTrajectoryReporter();
    statesColInterval->setName("states_collector_interval");
    statesColInterval->set_report_time_interval(0.01);
    model.addComponent(statesColInterval);

    {
        auto& state = model.initSystem();
        SimTK::RungeKuttaMersonIntegrator integrator(model.getSystem());
        SimTK::TimeStepper ts(model.getSystem(), integrator);
        ts.initialize(state);
        ts.setReportAllSignificantStates(true);
        integrator.setReturnEveryInternalStep(true);

        while (ts.getState().getTime() < finalTime) {
            ts.stepTo(finalTime);
            model.getMultibodySystem().realize(ts.getState(), SimTK::Stage::Report);
        }

        SimTK_TEST(statesColInterval->getStates().getSize() == 6);
        std::vector<double> times { 0, 0.01, 0.02, 0.03, 0.04, 0.05 };
        int i = 0;
        for (const auto& s : statesColInterval->getStates()) {
            ASSERT_EQUAL(s.getTime(), times[i], 1e-5);
            ++i;
        }
    }
}