void mexFunction(
int nlhs, mxArray *plhs[],
int nrhs, const mxArray *prhs[])
{
mwSize n, n1; /*mxGetM*/
bool type; /*true*/
if (nrhs<1) mexErrMsgTxt("Not enough input arguments.");
if (nrhs>2) mexErrMsgTxt("Too many input arguments.");
if (nlhs>1) mexErrMsgTxt("Too many output arguments.");
if (!mxIsDouble(prhs[0]) || mxIsSparse(prhs[0]))
mexErrMsgTxt("Function 'vech' not defined for variables of input class");
n=mxGetM(prhs[0]);
if (mxGetN(prhs[0])!=n) mexErrMsgTxt("Input matrix must be square");
n1=n*(n+1)/2;
if (nrhs>1)
if (*mxGetPr(prhs[1])==1) type=true; else type=false;
else
type=false;
if (mxIsComplex(prhs[0]))
{
plhs[0]=mxCreateDoubleMatrix(n1,1,mxCOMPLEX);
vech(mxGetPr(prhs[0]),mxGetPr(plhs[0]),n,type);
vech(mxGetPi(prhs[0]),mxGetPi(plhs[0]),n,type);
}
else
{
plhs[0]=mxCreateDoubleMatrix(n1,1,mxREAL);
vech(mxGetPr(prhs[0]),mxGetPr(plhs[0]),n,type);
}
}
void mexFunction(
int nlhs, mxArray *plhs[],
int nrhs, const mxArray *prhs[])
{
/* ***************** */
/* Declare variables */
/* ***************** */
double *A, *Aptr, *V, *Vptr, *temp;
mwSize m, n, mm, mn, m1, i, N, j;
/* ********************************************** */
/* Determine input sizes and perform error checks */
/* ********************************************** */
if (nrhs<3 || nrhs>3)
mexErrMsgTxt("Three arguments must be passed");
if (nlhs>1)
mexErrMsgTxt("ROWVECH produce only on output");
for (i=0; i<3; i++)
if (!mxIsDouble(prhs[i]) || mxIsSparse(prhs[i]))
mexErrMsgTxt("Inputs must be full double matrices");
if (mxGetNumberOfElements(prhs[1])!=1)
mexErrMsgTxt("m must be a scalar");
if (mxGetNumberOfElements(prhs[2])!=1)
mexErrMsgTxt("n must be a scalar");
m=*mxGetPr(prhs[1]);
n=*mxGetPr(prhs[2]);
mm=m*m;
mn=m*n;
m1=m*(m+1)/2;
if (mxGetN(prhs[0])!=mn)
mexErrMsgTxt("Inputs are incompatible");
N=mxGetM(prhs[0]);
V=mxCalloc(N*m1,sizeof(double));
A=mxCalloc(N*mn,sizeof(double));
temp=mxCalloc(mm,sizeof(double));
transpose(mxGetPr(prhs[0]),A,N,mn);
Vptr=V;
Aptr=A;
for(i=0; i<N; i++, Vptr+=m1, Aptr+=mn)
{
outprod(Aptr,temp,m,n);
for (j=0; j<mm; j+=(m+1)) temp[j]/=2;
vech(temp,Vptr,m,false);
}
mxFree(temp);
mxFree(A);
plhs[0]=mxCreateDoubleMatrix(N,m1,mxREAL);
transpose(V,mxGetPr(plhs[0]),m1,N);
mxFree(V);
}
Matrix<double,10,1> Vectorize(const RigidBodyInertia & rbd_inertia)
{
Matrix<double,10,1> ret;
Vector MCOG = rbd_inertia.getMass()*rbd_inertia.getCOG();
//In RigidBodyInertia, the rotational inertia is stored with
//respect to the link frame of refence, not the COG one
ret << rbd_inertia.getMass(),
Map<Vector3d>(MCOG.data),
vech(rbd_inertia.getRotationalInertia());
return ret;
}
