download/aot_html/cam__set_current_state_estimate_8cpp_source.html

00001 #include "mex.h"
00002 #include "ConstraintApp.h"
00003
00016 void mexFunction( int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[] )
00017 {
00018   if ( nrhs != 2 ) {
00019     mexErrMsgTxt("requires exactly 2 parameters");
00020   }
00021
00022   if ( nlhs != 0 ) {
00023     mexErrMsgTxt("returns exactly 0 parameters");
00024   }
00025
00026   ConstraintApp* app(*(ConstraintApp**)mxGetData(prhs[0]));
00027
00028   int stateSize = app->GetStateSize();
00029   if ( mxGetN(prhs[1]) * mxGetM(prhs[1]) != stateSize ) {
00030     mexErrMsgTxt("state must be Sx1, where S is the state size");
00031   }
00032
00033   double* s((double*)mxGetData(prhs[1]));
00034
00035   std::vector<double> state(stateSize);
00036   std::copy(s, s+stateSize, state.begin());
00037   app->SetCurrentStateEstimate(state);
00038 }