void testCopyModel(string fileName)
{
Model *model = new Model(fileName);
SimTK::State defaultState = model->initSystem();
Model *modelCopy = new Model(*model);
// At this point properties should all match. assert that
ASSERT(*model==*modelCopy);
delete model;
SimTK::State& defaultStateOfCopy = modelCopy->initSystem();
// Compare state
defaultState.getY().dump("defaultState:Y");
ASSERT ((defaultState.getY()-defaultStateOfCopy.getY()).norm() < 1e-7);
defaultState.getZ().dump("defaultState:Z");
ASSERT ((defaultState.getZ()-defaultStateOfCopy.getZ()).norm() < 1e-7);
// Now delete original model and make sure copy can stand
Model *newModel = modelCopy->clone();
// Compare state again
delete modelCopy;
SimTK::State& defaultStateOfCopy2 = newModel->initSystem();
// Compare state
ASSERT ((defaultState.getY()-defaultStateOfCopy2.getY()).norm() < 1e-7);
ASSERT ((defaultState.getZ()-defaultStateOfCopy2.getZ()).norm() < 1e-7);
delete newModel;
}
void compareSimulations(SimTK::MultibodySystem &system, SimTK::State &state, Model *osimModel, SimTK::State &osim_state, string errorMessagePrefix = "")
{
using namespace SimTK;
// Set the initial states for both Simbody system and OpenSim model
Vector& qi = state.updQ();
Vector& ui = state.updU();
int nq_sb = initTestStates(qi, ui);
int nq = osim_state.getNQ();
// Push down to OpenSim "state"
if(nq == 2*nq_sb){ //more coordinates because OpenSim model is constrained
osim_state.updY()[0] = state.getY()[0];
osim_state.updY()[1] = state.getY()[1];
osim_state.updY()[nq] = state.getY()[nq_sb];
osim_state.updY()[nq+1] = state.getY()[nq_sb+1];
}
else
osim_state.updY() = state.getY();
//==========================================================================================================
// Integrate Simbody system
integrateSimbodySystem(system, state);
// Simbody model final states
qi = state.updQ();
ui = state.updU();
qi.dump("\nSimbody Final q's:");
ui.dump("\nSimbody Final u's:");
//==========================================================================================================
// Integrate OpenSim model
integrateOpenSimModel(osimModel, osim_state);
// Get the state at the end of the integration from OpenSim.
Vector& qf = osim_state.updQ();
Vector& uf = osim_state.updU();
cout<<"\nOpenSim Final q's:\n "<<qf<<endl;
cout<<"\nOpenSim Final u's:\n "<<uf<<endl;
//==========================================================================================================
// Compare Simulation Results
compareSimulationStates(qi, ui, qf, uf, errorMessagePrefix);
}