void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[]) {
double *Jul1,*Jul2, *Jul1Ret, *Jul2Ret;
mxArray *Jul1RetMATLAB,*Jul2RetMATLAB;
size_t numRow, numCol, numElements,i;
if(nrhs!=2) {
mexErrMsgTxt("Wrong number of inputs");
}
if(nlhs>2) {
mexErrMsgTxt("Wrong number of outputs.");
return;
}
numRow=mxGetM(prhs[0]);
numCol=mxGetN(prhs[0]);
numElements=numRow*numCol;
if(numElements==0||numRow!=mxGetM(prhs[1])||numCol!=mxGetN(prhs[1])) {
mexErrMsgTxt("The dimensionalities of the inputs are incorrect.");
return;
}
checkRealDoubleArray(prhs[0]);
checkRealDoubleArray(prhs[1]);
Jul1=(double*)mxGetData(prhs[0]);
Jul2=(double*)mxGetData(prhs[1]);
//Allocate space for the return variables.
Jul1RetMATLAB=mxCreateDoubleMatrix(numRow,numCol,mxREAL);
Jul1Ret=(double*)mxGetData(Jul1RetMATLAB);
Jul2RetMATLAB=mxCreateDoubleMatrix(numRow,numCol,mxREAL);
Jul2Ret=(double*)mxGetData(Jul2RetMATLAB);
for(i=0; i<numElements; i++) {
int retVal;
/*Call the function in the SOFA library.*/
retVal=iauUtctai(Jul1[i], Jul2[i], Jul1Ret+i, Jul2Ret+i);
switch(retVal) {
case 1:
mexWarnMsgTxt("Dubious Date entered.");
break;
case -1:
mxDestroyArray(Jul1RetMATLAB);
mxDestroyArray(Jul2RetMATLAB);
mexErrMsgTxt("Unacceptable date entered");
break;
default:
break;
}
}
plhs[0]=Jul1RetMATLAB;
if(nlhs>1) {
plhs[1]=Jul2RetMATLAB;
} else {
mxDestroyArray(Jul2RetMATLAB);
}
}
void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[]) {
size_t numVertices;
double *vertices, A;
if(nrhs!=1) {
mexErrMsgTxt("Incorrect number of inputs.");
return;
}
if(nlhs>1) {
mexErrMsgTxt("Incorrect number of outputs.");
return;
}
checkRealDoubleArray(prhs[0]);
//If an empty matrix is passed, return zero area.
if(mxIsEmpty(prhs[0])) {
const double retVal=0;
plhs[0]=doubleMat2Matlab(&retVal,1,1);
return;
}
if(mxGetM(prhs[0])!=2) {
mexErrMsgTxt("The points have the wrong dimensionality.");
return;
}
numVertices=mxGetN(prhs[0]);
vertices=reinterpret_cast<double*>(mxGetData(prhs[0]));
A=signedPolygonAreaCPP(vertices,numVertices);
plhs[0]=doubleMat2Matlab(&A,1,1);
}
void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[]) {
double c, *vObsFrame, *vObjInFrame, *vObjRef;
mxArray *retMATLAB;
size_t i, numVec;
if(nrhs!=2) {
mexErrMsgTxt("Incorrect number of inputs.");
return;
}
if(nlhs>1) {
mexErrMsgTxt("Wrong number of outputs.");
return;
}
numVec=mxGetN(prhs[0]);
if(numVec!=mxGetN(prhs[1])||mxGetM(prhs[0])!=3||mxGetM(prhs[1])!=3) {
mexErrMsgTxt("The input vectors have the wrong dimensionality.");
return;
}
checkRealDoubleArray(prhs[0]);
vObsFrame=(double*)mxGetData(prhs[0]);
checkRealDoubleArray(prhs[1]);
vObjInFrame=(double*)mxGetData(prhs[1]);
c=getScalarMatlabClassConst("Constants","speedOfLight");
//Allocate space for the return values.
retMATLAB=mxCreateDoubleMatrix(3,numVec,mxREAL);
vObjRef=(double*)mxGetData(retMATLAB);
for(i=0; i<numVec; i++) {
relVecAddC(c,vObsFrame+3*i,vObjInFrame+3*i,vObjRef+3*i);
}
plhs[0]=retMATLAB;
}
void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[]) {
double *Jul1,*Jul2,*epochJ;
size_t numRow, numCol, numElements,i;
mxArray *JulianEpochRetMATLAB;
if(nrhs!=2){
mexErrMsgTxt("Wrong number of inputs");
}
if(nlhs>1){
mexErrMsgTxt("Wrong number of outputs.");
return;
}
numRow=mxGetM(prhs[0]);
numCol=mxGetN(prhs[0]);
numElements=numRow*numCol;
if(numElements==0||numRow!=mxGetM(prhs[1])||numCol!=mxGetN(prhs[1])) {
mexErrMsgTxt("The dimensionalities of the inputs are incorrect.");
return;
}
checkRealDoubleArray(prhs[0]);
checkRealDoubleArray(prhs[1]);
Jul1=(double*)mxGetData(prhs[0]);
Jul2=(double*)mxGetData(prhs[1]);
JulianEpochRetMATLAB=mxCreateDoubleMatrix(numRow,numCol,mxREAL);
epochJ=(double*)mxGetData(JulianEpochRetMATLAB);
for(i=0;i<numElements;i++) {
/*Call the function in the SOFA library.*/
epochJ[i]=iauEpj(Jul1[i], Jul2[i]);
}
plhs[0]=JulianEpochRetMATLAB;
}
void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[]) {
double *vec, *retVec, TT1,TT2;
double rb[3][3], rp[3][3], rbp[3][3];
mxArray *retMATLAB;
size_t i, numItems;
if(nrhs!=3) {
mexErrMsgTxt("Wrong number of inputs");
return;
}
if(nlhs>1) {
mexErrMsgTxt("Wrong number of outputs");
return;
}
numItems=mxGetN(prhs[0]);
if(mxGetM(prhs[0])!=3) {
mexErrMsgTxt("vec has the wrong dimensionality. It must be an 3XN matrix.");
return;
}
checkRealDoubleArray(prhs[0]);
vec=(double*)mxGetData(prhs[0]);
TT1=getDoubleFromMatlab(prhs[1]);
TT2=getDoubleFromMatlab(prhs[2]);
//Allocate the return vector
retMATLAB=mxCreateDoubleMatrix(3,numItems,mxREAL);
retVec=mxGetData(retMATLAB);
//Call the IAU function to get the rotation matrix.
iauBp06(TT1, TT2, rb, rp, rbp);
//Invert the rotation matrix by transposing it.
iauTr(rb, rb);
for(i=0; i<numItems; i++) {
//Multiply the original vectors by the matrix to put it into the ICRS.
iauRxp(rb, vec+3*i, retVec+3*i);
}
//Set the return value.
plhs[0]=retMATLAB;
}
void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[]) {
size_t numRow, numCol,numPoints, foundIdx;
double *arr;
mxArray *retIdx;
if(nrhs<1){
mexErrMsgTxt("Not enough inputs.");
return;
}
if(nrhs>1) {
mexErrMsgTxt("Too many inputs.");
return;
}
if(nlhs>1) {
mexErrMsgTxt("Too many outputs.");
}
/*Verify the validity of the vector to search.*/
checkRealDoubleArray(prhs[0]);
numRow = mxGetM(prhs[0]);
numCol = mxGetN(prhs[0]);
if(numRow>1&&numCol>1){
mexErrMsgTxt("Too many dimensions in the array provided.");
return;
} else {
if(numRow>numCol) {
numPoints=numRow;
} else {
numPoints=numCol;
}
}
arr = reinterpret_cast<double*>(mxGetData(prhs[0]));
foundIdx= findFirstMaxCPP(arr,numPoints);
//Set the return value
retIdx=allocUnsignedSizeMatInMatlab(1,1);
*reinterpret_cast<size_t *>(mxGetData(retIdx))=foundIdx+1;
plhs[0]=retIdx;
}
void mexFunction(const int nlhs, mxArray *plhs[], const int nrhs, const mxArray *prhs[]) {
double a,c,scalFactor;
double *point;
ClusterSetCPP<double> CStdDev;
ClusterSetCPP<double> SStdDev;
size_t numPoints;
size_t i, M, totalNumEls;
mxArray *sigma2MATLAB;
//This variable is only used if nlhs>1. It is set to zero here to
//suppress a warning if compiled using -Wconditional-uninitialized.
mxArray *SigmaMATLAB=NULL;
double *sigma2,*Sigma;
bool didOverflow;
size_t *theOffsetArray, *theClusterSizes;
if(nrhs!=6) {
mexErrMsgTxt("Wrong number of inputs.");
}
if(nlhs>3) {
mexErrMsgTxt("Invalid number of outputs.");
}
//Check the validity of the ClusterSets
checkRealDoubleArray(prhs[0]);
checkRealDoubleArray(prhs[1]);
//Make sure that both of the coefficient sets have the same number of
//elements and are not empty.
if(mxIsEmpty(prhs[0])||mxIsEmpty(prhs[1])||mxGetM(prhs[0])!=mxGetM(prhs[1])||mxGetN(prhs[0])!=mxGetN(prhs[1])) {
mexErrMsgTxt("Invalid ClusterSet data passed.");
}
CStdDev.clusterEls=reinterpret_cast<double*>(mxGetData(prhs[0]));
SStdDev.clusterEls=reinterpret_cast<double*>(mxGetData(prhs[1]));
//The number of elements in the ClusterSets
totalNumEls=mxGetM(prhs[0])*mxGetN(prhs[0]);
//We do not want to trust that the offset arrays and the cluster arrays
//are valid. Also, the data types of the passed values can vary (e.g.,
//might be doubles). Thus, we this function does not take the offset
//and number of clusters data from a ClusterSet class. Rather, it
//recreates the values based on the length of the data. This avoids
//possibly reading past the end of an array.
//Since the total number of points in a ClusterSet
//is (M+1)*(M+2)/2, where M is the number of clusters -1, we can easily
//verify that a valid number of points was passed.
//Some versions of Windows SDK have problems with passing an integer
//type to the sqrt function, so this explicit typecasting should take
//care of the problem.
M=(-3+static_cast<size_t>(sqrt(static_cast<double>(1+8*totalNumEls))))/2;
if((M+1)*(M+2)/2!=totalNumEls) {
mexErrMsgTxt("The ClusterSets contain an inconsistent number of elements.");
}
//Otherwise, fill in the values for the cluster sizes and the ofset
//array.
CStdDev.numClust=M+1;
SStdDev.numClust=M+1;
theOffsetArray=new size_t[M+1];
theClusterSizes=new size_t[M+1];
theClusterSizes[0]=1;
theOffsetArray[0]=0;
for(i=1;i<=M;i++) {
theOffsetArray[i]=theOffsetArray[i-1]+theClusterSizes[i-1];
theClusterSizes[i]=i+1;
}
CStdDev.totalNumEl=totalNumEls;
SStdDev.totalNumEl=totalNumEls;
CStdDev.offsetArray=theOffsetArray;
SStdDev.offsetArray=theOffsetArray;
CStdDev.clusterSizes=theClusterSizes;
SStdDev.clusterSizes=theClusterSizes;
//Get the other parameters.
checkRealDoubleArray(prhs[2]);
point=reinterpret_cast<double*>(mxGetData(prhs[2]));
numPoints=mxGetN(prhs[2]);
a=getDoubleFromMatlab(prhs[3]);
c=getDoubleFromMatlab(prhs[4]);
scalFactor=getDoubleFromMatlab(prhs[5]);
//Allocate space for the return values
sigma2MATLAB=mxCreateDoubleMatrix(numPoints, 1,mxREAL);
sigma2=reinterpret_cast<double*>(mxGetData(sigma2MATLAB));
if(nlhs>1) {
mwSize dims[3];
//A 3X3X numPoints array is needed for the covariance matrices.
dims[0]=3;
dims[1]=3;
dims[2]=numPoints;
SigmaMATLAB=mxCreateNumericArray(3,dims,mxDOUBLE_CLASS,mxREAL);
Sigma=reinterpret_cast<double*>(mxGetData(SigmaMATLAB));
} else {
Sigma=NULL;
}
didOverflow=spherHarmonicCovCPP(sigma2,Sigma,CStdDev,SStdDev,point,numPoints,a,c,scalFactor);
//Free the allocated space for the offet array and cluster sizes
delete[] theOffsetArray;
delete[] theClusterSizes;
plhs[0]=sigma2MATLAB;
if(nlhs>1) {
plhs[1]=SigmaMATLAB;
if(nlhs>2) {
plhs[2]=boolMat2Matlab(&didOverflow,1,1);
}
}
}
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 n;
size_t prodDim;
double *A, *B, *K;
mxArray *retMat;
const double sqrt2=sqrt(2);
const double dblSqrt2=2*sqrt2;
if(nrhs!=2) {
mexErrMsgTxt("Incorrect number of inputs.");
return;
}
if(nlhs>1) {
mexErrMsgTxt("Wrong number of outputs.");
return;
}
if((mxIsEmpty(prhs[0])&&!mxIsEmpty(prhs[1]))||(!mxIsEmpty(prhs[0])&&mxIsEmpty(prhs[1]))) {
mexErrMsgTxt("Invalid use of empty matrices as inputs.");
return;
}
//If two empty matrices are passed, then just return an empty matrix.
if(mxIsEmpty(prhs[0])&&mxIsEmpty(prhs[1])) {
plhs[0]=mxCreateDoubleMatrix(0,0,mxREAL);
return;
}
checkRealDoubleArray(prhs[0]);
checkRealDoubleArray(prhs[1]);
if(mxGetNumberOfDimensions(prhs[0])>2||mxGetNumberOfDimensions(prhs[1])>2) {
mexErrMsgTxt("This function does not work with hypermatrices.");
return;
}
n=mxGetN(prhs[0]);
if(n!=mxGetM(prhs[0])||n!=mxGetN(prhs[1])||n!=mxGetM(prhs[1])) {
mexErrMsgTxt("This function only works with pairs of squares matrices.");
return;
}
prodDim=(n*(n+1))/2;
//Allocate space for the return matrix
retMat=mxCreateDoubleMatrix(prodDim,prodDim,mxREAL);
A=(double*)mxGetData(prhs[0]);
B=(double*)mxGetData(prhs[1]);
K=(double*)mxGetData(retMat);
{
size_t i1,i2,j1,j2;
for(i1=1;i1<=n;i1++) {
size_t col=i1+((i1-1)*(2*n-i1))/2;
for(j1=1;j1<=n;j1++) {
size_t KIdx=prodDim*(col-1)+j1+((j1-1)*(2*n-j1))/2-1;
const size_t j1i1=j1+n*(i1-1)-1;
size_t j2i1=j1i1;
K[KIdx]=(A[j1i1]*B[j1i1]+B[j1i1]*A[j1i1])/2;
for(j2=(j1+1);j2<=n;j2++) {
KIdx++;
j2i1++;
K[KIdx]=(A[j1i1]*B[j2i1]+B[j1i1]*A[j2i1])/sqrt2;
}
}
for(i2=(i1+1);i2<=n;i2++) {
col++;
for(j1=1;j1<=n;j1++) {
size_t KIdx=prodDim*(col-1)+j1+((j1-1)*(2*n-j1))/2-1;
const size_t j1i1=j1+n*(i1-1)-1;
const size_t j1i2=j1+n*(i2-1)-1;
size_t j2i1=j1i1;
size_t j2i2=j1i2;
K[KIdx]=(A[j1i1]*B[j1i2]+A[j1i2]*B[j1i1]+B[j1i1]*A[j1i2]+B[j1i2]*A[j1i1])/dblSqrt2;
for(j2=(j1+1);j2<=n;j2++) {
KIdx++;
j2i1++;
j2i2++;
K[KIdx]=(A[j1i1]*B[j2i2]+A[j1i2]*B[j2i1]+B[j1i1]*A[j2i2]+B[j1i2]*A[j2i1])/2;
}
}
}
}
}
plhs[0]=retMat;
}
void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[]) {
const mxArray *MatlabFunctionHandle;
direct_algorithm theAlgorithm=DIRECT_ORIGINAL;
const direct_objective_func f=&MatlabCallback;
size_t numDim;
double *lowerBounds;
double *upperBounds;
int maxFEval=500;
int maxIter=100;
double epsilon=1e-4;
double epsilonAbs=0;
double volumeRelTol=1e-10;
double sigmaRelTol=0;
double fGlobal=DIRECT_UNKNOWN_FGLOBAL;
double fGlobalRelTol=DIRECT_UNKNOWN_FGLOBAL;
int maxDiv=5000;
//To hold return values
direct_return_code retVal;
double *x;
double fVal;
if(nrhs<3||nrhs>4){
mexErrMsgTxt("Wrong number of inputs");
}
if(nlhs>3) {
mexErrMsgTxt("Wrong number of outputs.");
}
//Check that a function handle was passed.
if(!mxIsClass(prhs[0],"function_handle")) {
mexErrMsgTxt("The first input must be a function handle.");
}
MatlabFunctionHandle=prhs[0];
//Check the lower and upper bounds
checkRealDoubleArray(prhs[1]);
checkRealDoubleArray(prhs[2]);
numDim=mxGetM(prhs[1]);
if(numDim<1||mxGetN(prhs[1])!=1) {
mexErrMsgTxt("The lower bounds have the wrong dimensionality.");
}
if(mxGetM(prhs[2])!=numDim||mxGetN(prhs[1])!=1) {
mexErrMsgTxt("The upper bounds have the wrong dimensionality.");
}
//The function in the library only uses integers for dimensions.
if(numDim>INT_MAX) {
mexErrMsgTxt("The problem has too many dimensions to be solved using this function.");
}
lowerBounds=(double*)mxGetData(prhs[1]);
upperBounds=(double*)mxGetData(prhs[2]);
//If a structure of options was given
if(nrhs>3&&!mxIsEmpty(prhs[3])) {
//We have to check for every possible structure member.
mxArray *theField;
if(!mxIsStruct(prhs[3])) {
mexErrMsgTxt("The options must be given in a structure.");
}
theField=mxGetField(prhs[3],0,"algorithm");
if(theField!=NULL) {//If the field is present.
int algType=getIntFromMatlab(theField);
//Algorithm type
switch(algType) {
case 0:
theAlgorithm=DIRECT_ORIGINAL;
break;
case 1:
theAlgorithm=DIRECT_GABLONSKY;
break;
default:
mexErrMsgTxt("Invalid algorithm specified");
}
}
theField=mxGetField(prhs[3],0,"maxDiv");
if(theField!=NULL) {//If the field is present.
maxDiv=getIntFromMatlab(theField);
if(maxDiv<1) {
mexErrMsgTxt("Invalid maxDiv specified");
}
}
theField=mxGetField(prhs[3],0,"maxIter");
if(theField!=NULL) {//If the field is present.
maxIter=getIntFromMatlab(theField);
if(maxIter<1) {
mexErrMsgTxt("Invalid maxIter specified");
}
}
theField=mxGetField(prhs[3],0,"maxFEval");
if(theField!=NULL) {//If the field is present.
maxFEval=getIntFromMatlab(theField);
if(maxIter<1) {
mexErrMsgTxt("Invalid maxFEval specified");
}
}
theField=mxGetField(prhs[3],0,"epsilon");
if(theField!=NULL) {//If the field is present.
epsilon=getDoubleFromMatlab(theField);
if(fabs(epsilon)>=1) {
mexErrMsgTxt("Invalid epsilon specified");
}
}
theField=mxGetField(prhs[3],0,"epsilonAbs");
if(theField!=NULL) {//If the field is present.
epsilonAbs=getDoubleFromMatlab(theField);
if(epsilonAbs>=1||epsilonAbs<0) {
mexErrMsgTxt("Invalid epsilonAbs specified");
}
}
theField=mxGetField(prhs[3],0,"volumeRelTol");
if(theField!=NULL) {//If the field is present.
volumeRelTol=getDoubleFromMatlab(theField);
if(volumeRelTol>=1||volumeRelTol<0) {
mexErrMsgTxt("Invalid volumeRelTol specified");
}
}
theField=mxGetField(prhs[3],0,"sigmaRelTol");
if(theField!=NULL) {//If the field is present.
sigmaRelTol=getDoubleFromMatlab(theField);
if(sigmaRelTol>=1||sigmaRelTol<0) {
mexErrMsgTxt("Invalid sigmaRelTol specified");
}
}
theField=mxGetField(prhs[3],0,"fGlobal");
if(theField!=NULL) {//If the field is present.
fGlobal=getDoubleFromMatlab(theField);
fGlobalRelTol=1e-12;//Default value if fGlobal is given.
}
theField=mxGetField(prhs[3],0,"fGlobalRelTol");
if(theField!=NULL) {//If the field is present.
fGlobalRelTol=getDoubleFromMatlab(theField);
if(fGlobalRelTol<0) {
mexErrMsgTxt("Invalid fGlobalRelTol specified");
}
}
}
//Allocate space for the return values
x=mxCalloc(numDim, sizeof(double));
retVal=direct_optimize(f,//Objective function pointer
(void*)MatlabFunctionHandle,//Data that passed to the objective function. Here, the function handle passed by the user.
(int)numDim,//The dimensionality of the problem
lowerBounds,//Array of the lower bound of each dimension.
upperBounds,//Array of the upper bound of each dimension.
x,//An array that will be set to the optimum value on return.
&fVal,//On return, set to the minimum value.
maxFEval,//Maximum number of function evaluations.
maxIter,//Maximum number of iterations
epsilon,//Jones' epsilon parameter (1e-4 is recommended)
epsilonAbs,//An absolute version of magic_eps
//Relative tolerance on the hypercube volume (use 0 if none). This is
//a percentage (0-1) of the volume of the original hypercube
volumeRelTol,
//Relative tolerance on the hypercube measure (0 if none)
sigmaRelTol,
//A pointer to a variable that can terminate the optimization. This is
//in the library's code so that an external thread can write to this
//and force the optimization to stop. Here, we are not using it, so we
//can pass NULL.
NULL,
fGlobal,//Function value of the global optimum, if known. Use DIRECT_UNKNOWN_FGLOBAL if unknown.
fGlobalRelTol,//Relative tolerance for convergence if fglobal is known. If unknown, use DIRECT_UNKNOWN_FGLOBAL.
NULL,//There is no output to a file.
theAlgorithm,//The algorithm to use
maxDiv);//The maximum number of hyperrectangle divisions
//If an error occurred
if(retVal<0) {
mxFree(x);
switch(retVal){
case DIRECT_INVALID_BOUNDS:
mexErrMsgTxt("Upper bounds are <= lower bounds for one or more dimensions.");
case DIRECT_MAXFEVAL_TOOBIG:
mexErrMsgTxt("maxFEval is too large.");
case DIRECT_INIT_FAILED:
mexErrMsgTxt("An initialization step failed.");
case DIRECT_SAMPLEPOINTS_FAILED:
mexErrMsgTxt("An error occurred creating sampling points.");
case DIRECT_SAMPLE_FAILED:
mexErrMsgTxt("An error occurred while sampling the function.");
case DIRECT_OUT_OF_MEMORY:
mexErrMsgTxt("Out of memory");
case DIRECT_INVALID_ARGS:
mexErrMsgTxt("Invalid arguments provided");
//These errors should not occur either because they should
//never be called or because retVal>=0 so these are not
//actually errors.
case DIRECT_FORCED_STOP:
case DIRECT_MAXFEVAL_EXCEEDED:
case DIRECT_MAXITER_EXCEEDED:
case DIRECT_GLOBAL_FOUND:
case DIRECT_VOLTOL:
case DIRECT_SIGMATOL:
case DIRECT_MAXTIME_EXCEEDED:
default:
mexErrMsgTxt("An unknown error occurred.");
}
}
//Set the return values
plhs[0]=mxCreateNumericMatrix(0, 0, mxDOUBLE_CLASS, mxREAL);
mxFree(mxGetPr(plhs[0]));
mxSetPr(plhs[0], x);
mxSetM(plhs[0], numDim);
mxSetN(plhs[0], 1);
if(nlhs>1) {
plhs[1]=doubleMat2Matlab(&fVal,1,1);
if(nlhs>2) {
plhs[2]=intMat2MatlabDoubles(&retVal,1,1);
}
}
}
void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[]) {
size_t numRow,numVec;
mxArray *retMat;
double *retData;
double ITRS2TIRS[3][3];//Inverse polar motion matrix
double *xVec, TT1, TT2;
double xp=0;
double yp=0;//The polar motion coordinates
if(nrhs<3||nrhs>4){
mexErrMsgTxt("Wrong number of inputs");
}
if(nlhs>2) {
mexErrMsgTxt("Wrong number of outputs.");
}
numRow = mxGetM(prhs[0]);
numVec = mxGetN(prhs[0]);
if(!(numRow==3||numRow==6)) {
mexErrMsgTxt("The input vector has a bad dimensionality.");
}
checkRealDoubleArray(prhs[0]);
xVec=(double*)mxGetData(prhs[0]);
TT1=getDoubleFromMatlab(prhs[1]);
TT2=getDoubleFromMatlab(prhs[2]);
//If xpyp should be found using the function getEOP.
if(nrhs<4||mxIsEmpty(prhs[3])) {
mxArray *retVals[1];
double *xpyp;
mxArray *JulUTCMATLAB[2];
double JulUTC[2];
int retVal;
//Get the time in UTC to look up the parameters by going to TAI and
//then UTC.
retVal=iauTttai(TT1, TT2, &JulUTC[0], &JulUTC[1]);
if(retVal!=0) {
mexErrMsgTxt("An error occurred computing TAI.");
}
retVal=iauTaiutc(JulUTC[0], JulUTC[1], &JulUTC[0], &JulUTC[1]);
switch(retVal){
case 1:
mexWarnMsgTxt("Dubious Date entered.");
break;
case -1:
mexErrMsgTxt("Unacceptable date entered");
break;
default:
break;
}
JulUTCMATLAB[0]=doubleMat2Matlab(&JulUTC[0],1,1);
JulUTCMATLAB[1]=doubleMat2Matlab(&JulUTC[1],1,1);
//Get the Earth orientation parameters for the given date.
mexCallMATLAB(1,retVals,2,JulUTCMATLAB,"getEOP");
mxDestroyArray(JulUTCMATLAB[0]);
mxDestroyArray(JulUTCMATLAB[1]);
checkRealDoubleArray(retVals[0]);
if(mxGetM(retVals[0])!=2||mxGetN(retVals[0])!=1) {
mxDestroyArray(retVals[0]);
mexErrMsgTxt("Error using the getEOP function.");
return;
}
xpyp=(double*)mxGetData(retVals[0]);
xp=xpyp[0];
yp=xpyp[1];
//Free the returned array.
mxDestroyArray(retVals[0]);
}
//Get polar motion coordinates, if given.
if(nrhs>3&&!mxIsEmpty(prhs[3])) {
size_t dim1, dim2;
checkRealDoubleArray(prhs[3]);
dim1 = mxGetM(prhs[3]);
dim2 = mxGetN(prhs[3]);
if((dim1==2&&dim2==1)||(dim1==1&&dim2==2)) {
double *xpyp=(double*)mxGetData(prhs[3]);
xp=xpyp[0];
yp=xpyp[1];
} else {
mexErrMsgTxt("The polar motion coordinates have the wrong dimensionality.");
return;
}
}
//Get the rotation matrix from TIRS to ITRS.
{
double sp;
double TIRS2ITRS[3][3];//Polar motion matrix
//Get the Terrestrial Intermediate Origin (TIO) locator s' in
//radians
sp=iauSp00(TT1,TT2);
//Get the polar motion matrix
iauPom00(xp,yp,sp,TIRS2ITRS);
//The inverse polar motion matrix is given by the transpose of the
//polar motion matrix.
iauTr(TIRS2ITRS, ITRS2TIRS);
}
//Allocate space for the return vectors.
retMat=mxCreateDoubleMatrix(numRow,numVec,mxREAL);
retData=(double*)mxGetData(retMat);
{
size_t curVec;
for(curVec=0;curVec<numVec;curVec++) {
//Multiply the position vector with the rotation matrix.
iauRxp(ITRS2TIRS, xVec+numRow*curVec, retData+numRow*curVec);
//Multiply the velocity vector with the rotation matrix.
if(numRow>3) {
double *velITRS=xVec+numRow*curVec+3;//Velocity in ITRS
double *retDataVel=retData+numRow*curVec+3;//Velocity in ITRS
//Convert velocity from ITRS to TIRS.
iauRxp(ITRS2TIRS, velITRS, retDataVel);
}
}
}
plhs[0]=retMat;
if(nlhs>1) {
double *elPtr;
size_t i,j;
plhs[1]=mxCreateDoubleMatrix(3,3,mxREAL);
elPtr=(double*)mxGetData(plhs[1]);
for (i=0;i<3;i++) {
for(j=0;j<3;j++) {
elPtr[i+3*j]=ITRS2TIRS[i][j];
}
}
}
}
void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[]) {
double *year,*month,*day,*hour,*minute,*second,*Jul1,*Jul2;
mxArray *Jul1MATLAB,*Jul2MATLAB;
size_t numRow,numCol,numElements,i;
if(nrhs!=6){
mexErrMsgTxt("Wrong number of inputs");
}
if(nlhs>2){
mexErrMsgTxt("Wrong number of outputs.");
return;
}
numRow=mxGetM(prhs[0]);
numCol=mxGetN(prhs[0]);
numElements=numRow*numCol;
if(numElements==0) {
mexErrMsgTxt("The dimensionalities of the inputs are incorrect.");
return;
}
for(i=0;i<6;i++) {
if(numRow==mxGetM(prhs[i])&&numCol==mxGetN(prhs[i])) {
checkRealDoubleArray(prhs[i]);
} else {
mexErrMsgTxt("The dimensionalities of the inputs are incorrect.");
return;
}
}
/*Extract the input arguments.*/
year=(double*)mxGetData(prhs[0]);
month=(double*)mxGetData(prhs[1]);
day=(double*)mxGetData(prhs[2]);
hour=(double*)mxGetData(prhs[3]);
minute=(double*)mxGetData(prhs[4]);
second=(double*)mxGetData(prhs[5]);
//Allocate space for the return variables.
Jul1MATLAB=mxCreateDoubleMatrix(numRow,numCol,mxREAL);
Jul1=(double*)mxGetData(Jul1MATLAB);
Jul2MATLAB=mxCreateDoubleMatrix(numRow,numCol,mxREAL);
Jul2=(double*)mxGetData(Jul2MATLAB);
/*Call the function in the SOFA library.*/
for(i=0;i<numElements;i++) {
int retVal;
retVal=iauDtf2d("UTC",(int)year[i],(int)month[i],(int)day[i],(int)hour[i],(int)minute[i],second[i], &Jul1[i], &Jul2[i]);
switch(retVal){
case 3:
mexWarnMsgTxt("The time is after the end of the day and the year is dubious.");
break;
case 2:
mexWarnMsgTxt("The time is after the end of the day.");
break;
case 1:
mexWarnMsgTxt("The year is dubious.");
break;
case -1:
mxDestroyArray(Jul1MATLAB);
mxDestroyArray(Jul2MATLAB);
mexErrMsgTxt("Bad year given.");
return;
case -2:
mxDestroyArray(Jul1MATLAB);
mxDestroyArray(Jul2MATLAB);
mexErrMsgTxt("Bad month given.");
return;
case -3:
mxDestroyArray(Jul1MATLAB);
mxDestroyArray(Jul2MATLAB);
mexErrMsgTxt("Bad day given.");
return;
case -4:
mxDestroyArray(Jul1MATLAB);
mxDestroyArray(Jul2MATLAB);
mexErrMsgTxt("Bad hour given.");
return;
case -5:
mxDestroyArray(Jul1MATLAB);
mxDestroyArray(Jul2MATLAB);
mexErrMsgTxt("Bad minute given.");
return;
case-6:
mxDestroyArray(Jul1MATLAB);
mxDestroyArray(Jul2MATLAB);
mexErrMsgTxt("Bad second given.");
return;
default:
break;
}
}
plhs[0]=Jul1MATLAB;
if(nlhs>1) {
plhs[1]=Jul2MATLAB;
} else {
mxDestroyArray(Jul2MATLAB);
}
}
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;
}
void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[]) {
size_t numSource, numBodies, baseIdx,curBody,curSource;
double *vObsSource,posObs[3], *MSolar, *xBody, *deflecLimit;
iauLDBODY *bodyParam;
mxArray *vDeflMATLAB;
double *vDefl;
if(nrhs<4||nrhs>5) {
mexErrMsgTxt("Incorrect number of inputs.");
return;
}
if(nlhs>1) {
mexErrMsgTxt("Wrong number of outputs.");
return;
}
//Check the inputs
checkRealDoubleArray(prhs[0]);
checkRealDoubleArray(prhs[1]);
checkRealDoubleArray(prhs[2]);
//Check the inputs
numSource=mxGetN(prhs[0]);
if(mxGetM(prhs[0])!=3||numSource==0) {
mexErrMsgTxt("The input vObsSource has the wrong dimensionality.");
}
if(mxGetM(prhs[1])!=3||mxGetN(prhs[1])!=1) {
mexErrMsgTxt("The input posObs has the wrong dimensionality.");
}
numBodies=mxGetM(prhs[2]);
if(numBodies==0||mxGetN(prhs[2])!=1) {
mexErrMsgTxt("The input MSolar has the wrong dimensionality.");
}
if(mxGetM(prhs[3])!=6||mxGetN(prhs[2])!=numBodies) {
mexErrMsgTxt("The input xBody has the wrong dimensionality.");
}
if(nrhs>3) {
checkRealDoubleArray(prhs[4]);
if(mxGetM(prhs[4])!=numBodies||mxGetN(prhs[4])!=1) {
mexErrMsgTxt("The input deflecLimit has the wrong dimensionality.");
}
} else {
deflecLimit=NULL;
}
vObsSource=(double*)mxGetData(prhs[0]);
//Get the observer position and convert from meters to AU.
{
double *temp=(double*)mxGetData(prhs[1]);
posObs[0]=temp[0]/DAU;
posObs[1]=temp[1]/DAU;
posObs[2]=temp[2]/DAU;
}
MSolar=(double*)mxGetData(prhs[2]);
//The units have to be converted to AU and AU/Day
xBody=(double*)mxGetData(prhs[3]);
//Allocate space to hold the parameters of the astronomical bodies in a
//manner suitable for the iauLdn function.
bodyParam=(iauLDBODY*)mxMalloc(sizeof(iauLDBODY)*numBodies);
baseIdx=0;
for(curBody=0;curBody<numBodies;curBody++) {
int i;
bodyParam[curBody].bm=MSolar[curBody];
if(deflecLimit!=NULL) {
bodyParam[curBody].dl=deflecLimit[curBody];
} else {
//A value suitably small for Saturn.
bodyParam[curBody].dl=3e-10;
}
//Position
for(i=0;i<3;i++) {
//The division converts from meters to AU.
bodyParam[curBody].pv[0][i]=xBody[baseIdx+i]/DAU;
}
//Velocity
for(i=0;i<3;i++) {
//Convert from meters per second BCRS to AU/ day.
bodyParam[curBody].pv[1][i]=xBody[3+baseIdx+i]*(1/DAU)*(1/DAYSEC);
}
baseIdx+=6;
}
//Allocate space for the return values.
vDeflMATLAB=mxCreateDoubleMatrix(3,numSource,mxREAL);
vDefl=(double*)mxGetData(vDeflMATLAB);
baseIdx=0;
for(curSource=0;curSource<numSource;curSource++) {
double vecMag, sc[3];//Unit vector to the source
//Get a unit direction vector and magnitude to the current source.
iauPn(vObsSource+baseIdx, &vecMag, sc);
iauLdn(numBodies, bodyParam, posObs, sc,vDefl+baseIdx);
//Deal with possibly non-unit magnitudes on the input.
vDefl[baseIdx]*=vecMag;
vDefl[baseIdx+1]*=vecMag;
vDefl[baseIdx+2]*=vecMag;
baseIdx+=3;
}
//Free temporary memory; set the return value.
mxFree(bodyParam);
plhs[0]=vDeflMATLAB;
}
void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[]) {
double *points,a,b,b2,f,e2,eps2,ec;
size_t numVec,i;
mxArray *retMat;
double *retData;
if(nrhs>3||nrhs<1){
mexErrMsgTxt("Wrong number of inputs");
}
if(nlhs>1) {
mexErrMsgTxt("Wrong number of outputs.");
return;
}
checkRealDoubleArray(prhs[0]);
numVec = mxGetN(prhs[0]);
if(mxGetM(prhs[0])!=3) {
mexErrMsgTxt("The input vector has a bad dimensionality.");
}
points=(double*)mxGetData(prhs[0]);
//points[0] is x
//points[1] is y
//points[2] is z
if(nrhs>1) {
a=getDoubleFromMatlab(prhs[1]);
} else {
a=getScalarMatlabClassConst("Constants", "WGS84SemiMajorAxis");
}
if(nrhs>2) {
f=getDoubleFromMatlab(prhs[2]);
} else {
f=getScalarMatlabClassConst("Constants", "WGS84Flattening");
}
b=a*(1-f);//The semi-minor axis of the reference ellipsoid.
b2=b*b;
//The square of the first numerical eccentricity.
e2=2*f-f*f;
//The square of the second numerical eccentricity.
eps2=a*a/(b2)-1;
//This value is used if Fukushima's method is chosen.
ec=sqrt(1-e2);
//Allocate space for the return variables.
retMat=mxCreateDoubleMatrix(3,numVec,mxREAL);
retData=(double*)mxGetData(retMat);
for(i=0;i<numVec;i++) {
double *phi, *lambda, *h;
double x0,y0,z0;
double r0,p,s,q;
//Get the Cartesian point to convert.
x0=points[3*i];
y0=points[3*i+1];
z0=points[3*i+2];
//Get the addresses of where the converted components will go
phi=retData+3*i;
lambda=retData+3*i+1;
h=retData+3*i+2;
r0=sqrt(x0*x0+y0*y0);
p=fabs(z0)/eps2;
s=r0*r0/(e2*eps2);
q=p*p-b2+s;
*lambda=atan2(y0,x0);
if(q>=0) {//Use Sofair's algorithm
double u,v,P,Q,t,c,w,z,Ne,val;
u=p/sqrt(q);
v=b2*u*u/q;
P=27.0*v*s/q;
Q=pow(sqrt(P+1.0)+sqrt(P),2.0/3.0);
t=(1.0+Q+1/Q)/6.0;
c=u*u-1+2*t;
//This condition prevents finite precision problems due to
//subtraction within the square root.
c=fMax(c,0);
c=sqrt(c);
w=(c-u)/2.0;
//The z coordinate of the closest point projected on the ellipsoid.
//The fmax command deals with precision problems when the argument
//is nearly zero. The problems arise due to the subtraction within
//the square root.
z=sqrt(t*t+v)-u*w-t/2.0-1.0/4.0;
z=fMax(z,0);
z=copySign(sqrt(q)*(w+sqrt(z)),z0);
Ne=a*sqrt(1+eps2*z*z/b2);
//The min and max terms deals with finite precision problems.
val=fMin(z*(eps2+1)/Ne,1);
val=fMax(val,-1.0);
*phi=asin(val);
*h=r0*cos(*phi)+z0*sin(*phi)-a*a/Ne;
} else {//Use Fukushima's algorithm.
double Cc,P,Z,S,C;
//A gets a value within the loop. This initialization is just
//to silence a warning when compiling with
//-Wconditional-uninitialized
double A=0;
size_t curIter;
const size_t maxIter=6;
P=r0/a;
Z=(ec/a)*fabs(z0);
S=Z;
C=ec*P;
//Loop until convergence. Assume convergence in 6 iterations.
for(curIter=0;curIter<maxIter;curIter++) {
double B,F,D,SNew,CNew;
A=sqrt(S*S+C*C);
B=1.5*e2*S*C*C*((P*S-Z*C)*A-e2*S*C);
F=P*A*A*A-e2*C*C*C;
D=Z*A*A*A+e2*S*S*S;
SNew=D*F-B*S;
CNew=F*F-B*C;
SNew=SNew/CNew;
if(!isFinite(SNew)) {
S=SNew;
C=1;
A=sqrt(S*S+C*C);
break;
} else {
S=SNew;
C=1;
}
}
Cc=ec*C;
//If the point is along the z-axis, then SNew and CNew will
//both be zero, leading to a non-finite result.
if(!isFinite(S)) {
*phi=copySign(pi/2,z0);
*h=fabs(z0)-b;
} else {
*phi=copySign(atan(S/Cc),z0);
*h=(r0*Cc+fabs(z0)*S-b*A)/sqrt(Cc*Cc+S*S);
}
}
}
plhs[0]=retMat;
}
void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[]) {
char cmd[64];
metricTreeCPP *theTree;
if(nrhs>4) {
mexErrMsgTxt("Too many inputs.");
}
//Get the command string that is passed.
mxGetString(prhs[0], cmd, sizeof(cmd));
//prhs[0] is assumed to be the string telling
if(!strcmp("metricTreeCPP", cmd)){
size_t k, N;
mxArray *retPtr;
k=getSizeTFromMatlab(prhs[1]);
N=getSizeTFromMatlab(prhs[2]);
theTree = new metricTreeCPP(k,N);
//Convert the pointer to a Matlab matrix to return.
retPtr=ptr2Matlab<metricTreeCPP*>(theTree);
//Lock this mex file so that it can not be cleared until the object
//has been deleted (This avoids a memory leak).
mexLock();
//Return the pointer to the tree
plhs[0]=retPtr;
} else if(!strcmp("buildTreeFromBatch",cmd)) {
double *dataBatch;
//Get the pointer back from Matlab.
theTree=Matlab2Ptr<metricTreeCPP*>(prhs[1]);
checkRealDoubleArray(prhs[2]);
dataBatch=reinterpret_cast<double*>(mxGetData(prhs[2]));
theTree->buildTreeFromBatch(dataBatch);
} else if(!strcmp("searchRadius",cmd)) {
size_t numPoints;
ClusterSetCPP<size_t> pointClust;
ClusterSetCPP<double> distClust;
double *point;
double *radius;
mxArray *clustParams[3];
//Get the inputs
theTree=Matlab2Ptr<metricTreeCPP*>(prhs[1]);
checkRealDoubleArray(prhs[2]);
checkRealDoubleArray(prhs[3]);
point=reinterpret_cast<double*>(mxGetData(prhs[2]));
radius=reinterpret_cast<double*>(mxGetData(prhs[3]));
numPoints=mxGetN(prhs[2]);
if(mxGetM(prhs[2])!=theTree->k){
mexErrMsgTxt("Invalid point size passed.");
}
//Run the search; rangeCluster now contains the results.
theTree->searchRadius(pointClust,distClust,point,radius, numPoints);
//Put the results into an instance of the ClusterSet container
//class in Matlab.
clustParams[0]=unsignedSizeMat2Matlab(pointClust.clusterEls,pointClust.totalNumEl,1);
clustParams[1]=unsignedSizeMat2Matlab(pointClust.clusterSizes,pointClust.numClust,1);
clustParams[2]=unsignedSizeMat2Matlab(pointClust.offsetArray,pointClust.numClust,1);
//Return a ClusterSet containing the appropriate data.
mexCallMATLAB(1, &(plhs[0]), 3, clustParams, "ClusterSet");
//If the distances should also be returned.
if(nlhs>1) {
clustParams[0]=doubleMat2Matlab(distClust.clusterEls,distClust.totalNumEl,1);
clustParams[1]=unsignedSizeMat2Matlab(distClust.clusterSizes,distClust.numClust,1);
clustParams[2]=unsignedSizeMat2Matlab(distClust.offsetArray,distClust.numClust,1);
//Return a ClusterSet containing the appropriate data.
mexCallMATLAB(1, &(plhs[1]), 3, clustParams, "ClusterSet");
}
} else if(!strcmp("~metricTreeCPP", cmd)){
theTree=Matlab2Ptr<metricTreeCPP*>(prhs[1]);
delete theTree;
//Unlock the mex file allowing it to be cleared.
mexUnlock();
} else if(!strcmp("getAllData", cmd)){
size_t N;
size_t k;
theTree=Matlab2Ptr<metricTreeCPP*>(prhs[1]);
N=theTree->N;
k=theTree->k;
plhs[0]=unsignedSizeMat2Matlab(theTree->DATAIDX,N, 1);
if(nlhs>1) {
plhs[1]=signedSizeMat2Matlab(theTree->innerChild,N, 1);
if(nlhs>2) {
plhs[2]=signedSizeMat2Matlab(theTree->outerChild,N, 1);
if(nlhs>3) {
plhs[3]=doubleMat2Matlab(theTree->innerRadii,N, 1);
if(nlhs>4) {
plhs[4]=doubleMat2Matlab(theTree->outerRadii,N, 1);
if(nlhs>5) {
plhs[5]=doubleMat2Matlab(theTree->data,k, N);
}
}
}
}
}
} else if(!strcmp("getN", cmd)) {
theTree=Matlab2Ptr<metricTreeCPP*>(prhs[1]);
plhs[0]=unsignedSizeMat2Matlab(&(theTree->N),1,1);
} else if(!strcmp("getk", cmd)) {
theTree=Matlab2Ptr<metricTreeCPP*>(prhs[1]);
plhs[0]=unsignedSizeMat2Matlab(&(theTree->k),1,1);
}else {
mexErrMsgTxt("Invalid string passed to metricTreeCPPInt.");
}
}
void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[]) {
size_t numPolyVertices, numClipVertices;
double *convexClipPolygon, *curPolygon, *clippedPolygonNew;
double *prevClipVertex;
mxArray *curPolygonMat, *clippedPolygonNewMat;
size_t curClip,numVertices;
if(nrhs!=2) {
mexErrMsgTxt("Incorrect number of inputs.");
return;
}
if(nlhs>1) {
mexErrMsgTxt("Incorrect number of outputs.");
return;
}
//Return an empty matrix if an empty polygon is passed.
if(mxIsEmpty(prhs[0])){
plhs[0]=mxCreateDoubleMatrix(0,0,mxREAL);
return;
}
if(mxGetM(prhs[0])!=2||mxGetM(prhs[1])!=2) {
mexErrMsgTxt("The polygons must be two-dimensional.");
return;
}
checkRealDoubleArray(prhs[0]);
numPolyVertices=mxGetN(prhs[0]);
if(numPolyVertices<3) {
mexErrMsgTxt("The polygon to be clipped must have at least three vertices.");
return;
}
checkRealDoubleArray(prhs[1]);
numClipVertices=mxGetN(prhs[1]);
if(numClipVertices<3) {
mexErrMsgTxt("The clipping polygon must have at least three vertices.");
return;
}
convexClipPolygon=(double*)mxGetData(prhs[1]);
//The clipping polygon must have its vertices going in a
//counterclockwise order. Check whether that is the case. If not, then
//reverse the order.
if(signedPolygonAreaCPP(convexClipPolygon,numClipVertices)<0) {
mexErrMsgTxt("The vertices of the clipping polygon should be in a counterclockwise order. Reverse the order of the vertices and try again.");
return;
}
//Allocate space for the new polygon and for temporary scratch space.
{
//The maximum possible number of vertices in the clipped polygon.
const size_t maxClippedVertices=(numPolyVertices*3)/2+1;
size_t i;
double *inputEls=(double*)mxGetData(prhs[0]);
curPolygonMat=mxCreateDoubleMatrix(2,maxClippedVertices,mxREAL);
clippedPolygonNewMat=mxCreateDoubleMatrix(2,maxClippedVertices,mxREAL);
curPolygon=(double*)mxGetData(curPolygonMat);
clippedPolygonNew=(double*)mxGetData(clippedPolygonNewMat);
//Copy the values in prhs[0] into curPolygon.
for(i=0;i<2*numPolyVertices;i++) {
curPolygon[i]=inputEls[i];
}
//The current number of vertices in curPolygon.
numVertices=numPolyVertices;
}
//The first clipping edge will be the one from the end to the
//beginning.
prevClipVertex=convexClipPolygon+2*(numClipVertices-1);
//For each edge, create the reduced polygon by clipping with that edge.
for(curClip=0;curClip<numClipVertices;curClip++) {
double *curClipVertex=convexClipPolygon+2*curClip;
double *prevVertex;
//The polygon will be clipped to this edge this iteration.
const double curClipEdge[2]={curClipVertex[0]-prevClipVertex[0],curClipVertex[1]-prevClipVertex[1]};
size_t curV, numVerticesNew;
double *curClippedPolygonNew=clippedPolygonNew;
//Number of vertices that have been added to clippedPolygonNew
numVerticesNew=0;
prevVertex=curPolygon+2*(numVertices-1);
for(curV=0;curV<numVertices;curV++) {
double *curVertex=curPolygon+2*curV;
//Note that the clip edges are infinitely extended so that
//vertices outside of the clipping region are involved.
if(vertexIsInsideClipEdge(curVertex,prevClipVertex,curClipEdge)) {
//If the current vertex is inside of the clipping region,
//then add it, but if the previous vertex was not in the
//clipping region, then an extra vertex at the edge of the
//boundary region needs to be added.
if(!vertexIsInsideClipEdge(prevVertex,prevClipVertex,curClipEdge)) {
//[prevVertex,curVertex]
const double line1[4]={prevVertex[0],prevVertex[1],curVertex[0],curVertex[1]};
//[curClipVertex,prevClipVertex]
const double line2[4]={curClipVertex[0],curClipVertex[1],prevClipVertex[0],prevClipVertex[1]};
twoLineIntersectionPoint2DCPP(line1,line2,curClippedPolygonNew);
curClippedPolygonNew+=2;
numVerticesNew++;
}
curClippedPolygonNew[0]=curVertex[0];
curClippedPolygonNew[1]=curVertex[1];
curClippedPolygonNew+=2;
numVerticesNew++;
}else if(vertexIsInsideClipEdge(prevVertex,prevClipVertex,curClipEdge)) {
//If the previous vertex was inside of the clipping region
//and this vertex is not, then add a line segment from the
//previous vertex to the edge of the clipping region.
//[prevVertex,curVertex]
const double line1[4]={prevVertex[0],prevVertex[1],curVertex[0],curVertex[1]};
//[curClipVertex,prevClipVertex]
const double line2[4]={curClipVertex[0],curClipVertex[1],prevClipVertex[0],prevClipVertex[1]};
twoLineIntersectionPoint2DCPP(line1,line2,curClippedPolygonNew);
curClippedPolygonNew+=2;
numVerticesNew++;
}
prevVertex=curVertex;
}
{//Swap the buffers.
double *temp=curPolygon;
curPolygon=clippedPolygonNew;
numVertices=numVerticesNew;
clippedPolygonNew=temp;
}
//The object is not in the viewing area at all.
if(numVertices==0) {
mxFree(curPolygonMat);
mxFree(clippedPolygonNewMat);
plhs[0]=mxCreateDoubleMatrix(0,0,mxREAL);
return;
}
prevClipVertex=curClipVertex;
}
//curPolygon is what should be returned. This corresponds to either
//curPolygonMat or clippedPolygonNewMat, since the buffers were
//exchanged during the loops. Determine which is curPolygon and return
//it, freeing the other.
if(curPolygon==(double*)mxGetData(curPolygonMat)) {
//Resize to fit.
mxSetN(curPolygonMat,numVertices);
plhs[0]=curPolygonMat;
mxDestroyArray(clippedPolygonNewMat);
} else {
mxSetN(clippedPolygonNewMat,numVertices);
plhs[0]=clippedPolygonNewMat;
mxDestroyArray(curPolygonMat);
}
}
void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[]) {
size_t numRow,numVec;
mxArray *retMat;
double *xVec, *retData;
double TT1, TT2, UT11, UT12;
//The if-statements below should properly initialize all of the EOP.
//The following initializations to zero are to suppress warnings when
//compiling with -Wconditional-uninitialized.
double xp=0;
double yp=0;
double deltaT=0;
double LOD=0;
double ITRS2TEME[3][3];
double PEF2TEME[3][3];
double WInv[3][3];//The inverse polar motion matrix to go from ITRS to PEF.
double Omega[3];//The angular velocity vector for the Earth's rotation.
if(nrhs<3||nrhs>6){
mexErrMsgTxt("Wrong number of inputs");
}
if(nlhs>2) {
mexErrMsgTxt("Wrong number of outputs.");
return;
}
checkRealDoubleArray(prhs[0]);
numRow = mxGetM(prhs[0]);
numVec = mxGetN(prhs[0]);
if(!(numRow==3||numRow==6)) {
mexErrMsgTxt("The input vector has a bad dimensionality.");
}
xVec=(double*)mxGetData(prhs[0]);
TT1=getDoubleFromMatlab(prhs[1]);
TT2=getDoubleFromMatlab(prhs[2]);
//If some values from the function getEOP will be needed
if(nrhs<6||mxIsEmpty(prhs[3])||mxIsEmpty(prhs[4])||mxIsEmpty(prhs[5])) {
mxArray *retVals[5];
double *xpyp;
mxArray *JulUTCMATLAB[2];
double JulUTC[2];
int retVal;
//Get the time in UTC to look up the parameters by going to TAI and
//then UTC.
retVal=iauTttai(TT1, TT2, &JulUTC[0], &JulUTC[1]);
if(retVal!=0) {
mexErrMsgTxt("An error occurred computing TAI.");
}
retVal=iauTaiutc(JulUTC[0], JulUTC[1], &JulUTC[0], &JulUTC[1]);
switch(retVal){
case 1:
mexWarnMsgTxt("Dubious Date entered.");
break;
case -1:
mexErrMsgTxt("Unacceptable date entered");
break;
default:
break;
}
JulUTCMATLAB[0]=doubleMat2Matlab(&JulUTC[0],1,1);
JulUTCMATLAB[1]=doubleMat2Matlab(&JulUTC[1],1,1);
//Get the Earth orientation parameters for the given date.
mexCallMATLAB(5,retVals,2,JulUTCMATLAB,"getEOP");
mxDestroyArray(JulUTCMATLAB[0]);
mxDestroyArray(JulUTCMATLAB[1]);
checkRealDoubleArray(retVals[0]);
checkRealDoubleArray(retVals[1]);
if(mxGetM(retVals[0])!=2||mxGetN(retVals[0])!=1||mxGetM(retVals[1])!=2||mxGetN(retVals[1])!=1) {
mxDestroyArray(retVals[0]);
mxDestroyArray(retVals[1]);
mxDestroyArray(retVals[2]);
mxDestroyArray(retVals[3]);
mxDestroyArray(retVals[4]);
mexErrMsgTxt("Error using the getEOP function.");
return;
}
xpyp=(double*)mxGetData(retVals[0]);
xp=xpyp[0];
yp=xpyp[1];
//The celestial pole offsets are not used.
//This is TT-UT1
deltaT=getDoubleFromMatlab(retVals[3]);
LOD=getDoubleFromMatlab(retVals[4]);
//Free the returned arrays.
mxDestroyArray(retVals[0]);
mxDestroyArray(retVals[1]);
mxDestroyArray(retVals[2]);
mxDestroyArray(retVals[3]);
mxDestroyArray(retVals[4]);
}
//If deltaT=TT-UT1 is given
if(nrhs>3&&!mxIsEmpty(prhs[3])) {
deltaT=getDoubleFromMatlab(prhs[3]);
}
//Obtain UT1 from terestrial time and deltaT.
iauTtut1(TT1, TT2, deltaT, &UT11, &UT12);
//Get polar motion values, if given.
if(nrhs>4&&!mxIsEmpty(prhs[4])) {
size_t dim1, dim2;
checkRealDoubleArray(prhs[4]);
dim1 = mxGetM(prhs[4]);
dim2 = mxGetN(prhs[4]);
if((dim1==2&&dim2==1)||(dim1==1&&dim2==2)) {
double *xpyp=(double*)mxGetData(prhs[4]);
xp=xpyp[0];
yp=xpyp[1];
} else {
mexErrMsgTxt("The celestial pole offsets have the wrong dimensionality.");
return;
}
}
//If LOD is given
if(nrhs>5&&!mxIsEmpty(prhs[5])) {
LOD=getDoubleFromMatlab(prhs[5]);
}
{
double GMST1982=iauGmst82(UT11, UT12);
double TEME2PEF[3][3];
double TEME2ITRS[3][3];
double W[3][3];
double omega;
//Get Greenwhich mean sidereal time under the IAU's 1982 model. This
//is given in radians and will be used to build a rotation matrix to
//rotate into the PEF system.
GMST1982=iauGmst82(UT11, UT12);
{
double cosGMST,sinGMST;
cosGMST=cos(GMST1982);
sinGMST=sin(GMST1982);
//Build the rotation matrix to rotate by GMST about the z-axis. This
//will put the position vector in the PEF system.
TEME2PEF[0][0]=cosGMST;
TEME2PEF[0][1]=sinGMST;
TEME2PEF[0][2]=0;
TEME2PEF[1][0]=-sinGMST;
TEME2PEF[1][1]=cosGMST;
TEME2PEF[1][2]=0;
TEME2PEF[2][0]=0;
TEME2PEF[2][1]=0;
TEME2PEF[2][2]=1.0;
}
//The inverse rotation is just the transpose
iauTr(TEME2PEF, PEF2TEME);
//To go from PEF to ITRS, we need to build the polar motion matrix
//using the IAU's 1980 conventions.
{
double cosXp,sinXp,cosYp,sinYp;
cosXp=cos(xp);
sinXp=sin(xp);
cosYp=cos(yp);
sinYp=sin(yp);
W[0][0]=cosXp;
W[0][1]=sinXp*sinYp;
W[0][2]=sinXp*cosYp;
W[1][0]=0;
W[1][1]=cosYp;
W[1][2]=-sinYp;
W[2][0]=-sinXp;
W[2][1]=cosXp*sinXp;
W[2][2]=cosXp*cosYp;
}
//The inverse rotation is just the transpose
iauTr(W, WInv);
//The total rotation matrix is thus the product of the two rotations.
//TEME2ITRS=W*TEME2PEF;
iauRxr(W, TEME2PEF, TEME2ITRS);
//We want the inverse rotation
iauTr(TEME2ITRS, ITRS2TEME);
//The angular velocity vector of the Earth in the TIRS in radians.
omega=getScalarMatlabClassConst("Constants","IERSMeanEarthRotationRate");
//Adjust for LOD
omega=omega*(1-LOD/86400.0);//86400.0 is the number of seconds in a TT day.
Omega[0]=0;
Omega[1]=0;
Omega[2]=omega;
}
//Allocate space for the return vectors.
retMat=mxCreateDoubleMatrix(numRow,numVec,mxREAL);
retData=(double*)mxGetData(retMat);
{
size_t curVec;
for(curVec=0;curVec<numVec;curVec++) {
//Multiply the position vector with the rotation matrix.
iauRxp(ITRS2TEME, xVec+numRow*curVec, retData+numRow*curVec);
//If a velocity vector was given.
if(numRow>3) {
double *posITRS=xVec+numRow*curVec;
double *velITRS=xVec+numRow*curVec+3;//Velocity in TEME
double posPEF[3];
double velPEF[3];
double *retDataVel=retData+numRow*curVec+3;
double rotVel[3];
//If a velocity was provided with the position, first
//convert to PEF coordinates, then account for the rotation
//of the Earth, then rotate into TEME coordinates.
//Convert velocity from ITRS to PEF.
iauRxp(WInv, velITRS, velPEF);
//Convert position from ITRS to PEF
iauRxp(WInv, posITRS, posPEF);
//Evaluate the cross product for the angular velocity due
//to the Earth's rotation.
iauPxp(Omega, posPEF, rotVel);
//Add the instantaneous velocity due to rotation.
iauPpp(velPEF, rotVel, retDataVel);
//Rotate from the PEF into the TEME
iauRxp(PEF2TEME, retDataVel, retDataVel);
}
}
}
plhs[0]=retMat;
if(nlhs>1) {
double *elPtr;
size_t i,j;
plhs[1]=mxCreateDoubleMatrix(3,3,mxREAL);
elPtr=(double*)mxGetData(plhs[1]);
for (i=0;i<3;i++) {
for(j=0;j<3;j++) {
elPtr[i+3*j]=ITRS2TEME[i][j];
}
}
}
}
void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[]) {
double TT1,TT2,*xVec;
double deltaT=0;
double LOD=0;
size_t numRow,numVec;
double CIRS2TIRS[3][3];
double TIRS2CIRS[3][3];
double Omega[3];//The rotation vector in the TIRS
mxArray *retMat;
double *retData;
if(nrhs<3||nrhs>5){
mexErrMsgTxt("Wrong number of inputs");
}
if(nlhs>2) {
mexErrMsgTxt("Wrong number of outputs.");
}
checkRealDoubleArray(prhs[0]);
numRow = mxGetM(prhs[0]);
numVec = mxGetN(prhs[0]);
if(!(numRow==3||numRow==6)) {
mexErrMsgTxt("The input vector has a bad dimensionality.");
}
xVec=(double*)mxGetData(prhs[0]);
TT1=getDoubleFromMatlab(prhs[1]);
TT2=getDoubleFromMatlab(prhs[2]);
//If some values from the function getEOP will be needed
if(nrhs<=4||mxIsEmpty(prhs[3])||mxIsEmpty(prhs[4])) {
mxArray *retVals[5];
mxArray *JulUTCMATLAB[2];
double JulUTC[2];
int retVal;
//Get the time in UTC to look up the parameters by going to TAI and
//then UTC.
retVal=iauTttai(TT1, TT2, &JulUTC[0], &JulUTC[1]);
if(retVal!=0) {
mexErrMsgTxt("An error occurred computing TAI.");
}
retVal=iauTaiutc(JulUTC[0], JulUTC[1], &JulUTC[0], &JulUTC[1]);
switch(retVal){
case 1:
mexWarnMsgTxt("Dubious Date entered.");
break;
case -1:
mexErrMsgTxt("Unacceptable date entered");
break;
default:
break;
}
JulUTCMATLAB[0]=doubleMat2Matlab(&JulUTC[0],1,1);
JulUTCMATLAB[1]=doubleMat2Matlab(&JulUTC[1],1,1);
//Get the Earth orientation parameters for the given date.
mexCallMATLAB(5,retVals,2,JulUTCMATLAB,"getEOP");
mxDestroyArray(JulUTCMATLAB[0]);
mxDestroyArray(JulUTCMATLAB[1]);
//We do not need the polar motion coordinates.
mxDestroyArray(retVals[0]);
//We do not need the celestial pole offsets.
mxDestroyArray(retVals[1]);
//This is TT-UT1
deltaT=getDoubleFromMatlab(retVals[3]);
LOD=getDoubleFromMatlab(retVals[4]);
//Free the returned arrays.
mxDestroyArray(retVals[2]);
mxDestroyArray(retVals[3]);
mxDestroyArray(retVals[4]);
}
//If deltaT=TT-UT1 is given
if(nrhs>3&&!mxIsEmpty(prhs[3])) {
deltaT=getDoubleFromMatlab(prhs[3]);
}
//If LOD is given
if(nrhs>4&&!mxIsEmpty(prhs[4])) {
LOD=getDoubleFromMatlab(prhs[4]);
}
//Compute the rotation matrix for going from CIRS to TIRS as well as
//the instantaneous vector angular momentum due to the Earth's rotation
//in GCRS coordinates.
{
double UT11, UT12;
double era, omega;
//Obtain UT1 from terestrial time and deltaT.
iauTtut1(TT1, TT2, deltaT, &UT11, &UT12);
//Find the Earth rotation angle for the given UT1 time.
era = iauEra00(UT11, UT12);
//Construct the rotation matrix.
CIRS2TIRS[0][0]=1;
CIRS2TIRS[0][1]=0;
CIRS2TIRS[0][2]=0;
CIRS2TIRS[1][0]=0;
CIRS2TIRS[1][1]=1;
CIRS2TIRS[1][2]=0;
CIRS2TIRS[2][0]=0;
CIRS2TIRS[2][1]=0;
CIRS2TIRS[2][2]=1;
iauRz(era, CIRS2TIRS);
//To go from the TIRS to the GCRS, we need to use the inverse rotation
//matrix, which is just the transpose of the rotation matrix.
iauTr(CIRS2TIRS, TIRS2CIRS);
//Next, to be able to transform the velocity, the rotation of the Earth
//has to be taken into account.
//The angular velocity vector of the Earth in the TIRS in radians.
omega=getScalarMatlabClassConst("Constants","IERSMeanEarthRotationRate");
//Adjust for LOD
omega=omega*(1-LOD/86400.0);//86400.0 is the number of seconds in a TT
//day.
Omega[0]=0;
Omega[1]=0;
Omega[2]=omega;
}
//Allocate space for the return vectors.
retMat=mxCreateDoubleMatrix(numRow,numVec,mxREAL);
retData=(double*)mxGetData(retMat);
{
size_t curVec;
for(curVec=0;curVec<numVec;curVec++) {
//Multiply the position vector with the rotation matrix.
iauRxp(TIRS2CIRS, xVec+numRow*curVec, retData+numRow*curVec);
//If a velocity vector was given.
if(numRow>3) {
double *posTIRS=xVec+numRow*curVec;
double *velTIRS=xVec+numRow*curVec+3;//Velocity in GCRS
double *retDataVel=retData+numRow*curVec+3;
double rotVel[3];
//Evaluate the cross product for the angular velocity due
//to the Earth's rotation.
iauPxp(Omega, posTIRS, rotVel);
//Add the instantaneous velocity due to rotation.
iauPpp(velTIRS, rotVel, retDataVel);
//Rotate from TIRS to GCRS
iauRxp(TIRS2CIRS, retDataVel, retDataVel);
}
}
}
plhs[0]=retMat;
if(nlhs>1) {
double *elPtr;
size_t i,j;
plhs[1]=mxCreateDoubleMatrix(3,3,mxREAL);
elPtr=(double*)mxGetData(plhs[1]);
for (i=0;i<3;i++) {
for(j=0;j<3;j++) {
elPtr[i+3*j]=TIRS2CIRS[i][j];
}
}
}
}
void mexFunction(const int nlhs, mxArray *plhs[], const int nrhs, const mxArray *prhs[]) {
size_t numRow, numCol;
mxArray *AMat=NULL;
double *A,permVal;
if(nrhs<1) {
mexErrMsgTxt("Not enough inputs.");
}
if(nrhs>3) {
mexErrMsgTxt("Too many inputs.");
}
if(nlhs>1) {
mexErrMsgTxt("Too many outputs.");
}
numRow = mxGetM(prhs[0]);
numCol = mxGetN(prhs[0]);
//Empty matrices have a matrix permanent of one by definition.
if(numRow==0||numCol==0) {
const double retVal=1;
plhs[0]=doubleMat2Matlab(&retVal,1,1);
return;
}
checkRealDoubleArray(prhs[0]);
//If skip lists have bee provided, then use them.
if(nrhs>1) {
size_t i, numRowsSkipped,numColsSkipped,numRowsKept,numColsKept;
size_t arrayLen;
bool *boolColsSkip, *boolRowsSkip;
size_t *keptBuffer, *rows2Keep, *cols2Keep;
bool didAlloc=false;
if(nrhs<3||mxIsEmpty(prhs[2])) {
boolColsSkip=reinterpret_cast<bool*>(mxMalloc(numCol*sizeof(bool)));
//Set all of the entries to false since no column is skipped.
std::fill_n(boolColsSkip, numCol, false);
arrayLen=numCol;
} else {
boolColsSkip=copyBoolArrayFromMatlab(prhs[2], &arrayLen);
if(arrayLen!=numCol) {
mxFree(boolColsSkip);
mexErrMsgTxt("The column skip list has the wrong length.");
}
}
//Count the number of skipped columns
numColsSkipped=0;
for(i=0; i<numCol; i++) {
numColsSkipped+=boolColsSkip[i];
}
if(mxIsEmpty(prhs[1])) {
boolRowsSkip=reinterpret_cast<bool*>(mxMalloc(numRow*sizeof(bool)));
//Set all of the entries to false since no column is skipped.
std::fill_n(boolRowsSkip, numRow, false);
arrayLen=numRow;
} else {
boolRowsSkip=copyBoolArrayFromMatlab(prhs[1], &arrayLen);
if(arrayLen!=numRow) {
mxFree(boolRowsSkip);
mexErrMsgTxt("The row skip list has the wrong length.");
}
}
//Count the number of skipped rows
numRowsSkipped=0;
for(i=0; i<arrayLen; i++) {
numRowsSkipped+=boolRowsSkip[i];
}
numRowsKept=numRow-numRowsSkipped;
numColsKept=numCol-numColsSkipped;
//Empty matrices have a matrix permanent of one by definition.
if(numRowsKept==0||numColsKept==0) {
const double retVal=1;
plhs[0]=doubleMat2Matlab(&retVal,1,1);
mxFree(boolColsSkip);
mxFree(boolRowsSkip);
return;
}
if(numRowsKept<=numColsKept) {
A=reinterpret_cast<double*>(mxGetData(prhs[0]));
} else {
std::swap(numRowsKept, numColsKept);
//This is freed using mxDestroyArray
AMat=mxCreateDoubleMatrix(numRowsKept,numColsKept,mxREAL);
didAlloc=true;
mexCallMATLAB(1, &AMat, 1, const_cast<mxArray **>(&prhs[0]), "transpose");
A=reinterpret_cast<double*>(mxGetData(AMat));
}
//Set the mapping of indices of the rows in the submatrix to
//indices in the full matrix and similarly, set the mapping of
//columns in the submatrix to columns in the full matrix.
//The two buffers will be allocated in one go and then freed
//together in the end.
keptBuffer=reinterpret_cast<size_t*>(mxMalloc(sizeof(size_t)*(numRowsKept+numColsKept)));
rows2Keep=keptBuffer;
cols2Keep=keptBuffer+numRowsKept;
{ //Do the mapping of the rows.
size_t cumSkip, curIdx, curRow;
cumSkip=0;
curIdx=0;
for(curRow=0; curRow<numRow; curRow++) {
if(boolRowsSkip[curRow]==true) {
cumSkip++;
} else {
rows2Keep[curIdx]=curIdx+cumSkip;
curIdx++;
}
}
}
{ //Do the mapping of the columns.
size_t cumSkip, curIdx, curCol;
cumSkip=0;
curIdx=0;
for(curCol=0; curCol<numCol; curCol++) {
if(boolColsSkip[curCol]==true) {
cumSkip++;
} else {
cols2Keep[curIdx]=curIdx+cumSkip;
curIdx++;
}
}
}
{
size_t* buffer;
buffer=reinterpret_cast<size_t*>(mxMalloc(numColsKept*sizeof(size_t)));
permVal=permCPPSkip(A, numRow, rows2Keep, cols2Keep, numRowsKept, numColsKept,buffer);
mxFree(buffer);
}
//Get rid of temporary space.
if(didAlloc) {
mxDestroyArray(AMat);
}
mxFree(keptBuffer);
mxFree(boolColsSkip);
mxFree(boolRowsSkip);
//Set the return value
plhs[0]=doubleMat2Matlab(&permVal,1,1);
return;
}
if(numRow==numCol) {
double *buffer;
A=reinterpret_cast<double*>(mxGetData(prhs[0]));
//Allocate temporary scratch space.
buffer = reinterpret_cast<double*>(mxMalloc((2*numRow+1)*sizeof(double)));
permVal=permSquareCPP(A,numRow,buffer);
mxFree(buffer);
} else {
bool didAlloc=false;
size_t *buffer;
if(numRow<=numCol) {
A=reinterpret_cast<double*>(mxGetData(prhs[0]));
} else {
std::swap(numRow, numCol);
//This is freed using mxDestroyArray
AMat=mxCreateDoubleMatrix(numRow,numCol,mxREAL);
didAlloc=true;
mexCallMATLAB(1, &AMat, 1, const_cast<mxArray **>(&prhs[0]), "transpose");
A=reinterpret_cast<double*>(mxGetData(AMat));
}
buffer=reinterpret_cast<size_t*>(mxMalloc(numCol*sizeof(size_t)));
permVal=permCPP(A,numRow,numCol,buffer);
mxFree(buffer);
//Get rid of temporary space.
if(didAlloc) {
mxDestroyArray(AMat);
}
}
//Set the return value
plhs[0]=doubleMat2Matlab(&permVal,1,1);
}
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(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[]) {
double TT1,TT2,TDB1,TDB2,Jul2,deltaTTUT1,deltaT,Jul1UT1, Jul2UT1,UT1Frac;
double u, v, elon;
int retVal;
if(nrhs<2||nrhs>4){
mexErrMsgTxt("Wrong number of inputs.");
return;
}
if(nlhs>2){
mexErrMsgTxt("Wrong number of outputs.");
return;
}
TT1=getDoubleFromMatlab(prhs[0]);
TT2=getDoubleFromMatlab(prhs[1]);
//If the parameter is given and is not just an empty matrix.
if(nrhs>2&&!(mxGetM(prhs[2])==0||mxGetN(prhs[2])==0)) {
deltaTTUT1=getDoubleFromMatlab(prhs[2]);
} else {
mxArray *retVals[4];
mxArray *JulUTCMATLAB[2];
double JulUTC[2];
//Get the time in UTC to look up the parameters by going to TAI and
//then UTC.
retVal=iauTttai(TT1, TT2, &JulUTC[0], &JulUTC[1]);
if(retVal!=0) {
mexErrMsgTxt("An error occurred computing TAI.");
}
retVal=iauTaiutc(JulUTC[0], JulUTC[1], &JulUTC[0], &JulUTC[1]);
switch(retVal){
case 1:
mexWarnMsgTxt("Dubious Date entered.");
break;
case -1:
mexErrMsgTxt("Unacceptable date entered");
break;
default:
break;
}
JulUTCMATLAB[0]=doubleMat2Matlab(&JulUTC[0],1,1);
JulUTCMATLAB[1]=doubleMat2Matlab(&JulUTC[1],1,1);
//Get the Earth orientation parameters for the given date.
mexCallMATLAB(4,retVals,2,JulUTCMATLAB,"getEOP");
//This is TT-UT1
deltaTTUT1=getDoubleFromMatlab(retVals[3]);
//Free the returned arrays.
mxDestroyArray(retVals[0]);
mxDestroyArray(retVals[1]);
mxDestroyArray(retVals[2]);
mxDestroyArray(retVals[3]);
mxDestroyArray(JulUTCMATLAB[0]);
mxDestroyArray(JulUTCMATLAB[1]);
}
if(nrhs>3) {
double *xyzClock;
if(mxGetM(prhs[3])!=3||mxGetN(prhs[3])!=1) {
mexErrMsgTxt("The dimensionality of the clock location is incorrect.");
return;
}
checkRealDoubleArray(prhs[3]);
xyzClock=(double*)mxGetData(prhs[3]);
//Convert from meters to kilometers.
xyzClock[0]/=1000;xyzClock[1]/=1000;xyzClock[2]/=1000;
u=sqrt(xyzClock[0]*xyzClock[0] + xyzClock[1]*xyzClock[1]);
v=xyzClock[2];
elon=atan2(xyzClock[1],xyzClock[0]);
} else {
u=0;
v=0;
elon=0;
}
//Get UT1
retVal=iauTtut1(TT1, TT2, deltaTTUT1, &Jul1UT1, &Jul2UT1);
switch(retVal) {
case -1:
mexErrMsgTxt("Unacceptable date provided.");
return;
case 1:
mexWarnMsgTxt("Dubious year provided\n");
break;
default:
break;
}
//Find the fraction of a Julian day in UT1
UT1Frac=(Jul1UT1-floor(Jul1UT1))+(Jul2UT1-floor(Jul2UT1));
UT1Frac=UT1Frac-floor(UT1Frac);
/*Compute TDB-TT. The function iauDtdb takes TDB as an input, but we
*only have TT. The documentation for iauDtdb says that TT can be
*substituted within the precision limits of the function. However, an
*extra iteration is performed here to try to make the result
*consistent with TDB2TT to a few more digits of precision.*/
TDB1=TT1;
TDB2=TT2;
{
int curIter;
for(curIter=0;curIter<2;curIter++) {
deltaT = iauDtdb(TDB1, TDB2, UT1Frac, elon, u, v);
//TT -> TDB
retVal = iauTttdb(TT1, TT2, deltaT, &TDB1, &TDB2);
if(retVal!=0) {
mexErrMsgTxt("An error occured during an intermediate conversion to TDB.\n");
return;
}
}
}
plhs[0]=doubleMat2Matlab(&TDB1,1, 1);
if(nlhs>1) {
plhs[1]=doubleMat2Matlab(&TDB2,1, 1);
}
}
void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[]) {
size_t numRow,numVec;
mxArray *retMat;
double *xVec, *retData;
double TT1, TT2, UT11, UT12;
//The if-statements below should properly initialize all of the EOP.
//The following initializations to zero are to suppress warnings when
//compiling with -Wconditional-uninitialized.
double dX=0;
double dY=0;
double deltaT=0;
double LOD=0;
double GCRS2TIRS[3][3];
//Polar motion matrix. ITRS=POM*TIRS. We will just be setting it to the
//identity matrix as polar motion is not taken into account when going
//to the TIRS.
double rident[3][3]={{1,0,0},{0,1,0},{0,0,1}};
double Omega[3];//The rotation vector in the TIRS
if(nrhs<3||nrhs>6){
mexErrMsgTxt("Wrong number of inputs");
}
if(nlhs>2) {
mexErrMsgTxt("Wrong number of outputs.");
}
checkRealDoubleArray(prhs[0]);
numRow = mxGetM(prhs[0]);
numVec = mxGetN(prhs[0]);
if(!(numRow==3||numRow==6)) {
mexErrMsgTxt("The input vector has a bad dimensionality.");
}
xVec=(double*)mxGetData(prhs[0]);
TT1=getDoubleFromMatlab(prhs[1]);
TT2=getDoubleFromMatlab(prhs[2]);
//If some values from the function getEOP will be needed
if(nrhs<=5||mxIsEmpty(prhs[3])||mxIsEmpty(prhs[4])||mxIsEmpty(prhs[5])) {
mxArray *retVals[5];
double *dXdY;
mxArray *JulUTCMATLAB[2];
double JulUTC[2];
int retVal;
//Get the time in UTC to look up the parameters by going to TAI and
//then UTC.
retVal=iauTttai(TT1, TT2, &JulUTC[0], &JulUTC[1]);
if(retVal!=0) {
mexErrMsgTxt("An error occurred computing TAI.");
}
retVal=iauTaiutc(JulUTC[0], JulUTC[1], &JulUTC[0], &JulUTC[1]);
switch(retVal){
case 1:
mexWarnMsgTxt("Dubious Date entered.");
break;
case -1:
mexErrMsgTxt("Unacceptable date entered");
break;
default:
break;
}
JulUTCMATLAB[0]=doubleMat2Matlab(&JulUTC[0],1,1);
JulUTCMATLAB[1]=doubleMat2Matlab(&JulUTC[1],1,1);
//Get the Earth orientation parameters for the given date.
mexCallMATLAB(5,retVals,2,JulUTCMATLAB,"getEOP");
mxDestroyArray(JulUTCMATLAB[0]);
mxDestroyArray(JulUTCMATLAB[1]);
//%We do not need the polar motion coordinates.
mxDestroyArray(retVals[0]);
checkRealDoubleArray(retVals[1]);
if(mxGetM(retVals[1])!=2||mxGetN(retVals[1])!=1) {
mxDestroyArray(retVals[1]);
mxDestroyArray(retVals[2]);
mxDestroyArray(retVals[3]);
mxDestroyArray(retVals[4]);
mexErrMsgTxt("Error using the getEOP function.");
return;
}
dXdY=(double*)mxGetData(retVals[1]);
dX=dXdY[0];
dY=dXdY[1];
//This is TT-UT1
deltaT=getDoubleFromMatlab(retVals[3]);
LOD=getDoubleFromMatlab(retVals[4]);
//Free the returned arrays.
mxDestroyArray(retVals[1]);
mxDestroyArray(retVals[2]);
mxDestroyArray(retVals[3]);
mxDestroyArray(retVals[4]);
}
//If deltaT=TT-UT1 is given
if(nrhs>3&&!mxIsEmpty(prhs[3])) {
deltaT=getDoubleFromMatlab(prhs[3]);
}
//Obtain UT1 from terestrial time and deltaT.
iauTtut1(TT1, TT2, deltaT, &UT11, &UT12);
//Get celestial pole offsets, if given.
if(nrhs>4&&!mxIsEmpty(prhs[4])) {
size_t dim1, dim2;
checkRealDoubleArray(prhs[4]);
dim1 = mxGetM(prhs[4]);
dim2 = mxGetN(prhs[4]);
if((dim1==2&&dim2==1)||(dim1==1&&dim2==2)) {
double *dXdY=(double*)mxGetData(prhs[4]);
dX=dXdY[0];
dY=dXdY[1];
} else {
mexErrMsgTxt("The celestial pole offsets have the wrong dimensionality.");
return;
}
}
//If LOD is given
if(nrhs>5&&mxIsEmpty(prhs[5])) {
LOD=getDoubleFromMatlab(prhs[5]);
}
//Compute the rotation matrix for going from GCRS to ITRS as well as
//the instantaneous vector angular momentum due to the Earth's rotation
//in TIRS coordinates.
{
double x, y, s, era;
double rc2i[3][3];
double omega;
//Get the X,Y coordinates of the Celestial Intermediate Pole (CIP) and
//the Celestial Intermediate Origin (CIO) locator s, using the IAU 2006
//precession and IAU 2000A nutation models.
iauXys06a(TT1, TT2, &x, &y, &s);
//Add the CIP offsets.
x += dX;
y += dY;
//Get the GCRS-to-CIRS matrix
iauC2ixys(x, y, s, rc2i);
//Find the Earth rotation angle for the given UT1 time.
era = iauEra00(UT11, UT12);
//Set the polar motion matrix to the identity matrix so that the
//conversion stops at the TIRS instead of the ITRS.
//Combine the GCRS-to-CIRS matrix, the Earth rotation angle, and use
//the identity matrix instead of the polar motion matrix to get a
//to get the rotation matrix to go from GCRS to TIRS.
iauC2tcio(rc2i, era, rident,GCRS2TIRS);
//Next, to be able to transform the velocity, the rotation of the Earth
//has to be taken into account.
//The angular velocity vector of the Earth in the TIRS in radians.
omega=getScalarMatlabClassConst("Constants","IERSMeanEarthRotationRate");
//Adjust for LOD
omega=omega*(1-LOD/86400.0);//86400.0 is the number of seconds in a TT
//day.
Omega[0]=0;
Omega[1]=0;
Omega[2]=omega;
}
//Allocate space for the return vectors.
retMat=mxCreateDoubleMatrix(numRow,numVec,mxREAL);
retData=(double*)mxGetData(retMat);
{
size_t curVec;
for(curVec=0;curVec<numVec;curVec++) {
//Multiply the position vector with the rotation matrix.
iauRxp(GCRS2TIRS, xVec+numRow*curVec, retData+numRow*curVec);
//If a velocity vector was given.
if(numRow>3) {
double *posGCRS=xVec+numRow*curVec;
double posTIRS[3];
double *velGCRS=xVec+numRow*curVec+3;//Velocity in GCRS
double velTIRS[3];
double *retDataVel=retData+numRow*curVec+3;
double rotVel[3];
//If a velocity was provided with the position, first
//convert to TIRS coordinates, then account for the
//rotation of the Earth.
//Convert velocity from GCRS to TIRS.
iauRxp(GCRS2TIRS, velGCRS, velTIRS);
//Convert position from GCRS to TIRS
iauRxp(GCRS2TIRS, posGCRS, posTIRS);
//Evaluate the cross product for the angular velocity due
//to the Earth's rotation.
iauPxp(Omega, posTIRS, rotVel);
//Subtract out the instantaneous velocity due to rotation.
iauPmp(velTIRS, rotVel, retDataVel);
}
}
}
plhs[0]=retMat;
//If the rotation matrix is desired on the output.
if(nlhs>1) {
double *elPtr;
size_t i,j;
plhs[1]=mxCreateDoubleMatrix(3,3,mxREAL);
elPtr=(double*)mxGetData(plhs[1]);
for (i=0;i<3;i++) {
for(j=0;j<3;j++) {
elPtr[i+3*j]=GCRS2TIRS[i][j];
}
}
}
}
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[]) {
double TT1, TT2, dX, dY, *xVec;
size_t numRow, numVec;
mxArray *retMat;
double *retData;
double GCRS2CIRS[3][3];
if(nrhs<3||nrhs>4){
mexErrMsgTxt("Wrong number of inputs");
}
if(nlhs>2) {
mexErrMsgTxt("Wrong number of outputs.");
}
checkRealDoubleArray(prhs[0]);
numRow = mxGetM(prhs[0]);
numVec = mxGetN(prhs[0]);
if(!(numRow==3||numRow==6)) {
mexErrMsgTxt("The input vector has a bad dimensionality.");
}
xVec=(double*)mxGetData(prhs[0]);
TT1=getDoubleFromMatlab(prhs[1]);
TT2=getDoubleFromMatlab(prhs[2]);
//If some values from the function getEOP will be needed.
if(nrhs<4||mxGetM(prhs[3])==0) {
mxArray *retVals[2];
double *dXdY;
mxArray *JulUTCMATLAB[2];
double JulUTC[2];
int retVal;
//Get the time in UTC to look up the parameters by going to TAI and
//then UTC.
retVal=iauTttai(TT1, TT2, &JulUTC[0], &JulUTC[1]);
if(retVal!=0) {
mexErrMsgTxt("An error occurred computing TAI.");
}
retVal=iauTaiutc(JulUTC[0], JulUTC[1], &JulUTC[0], &JulUTC[1]);
switch(retVal){
case 1:
mexWarnMsgTxt("Dubious Date entered.");
break;
case -1:
mexErrMsgTxt("Unacceptable date entered");
break;
default:
break;
}
JulUTCMATLAB[0]=doubleMat2Matlab(&JulUTC[0],1,1);
JulUTCMATLAB[1]=doubleMat2Matlab(&JulUTC[1],1,1);
//Get the Earth orientation parameters for the given date.
mexCallMATLAB(2,retVals,2,JulUTCMATLAB,"getEOP");
mxDestroyArray(JulUTCMATLAB[0]);
mxDestroyArray(JulUTCMATLAB[1]);
//%We do not need the polar motion coordinates.
mxDestroyArray(retVals[0]);
checkRealDoubleArray(retVals[1]);
if(mxGetM(retVals[1])!=2||mxGetN(retVals[1])!=1) {
mxDestroyArray(retVals[1]);
mexErrMsgTxt("Error using the getEOP function.");
return;
}
dXdY=(double*)mxGetData(retVals[1]);
dX=dXdY[0];
dY=dXdY[1];
//Free the returned arrays.
mxDestroyArray(retVals[1]);
} else {//Get the celestial pole offsets
size_t dim1, dim2;
checkRealDoubleArray(prhs[4]);
dim1 = mxGetM(prhs[4]);
dim2 = mxGetN(prhs[4]);
if((dim1==2&&dim2==1)||(dim1==1&&dim2==2)) {
double *dXdY=(double*)mxGetData(prhs[4]);
dX=dXdY[0];
dY=dXdY[1];
} else {
mexErrMsgTxt("The celestial pole offsets have the wrong dimensionality.");
return;
}
}
{
double x, y, s;
double omega;
//Get the X,Y coordinates of the Celestial Intermediate Pole (CIP) and
//the Celestial Intermediate Origin (CIO) locator s, using the IAU 2006
//precession and IAU 2000A nutation models.
iauXys06a(TT1, TT2, &x, &y, &s);
//Add the CIP offsets.
x += dX;
y += dY;
//Get the GCRS-to-CIRS matrix
iauC2ixys(x, y, s, GCRS2CIRS);
}
//Allocate space for the return vectors.
retMat=mxCreateDoubleMatrix(numRow,numVec,mxREAL);
retData=(double*)mxGetData(retMat);
{
size_t curVec;
for(curVec=0;curVec<numVec;curVec++) {
//Multiply the position vector with the rotation matrix.
iauRxp(GCRS2CIRS, xVec+numRow*curVec, retData+numRow*curVec);
//If a velocity vector was given.
if(numRow>3) {
double *velGCRS=xVec+numRow*curVec+3;//Velocity in GCRS
double *retDataVel=retData+numRow*curVec+3;
//Convert velocity from GCRS to CIRS.
iauRxp(GCRS2CIRS, velGCRS, retDataVel);
}
}
}
plhs[0]=retMat;
//If the rotation matrix is desired on the output.
if(nlhs>1) {
double *elPtr;
size_t i,j;
plhs[1]=mxCreateDoubleMatrix(3,3,mxREAL);
elPtr=(double*)mxGetData(plhs[1]);
for (i=0;i<3;i++) {
for(j=0;j<3;j++) {
elPtr[i+3*j]=GCRS2CIRS[i][j];
}
}
}
}
void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[]) {
double *vOrig;
double *obsVel;
mxArray *retMATLAB;
double c, *retVec;
size_t numVec;
if(nrhs<2||nrhs>3) {
mexErrMsgTxt("Incorrect number of inputs.");
return;
}
if(nlhs>1) {
mexErrMsgTxt("Wrong number of outputs.");
return;
}
numVec=mxGetN(prhs[0]);
if(numVec!=mxGetN(prhs[1])||mxGetM(prhs[0])!=3||mxGetM(prhs[1])!=3) {
mexErrMsgTxt("The input vectors have the wrong dimensionality.");
return;
}
checkRealDoubleArray(prhs[0]);
vOrig=(double*)mxGetData(prhs[0]);
checkRealDoubleArray(prhs[1]);
obsVel=(double*)mxGetData(prhs[1]);
c=getScalarMatlabClassConst("Constants","speedOfLight");
//Allocate space for the return values.
retMATLAB=mxCreateDoubleMatrix(3,numVec,mxREAL);
retVec=(double*)mxGetData(retMATLAB);
//If the third parameter is provided, then use the algorithm from the IAU.
if(nrhs>2) {
double AU, *sunDist;
size_t curVec;
//Needed to convert units.
AU=getScalarMatlabClassConst("Constants","AstronomialUnit");
//If the dimensionality is wrong.
if(!((mxGetM(prhs[2])==1&&mxGetN(prhs[2])==numVec)||(mxGetM(prhs[2])==numVec&&mxGetN(prhs[2])==1))) {
mxDestroyArray(retMATLAB);
mexErrMsgTxt("The input vectors have the wrong dimensionality.");
return;
}
checkRealDoubleArray(prhs[0]);
sunDist=(double*)mxGetData(prhs[2]);
for(curVec=0;curVec<numVec;curVec++) {
double vecMag,unitVec[3];
double v[3];
double s;
double bm1;
//Get a unit direction vector and magnitude.
iauPn(vOrig+3*curVec, &vecMag, unitVec);
//Convert the velocity to units of the speed of light.
v[0]=obsVel[3*curVec]/c;
v[1]=obsVel[3*curVec+1]/c;
v[2]=obsVel[3*curVec+2]/c;
//The distance to the sun in AU.
s=sunDist[curVec]/AU;
//The reciprocal of the Lorentz factor.
bm1=sqrt(1-(v[0]*v[0]+v[1]*v[1]+v[2]*v[2]));
//Perform the correction with the IAU's function.
iauAb(unitVec, v, s, bm1,retVec+3*curVec);
//Set the magnitude back to what it was.
retVec[3*curVec]*=vecMag;
retVec[3*curVec+1]*=vecMag;
retVec[3*curVec+2]*=vecMag;
}
} else {
//If the distance to the sun is not given, then just perform a normal
//special relativistic correction.
size_t curVec;
for(curVec=0;curVec<numVec;curVec++) {
double vecMag,lightVec[3];
//Get a unit direction vector and magnitude.
iauPn(vOrig+3*curVec, &vecMag, lightVec);
//The light is in direction lightVec with speed c
lightVec[0]*=c;
lightVec[1]*=c;
lightVec[2]*=c;
//The light travels at speed c with true direction uPosition.
//Add the velocity vector of the observer to that of light.
relVecAddC(c,obsVel+3*curVec,lightVec,retVec+3*curVec);
//Because one vector had a magnitude of c, the returned vector
//must have the same magnitude. Restore the previous magnitude.
retVec[3*curVec]*=vecMag/c;
retVec[3*curVec+1]*=vecMag/c;
retVec[3*curVec+2]*=vecMag/c;
}
}
//Set the return value.
plhs[0]=retMATLAB;
}
