void d_gemm(SEXP rtransa, SEXP rtransb, SEXP ralpha, SEXP ra, SEXP rlda,
SEXP rb, SEXP rldb, SEXP rbeta, SEXP rc, SEXP rldc)
{
char
transa = getTranspose(rtransa),
transb = getTranspose(rtransb);
double
alpha = asReal(ralpha), beta = asReal(rbeta),
* a, * b, * c;
int
m, n, k,
rowsa, colsa, lda = asInteger(rlda),
rowsb, colsb, ldb = asInteger(rldb),
rowsc, colsc, ldc = asInteger(rldc);
unpackMatrix(ra, &rowsa, &colsa, &a);
unpackMatrix(rb, &rowsb, &colsb, &b);
unpackMatrix(rc, &rowsc, &colsc, &c);
m = rowsa;
n = colsb;
k = colsa;
if(isTranspose(transa)) {
m = colsa;
k = rowsa;
}
if(isTranspose(transb))
n = rowsb;
cublasDgemm(transa, transb, m, n, k, alpha, a, lda, b, ldb, beta, c, ldc);
checkCublasError("d_gemm");
}
gMatrixSparse gMatrixSparse::operator*(gMatrixSparse &right)
{
assert(numCols == right.numRows);
vector<elm> vect;
gMatrixSparse trans = getTranspose();
bool nempty[numRows];
std::map<int, gMatrixSparse::gColumnSparse> rowMap;
for (int i = 0; i < numRows; i++) {
nempty[i] = !trans.colIsEmpty(i);
if (nempty[i]) rowMap[i] = trans.getColumn(i);
}
for (int c = 0; c < right.numCols; c++) {
if (right.colIsEmpty(c)) continue;
gColumnSparse col = right.getColumn(c);
for (int r = 0; r < numRows; r++) {
if (nempty[r]) {
double val = col.dot(rowMap[r]);
vect.push_back(elm(r, c, val));
}
}
}
return gMatrixSparse(numRows, right.numCols, vect, true);
}
void d_syr2k(SEXP ruplo, SEXP rtrans, SEXP ralpha, SEXP ra, SEXP rlda,
SEXP rb, SEXP rldb, SEXP rbeta, SEXP rc, SEXP rldc)
{
char
trans = getTranspose(rtrans),
uplo = getSymLoc(ruplo);
double
alpha = asReal(ralpha), beta = asReal(rbeta),
* a, * b, * c;
int
k,
rowsa, colsa, lda = asInteger(rlda),
rowsb, colsb, ldb = asInteger(rldb),
rowsc, colsc, ldc = asInteger(rldc);
k = rowsa;
if((trans == 'N') || (trans == 'n')) {
k = colsa;
}
unpackMatrix(ra, &rowsa, &colsa, &a);
unpackMatrix(rb, &rowsb, &colsb, &b);
unpackMatrix(rc, &rowsc, &colsc, &c);
cublasDsyr2k(uplo, trans, rowsc, k, alpha, a, lda, b, ldb, beta, c, ldc);
checkCublasError("d_syr2k");
}
gMatrixSparse gMatrixSparse::getNearSymmetric()
{
gMatrixSparse trans = getTranspose();
struct miniMapper : public dualMapper {
vector<elm> vect;
virtual void map(int row, int col, double valLeft, double valRight) {
vect.push_back(elm(row, col, (valLeft + valRight) * .5));
}
} mapper;
gMatrixSparse::dualMap(*this, trans, mapper);
return gMatrixSparse(numRows, numCols, mapper.vect, true);
}
void d_tpsv(SEXP ruplo, SEXP rtrans, SEXP rdiag, SEXP ra, SEXP rx, SEXP rincx)
{
char
uplo = getSymLoc(ruplo),
trans = getTranspose(rtrans),
diag = getUnitTri(rdiag);
double
* a, * x;
int
rowsa, colsa,
nx, incx = asInteger(rincx);
unpackVector(rx, &nx, &x);
unpackMatrix(ra, &rowsa, &colsa, &a);
cublasDtpsv(uplo, trans, diag, rowsa, a, x, incx);
checkCublasError("d_tpsv");
}
void d_trsm(SEXP rside, SEXP ruplo, SEXP rtrans, SEXP rdiag,
SEXP ralpha, SEXP ra, SEXP rlda, SEXP rb, SEXP rldb)
{
char
trans = getTranspose(rtrans),
diag = getUnitTri(rdiag),
side = getSide(rside),
uplo = getSymLoc(ruplo);
double
alpha = asReal(ralpha),
* a, * b;
int
rowsa, colsa, lda = asInteger(rlda),
rowsb, colsb, ldb = asInteger(rldb);
unpackMatrix(ra, &rowsa, &colsa, &a);
unpackMatrix(rb, &rowsb, &colsb, &b);
cublasDtrsm(side, uplo, trans, diag, rowsb, colsb, alpha, a, lda, b, ldb);
checkCublasError("d_trsm");
}
void d_gemv(SEXP rtrans, SEXP ralpha, SEXP ra, SEXP rlda, SEXP rx, SEXP rincx,
SEXP rbeta, SEXP ry, SEXP rincy)
{
char
trans = getTranspose(rtrans);
double
alpha = asReal(ralpha), beta = asReal(rbeta),
* a, * x, * y;
int
nx, ny, rowsa, colsa,
lda = asInteger(rlda),
incx = asInteger(rincx),
incy = asInteger(rincy);
unpackVector(rx, &nx, &x);
unpackVector(ry, &ny, &y);
unpackMatrix(ra, &rowsa, &colsa, &a);
cublasDgemv(trans, rowsa, colsa, alpha, a, lda, x, incx, beta, y, incy);
checkCublasError("d_gemv");
}
// Fakes a midi piano keyboard using the PC keyboard
bool CSong::pcKeyPress(int key, bool down)
{
int i;
size_t j;
CMidiEvent midi;
const int cfg_pcKeyVolume = 64;
const int cfg_pcKeyChannel = 1-1;
if (key == 't') // the tab key on the PC fakes good notes
{
if (down)
m_fakeChord = getWantedChord();
for (i = 0; i < m_fakeChord.length(); i++)
{
if (down)
midi.noteOnEvent(0, cfg_pcKeyChannel, m_fakeChord.getNote(i).pitch() + getTranspose(), cfg_pcKeyVolume);
else
midi.noteOffEvent(0, cfg_pcKeyChannel, m_fakeChord.getNote(i).pitch() + getTranspose(), cfg_pcKeyVolume);
expandPianistInput(midi);
}
return true;
}
for (j = 0; j < arraySize(pcNoteLookup); j++)
{
if ( key==pcNoteLookup[j].key)
{
if (down)
midi.noteOnEvent(0, cfg_pcKeyChannel, pcNoteLookup[j].note, cfg_pcKeyVolume);
else
midi.noteOffEvent(0, cfg_pcKeyChannel, pcNoteLookup[j].note, cfg_pcKeyVolume);
expandPianistInput(midi);
return true;
}
}
//printf("pcKeyPress %d %d\n", m_pcNote, key);
return false;
}
Matrix4& Matrix4::transpose() {
return (*this = getTranspose());
}
Matrix3& Matrix3::transpose() {
return (*this = getTranspose());
}
bool gMatrixSparse::isSymmetric()
{
if (numRows != numCols) return false;
return (getTranspose() - *this).nnz == 0;
}
void Matrix3x3::transpose()
{
(*this) = getTranspose();
}
void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *Prhs[])
{
register int i;
register double *pdbl;
mxArray **prhs=(mxArray **)&Prhs[0], *At, *Ct;
struct mexdata mdata;
int len, status;
double *p, *p0, *ret, *x;
int m, n, havejac, Arows, Crows, itmax, nopts, mintype, nextra;
double opts[LM_OPTS_SZ]={LM_INIT_MU, LM_STOP_THRESH, LM_STOP_THRESH, LM_STOP_THRESH, LM_DIFF_DELTA};
double info[LM_INFO_SZ];
double *lb=NULL, *ub=NULL, *A=NULL, *b=NULL, *wghts=NULL, *C=NULL, *d=NULL, *covar=NULL;
/* parse input args; start by checking their number */
if((nrhs<5))
matlabFmtdErrMsgTxt("levmar: at least 5 input arguments required (got %d).", nrhs);
if(nlhs>4)
matlabFmtdErrMsgTxt("levmar: too many output arguments (max. 4, got %d).", nlhs);
else if(nlhs<2)
matlabFmtdErrMsgTxt("levmar: too few output arguments (min. 2, got %d).", nlhs);
/* note that in order to accommodate optional args, prhs & nrhs are adjusted accordingly below */
/** func **/
/* first argument must be a string , i.e. a char row vector */
if(mxIsChar(prhs[0])!=1)
mexErrMsgTxt("levmar: first argument must be a string.");
if(mxGetM(prhs[0])!=1)
mexErrMsgTxt("levmar: first argument must be a string (i.e. char row vector).");
/* store supplied name */
len=mxGetN(prhs[0])+1;
mdata.fname=mxCalloc(len, sizeof(char));
status=mxGetString(prhs[0], mdata.fname, len);
if(status!=0)
mexErrMsgTxt("levmar: not enough space. String is truncated.");
/** jac (optional) **/
/* check whether second argument is a string */
if(mxIsChar(prhs[1])==1){
if(mxGetM(prhs[1])!=1)
mexErrMsgTxt("levmar: second argument must be a string (i.e. row vector).");
/* store supplied name */
len=mxGetN(prhs[1])+1;
mdata.jacname=mxCalloc(len, sizeof(char));
status=mxGetString(prhs[1], mdata.jacname, len);
if(status!=0)
mexErrMsgTxt("levmar: not enough space. String is truncated.");
havejac=1;
++prhs;
--nrhs;
}
else{
mdata.jacname=NULL;
havejac=0;
}
#ifdef DEBUG
fflush(stderr);
fprintf(stderr, "LEVMAR: %s analytic Jacobian\n", havejac? "with" : "no");
#endif /* DEBUG */
/* CHECK
if(!mxIsDouble(prhs[1]) || mxIsComplex(prhs[1]) || !(mxGetM(prhs[1])==1 && mxGetN(prhs[1])==1))
*/
/** p0 **/
/* the second required argument must be a real row or column vector */
if(!mxIsDouble(prhs[1]) || mxIsComplex(prhs[1]) || !(mxGetM(prhs[1])==1 || mxGetN(prhs[1])==1))
mexErrMsgTxt("levmar: p0 must be a real vector.");
p0=mxGetPr(prhs[1]);
/* determine if we have a row or column vector and retrieve its
* size, i.e. the number of parameters
*/
if(mxGetM(prhs[1])==1){
m=mxGetN(prhs[1]);
mdata.isrow_p0=1;
}
else{
m=mxGetM(prhs[1]);
mdata.isrow_p0=0;
}
/* copy input parameter vector to avoid destroying it */
p=mxMalloc(m*sizeof(double));
for(i=0; i<m; ++i)
p[i]=p0[i];
/** x **/
/* the third required argument must be a real row or column vector */
if(!mxIsDouble(prhs[2]) || mxIsComplex(prhs[2]) || !(mxGetM(prhs[2])==1 || mxGetN(prhs[2])==1))
mexErrMsgTxt("levmar: x must be a real vector.");
x=mxGetPr(prhs[2]);
n=__MAX__(mxGetM(prhs[2]), mxGetN(prhs[2]));
/** itmax **/
/* the fourth required argument must be a scalar */
if(!mxIsDouble(prhs[3]) || mxIsComplex(prhs[3]) || mxGetM(prhs[3])!=1 || mxGetN(prhs[3])!=1)
mexErrMsgTxt("levmar: itmax must be a scalar.");
itmax=(int)mxGetScalar(prhs[3]);
/** opts **/
/* the fifth required argument must be a real row or column vector */
if(!mxIsDouble(prhs[4]) || mxIsComplex(prhs[4]) || (!(mxGetM(prhs[4])==1 || mxGetN(prhs[4])==1) &&
!(mxGetM(prhs[4])==0 && mxGetN(prhs[4])==0)))
mexErrMsgTxt("levmar: opts must be a real vector.");
pdbl=mxGetPr(prhs[4]);
nopts=__MAX__(mxGetM(prhs[4]), mxGetN(prhs[4]));
if(nopts!=0){ /* if opts==[], nothing needs to be done and the defaults are used */
if(nopts>LM_OPTS_SZ)
matlabFmtdErrMsgTxt("levmar: opts must have at most %d elements, got %d.", LM_OPTS_SZ, nopts);
else if(nopts<((havejac)? LM_OPTS_SZ-1 : LM_OPTS_SZ))
matlabFmtdWarnMsgTxt("levmar: only the %d first elements of opts specified, remaining set to defaults.", nopts);
for(i=0; i<nopts; ++i)
opts[i]=pdbl[i];
}
#ifdef DEBUG
else{
fflush(stderr);
fprintf(stderr, "LEVMAR: empty options vector, using defaults\n");
}
#endif /* DEBUG */
/** mintype (optional) **/
/* check whether sixth argument is a string */
if(nrhs>=6 && mxIsChar(prhs[5])==1 && mxGetM(prhs[5])==1){
char *minhowto;
/* examine supplied name */
len=mxGetN(prhs[5])+1;
minhowto=mxCalloc(len, sizeof(char));
status=mxGetString(prhs[5], minhowto, len);
if(status!=0)
mexErrMsgTxt("levmar: not enough space. String is truncated.");
for(i=0; minhowto[i]; ++i)
minhowto[i]=tolower(minhowto[i]);
if(!strncmp(minhowto, "unc", 3)) mintype=MIN_UNCONSTRAINED;
else if(!strncmp(minhowto, "bc", 2)) mintype=MIN_CONSTRAINED_BC;
else if(!strncmp(minhowto, "lec", 3)) mintype=MIN_CONSTRAINED_LEC;
else if(!strncmp(minhowto, "blec", 4)) mintype=MIN_CONSTRAINED_BLEC;
else if(!strncmp(minhowto, "bleic", 5)) mintype=MIN_CONSTRAINED_BLEIC;
else if(!strncmp(minhowto, "blic", 4)) mintype=MIN_CONSTRAINED_BLIC;
else if(!strncmp(minhowto, "leic", 4)) mintype=MIN_CONSTRAINED_LEIC;
else if(!strncmp(minhowto, "lic", 3)) mintype=MIN_CONSTRAINED_BLIC;
else matlabFmtdErrMsgTxt("levmar: unknown minimization type '%s'.", minhowto);
mxFree(minhowto);
++prhs;
--nrhs;
}
else
mintype=MIN_UNCONSTRAINED;
if(mintype==MIN_UNCONSTRAINED) goto extraargs;
/* arguments below this point are optional and their presence depends
* upon the minimization type determined above
*/
/** lb, ub **/
if(nrhs>=7 && (mintype==MIN_CONSTRAINED_BC || mintype==MIN_CONSTRAINED_BLEC || mintype==MIN_CONSTRAINED_BLIC || mintype==MIN_CONSTRAINED_BLEIC)){
/* check if the next two arguments are real row or column vectors */
if(mxIsDouble(prhs[5]) && !mxIsComplex(prhs[5]) && (mxGetM(prhs[5])==1 || mxGetN(prhs[5])==1)){
if(mxIsDouble(prhs[6]) && !mxIsComplex(prhs[6]) && (mxGetM(prhs[6])==1 || mxGetN(prhs[6])==1)){
if((i=__MAX__(mxGetM(prhs[5]), mxGetN(prhs[5])))!=m)
matlabFmtdErrMsgTxt("levmar: lb must have %d elements, got %d.", m, i);
if((i=__MAX__(mxGetM(prhs[6]), mxGetN(prhs[6])))!=m)
matlabFmtdErrMsgTxt("levmar: ub must have %d elements, got %d.", m, i);
lb=mxGetPr(prhs[5]);
ub=mxGetPr(prhs[6]);
prhs+=2;
nrhs-=2;
}
}
}
/** A, b **/
if(nrhs>=7 && (mintype==MIN_CONSTRAINED_LEC || mintype==MIN_CONSTRAINED_BLEC || mintype==MIN_CONSTRAINED_LEIC || mintype==MIN_CONSTRAINED_BLEIC)){
/* check if the next two arguments are a real matrix and a real row or column vector */
if(mxIsDouble(prhs[5]) && !mxIsComplex(prhs[5]) && mxGetM(prhs[5])>=1 && mxGetN(prhs[5])>=1){
if(mxIsDouble(prhs[6]) && !mxIsComplex(prhs[6]) && (mxGetM(prhs[6])==1 || mxGetN(prhs[6])==1)){
if((i=mxGetN(prhs[5]))!=m)
matlabFmtdErrMsgTxt("levmar: A must have %d columns, got %d.", m, i);
if((i=__MAX__(mxGetM(prhs[6]), mxGetN(prhs[6])))!=(Arows=mxGetM(prhs[5])))
matlabFmtdErrMsgTxt("levmar: b must have %d elements, got %d.", Arows, i);
At=prhs[5];
b=mxGetPr(prhs[6]);
A=getTranspose(At);
prhs+=2;
nrhs-=2;
}
}
}
/* wghts */
/* check if we have a weights vector */
if(nrhs>=6 && mintype==MIN_CONSTRAINED_BLEC){ /* only check if we have seen both box & linear constraints */
if(mxIsDouble(prhs[5]) && !mxIsComplex(prhs[5]) && (mxGetM(prhs[5])==1 || mxGetN(prhs[5])==1)){
if(__MAX__(mxGetM(prhs[5]), mxGetN(prhs[5]))==m){
wghts=mxGetPr(prhs[5]);
++prhs;
--nrhs;
}
}
}
/** C, d **/
if(nrhs>=7 && (mintype==MIN_CONSTRAINED_BLEIC || mintype==MIN_CONSTRAINED_BLIC || mintype==MIN_CONSTRAINED_LEIC || mintype==MIN_CONSTRAINED_LIC)){
/* check if the next two arguments are a real matrix and a real row or column vector */
if(mxIsDouble(prhs[5]) && !mxIsComplex(prhs[5]) && mxGetM(prhs[5])>=1 && mxGetN(prhs[5])>=1){
if(mxIsDouble(prhs[6]) && !mxIsComplex(prhs[6]) && (mxGetM(prhs[6])==1 || mxGetN(prhs[6])==1)){
if((i=mxGetN(prhs[5]))!=m)
matlabFmtdErrMsgTxt("levmar: C must have %d columns, got %d.", m, i);
if((i=__MAX__(mxGetM(prhs[6]), mxGetN(prhs[6])))!=(Crows=mxGetM(prhs[5])))
matlabFmtdErrMsgTxt("levmar: d must have %d elements, got %d.", Crows, i);
Ct=prhs[5];
d=mxGetPr(prhs[6]);
C=getTranspose(Ct);
prhs+=2;
nrhs-=2;
}
}
}
/* arguments below this point are assumed to be extra arguments passed
* to every invocation of the fitting function and its Jacobian
*/
extraargs:
/* handle any extra args and allocate memory for
* passing the current parameter estimate to matlab
*/
nextra=nrhs-5;
mdata.nrhs=nextra+1;
mdata.rhs=(mxArray **)mxMalloc(mdata.nrhs*sizeof(mxArray *));
for(i=0; i<nextra; ++i)
mdata.rhs[i+1]=(mxArray *)prhs[nrhs-nextra+i]; /* discard 'const' modifier */
#ifdef DEBUG
fflush(stderr);
fprintf(stderr, "LEVMAR: %d extra args\n", nextra);
#endif /* DEBUG */
if(mdata.isrow_p0){ /* row vector */
mdata.rhs[0]=mxCreateDoubleMatrix(1, m, mxREAL);
/*
mxSetM(mdata.rhs[0], 1);
mxSetN(mdata.rhs[0], m);
*/
}
else{ /* column vector */
mdata.rhs[0]=mxCreateDoubleMatrix(m, 1, mxREAL);
/*
mxSetM(mdata.rhs[0], m);
mxSetN(mdata.rhs[0], 1);
*/
}
/* ensure that the supplied function & Jacobian are as expected */
if(checkFuncAndJacobian(p, m, n, havejac, &mdata)){
status=LM_ERROR;
goto cleanup;
}
if(nlhs>3) /* covariance output required */
covar=mxMalloc(m*m*sizeof(double));
/* invoke levmar */
switch(mintype){
case MIN_UNCONSTRAINED: /* no constraints */
if(havejac)
status=dlevmar_der(func, jacfunc, p, x, m, n, itmax, opts, info, NULL, covar, (void *)&mdata);
else
status=dlevmar_dif(func, p, x, m, n, itmax, opts, info, NULL, covar, (void *)&mdata);
#ifdef DEBUG
fflush(stderr);
fprintf(stderr, "LEVMAR: calling dlevmar_der()/dlevmar_dif()\n");
#endif /* DEBUG */
break;
case MIN_CONSTRAINED_BC: /* box constraints */
if(havejac)
status=dlevmar_bc_der(func, jacfunc, p, x, m, n, lb, ub, itmax, opts, info, NULL, covar, (void *)&mdata);
else
status=dlevmar_bc_dif(func, p, x, m, n, lb, ub, itmax, opts, info, NULL, covar, (void *)&mdata);
#ifdef DEBUG
fflush(stderr);
fprintf(stderr, "LEVMAR: calling dlevmar_bc_der()/dlevmar_bc_dif()\n");
#endif /* DEBUG */
break;
case MIN_CONSTRAINED_LEC: /* linear equation constraints */
#ifdef HAVE_LAPACK
if(havejac)
status=dlevmar_lec_der(func, jacfunc, p, x, m, n, A, b, Arows, itmax, opts, info, NULL, covar, (void *)&mdata);
else
status=dlevmar_lec_dif(func, p, x, m, n, A, b, Arows, itmax, opts, info, NULL, covar, (void *)&mdata);
#else
mexErrMsgTxt("levmar: no linear constraints support, HAVE_LAPACK was not defined during MEX-file compilation.");
#endif /* HAVE_LAPACK */
#ifdef DEBUG
fflush(stderr);
fprintf(stderr, "LEVMAR: calling dlevmar_lec_der()/dlevmar_lec_dif()\n");
#endif /* DEBUG */
break;
case MIN_CONSTRAINED_BLEC: /* box & linear equation constraints */
#ifdef HAVE_LAPACK
if(havejac)
status=dlevmar_blec_der(func, jacfunc, p, x, m, n, lb, ub, A, b, Arows, wghts, itmax, opts, info, NULL, covar, (void *)&mdata);
else
status=dlevmar_blec_dif(func, p, x, m, n, lb, ub, A, b, Arows, wghts, itmax, opts, info, NULL, covar, (void *)&mdata);
#else
mexErrMsgTxt("levmar: no box & linear constraints support, HAVE_LAPACK was not defined during MEX-file compilation.");
#endif /* HAVE_LAPACK */
#ifdef DEBUG
fflush(stderr);
fprintf(stderr, "LEVMAR: calling dlevmar_blec_der()/dlevmar_blec_dif()\n");
#endif /* DEBUG */
break;
case MIN_CONSTRAINED_BLEIC: /* box, linear equation & inequalities constraints */
#ifdef HAVE_LAPACK
if(havejac)
status=dlevmar_bleic_der(func, jacfunc, p, x, m, n, lb, ub, A, b, Arows, C, d, Crows, itmax, opts, info, NULL, covar, (void *)&mdata);
else
status=dlevmar_bleic_dif(func, p, x, m, n, lb, ub, A, b, Arows, C, d, Crows, itmax, opts, info, NULL, covar, (void *)&mdata);
#else
mexErrMsgTxt("levmar: no box, linear equation & inequality constraints support, HAVE_LAPACK was not defined during MEX-file compilation.");
#endif /* HAVE_LAPACK */
#ifdef DEBUG
fflush(stderr);
fprintf(stderr, "LEVMAR: calling dlevmar_bleic_der()/dlevmar_bleic_dif()\n");
#endif /* DEBUG */
break;
case MIN_CONSTRAINED_BLIC: /* box, linear inequalities constraints */
#ifdef HAVE_LAPACK
if(havejac)
status=dlevmar_bleic_der(func, jacfunc, p, x, m, n, lb, ub, NULL, NULL, 0, C, d, Crows, itmax, opts, info, NULL, covar, (void *)&mdata);
else
status=dlevmar_bleic_dif(func, p, x, m, n, lb, ub, NULL, NULL, 0, C, d, Crows, itmax, opts, info, NULL, covar, (void *)&mdata);
#else
mexErrMsgTxt("levmar: no box & linear inequality constraints support, HAVE_LAPACK was not defined during MEX-file compilation.");
#endif /* HAVE_LAPACK */
#ifdef DEBUG
fflush(stderr);
fprintf(stderr, "LEVMAR: calling dlevmar_blic_der()/dlevmar_blic_dif()\n");
#endif /* DEBUG */
break;
case MIN_CONSTRAINED_LEIC: /* linear equation & inequalities constraints */
#ifdef HAVE_LAPACK
if(havejac)
status=dlevmar_bleic_der(func, jacfunc, p, x, m, n, NULL, NULL, A, b, Arows, C, d, Crows, itmax, opts, info, NULL, covar, (void *)&mdata);
else
status=dlevmar_bleic_dif(func, p, x, m, n, NULL, NULL, A, b, Arows, C, d, Crows, itmax, opts, info, NULL, covar, (void *)&mdata);
#else
mexErrMsgTxt("levmar: no linear equation & inequality constraints support, HAVE_LAPACK was not defined during MEX-file compilation.");
#endif /* HAVE_LAPACK */
#ifdef DEBUG
fflush(stderr);
fprintf(stderr, "LEVMAR: calling dlevmar_leic_der()/dlevmar_leic_dif()\n");
#endif /* DEBUG */
break;
case MIN_CONSTRAINED_LIC: /* linear inequalities constraints */
#ifdef HAVE_LAPACK
if(havejac)
status=dlevmar_bleic_der(func, jacfunc, p, x, m, n, NULL, NULL, NULL, NULL, 0, C, d, Crows, itmax, opts, info, NULL, covar, (void *)&mdata);
else
status=dlevmar_bleic_dif(func, p, x, m, n, NULL, NULL, NULL, NULL, 0, C, d, Crows, itmax, opts, info, NULL, covar, (void *)&mdata);
#else
mexErrMsgTxt("levmar: no linear equation & inequality constraints support, HAVE_LAPACK was not defined during MEX-file compilation.");
#endif /* HAVE_LAPACK */
#ifdef DEBUG
fflush(stderr);
fprintf(stderr, "LEVMAR: calling dlevmar_lic_der()/dlevmar_lic_dif()\n");
#endif /* DEBUG */
break;
default:
mexErrMsgTxt("levmar: unexpected internal error.");
}
#ifdef DEBUG
fflush(stderr);
printf("LEVMAR: minimization returned %d in %g iter, reason %g\n\tSolution: ", status, info[5], info[6]);
for(i=0; i<m; ++i)
printf("%.7g ", p[i]);
printf("\n\n\tMinimization info:\n\t");
for(i=0; i<LM_INFO_SZ; ++i)
printf("%g ", info[i]);
printf("\n");
#endif /* DEBUG */
/* copy back return results */
/** ret **/
plhs[0]=mxCreateDoubleMatrix(1, 1, mxREAL);
ret=mxGetPr(plhs[0]);
ret[0]=(double)status;
/** popt **/
plhs[1]=(mdata.isrow_p0==1)? mxCreateDoubleMatrix(1, m, mxREAL) : mxCreateDoubleMatrix(m, 1, mxREAL);
pdbl=mxGetPr(plhs[1]);
for(i=0; i<m; ++i)
pdbl[i]=p[i];
/** info **/
if(nlhs>2){
plhs[2]=mxCreateDoubleMatrix(1, LM_INFO_SZ, mxREAL);
pdbl=mxGetPr(plhs[2]);
for(i=0; i<LM_INFO_SZ; ++i)
pdbl[i]=info[i];
}
/** covar **/
if(nlhs>3){
plhs[3]=mxCreateDoubleMatrix(m, m, mxREAL);
pdbl=mxGetPr(plhs[3]);
for(i=0; i<m*m; ++i) /* covariance matrices are symmetric, thus no need to transpose! */
pdbl[i]=covar[i];
}
cleanup:
/* cleanup */
mxDestroyArray(mdata.rhs[0]);
if(A) mxFree(A);
if(C) mxFree(C);
mxFree(mdata.fname);
if(havejac) mxFree(mdata.jacname);
mxFree(p);
mxFree(mdata.rhs);
if(covar) mxFree(covar);
if(status==LM_ERROR)
mexWarnMsgTxt("levmar: optimization returned with an error!");
}
//------------------------------------------------------------------------------
// getTriDiagonal
//------------------------------------------------------------------------------
bool Matrix::getTriDiagonal(Matrix* const pA) const
{
//-------------------------------------------------------
// initial compatibility and error checks
//-------------------------------------------------------
//if (!isSymmetric()) return 0;
const int N = getRows();
const auto pAI = new Matrix(*this);
for (int k=0; k<N-2; k++) {
double gama = (*pAI)(k+1,k);
double alfa = (gama * gama);
for (int j=k+2; j<N; j++) {
double zeta = (*pAI)(j,k);
alfa += (zeta * zeta);
}
alfa = -sign(gama) * std::sqrt(alfa);
double beta = std::sqrt(0.5 * alfa * (alfa - gama));
//----------------------------------------------------
// construct column vector X
//----------------------------------------------------
const auto pX = new CVector(N);
for (int p=0; p<k+1; p++) {
(*pX)[p] = 0.0;
}
(*pX)[k+1] = (gama - alfa) / beta / 2.0;
for (int q=k+2; q<N; q++) {
(*pX)[q] = (*pAI)(q,k) / beta / 2.0;
}
//----------------------------------------------------
// construct row vector Y = X'
//----------------------------------------------------
RVector* pY = pX->getTranspose();
//----------------------------------------------------
// M = 2*X*Y = 2*X*X'
//----------------------------------------------------
Matrix* pM = outerProduct(*pX, *pY);
pM->multiply(2.0);
//----------------------------------------------------
// H = I - M = I - 2*X*X'
//----------------------------------------------------
const auto pH = new Matrix(N,N);
pH->makeIdent();
pH->subtract(*pM);
//----------------------------------------------------
// A = H*A*H
//----------------------------------------------------
Matrix* pAI0 = base::multiply(*pH, *pAI);
Matrix* pAI1 = base::multiply(*pAI0, *pH);
*pAI = *pAI1;
//----------------------------------------------------
// unref intermediate loop pointers
//----------------------------------------------------
pX->unref();
pY->unref();
pM->unref();
pH->unref();
pAI0->unref();
pAI1->unref();
}
//-------------------------------------------------------
// A = H(N-3)*...*H1*H0* A *H0*H1*...*H(N-3)
//-------------------------------------------------------
*pA = *pAI;
//----------------------------------------------------
// unref pointers
//----------------------------------------------------
pAI->unref();
return true;
}
//------------------------------------------------------------------------------
// getQR
// Returns pre-ref'd pointers to the lower QR (pQ) and upper QR (pR) matrices
// of 'this' matrix, or zero if the matrix can not be QR-ized
//------------------------------------------------------------------------------
bool Matrix::getQR(Matrix* const pQ, Matrix* const pR) const
{
//-------------------------------------------------------
// initial compatibility and error checks
//-------------------------------------------------------
bool b1 = isGoodMatrix();
bool b2 = isSquare();
if (!b1 || !b2) return false;
//-------------------------------------------------------
// Initialize intermediate R matrix to 'this' matrix
//-------------------------------------------------------
const auto pRI = new Matrix(*this);
//-------------------------------------------------------
// Initialize intermediate Q matrix to 'identity' matrix
//-------------------------------------------------------
const int N = getRows();
const auto pQI = new Matrix(N,N);
pQI->makeIdent();
//-------------------------------------------------------
// X and V are intermediate vectors
//-------------------------------------------------------
const auto pX = new CVector(N);
const auto pV = new CVector(N);
//-------------------------------------------------------
// Begin loop
//-------------------------------------------------------
for (int k = 0; k < N-1; k++) {
pX->fillWith(0.0);
for (int i = k; i<N ; i++) {
(*pX)[i] = (*pRI)(i,k);
}
double g = pX->getNorm();
(*pV) = (*pX);
(*pV)[k] += g;
double s = pV->getNorm();
if (s == 0.0) {
pQI->unref();
pRI->unref();
pX->unref();
pV->unref();
return false;
}
CVector* pW = base::multiply((*pV), 1.0/s);
RVector* pWT = pW->getTranspose();
{
//----------------------------------------------------
// U' = (2*R'*W)'
//----------------------------------------------------
Matrix* pRIT = pRI->getTranspose();
CVector* pU0 = base::multiply((*pRIT), (*pW));
CVector* pU = base::multiply((*pU0), 2.0);
RVector* pUT = pU->getTranspose();
pU0->unref();
pU->unref();
pRIT->unref();
//----------------------------------------------------
// R = R - W*U'
//----------------------------------------------------
Matrix* pM1 = outerProduct(*pW, *pUT);
pRI->subtract(*pM1);
pM1->unref();
pUT->unref();
}
//----------------------------------------------------
// Q = Q - 2*Q*W*W'
//----------------------------------------------------
{
Matrix* pM2 = outerProduct(*pW, *pWT);
Matrix* pM3 = base::multiply(*pQI, *pM2);
Matrix* pM4 = base::multiply(*pM3, 2.0);
pQI->subtract(*pM4);
pM2->unref();
pM3->unref();
pM4->unref();
}
//-------------------------------------------------------
// Unref pointers
//-------------------------------------------------------
pW->unref();
pWT->unref();
}
//-------------------------------------------------------
// Assign results to argument list variables for output
//-------------------------------------------------------
*pQ = *pQI;
*pR = *pRI;
//-------------------------------------------------------
// Unref pointers
//-------------------------------------------------------
pQI->unref();
pRI->unref();
pX->unref();
pV->unref();
return true;
}
//------------------------------------------------------------------------------
// Dominant Eigenvalue Power Method
//------------------------------------------------------------------------------
bool Matrix::getEigenPower(const double maxErr, const int maxIter,
double* const pEigenVal, CVector* const pEigenVec)
{
//-------------------------------------------------------
// initial compatibility and error checks
//-------------------------------------------------------
if (!isGoodMatrix() || !isSquare()) return false;
//-------------------------------------------------------
// initialize variables
//-------------------------------------------------------
int Iter = 0; // iterator initialized to zero
double Err = 10.0*maxErr; // make Err > maxErr on entry
const auto pA = new Matrix(*this); // A is a buffer matrix for 'this' matrix
const int N = pA->getRows(); // pA->getCols works too since A is square
double alfa = 0.0; // current eigenvalue estimate
const auto pZ = new CVector(N); // current eigenvector estimate
pZ->fillWith(1.0); // all 1's in initial estimate
//-------------------------------------------------------
// iterate solutions to desired accuracy or iteration limit
//-------------------------------------------------------
while ((Err > maxErr) && (Iter < maxIter))
{
{
//----------------------------------------------------
// get estimate of eigenvector
//----------------------------------------------------
CVector* pW = base::multiply(*pA, *pZ); // get new estimate (W) based on old estimate (Z)
const double Wmag = pW->getMaxMag(); // max mag value from elements of vector W
if (Wmag == 0.0) {
// mag value is zero; cleanup and leave
pW->unref();
pZ->unref();
pA->unref();
return false;
}
pW->multiply(1.0/Wmag); // normalize eigenvector with max element of W
//----------------------------------------------------
// get estimate of eigenvalue
//----------------------------------------------------
alfa = Wmag; // save Wmag eigenvalue estimate
//----------------------------------------------------
// refine eigenvalue estimate by using Rayleigh quotient
// (may or may not be worth the additional calculations)
//----------------------------------------------------
#if 0
{
RVector* pZT = pZ->getTranspose();
double num = base::dotProduct(*pZT, *pW);
double den = base::dotProduct(*pZT, *pZ);
alfa *= (num / den); // save Rayleigh eigenvalue estimate
pZT->unref();
}
#endif
//----------------------------------------------------
// save eigenvector W estimate in vector Z to begin next loop
//----------------------------------------------------
*pZ = *pW; // save eigenvector estimate for next iteration
pW->unref();
}
//----------------------------------------------------
// calculate error of estimate: Err = ||A*Z - alfa*Z||
//----------------------------------------------------
{
CVector* pE = base::multiply(*pA, *pZ);
CVector* pW1 = base::multiply(*pZ, alfa); // pW1 used here as intermediate vector
pE->subtract(*pW1);
Err = pE->getNorm(); // magnitude of error vector
pE->unref();
pW1->unref();
}
//----------------------------------------------------
// increment iterator for next loop
//----------------------------------------------------
Iter++;
}
//-------------------------------------------------------
// copy eigenvalue and eigenvector results to output
//-------------------------------------------------------
*pEigenVal = alfa;
*pEigenVec = *pZ;
//-------------------------------------------------------
// unref pointers
//-------------------------------------------------------
pZ->unref();
pA->unref();
return true;
}
