static double MatlabCallback(int n, const double *x, int *undefined_flag, void *data) {
mxArray *rhs[2];
mxArray *lhs[2];
double *oldPtr;
double fVal;
bool violatesConstraints;
//feval in Matlab will take the function handle and the state as
//inputs.
//The first function handle is f.
rhs[0]=(mxArray*)data;
rhs[1]=mxCreateNumericMatrix(0, 0, mxDOUBLE_CLASS, mxREAL);
//Set the matrix data to x
oldPtr=mxGetPr(rhs[1]);
//x will not be modified, but the const must be typecast away to use
//the mxSetPr function.
mxSetPr(rhs[1],(double*)x);
mxSetM(rhs[1], (size_t)n);
mxSetN(rhs[1], 1);
//Get the function value and gradient.
mexCallMATLAB(2,lhs,2,rhs,"feval");
//Get the function value.
fVal=getDoubleFromMatlab(lhs[0]);
violatesConstraints=getBoolFromMatlab(lhs[1]);
if(violatesConstraints) {
*undefined_flag=1;
}
//Get rid of the returned Matlab Matrices.
mxDestroyArray(lhs[0]);
mxDestroyArray(lhs[1]);
//Set the data pointer back to what it was during allocation that
//mxDestroyArray does not have a problem.
mxSetPr(rhs[1],oldPtr);
mxSetM(rhs[1], 0);
mxSetN(rhs[1], 0);
//Get rid of the temporary natrix.
mxDestroyArray(rhs[1]);
return fVal;
}
void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[]) {
double gain, *C, CDelta;
size_t numRow, numCol, i;
ptrdiff_t *col4row, *row4col;
mxArray *col4rowMATLAB, *row4colMATLAB, *uMATLAB, *vMATLAB, *CMat;
bool didFlip=false;
bool maximize=false;
if(nrhs<1){
mexErrMsgTxt("Not enough inputs.");
}
if(nrhs==2){
maximize=getBoolFromMatlab(prhs[1]);
}
if(nrhs>2) {
mexErrMsgTxt("Too many inputs.");
}
if(nlhs>5) {
mexErrMsgTxt("Too many outputs.");
}
/*Verify the validity of the assignment matrix.*/
checkRealDoubleArray(prhs[0]);
/* Get the dimensions of the input data and the pointer to the matrix.
* It is assumed that the matrix is not so large in M or N as to cause
* an overflow when using a SIGNED integer data type.*/
numRow = mxGetM(prhs[0]);
numCol = mxGetN(prhs[0]);
/* Transpose the matrix, if necessary, so that the number of rows is
* >= the number of columns.*/
if(numRow>=numCol) {
CMat=mxDuplicateArray(prhs[0]);
} else {
size_t temp;
temp=numCol;
numCol=numRow;
numRow=temp;
CMat=mxCreateDoubleMatrix(numCol,numRow,mxREAL);
mexCallMATLAB(1, &CMat, 1, (mxArray **)&prhs[0], "transpose");
didFlip=true;
}
C = (double*)mxGetData(CMat);
/* The cost matrix must have all non-negative elements for the
* assignment algorithm to work. This forces all of the elements to be
* positive. The delta is added back in when computing the gain in the
* end.*/
if(maximize==false) {
CDelta=INFINITY;
for(i=0;i<numRow*numCol;i++) {
if(C[i]<CDelta)
CDelta=C[i];
}
for(i=0;i<numRow*numCol;i++) {
C[i]=C[i]-CDelta;
}
} else {
CDelta=-INFINITY;
for(i=0;i<numRow*numCol;i++) {
if(C[i]>CDelta)
CDelta=C[i];
}
for(i=0;i<numRow*numCol;i++) {
C[i]=-C[i]+CDelta;
}
}
CDelta=CDelta*numCol;
//Allocate space for the return variables to Matlab.
col4rowMATLAB= allocSignedSizeMatInMatlab(numRow, 1);
row4colMATLAB= allocSignedSizeMatInMatlab(numCol, 1);
/* These will hold the dual variable values. They are all initialized
* to zero.*/
uMATLAB = mxCreateNumericMatrix(numCol,1,mxDOUBLE_CLASS,mxREAL);
vMATLAB = mxCreateNumericMatrix(numRow,1,mxDOUBLE_CLASS,mxREAL);
col4row=(ptrdiff_t*)mxGetData(col4rowMATLAB);
row4col=(ptrdiff_t*)mxGetData(row4colMATLAB);
gain=shortestPathCFast(C,col4row,row4col, (double*)mxGetData(uMATLAB), (double*)mxGetData(vMATLAB), numRow, numCol);
mxDestroyArray(CMat);
/* If a transposed array was used */
if(didFlip==true) {
size_t temp;
ptrdiff_t *tempIPtr;
mxArray *tempMat;
temp=numCol;
numCol=numRow;
numRow=temp;
tempIPtr=row4col;
row4col=col4row;
col4row=tempIPtr;
tempMat=row4colMATLAB;
row4colMATLAB=col4rowMATLAB;
col4rowMATLAB=tempMat;
tempMat=uMATLAB;
uMATLAB=vMATLAB;
vMATLAB=tempMat;
}
//If there is no feasible solution.
if(gain==-1){
mxDestroyArray(col4rowMATLAB);
mxDestroyArray(row4colMATLAB);
plhs[0]=mxCreateDoubleMatrix(0,0,mxREAL);
if(nlhs>1){
plhs[1]=mxCreateDoubleMatrix(0,0,mxREAL);
if(nlhs>2){
plhs[2]=mxCreateDoubleScalar(gain);
if(nlhs>3) {
plhs[3]=uMATLAB;
if(nlhs>4){
plhs[4]=vMATLAB;
}
}
}
}
return;
} else {
/* Adjust for shifting that was done to make everything positive.*/
if(maximize==false) {
gain=gain+CDelta;
} else {
gain=-gain+CDelta;
}
/* Convert the C indices into indices for MATLAB.*/
for(i=0;i<numRow;i++){
col4row[i]=col4row[i]+1;
}
plhs[0]=col4rowMATLAB;
if(nlhs>1){
plhs[1]=row4colMATLAB;
for(i=0;i<numCol;i++){
row4col[i]=row4col[i]+1;
}
if(nlhs>2){
plhs[2]=mxCreateDoubleScalar(gain);
if(nlhs>3) {
plhs[3]=uMATLAB;
if(nlhs>4){
plhs[4]=vMATLAB;
}
}
}
}
}
/*Free the u and v matrices if they are not returned.*/
if(nlhs<4){
mxDestroyArray(uMATLAB);
}
if(nlhs<5){
mxDestroyArray(vMATLAB);
}
return;
}
void mexFunction(const int nlhs, mxArray *plhs[], const int nrhs, const mxArray *prhs[]) {
bool diagAugment=false;
size_t numRow, numCol,numBetaCols;
mxArray *betaMatlab;
double *A, *beta;
if(nrhs<1){
mexErrMsgTxt("Not enough inputs.");
}
if(nrhs>2) {
mexErrMsgTxt("Too many inputs.");
}
if(nlhs>1) {
mexErrMsgTxt("Too many outputs.");
}
numRow = mxGetM(prhs[0]);
numCol = mxGetN(prhs[0]);
//If an empty matrix is passed, then return an empty matrix.
if(numRow==0||numCol==0) {
plhs[0]=mxCreateDoubleMatrix(0,0,mxREAL);
return;
}
checkRealDoubleArray(prhs[0]);
if(nrhs>1) {
diagAugment=getBoolFromMatlab(prhs[1]);
}
//Get the matrix.
A=(double*)mxGetData(prhs[0]);
//If we are solving the general probability problem.
if(diagAugment==false) {
//If we are here, then we just want general assignment probabilities,
//not specialized to target tracking applications.
size_t *buffer,*rows2Keep,*cols2Keep;
size_t *buff4PermFunc;//Buffer for the permanent function.
const size_t numRowsKept=numRow-1;
const size_t numColsKept=numCol-1;
size_t curRow,curCol;
//Allocate the return values.
numBetaCols=numCol;
betaMatlab=mxCreateDoubleMatrix(numRow,numBetaCols,mxREAL);
beta=(double*)mxGetData(betaMatlab);
//Allocate the space for the indices of the rows and columns that
//are kept in the submatrix to be passed to the permCPPSkip
//function.
buffer= new size_t[numRowsKept+2*numColsKept];
rows2Keep=buffer;
cols2Keep=rows2Keep+numRowsKept;
buff4PermFunc=cols2Keep+numColsKept;
for(curRow=0;curRow<numRow;curRow++) {
size_t i;
//Fill in the indices of the rows to keep.
for(i=curRow;i<numRowsKept;i++) {
rows2Keep[i]=i+1;
}
for(curCol=0;curCol<numCol;curCol++){
double ati=A[curRow+curCol*numRow];
//The A matrix removing the row and column corresponding
//to the selected association.
//Fill in the indices of the columns to keep.
for(i=curCol;i<numColsKept;i++) {
cols2Keep[i]=i+1;
}
beta[curRow+curCol*numRow]=ati*permCPPSkip(A,numRow,rows2Keep, cols2Keep, numRowsKept, numColsKept, buff4PermFunc);
cols2Keep[curCol]=curCol;
}
rows2Keep[curRow]=curRow;
}
//Delete the buffers
delete buffer;
}else {
//This is the case where we are solving for target-measurement
//assignment probabilities with missed detections.
size_t *buffer,*rows2Keep,*cols2Keep;
size_t *buff4PermFunc;//Buffer for the permanent function.
const size_t numRowsKept=numRow-1;
const size_t numColsKept=numCol-1;
size_t numTar,numMeas;
size_t curTar,curMeas;
if(numCol<numRow) {
mexErrMsgTxt("The number of columns cannot be less than the number of rows when diagAugment is true.");
}
numTar=numRow;
numMeas=numCol-numTar;
numBetaCols=numMeas+1;
//Allocate the return values.
betaMatlab=mxCreateDoubleMatrix(numRow,numBetaCols,mxREAL);
beta=(double*)mxGetData(betaMatlab);
//Allocate the space for the indices of the rows and columns
//that are kept in the submatrix to be passed to the
//permCPPSkip function.
buffer= new size_t[numRow+2*numCol];
rows2Keep=buffer;
cols2Keep=rows2Keep+numRowsKept;
buff4PermFunc=cols2Keep+numColsKept;
for(curTar=0;curTar<numTar;curTar++) {
size_t i;
double ati;
//Fill in the indices of the rows to keep.
for(i=curTar;i<numRowsKept;i++) {
rows2Keep[i]=i+1;
}
for(curMeas=0;curMeas<numMeas;curMeas++) {
//The measurement hypotheses
ati=A[curTar+curMeas*numRow];
//The A matrix removing the row and column corresponding
//to the selected association.
//Fill in the indices of the columns to keep.
for(i=curMeas;i<numColsKept;i++) {
cols2Keep[i]=i+1;
}
beta[curTar+curMeas*numRow]=ati*permCPPSkip(A,numRow,rows2Keep, cols2Keep, numRowsKept, numColsKept, buff4PermFunc);
cols2Keep[curMeas]=curMeas;
}
//The missed detection hypothesis
curMeas=numMeas+curTar;
ati=A[curTar+curMeas*numRow];
//Fill in the indices of the columns to keep.
for(i=numMeas;i<curMeas;i++) {
cols2Keep[i]=i;
}
for(i=curMeas;i<numColsKept;i++) {
cols2Keep[i]=i+1;
}
beta[curTar+numMeas*numRow]=ati*permCPPSkip(A,numRow,rows2Keep, cols2Keep, numRowsKept, numColsKept, buff4PermFunc);
rows2Keep[curTar]=curTar;
}
//Delete the buffers
delete buffer;
}
//Normalize the return values.
//It is faster to normalize the betas this way then to compute the
//normalization constant by finding the permanent of the entire A
//matrix.
{
double sumVal;
size_t curRow,curCol;
for(curRow=0;curRow<numRow;curRow++) {
sumVal=0;
//Compute the sum across the columns
for(curCol=0;curCol<numBetaCols;curCol++) {
sumVal+=beta[curRow+curCol*numRow];
}
//Normalize the value across the columns.
for(curCol=0;curCol<numBetaCols;curCol++) {
size_t idx=curRow+curCol*numRow;
beta[idx]/=sumVal;
}
}
}
//Set the return values.
plhs[0]=betaMatlab;
}
void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[]) {
//The Julan date in TT at the epoch used in the orbital propagation
//algorithm: 0:00 January 1 1950
const double epochDate=2433281.5;
double *deltaT;
size_t numTimes;
double SGP4Elements[7],elementEpochTime;
bool opsMode=0;
char opsChar;
bool gravityModel=0;
gravconsttype gravConstType;
mxArray *retMat;
double *retVec;
mxArray *errorMat;
double *errorVals;
if(nrhs<2||nrhs>6||nrhs==3) {
mexErrMsgTxt("Incorrect number of inputs.");
return;
}
if(nlhs>2) {
mexErrMsgTxt("Wrong number of outputs.");
return;
}
{
const size_t M=mxGetM(prhs[0]);
const size_t N=mxGetN(prhs[0]);
if(!((M==7&&N==1)||(N==7&&M==1))) {
mexErrMsgTxt("The SGP4 elements have the wrong dimensionality.");
return;
}
}
checkRealDoubleArray(prhs[0]);
{
size_t i;
double *theEls;
theEls=(double*)mxGetData(prhs[0]);
//Copy the elements into SGP4Elements, adjusting the units from SI
//to the values used here.
for(i=0;i<5;i++) {
SGP4Elements[i]=theEls[i];
}
//The multipliation by 60 changes the value from radians per second
//to radians per minute as desired by the SGP4 code.
SGP4Elements[5]=theEls[5]*60.0;
SGP4Elements[6]=theEls[6];
}
{
const size_t M=mxGetM(prhs[1]);
const size_t N=mxGetN(prhs[1]);
if((M!=1&&N!=1)||mxIsEmpty(prhs[1])) {
mexErrMsgTxt("The times have the wrong dimensionality.");
return;
}
numTimes=std::max(M,N);
}
checkRealDoubleArray(prhs[1]);
deltaT=(double*)mxGetData(prhs[1]);
if(nrhs>2&&!mxIsEmpty(prhs[2])&&!mxIsEmpty(prhs[3])) {
const double TT1=getDoubleFromMatlab(prhs[2]);
const double TT2=getDoubleFromMatlab(prhs[3]);
if(TT1>TT2) {
elementEpochTime=(TT1-epochDate)+TT2;
} else {
elementEpochTime=(TT2-epochDate)+TT1;
}
} else {
//No time is given. Make sure that the deep space propagator is not
//going to be used. Otherwise, the time value does not matter.
elementEpochTime=0;
if(2*pi/SGP4Elements[5]>=225) {
mexErrMsgTxt("The elements imply the use of the deep space propagator, but no time was given.");
return;
}
}
if(nrhs>4) {
opsMode=getBoolFromMatlab(prhs[4]);
}
if(opsMode==0) {
opsChar='a';
} else {
opsChar='i';
}
if(nrhs>5) {
gravityModel=getBoolFromMatlab(prhs[5]);
}
if(gravityModel==0) {
gravConstType=wgs72;
} else {
gravConstType=wgs84;
}
//Allocate space for the return values
retMat=mxCreateDoubleMatrix(6,numTimes,mxREAL);
retVec=(double*)mxGetData(retMat);
errorMat=mxCreateDoubleMatrix(numTimes,1,mxREAL);
errorVals=(double*)mxGetData(errorMat);
{
//This will be filled with the ephemeris information needed for
//propagating the satellite.
elsetrec satRec;
size_t i;
sgp4init(gravConstType,//Specified WGS-84 or WGS-72
opsChar,
elementEpochTime,//Epoch time of the orbital elements.
SGP4Elements[6],//BSTAR drag term.
SGP4Elements[0],//Eccentricity
SGP4Elements[2],//Argument of perigee
SGP4Elements[1],//Inclination
SGP4Elements[4],//Mean anomaly
SGP4Elements[5],//Mean motion
SGP4Elements[3],//Righ ascension of the ascending node.
satRec);//Gets filled by the function.
//Do the actual propagation.
//The division by 60 transforms the time value from seconds to
//minutes, as the SGP4 code wants the result in minutes.
for(i=0;i<numTimes;i++) {
double *r=retVec+6*i;
double *v=retVec+6*i+3;
int j;
sgp4(gravConstType, satRec, deltaT[i]/60.0, r, v);
//Convert the units from kilometers and kilometers per second
//to meters and meters per second.
for(j=0;j<3;j++) {
r[j]=1000*r[j];
v[j]=1000*v[j];
}
errorVals[i]=(double)satRec.error;
}
}
//Save the result
plhs[0]=retMat;
//If the error value is desired.
if(nlhs>1) {
plhs[1]=errorMat;
} else{
mxDestroyArray(errorMat);
}
}
void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[]) {
size_t i, numRows,numCols, numEls;
double minBound, maxBound, *vals, *wrapVals;
mxArray *retMat;
bool mirrorWrap=false;
if(nrhs<3){
mexErrMsgTxt("Not enough inputs.");
return;
}
if(nrhs>4) {
mexErrMsgTxt("Too many inputs.");
return;
}
if(nlhs>1) {
mexErrMsgTxt("Too many outputs.");
return;
}
checkRealDoubleArray(prhs[0]);
numRows=mxGetM(prhs[0]);
numCols=mxGetN(prhs[0]);
numEls=numRows*numCols;
//If given an empty matrix, return an empty matrix.
if(numEls==0) {
plhs[0]=mxCreateDoubleMatrix(0,0,mxREAL);
return;
}
vals=(double*)mxGetData(prhs[0]);
minBound=getDoubleFromMatlab(prhs[1]);
maxBound=getDoubleFromMatlab(prhs[2]);
if(maxBound<=minBound) {
mexErrMsgTxt("the maximum bound must be less than the minimum bound.");
}
if(nrhs>3) {
mirrorWrap=getBoolFromMatlab(prhs[3]);
}
//Allocate space for the return values.
retMat=mxCreateDoubleMatrix(numRows,numCols,mxREAL);
wrapVals=(double*)mxGetData(retMat);
if(mirrorWrap) {
for(i=0;i<numEls;i++) {
wrapVals[i]=wrapRangeMirrorCPP(vals[i],minBound,maxBound);
}
} else {
for(i=0;i<numEls;i++) {
wrapVals[i]=wrapRangeCPP(vals[i],minBound,maxBound);
}
}
plhs[0]=retMat;
}
