void addflops(unsigned fl)
{
#if 0
mxArray *flopcount = mexGetArray("flopcount","global");
if(flopcount && !mxIsEmpty(flopcount))) {
*mxGetPr(flopcount) += fl;
mexPutArray(flopcount,"global");
}
#else
/* Matlab 6.5 */
mxArray *flopcount = mexGetVariable("global","flopcount");
if(flopcount && !mxIsEmpty(flopcount)) {
*mxGetPr(flopcount) += fl;
mexPutVariable("global","flopcount",flopcount);
}
#endif
}
void storeMBSdata(MBSdataStruct *s, char *s_name){
mxArray *s_ptr, *field_value_ptr;
int i,j;
/* Get pointer to MBSdataStruct */
#ifdef OLDVER
s_ptr = mexGetArray(s_name,"base"); //v6
#else
s_ptr = mexGetVariable("base", s_name);
#endif
if (s_ptr == NULL){
mexErrMsgTxt("Could not get structure variable.\n");
}
//=== Work Variables ==========================================================//
/* q */
field_value_ptr = mxGetField(s_ptr, 0, "q");
for(i=1;i<=s->njoint;i++)
{
mxGetPr(field_value_ptr)[i-1] = s->q[i];
}
mxSetField(s_ptr, 0, "q", field_value_ptr);
/* qd */
field_value_ptr = mxGetField(s_ptr, 0, "qd");
for(i=1;i<=s->njoint;i++)
{
mxGetPr(field_value_ptr)[i-1] = s->qd[i];
}
mxSetField(s_ptr, 0, "qd", field_value_ptr);
/* qdd */
field_value_ptr = mxGetField(s_ptr, 0, "qdd");
for(i=1;i<=s->njoint;i++)
{
mxGetPr(field_value_ptr)[i-1] = s->qdd[i];
}
mxSetField(s_ptr, 0, "qdd", field_value_ptr);
/* frc */
field_value_ptr = mxGetField(s_ptr, 0, "frc");
for(i=1;i<=3;i++)
{
for(j=1;j<=s->nbody;j++)
{
mxGetPr(field_value_ptr)[(i-1)+3*(j-1)] = s->frc[i][j];
}
}
mxSetField(s_ptr, 0, "frc", field_value_ptr);
/* trq */
field_value_ptr = mxGetField(s_ptr, 0, "trq");
for(i=1;i<=3;i++)
{
for(j=1;j<=s->nbody;j++)
{
mxGetPr(field_value_ptr)[(i-1)+3*(j-1)] = s->trq[i][j];
}
}
mxSetField(s_ptr, 0, "trq", field_value_ptr);
/* Qq */
field_value_ptr = mxGetField(s_ptr, 0, "Qq");
for(i=1;i<=s->njoint;i++)
{
mxGetPr(field_value_ptr)[i-1] = s->Qq[i];
}
mxSetField(s_ptr, 0, "Qq", field_value_ptr);
/* Fl */
//field_value_ptr = mxGetField(s_ptr, 0, "Fl");
field_value_ptr = mxCreateDoubleMatrix(1,s->Nlink,mxREAL);
for(i=1;i<=s->Nlink;i++)
{
mxGetPr(field_value_ptr)[i-1] = s->Fl[i];
}
mxSetField(s_ptr, 0, "Fl", field_value_ptr);
/* Z */
//field_value_ptr = mxGetField(s_ptr, 0, "Z");
field_value_ptr = mxCreateDoubleMatrix(1,s->Nlink,mxREAL);
for(i=1;i<=s->Nlink;i++)
{
mxGetPr(field_value_ptr)[i-1] = s->Z[i];
}
mxSetField(s_ptr, 0, "Z", field_value_ptr);
/* Zd */
//field_value_ptr = mxGetField(s_ptr, 0, "Zd");
field_value_ptr = mxCreateDoubleMatrix(1,s->Nlink,mxREAL);
for(i=1;i<=s->Nlink;i++)
{
mxGetPr(field_value_ptr)[i-1] = s->Zd[i];
}
mxSetField(s_ptr, 0, "Zd", field_value_ptr);
/* ux */
field_value_ptr = mxGetField(s_ptr, 0, "ux");
for(i=1;i<=s->Nux;i++)
{
mxGetPr(field_value_ptr)[i-1] = s->ux[i];
}
mxSetField(s_ptr, 0, "ux", field_value_ptr);
/* uxd */
field_value_ptr = mxGetField(s_ptr, 0, "uxd");
for(i=1;i<=s->Nux;i++)
{
mxGetPr(field_value_ptr)[i-1] = s->uxd[i];
}
mxSetField(s_ptr, 0, "uxd", field_value_ptr);
#ifdef OLDVER
mexPutArray(s_ptr,"base"); //v6
#else
mexPutVariable("base", s_name,s_ptr);
#endif
}